drivers/net/wireless/cw1200/wsm.c

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