drivers/net/wireless/st/cw1200/wsm.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * WSM host interface (HI) implementation for
   4  * ST-Ericsson CW1200 mac80211 drivers.
   5  *
   6  * Copyright (c) 2010, ST-Ericsson
   7  * Author: Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
   8  */
   9
  10 #include <linux/skbuff.h>
  11 #include <linux/wait.h>
  12 #include <linux/delay.h>
  13 #include <linux/sched.h>
  14 #include <linux/random.h>
  15
  16 #include "cw1200.h"
  17 #include "wsm.h"
  18 #include "bh.h"
  19 #include "sta.h"
  20 #include "debug.h"
  21
  22 #define WSM_CMD_TIMEOUT         (2 * HZ) /* With respect to interrupt loss */
  23 #define WSM_CMD_START_TIMEOUT   (7 * HZ)
  24 #define WSM_CMD_RESET_TIMEOUT   (3 * HZ) /* 2 sec. timeout was observed.   */
  25 #define WSM_CMD_MAX_TIMEOUT     (3 * HZ)
  26
  27 #define WSM_SKIP(buf, size)                                             \
  28         do {                                                            \
  29                 if ((buf)->data + size > (buf)->end)                    \
  30                         goto underflow;                                 \
  31                 (buf)->data += size;                                    \
  32         } while (0)
  33
  34 #define WSM_GET(buf, ptr, size)                                         \
  35         do {                                                            \
  36                 if ((buf)->data + size > (buf)->end)                    \
  37                         goto underflow;                                 \
  38                 memcpy(ptr, (buf)->data, size);                         \
  39                 (buf)->data += size;                                    \
  40         } while (0)
  41
  42 #define __WSM_GET(buf, type, type2, cvt)                                \
  43         ({                                                              \
  44                 type val;                                               \
  45                 if ((buf)->data + sizeof(type) > (buf)->end)            \
  46                         goto underflow;                                 \
  47                 val = cvt(*(type2 *)(buf)->data);                       \
  48                 (buf)->data += sizeof(type);                            \
  49                 val;                                                    \
  50         })
  51
  52 #define WSM_GET8(buf)  __WSM_GET(buf, u8, u8, (u8))
  53 #define WSM_GET16(buf) __WSM_GET(buf, u16, __le16, __le16_to_cpu)
  54 #define WSM_GET32(buf) __WSM_GET(buf, u32, __le32, __le32_to_cpu)
  55
  56 #define WSM_PUT(buf, ptr, size)                                         \
  57         do {                                                            \
  58                 if ((buf)->data + size > (buf)->end)            \
  59                         if (wsm_buf_reserve((buf), size))       \
  60                                 goto nomem;                             \
  61                 memcpy((buf)->data, ptr, size);                         \
  62                 (buf)->data += size;                                    \
  63         } while (0)
  64
  65 #define __WSM_PUT(buf, val, type, type2, cvt)                           \
  66         do {                                                            \
  67                 if ((buf)->data + sizeof(type) > (buf)->end)            \
  68                         if (wsm_buf_reserve((buf), sizeof(type))) \
  69                                 goto nomem;                             \
  70                 *(type2 *)(buf)->data = cvt(val);                       \
  71                 (buf)->data += sizeof(type);                            \
  72         } while (0)
  73
  74 #define WSM_PUT8(buf, val)  __WSM_PUT(buf, val, u8, u8, (u8))
  75 #define WSM_PUT16(buf, val) __WSM_PUT(buf, val, u16, __le16, __cpu_to_le16)
  76 #define WSM_PUT32(buf, val) __WSM_PUT(buf, val, u32, __le32, __cpu_to_le32)
  77
  78 static void wsm_buf_reset(struct wsm_buf *buf);
  79 static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size);
  80
  81 static int wsm_cmd_send(struct cw1200_common *priv,
  82                         struct wsm_buf *buf,
  83                         void *arg, u16 cmd, long tmo);
  84
  85 #define wsm_cmd_lock(__priv) mutex_lock(&((__priv)->wsm_cmd_mux))
  86 #define wsm_cmd_unlock(__priv) mutex_unlock(&((__priv)->wsm_cmd_mux))
  87
  88 /* ******************************************************************** */
  89 /* WSM API implementation                                               */
  90
  91 static int wsm_generic_confirm(struct cw1200_common *priv,
  92                              void *arg,
  93                              struct wsm_buf *buf)
  94 {
  95         u32 status = WSM_GET32(buf);
  96         if (status != WSM_STATUS_SUCCESS)
  97                 return -EINVAL;
  98         return 0;
  99
 100 underflow:
 101         WARN_ON(1);
 102         return -EINVAL;
 103 }
 104
 105 int wsm_configuration(struct cw1200_common *priv, struct wsm_configuration *arg)
 106 {
 107         int ret;
 108         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 109
 110         wsm_cmd_lock(priv);
 111
 112         WSM_PUT32(buf, arg->dot11MaxTransmitMsduLifeTime);
 113         WSM_PUT32(buf, arg->dot11MaxReceiveLifeTime);
 114         WSM_PUT32(buf, arg->dot11RtsThreshold);
 115
 116         /* DPD block. */
 117         WSM_PUT16(buf, arg->dpdData_size + 12);
 118         WSM_PUT16(buf, 1); /* DPD version */
 119         WSM_PUT(buf, arg->dot11StationId, ETH_ALEN);
 120         WSM_PUT16(buf, 5); /* DPD flags */
 121         WSM_PUT(buf, arg->dpdData, arg->dpdData_size);
 122
 123         ret = wsm_cmd_send(priv, buf, arg,
 124                            WSM_CONFIGURATION_REQ_ID, WSM_CMD_TIMEOUT);
 125
 126         wsm_cmd_unlock(priv);
 127         return ret;
 128
 129 nomem:
 130         wsm_cmd_unlock(priv);
 131         return -ENOMEM;
 132 }
 133
 134 static int wsm_configuration_confirm(struct cw1200_common *priv,
 135                                      struct wsm_configuration *arg,
 136                                      struct wsm_buf *buf)
 137 {
 138         int i;
 139         int status;
 140
 141         status = WSM_GET32(buf);
 142         if (WARN_ON(status != WSM_STATUS_SUCCESS))
 143                 return -EINVAL;
 144
 145         WSM_GET(buf, arg->dot11StationId, ETH_ALEN);
 146         arg->dot11FrequencyBandsSupported = WSM_GET8(buf);
 147         WSM_SKIP(buf, 1);
 148         arg->supportedRateMask = WSM_GET32(buf);
 149         for (i = 0; i < 2; ++i) {
 150                 arg->txPowerRange[i].min_power_level = WSM_GET32(buf);
 151                 arg->txPowerRange[i].max_power_level = WSM_GET32(buf);
 152                 arg->txPowerRange[i].stepping = WSM_GET32(buf);
 153         }
 154         return 0;
 155
 156 underflow:
 157         WARN_ON(1);
 158         return -EINVAL;
 159 }
 160
 161 /* ******************************************************************** */
 162
 163 int wsm_reset(struct cw1200_common *priv, const struct wsm_reset *arg)
 164 {
 165         int ret;
 166         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 167         u16 cmd = WSM_RESET_REQ_ID | WSM_TX_LINK_ID(arg->link_id);
 168
 169         wsm_cmd_lock(priv);
 170
 171         WSM_PUT32(buf, arg->reset_statistics ? 0 : 1);
 172         ret = wsm_cmd_send(priv, buf, NULL, cmd, WSM_CMD_RESET_TIMEOUT);
 173         wsm_cmd_unlock(priv);
 174         return ret;
 175
 176 nomem:
 177         wsm_cmd_unlock(priv);
 178         return -ENOMEM;
 179 }
 180
 181 /* ******************************************************************** */
 182
 183 struct wsm_mib {
 184         u16 mib_id;
 185         void *buf;
 186         size_t buf_size;
 187 };
 188
 189 int wsm_read_mib(struct cw1200_common *priv, u16 mib_id, void *_buf,
 190                         size_t buf_size)
 191 {
 192         int ret;
 193         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 194         struct wsm_mib mib_buf = {
 195                 .mib_id = mib_id,
 196                 .buf = _buf,
 197                 .buf_size = buf_size,
 198         };
 199         wsm_cmd_lock(priv);
 200
 201         WSM_PUT16(buf, mib_id);
 202         WSM_PUT16(buf, 0);
 203
 204         ret = wsm_cmd_send(priv, buf, &mib_buf,
 205                            WSM_READ_MIB_REQ_ID, WSM_CMD_TIMEOUT);
 206         wsm_cmd_unlock(priv);
 207         return ret;
 208
 209 nomem:
 210         wsm_cmd_unlock(priv);
 211         return -ENOMEM;
 212 }
 213
 214 static int wsm_read_mib_confirm(struct cw1200_common *priv,
 215                                 struct wsm_mib *arg,
 216                                 struct wsm_buf *buf)
 217 {
 218         u16 size;
 219         if (WARN_ON(WSM_GET32(buf) != WSM_STATUS_SUCCESS))
 220                 return -EINVAL;
 221
 222         if (WARN_ON(WSM_GET16(buf) != arg->mib_id))
 223                 return -EINVAL;
 224
 225         size = WSM_GET16(buf);
 226         if (size > arg->buf_size)
 227                 size = arg->buf_size;
 228
 229         WSM_GET(buf, arg->buf, size);
 230         arg->buf_size = size;
 231         return 0;
 232
 233 underflow:
 234         WARN_ON(1);
 235         return -EINVAL;
 236 }
 237
 238 /* ******************************************************************** */
 239
 240 int wsm_write_mib(struct cw1200_common *priv, u16 mib_id, void *_buf,
 241                         size_t buf_size)
 242 {
 243         int ret;
 244         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 245         struct wsm_mib mib_buf = {
 246                 .mib_id = mib_id,
 247                 .buf = _buf,
 248                 .buf_size = buf_size,
 249         };
 250
 251         wsm_cmd_lock(priv);
 252
 253         WSM_PUT16(buf, mib_id);
 254         WSM_PUT16(buf, buf_size);
 255         WSM_PUT(buf, _buf, buf_size);
 256
 257         ret = wsm_cmd_send(priv, buf, &mib_buf,
 258                            WSM_WRITE_MIB_REQ_ID, WSM_CMD_TIMEOUT);
 259         wsm_cmd_unlock(priv);
 260         return ret;
 261
 262 nomem:
 263         wsm_cmd_unlock(priv);
 264         return -ENOMEM;
 265 }
 266
 267 static int wsm_write_mib_confirm(struct cw1200_common *priv,
 268                                 struct wsm_mib *arg,
 269                                 struct wsm_buf *buf)
 270 {
 271         int ret;
 272
 273         ret = wsm_generic_confirm(priv, arg, buf);
 274         if (ret)
 275                 return ret;
 276
 277         if (arg->mib_id == WSM_MIB_ID_OPERATIONAL_POWER_MODE) {
 278                 /* OperationalMode: update PM status. */
 279                 const char *p = arg->buf;
 280                 cw1200_enable_powersave(priv, (p[0] & 0x0F) ? true : false);
 281         }
 282         return 0;
 283 }
 284
 285 /* ******************************************************************** */
 286
 287 int wsm_scan(struct cw1200_common *priv, const struct wsm_scan *arg)
 288 {
 289         int i;
 290         int ret;
 291         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 292
 293         if (arg->num_channels > 48)
 294                 return -EINVAL;
 295
 296         if (arg->num_ssids > 2)
 297                 return -EINVAL;
 298
 299         if (arg->band > 1)
 300                 return -EINVAL;
 301
 302         wsm_cmd_lock(priv);
 303
 304         WSM_PUT8(buf, arg->band);
 305         WSM_PUT8(buf, arg->type);
 306         WSM_PUT8(buf, arg->flags);
 307         WSM_PUT8(buf, arg->max_tx_rate);
 308         WSM_PUT32(buf, arg->auto_scan_interval);
 309         WSM_PUT8(buf, arg->num_probes);
 310         WSM_PUT8(buf, arg->num_channels);
 311         WSM_PUT8(buf, arg->num_ssids);
 312         WSM_PUT8(buf, arg->probe_delay);
 313
 314         for (i = 0; i < arg->num_channels; ++i) {
 315                 WSM_PUT16(buf, arg->ch[i].number);
 316                 WSM_PUT16(buf, 0);
 317                 WSM_PUT32(buf, arg->ch[i].min_chan_time);
 318                 WSM_PUT32(buf, arg->ch[i].max_chan_time);
 319                 WSM_PUT32(buf, 0);
 320         }
 321
 322         for (i = 0; i < arg->num_ssids; ++i) {
 323                 WSM_PUT32(buf, arg->ssids[i].length);
 324                 WSM_PUT(buf, &arg->ssids[i].ssid[0],
 325                         sizeof(arg->ssids[i].ssid));
 326         }
 327
 328         ret = wsm_cmd_send(priv, buf, NULL,
 329                            WSM_START_SCAN_REQ_ID, WSM_CMD_TIMEOUT);
 330         wsm_cmd_unlock(priv);
 331         return ret;
 332
 333 nomem:
 334         wsm_cmd_unlock(priv);
 335         return -ENOMEM;
 336 }
 337
 338 /* ******************************************************************** */
 339
 340 int wsm_stop_scan(struct cw1200_common *priv)
 341 {
 342         int ret;
 343         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 344         wsm_cmd_lock(priv);
 345         ret = wsm_cmd_send(priv, buf, NULL,
 346                            WSM_STOP_SCAN_REQ_ID, WSM_CMD_TIMEOUT);
 347         wsm_cmd_unlock(priv);
 348         return ret;
 349 }
 350
 351
 352 static int wsm_tx_confirm(struct cw1200_common *priv,
 353                           struct wsm_buf *buf,
 354                           int link_id)
 355 {
 356         struct wsm_tx_confirm tx_confirm;
 357
 358         tx_confirm.packet_id = WSM_GET32(buf);
 359         tx_confirm.status = WSM_GET32(buf);
 360         tx_confirm.tx_rate = WSM_GET8(buf);
 361         tx_confirm.ack_failures = WSM_GET8(buf);
 362         tx_confirm.flags = WSM_GET16(buf);
 363         tx_confirm.media_delay = WSM_GET32(buf);
 364         tx_confirm.tx_queue_delay = WSM_GET32(buf);
 365
 366         cw1200_tx_confirm_cb(priv, link_id, &tx_confirm);
 367         return 0;
 368
 369 underflow:
 370         WARN_ON(1);
 371         return -EINVAL;
 372 }
 373
 374 static int wsm_multi_tx_confirm(struct cw1200_common *priv,
 375                                 struct wsm_buf *buf, int link_id)
 376 {
 377         int ret;
 378         int count;
 379
 380         count = WSM_GET32(buf);
 381         if (WARN_ON(count <= 0))
 382                 return -EINVAL;
 383
 384         if (count > 1) {
 385                 /* We already released one buffer, now for the rest */
 386                 ret = wsm_release_tx_buffer(priv, count - 1);
 387                 if (ret < 0)
 388                         return ret;
 389                 else if (ret > 0)
 390                         cw1200_bh_wakeup(priv);
 391         }
 392
 393         cw1200_debug_txed_multi(priv, count);
 394         do {
 395                 ret = wsm_tx_confirm(priv, buf, link_id);
 396         } while (!ret && --count);
 397
 398         return ret;
 399
 400 underflow:
 401         WARN_ON(1);
 402         return -EINVAL;
 403 }
 404
 405 /* ******************************************************************** */
 406
 407 static int wsm_join_confirm(struct cw1200_common *priv,
 408                             struct wsm_join_cnf *arg,
 409                             struct wsm_buf *buf)
 410 {
 411         arg->status = WSM_GET32(buf);
 412         if (WARN_ON(arg->status) != WSM_STATUS_SUCCESS)
 413                 return -EINVAL;
 414
 415         arg->min_power_level = WSM_GET32(buf);
 416         arg->max_power_level = WSM_GET32(buf);
 417
 418         return 0;
 419
 420 underflow:
 421         WARN_ON(1);
 422         return -EINVAL;
 423 }
 424
 425 int wsm_join(struct cw1200_common *priv, struct wsm_join *arg)
 426 {
 427         int ret;
 428         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 429         struct wsm_join_cnf resp;
 430         wsm_cmd_lock(priv);
 431
 432         WSM_PUT8(buf, arg->mode);
 433         WSM_PUT8(buf, arg->band);
 434         WSM_PUT16(buf, arg->channel_number);
 435         WSM_PUT(buf, &arg->bssid[0], sizeof(arg->bssid));
 436         WSM_PUT16(buf, arg->atim_window);
 437         WSM_PUT8(buf, arg->preamble_type);
 438         WSM_PUT8(buf, arg->probe_for_join);
 439         WSM_PUT8(buf, arg->dtim_period);
 440         WSM_PUT8(buf, arg->flags);
 441         WSM_PUT32(buf, arg->ssid_len);
 442         WSM_PUT(buf, &arg->ssid[0], sizeof(arg->ssid));
 443         WSM_PUT32(buf, arg->beacon_interval);
 444         WSM_PUT32(buf, arg->basic_rate_set);
 445
 446         priv->tx_burst_idx = -1;
 447         ret = wsm_cmd_send(priv, buf, &resp,
 448                            WSM_JOIN_REQ_ID, WSM_CMD_TIMEOUT);
 449         /* TODO:  Update state based on resp.min|max_power_level */
 450
 451         priv->join_complete_status = resp.status;
 452
 453         wsm_cmd_unlock(priv);
 454         return ret;
 455
 456 nomem:
 457         wsm_cmd_unlock(priv);
 458         return -ENOMEM;
 459 }
 460
 461 /* ******************************************************************** */
 462
 463 int wsm_set_bss_params(struct cw1200_common *priv,
 464                        const struct wsm_set_bss_params *arg)
 465 {
 466         int ret;
 467         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 468
 469         wsm_cmd_lock(priv);
 470
 471         WSM_PUT8(buf, (arg->reset_beacon_loss ?  0x1 : 0));
 472         WSM_PUT8(buf, arg->beacon_lost_count);
 473         WSM_PUT16(buf, arg->aid);
 474         WSM_PUT32(buf, arg->operational_rate_set);
 475
 476         ret = wsm_cmd_send(priv, buf, NULL,
 477                            WSM_SET_BSS_PARAMS_REQ_ID, WSM_CMD_TIMEOUT);
 478
 479         wsm_cmd_unlock(priv);
 480         return ret;
 481
 482 nomem:
 483         wsm_cmd_unlock(priv);
 484         return -ENOMEM;
 485 }
 486
 487 /* ******************************************************************** */
 488
 489 int wsm_add_key(struct cw1200_common *priv, const struct wsm_add_key *arg)
 490 {
 491         int ret;
 492         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 493
 494         wsm_cmd_lock(priv);
 495
 496         WSM_PUT(buf, arg, sizeof(*arg));
 497
 498         ret = wsm_cmd_send(priv, buf, NULL,
 499                            WSM_ADD_KEY_REQ_ID, WSM_CMD_TIMEOUT);
 500
 501         wsm_cmd_unlock(priv);
 502         return ret;
 503
 504 nomem:
 505         wsm_cmd_unlock(priv);
 506         return -ENOMEM;
 507 }
 508
 509 /* ******************************************************************** */
 510
 511 int wsm_remove_key(struct cw1200_common *priv, const struct wsm_remove_key *arg)
 512 {
 513         int ret;
 514         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 515
 516         wsm_cmd_lock(priv);
 517
 518         WSM_PUT8(buf, arg->index);
 519         WSM_PUT8(buf, 0);
 520         WSM_PUT16(buf, 0);
 521
 522         ret = wsm_cmd_send(priv, buf, NULL,
 523                            WSM_REMOVE_KEY_REQ_ID, WSM_CMD_TIMEOUT);
 524
 525         wsm_cmd_unlock(priv);
 526         return ret;
 527
 528 nomem:
 529         wsm_cmd_unlock(priv);
 530         return -ENOMEM;
 531 }
 532
 533 /* ******************************************************************** */
 534
 535 int wsm_set_tx_queue_params(struct cw1200_common *priv,
 536                 const struct wsm_set_tx_queue_params *arg, u8 id)
 537 {
 538         int ret;
 539         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 540         u8 queue_id_to_wmm_aci[] = {3, 2, 0, 1};
 541
 542         wsm_cmd_lock(priv);
 543
 544         WSM_PUT8(buf, queue_id_to_wmm_aci[id]);
 545         WSM_PUT8(buf, 0);
 546         WSM_PUT8(buf, arg->ackPolicy);
 547         WSM_PUT8(buf, 0);
 548         WSM_PUT32(buf, arg->maxTransmitLifetime);
 549         WSM_PUT16(buf, arg->allowedMediumTime);
 550         WSM_PUT16(buf, 0);
 551
 552         ret = wsm_cmd_send(priv, buf, NULL, 0x0012, WSM_CMD_TIMEOUT);
 553
 554         wsm_cmd_unlock(priv);
 555         return ret;
 556
 557 nomem:
 558         wsm_cmd_unlock(priv);
 559         return -ENOMEM;
 560 }
 561
 562 /* ******************************************************************** */
 563
 564 int wsm_set_edca_params(struct cw1200_common *priv,
 565                                 const struct wsm_edca_params *arg)
 566 {
 567         int ret;
 568         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 569
 570         wsm_cmd_lock(priv);
 571
 572         /* Implemented according to specification. */
 573
 574         WSM_PUT16(buf, arg->params[3].cwmin);
 575         WSM_PUT16(buf, arg->params[2].cwmin);
 576         WSM_PUT16(buf, arg->params[1].cwmin);
 577         WSM_PUT16(buf, arg->params[0].cwmin);
 578
 579         WSM_PUT16(buf, arg->params[3].cwmax);
 580         WSM_PUT16(buf, arg->params[2].cwmax);
 581         WSM_PUT16(buf, arg->params[1].cwmax);
 582         WSM_PUT16(buf, arg->params[0].cwmax);
 583
 584         WSM_PUT8(buf, arg->params[3].aifns);
 585         WSM_PUT8(buf, arg->params[2].aifns);
 586         WSM_PUT8(buf, arg->params[1].aifns);
 587         WSM_PUT8(buf, arg->params[0].aifns);
 588
 589         WSM_PUT16(buf, arg->params[3].txop_limit);
 590         WSM_PUT16(buf, arg->params[2].txop_limit);
 591         WSM_PUT16(buf, arg->params[1].txop_limit);
 592         WSM_PUT16(buf, arg->params[0].txop_limit);
 593
 594         WSM_PUT32(buf, arg->params[3].max_rx_lifetime);
 595         WSM_PUT32(buf, arg->params[2].max_rx_lifetime);
 596         WSM_PUT32(buf, arg->params[1].max_rx_lifetime);
 597         WSM_PUT32(buf, arg->params[0].max_rx_lifetime);
 598
 599         ret = wsm_cmd_send(priv, buf, NULL,
 600                            WSM_EDCA_PARAMS_REQ_ID, WSM_CMD_TIMEOUT);
 601         wsm_cmd_unlock(priv);
 602         return ret;
 603
 604 nomem:
 605         wsm_cmd_unlock(priv);
 606         return -ENOMEM;
 607 }
 608
 609 /* ******************************************************************** */
 610
 611 int wsm_switch_channel(struct cw1200_common *priv,
 612                         const struct wsm_switch_channel *arg)
 613 {
 614         int ret;
 615         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 616
 617         wsm_cmd_lock(priv);
 618
 619         WSM_PUT8(buf, arg->mode);
 620         WSM_PUT8(buf, arg->switch_count);
 621         WSM_PUT16(buf, arg->channel_number);
 622
 623         priv->channel_switch_in_progress = 1;
 624
 625         ret = wsm_cmd_send(priv, buf, NULL,
 626                            WSM_SWITCH_CHANNEL_REQ_ID, WSM_CMD_TIMEOUT);
 627         if (ret)
 628                 priv->channel_switch_in_progress = 0;
 629
 630         wsm_cmd_unlock(priv);
 631         return ret;
 632
 633 nomem:
 634         wsm_cmd_unlock(priv);
 635         return -ENOMEM;
 636 }
 637
 638 /* ******************************************************************** */
 639
 640 int wsm_set_pm(struct cw1200_common *priv, const struct wsm_set_pm *arg)
 641 {
 642         int ret;
 643         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 644         priv->ps_mode_switch_in_progress = 1;
 645
 646         wsm_cmd_lock(priv);
 647
 648         WSM_PUT8(buf, arg->mode);
 649         WSM_PUT8(buf, arg->fast_psm_idle_period);
 650         WSM_PUT8(buf, arg->ap_psm_change_period);
 651         WSM_PUT8(buf, arg->min_auto_pspoll_period);
 652
 653         ret = wsm_cmd_send(priv, buf, NULL,
 654                            WSM_SET_PM_REQ_ID, WSM_CMD_TIMEOUT);
 655
 656         wsm_cmd_unlock(priv);
 657         return ret;
 658
 659 nomem:
 660         wsm_cmd_unlock(priv);
 661         return -ENOMEM;
 662 }
 663
 664 /* ******************************************************************** */
 665
 666 int wsm_start(struct cw1200_common *priv, const struct wsm_start *arg)
 667 {
 668         int ret;
 669         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 670
 671         wsm_cmd_lock(priv);
 672
 673         WSM_PUT8(buf, arg->mode);
 674         WSM_PUT8(buf, arg->band);
 675         WSM_PUT16(buf, arg->channel_number);
 676         WSM_PUT32(buf, arg->ct_window);
 677         WSM_PUT32(buf, arg->beacon_interval);
 678         WSM_PUT8(buf, arg->dtim_period);
 679         WSM_PUT8(buf, arg->preamble);
 680         WSM_PUT8(buf, arg->probe_delay);
 681         WSM_PUT8(buf, arg->ssid_len);
 682         WSM_PUT(buf, arg->ssid, sizeof(arg->ssid));
 683         WSM_PUT32(buf, arg->basic_rate_set);
 684
 685         priv->tx_burst_idx = -1;
 686         ret = wsm_cmd_send(priv, buf, NULL,
 687                            WSM_START_REQ_ID, WSM_CMD_START_TIMEOUT);
 688
 689         wsm_cmd_unlock(priv);
 690         return ret;
 691
 692 nomem:
 693         wsm_cmd_unlock(priv);
 694         return -ENOMEM;
 695 }
 696
 697 /* ******************************************************************** */
 698
 699 int wsm_beacon_transmit(struct cw1200_common *priv,
 700                         const struct wsm_beacon_transmit *arg)
 701 {
 702         int ret;
 703         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 704
 705         wsm_cmd_lock(priv);
 706
 707         WSM_PUT32(buf, arg->enable_beaconing ? 1 : 0);
 708
 709         ret = wsm_cmd_send(priv, buf, NULL,
 710                            WSM_BEACON_TRANSMIT_REQ_ID, WSM_CMD_TIMEOUT);
 711
 712         wsm_cmd_unlock(priv);
 713         return ret;
 714
 715 nomem:
 716         wsm_cmd_unlock(priv);
 717         return -ENOMEM;
 718 }
 719
 720 /* ******************************************************************** */
 721
 722 int wsm_start_find(struct cw1200_common *priv)
 723 {
 724         int ret;
 725         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 726
 727         wsm_cmd_lock(priv);
 728         ret = wsm_cmd_send(priv, buf, NULL, 0x0019, WSM_CMD_TIMEOUT);
 729         wsm_cmd_unlock(priv);
 730         return ret;
 731 }
 732
 733 /* ******************************************************************** */
 734
 735 int wsm_stop_find(struct cw1200_common *priv)
 736 {
 737         int ret;
 738         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 739
 740         wsm_cmd_lock(priv);
 741         ret = wsm_cmd_send(priv, buf, NULL, 0x001A, WSM_CMD_TIMEOUT);
 742         wsm_cmd_unlock(priv);
 743         return ret;
 744 }
 745
 746 /* ******************************************************************** */
 747
 748 int wsm_map_link(struct cw1200_common *priv, const struct wsm_map_link *arg)
 749 {
 750         int ret;
 751         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 752         u16 cmd = 0x001C | WSM_TX_LINK_ID(arg->link_id);
 753
 754         wsm_cmd_lock(priv);
 755
 756         WSM_PUT(buf, &arg->mac_addr[0], sizeof(arg->mac_addr));
 757         WSM_PUT16(buf, 0);
 758
 759         ret = wsm_cmd_send(priv, buf, NULL, cmd, WSM_CMD_TIMEOUT);
 760
 761         wsm_cmd_unlock(priv);
 762         return ret;
 763
 764 nomem:
 765         wsm_cmd_unlock(priv);
 766         return -ENOMEM;
 767 }
 768
 769 /* ******************************************************************** */
 770
 771 int wsm_update_ie(struct cw1200_common *priv,
 772                   const struct wsm_update_ie *arg)
 773 {
 774         int ret;
 775         struct wsm_buf *buf = &priv->wsm_cmd_buf;
 776
 777         wsm_cmd_lock(priv);
 778
 779         WSM_PUT16(buf, arg->what);
 780         WSM_PUT16(buf, arg->count);
 781         WSM_PUT(buf, arg->ies, arg->length);
 782
 783         ret = wsm_cmd_send(priv, buf, NULL, 0x001B, WSM_CMD_TIMEOUT);
 784
 785         wsm_cmd_unlock(priv);
 786         return ret;
 787
 788 nomem:
 789         wsm_cmd_unlock(priv);
 790         return -ENOMEM;
 791 }
 792
 793 /* ******************************************************************** */
 794 int wsm_set_probe_responder(struct cw1200_common *priv, bool enable)
 795 {
 796         priv->rx_filter.probeResponder = enable;
 797         return wsm_set_rx_filter(priv, &priv->rx_filter);
 798 }
 799
 800 /* ******************************************************************** */
 801 /* WSM indication events implementation                                 */
 802 const char * const cw1200_fw_types[] = {
 803         "ETF",
 804         "WFM",
 805         "WSM",
 806         "HI test",
 807         "Platform test"
 808 };
 809
 810 static int wsm_startup_indication(struct cw1200_common *priv,
 811                                         struct wsm_buf *buf)
 812 {
 813         priv->wsm_caps.input_buffers     = WSM_GET16(buf);
 814         priv->wsm_caps.input_buffer_size = WSM_GET16(buf);
 815         priv->wsm_caps.hw_id      = WSM_GET16(buf);
 816         priv->wsm_caps.hw_subid   = WSM_GET16(buf);
 817         priv->wsm_caps.status     = WSM_GET16(buf);
 818         priv->wsm_caps.fw_cap     = WSM_GET16(buf);
 819         priv->wsm_caps.fw_type    = WSM_GET16(buf);
 820         priv->wsm_caps.fw_api     = WSM_GET16(buf);
 821         priv->wsm_caps.fw_build   = WSM_GET16(buf);
 822         priv->wsm_caps.fw_ver     = WSM_GET16(buf);
 823         WSM_GET(buf, priv->wsm_caps.fw_label, sizeof(priv->wsm_caps.fw_label));
 824         priv->wsm_caps.fw_label[sizeof(priv->wsm_caps.fw_label) - 1] = 0; /* Do not trust FW too much... */
 825
 826         if (WARN_ON(priv->wsm_caps.status))
 827                 return -EINVAL;
 828
 829         if (WARN_ON(priv->wsm_caps.fw_type > 4))
 830                 return -EINVAL;
 831
 832         pr_info("CW1200 WSM init done.\n"
 833                 "   Input buffers: %d x %d bytes\n"
 834                 "   Hardware: %d.%d\n"
 835                 "   %s firmware [%s], ver: %d, build: %d,"
 836                 "   api: %d, cap: 0x%.4X\n",
 837                 priv->wsm_caps.input_buffers,
 838                 priv->wsm_caps.input_buffer_size,
 839                 priv->wsm_caps.hw_id, priv->wsm_caps.hw_subid,
 840                 cw1200_fw_types[priv->wsm_caps.fw_type],
 841                 priv->wsm_caps.fw_label, priv->wsm_caps.fw_ver,
 842                 priv->wsm_caps.fw_build,
 843                 priv->wsm_caps.fw_api, priv->wsm_caps.fw_cap);
 844
 845         /* Disable unsupported frequency bands */
 846         if (!(priv->wsm_caps.fw_cap & 0x1))
 847                 priv->hw->wiphy->bands[NL80211_BAND_2GHZ] = NULL;
 848         if (!(priv->wsm_caps.fw_cap & 0x2))
 849                 priv->hw->wiphy->bands[NL80211_BAND_5GHZ] = NULL;
 850
 851         priv->firmware_ready = 1;
 852         wake_up(&priv->wsm_startup_done);
 853         return 0;
 854
 855 underflow:
 856         WARN_ON(1);
 857         return -EINVAL;
 858 }
 859
 860 static int wsm_receive_indication(struct cw1200_common *priv,
 861                                   int link_id,
 862                                   struct wsm_buf *buf,
 863                                   struct sk_buff **skb_p)
 864 {
 865         struct wsm_rx rx;
 866         struct ieee80211_hdr *hdr;
 867         size_t hdr_len;
 868         __le16 fctl;
 869
 870         rx.status = WSM_GET32(buf);
 871         rx.channel_number = WSM_GET16(buf);
 872         rx.rx_rate = WSM_GET8(buf);
 873         rx.rcpi_rssi = WSM_GET8(buf);
 874         rx.flags = WSM_GET32(buf);
 875
 876         /* FW Workaround: Drop probe resp or
 877            beacon when RSSI is 0
 878         */
 879         hdr = (struct ieee80211_hdr *)(*skb_p)->data;
 880
 881         if (!rx.rcpi_rssi &&
 882             (ieee80211_is_probe_resp(hdr->frame_control) ||
 883              ieee80211_is_beacon(hdr->frame_control)))
 884                 return 0;
 885
 886         /* If no RSSI subscription has been made,
 887          * convert RCPI to RSSI here
 888          */
 889         if (!priv->cqm_use_rssi)
 890                 rx.rcpi_rssi = rx.rcpi_rssi / 2 - 110;
 891
 892         fctl = *(__le16 *)buf->data;
 893         hdr_len = buf->data - buf->begin;
 894         skb_pull(*skb_p, hdr_len);
 895         if (!rx.status && ieee80211_is_deauth(fctl)) {
 896                 if (priv->join_status == CW1200_JOIN_STATUS_STA) {
 897                         /* Shedule unjoin work */
 898                         pr_debug("[WSM] Issue unjoin command (RX).\n");
 899                         wsm_lock_tx_async(priv);
 900                         if (queue_work(priv->workqueue,
 901                                        &priv->unjoin_work) <= 0)
 902                                 wsm_unlock_tx(priv);
 903                 }
 904         }
 905         cw1200_rx_cb(priv, &rx, link_id, skb_p);
 906         if (*skb_p)
 907                 skb_push(*skb_p, hdr_len);
 908
 909         return 0;
 910
 911 underflow:
 912         return -EINVAL;
 913 }
 914
 915 static int wsm_event_indication(struct cw1200_common *priv, struct wsm_buf *buf)
 916 {
 917         int first;
 918         struct cw1200_wsm_event *event;
 919
 920         if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
 921                 /* STA is stopped. */
 922                 return 0;
 923         }
 924
 925         event = kzalloc(sizeof(struct cw1200_wsm_event), GFP_KERNEL);
 926         if (!event)
 927                 return -ENOMEM;
 928
 929         event->evt.id = WSM_GET32(buf);
 930         event->evt.data = WSM_GET32(buf);
 931
 932         pr_debug("[WSM] Event: %d(%d)\n",
 933                  event->evt.id, event->evt.data);
 934
 935         spin_lock(&priv->event_queue_lock);
 936         first = list_empty(&priv->event_queue);
 937         list_add_tail(&event->link, &priv->event_queue);
 938         spin_unlock(&priv->event_queue_lock);
 939
 940         if (first)
 941                 queue_work(priv->workqueue, &priv->event_handler);
 942
 943         return 0;
 944
 945 underflow:
 946         kfree(event);
 947         return -EINVAL;
 948 }
 949
 950 static int wsm_channel_switch_indication(struct cw1200_common *priv,
 951                                          struct wsm_buf *buf)
 952 {
 953         WARN_ON(WSM_GET32(buf));
 954
 955         priv->channel_switch_in_progress = 0;
 956         wake_up(&priv->channel_switch_done);
 957
 958         wsm_unlock_tx(priv);
 959
 960         return 0;
 961
 962 underflow:
 963         return -EINVAL;
 964 }
 965
 966 static int wsm_set_pm_indication(struct cw1200_common *priv,
 967                                  struct wsm_buf *buf)
 968 {
 969         /* TODO:  Check buf (struct wsm_set_pm_complete) for validity */
 970         if (priv->ps_mode_switch_in_progress) {
 971                 priv->ps_mode_switch_in_progress = 0;
 972                 wake_up(&priv->ps_mode_switch_done);
 973         }
 974         return 0;
 975 }
 976
 977 static int wsm_scan_started(struct cw1200_common *priv, void *arg,
 978                             struct wsm_buf *buf)
 979 {
 980         u32 status = WSM_GET32(buf);
 981         if (status != WSM_STATUS_SUCCESS) {
 982                 cw1200_scan_failed_cb(priv);
 983                 return -EINVAL;
 984         }
 985         return 0;
 986
 987 underflow:
 988         WARN_ON(1);
 989         return -EINVAL;
 990 }
 991
 992 static int wsm_scan_complete_indication(struct cw1200_common *priv,
 993                                         struct wsm_buf *buf)
 994 {
 995         struct wsm_scan_complete arg;
 996         arg.status = WSM_GET32(buf);
 997         arg.psm = WSM_GET8(buf);
 998         arg.num_channels = WSM_GET8(buf);
 999         cw1200_scan_complete_cb(priv, &arg);
1000
1001         return 0;
1002
1003 underflow:
1004         return -EINVAL;
1005 }
1006
1007 static int wsm_join_complete_indication(struct cw1200_common *priv,
1008                                         struct wsm_buf *buf)
1009 {
1010         struct wsm_join_complete arg;
1011         arg.status = WSM_GET32(buf);
1012         pr_debug("[WSM] Join complete indication, status: %d\n", arg.status);
1013         cw1200_join_complete_cb(priv, &arg);
1014
1015         return 0;
1016
1017 underflow:
1018         return -EINVAL;
1019 }
1020
1021 static int wsm_find_complete_indication(struct cw1200_common *priv,
1022                                         struct wsm_buf *buf)
1023 {
1024         pr_warn("Implement find_complete_indication\n");
1025         return 0;
1026 }
1027
1028 static int wsm_ba_timeout_indication(struct cw1200_common *priv,
1029                                      struct wsm_buf *buf)
1030 {
1031         u32 dummy;
1032         u8 tid;
1033         u8 dummy2;
1034         u8 addr[ETH_ALEN];
1035
1036         dummy = WSM_GET32(buf);
1037         tid = WSM_GET8(buf);
1038         dummy2 = WSM_GET8(buf);
1039         WSM_GET(buf, addr, ETH_ALEN);
1040
1041         pr_info("BlockACK timeout, tid %d, addr %pM\n",
1042                 tid, addr);
1043
1044         return 0;
1045
1046 underflow:
1047         return -EINVAL;
1048 }
1049
1050 static int wsm_suspend_resume_indication(struct cw1200_common *priv,
1051                                          int link_id, struct wsm_buf *buf)
1052 {
1053         u32 flags;
1054         struct wsm_suspend_resume arg;
1055
1056         flags = WSM_GET32(buf);
1057         arg.link_id = link_id;
1058         arg.stop = !(flags & 1);
1059         arg.multicast = !!(flags & 8);
1060         arg.queue = (flags >> 1) & 3;
1061
1062         cw1200_suspend_resume(priv, &arg);
1063
1064         return 0;
1065
1066 underflow:
1067         return -EINVAL;
1068 }
1069
1070
1071 /* ******************************************************************** */
1072 /* WSM TX                                                               */
1073
1074 static int wsm_cmd_send(struct cw1200_common *priv,
1075                         struct wsm_buf *buf,
1076                         void *arg, u16 cmd, long tmo)
1077 {
1078         size_t buf_len = buf->data - buf->begin;
1079         int ret;
1080
1081         /* Don't bother if we're dead. */
1082         if (priv->bh_error) {
1083                 ret = 0;
1084                 goto done;
1085         }
1086
1087         /* Block until the cmd buffer is completed.  Tortuous. */
1088         spin_lock(&priv->wsm_cmd.lock);
1089         while (!priv->wsm_cmd.done) {
1090                 spin_unlock(&priv->wsm_cmd.lock);
1091                 spin_lock(&priv->wsm_cmd.lock);
1092         }
1093         priv->wsm_cmd.done = 0;
1094         spin_unlock(&priv->wsm_cmd.lock);
1095
1096         if (cmd == WSM_WRITE_MIB_REQ_ID ||
1097             cmd == WSM_READ_MIB_REQ_ID)
1098                 pr_debug("[WSM] >>> 0x%.4X [MIB: 0x%.4X] (%zu)\n",
1099                          cmd, __le16_to_cpu(((__le16 *)buf->begin)[2]),
1100                          buf_len);
1101         else
1102                 pr_debug("[WSM] >>> 0x%.4X (%zu)\n", cmd, buf_len);
1103
1104         /* Due to buggy SPI on CW1200, we need to
1105          * pad the message by a few bytes to ensure
1106          * that it's completely received.
1107          */
1108         buf_len += 4;
1109
1110         /* Fill HI message header */
1111         /* BH will add sequence number */
1112         ((__le16 *)buf->begin)[0] = __cpu_to_le16(buf_len);
1113         ((__le16 *)buf->begin)[1] = __cpu_to_le16(cmd);
1114
1115         spin_lock(&priv->wsm_cmd.lock);
1116         BUG_ON(priv->wsm_cmd.ptr);
1117         priv->wsm_cmd.ptr = buf->begin;
1118         priv->wsm_cmd.len = buf_len;
1119         priv->wsm_cmd.arg = arg;
1120         priv->wsm_cmd.cmd = cmd;
1121         spin_unlock(&priv->wsm_cmd.lock);
1122
1123         cw1200_bh_wakeup(priv);
1124
1125         /* Wait for command completion */
1126         ret = wait_event_timeout(priv->wsm_cmd_wq,
1127                                  priv->wsm_cmd.done, tmo);
1128
1129         if (!ret && !priv->wsm_cmd.done) {
1130                 spin_lock(&priv->wsm_cmd.lock);
1131                 priv->wsm_cmd.done = 1;
1132                 priv->wsm_cmd.ptr = NULL;
1133                 spin_unlock(&priv->wsm_cmd.lock);
1134                 if (priv->bh_error) {
1135                         /* Return ok to help system cleanup */
1136                         ret = 0;
1137                 } else {
1138                         pr_err("CMD req (0x%04x) stuck in firmware, killing BH\n", priv->wsm_cmd.cmd);
1139                         print_hex_dump_bytes("REQDUMP: ", DUMP_PREFIX_NONE,
1140                                              buf->begin, buf_len);
1141                         pr_err("Outstanding outgoing frames:  %d\n", priv->hw_bufs_used);
1142
1143                         /* Kill BH thread to report the error to the top layer. */
1144                         atomic_add(1, &priv->bh_term);
1145                         wake_up(&priv->bh_wq);
1146                         ret = -ETIMEDOUT;
1147                 }
1148         } else {
1149                 spin_lock(&priv->wsm_cmd.lock);
1150                 BUG_ON(!priv->wsm_cmd.done);
1151                 ret = priv->wsm_cmd.ret;
1152                 spin_unlock(&priv->wsm_cmd.lock);
1153         }
1154 done:
1155         wsm_buf_reset(buf);
1156         return ret;
1157 }
1158
1159 /* ******************************************************************** */
1160 /* WSM TX port control                                                  */
1161
1162 void wsm_lock_tx(struct cw1200_common *priv)
1163 {
1164         wsm_cmd_lock(priv);
1165         if (atomic_add_return(1, &priv->tx_lock) == 1) {
1166                 if (wsm_flush_tx(priv))
1167                         pr_debug("[WSM] TX is locked.\n");
1168         }
1169         wsm_cmd_unlock(priv);
1170 }
1171
1172 void wsm_lock_tx_async(struct cw1200_common *priv)
1173 {
1174         if (atomic_add_return(1, &priv->tx_lock) == 1)
1175                 pr_debug("[WSM] TX is locked (async).\n");
1176 }
1177
1178 bool wsm_flush_tx(struct cw1200_common *priv)
1179 {
1180         unsigned long timestamp = jiffies;
1181         bool pending = false;
1182         long timeout;
1183         int i;
1184
1185         /* Flush must be called with TX lock held. */
1186         BUG_ON(!atomic_read(&priv->tx_lock));
1187
1188         /* First check if we really need to do something.
1189          * It is safe to use unprotected access, as hw_bufs_used
1190          * can only decrements.
1191          */
1192         if (!priv->hw_bufs_used)
1193                 return true;
1194
1195         if (priv->bh_error) {
1196                 /* In case of failure do not wait for magic. */
1197                 pr_err("[WSM] Fatal error occurred, will not flush TX.\n");
1198                 return false;
1199         } else {
1200                 /* Get a timestamp of "oldest" frame */
1201                 for (i = 0; i < 4; ++i)
1202                         pending |= cw1200_queue_get_xmit_timestamp(
1203                                         &priv->tx_queue[i],
1204                                         &timestamp, 0xffffffff);
1205                 /* If there's nothing pending, we're good */
1206                 if (!pending)
1207                         return true;
1208
1209                 timeout = timestamp + WSM_CMD_LAST_CHANCE_TIMEOUT - jiffies;
1210                 if (timeout < 0 || wait_event_timeout(priv->bh_evt_wq,
1211                                                       !priv->hw_bufs_used,
1212                                                       timeout) <= 0) {
1213                         /* Hmmm... Not good. Frame had stuck in firmware. */
1214                         priv->bh_error = 1;
1215                         wiphy_err(priv->hw->wiphy, "[WSM] TX Frames (%d) stuck in firmware, killing BH\n", priv->hw_bufs_used);
1216                         wake_up(&priv->bh_wq);
1217                         return false;
1218                 }
1219
1220                 /* Ok, everything is flushed. */
1221                 return true;
1222         }
1223 }
1224
1225 void wsm_unlock_tx(struct cw1200_common *priv)
1226 {
1227         int tx_lock;
1228         tx_lock = atomic_sub_return(1, &priv->tx_lock);
1229         BUG_ON(tx_lock < 0);
1230
1231         if (tx_lock == 0) {
1232                 if (!priv->bh_error)
1233                         cw1200_bh_wakeup(priv);
1234                 pr_debug("[WSM] TX is unlocked.\n");
1235         }
1236 }
1237
1238 /* ******************************************************************** */
1239 /* WSM RX                                                               */
1240
1241 int wsm_handle_exception(struct cw1200_common *priv, u8 *data, size_t len)
1242 {
1243         struct wsm_buf buf;
1244         u32 reason;
1245         u32 reg[18];
1246         char fname[48];
1247         unsigned int i;
1248
1249         static const char * const reason_str[] = {
1250                 "undefined instruction",
1251                 "prefetch abort",
1252                 "data abort",
1253                 "unknown error",
1254         };
1255
1256         buf.begin = buf.data = data;
1257         buf.end = &buf.begin[len];
1258
1259         reason = WSM_GET32(&buf);
1260         for (i = 0; i < ARRAY_SIZE(reg); ++i)
1261                 reg[i] = WSM_GET32(&buf);
1262         WSM_GET(&buf, fname, sizeof(fname));
1263
1264         if (reason < 4)
1265                 wiphy_err(priv->hw->wiphy,
1266                           "Firmware exception: %s.\n",
1267                           reason_str[reason]);
1268         else
1269                 wiphy_err(priv->hw->wiphy,
1270                           "Firmware assert at %.*s, line %d\n",
1271                           (int) sizeof(fname), fname, reg[1]);
1272
1273         for (i = 0; i < 12; i += 4)
1274                 wiphy_err(priv->hw->wiphy,
1275                           "R%d: 0x%.8X, R%d: 0x%.8X, R%d: 0x%.8X, R%d: 0x%.8X,\n",
1276                           i + 0, reg[i + 0], i + 1, reg[i + 1],
1277                           i + 2, reg[i + 2], i + 3, reg[i + 3]);
1278         wiphy_err(priv->hw->wiphy,
1279                   "R12: 0x%.8X, SP: 0x%.8X, LR: 0x%.8X, PC: 0x%.8X,\n",
1280                   reg[i + 0], reg[i + 1], reg[i + 2], reg[i + 3]);
1281         i += 4;
1282         wiphy_err(priv->hw->wiphy,
1283                   "CPSR: 0x%.8X, SPSR: 0x%.8X\n",
1284                   reg[i + 0], reg[i + 1]);
1285
1286         print_hex_dump_bytes("R1: ", DUMP_PREFIX_NONE,
1287                              fname, sizeof(fname));
1288         return 0;
1289
1290 underflow:
1291         wiphy_err(priv->hw->wiphy, "Firmware exception.\n");
1292         print_hex_dump_bytes("Exception: ", DUMP_PREFIX_NONE,
1293                              data, len);
1294         return -EINVAL;
1295 }
1296
1297 int wsm_handle_rx(struct cw1200_common *priv, u16 id,
1298                   struct wsm_hdr *wsm, struct sk_buff **skb_p)
1299 {
1300         int ret = 0;
1301         struct wsm_buf wsm_buf;
1302         int link_id = (id >> 6) & 0x0F;
1303
1304         /* Strip link id. */
1305         id &= ~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
1306
1307         wsm_buf.begin = (u8 *)&wsm[0];
1308         wsm_buf.data = (u8 *)&wsm[1];
1309         wsm_buf.end = &wsm_buf.begin[__le16_to_cpu(wsm->len)];
1310
1311         pr_debug("[WSM] <<< 0x%.4X (%td)\n", id,
1312                  wsm_buf.end - wsm_buf.begin);
1313
1314         if (id == WSM_TX_CONFIRM_IND_ID) {
1315                 ret = wsm_tx_confirm(priv, &wsm_buf, link_id);
1316         } else if (id == WSM_MULTI_TX_CONFIRM_ID) {
1317                 ret = wsm_multi_tx_confirm(priv, &wsm_buf, link_id);
1318         } else if (id & 0x0400) {
1319                 void *wsm_arg;
1320                 u16 wsm_cmd;
1321
1322                 /* Do not trust FW too much. Protection against repeated
1323                  * response and race condition removal (see above).
1324                  */
1325                 spin_lock(&priv->wsm_cmd.lock);
1326                 wsm_arg = priv->wsm_cmd.arg;
1327                 wsm_cmd = priv->wsm_cmd.cmd &
1328                                 ~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
1329                 priv->wsm_cmd.cmd = 0xFFFF;
1330                 spin_unlock(&priv->wsm_cmd.lock);
1331
1332                 if (WARN_ON((id & ~0x0400) != wsm_cmd)) {
1333                         /* Note that any non-zero is a fatal retcode. */
1334                         ret = -EINVAL;
1335                         goto out;
1336                 }
1337
1338                 /* Note that wsm_arg can be NULL in case of timeout in
1339                  * wsm_cmd_send().
1340                  */
1341
1342                 switch (id) {
1343                 case WSM_READ_MIB_RESP_ID:
1344                         if (wsm_arg)
1345                                 ret = wsm_read_mib_confirm(priv, wsm_arg,
1346                                                                 &wsm_buf);
1347                         break;
1348                 case WSM_WRITE_MIB_RESP_ID:
1349                         if (wsm_arg)
1350                                 ret = wsm_write_mib_confirm(priv, wsm_arg,
1351                                                             &wsm_buf);
1352                         break;
1353                 case WSM_START_SCAN_RESP_ID:
1354                         if (wsm_arg)
1355                                 ret = wsm_scan_started(priv, wsm_arg, &wsm_buf);
1356                         break;
1357                 case WSM_CONFIGURATION_RESP_ID:
1358                         if (wsm_arg)
1359                                 ret = wsm_configuration_confirm(priv, wsm_arg,
1360                                                                 &wsm_buf);
1361                         break;
1362                 case WSM_JOIN_RESP_ID:
1363                         if (wsm_arg)
1364                                 ret = wsm_join_confirm(priv, wsm_arg, &wsm_buf);
1365                         break;
1366                 case WSM_STOP_SCAN_RESP_ID:
1367                 case WSM_RESET_RESP_ID:
1368                 case WSM_ADD_KEY_RESP_ID:
1369                 case WSM_REMOVE_KEY_RESP_ID:
1370                 case WSM_SET_PM_RESP_ID:
1371                 case WSM_SET_BSS_PARAMS_RESP_ID:
1372                 case 0x0412: /* set_tx_queue_params */
1373                 case WSM_EDCA_PARAMS_RESP_ID:
1374                 case WSM_SWITCH_CHANNEL_RESP_ID:
1375                 case WSM_START_RESP_ID:
1376                 case WSM_BEACON_TRANSMIT_RESP_ID:
1377                 case 0x0419: /* start_find */
1378                 case 0x041A: /* stop_find */
1379                 case 0x041B: /* update_ie */
1380                 case 0x041C: /* map_link */
1381                         WARN_ON(wsm_arg != NULL);
1382                         ret = wsm_generic_confirm(priv, wsm_arg, &wsm_buf);
1383                         if (ret) {
1384                                 wiphy_warn(priv->hw->wiphy,
1385                                            "wsm_generic_confirm failed for request 0x%04x.\n",
1386                                            id & ~0x0400);
1387
1388                                 /* often 0x407 and 0x410 occur, this means we're dead.. */
1389                                 if (priv->join_status >= CW1200_JOIN_STATUS_JOINING) {
1390                                         wsm_lock_tx(priv);
1391                                         if (queue_work(priv->workqueue, &priv->unjoin_work) <= 0)
1392                                                 wsm_unlock_tx(priv);
1393                                 }
1394                         }
1395                         break;
1396                 default:
1397                         wiphy_warn(priv->hw->wiphy,
1398                                    "Unrecognized confirmation 0x%04x\n",
1399                                    id & ~0x0400);
1400                 }
1401
1402                 spin_lock(&priv->wsm_cmd.lock);
1403                 priv->wsm_cmd.ret = ret;
1404                 priv->wsm_cmd.done = 1;
1405                 spin_unlock(&priv->wsm_cmd.lock);
1406
1407                 ret = 0; /* Error response from device should ne stop BH. */
1408
1409                 wake_up(&priv->wsm_cmd_wq);
1410         } else if (id & 0x0800) {
1411                 switch (id) {
1412                 case WSM_STARTUP_IND_ID:
1413                         ret = wsm_startup_indication(priv, &wsm_buf);
1414                         break;
1415                 case WSM_RECEIVE_IND_ID:
1416                         ret = wsm_receive_indication(priv, link_id,
1417                                                      &wsm_buf, skb_p);
1418                         break;
1419                 case 0x0805:
1420                         ret = wsm_event_indication(priv, &wsm_buf);
1421                         break;
1422                 case WSM_SCAN_COMPLETE_IND_ID:
1423                         ret = wsm_scan_complete_indication(priv, &wsm_buf);
1424                         break;
1425                 case 0x0808:
1426                         ret = wsm_ba_timeout_indication(priv, &wsm_buf);
1427                         break;
1428                 case 0x0809:
1429                         ret = wsm_set_pm_indication(priv, &wsm_buf);
1430                         break;
1431                 case 0x080A:
1432                         ret = wsm_channel_switch_indication(priv, &wsm_buf);
1433                         break;
1434                 case 0x080B:
1435                         ret = wsm_find_complete_indication(priv, &wsm_buf);
1436                         break;
1437                 case 0x080C:
1438                         ret = wsm_suspend_resume_indication(priv,
1439                                         link_id, &wsm_buf);
1440                         break;
1441                 case 0x080F:
1442                         ret = wsm_join_complete_indication(priv, &wsm_buf);
1443                         break;
1444                 default:
1445                         pr_warn("Unrecognised WSM ID %04x\n", id);
1446                 }
1447         } else {
1448                 WARN_ON(1);
1449                 ret = -EINVAL;
1450         }
1451 out:
1452         return ret;
1453 }
1454
1455 static bool wsm_handle_tx_data(struct cw1200_common *priv,
1456                                struct wsm_tx *wsm,
1457                                const struct ieee80211_tx_info *tx_info,
1458                                const struct cw1200_txpriv *txpriv,
1459                                struct cw1200_queue *queue)
1460 {
1461         bool handled = false;
1462         const struct ieee80211_hdr *frame =
1463                 (struct ieee80211_hdr *)&((u8 *)wsm)[txpriv->offset];
1464         __le16 fctl = frame->frame_control;
1465         enum {
1466                 do_probe,
1467                 do_drop,
1468                 do_wep,
1469                 do_tx,
1470         } action = do_tx;
1471
1472         switch (priv->mode) {
1473         case NL80211_IFTYPE_STATION:
1474                 if (priv->join_status == CW1200_JOIN_STATUS_MONITOR)
1475                         action = do_tx;
1476                 else if (priv->join_status < CW1200_JOIN_STATUS_PRE_STA)
1477                         action = do_drop;
1478                 break;
1479         case NL80211_IFTYPE_AP:
1480                 if (!priv->join_status) {
1481                         action = do_drop;
1482                 } else if (!(BIT(txpriv->raw_link_id) &
1483                              (BIT(0) | priv->link_id_map))) {
1484                         wiphy_warn(priv->hw->wiphy,
1485                                    "A frame with expired link id is dropped.\n");
1486                         action = do_drop;
1487                 }
1488                 if (cw1200_queue_get_generation(wsm->packet_id) >
1489                                 CW1200_MAX_REQUEUE_ATTEMPTS) {
1490                         /* HACK!!! WSM324 firmware has tendency to requeue
1491                          * multicast frames in a loop, causing performance
1492                          * drop and high power consumption of the driver.
1493                          * In this situation it is better just to drop
1494                          * the problematic frame.
1495                          */
1496                         wiphy_warn(priv->hw->wiphy,
1497                                    "Too many attempts to requeue a frame; dropped.\n");
1498                         action = do_drop;
1499                 }
1500                 break;
1501         case NL80211_IFTYPE_ADHOC:
1502                 if (priv->join_status != CW1200_JOIN_STATUS_IBSS)
1503                         action = do_drop;
1504                 break;
1505         case NL80211_IFTYPE_MESH_POINT:
1506                 action = do_tx; /* TODO:  Test me! */
1507                 break;
1508         case NL80211_IFTYPE_MONITOR:
1509         default:
1510                 action = do_drop;
1511                 break;
1512         }
1513
1514         if (action == do_tx) {
1515                 if (ieee80211_is_nullfunc(fctl)) {
1516                         spin_lock(&priv->bss_loss_lock);
1517                         if (priv->bss_loss_state) {
1518                                 priv->bss_loss_confirm_id = wsm->packet_id;
1519                                 wsm->queue_id = WSM_QUEUE_VOICE;
1520                         }
1521                         spin_unlock(&priv->bss_loss_lock);
1522                 } else if (ieee80211_is_probe_req(fctl)) {
1523                         action = do_probe;
1524                 } else if (ieee80211_is_deauth(fctl) &&
1525                            priv->mode != NL80211_IFTYPE_AP) {
1526                         pr_debug("[WSM] Issue unjoin command due to tx deauth.\n");
1527                         wsm_lock_tx_async(priv);
1528                         if (queue_work(priv->workqueue,
1529                                        &priv->unjoin_work) <= 0)
1530                                 wsm_unlock_tx(priv);
1531                 } else if (ieee80211_has_protected(fctl) &&
1532                            tx_info->control.hw_key &&
1533                            tx_info->control.hw_key->keyidx != priv->wep_default_key_id &&
1534                            (tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
1535                             tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_WEP104)) {
1536                         action = do_wep;
1537                 }
1538         }
1539
1540         switch (action) {
1541         case do_probe:
1542                 /* An interesting FW "feature". Device filters probe responses.
1543                  * The easiest way to get it back is to convert
1544                  * probe request into WSM start_scan command.
1545                  */
1546                 pr_debug("[WSM] Convert probe request to scan.\n");
1547                 wsm_lock_tx_async(priv);
1548                 priv->pending_frame_id = wsm->packet_id;
1549                 if (queue_delayed_work(priv->workqueue,
1550                                        &priv->scan.probe_work, 0) <= 0)
1551                         wsm_unlock_tx(priv);
1552                 handled = true;
1553                 break;
1554         case do_drop:
1555                 pr_debug("[WSM] Drop frame (0x%.4X).\n", fctl);
1556                 BUG_ON(cw1200_queue_remove(queue, wsm->packet_id));
1557                 handled = true;
1558                 break;
1559         case do_wep:
1560                 pr_debug("[WSM] Issue set_default_wep_key.\n");
1561                 wsm_lock_tx_async(priv);
1562                 priv->wep_default_key_id = tx_info->control.hw_key->keyidx;
1563                 priv->pending_frame_id = wsm->packet_id;
1564                 if (queue_work(priv->workqueue, &priv->wep_key_work) <= 0)
1565                         wsm_unlock_tx(priv);
1566                 handled = true;
1567                 break;
1568         case do_tx:
1569                 pr_debug("[WSM] Transmit frame.\n");
1570                 break;
1571         default:
1572                 /* Do nothing */
1573                 break;
1574         }
1575         return handled;
1576 }
1577
1578 static int cw1200_get_prio_queue(struct cw1200_common *priv,
1579                                  u32 link_id_map, int *total)
1580 {
1581         static const int urgent = BIT(CW1200_LINK_ID_AFTER_DTIM) |
1582                 BIT(CW1200_LINK_ID_UAPSD);
1583         struct wsm_edca_queue_params *edca;
1584         unsigned score, best = -1;
1585         int winner = -1;
1586         int queued;
1587         int i;
1588
1589         /* search for a winner using edca params */
1590         for (i = 0; i < 4; ++i) {
1591                 queued = cw1200_queue_get_num_queued(&priv->tx_queue[i],
1592                                 link_id_map);
1593                 if (!queued)
1594                         continue;
1595                 *total += queued;
1596                 edca = &priv->edca.params[i];
1597                 score = ((edca->aifns + edca->cwmin) << 16) +
1598                         ((edca->cwmax - edca->cwmin) *
1599                          (get_random_int() & 0xFFFF));
1600                 if (score < best && (winner < 0 || i != 3)) {
1601                         best = score;
1602                         winner = i;
1603                 }
1604         }
1605
1606         /* override winner if bursting */
1607         if (winner >= 0 && priv->tx_burst_idx >= 0 &&
1608             winner != priv->tx_burst_idx &&
1609             !cw1200_queue_get_num_queued(
1610                     &priv->tx_queue[winner],
1611                     link_id_map & urgent) &&
1612             cw1200_queue_get_num_queued(
1613                     &priv->tx_queue[priv->tx_burst_idx],
1614                     link_id_map))
1615                 winner = priv->tx_burst_idx;
1616
1617         return winner;
1618 }
1619
1620 static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
1621                                      struct cw1200_queue **queue_p,
1622                                      u32 *tx_allowed_mask_p,
1623                                      bool *more)
1624 {
1625         int idx;
1626         u32 tx_allowed_mask;
1627         int total = 0;
1628
1629         /* Search for a queue with multicast frames buffered */
1630         if (priv->tx_multicast) {
1631                 tx_allowed_mask = BIT(CW1200_LINK_ID_AFTER_DTIM);
1632                 idx = cw1200_get_prio_queue(priv,
1633                                 tx_allowed_mask, &total);
1634                 if (idx >= 0) {
1635                         *more = total > 1;
1636                         goto found;
1637                 }
1638         }
1639
1640         /* Search for unicast traffic */
1641         tx_allowed_mask = ~priv->sta_asleep_mask;
1642         tx_allowed_mask |= BIT(CW1200_LINK_ID_UAPSD);
1643         if (priv->sta_asleep_mask) {
1644                 tx_allowed_mask |= priv->pspoll_mask;
1645                 tx_allowed_mask &= ~BIT(CW1200_LINK_ID_AFTER_DTIM);
1646         } else {
1647                 tx_allowed_mask |= BIT(CW1200_LINK_ID_AFTER_DTIM);
1648         }
1649         idx = cw1200_get_prio_queue(priv,
1650                         tx_allowed_mask, &total);
1651         if (idx < 0)
1652                 return -ENOENT;
1653
1654 found:
1655         *queue_p = &priv->tx_queue[idx];
1656         *tx_allowed_mask_p = tx_allowed_mask;
1657         return 0;
1658 }
1659
1660 int wsm_get_tx(struct cw1200_common *priv, u8 **data,
1661                size_t *tx_len, int *burst)
1662 {
1663         struct wsm_tx *wsm = NULL;
1664         struct ieee80211_tx_info *tx_info;
1665         struct cw1200_queue *queue = NULL;
1666         int queue_num;
1667         u32 tx_allowed_mask = 0;
1668         const struct cw1200_txpriv *txpriv = NULL;
1669         int count = 0;
1670
1671         /* More is used only for broadcasts. */
1672         bool more = false;
1673
1674         if (priv->wsm_cmd.ptr) { /* CMD request */
1675                 ++count;
1676                 spin_lock(&priv->wsm_cmd.lock);
1677                 BUG_ON(!priv->wsm_cmd.ptr);
1678                 *data = priv->wsm_cmd.ptr;
1679                 *tx_len = priv->wsm_cmd.len;
1680                 *burst = 1;
1681                 spin_unlock(&priv->wsm_cmd.lock);
1682         } else {
1683                 for (;;) {
1684                         int ret;
1685
1686                         if (atomic_add_return(0, &priv->tx_lock))
1687                                 break;
1688
1689                         spin_lock_bh(&priv->ps_state_lock);
1690
1691                         ret = wsm_get_tx_queue_and_mask(priv, &queue,
1692                                                         &tx_allowed_mask, &more);
1693                         queue_num = queue - priv->tx_queue;
1694
1695                         if (priv->buffered_multicasts &&
1696                             (ret || !more) &&
1697                             (priv->tx_multicast || !priv->sta_asleep_mask)) {
1698                                 priv->buffered_multicasts = false;
1699                                 if (priv->tx_multicast) {
1700                                         priv->tx_multicast = false;
1701                                         queue_work(priv->workqueue,
1702                                                    &priv->multicast_stop_work);
1703                                 }
1704                         }
1705
1706                         spin_unlock_bh(&priv->ps_state_lock);
1707
1708                         if (ret)
1709                                 break;
1710
1711                         if (cw1200_queue_get(queue,
1712                                              tx_allowed_mask,
1713                                              &wsm, &tx_info, &txpriv))
1714                                 continue;
1715
1716                         if (wsm_handle_tx_data(priv, wsm,
1717                                                tx_info, txpriv, queue))
1718                                 continue;  /* Handled by WSM */
1719
1720                         wsm->hdr.id &= __cpu_to_le16(
1721                                 ~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX));
1722                         wsm->hdr.id |= cpu_to_le16(
1723                                 WSM_TX_LINK_ID(txpriv->raw_link_id));
1724                         priv->pspoll_mask &= ~BIT(txpriv->raw_link_id);
1725
1726                         *data = (u8 *)wsm;
1727                         *tx_len = __le16_to_cpu(wsm->hdr.len);
1728
1729                         /* allow bursting if txop is set */
1730                         if (priv->edca.params[queue_num].txop_limit)
1731                                 *burst = min(*burst,
1732                                              (int)cw1200_queue_get_num_queued(queue, tx_allowed_mask) + 1);
1733                         else
1734                                 *burst = 1;
1735
1736                         /* store index of bursting queue */
1737                         if (*burst > 1)
1738                                 priv->tx_burst_idx = queue_num;
1739                         else
1740                                 priv->tx_burst_idx = -1;
1741
1742                         if (more) {
1743                                 struct ieee80211_hdr *hdr =
1744                                         (struct ieee80211_hdr *)
1745                                         &((u8 *)wsm)[txpriv->offset];
1746                                 /* more buffered multicast/broadcast frames
1747                                  *  ==> set MoreData flag in IEEE 802.11 header
1748                                  *  to inform PS STAs
1749                                  */
1750                                 hdr->frame_control |=
1751                                         cpu_to_le16(IEEE80211_FCTL_MOREDATA);
1752                         }
1753
1754                         pr_debug("[WSM] >>> 0x%.4X (%zu) %p %c\n",
1755                                  0x0004, *tx_len, *data,
1756                                  wsm->more ? 'M' : ' ');
1757                         ++count;
1758                         break;
1759                 }
1760         }
1761
1762         return count;
1763 }
1764
1765 void wsm_txed(struct cw1200_common *priv, u8 *data)
1766 {
1767         if (data == priv->wsm_cmd.ptr) {
1768                 spin_lock(&priv->wsm_cmd.lock);
1769                 priv->wsm_cmd.ptr = NULL;
1770                 spin_unlock(&priv->wsm_cmd.lock);
1771         }
1772 }
1773
1774 /* ******************************************************************** */
1775 /* WSM buffer                                                           */
1776
1777 void wsm_buf_init(struct wsm_buf *buf)
1778 {
1779         BUG_ON(buf->begin);
1780         buf->begin = kmalloc(FWLOAD_BLOCK_SIZE, GFP_KERNEL | GFP_DMA);
1781         buf->end = buf->begin ? &buf->begin[FWLOAD_BLOCK_SIZE] : buf->begin;
1782         wsm_buf_reset(buf);
1783 }
1784
1785 void wsm_buf_deinit(struct wsm_buf *buf)
1786 {
1787         kfree(buf->begin);
1788         buf->begin = buf->data = buf->end = NULL;
1789 }
1790
1791 static void wsm_buf_reset(struct wsm_buf *buf)
1792 {
1793         if (buf->begin) {
1794                 buf->data = &buf->begin[4];
1795                 *(u32 *)buf->begin = 0;
1796         } else {
1797                 buf->data = buf->begin;
1798         }
1799 }
1800
1801 static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size)
1802 {
1803         size_t pos = buf->data - buf->begin;
1804         size_t size = pos + extra_size;
1805         u8 *tmp;
1806
1807         size = round_up(size, FWLOAD_BLOCK_SIZE);
1808
1809         tmp = krealloc(buf->begin, size, GFP_KERNEL | GFP_DMA);
1810         if (!tmp) {
1811                 wsm_buf_deinit(buf);
1812                 return -ENOMEM;
1813         }
1814
1815         buf->begin = tmp;
1816         buf->data = &buf->begin[pos];
1817         buf->end = &buf->begin[size];
1818         return 0;
1819 }