drivers/net/wireless/cw1200/itp.c

   1 /*
   2  * mac80211 glue code for mac80211 ST-Ericsson CW1200 drivers
   3  * ITP code
   4  *
   5  * Copyright (c) 2010, ST-Ericsson
   6  * Author: Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License version 2 as
  10  * published by the Free Software Foundation.
  11  */
  12
  13 #include <linux/module.h>
  14 #include <linux/debugfs.h>
  15 #include <linux/poll.h>
  16 #include <linux/time.h>
  17 #include <linux/random.h>
  18 #include <linux/kallsyms.h>
  19 #include <net/mac80211.h>
  20 #include "cw1200.h"
  21 #include "debug.h"
  22 #include "itp.h"
  23 #include "sta.h"
  24
  25 static int __cw1200_itp_open(struct cw1200_common *priv);
  26 static int __cw1200_itp_close(struct cw1200_common *priv);
  27 static void cw1200_itp_rx_start(struct cw1200_common *priv);
  28 static void cw1200_itp_rx_stop(struct cw1200_common *priv);
  29 static void cw1200_itp_rx_stats(struct cw1200_common *priv);
  30 static void cw1200_itp_rx_reset(struct cw1200_common *priv);
  31 static void cw1200_itp_tx_stop(struct cw1200_common *priv);
  32 static void cw1200_itp_handle(struct cw1200_common *priv,
  33                               struct sk_buff *skb);
  34 static void cw1200_itp_err(struct cw1200_common *priv,
  35                            int err,
  36                            int arg);
  37 static void __cw1200_itp_tx_stop(struct cw1200_common *priv);
  38
  39 static ssize_t cw1200_itp_read(struct file *file,
  40         char __user *user_buf, size_t count, loff_t *ppos)
  41 {
  42         struct cw1200_common *priv = file->private_data;
  43         struct cw1200_itp *itp = &priv->debug->itp;
  44         struct sk_buff *skb;
  45         int ret;
  46
  47         if (skb_queue_empty(&itp->log_queue))
  48                 return 0;
  49
  50         skb = skb_dequeue(&itp->log_queue);
  51         ret = copy_to_user(user_buf, skb->data, skb->len);
  52         *ppos += skb->len;
  53         skb->data[skb->len] = 0;
  54         pr_debug("[ITP] >>> %s", skb->data);
  55         consume_skb(skb);
  56
  57         return skb->len - ret;
  58 }
  59
  60 static ssize_t cw1200_itp_write(struct file *file,
  61         const char __user *user_buf, size_t count, loff_t *ppos)
  62 {
  63         struct cw1200_common *priv = file->private_data;
  64         struct sk_buff *skb;
  65
  66         if (!count || count > 1024)
  67                 return -EINVAL;
  68         skb = dev_alloc_skb(count + 1);
  69         if (!skb)
  70                 return -ENOMEM;
  71         skb_trim(skb, 0);
  72         skb_put(skb, count + 1);
  73         if (copy_from_user(skb->data, user_buf, count)) {
  74                 kfree_skb(skb);
  75                 return -EFAULT;
  76         }
  77         skb->data[count] = 0;
  78
  79         cw1200_itp_handle(priv, skb);
  80         consume_skb(skb);
  81         return count;
  82 }
  83
  84 static unsigned int cw1200_itp_poll(struct file *file, poll_table *wait)
  85 {
  86         struct cw1200_common *priv = file->private_data;
  87         struct cw1200_itp *itp = &priv->debug->itp;
  88         unsigned int mask = 0;
  89
  90         poll_wait(file, &itp->read_wait, wait);
  91
  92         if (!skb_queue_empty(&itp->log_queue))
  93                 mask |= POLLIN | POLLRDNORM;
  94
  95         mask |= POLLOUT | POLLWRNORM;
  96
  97         return mask;
  98 }
  99
 100 static int cw1200_itp_open(struct inode *inode, struct file *file)
 101 {
 102         struct cw1200_common *priv = inode->i_private;
 103         struct cw1200_itp *itp = &priv->debug->itp;
 104         int ret = 0;
 105
 106         file->private_data = priv;
 107         if (atomic_inc_return(&itp->open_count) == 1) {
 108                 ret = __cw1200_itp_open(priv);
 109                 if (ret && !atomic_dec_return(&itp->open_count))
 110                         __cw1200_itp_close(priv);
 111         } else {
 112                 atomic_dec(&itp->open_count);
 113                 ret = -EBUSY;
 114         }
 115
 116         return ret;
 117 }
 118
 119 static int cw1200_itp_close(struct inode *inode, struct file *file)
 120 {
 121         struct cw1200_common *priv = file->private_data;
 122         struct cw1200_itp *itp = &priv->debug->itp;
 123         if (!atomic_dec_return(&itp->open_count)) {
 124                 __cw1200_itp_close(priv);
 125                 wake_up(&itp->close_wait);
 126         }
 127         return 0;
 128 }
 129
 130 static const struct file_operations fops_itp = {
 131         .open = cw1200_itp_open,
 132         .read = cw1200_itp_read,
 133         .write = cw1200_itp_write,
 134         .poll = cw1200_itp_poll,
 135         .release = cw1200_itp_close,
 136         .llseek = default_llseek,
 137         .owner = THIS_MODULE,
 138 };
 139
 140 static void cw1200_itp_fill_pattern(u8 *data, int size,
 141                 enum cw1200_itp_data_modes mode)
 142 {
 143         if (size <= 0)
 144                 return;
 145
 146         switch (mode) {
 147         default:
 148         case ITP_DATA_ZEROS:
 149                 memset(data, 0x0, size);
 150                 break;
 151         case ITP_DATA_ONES:
 152                 memset(data, 0xff, size);
 153                 break;
 154         case ITP_DATA_ZERONES:
 155                 memset(data, 0x55, size);
 156                 break;
 157         case ITP_DATA_RANDOM:
 158                 get_random_bytes(data, size);
 159                 break;
 160         }
 161         return;
 162 }
 163
 164 static void cw1200_itp_tx_work(struct work_struct *work)
 165 {
 166         struct cw1200_itp *itp = container_of(work, struct cw1200_itp,
 167                     tx_work.work);
 168         struct cw1200_common *priv = itp->priv;
 169         atomic_set(&priv->bh_tx, 1);
 170         wake_up(&priv->bh_wq);
 171 }
 172
 173 static void cw1200_itp_tx_finish(struct work_struct *work)
 174 {
 175         struct cw1200_itp *itp = container_of(work, struct cw1200_itp,
 176                     tx_finish.work);
 177         __cw1200_itp_tx_stop(itp->priv);
 178 }
 179
 180 int cw1200_itp_init(struct cw1200_common *priv)
 181 {
 182         struct cw1200_itp *itp = &priv->debug->itp;
 183
 184         itp->priv = priv;
 185         atomic_set(&itp->open_count, 0);
 186         atomic_set(&itp->stop_tx, 0);
 187         atomic_set(&itp->awaiting_confirm, 0);
 188         skb_queue_head_init(&itp->log_queue);
 189         spin_lock_init(&itp->tx_lock);
 190         init_waitqueue_head(&itp->read_wait);
 191         init_waitqueue_head(&itp->write_wait);
 192         init_waitqueue_head(&itp->close_wait);
 193         INIT_DELAYED_WORK(&itp->tx_work, cw1200_itp_tx_work);
 194         INIT_DELAYED_WORK(&itp->tx_finish, cw1200_itp_tx_finish);
 195         itp->data = NULL;
 196         itp->hdr_len = WSM_TX_EXTRA_HEADROOM +
 197                         sizeof(struct ieee80211_hdr_3addr);
 198
 199         if (!debugfs_create_file("itp", S_IRUSR | S_IWUSR,
 200                                  priv->debug->debugfs_phy, priv, &fops_itp))
 201                 return -ENOMEM;
 202
 203         return 0;
 204 }
 205
 206 void cw1200_itp_release(struct cw1200_common *priv)
 207 {
 208         struct cw1200_itp *itp = &priv->debug->itp;
 209
 210         wait_event_interruptible(itp->close_wait,
 211                                  !atomic_read(&itp->open_count));
 212
 213         WARN_ON(atomic_read(&itp->open_count));
 214
 215         skb_queue_purge(&itp->log_queue);
 216         cw1200_itp_tx_stop(priv);
 217 }
 218
 219 static int __cw1200_itp_open(struct cw1200_common *priv)
 220 {
 221         struct cw1200_itp *itp = &priv->debug->itp;
 222
 223         if (!priv->vif)
 224                 return -EINVAL;
 225         if (priv->join_status)
 226                 return -EINVAL;
 227         itp->saved_channel = priv->channel;
 228         if (!priv->channel)
 229                 priv->channel = &priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ]->channels[0];
 230         wsm_set_bssid_filtering(priv, false);
 231         cw1200_itp_rx_reset(priv);
 232         return 0;
 233 }
 234
 235 static int __cw1200_itp_close(struct cw1200_common *priv)
 236 {
 237         struct cw1200_itp *itp = &priv->debug->itp;
 238         if (atomic_read(&itp->test_mode) == TEST_MODE_RX_TEST)
 239                 cw1200_itp_rx_stop(priv);
 240         cw1200_itp_tx_stop(priv);
 241         cw1200_disable_listening(priv);
 242         cw1200_update_filtering(priv);
 243         priv->channel = itp->saved_channel;
 244         return 0;
 245 }
 246
 247 bool cw1200_is_itp(struct cw1200_common *priv)
 248 {
 249         struct cw1200_itp *itp = &priv->debug->itp;
 250         return atomic_read(&itp->open_count) != 0;
 251 }
 252
 253 static void cw1200_itp_rx_reset(struct cw1200_common *priv)
 254 {
 255         struct cw1200_itp *itp = &priv->debug->itp;
 256         itp->rx_cnt = 0;
 257         itp->rx_rssi = 0;
 258         itp->rx_rssi_max = -1000;
 259         itp->rx_rssi_min = 1000;
 260 }
 261
 262 static void cw1200_itp_rx_start(struct cw1200_common *priv)
 263 {
 264         struct cw1200_itp *itp = &priv->debug->itp;
 265
 266         pr_debug("[ITP] RX start, band = %d, ch = %d\n",
 267                  itp->band, itp->ch);
 268         atomic_set(&itp->test_mode, TEST_MODE_RX_TEST);
 269         cw1200_update_listening(priv, false);
 270         priv->channel = &priv->hw->
 271                 wiphy->bands[itp->band]->channels[itp->ch];
 272         cw1200_update_listening(priv, true);
 273         wsm_set_bssid_filtering(priv, false);
 274 }
 275
 276 static void cw1200_itp_rx_stop(struct cw1200_common *priv)
 277 {
 278         struct cw1200_itp *itp = &priv->debug->itp;
 279         pr_debug("[ITP] RX stop\n");
 280         atomic_set(&itp->test_mode, TEST_MODE_NO_TEST);
 281         cw1200_itp_rx_reset(priv);
 282 }
 283
 284 static void cw1200_itp_rx_stats(struct cw1200_common *priv)
 285 {
 286         struct cw1200_itp *itp = &priv->debug->itp;
 287         struct sk_buff *skb;
 288         char buf[128];
 289         int len, ret;
 290         struct wsm_mib_counters_table counters;
 291
 292         ret = wsm_get_counters_table(priv, &counters);
 293
 294         if (ret)
 295                 cw1200_itp_err(priv, -EBUSY, 20);
 296
 297         if (!itp->rx_cnt)
 298                 len = snprintf(buf, sizeof(buf), "1,0,0,0,0,%d\n",
 299                                counters.rx_packet_errors);
 300         else
 301                 len = snprintf(buf, sizeof(buf), "1,%d,%ld,%d,%d,%d\n",
 302                                itp->rx_cnt,
 303                                itp->rx_cnt ? itp->rx_rssi / itp->rx_cnt : 0,
 304                                itp->rx_rssi_min, itp->rx_rssi_max,
 305                                counters.rx_packet_errors);
 306
 307         if (len <= 0) {
 308                 cw1200_itp_err(priv, -EBUSY, 21);
 309                 return;
 310         }
 311
 312         skb = dev_alloc_skb(len);
 313         if (!skb) {
 314                 cw1200_itp_err(priv, -ENOMEM, 22);
 315                 return;
 316         }
 317
 318         itp->rx_cnt = 0;
 319         itp->rx_rssi = 0;
 320         itp->rx_rssi_max = -1000;
 321         itp->rx_rssi_min = 1000;
 322
 323         skb_trim(skb, 0);
 324         skb_put(skb, len);
 325
 326         memcpy(skb->data, buf, len);
 327         skb_queue_tail(&itp->log_queue, skb);
 328         wake_up(&itp->read_wait);
 329 }
 330
 331 static void cw1200_itp_tx_start(struct cw1200_common *priv)
 332 {
 333         struct wsm_tx *tx;
 334         struct ieee80211_hdr_3addr *hdr;
 335         struct cw1200_itp *itp = &priv->debug->itp;
 336         struct wsm_mib_association_mode assoc_mode = {
 337                 .flags = WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE,
 338                 .preamble = itp->preamble,
 339         };
 340         int len;
 341         u8 da_addr[6] = ITP_DEFAULT_DA_ADDR;
 342
 343         /* Rates index 4 and 5 are not supported */
 344         if (itp->rate > 3)
 345                 itp->rate += 2;
 346
 347         pr_debug("[ITP] TX start: band = %d, ch = %d, rate = %d, preamble = %d, number = %d, data_mode = %d, interval = %d, power = %d, data_len = %d\n",
 348                  itp->band, itp->ch, itp->rate, itp->preamble,
 349                  itp->number, itp->data_mode, itp->interval_us,
 350                  itp->power, itp->data_len);
 351
 352         len = itp->hdr_len + itp->data_len;
 353
 354         itp->data = kmalloc(len, GFP_KERNEL);
 355         tx = (struct wsm_tx *)itp->data;
 356         tx->hdr.len = itp->data_len + itp->hdr_len;
 357         tx->hdr.id = __cpu_to_le16(0x0004 | 1 << 6);
 358         tx->max_tx_rate = itp->rate;
 359         tx->queue_id = 3;
 360         tx->more = 0;
 361         tx->flags = 0xc;
 362         tx->packet_id = 0x55ff55;
 363         tx->reserved = 0;
 364         tx->expire_time = 1;
 365
 366         if (itp->preamble == ITP_PREAMBLE_GREENFIELD)
 367                 tx->ht_tx_parameters = WSM_HT_TX_GREENFIELD;
 368         else if (itp->preamble == ITP_PREAMBLE_MIXED)
 369                 tx->ht_tx_parameters = WSM_HT_TX_MIXED;
 370
 371         hdr = (struct ieee80211_hdr_3addr *)&itp->data[sizeof(struct wsm_tx)];
 372         memset(hdr, 0, sizeof(*hdr));
 373         hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_FCTL_TODS);
 374         memcpy(hdr->addr1, da_addr, ETH_ALEN);
 375         memcpy(hdr->addr2, priv->vif->addr, ETH_ALEN);
 376         memcpy(hdr->addr3, da_addr, ETH_ALEN);
 377
 378         cw1200_itp_fill_pattern(&itp->data[itp->hdr_len],
 379                                 itp->data_len, itp->data_mode);
 380
 381         cw1200_update_listening(priv, false);
 382         priv->channel = &priv->hw->wiphy->bands[itp->band]->channels[itp->ch];
 383         WARN_ON(wsm_set_output_power(priv, itp->power));
 384         if (itp->preamble == ITP_PREAMBLE_SHORT ||
 385             itp->preamble == ITP_PREAMBLE_LONG)
 386                 WARN_ON(wsm_set_association_mode(priv,
 387                                                  &assoc_mode));
 388         wsm_set_bssid_filtering(priv, false);
 389         cw1200_update_listening(priv, true);
 390
 391         spin_lock_bh(&itp->tx_lock);
 392         atomic_set(&itp->test_mode, TEST_MODE_TX_TEST);
 393         atomic_set(&itp->awaiting_confirm, 0);
 394         atomic_set(&itp->stop_tx, 0);
 395         atomic_set(&priv->bh_tx, 1);
 396         ktime_get_ts(&itp->last_sent);
 397         wake_up(&priv->bh_wq);
 398         spin_unlock_bh(&itp->tx_lock);
 399 }
 400
 401 void __cw1200_itp_tx_stop(struct cw1200_common *priv)
 402 {
 403         struct cw1200_itp *itp = &priv->debug->itp;
 404         spin_lock_bh(&itp->tx_lock);
 405         kfree(itp->data);
 406         itp->data = NULL;
 407         atomic_set(&itp->test_mode, TEST_MODE_NO_TEST);
 408         spin_unlock_bh(&itp->tx_lock);
 409 }
 410
 411 static void cw1200_itp_tx_stop(struct cw1200_common *priv)
 412 {
 413         struct cw1200_itp *itp = &priv->debug->itp;
 414         pr_debug("[ITP] TX stop\n");
 415         atomic_set(&itp->stop_tx, 1);
 416         flush_workqueue(priv->workqueue);
 417
 418         /* time for FW to confirm all tx requests */
 419         msleep(500);
 420
 421         __cw1200_itp_tx_stop(priv);
 422 }
 423
 424 static int cw1200_print_fw_version(struct cw1200_common *priv,
 425                                    u8 *buf, size_t len)
 426 {
 427         return snprintf(buf, len, "%s %d.%d",
 428                         cw1200_fw_types[priv->wsm_caps.fw_type],
 429                         priv->wsm_caps.fw_ver,
 430                         priv->wsm_caps.fw_build);
 431 }
 432
 433 static void cw1200_itp_get_version(struct cw1200_common *priv,
 434                 enum cw1200_itp_version_type type)
 435 {
 436         struct cw1200_itp *itp = &priv->debug->itp;
 437         struct sk_buff *skb;
 438         char buf[ITP_BUF_SIZE];
 439         size_t size = 0;
 440         int len;
 441         pr_debug("[ITP] print %s version\n",
 442                  type == ITP_CHIP_ID ? "chip" : "firmware");
 443
 444         len = snprintf(buf, ITP_BUF_SIZE, "2,");
 445         if (len <= 0) {
 446                 cw1200_itp_err(priv, -EINVAL, 40);
 447                 return;
 448         }
 449         size += len;
 450
 451         switch (type) {
 452         case ITP_CHIP_ID:
 453                 len = cw1200_print_fw_version(priv, buf+size,
 454                                 ITP_BUF_SIZE - size);
 455
 456                 if (len <= 0) {
 457                         cw1200_itp_err(priv, -EINVAL, 41);
 458                         return;
 459                 }
 460                 size += len;
 461                 break;
 462         case ITP_FW_VER:
 463                 len = snprintf(buf+size, ITP_BUF_SIZE - size,
 464                                 "%d.%d", priv->wsm_caps.hw_id,
 465                                 priv->wsm_caps.hw_subid);
 466                 if (len <= 0) {
 467                         cw1200_itp_err(priv, -EINVAL, 42);
 468                         return;
 469                 }
 470                 size += len;
 471                 break;
 472         default:
 473                 cw1200_itp_err(priv, -EINVAL, 43);
 474                 break;
 475         }
 476
 477         len = snprintf(buf+size, ITP_BUF_SIZE-size, "\n");
 478         if (len <= 0) {
 479                 cw1200_itp_err(priv, -EINVAL, 44);
 480                 return;
 481         }
 482         size += len;
 483
 484         skb = dev_alloc_skb(size);
 485         if (!skb) {
 486                 cw1200_itp_err(priv, -ENOMEM, 45);
 487                 return;
 488         }
 489
 490         skb_trim(skb, 0);
 491         skb_put(skb, size);
 492
 493         memcpy(skb->data, buf, size);
 494         skb_queue_tail(&itp->log_queue, skb);
 495         wake_up(&itp->read_wait);
 496 }
 497
 498 int cw1200_itp_get_tx(struct cw1200_common *priv, u8 **data,
 499                 size_t *tx_len, int *burst)
 500 {
 501         struct cw1200_itp *itp;
 502         struct timespec now;
 503         int time_left_us;
 504
 505         if (!priv->debug)
 506                 return 0;
 507
 508         itp     = &priv->debug->itp;
 509
 510         if (!itp)
 511                 return 0;
 512
 513         spin_lock_bh(&itp->tx_lock);
 514         if (atomic_read(&itp->test_mode) != TEST_MODE_TX_TEST)
 515                 goto out;
 516
 517         if (atomic_read(&itp->stop_tx))
 518                 goto out;
 519
 520         if (itp->number == 0) {
 521                 atomic_set(&itp->stop_tx, 1);
 522                 queue_delayed_work(priv->workqueue, &itp->tx_finish, HZ/10);
 523                 goto out;
 524         }
 525
 526         if (!itp->data)
 527                 goto out;
 528
 529         if (priv->hw_bufs_used >= 2) {
 530                 if (!atomic_read(&priv->bh_rx))
 531                         atomic_set(&priv->bh_rx, 1);
 532                 atomic_set(&priv->bh_tx, 1);
 533                 goto out;
 534         }
 535
 536         ktime_get_ts(&now);
 537         time_left_us = (itp->last_sent.tv_sec - now.tv_sec)*1000000 +
 538                 (itp->last_sent.tv_nsec - now.tv_nsec)/1000 +
 539                 itp->interval_us;
 540
 541         if (time_left_us > ITP_TIME_THRES_US) {
 542                 queue_delayed_work(priv->workqueue, &itp->tx_work,
 543                                    ITP_US_TO_MS(time_left_us)*HZ/1000);
 544                 goto out;
 545         }
 546
 547         if (time_left_us > 50)
 548                 udelay(time_left_us);
 549
 550         if (itp->number > 0)
 551                 itp->number--;
 552
 553         *data = itp->data;
 554         *tx_len = itp->data_len + itp->hdr_len;
 555
 556         if (itp->data_mode == ITP_DATA_RANDOM)
 557                 cw1200_itp_fill_pattern(&itp->data[itp->hdr_len],
 558                                         itp->data_len, itp->data_mode);
 559         *burst = 2;
 560         atomic_set(&priv->bh_tx, 1);
 561         ktime_get_ts(&itp->last_sent);
 562         atomic_add(1, &itp->awaiting_confirm);
 563         spin_unlock_bh(&itp->tx_lock);
 564         return 1;
 565
 566 out:
 567         spin_unlock_bh(&itp->tx_lock);
 568         return 0;
 569 }
 570
 571 bool cw1200_itp_rxed(struct cw1200_common *priv, struct sk_buff *skb)
 572 {
 573         struct cw1200_itp *itp = &priv->debug->itp;
 574         struct ieee80211_rx_status *rx = IEEE80211_SKB_RXCB(skb);
 575         int signal;
 576
 577         if (atomic_read(&itp->test_mode) != TEST_MODE_RX_TEST)
 578                 return cw1200_is_itp(priv);
 579         if (rx->freq != priv->channel->center_freq)
 580                 return true;
 581
 582         signal = rx->signal;
 583         itp->rx_cnt++;
 584         itp->rx_rssi += signal;
 585         if (itp->rx_rssi_min > rx->signal)
 586                 itp->rx_rssi_min = rx->signal;
 587         if (itp->rx_rssi_max < rx->signal)
 588                 itp->rx_rssi_max = rx->signal;
 589
 590         return true;
 591 }
 592
 593 void cw1200_itp_wake_up_tx(struct cw1200_common *priv)
 594 {
 595         wake_up(&priv->debug->itp.write_wait);
 596 }
 597
 598 bool cw1200_itp_tx_running(struct cw1200_common *priv)
 599 {
 600         if (atomic_read(&priv->debug->itp.awaiting_confirm) ||
 601             atomic_read(&priv->debug->itp.test_mode) ==
 602             TEST_MODE_TX_TEST) {
 603                 atomic_sub(1, &priv->debug->itp.awaiting_confirm);
 604                 return true;
 605         }
 606         return false;
 607 }
 608
 609 static void cw1200_itp_handle(struct cw1200_common *priv,
 610                               struct sk_buff *skb)
 611 {
 612         struct cw1200_itp *itp = &priv->debug->itp;
 613         const struct wiphy *wiphy = priv->hw->wiphy;
 614         int cmd;
 615         int ret;
 616
 617         pr_debug("[ITP] <<< %s", skb->data);
 618         if (sscanf(skb->data, "%d", &cmd) != 1) {
 619                 cw1200_itp_err(priv, -EINVAL, 1);
 620                 return;
 621         }
 622
 623         switch (cmd) {
 624         case 1: /* RX test */
 625                 if (atomic_read(&itp->test_mode)) {
 626                         cw1200_itp_err(priv, -EBUSY, 0);
 627                         return;
 628                 }
 629                 ret = sscanf(skb->data, "%d,%d,%d",
 630                                 &cmd, &itp->band, &itp->ch);
 631                 if (ret != 3) {
 632                         cw1200_itp_err(priv, -EINVAL, ret + 1);
 633                         return;
 634                 }
 635                 if (itp->band >= 2) {
 636                         cw1200_itp_err(priv, -EINVAL, 2);
 637                 } else if (!wiphy->bands[itp->band]) {
 638                         cw1200_itp_err(priv, -EINVAL, 2);
 639                 } else if (itp->ch >= wiphy->bands[itp->band]->n_channels) {
 640                         cw1200_itp_err(priv, -EINVAL, 3);
 641                 } else {
 642                         cw1200_itp_rx_stats(priv);
 643                         cw1200_itp_rx_start(priv);
 644                 }
 645                 break;
 646         case 2: /* RX stat */
 647                 cw1200_itp_rx_stats(priv);
 648                 break;
 649         case 3: /* RX/TX stop */
 650                 if (atomic_read(&itp->test_mode) == TEST_MODE_RX_TEST) {
 651                         cw1200_itp_rx_stats(priv);
 652                         cw1200_itp_rx_stop(priv);
 653                 } else if (atomic_read(&itp->test_mode) == TEST_MODE_TX_TEST) {
 654                         cw1200_itp_tx_stop(priv);
 655                 } else {
 656                         cw1200_itp_err(priv, -EBUSY, 0);
 657                 }
 658                 break;
 659         case 4: /* TX start */
 660                 if (atomic_read(&itp->test_mode) != TEST_MODE_NO_TEST) {
 661                         cw1200_itp_err(priv, -EBUSY, 0);
 662                         return;
 663                 }
 664                 ret = sscanf(skb->data, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
 665                                 &cmd, &itp->band, &itp->ch, &itp->rate,
 666                                 &itp->preamble, &itp->number, &itp->data_mode,
 667                                 &itp->interval_us, &itp->power, &itp->data_len);
 668                 if (ret != 10) {
 669                         cw1200_itp_err(priv, -EINVAL, ret + 1);
 670                         return;
 671                 }
 672                 if (itp->band >= 2) {
 673                         cw1200_itp_err(priv, -EINVAL, 2);
 674                 } else if (!wiphy->bands[itp->band]) {
 675                         cw1200_itp_err(priv, -EINVAL, 2);
 676                 } else if (itp->ch >= wiphy->bands[itp->band]->n_channels) {
 677                         cw1200_itp_err(priv, -EINVAL, 3);
 678                 } else if (itp->rate >= 20) {
 679                         cw1200_itp_err(priv, -EINVAL, 4);
 680                 } else if (itp->preamble >= ITP_PREAMBLE_MAX) {
 681                         cw1200_itp_err(priv, -EINVAL, 5);
 682                 } else if (itp->data_mode >= ITP_DATA_MAX_MODE) {
 683                         cw1200_itp_err(priv, -EINVAL, 7);
 684                 } else if (itp->data_len < ITP_MIN_DATA_SIZE ||
 685                            itp->data_len > (priv->wsm_caps.input_buffer_size - itp->hdr_len)) {
 686                         cw1200_itp_err(priv, -EINVAL, 8);
 687                 } else {
 688                     cw1200_itp_tx_start(priv);
 689                 }
 690                 break;
 691         case 5:
 692                 cw1200_itp_get_version(priv, ITP_CHIP_ID);
 693                 break;
 694         case 6:
 695                 cw1200_itp_get_version(priv, ITP_FW_VER);
 696                 break;
 697         }
 698 }
 699
 700 static void cw1200_itp_err(struct cw1200_common *priv,
 701                            int err, int arg)
 702 {
 703         struct cw1200_itp *itp = &priv->debug->itp;
 704         struct sk_buff *skb;
 705         static char buf[255];
 706         int len;
 707
 708         len = snprintf(buf, sizeof(buf), "%d,%d\n",
 709                 err, arg);
 710         if (len <= 0)
 711                 return;
 712
 713         skb = dev_alloc_skb(len);
 714         if (!skb)
 715                 return;
 716
 717         skb_trim(skb, 0);
 718         skb_put(skb, len);
 719
 720         memcpy(skb->data, buf, len);
 721         skb_queue_tail(&itp->log_queue, skb);
 722         wake_up(&itp->read_wait);
 723
 724         len = sprint_symbol(buf,
 725                         (unsigned long)__builtin_return_address(0));
 726         if (len <= 0)
 727                 return;
 728         pr_debug("[ITP] error %d,%d from %s\n",
 729                  err, arg, buf);
 730 }