drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c

   1 /*
   2  * Copyright (c) 2010 Broadcom Corporation
   3  *
   4  * Permission to use, copy, modify, and/or distribute this software for any
   5  * purpose with or without fee is hereby granted, provided that the above
   6  * copyright notice and this permission notice appear in all copies.
   7  *
   8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
  11  * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
  13  * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  14  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15  */
  16 #include <linux/types.h>
  17 #include <linux/module.h>
  18 #include <linux/if_ether.h>
  19 #include <linux/spinlock.h>
  20 #include <linux/skbuff.h>
  21 #include <linux/netdevice.h>
  22 #include <linux/etherdevice.h>
  23 #include <linux/err.h>
  24 #include <uapi/linux/nl80211.h>
  25 #include <net/cfg80211.h>
  26
  27 #include <brcmu_utils.h>
  28 #include <brcmu_wifi.h>
  29 #include "dhd.h"
  30 #include "dhd_proto.h"
  31 #include "dhd_dbg.h"
  32 #include "dhd_bus.h"
  33 #include "fwil.h"
  34 #include "fweh.h"
  35 #include "fwsignal.h"
  36
  37 /**
  38  * DOC: Firmware Signalling
  39  *
  40  * Firmware can send signals to host and vice versa, which are passed in the
  41  * data packets using TLV based header. This signalling layer is on top of the
  42  * BDC bus protocol layer.
  43  */
  44
  45 /*
  46  * single definition for firmware-driver flow control tlv's.
  47  *
  48  * each tlv is specified by BRCMF_FWS_TLV_DEF(name, ID, length).
  49  * A length value 0 indicates variable length tlv.
  50  */
  51 #define BRCMF_FWS_TLV_DEFLIST \
  52         BRCMF_FWS_TLV_DEF(MAC_OPEN, 1, 1) \
  53         BRCMF_FWS_TLV_DEF(MAC_CLOSE, 2, 1) \
  54         BRCMF_FWS_TLV_DEF(MAC_REQUEST_CREDIT, 3, 2) \
  55         BRCMF_FWS_TLV_DEF(TXSTATUS, 4, 4) \
  56         BRCMF_FWS_TLV_DEF(PKTTAG, 5, 4) \
  57         BRCMF_FWS_TLV_DEF(MACDESC_ADD,  6, 8) \
  58         BRCMF_FWS_TLV_DEF(MACDESC_DEL, 7, 8) \
  59         BRCMF_FWS_TLV_DEF(RSSI, 8, 1) \
  60         BRCMF_FWS_TLV_DEF(INTERFACE_OPEN, 9, 1) \
  61         BRCMF_FWS_TLV_DEF(INTERFACE_CLOSE, 10, 1) \
  62         BRCMF_FWS_TLV_DEF(FIFO_CREDITBACK, 11, 6) \
  63         BRCMF_FWS_TLV_DEF(PENDING_TRAFFIC_BMP, 12, 2) \
  64         BRCMF_FWS_TLV_DEF(MAC_REQUEST_PACKET, 13, 3) \
  65         BRCMF_FWS_TLV_DEF(HOST_REORDER_RXPKTS, 14, 10) \
  66         BRCMF_FWS_TLV_DEF(TRANS_ID, 18, 6) \
  67         BRCMF_FWS_TLV_DEF(COMP_TXSTATUS, 19, 1) \
  68         BRCMF_FWS_TLV_DEF(FILLER, 255, 0)
  69
  70 /*
  71  * enum brcmf_fws_tlv_type - definition of tlv identifiers.
  72  */
  73 #define BRCMF_FWS_TLV_DEF(name, id, len) \
  74         BRCMF_FWS_TYPE_ ## name =  id,
  75 enum brcmf_fws_tlv_type {
  76         BRCMF_FWS_TLV_DEFLIST
  77         BRCMF_FWS_TYPE_INVALID
  78 };
  79 #undef BRCMF_FWS_TLV_DEF
  80
  81 /*
  82  * enum brcmf_fws_tlv_len - definition of tlv lengths.
  83  */
  84 #define BRCMF_FWS_TLV_DEF(name, id, len) \
  85         BRCMF_FWS_TYPE_ ## name ## _LEN = (len),
  86 enum brcmf_fws_tlv_len {
  87         BRCMF_FWS_TLV_DEFLIST
  88 };
  89 #undef BRCMF_FWS_TLV_DEF
  90
  91 #ifdef DEBUG
  92 /*
  93  * brcmf_fws_tlv_names - array of tlv names.
  94  */
  95 #define BRCMF_FWS_TLV_DEF(name, id, len) \
  96         { id, #name },
  97 static struct {
  98         enum brcmf_fws_tlv_type id;
  99         const char *name;
 100 } brcmf_fws_tlv_names[] = {
 101         BRCMF_FWS_TLV_DEFLIST
 102 };
 103 #undef BRCMF_FWS_TLV_DEF
 104
 105 static const char *brcmf_fws_get_tlv_name(enum brcmf_fws_tlv_type id)
 106 {
 107         int i;
 108
 109         for (i = 0; i < ARRAY_SIZE(brcmf_fws_tlv_names); i++)
 110                 if (brcmf_fws_tlv_names[i].id == id)
 111                         return brcmf_fws_tlv_names[i].name;
 112
 113         return "INVALID";
 114 }
 115 #else
 116 static const char *brcmf_fws_get_tlv_name(enum brcmf_fws_tlv_type id)
 117 {
 118         return "NODEBUG";
 119 }
 120 #endif /* DEBUG */
 121
 122 /*
 123  * flags used to enable tlv signalling from firmware.
 124  */
 125 #define BRCMF_FWS_FLAGS_RSSI_SIGNALS                    0x0001
 126 #define BRCMF_FWS_FLAGS_XONXOFF_SIGNALS                 0x0002
 127 #define BRCMF_FWS_FLAGS_CREDIT_STATUS_SIGNALS           0x0004
 128 #define BRCMF_FWS_FLAGS_HOST_PROPTXSTATUS_ACTIVE        0x0008
 129 #define BRCMF_FWS_FLAGS_PSQ_GENERATIONFSM_ENABLE        0x0010
 130 #define BRCMF_FWS_FLAGS_PSQ_ZERO_BUFFER_ENABLE          0x0020
 131 #define BRCMF_FWS_FLAGS_HOST_RXREORDER_ACTIVE           0x0040
 132
 133 #define BRCMF_FWS_MAC_DESC_TABLE_SIZE                   32
 134 #define BRCMF_FWS_MAC_DESC_ID_INVALID                   0xff
 135
 136 #define BRCMF_FWS_HOSTIF_FLOWSTATE_OFF                  0
 137 #define BRCMF_FWS_HOSTIF_FLOWSTATE_ON                   1
 138 #define BRCMF_FWS_FLOWCONTROL_HIWATER                   128
 139 #define BRCMF_FWS_FLOWCONTROL_LOWATER                   64
 140
 141 #define BRCMF_FWS_PSQ_PREC_COUNT                ((NL80211_NUM_ACS + 1) * 2)
 142 #define BRCMF_FWS_PSQ_LEN                               256
 143
 144 #define BRCMF_FWS_HTOD_FLAG_PKTFROMHOST                 0x01
 145 #define BRCMF_FWS_HTOD_FLAG_PKT_REQUESTED               0x02
 146
 147 /**
 148  * enum brcmf_fws_skb_state - indicates processing state of skb.
 149  *
 150  * @BRCMF_FWS_SKBSTATE_NEW: sk_buff is newly arrived in the driver.
 151  * @BRCMF_FWS_SKBSTATE_DELAYED: sk_buff had to wait on queue.
 152  * @BRCMF_FWS_SKBSTATE_SUPPRESSED: sk_buff has been suppressed by firmware.
 153  */
 154 enum brcmf_fws_skb_state {
 155         BRCMF_FWS_SKBSTATE_NEW,
 156         BRCMF_FWS_SKBSTATE_DELAYED,
 157         BRCMF_FWS_SKBSTATE_SUPPRESSED
 158 };
 159
 160 /**
 161  * struct brcmf_skbuff_cb - control buffer associated with skbuff.
 162  *
 163  * @if_flags: holds interface index and packet related flags.
 164  * @htod: host to device packet identifier (used in PKTTAG tlv).
 165  * @state: transmit state of the packet.
 166  * @mac: descriptor related to destination for this packet.
 167  *
 168  * This information is stored in control buffer struct sk_buff::cb, which
 169  * provides 48 bytes of storage so this structure should not exceed that.
 170  */
 171 struct brcmf_skbuff_cb {
 172         u16 if_flags;
 173         u32 htod;
 174         enum brcmf_fws_skb_state state;
 175         struct brcmf_fws_mac_descriptor *mac;
 176 };
 177
 178 /*
 179  * macro casting skbuff control buffer to struct brcmf_skbuff_cb.
 180  */
 181 #define brcmf_skbcb(skb)        ((struct brcmf_skbuff_cb *)((skb)->cb))
 182
 183 /*
 184  * sk_buff control if flags
 185  *
 186  *      b[11]  - packet sent upon firmware request.
 187  *      b[10]  - packet only contains signalling data.
 188  *      b[9]   - packet is a tx packet.
 189  *      b[8]   - packet uses FIFO credit (non-pspoll).
 190  *      b[7]   - interface in AP mode.
 191  *      b[6:4] - AC FIFO number.
 192  *      b[3:0] - interface index.
 193  */
 194 #define BRCMF_SKB_IF_FLAGS_REQUESTED_MASK       0x0800
 195 #define BRCMF_SKB_IF_FLAGS_REQUESTED_SHIFT      11
 196 #define BRCMF_SKB_IF_FLAGS_SIGNAL_ONLY_MASK     0x0400
 197 #define BRCMF_SKB_IF_FLAGS_SIGNAL_ONLY_SHIFT    10
 198 #define BRCMF_SKB_IF_FLAGS_TRANSMIT_MASK        0x0200
 199 #define BRCMF_SKB_IF_FLAGS_TRANSMIT_SHIFT       9
 200 #define BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK     0x0100
 201 #define BRCMF_SKB_IF_FLAGS_CREDITCHECK_SHIFT    8
 202 #define BRCMF_SKB_IF_FLAGS_IF_AP_MASK           0x0080
 203 #define BRCMF_SKB_IF_FLAGS_IF_AP_SHIFT          7
 204 #define BRCMF_SKB_IF_FLAGS_FIFO_MASK            0x0070
 205 #define BRCMF_SKB_IF_FLAGS_FIFO_SHIFT           4
 206 #define BRCMF_SKB_IF_FLAGS_INDEX_MASK           0x000f
 207 #define BRCMF_SKB_IF_FLAGS_INDEX_SHIFT          0
 208
 209 #define brcmf_skb_if_flags_set_field(skb, field, value) \
 210         brcmu_maskset16(&(brcmf_skbcb(skb)->if_flags), \
 211                         BRCMF_SKB_IF_FLAGS_ ## field ## _MASK, \
 212                         BRCMF_SKB_IF_FLAGS_ ## field ## _SHIFT, (value))
 213 #define brcmf_skb_if_flags_get_field(skb, field) \
 214         brcmu_maskget16(brcmf_skbcb(skb)->if_flags, \
 215                         BRCMF_SKB_IF_FLAGS_ ## field ## _MASK, \
 216                         BRCMF_SKB_IF_FLAGS_ ## field ## _SHIFT)
 217
 218 /*
 219  * sk_buff control packet identifier
 220  *
 221  * 32-bit packet identifier used in PKTTAG tlv from host to dongle.
 222  *
 223  * - Generated at the host (e.g. dhd)
 224  * - Seen as a generic sequence number by firmware except for the flags field.
 225  *
 226  * Generation   : b[31] => generation number for this packet [host->fw]
 227  *                         OR, current generation number [fw->host]
 228  * Flags        : b[30:27] => command, status flags
 229  * FIFO-AC      : b[26:24] => AC-FIFO id
 230  * h-slot       : b[23:8] => hanger-slot
 231  * freerun      : b[7:0] => A free running counter
 232  */
 233 #define BRCMF_SKB_HTOD_TAG_GENERATION_MASK              0x80000000
 234 #define BRCMF_SKB_HTOD_TAG_GENERATION_SHIFT             31
 235 #define BRCMF_SKB_HTOD_TAG_FLAGS_MASK                   0x78000000
 236 #define BRCMF_SKB_HTOD_TAG_FLAGS_SHIFT                  27
 237 #define BRCMF_SKB_HTOD_TAG_FIFO_MASK                    0x07000000
 238 #define BRCMF_SKB_HTOD_TAG_FIFO_SHIFT                   24
 239 #define BRCMF_SKB_HTOD_TAG_HSLOT_MASK                   0x00ffff00
 240 #define BRCMF_SKB_HTOD_TAG_HSLOT_SHIFT                  8
 241 #define BRCMF_SKB_HTOD_TAG_FREERUN_MASK                 0x000000ff
 242 #define BRCMF_SKB_HTOD_TAG_FREERUN_SHIFT                        0
 243
 244 #define brcmf_skb_htod_tag_set_field(skb, field, value) \
 245         brcmu_maskset32(&(brcmf_skbcb(skb)->htod), \
 246                         BRCMF_SKB_HTOD_TAG_ ## field ## _MASK, \
 247                         BRCMF_SKB_HTOD_TAG_ ## field ## _SHIFT, (value))
 248 #define brcmf_skb_htod_tag_get_field(skb, field) \
 249         brcmu_maskget32(brcmf_skbcb(skb)->htod, \
 250                         BRCMF_SKB_HTOD_TAG_ ## field ## _MASK, \
 251                         BRCMF_SKB_HTOD_TAG_ ## field ## _SHIFT)
 252
 253 #define BRCMF_FWS_TXSTAT_GENERATION_MASK        0x80000000
 254 #define BRCMF_FWS_TXSTAT_GENERATION_SHIFT       31
 255 #define BRCMF_FWS_TXSTAT_FLAGS_MASK             0x78000000
 256 #define BRCMF_FWS_TXSTAT_FLAGS_SHIFT            27
 257 #define BRCMF_FWS_TXSTAT_FIFO_MASK              0x07000000
 258 #define BRCMF_FWS_TXSTAT_FIFO_SHIFT             24
 259 #define BRCMF_FWS_TXSTAT_HSLOT_MASK             0x00FFFF00
 260 #define BRCMF_FWS_TXSTAT_HSLOT_SHIFT            8
 261 #define BRCMF_FWS_TXSTAT_PKTID_MASK             0x00FFFFFF
 262 #define BRCMF_FWS_TXSTAT_PKTID_SHIFT            0
 263
 264 #define brcmf_txstatus_get_field(txs, field) \
 265         brcmu_maskget32(txs, BRCMF_FWS_TXSTAT_ ## field ## _MASK, \
 266                         BRCMF_FWS_TXSTAT_ ## field ## _SHIFT)
 267
 268 /**
 269  * enum brcmf_fws_fifo - fifo indices used by dongle firmware.
 270  *
 271  * @BRCMF_FWS_FIFO_AC_BK: fifo for background traffic.
 272  * @BRCMF_FWS_FIFO_AC_BE: fifo for best-effort traffic.
 273  * @BRCMF_FWS_FIFO_AC_VI: fifo for video traffic.
 274  * @BRCMF_FWS_FIFO_AC_VO: fifo for voice traffic.
 275  * @BRCMF_FWS_FIFO_BCMC: fifo for broadcast/multicast (AP only).
 276  * @BRCMF_FWS_FIFO_ATIM: fifo for ATIM (AP only).
 277  * @BRCMF_FWS_FIFO_COUNT: number of fifos.
 278  */
 279 enum brcmf_fws_fifo {
 280         BRCMF_FWS_FIFO_AC_BK,
 281         BRCMF_FWS_FIFO_AC_BE,
 282         BRCMF_FWS_FIFO_AC_VI,
 283         BRCMF_FWS_FIFO_AC_VO,
 284         BRCMF_FWS_FIFO_BCMC,
 285         BRCMF_FWS_FIFO_ATIM,
 286         BRCMF_FWS_FIFO_COUNT
 287 };
 288
 289 /**
 290  * enum brcmf_fws_txstatus - txstatus flag values.
 291  *
 292  * @BRCMF_FWS_TXSTATUS_DISCARD:
 293  *      host is free to discard the packet.
 294  * @BRCMF_FWS_TXSTATUS_CORE_SUPPRESS:
 295  *      802.11 core suppressed the packet.
 296  * @BRCMF_FWS_TXSTATUS_FW_PS_SUPPRESS:
 297  *      firmware suppress the packet as device is already in PS mode.
 298  * @BRCMF_FWS_TXSTATUS_FW_TOSSED:
 299  *      firmware tossed the packet.
 300  */
 301 enum brcmf_fws_txstatus {
 302         BRCMF_FWS_TXSTATUS_DISCARD,
 303         BRCMF_FWS_TXSTATUS_CORE_SUPPRESS,
 304         BRCMF_FWS_TXSTATUS_FW_PS_SUPPRESS,
 305         BRCMF_FWS_TXSTATUS_FW_TOSSED
 306 };
 307
 308 enum brcmf_fws_fcmode {
 309         BRCMF_FWS_FCMODE_NONE,
 310         BRCMF_FWS_FCMODE_IMPLIED_CREDIT,
 311         BRCMF_FWS_FCMODE_EXPLICIT_CREDIT
 312 };
 313
 314 enum brcmf_fws_mac_desc_state {
 315         BRCMF_FWS_STATE_OPEN = 1,
 316         BRCMF_FWS_STATE_CLOSE
 317 };
 318
 319 /**
 320  * struct brcmf_fws_mac_descriptor - firmware signalling data per node/interface
 321  *
 322  * @occupied: slot is in use.
 323  * @mac_handle: handle for mac entry determined by firmware.
 324  * @interface_id: interface index.
 325  * @state: current state.
 326  * @suppressed: mac entry is suppressed.
 327  * @generation: generation bit.
 328  * @ac_bitmap: ac queue bitmap.
 329  * @requested_credit: credits requested by firmware.
 330  * @ea: ethernet address.
 331  * @seq: per-node free-running sequence.
 332  * @psq: power-save queue.
 333  * @transit_count: packet in transit to firmware.
 334  */
 335 struct brcmf_fws_mac_descriptor {
 336         u8 occupied;
 337         u8 mac_handle;
 338         u8 interface_id;
 339         u8 state;
 340         bool suppressed;
 341         u8 generation;
 342         u8 ac_bitmap;
 343         u8 requested_credit;
 344         u8 requested_packet;
 345         u8 ea[ETH_ALEN];
 346         u8 seq[BRCMF_FWS_FIFO_COUNT];
 347         struct pktq psq;
 348         int transit_count;
 349         int suppress_count;
 350         int suppr_transit_count;
 351         bool send_tim_signal;
 352         u8 traffic_pending_bmp;
 353         u8 traffic_lastreported_bmp;
 354 };
 355
 356 #define BRCMF_FWS_HANGER_MAXITEMS       1024
 357
 358 /**
 359  * enum brcmf_fws_hanger_item_state - state of hanger item.
 360  *
 361  * @BRCMF_FWS_HANGER_ITEM_STATE_FREE: item is free for use.
 362  * @BRCMF_FWS_HANGER_ITEM_STATE_INUSE: item is in use.
 363  * @BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED: item was suppressed.
 364  */
 365 enum brcmf_fws_hanger_item_state {
 366         BRCMF_FWS_HANGER_ITEM_STATE_FREE = 1,
 367         BRCMF_FWS_HANGER_ITEM_STATE_INUSE,
 368         BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED
 369 };
 370
 371
 372 /**
 373  * struct brcmf_fws_hanger_item - single entry for tx pending packet.
 374  *
 375  * @state: entry is either free or occupied.
 376  * @gen: generation.
 377  * @pkt: packet itself.
 378  */
 379 struct brcmf_fws_hanger_item {
 380         enum brcmf_fws_hanger_item_state state;
 381         u8 gen;
 382         struct sk_buff *pkt;
 383 };
 384
 385 /**
 386  * struct brcmf_fws_hanger - holds packets awaiting firmware txstatus.
 387  *
 388  * @pushed: packets pushed to await txstatus.
 389  * @popped: packets popped upon handling txstatus.
 390  * @failed_to_push: packets that could not be pushed.
 391  * @failed_to_pop: packets that could not be popped.
 392  * @failed_slotfind: packets for which failed to find an entry.
 393  * @slot_pos: last returned item index for a free entry.
 394  * @items: array of hanger items.
 395  */
 396 struct brcmf_fws_hanger {
 397         u32 pushed;
 398         u32 popped;
 399         u32 failed_to_push;
 400         u32 failed_to_pop;
 401         u32 failed_slotfind;
 402         u32 slot_pos;
 403         struct brcmf_fws_hanger_item items[BRCMF_FWS_HANGER_MAXITEMS];
 404 };
 405
 406 struct brcmf_fws_macdesc_table {
 407         struct brcmf_fws_mac_descriptor nodes[BRCMF_FWS_MAC_DESC_TABLE_SIZE];
 408         struct brcmf_fws_mac_descriptor iface[BRCMF_MAX_IFS];
 409         struct brcmf_fws_mac_descriptor other;
 410 };
 411
 412 struct brcmf_fws_info {
 413         struct brcmf_pub *drvr;
 414         struct brcmf_fws_stats stats;
 415         struct brcmf_fws_hanger hanger;
 416         enum brcmf_fws_fcmode fcmode;
 417         struct brcmf_fws_macdesc_table desc;
 418         struct workqueue_struct *fws_wq;
 419         struct work_struct fws_dequeue_work;
 420         u32 fifo_enqpkt[BRCMF_FWS_FIFO_COUNT];
 421         int fifo_credit[BRCMF_FWS_FIFO_COUNT];
 422         int deq_node_pos[BRCMF_FWS_FIFO_COUNT];
 423         u32 fifo_credit_map;
 424         u32 fifo_delay_map;
 425 };
 426
 427 /*
 428  * brcmf_fws_prio2fifo - mapping from 802.1d priority to firmware fifo index.
 429  */
 430 static const int brcmf_fws_prio2fifo[] = {
 431         BRCMF_FWS_FIFO_AC_BE,
 432         BRCMF_FWS_FIFO_AC_BK,
 433         BRCMF_FWS_FIFO_AC_BK,
 434         BRCMF_FWS_FIFO_AC_BE,
 435         BRCMF_FWS_FIFO_AC_VI,
 436         BRCMF_FWS_FIFO_AC_VI,
 437         BRCMF_FWS_FIFO_AC_VO,
 438         BRCMF_FWS_FIFO_AC_VO
 439 };
 440
 441 static int fcmode;
 442 module_param(fcmode, int, S_IRUSR);
 443 MODULE_PARM_DESC(fcmode, "mode of firmware signalled flow control");
 444
 445 #define BRCMF_FWS_TLV_DEF(name, id, len) \
 446         case BRCMF_FWS_TYPE_ ## name: \
 447                 return len;
 448
 449 /**
 450  * brcmf_fws_get_tlv_len() - returns defined length for given tlv id.
 451  *
 452  * @fws: firmware-signalling information.
 453  * @id: identifier of the TLV.
 454  *
 455  * Return: the specified length for the given TLV; Otherwise -EINVAL.
 456  */
 457 static int brcmf_fws_get_tlv_len(struct brcmf_fws_info *fws,
 458                                  enum brcmf_fws_tlv_type id)
 459 {
 460         switch (id) {
 461         BRCMF_FWS_TLV_DEFLIST
 462         default:
 463                 fws->stats.tlv_invalid_type++;
 464                 break;
 465         }
 466         return -EINVAL;
 467 }
 468 #undef BRCMF_FWS_TLV_DEF
 469
 470 static bool brcmf_fws_ifidx_match(struct sk_buff *skb, void *arg)
 471 {
 472         u32 ifidx = brcmf_skb_if_flags_get_field(skb, INDEX);
 473         return ifidx == *(int *)arg;
 474 }
 475
 476 static void brcmf_fws_psq_flush(struct brcmf_fws_info *fws, struct pktq *q,
 477                                 int ifidx)
 478 {
 479         bool (*matchfn)(struct sk_buff *, void *) = NULL;
 480         struct sk_buff *skb;
 481         int prec;
 482
 483         if (ifidx != -1)
 484                 matchfn = brcmf_fws_ifidx_match;
 485         for (prec = 0; prec < q->num_prec; prec++) {
 486                 skb = brcmu_pktq_pdeq_match(q, prec, matchfn, &ifidx);
 487                 while (skb) {
 488                         brcmu_pkt_buf_free_skb(skb);
 489                         skb = brcmu_pktq_pdeq_match(q, prec, matchfn, &ifidx);
 490                 }
 491         }
 492 }
 493
 494 static void brcmf_fws_hanger_init(struct brcmf_fws_hanger *hanger)
 495 {
 496         int i;
 497
 498         brcmf_dbg(TRACE, "enter\n");
 499         memset(hanger, 0, sizeof(*hanger));
 500         for (i = 0; i < ARRAY_SIZE(hanger->items); i++)
 501                 hanger->items[i].state = BRCMF_FWS_HANGER_ITEM_STATE_FREE;
 502 }
 503
 504 static u32 brcmf_fws_hanger_get_free_slot(struct brcmf_fws_hanger *h)
 505 {
 506         u32 i;
 507
 508         brcmf_dbg(TRACE, "enter\n");
 509         i = (h->slot_pos + 1) % BRCMF_FWS_HANGER_MAXITEMS;
 510
 511         while (i != h->slot_pos) {
 512                 if (h->items[i].state == BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
 513                         h->slot_pos = i;
 514                         goto done;
 515                 }
 516                 i++;
 517                 if (i == BRCMF_FWS_HANGER_MAXITEMS)
 518                         i = 0;
 519         }
 520         brcmf_err("all slots occupied\n");
 521         h->failed_slotfind++;
 522         i = BRCMF_FWS_HANGER_MAXITEMS;
 523 done:
 524         brcmf_dbg(TRACE, "exit: %d\n", i);
 525         return i;
 526 }
 527
 528 static int brcmf_fws_hanger_pushpkt(struct brcmf_fws_hanger *h,
 529                                            struct sk_buff *pkt, u32 slot_id)
 530 {
 531         brcmf_dbg(TRACE, "enter\n");
 532         if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
 533                 return -ENOENT;
 534
 535         if (h->items[slot_id].state != BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
 536                 brcmf_err("slot is not free\n");
 537                 h->failed_to_push++;
 538                 return -EINVAL;
 539         }
 540
 541         h->items[slot_id].state = BRCMF_FWS_HANGER_ITEM_STATE_INUSE;
 542         h->items[slot_id].pkt = pkt;
 543         h->pushed++;
 544         return 0;
 545 }
 546
 547 static int brcmf_fws_hanger_poppkt(struct brcmf_fws_hanger *h,
 548                                           u32 slot_id, struct sk_buff **pktout,
 549                                           bool remove_item)
 550 {
 551         brcmf_dbg(TRACE, "enter\n");
 552         if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
 553                 return -ENOENT;
 554
 555         if (h->items[slot_id].state == BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
 556                 brcmf_err("entry not in use\n");
 557                 h->failed_to_pop++;
 558                 return -EINVAL;
 559         }
 560
 561         *pktout = h->items[slot_id].pkt;
 562         if (remove_item) {
 563                 h->items[slot_id].state = BRCMF_FWS_HANGER_ITEM_STATE_FREE;
 564                 h->items[slot_id].pkt = NULL;
 565                 h->items[slot_id].gen = 0xff;
 566                 h->popped++;
 567         }
 568         return 0;
 569 }
 570
 571 static int brcmf_fws_hanger_mark_suppressed(struct brcmf_fws_hanger *h,
 572                                                    u32 slot_id, u8 gen)
 573 {
 574         brcmf_dbg(TRACE, "enter\n");
 575
 576         if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
 577                 return -ENOENT;
 578
 579         h->items[slot_id].gen = gen;
 580
 581         if (h->items[slot_id].state != BRCMF_FWS_HANGER_ITEM_STATE_INUSE) {
 582                 brcmf_err("entry not in use\n");
 583                 return -EINVAL;
 584         }
 585
 586         h->items[slot_id].state = BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED;
 587         return 0;
 588 }
 589
 590 static int brcmf_fws_hanger_get_genbit(struct brcmf_fws_hanger *hanger,
 591                                               struct sk_buff *pkt, u32 slot_id,
 592                                               int *gen)
 593 {
 594         brcmf_dbg(TRACE, "enter\n");
 595         *gen = 0xff;
 596
 597         if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
 598                 return -ENOENT;
 599
 600         if (hanger->items[slot_id].state == BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
 601                 brcmf_err("slot not in use\n");
 602                 return -EINVAL;
 603         }
 604
 605         *gen = hanger->items[slot_id].gen;
 606         return 0;
 607 }
 608
 609 static void brcmf_fws_hanger_cleanup(struct brcmf_fws_info *fws,
 610                                      bool (*fn)(struct sk_buff *, void *),
 611                                      int ifidx)
 612 {
 613         struct brcmf_fws_hanger *h = &fws->hanger;
 614         struct sk_buff *skb;
 615         int i;
 616         enum brcmf_fws_hanger_item_state s;
 617
 618         brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
 619         for (i = 0; i < ARRAY_SIZE(h->items); i++) {
 620                 s = h->items[i].state;
 621                 if (s == BRCMF_FWS_HANGER_ITEM_STATE_INUSE ||
 622                     s == BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED) {
 623                         skb = h->items[i].pkt;
 624                         if (fn == NULL || fn(skb, &ifidx)) {
 625                                 /* suppress packets freed from psq */
 626                                 if (s == BRCMF_FWS_HANGER_ITEM_STATE_INUSE)
 627                                         brcmu_pkt_buf_free_skb(skb);
 628                                 h->items[i].state =
 629                                         BRCMF_FWS_HANGER_ITEM_STATE_FREE;
 630                         }
 631                 }
 632         }
 633 }
 634
 635 static void brcmf_fws_init_mac_descriptor(struct brcmf_fws_mac_descriptor *desc,
 636                                           u8 *addr, u8 ifidx)
 637 {
 638         brcmf_dbg(TRACE,
 639                   "enter: desc %p ea=%pM, ifidx=%u\n", desc, addr, ifidx);
 640         desc->occupied = 1;
 641         desc->state = BRCMF_FWS_STATE_OPEN;
 642         desc->requested_credit = 0;
 643         /* depending on use may need ifp->bssidx instead */
 644         desc->interface_id = ifidx;
 645         desc->ac_bitmap = 0xff; /* update this when handling APSD */
 646         if (addr)
 647                 memcpy(&desc->ea[0], addr, ETH_ALEN);
 648 }
 649
 650 static
 651 void brcmf_fws_clear_mac_descriptor(struct brcmf_fws_mac_descriptor *desc)
 652 {
 653         brcmf_dbg(TRACE,
 654                   "enter: ea=%pM, ifidx=%u\n", desc->ea, desc->interface_id);
 655         desc->occupied = 0;
 656         desc->state = BRCMF_FWS_STATE_CLOSE;
 657         desc->requested_credit = 0;
 658 }
 659
 660 static struct brcmf_fws_mac_descriptor *
 661 brcmf_fws_mac_descriptor_lookup(struct brcmf_fws_info *fws, u8 *ea)
 662 {
 663         struct brcmf_fws_mac_descriptor *entry;
 664         int i;
 665
 666         brcmf_dbg(TRACE, "enter: ea=%pM\n", ea);
 667         if (ea == NULL)
 668                 return ERR_PTR(-EINVAL);
 669
 670         entry = &fws->desc.nodes[0];
 671         for (i = 0; i < ARRAY_SIZE(fws->desc.nodes); i++) {
 672                 if (entry->occupied && !memcmp(entry->ea, ea, ETH_ALEN))
 673                         return entry;
 674                 entry++;
 675         }
 676
 677         return ERR_PTR(-ENOENT);
 678 }
 679
 680 static struct brcmf_fws_mac_descriptor*
 681 brcmf_fws_find_mac_desc(struct brcmf_fws_info *fws, int ifidx, u8 *da)
 682 {
 683         struct brcmf_fws_mac_descriptor *entry = &fws->desc.other;
 684         struct brcmf_if *ifp;
 685         bool multicast;
 686
 687         brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
 688
 689         multicast = is_multicast_ether_addr(da);
 690         ifp = fws->drvr->iflist[ifidx ? ifidx + 1 : 0];
 691         if (WARN_ON(!ifp))
 692                 goto done;
 693
 694         /* Multicast destination and P2P clients get the interface entry.
 695          * STA gets the interface entry if there is no exact match. For
 696          * example, TDLS destinations have their own entry.
 697          */
 698         entry = NULL;
 699         if (multicast && ifp->fws_desc)
 700                 entry = ifp->fws_desc;
 701
 702         if (entry != NULL && multicast)
 703                 goto done;
 704
 705         entry = brcmf_fws_mac_descriptor_lookup(fws, da);
 706         if (IS_ERR(entry))
 707                 entry = &fws->desc.other;
 708
 709 done:
 710         brcmf_dbg(TRACE, "exit: entry=%p\n", entry);
 711         return entry;
 712 }
 713
 714 static bool brcmf_fws_mac_desc_ready(struct brcmf_fws_mac_descriptor *entry,
 715                                      int fifo)
 716 {
 717         bool ready;
 718
 719         /*
 720          * destination entry is ready when firmware says it is OPEN
 721          * and there are no packets enqueued for it.
 722          */
 723         ready = entry->state == BRCMF_FWS_STATE_OPEN &&
 724                 !entry->suppressed &&
 725                 brcmu_pktq_mlen(&entry->psq, 3 << (fifo * 2)) == 0;
 726
 727         /*
 728          * Or when the destination entry is CLOSED, but firmware has
 729          * specifically requested packets for this entry.
 730          */
 731         ready = ready || (entry->state == BRCMF_FWS_STATE_CLOSE &&
 732                 (entry->requested_credit + entry->requested_packet));
 733         return ready;
 734 }
 735
 736 static void brcmf_fws_mac_desc_cleanup(struct brcmf_fws_info *fws,
 737                                        struct brcmf_fws_mac_descriptor *entry,
 738                                        int ifidx)
 739 {
 740         brcmf_dbg(TRACE, "enter: entry=(ea=%pM, ifid=%d), ifidx=%d\n",
 741                   entry->ea, entry->interface_id, ifidx);
 742         if (entry->occupied && (ifidx == -1 || ifidx == entry->interface_id)) {
 743                 brcmf_dbg(TRACE, "flush psq: ifidx=%d, qlen=%d\n",
 744                           ifidx, entry->psq.len);
 745                 brcmf_fws_psq_flush(fws, &entry->psq, ifidx);
 746                 entry->occupied = !!(entry->psq.len);
 747         }
 748 }
 749
 750 static void brcmf_fws_bus_txq_cleanup(struct brcmf_fws_info *fws,
 751                                       bool (*fn)(struct sk_buff *, void *),
 752                                       int ifidx)
 753 {
 754         struct brcmf_fws_hanger_item *hi;
 755         struct pktq *txq;
 756         struct sk_buff *skb;
 757         int prec;
 758         u32 hslot;
 759
 760         brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
 761         txq = brcmf_bus_gettxq(fws->drvr->bus_if);
 762         if (IS_ERR(txq)) {
 763                 brcmf_dbg(TRACE, "no txq to clean up\n");
 764                 return;
 765         }
 766
 767         for (prec = 0; prec < txq->num_prec; prec++) {
 768                 skb = brcmu_pktq_pdeq_match(txq, prec, fn, &ifidx);
 769                 while (skb) {
 770                         hslot = brcmf_skb_htod_tag_get_field(skb, HSLOT);
 771                         hi = &fws->hanger.items[hslot];
 772                         WARN_ON(skb != hi->pkt);
 773                         hi->state = BRCMF_FWS_HANGER_ITEM_STATE_FREE;
 774                         brcmu_pkt_buf_free_skb(skb);
 775                         skb = brcmu_pktq_pdeq_match(txq, prec, fn, &ifidx);
 776                 }
 777         }
 778 }
 779
 780 static void brcmf_fws_cleanup(struct brcmf_fws_info *fws, int ifidx)
 781 {
 782         int i;
 783         struct brcmf_fws_mac_descriptor *table;
 784         bool (*matchfn)(struct sk_buff *, void *) = NULL;
 785
 786         brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
 787         if (fws == NULL)
 788                 return;
 789
 790         if (ifidx != -1)
 791                 matchfn = brcmf_fws_ifidx_match;
 792
 793         /* cleanup individual nodes */
 794         table = &fws->desc.nodes[0];
 795         for (i = 0; i < ARRAY_SIZE(fws->desc.nodes); i++)
 796                 brcmf_fws_mac_desc_cleanup(fws, &table[i], ifidx);
 797
 798         brcmf_fws_mac_desc_cleanup(fws, &fws->desc.other, ifidx);
 799         brcmf_fws_bus_txq_cleanup(fws, matchfn, ifidx);
 800         brcmf_fws_hanger_cleanup(fws, matchfn, ifidx);
 801 }
 802
 803 static void brcmf_fws_tim_update(struct brcmf_fws_info *ctx,
 804                                  struct brcmf_fws_mac_descriptor *entry,
 805                                  int prec)
 806 {
 807         brcmf_dbg(TRACE, "enter: ea=%pM\n", entry->ea);
 808         if (entry->state == BRCMF_FWS_STATE_CLOSE) {
 809                 /* check delayedQ and suppressQ in one call using bitmap */
 810                 if (brcmu_pktq_mlen(&entry->psq, 3 << (prec * 2)) == 0)
 811                         entry->traffic_pending_bmp =
 812                                 entry->traffic_pending_bmp & ~NBITVAL(prec);
 813                 else
 814                         entry->traffic_pending_bmp =
 815                                 entry->traffic_pending_bmp | NBITVAL(prec);
 816         }
 817         /* request a TIM update to firmware at the next piggyback opportunity */
 818         if (entry->traffic_lastreported_bmp != entry->traffic_pending_bmp)
 819                 entry->send_tim_signal = true;
 820 }
 821
 822 static void
 823 brcmf_fws_flow_control_check(struct brcmf_fws_info *fws, struct pktq *pq,
 824                              u8 if_id)
 825 {
 826         struct brcmf_if *ifp = fws->drvr->iflist[if_id];
 827
 828         brcmf_dbg(TRACE,
 829                   "enter: bssidx=%d, ifidx=%d\n", ifp->bssidx, ifp->ifidx);
 830         if (WARN_ON(!ifp))
 831                 return;
 832
 833         if ((ifp->netif_stop & BRCMF_NETIF_STOP_REASON_FWS_FC) &&
 834             pq->len <= BRCMF_FWS_FLOWCONTROL_LOWATER)
 835                 brcmf_txflowblock_if(ifp,
 836                                      BRCMF_NETIF_STOP_REASON_FWS_FC, false);
 837         if (!(ifp->netif_stop & BRCMF_NETIF_STOP_REASON_FWS_FC) &&
 838             pq->len >= BRCMF_FWS_FLOWCONTROL_HIWATER)
 839                 brcmf_txflowblock_if(ifp, BRCMF_NETIF_STOP_REASON_FWS_FC, true);
 840         return;
 841 }
 842
 843 static int brcmf_fws_rssi_indicate(struct brcmf_fws_info *fws, s8 rssi)
 844 {
 845         brcmf_dbg(CTL, "rssi %d\n", rssi);
 846         return 0;
 847 }
 848
 849 static
 850 int brcmf_fws_macdesc_indicate(struct brcmf_fws_info *fws, u8 type, u8 *data)
 851 {
 852         struct brcmf_fws_mac_descriptor *entry, *existing;
 853         u8 mac_handle;
 854         u8 ifidx;
 855         u8 *addr;
 856
 857         mac_handle = *data++;
 858         ifidx = *data++;
 859         addr = data;
 860
 861         entry = &fws->desc.nodes[mac_handle & 0x1F];
 862         if (type == BRCMF_FWS_TYPE_MACDESC_DEL) {
 863                 brcmf_dbg(TRACE, "deleting mac %pM idx %d\n", addr, ifidx);
 864                 if (entry->occupied)
 865                         brcmf_fws_clear_mac_descriptor(entry);
 866                 else
 867                         fws->stats.mac_update_failed++;
 868                 return 0;
 869         }
 870
 871         brcmf_dbg(TRACE,
 872                   "add mac %pM handle %u idx %d\n", addr, mac_handle, ifidx);
 873         existing = brcmf_fws_mac_descriptor_lookup(fws, addr);
 874         if (IS_ERR(existing)) {
 875                 if (!entry->occupied) {
 876                         entry->mac_handle = mac_handle;
 877                         brcmf_fws_init_mac_descriptor(entry, addr, ifidx);
 878                         brcmu_pktq_init(&entry->psq, BRCMF_FWS_PSQ_PREC_COUNT,
 879                                         BRCMF_FWS_PSQ_LEN);
 880                 } else {
 881                         fws->stats.mac_update_failed++;
 882                 }
 883         } else {
 884                 if (entry != existing) {
 885                         brcmf_dbg(TRACE, "relocate mac\n");
 886                         memcpy(entry, existing,
 887                                offsetof(struct brcmf_fws_mac_descriptor, psq));
 888                         entry->mac_handle = mac_handle;
 889                         brcmf_fws_clear_mac_descriptor(existing);
 890                 } else {
 891                         brcmf_dbg(TRACE, "use existing\n");
 892                         WARN_ON(entry->mac_handle != mac_handle);
 893                         /* TODO: what should we do here: continue, reinit, .. */
 894                 }
 895         }
 896         return 0;
 897 }
 898
 899 static int brcmf_fws_macdesc_state_indicate(struct brcmf_fws_info *fws,
 900                                             u8 type, u8 *data)
 901 {
 902         struct brcmf_fws_mac_descriptor *entry;
 903         u8 mac_handle;
 904         int i;
 905
 906         mac_handle = data[0];
 907         entry = &fws->desc.nodes[mac_handle & 0x1F];
 908         if (!entry->occupied) {
 909                 fws->stats.mac_ps_update_failed++;
 910                 return -ESRCH;
 911         }
 912
 913         /* a state update should wipe old credits? */
 914         entry->requested_credit = 0;
 915         if (type == BRCMF_FWS_TYPE_MAC_OPEN) {
 916                 entry->state = BRCMF_FWS_STATE_OPEN;
 917         } else {
 918                 entry->state = BRCMF_FWS_STATE_CLOSE;
 919                 for (i = BRCMF_FWS_FIFO_AC_BE; i < NL80211_NUM_ACS; i++)
 920                         brcmf_fws_tim_update(fws, entry, i);
 921         }
 922         return 0;
 923 }
 924
 925 static int brcmf_fws_interface_state_indicate(struct brcmf_fws_info *fws,
 926                                               u8 type, u8 *data)
 927 {
 928         struct brcmf_fws_mac_descriptor *entry;
 929         u8 ifidx;
 930         int ret;
 931
 932         ifidx = data[0];
 933
 934         brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
 935         if (ifidx >= BRCMF_MAX_IFS) {
 936                 ret = -ERANGE;
 937                 goto fail;
 938         }
 939
 940         entry = &fws->desc.iface[ifidx];
 941         if (!entry->occupied) {
 942                 ret = -ESRCH;
 943                 goto fail;
 944         }
 945
 946         switch (type) {
 947         case BRCMF_FWS_TYPE_INTERFACE_OPEN:
 948                 entry->state = BRCMF_FWS_STATE_OPEN;
 949                 return 0;
 950         case BRCMF_FWS_TYPE_INTERFACE_CLOSE:
 951                 entry->state = BRCMF_FWS_STATE_CLOSE;
 952                 return 0;
 953         default:
 954                 ret = -EINVAL;
 955                 break;
 956         }
 957 fail:
 958         fws->stats.if_update_failed++;
 959         return ret;
 960 }
 961
 962 static int brcmf_fws_request_indicate(struct brcmf_fws_info *fws, u8 type,
 963                                       u8 *data)
 964 {
 965         struct brcmf_fws_mac_descriptor *entry;
 966
 967         entry = &fws->desc.nodes[data[1] & 0x1F];
 968         if (!entry->occupied) {
 969                 if (type == BRCMF_FWS_TYPE_MAC_REQUEST_CREDIT)
 970                         fws->stats.credit_request_failed++;
 971                 else
 972                         fws->stats.packet_request_failed++;
 973                 return -ESRCH;
 974         }
 975
 976         if (type == BRCMF_FWS_TYPE_MAC_REQUEST_CREDIT)
 977                 entry->requested_credit = data[0];
 978         else
 979                 entry->requested_packet = data[0];
 980
 981         entry->ac_bitmap = data[2];
 982         return 0;
 983 }
 984
 985 static void brcmf_fws_return_credits(struct brcmf_fws_info *fws,
 986                                      u8 fifo, u8 credits)
 987 {
 988         if (!credits)
 989                 return;
 990
 991         fws->fifo_credit_map |= 1 << fifo;
 992         fws->fifo_credit[fifo] += credits;
 993 }
 994
 995 static void brcmf_fws_schedule_deq(struct brcmf_fws_info *fws)
 996 {
 997         /* only schedule dequeue when there are credits for delayed traffic */
 998         if (fws->fifo_credit_map & fws->fifo_delay_map)
 999                 queue_work(fws->fws_wq, &fws->fws_dequeue_work);
1000 }
1001
1002 static void brcmf_skb_pick_up_credit(struct brcmf_fws_info *fws, int fifo,
1003                                      struct sk_buff *p)
1004 {
1005         struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(p)->mac;
1006
1007         if (brcmf_skbcb(p)->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK) {
1008                 if (fws->fcmode != BRCMF_FWS_FCMODE_IMPLIED_CREDIT)
1009                         return;
1010                 brcmf_fws_return_credits(fws, fifo, 1);
1011         } else {
1012                 /*
1013                  * if this packet did not count against FIFO credit, it
1014                  * must have taken a requested_credit from the destination
1015                  * entry (for pspoll etc.)
1016                  */
1017                 if (!brcmf_skb_if_flags_get_field(p, REQUESTED))
1018                         entry->requested_credit++;
1019         }
1020         brcmf_fws_schedule_deq(fws);
1021 }
1022
1023 static int brcmf_fws_enq(struct brcmf_fws_info *fws,
1024                          enum brcmf_fws_skb_state state, int fifo,
1025                          struct sk_buff *p)
1026 {
1027         int prec = 2 * fifo;
1028         u32 *qfull_stat = &fws->stats.delayq_full_error;
1029
1030         struct brcmf_fws_mac_descriptor *entry;
1031
1032         entry = brcmf_skbcb(p)->mac;
1033         if (entry == NULL) {
1034                 brcmf_err("no mac descriptor found for skb %p\n", p);
1035                 return -ENOENT;
1036         }
1037
1038         brcmf_dbg(TRACE, "enter: ea=%pM, qlen=%d\n", entry->ea, entry->psq.len);
1039         if (state == BRCMF_FWS_SKBSTATE_SUPPRESSED) {
1040                 prec += 1;
1041                 qfull_stat = &fws->stats.supprq_full_error;
1042         }
1043
1044         if (brcmu_pktq_penq(&entry->psq, prec, p) == NULL) {
1045                 *qfull_stat += 1;
1046                 return -ENFILE;
1047         }
1048
1049         /* increment total enqueued packet count */
1050         fws->fifo_delay_map |= 1 << fifo;
1051         fws->fifo_enqpkt[fifo]++;
1052
1053         /* update the sk_buff state */
1054         brcmf_skbcb(p)->state = state;
1055         if (state == BRCMF_FWS_SKBSTATE_SUPPRESSED)
1056                 entry->suppress_count++;
1057
1058         /*
1059          * A packet has been pushed so update traffic
1060          * availability bitmap, if applicable
1061          */
1062         brcmf_fws_tim_update(fws, entry, fifo);
1063         brcmf_fws_flow_control_check(fws, &entry->psq,
1064                                      brcmf_skb_if_flags_get_field(p, INDEX));
1065         return 0;
1066 }
1067
1068 static struct sk_buff *brcmf_fws_deq(struct brcmf_fws_info *fws, int fifo)
1069 {
1070         struct brcmf_fws_mac_descriptor *table;
1071         struct brcmf_fws_mac_descriptor *entry;
1072         struct sk_buff *p;
1073         int use_credit = 1;
1074         int num_nodes;
1075         int node_pos;
1076         int prec_out;
1077         int pmsk = 3;
1078         int i;
1079
1080         table = (struct brcmf_fws_mac_descriptor *)&fws->desc;
1081         num_nodes = sizeof(fws->desc) / sizeof(struct brcmf_fws_mac_descriptor);
1082         node_pos = fws->deq_node_pos[fifo];
1083
1084         for (i = 0; i < num_nodes; i++) {
1085                 entry = &table[(node_pos + i) % num_nodes];
1086                 if (!entry->occupied)
1087                         continue;
1088
1089                 if (entry->suppressed)
1090                         pmsk = 2;
1091                 p = brcmu_pktq_mdeq(&entry->psq, pmsk << (fifo * 2), &prec_out);
1092                 if (p == NULL) {
1093                         if (entry->suppressed) {
1094                                 if (entry->suppr_transit_count >
1095                                     entry->suppress_count)
1096                                         return NULL;
1097                                 entry->suppressed = false;
1098                                 p = brcmu_pktq_mdeq(&entry->psq,
1099                                                     1 << (fifo * 2), &prec_out);
1100                         }
1101                 }
1102                 if  (p == NULL)
1103                         continue;
1104
1105                 /* did the packet come from suppress sub-queue? */
1106                 if (entry->requested_credit > 0) {
1107                         entry->requested_credit--;
1108                         /*
1109                          * if the packet was pulled out while destination is in
1110                          * closed state but had a non-zero packets requested,
1111                          * then this should not count against the FIFO credit.
1112                          * That is due to the fact that the firmware will
1113                          * most likely hold onto this packet until a suitable
1114                          * time later to push it to the appropriate AC FIFO.
1115                          */
1116                         if (entry->state == BRCMF_FWS_STATE_CLOSE)
1117                                 use_credit = 0;
1118                 } else if (entry->requested_packet > 0) {
1119                         entry->requested_packet--;
1120                         brcmf_skb_if_flags_set_field(p, REQUESTED, 1);
1121                         if (entry->state == BRCMF_FWS_STATE_CLOSE)
1122                                 use_credit = 0;
1123                 }
1124                 brcmf_skb_if_flags_set_field(p, CREDITCHECK, use_credit);
1125
1126                 /* move dequeue position to ensure fair round-robin */
1127                 fws->deq_node_pos[fifo] = (node_pos + i + 1) % num_nodes;
1128                 brcmf_fws_flow_control_check(fws, &entry->psq,
1129                                              brcmf_skb_if_flags_get_field(p,
1130                                                                           INDEX)
1131                                              );
1132                 /*
1133                  * A packet has been picked up, update traffic
1134                  * availability bitmap, if applicable
1135                  */
1136                 brcmf_fws_tim_update(fws, entry, fifo);
1137
1138                 /*
1139                  * decrement total enqueued fifo packets and
1140                  * clear delay bitmap if done.
1141                  */
1142                 fws->fifo_enqpkt[fifo]--;
1143                 if (fws->fifo_enqpkt[fifo] == 0)
1144                         fws->fifo_delay_map &= ~(1 << fifo);
1145                 goto done;
1146         }
1147         p = NULL;
1148 done:
1149         brcmf_dbg(TRACE, "exit: fifo %d skb %p\n", fifo, p);
1150         return p;
1151 }
1152
1153 static int brcmf_fws_txstatus_suppressed(struct brcmf_fws_info *fws, int fifo,
1154                                          struct sk_buff *skb, u32 genbit)
1155 {
1156         struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(skb)->mac;
1157         u32 hslot;
1158         int ret;
1159
1160         hslot = brcmf_skb_htod_tag_get_field(skb, HSLOT);
1161
1162         /* this packet was suppressed */
1163         if (!entry->suppressed || entry->generation != genbit) {
1164                 entry->suppressed = true;
1165                 entry->suppress_count = brcmu_pktq_mlen(&entry->psq,
1166                                                         1 << (fifo * 2 + 1));
1167                 entry->suppr_transit_count = entry->transit_count;
1168         }
1169
1170         entry->generation = genbit;
1171
1172         ret = brcmf_fws_enq(fws, BRCMF_FWS_SKBSTATE_SUPPRESSED, fifo, skb);
1173         if (ret != 0) {
1174                 /* suppress q is full, drop this packet */
1175                 brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &skb,
1176                                         true);
1177         } else {
1178                 /*
1179                  * Mark suppressed to avoid a double free during
1180                  * wlfc cleanup
1181                  */
1182                 brcmf_fws_hanger_mark_suppressed(&fws->hanger, hslot,
1183                                                  genbit);
1184                 entry->suppress_count++;
1185         }
1186
1187         return ret;
1188 }
1189
1190 static int
1191 brcmf_fws_txstatus_process(struct brcmf_fws_info *fws, u8 flags, u32 hslot,
1192                            u32 genbit)
1193 {
1194         u32 fifo;
1195         int ret;
1196         bool remove_from_hanger = true;
1197         struct sk_buff *skb;
1198         struct brcmf_fws_mac_descriptor *entry = NULL;
1199
1200         brcmf_dbg(TRACE, "status: flags=0x%X, hslot=%d\n",
1201                   flags, hslot);
1202
1203         if (flags == BRCMF_FWS_TXSTATUS_DISCARD)
1204                 fws->stats.txs_discard++;
1205         else if (flags == BRCMF_FWS_TXSTATUS_CORE_SUPPRESS) {
1206                 fws->stats.txs_supp_core++;
1207                 remove_from_hanger = false;
1208         } else if (flags == BRCMF_FWS_TXSTATUS_FW_PS_SUPPRESS) {
1209                 fws->stats.txs_supp_ps++;
1210                 remove_from_hanger = false;
1211         } else if (flags == BRCMF_FWS_TXSTATUS_FW_TOSSED)
1212                 fws->stats.txs_tossed++;
1213         else
1214                 brcmf_err("unexpected txstatus\n");
1215
1216         ret = brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &skb,
1217                                       remove_from_hanger);
1218         if (ret != 0) {
1219                 brcmf_err("no packet in hanger slot: hslot=%d\n", hslot);
1220                 return ret;
1221         }
1222
1223         entry = brcmf_skbcb(skb)->mac;
1224         if (WARN_ON(!entry)) {
1225                 brcmu_pkt_buf_free_skb(skb);
1226                 return -EINVAL;
1227         }
1228
1229         /* pick up the implicit credit from this packet */
1230         fifo = brcmf_skb_htod_tag_get_field(skb, FIFO);
1231         brcmf_skb_pick_up_credit(fws, fifo, skb);
1232
1233         if (!remove_from_hanger)
1234                 ret = brcmf_fws_txstatus_suppressed(fws, fifo, skb, genbit);
1235
1236         if (remove_from_hanger || ret) {
1237                 entry->transit_count--;
1238                 if (entry->suppressed)
1239                         entry->suppr_transit_count--;
1240
1241                 brcmf_txfinalize(fws->drvr, skb, true);
1242         }
1243         return 0;
1244 }
1245
1246 static int brcmf_fws_fifocreditback_indicate(struct brcmf_fws_info *fws,
1247                                              u8 *data)
1248 {
1249         int i;
1250
1251         if (fws->fcmode != BRCMF_FWS_FCMODE_EXPLICIT_CREDIT) {
1252                 brcmf_dbg(INFO, "ignored\n");
1253                 return 0;
1254         }
1255
1256         brcmf_dbg(TRACE, "enter: data %pM\n", data);
1257         for (i = 0; i < BRCMF_FWS_FIFO_COUNT; i++)
1258                 brcmf_fws_return_credits(fws, i, data[i]);
1259
1260         brcmf_dbg(INFO, "map: credit %x delay %x\n", fws->fifo_credit_map,
1261                   fws->fifo_delay_map);
1262         brcmf_fws_schedule_deq(fws);
1263         return 0;
1264 }
1265
1266 static int brcmf_fws_txstatus_indicate(struct brcmf_fws_info *fws, u8 *data)
1267 {
1268         __le32 status_le;
1269         u32 status;
1270         u32 hslot;
1271         u32 genbit;
1272         u8 flags;
1273
1274         fws->stats.txs_indicate++;
1275         memcpy(&status_le, data, sizeof(status_le));
1276         status = le32_to_cpu(status_le);
1277         flags = brcmf_txstatus_get_field(status, FLAGS);
1278         hslot = brcmf_txstatus_get_field(status, HSLOT);
1279         genbit = brcmf_txstatus_get_field(status, GENERATION);
1280
1281         return brcmf_fws_txstatus_process(fws, flags, hslot, genbit);
1282 }
1283
1284 static int brcmf_fws_dbg_seqnum_check(struct brcmf_fws_info *fws, u8 *data)
1285 {
1286         __le32 timestamp;
1287
1288         memcpy(&timestamp, &data[2], sizeof(timestamp));
1289         brcmf_dbg(INFO, "received: seq %d, timestamp %d\n", data[1],
1290                   le32_to_cpu(timestamp));
1291         return 0;
1292 }
1293
1294 /* using macro so sparse checking does not complain
1295  * about locking imbalance.
1296  */
1297 #define brcmf_fws_lock(drvr, flags)                             \
1298 do {                                                            \
1299         flags = 0;                                              \
1300         spin_lock_irqsave(&((drvr)->fws_spinlock), (flags));    \
1301 } while (0)
1302
1303 /* using macro so sparse checking does not complain
1304  * about locking imbalance.
1305  */
1306 #define brcmf_fws_unlock(drvr, flags) \
1307         spin_unlock_irqrestore(&((drvr)->fws_spinlock), (flags))
1308
1309 static int brcmf_fws_notify_credit_map(struct brcmf_if *ifp,
1310                                        const struct brcmf_event_msg *e,
1311                                        void *data)
1312 {
1313         struct brcmf_fws_info *fws = ifp->drvr->fws;
1314         int i;
1315         ulong flags;
1316         u8 *credits = data;
1317
1318         if (e->datalen < BRCMF_FWS_FIFO_COUNT) {
1319                 brcmf_err("event payload too small (%d)\n", e->datalen);
1320                 return -EINVAL;
1321         }
1322
1323         brcmf_dbg(TRACE, "enter: credits %pM\n", credits);
1324         brcmf_fws_lock(ifp->drvr, flags);
1325         for (i = 0; i < ARRAY_SIZE(fws->fifo_credit); i++) {
1326                 if (*credits)
1327                         fws->fifo_credit_map |= 1 << i;
1328                 else
1329                         fws->fifo_credit_map &= ~(1 << i);
1330                 fws->fifo_credit[i] = *credits++;
1331         }
1332         brcmf_fws_schedule_deq(fws);
1333         brcmf_fws_unlock(ifp->drvr, flags);
1334         return 0;
1335 }
1336
1337 int brcmf_fws_hdrpull(struct brcmf_pub *drvr, int ifidx, s16 signal_len,
1338                       struct sk_buff *skb)
1339 {
1340         struct brcmf_fws_info *fws = drvr->fws;
1341         ulong flags;
1342         u8 *signal_data;
1343         s16 data_len;
1344         u8 type;
1345         u8 len;
1346         u8 *data;
1347
1348         brcmf_dbg(TRACE, "enter: ifidx %d, skblen %u, sig %d\n",
1349                   ifidx, skb->len, signal_len);
1350
1351         WARN_ON(signal_len > skb->len);
1352
1353         /* if flow control disabled, skip to packet data and leave */
1354         if (!signal_len || !drvr->fw_signals) {
1355                 skb_pull(skb, signal_len);
1356                 return 0;
1357         }
1358
1359         /* lock during tlv parsing */
1360         brcmf_fws_lock(drvr, flags);
1361
1362         fws->stats.header_pulls++;
1363         data_len = signal_len;
1364         signal_data = skb->data;
1365
1366         while (data_len > 0) {
1367                 /* extract tlv info */
1368                 type = signal_data[0];
1369
1370                 /* FILLER type is actually not a TLV, but
1371                  * a single byte that can be skipped.
1372                  */
1373                 if (type == BRCMF_FWS_TYPE_FILLER) {
1374                         signal_data += 1;
1375                         data_len -= 1;
1376                         continue;
1377                 }
1378                 len = signal_data[1];
1379                 data = signal_data + 2;
1380
1381                 brcmf_dbg(INFO, "tlv type=%d (%s), len=%d, data[0]=%d\n", type,
1382                           brcmf_fws_get_tlv_name(type), len, *data);
1383
1384                 /* abort parsing when length invalid */
1385                 if (data_len < len + 2)
1386                         break;
1387
1388                 if (len != brcmf_fws_get_tlv_len(fws, type))
1389                         break;
1390
1391                 switch (type) {
1392                 case BRCMF_FWS_TYPE_HOST_REORDER_RXPKTS:
1393                 case BRCMF_FWS_TYPE_COMP_TXSTATUS:
1394                         break;
1395                 case BRCMF_FWS_TYPE_MACDESC_ADD:
1396                 case BRCMF_FWS_TYPE_MACDESC_DEL:
1397                         brcmf_fws_macdesc_indicate(fws, type, data);
1398                         break;
1399                 case BRCMF_FWS_TYPE_MAC_OPEN:
1400                 case BRCMF_FWS_TYPE_MAC_CLOSE:
1401                         brcmf_fws_macdesc_state_indicate(fws, type, data);
1402                         break;
1403                 case BRCMF_FWS_TYPE_INTERFACE_OPEN:
1404                 case BRCMF_FWS_TYPE_INTERFACE_CLOSE:
1405                         brcmf_fws_interface_state_indicate(fws, type, data);
1406                         break;
1407                 case BRCMF_FWS_TYPE_MAC_REQUEST_CREDIT:
1408                 case BRCMF_FWS_TYPE_MAC_REQUEST_PACKET:
1409                         brcmf_fws_request_indicate(fws, type, data);
1410                         break;
1411                 case BRCMF_FWS_TYPE_TXSTATUS:
1412                         brcmf_fws_txstatus_indicate(fws, data);
1413                         break;
1414                 case BRCMF_FWS_TYPE_FIFO_CREDITBACK:
1415                         brcmf_fws_fifocreditback_indicate(fws, data);
1416                         break;
1417                 case BRCMF_FWS_TYPE_RSSI:
1418                         brcmf_fws_rssi_indicate(fws, *data);
1419                         break;
1420                 case BRCMF_FWS_TYPE_TRANS_ID:
1421                         brcmf_fws_dbg_seqnum_check(fws, data);
1422                         break;
1423                 case BRCMF_FWS_TYPE_PKTTAG:
1424                 case BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP:
1425                 default:
1426                         fws->stats.tlv_invalid_type++;
1427                         break;
1428                 }
1429
1430                 signal_data += len + 2;
1431                 data_len -= len + 2;
1432         }
1433
1434         if (data_len != 0)
1435                 fws->stats.tlv_parse_failed++;
1436
1437         /* signalling processing result does
1438          * not affect the actual ethernet packet.
1439          */
1440         skb_pull(skb, signal_len);
1441
1442         /* this may be a signal-only packet
1443          */
1444         if (skb->len == 0)
1445                 fws->stats.header_only_pkt++;
1446
1447         brcmf_fws_unlock(drvr, flags);
1448         return 0;
1449 }
1450
1451 static int brcmf_fws_hdrpush(struct brcmf_fws_info *fws, struct sk_buff *skb)
1452 {
1453         struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(skb)->mac;
1454         u8 *wlh;
1455         u16 data_offset = 0;
1456         u8 fillers;
1457         __le32 pkttag = cpu_to_le32(brcmf_skbcb(skb)->htod);
1458
1459         brcmf_dbg(TRACE, "enter: ea=%pM, ifidx=%u, pkttag=0x%08X\n",
1460                   entry->ea, entry->interface_id, le32_to_cpu(pkttag));
1461         if (entry->send_tim_signal)
1462                 data_offset += 2 + BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP_LEN;
1463
1464         /* +2 is for Type[1] and Len[1] in TLV, plus TIM signal */
1465         data_offset += 2 + BRCMF_FWS_TYPE_PKTTAG_LEN;
1466         fillers = round_up(data_offset, 4) - data_offset;
1467         data_offset += fillers;
1468
1469         skb_push(skb, data_offset);
1470         wlh = skb->data;
1471
1472         wlh[0] = BRCMF_FWS_TYPE_PKTTAG;
1473         wlh[1] = BRCMF_FWS_TYPE_PKTTAG_LEN;
1474         memcpy(&wlh[2], &pkttag, sizeof(pkttag));
1475         wlh += BRCMF_FWS_TYPE_PKTTAG_LEN + 2;
1476
1477         if (entry->send_tim_signal) {
1478                 entry->send_tim_signal = 0;
1479                 wlh[0] = BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP;
1480                 wlh[1] = BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP_LEN;
1481                 wlh[2] = entry->mac_handle;
1482                 wlh[3] = entry->traffic_pending_bmp;
1483                 wlh += BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP_LEN + 2;
1484                 entry->traffic_lastreported_bmp = entry->traffic_pending_bmp;
1485         }
1486         if (fillers)
1487                 memset(wlh, BRCMF_FWS_TYPE_FILLER, fillers);
1488
1489         brcmf_proto_hdrpush(fws->drvr, brcmf_skb_if_flags_get_field(skb, INDEX),
1490                             data_offset >> 2, skb);
1491         return 0;
1492 }
1493
1494 static int brcmf_fws_precommit_skb(struct brcmf_fws_info *fws, int fifo,
1495                                    struct sk_buff *p)
1496 {
1497         struct brcmf_skbuff_cb *skcb = brcmf_skbcb(p);
1498         struct brcmf_fws_mac_descriptor *entry = skcb->mac;
1499         int rc = 0;
1500         bool header_needed;
1501         int hslot = BRCMF_FWS_HANGER_MAXITEMS;
1502         u8 free_ctr;
1503         u8 ifidx;
1504         u8 flags;
1505
1506         header_needed = skcb->state != BRCMF_FWS_SKBSTATE_SUPPRESSED;
1507
1508         if (header_needed) {
1509                 /* obtaining free slot may fail, but that will be caught
1510                  * by the hanger push. This assures the packet has a BDC
1511                  * header upon return.
1512                  */
1513                 hslot = brcmf_fws_hanger_get_free_slot(&fws->hanger);
1514                 free_ctr = entry->seq[fifo];
1515                 brcmf_skb_htod_tag_set_field(p, HSLOT, hslot);
1516                 brcmf_skb_htod_tag_set_field(p, FREERUN, free_ctr);
1517                 brcmf_skb_htod_tag_set_field(p, GENERATION, 1);
1518                 entry->transit_count++;
1519         }
1520         brcmf_skb_if_flags_set_field(p, TRANSMIT, 1);
1521         brcmf_skb_htod_tag_set_field(p, FIFO, fifo);
1522
1523         flags = BRCMF_FWS_HTOD_FLAG_PKTFROMHOST;
1524         if (!(skcb->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK)) {
1525                 /*
1526                 Indicate that this packet is being sent in response to an
1527                 explicit request from the firmware side.
1528                 */
1529                 flags |= BRCMF_FWS_HTOD_FLAG_PKT_REQUESTED;
1530         }
1531         brcmf_skb_htod_tag_set_field(p, FLAGS, flags);
1532         if (header_needed) {
1533                 brcmf_fws_hdrpush(fws, p);
1534                 rc = brcmf_fws_hanger_pushpkt(&fws->hanger, p, hslot);
1535                 if (rc)
1536                         brcmf_err("hanger push failed: rc=%d\n", rc);
1537         } else {
1538                 int gen;
1539
1540                 /* remove old header */
1541                 rc = brcmf_proto_hdrpull(fws->drvr, false, &ifidx, p);
1542                 if (rc == 0) {
1543                         hslot = brcmf_skb_htod_tag_get_field(p, HSLOT);
1544                         brcmf_fws_hanger_get_genbit(&fws->hanger, p,
1545                                                     hslot, &gen);
1546                         brcmf_skb_htod_tag_set_field(p, GENERATION, gen);
1547
1548                         /* push new header */
1549                         brcmf_fws_hdrpush(fws, p);
1550                 }
1551         }
1552
1553         return rc;
1554 }
1555
1556 static int
1557 brcmf_fws_rollback_toq(struct brcmf_fws_info *fws, struct sk_buff *skb)
1558 {
1559         /*
1560         put the packet back to the head of queue
1561
1562         - suppressed packet goes back to suppress sub-queue
1563         - pull out the header, if new or delayed packet
1564
1565         Note: hslot is used only when header removal is done.
1566         */
1567         struct brcmf_fws_mac_descriptor *entry;
1568         enum brcmf_fws_skb_state state;
1569         struct sk_buff *pktout;
1570         int rc = 0;
1571         int fifo;
1572         int hslot;
1573         u8 ifidx;
1574
1575         fifo = brcmf_skb_if_flags_get_field(skb, FIFO);
1576         state = brcmf_skbcb(skb)->state;
1577         entry = brcmf_skbcb(skb)->mac;
1578
1579         if (entry != NULL) {
1580                 if (state == BRCMF_FWS_SKBSTATE_SUPPRESSED) {
1581                         /* wl-header is saved for suppressed packets */
1582                         pktout = brcmu_pktq_penq_head(&entry->psq, 2 * fifo + 1,
1583                                                       skb);
1584                         if (pktout == NULL) {
1585                                 brcmf_err("suppress queue full\n");
1586                                 rc = -ENOSPC;
1587                         }
1588                 } else {
1589                         hslot = brcmf_skb_htod_tag_get_field(skb, HSLOT);
1590
1591                         /* remove header first */
1592                         rc = brcmf_proto_hdrpull(fws->drvr, false, &ifidx, skb);
1593                         if (rc) {
1594                                 brcmf_err("header removal failed\n");
1595                                 /* free the hanger slot */
1596                                 brcmf_fws_hanger_poppkt(&fws->hanger, hslot,
1597                                                         &pktout, true);
1598                                 brcmf_txfinalize(fws->drvr, skb, false);
1599                                 rc = -EINVAL;
1600                                 goto fail;
1601                         }
1602
1603                         /* delay-q packets are going to delay-q */
1604                         pktout = brcmu_pktq_penq_head(&entry->psq,
1605                                                       2 * fifo, skb);
1606                         if (pktout == NULL) {
1607                                 brcmf_err("delay queue full\n");
1608                                 rc = -ENOSPC;
1609                         }
1610
1611                         /* free the hanger slot */
1612                         brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &pktout,
1613                                                 true);
1614
1615                         /* decrement sequence count */
1616                         entry->seq[fifo]--;
1617                 }
1618                 /*
1619                 if this packet did not count against FIFO credit, it must have
1620                 taken a requested_credit from the firmware (for pspoll etc.)
1621                 */
1622                 if (!(brcmf_skbcb(skb)->if_flags &
1623                       BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK))
1624                         entry->requested_credit++;
1625         } else {
1626                 brcmf_err("no mac entry linked\n");
1627                 rc = -ENOENT;
1628         }
1629
1630
1631 fail:
1632         if (rc)
1633                 fws->stats.rollback_failed++;
1634         else
1635                 fws->stats.rollback_success++;
1636         return rc;
1637 }
1638
1639 static int brcmf_fws_consume_credit(struct brcmf_fws_info *fws, int fifo,
1640                                     struct sk_buff *skb)
1641 {
1642         struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(skb)->mac;
1643         int *credit = &fws->fifo_credit[fifo];
1644         int use_credit = 1;
1645
1646         brcmf_dbg(TRACE, "enter: ac=%d, credits=%d\n", fifo, *credit);
1647
1648         if (entry->requested_credit > 0) {
1649                 /*
1650                  * if the packet was pulled out while destination is in
1651                  * closed state but had a non-zero packets requested,
1652                  * then this should not count against the FIFO credit.
1653                  * That is due to the fact that the firmware will
1654                  * most likely hold onto this packet until a suitable
1655                  * time later to push it to the appropriate AC FIFO.
1656                  */
1657                 entry->requested_credit--;
1658                 if (entry->state == BRCMF_FWS_STATE_CLOSE)
1659                         use_credit = 0;
1660         } else if (entry->requested_packet > 0) {
1661                 entry->requested_packet--;
1662                 brcmf_skb_if_flags_set_field(skb, REQUESTED, 1);
1663                 if (entry->state == BRCMF_FWS_STATE_CLOSE)
1664                         use_credit = 0;
1665         }
1666         brcmf_skb_if_flags_set_field(skb, CREDITCHECK, use_credit);
1667         if (!use_credit) {
1668                 brcmf_dbg(TRACE, "exit: no creditcheck set\n");
1669                 return 0;
1670         }
1671
1672         if (!(*credit)) {
1673                 brcmf_dbg(TRACE, "exit: credits depleted\n");
1674                 return -ENAVAIL;
1675         }
1676         (*credit)--;
1677         if (!(*credit))
1678                 fws->fifo_credit_map &= ~(1 << fifo);
1679         brcmf_dbg(TRACE, "exit: ac=%d, credits=%d\n", fifo, *credit);
1680         return 0;
1681 }
1682
1683 static int brcmf_fws_commit_skb(struct brcmf_fws_info *fws, int fifo,
1684                                 struct sk_buff *skb)
1685 {
1686         struct brcmf_skbuff_cb *skcb = brcmf_skbcb(skb);
1687         struct brcmf_fws_mac_descriptor *entry;
1688         struct brcmf_bus *bus = fws->drvr->bus_if;
1689         int rc;
1690
1691         entry = skcb->mac;
1692         if (IS_ERR(entry))
1693                 return PTR_ERR(entry);
1694
1695         rc = brcmf_fws_precommit_skb(fws, fifo, skb);
1696         if (rc < 0) {
1697                 fws->stats.generic_error++;
1698                 goto rollback;
1699         }
1700
1701         rc = brcmf_bus_txdata(bus, skb);
1702         if (rc < 0)
1703                 goto rollback;
1704
1705         entry->seq[fifo]++;
1706         fws->stats.pkt2bus++;
1707         if (brcmf_skbcb(skb)->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK) {
1708                 fws->stats.send_pkts[fifo]++;
1709                 fws->stats.fifo_credits_sent[fifo]++;
1710         }
1711
1712         return rc;
1713
1714 rollback:
1715         rc = brcmf_fws_rollback_toq(fws, skb);
1716         return rc;
1717 }
1718
1719 int brcmf_fws_process_skb(struct brcmf_if *ifp, struct sk_buff *skb)
1720 {
1721         struct brcmf_pub *drvr = ifp->drvr;
1722         struct brcmf_skbuff_cb *skcb = brcmf_skbcb(skb);
1723         struct ethhdr *eh = (struct ethhdr *)(skb->data);
1724         ulong flags;
1725         u8 ifidx = ifp->ifidx;
1726         int fifo = BRCMF_FWS_FIFO_BCMC;
1727         bool multicast = is_multicast_ether_addr(eh->h_dest);
1728
1729         /* determine the priority */
1730         if (!skb->priority)
1731                 skb->priority = cfg80211_classify8021d(skb);
1732
1733         drvr->tx_multicast += !!multicast;
1734         if (ntohs(eh->h_proto) == ETH_P_PAE)
1735                 atomic_inc(&ifp->pend_8021x_cnt);
1736
1737         if (!brcmf_fws_fc_active(drvr->fws)) {
1738                 /* If the protocol uses a data header, apply it */
1739                 brcmf_proto_hdrpush(drvr, ifidx, 0, skb);
1740
1741                 /* Use bus module to send data frame */
1742                 return brcmf_bus_txdata(drvr->bus_if, skb);
1743         }
1744
1745         /* set control buffer information */
1746         skcb->if_flags = 0;
1747         skcb->mac = brcmf_fws_find_mac_desc(drvr->fws, ifidx, eh->h_dest);
1748         skcb->state = BRCMF_FWS_SKBSTATE_NEW;
1749         brcmf_skb_if_flags_set_field(skb, INDEX, ifidx);
1750         if (!multicast)
1751                 fifo = brcmf_fws_prio2fifo[skb->priority];
1752         brcmf_skb_if_flags_set_field(skb, FIFO, fifo);
1753
1754         brcmf_dbg(TRACE, "ea=%pM, multi=%d, fifo=%d\n", eh->h_dest,
1755                   multicast, fifo);
1756
1757         brcmf_fws_lock(drvr, flags);
1758         if (!brcmf_fws_mac_desc_ready(skcb->mac, fifo) ||
1759             (!multicast &&
1760              brcmf_fws_consume_credit(drvr->fws, fifo, skb) < 0)) {
1761                 /* enqueue the packet in delayQ */
1762                 drvr->fws->fifo_delay_map |= 1 << fifo;
1763                 brcmf_fws_enq(drvr->fws, BRCMF_FWS_SKBSTATE_DELAYED, fifo, skb);
1764         } else {
1765                 brcmf_fws_commit_skb(drvr->fws, fifo, skb);
1766         }
1767         brcmf_fws_unlock(drvr, flags);
1768         return 0;
1769 }
1770
1771 void brcmf_fws_reset_interface(struct brcmf_if *ifp)
1772 {
1773         struct brcmf_fws_mac_descriptor *entry = ifp->fws_desc;
1774
1775         brcmf_dbg(TRACE, "enter: idx=%d\n", ifp->bssidx);
1776         if (!entry)
1777                 return;
1778
1779         brcmf_fws_init_mac_descriptor(entry, ifp->mac_addr, ifp->ifidx);
1780 }
1781
1782 void brcmf_fws_add_interface(struct brcmf_if *ifp)
1783 {
1784         struct brcmf_fws_info *fws = ifp->drvr->fws;
1785         struct brcmf_fws_mac_descriptor *entry;
1786
1787         brcmf_dbg(TRACE, "enter: idx=%d, mac=%pM\n",
1788                   ifp->bssidx, ifp->mac_addr);
1789         if (!ifp->ndev || !ifp->drvr->fw_signals)
1790                 return;
1791
1792         entry = &fws->desc.iface[ifp->ifidx];
1793         ifp->fws_desc = entry;
1794         brcmf_fws_init_mac_descriptor(entry, ifp->mac_addr, ifp->ifidx);
1795         brcmu_pktq_init(&entry->psq, BRCMF_FWS_PSQ_PREC_COUNT,
1796                         BRCMF_FWS_PSQ_LEN);
1797 }
1798
1799 void brcmf_fws_del_interface(struct brcmf_if *ifp)
1800 {
1801         struct brcmf_fws_mac_descriptor *entry = ifp->fws_desc;
1802
1803         brcmf_dbg(TRACE, "enter: idx=%d\n", ifp->bssidx);
1804         if (!entry)
1805                 return;
1806
1807         ifp->fws_desc = NULL;
1808         brcmf_fws_clear_mac_descriptor(entry);
1809         brcmf_fws_cleanup(ifp->drvr->fws, ifp->ifidx);
1810 }
1811
1812 static void brcmf_fws_dequeue_worker(struct work_struct *worker)
1813 {
1814         struct brcmf_fws_info *fws;
1815         struct sk_buff *skb;
1816         ulong flags;
1817         int fifo;
1818         int credit;
1819
1820         fws = container_of(worker, struct brcmf_fws_info, fws_dequeue_work);
1821
1822         brcmf_dbg(TRACE, "enter: fws=%p\n", fws);
1823         brcmf_fws_lock(fws->drvr, flags);
1824         for (fifo = NL80211_NUM_ACS; fifo >= 0; fifo--) {
1825                 brcmf_dbg(TRACE, "fifo %d credit %d\n", fifo,
1826                           fws->fifo_credit[fifo]);
1827                 for (credit = 0; credit < fws->fifo_credit[fifo]; /* nop */) {
1828                         skb = brcmf_fws_deq(fws, fifo);
1829                         if (!skb)
1830                                 break;
1831                         if (!brcmf_fws_commit_skb(fws, fifo, skb) &&
1832                             brcmf_skbcb(skb)->if_flags &
1833                             BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK)
1834                                 credit++;
1835                 }
1836                 fws->fifo_credit[fifo] -= credit;
1837         }
1838         brcmf_fws_unlock(fws->drvr, flags);
1839 }
1840
1841 int brcmf_fws_init(struct brcmf_pub *drvr)
1842 {
1843         u32 tlv = BRCMF_FWS_FLAGS_RSSI_SIGNALS;
1844         int rc;
1845
1846         if (!drvr->fw_signals)
1847                 return 0;
1848
1849         spin_lock_init(&drvr->fws_spinlock);
1850
1851         drvr->fws = kzalloc(sizeof(*(drvr->fws)), GFP_KERNEL);
1852         if (!drvr->fws) {
1853                 rc = -ENOMEM;
1854                 goto fail;
1855         }
1856
1857         /* set linkage back */
1858         drvr->fws->drvr = drvr;
1859         drvr->fws->fcmode = fcmode;
1860
1861         drvr->fws->fws_wq = create_singlethread_workqueue("brcmf_fws_wq");
1862         if (drvr->fws->fws_wq == NULL) {
1863                 brcmf_err("workqueue creation failed\n");
1864                 rc = -EBADF;
1865                 goto fail;
1866         }
1867         INIT_WORK(&drvr->fws->fws_dequeue_work, brcmf_fws_dequeue_worker);
1868
1869         /* enable firmware signalling if fcmode active */
1870         if (drvr->fws->fcmode != BRCMF_FWS_FCMODE_NONE)
1871                 tlv |= BRCMF_FWS_FLAGS_XONXOFF_SIGNALS |
1872                        BRCMF_FWS_FLAGS_CREDIT_STATUS_SIGNALS |
1873                        BRCMF_FWS_FLAGS_HOST_PROPTXSTATUS_ACTIVE;
1874
1875         rc = brcmf_fil_iovar_int_set(drvr->iflist[0], "tlv", tlv);
1876         if (rc < 0) {
1877                 brcmf_err("failed to set bdcv2 tlv signaling\n");
1878                 goto fail;
1879         }
1880
1881         if (brcmf_fweh_register(drvr, BRCMF_E_FIFO_CREDIT_MAP,
1882                                 brcmf_fws_notify_credit_map)) {
1883                 brcmf_err("register credit map handler failed\n");
1884                 goto fail;
1885         }
1886
1887         brcmf_fws_hanger_init(&drvr->fws->hanger);
1888         brcmf_fws_init_mac_descriptor(&drvr->fws->desc.other, NULL, 0);
1889         brcmu_pktq_init(&drvr->fws->desc.other.psq, BRCMF_FWS_PSQ_PREC_COUNT,
1890                         BRCMF_FWS_PSQ_LEN);
1891
1892         /* create debugfs file for statistics */
1893         brcmf_debugfs_create_fws_stats(drvr, &drvr->fws->stats);
1894
1895         /* TODO: remove upon feature delivery */
1896         brcmf_err("%s bdcv2 tlv signaling [%x]\n",
1897                   drvr->fw_signals ? "enabled" : "disabled", tlv);
1898         return 0;
1899
1900 fail:
1901         /* disable flow control entirely */
1902         drvr->fw_signals = false;
1903         brcmf_fws_deinit(drvr);
1904         return rc;
1905 }
1906
1907 void brcmf_fws_deinit(struct brcmf_pub *drvr)
1908 {
1909         struct brcmf_fws_info *fws = drvr->fws;
1910         ulong flags;
1911
1912         if (!fws)
1913                 return;
1914
1915         /* cleanup */
1916         brcmf_fws_lock(drvr, flags);
1917         brcmf_fws_cleanup(fws, -1);
1918         drvr->fws = NULL;
1919         brcmf_fws_unlock(drvr, flags);
1920
1921         /* free top structure */
1922         kfree(fws);
1923 }
1924
1925 bool brcmf_fws_fc_active(struct brcmf_fws_info *fws)
1926 {
1927         if (!fws)
1928                 return false;
1929
1930         brcmf_dbg(TRACE, "enter: mode=%d\n", fws->fcmode);
1931         return fws->fcmode != BRCMF_FWS_FCMODE_NONE;
1932 }
1933
1934 void brcmf_fws_bustxfail(struct brcmf_fws_info *fws, struct sk_buff *skb)
1935 {
1936         ulong flags;
1937
1938         brcmf_fws_lock(fws->drvr, flags);
1939         brcmf_fws_txstatus_process(fws, BRCMF_FWS_TXSTATUS_FW_TOSSED,
1940                                    brcmf_skb_htod_tag_get_field(skb, HSLOT), 0);
1941         /* the packet never reached firmware so reclaim credit */
1942         if (fws->fcmode == BRCMF_FWS_FCMODE_EXPLICIT_CREDIT &&
1943             brcmf_skbcb(skb)->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK) {
1944                 brcmf_fws_return_credits(fws,
1945                                          brcmf_skb_htod_tag_get_field(skb,
1946                                                                       FIFO),
1947                                          1);
1948                 brcmf_fws_schedule_deq(fws);
1949         }
1950         brcmf_fws_unlock(fws->drvr, flags);
1951 }