1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Datapath implementation for ST-Ericsson CW1200 mac80211 drivers
4 *
5 * Copyright (c) 2010, ST-Ericsson
6 * Author: Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
7 */
8
9 #include <net/mac80211.h>
10 #include <linux/etherdevice.h>
11 #include <linux/skbuff.h>
12
13 #include "cw1200.h"
14 #include "wsm.h"
15 #include "bh.h"
16 #include "sta.h"
17 #include "debug.h"
18
19 #define CW1200_INVALID_RATE_ID (0xFF)
20
21 static int cw1200_handle_action_rx(struct cw1200_common *priv,
22 struct sk_buff *skb);
23 static const struct ieee80211_rate *
24 cw1200_get_tx_rate(const struct cw1200_common *priv,
25 const struct ieee80211_tx_rate *rate);
26
27 /* ******************************************************************** */
28 /* TX queue lock / unlock */
29
cw1200_tx_queues_lock(struct cw1200_common * priv)30 static inline void cw1200_tx_queues_lock(struct cw1200_common *priv)
31 {
32 int i;
33 for (i = 0; i < 4; ++i)
34 cw1200_queue_lock(&priv->tx_queue[i]);
35 }
36
cw1200_tx_queues_unlock(struct cw1200_common * priv)37 static inline void cw1200_tx_queues_unlock(struct cw1200_common *priv)
38 {
39 int i;
40 for (i = 0; i < 4; ++i)
41 cw1200_queue_unlock(&priv->tx_queue[i]);
42 }
43
44 /* ******************************************************************** */
45 /* TX policy cache implementation */
46
tx_policy_dump(struct tx_policy * policy)47 static void tx_policy_dump(struct tx_policy *policy)
48 {
49 pr_debug("[TX policy] %.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X %.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X %.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X: %d\n",
50 policy->raw[0] & 0x0F, policy->raw[0] >> 4,
51 policy->raw[1] & 0x0F, policy->raw[1] >> 4,
52 policy->raw[2] & 0x0F, policy->raw[2] >> 4,
53 policy->raw[3] & 0x0F, policy->raw[3] >> 4,
54 policy->raw[4] & 0x0F, policy->raw[4] >> 4,
55 policy->raw[5] & 0x0F, policy->raw[5] >> 4,
56 policy->raw[6] & 0x0F, policy->raw[6] >> 4,
57 policy->raw[7] & 0x0F, policy->raw[7] >> 4,
58 policy->raw[8] & 0x0F, policy->raw[8] >> 4,
59 policy->raw[9] & 0x0F, policy->raw[9] >> 4,
60 policy->raw[10] & 0x0F, policy->raw[10] >> 4,
61 policy->raw[11] & 0x0F, policy->raw[11] >> 4,
62 policy->defined);
63 }
64
tx_policy_build(const struct cw1200_common * priv,struct tx_policy * policy,struct ieee80211_tx_rate * rates,size_t count)65 static void tx_policy_build(const struct cw1200_common *priv,
66 /* [out] */ struct tx_policy *policy,
67 struct ieee80211_tx_rate *rates, size_t count)
68 {
69 int i, j;
70 unsigned limit = priv->short_frame_max_tx_count;
71 unsigned total = 0;
72 BUG_ON(rates[0].idx < 0);
73 memset(policy, 0, sizeof(*policy));
74
75 /* Sort rates in descending order. */
76 for (i = 1; i < count; ++i) {
77 if (rates[i].idx < 0) {
78 count = i;
79 break;
80 }
81 if (rates[i].idx > rates[i - 1].idx) {
82 struct ieee80211_tx_rate tmp = rates[i - 1];
83 rates[i - 1] = rates[i];
84 rates[i] = tmp;
85 }
86 }
87
88 /* Eliminate duplicates. */
89 total = rates[0].count;
90 for (i = 0, j = 1; j < count; ++j) {
91 if (rates[j].idx == rates[i].idx) {
92 rates[i].count += rates[j].count;
93 } else if (rates[j].idx > rates[i].idx) {
94 break;
95 } else {
96 ++i;
97 if (i != j)
98 rates[i] = rates[j];
99 }
100 total += rates[j].count;
101 }
102 count = i + 1;
103
104 /* Re-fill policy trying to keep every requested rate and with
105 * respect to the global max tx retransmission count.
106 */
107 if (limit < count)
108 limit = count;
109 if (total > limit) {
110 for (i = 0; i < count; ++i) {
111 int left = count - i - 1;
112 if (rates[i].count > limit - left)
113 rates[i].count = limit - left;
114 limit -= rates[i].count;
115 }
116 }
117
118 /* HACK!!! Device has problems (at least) switching from
119 * 54Mbps CTS to 1Mbps. This switch takes enormous amount
120 * of time (100-200 ms), leading to valuable throughput drop.
121 * As a workaround, additional g-rates are injected to the
122 * policy.
123 */
124 if (count == 2 && !(rates[0].flags & IEEE80211_TX_RC_MCS) &&
125 rates[0].idx > 4 && rates[0].count > 2 &&
126 rates[1].idx < 2) {
127 int mid_rate = (rates[0].idx + 4) >> 1;
128
129 /* Decrease number of retries for the initial rate */
130 rates[0].count -= 2;
131
132 if (mid_rate != 4) {
133 /* Keep fallback rate at 1Mbps. */
134 rates[3] = rates[1];
135
136 /* Inject 1 transmission on lowest g-rate */
137 rates[2].idx = 4;
138 rates[2].count = 1;
139 rates[2].flags = rates[1].flags;
140
141 /* Inject 1 transmission on mid-rate */
142 rates[1].idx = mid_rate;
143 rates[1].count = 1;
144
145 /* Fallback to 1 Mbps is a really bad thing,
146 * so let's try to increase probability of
147 * successful transmission on the lowest g rate
148 * even more
149 */
150 if (rates[0].count >= 3) {
151 --rates[0].count;
152 ++rates[2].count;
153 }
154
155 /* Adjust amount of rates defined */
156 count += 2;
157 } else {
158 /* Keep fallback rate at 1Mbps. */
159 rates[2] = rates[1];
160
161 /* Inject 2 transmissions on lowest g-rate */
162 rates[1].idx = 4;
163 rates[1].count = 2;
164
165 /* Adjust amount of rates defined */
166 count += 1;
167 }
168 }
169
170 policy->defined = cw1200_get_tx_rate(priv, &rates[0])->hw_value + 1;
171
172 for (i = 0; i < count; ++i) {
173 register unsigned rateid, off, shift, retries;
174
175 rateid = cw1200_get_tx_rate(priv, &rates[i])->hw_value;
176 off = rateid >> 3; /* eq. rateid / 8 */
177 shift = (rateid & 0x07) << 2; /* eq. (rateid % 8) * 4 */
178
179 retries = rates[i].count;
180 if (retries > 0x0F) {
181 rates[i].count = 0x0f;
182 retries = 0x0F;
183 }
184 policy->tbl[off] |= __cpu_to_le32(retries << shift);
185 policy->retry_count += retries;
186 }
187
188 pr_debug("[TX policy] Policy (%zu): %d:%d, %d:%d, %d:%d, %d:%d\n",
189 count,
190 rates[0].idx, rates[0].count,
191 rates[1].idx, rates[1].count,
192 rates[2].idx, rates[2].count,
193 rates[3].idx, rates[3].count);
194 }
195
tx_policy_is_equal(const struct tx_policy * wanted,const struct tx_policy * cached)196 static inline bool tx_policy_is_equal(const struct tx_policy *wanted,
197 const struct tx_policy *cached)
198 {
199 size_t count = wanted->defined >> 1;
200 if (wanted->defined > cached->defined)
201 return false;
202 if (count) {
203 if (memcmp(wanted->raw, cached->raw, count))
204 return false;
205 }
206 if (wanted->defined & 1) {
207 if ((wanted->raw[count] & 0x0F) != (cached->raw[count] & 0x0F))
208 return false;
209 }
210 return true;
211 }
212
tx_policy_find(struct tx_policy_cache * cache,const struct tx_policy * wanted)213 static int tx_policy_find(struct tx_policy_cache *cache,
214 const struct tx_policy *wanted)
215 {
216 /* O(n) complexity. Not so good, but there's only 8 entries in
217 * the cache.
218 * Also lru helps to reduce search time.
219 */
220 struct tx_policy_cache_entry *it;
221 /* First search for policy in "used" list */
222 list_for_each_entry(it, &cache->used, link) {
223 if (tx_policy_is_equal(wanted, &it->policy))
224 return it - cache->cache;
225 }
226 /* Then - in "free list" */
227 list_for_each_entry(it, &cache->free, link) {
228 if (tx_policy_is_equal(wanted, &it->policy))
229 return it - cache->cache;
230 }
231 return -1;
232 }
233
tx_policy_use(struct tx_policy_cache * cache,struct tx_policy_cache_entry * entry)234 static inline void tx_policy_use(struct tx_policy_cache *cache,
235 struct tx_policy_cache_entry *entry)
236 {
237 ++entry->policy.usage_count;
238 list_move(&entry->link, &cache->used);
239 }
240
tx_policy_release(struct tx_policy_cache * cache,struct tx_policy_cache_entry * entry)241 static inline int tx_policy_release(struct tx_policy_cache *cache,
242 struct tx_policy_cache_entry *entry)
243 {
244 int ret = --entry->policy.usage_count;
245 if (!ret)
246 list_move(&entry->link, &cache->free);
247 return ret;
248 }
249
tx_policy_clean(struct cw1200_common * priv)250 void tx_policy_clean(struct cw1200_common *priv)
251 {
252 int idx, locked;
253 struct tx_policy_cache *cache = &priv->tx_policy_cache;
254 struct tx_policy_cache_entry *entry;
255
256 cw1200_tx_queues_lock(priv);
257 spin_lock_bh(&cache->lock);
258 locked = list_empty(&cache->free);
259
260 for (idx = 0; idx < TX_POLICY_CACHE_SIZE; idx++) {
261 entry = &cache->cache[idx];
262 /* Policy usage count should be 0 at this time as all queues
263 should be empty
264 */
265 if (WARN_ON(entry->policy.usage_count)) {
266 entry->policy.usage_count = 0;
267 list_move(&entry->link, &cache->free);
268 }
269 memset(&entry->policy, 0, sizeof(entry->policy));
270 }
271 if (locked)
272 cw1200_tx_queues_unlock(priv);
273
274 cw1200_tx_queues_unlock(priv);
275 spin_unlock_bh(&cache->lock);
276 }
277
278 /* ******************************************************************** */
279 /* External TX policy cache API */
280
tx_policy_init(struct cw1200_common * priv)281 void tx_policy_init(struct cw1200_common *priv)
282 {
283 struct tx_policy_cache *cache = &priv->tx_policy_cache;
284 int i;
285
286 memset(cache, 0, sizeof(*cache));
287
288 spin_lock_init(&cache->lock);
289 INIT_LIST_HEAD(&cache->used);
290 INIT_LIST_HEAD(&cache->free);
291
292 for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i)
293 list_add(&cache->cache[i].link, &cache->free);
294 }
295
tx_policy_get(struct cw1200_common * priv,struct ieee80211_tx_rate * rates,size_t count,bool * renew)296 static int tx_policy_get(struct cw1200_common *priv,
297 struct ieee80211_tx_rate *rates,
298 size_t count, bool *renew)
299 {
300 int idx;
301 struct tx_policy_cache *cache = &priv->tx_policy_cache;
302 struct tx_policy wanted;
303
304 tx_policy_build(priv, &wanted, rates, count);
305
306 spin_lock_bh(&cache->lock);
307 if (WARN_ON_ONCE(list_empty(&cache->free))) {
308 spin_unlock_bh(&cache->lock);
309 return CW1200_INVALID_RATE_ID;
310 }
311 idx = tx_policy_find(cache, &wanted);
312 if (idx >= 0) {
313 pr_debug("[TX policy] Used TX policy: %d\n", idx);
314 *renew = false;
315 } else {
316 struct tx_policy_cache_entry *entry;
317 *renew = true;
318 /* If policy is not found create a new one
319 * using the oldest entry in "free" list
320 */
321 entry = list_entry(cache->free.prev,
322 struct tx_policy_cache_entry, link);
323 entry->policy = wanted;
324 idx = entry - cache->cache;
325 pr_debug("[TX policy] New TX policy: %d\n", idx);
326 tx_policy_dump(&entry->policy);
327 }
328 tx_policy_use(cache, &cache->cache[idx]);
329 if (list_empty(&cache->free)) {
330 /* Lock TX queues. */
331 cw1200_tx_queues_lock(priv);
332 }
333 spin_unlock_bh(&cache->lock);
334 return idx;
335 }
336
tx_policy_put(struct cw1200_common * priv,int idx)337 static void tx_policy_put(struct cw1200_common *priv, int idx)
338 {
339 int usage, locked;
340 struct tx_policy_cache *cache = &priv->tx_policy_cache;
341
342 spin_lock_bh(&cache->lock);
343 locked = list_empty(&cache->free);
344 usage = tx_policy_release(cache, &cache->cache[idx]);
345 if (locked && !usage) {
346 /* Unlock TX queues. */
347 cw1200_tx_queues_unlock(priv);
348 }
349 spin_unlock_bh(&cache->lock);
350 }
351
tx_policy_upload(struct cw1200_common * priv)352 static int tx_policy_upload(struct cw1200_common *priv)
353 {
354 struct tx_policy_cache *cache = &priv->tx_policy_cache;
355 int i;
356 struct wsm_set_tx_rate_retry_policy arg = {
357 .num = 0,
358 };
359 spin_lock_bh(&cache->lock);
360
361 /* Upload only modified entries. */
362 for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i) {
363 struct tx_policy *src = &cache->cache[i].policy;
364 if (src->retry_count && !src->uploaded) {
365 struct wsm_tx_rate_retry_policy *dst =
366 &arg.tbl[arg.num];
367 dst->index = i;
368 dst->short_retries = priv->short_frame_max_tx_count;
369 dst->long_retries = priv->long_frame_max_tx_count;
370
371 dst->flags = WSM_TX_RATE_POLICY_FLAG_TERMINATE_WHEN_FINISHED |
372 WSM_TX_RATE_POLICY_FLAG_COUNT_INITIAL_TRANSMIT;
373 memcpy(dst->rate_count_indices, src->tbl,
374 sizeof(dst->rate_count_indices));
375 src->uploaded = 1;
376 ++arg.num;
377 }
378 }
379 spin_unlock_bh(&cache->lock);
380 cw1200_debug_tx_cache_miss(priv);
381 pr_debug("[TX policy] Upload %d policies\n", arg.num);
382 return wsm_set_tx_rate_retry_policy(priv, &arg);
383 }
384
tx_policy_upload_work(struct work_struct * work)385 void tx_policy_upload_work(struct work_struct *work)
386 {
387 struct cw1200_common *priv =
388 container_of(work, struct cw1200_common, tx_policy_upload_work);
389
390 pr_debug("[TX] TX policy upload.\n");
391 tx_policy_upload(priv);
392
393 wsm_unlock_tx(priv);
394 cw1200_tx_queues_unlock(priv);
395 }
396
397 /* ******************************************************************** */
398 /* cw1200 TX implementation */
399
400 struct cw1200_txinfo {
401 struct sk_buff *skb;
402 unsigned queue;
403 struct ieee80211_tx_info *tx_info;
404 const struct ieee80211_rate *rate;
405 struct ieee80211_hdr *hdr;
406 size_t hdrlen;
407 const u8 *da;
408 struct cw1200_sta_priv *sta_priv;
409 struct ieee80211_sta *sta;
410 struct cw1200_txpriv txpriv;
411 };
412
cw1200_rate_mask_to_wsm(struct cw1200_common * priv,u32 rates)413 u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv, u32 rates)
414 {
415 u32 ret = 0;
416 int i;
417 for (i = 0; i < 32; ++i) {
418 if (rates & BIT(i))
419 ret |= BIT(priv->rates[i].hw_value);
420 }
421 return ret;
422 }
423
424 static const struct ieee80211_rate *
cw1200_get_tx_rate(const struct cw1200_common * priv,const struct ieee80211_tx_rate * rate)425 cw1200_get_tx_rate(const struct cw1200_common *priv,
426 const struct ieee80211_tx_rate *rate)
427 {
428 if (rate->idx < 0)
429 return NULL;
430 if (rate->flags & IEEE80211_TX_RC_MCS)
431 return &priv->mcs_rates[rate->idx];
432 return &priv->hw->wiphy->bands[priv->channel->band]->
433 bitrates[rate->idx];
434 }
435
436 static int
cw1200_tx_h_calc_link_ids(struct cw1200_common * priv,struct cw1200_txinfo * t)437 cw1200_tx_h_calc_link_ids(struct cw1200_common *priv,
438 struct cw1200_txinfo *t)
439 {
440 if (t->sta && t->sta_priv->link_id)
441 t->txpriv.raw_link_id =
442 t->txpriv.link_id =
443 t->sta_priv->link_id;
444 else if (priv->mode != NL80211_IFTYPE_AP)
445 t->txpriv.raw_link_id =
446 t->txpriv.link_id = 0;
447 else if (is_multicast_ether_addr(t->da)) {
448 if (priv->enable_beacon) {
449 t->txpriv.raw_link_id = 0;
450 t->txpriv.link_id = CW1200_LINK_ID_AFTER_DTIM;
451 } else {
452 t->txpriv.raw_link_id = 0;
453 t->txpriv.link_id = 0;
454 }
455 } else {
456 t->txpriv.link_id = cw1200_find_link_id(priv, t->da);
457 if (!t->txpriv.link_id)
458 t->txpriv.link_id = cw1200_alloc_link_id(priv, t->da);
459 if (!t->txpriv.link_id) {
460 wiphy_err(priv->hw->wiphy,
461 "No more link IDs available.\n");
462 return -ENOENT;
463 }
464 t->txpriv.raw_link_id = t->txpriv.link_id;
465 }
466 if (t->txpriv.raw_link_id)
467 priv->link_id_db[t->txpriv.raw_link_id - 1].timestamp =
468 jiffies;
469 if (t->sta && (t->sta->uapsd_queues & BIT(t->queue)))
470 t->txpriv.link_id = CW1200_LINK_ID_UAPSD;
471 return 0;
472 }
473
474 static void
cw1200_tx_h_pm(struct cw1200_common * priv,struct cw1200_txinfo * t)475 cw1200_tx_h_pm(struct cw1200_common *priv,
476 struct cw1200_txinfo *t)
477 {
478 if (ieee80211_is_auth(t->hdr->frame_control)) {
479 u32 mask = ~BIT(t->txpriv.raw_link_id);
480 spin_lock_bh(&priv->ps_state_lock);
481 priv->sta_asleep_mask &= mask;
482 priv->pspoll_mask &= mask;
483 spin_unlock_bh(&priv->ps_state_lock);
484 }
485 }
486
487 static void
cw1200_tx_h_calc_tid(struct cw1200_common * priv,struct cw1200_txinfo * t)488 cw1200_tx_h_calc_tid(struct cw1200_common *priv,
489 struct cw1200_txinfo *t)
490 {
491 if (ieee80211_is_data_qos(t->hdr->frame_control)) {
492 u8 *qos = ieee80211_get_qos_ctl(t->hdr);
493 t->txpriv.tid = qos[0] & IEEE80211_QOS_CTL_TID_MASK;
494 } else if (ieee80211_is_data(t->hdr->frame_control)) {
495 t->txpriv.tid = 0;
496 }
497 }
498
499 static int
cw1200_tx_h_crypt(struct cw1200_common * priv,struct cw1200_txinfo * t)500 cw1200_tx_h_crypt(struct cw1200_common *priv,
501 struct cw1200_txinfo *t)
502 {
503 if (!t->tx_info->control.hw_key ||
504 !ieee80211_has_protected(t->hdr->frame_control))
505 return 0;
506
507 t->hdrlen += t->tx_info->control.hw_key->iv_len;
508 skb_put(t->skb, t->tx_info->control.hw_key->icv_len);
509
510 if (t->tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
511 skb_put(t->skb, 8); /* MIC space */
512
513 return 0;
514 }
515
516 static int
cw1200_tx_h_align(struct cw1200_common * priv,struct cw1200_txinfo * t,u8 * flags)517 cw1200_tx_h_align(struct cw1200_common *priv,
518 struct cw1200_txinfo *t,
519 u8 *flags)
520 {
521 size_t offset = (size_t)t->skb->data & 3;
522
523 if (!offset)
524 return 0;
525
526 if (offset & 1) {
527 wiphy_err(priv->hw->wiphy,
528 "Bug: attempt to transmit a frame with wrong alignment: %zu\n",
529 offset);
530 return -EINVAL;
531 }
532
533 if (skb_headroom(t->skb) < offset) {
534 wiphy_err(priv->hw->wiphy,
535 "Bug: no space allocated for DMA alignment. headroom: %d\n",
536 skb_headroom(t->skb));
537 return -ENOMEM;
538 }
539 skb_push(t->skb, offset);
540 t->hdrlen += offset;
541 t->txpriv.offset += offset;
542 *flags |= WSM_TX_2BYTES_SHIFT;
543 cw1200_debug_tx_align(priv);
544 return 0;
545 }
546
547 static int
cw1200_tx_h_action(struct cw1200_common * priv,struct cw1200_txinfo * t)548 cw1200_tx_h_action(struct cw1200_common *priv,
549 struct cw1200_txinfo *t)
550 {
551 struct ieee80211_mgmt *mgmt =
552 (struct ieee80211_mgmt *)t->hdr;
553 if (ieee80211_is_action(t->hdr->frame_control) &&
554 mgmt->u.action.category == WLAN_CATEGORY_BACK)
555 return 1;
556 else
557 return 0;
558 }
559
560 /* Add WSM header */
561 static struct wsm_tx *
cw1200_tx_h_wsm(struct cw1200_common * priv,struct cw1200_txinfo * t)562 cw1200_tx_h_wsm(struct cw1200_common *priv,
563 struct cw1200_txinfo *t)
564 {
565 struct wsm_tx *wsm;
566
567 if (skb_headroom(t->skb) < sizeof(struct wsm_tx)) {
568 wiphy_err(priv->hw->wiphy,
569 "Bug: no space allocated for WSM header. headroom: %d\n",
570 skb_headroom(t->skb));
571 return NULL;
572 }
573
574 wsm = skb_push(t->skb, sizeof(struct wsm_tx));
575 t->txpriv.offset += sizeof(struct wsm_tx);
576 memset(wsm, 0, sizeof(*wsm));
577 wsm->hdr.len = __cpu_to_le16(t->skb->len);
578 wsm->hdr.id = __cpu_to_le16(0x0004);
579 wsm->queue_id = wsm_queue_id_to_wsm(t->queue);
580 return wsm;
581 }
582
583 /* BT Coex specific handling */
584 static void
cw1200_tx_h_bt(struct cw1200_common * priv,struct cw1200_txinfo * t,struct wsm_tx * wsm)585 cw1200_tx_h_bt(struct cw1200_common *priv,
586 struct cw1200_txinfo *t,
587 struct wsm_tx *wsm)
588 {
589 u8 priority = 0;
590
591 if (!priv->bt_present)
592 return;
593
594 if (ieee80211_is_nullfunc(t->hdr->frame_control)) {
595 priority = WSM_EPTA_PRIORITY_MGT;
596 } else if (ieee80211_is_data(t->hdr->frame_control)) {
597 /* Skip LLC SNAP header (+6) */
598 u8 *payload = &t->skb->data[t->hdrlen];
599 __be16 *ethertype = (__be16 *)&payload[6];
600 if (be16_to_cpu(*ethertype) == ETH_P_PAE)
601 priority = WSM_EPTA_PRIORITY_EAPOL;
602 } else if (ieee80211_is_assoc_req(t->hdr->frame_control) ||
603 ieee80211_is_reassoc_req(t->hdr->frame_control)) {
604 struct ieee80211_mgmt *mgt_frame =
605 (struct ieee80211_mgmt *)t->hdr;
606
607 if (le16_to_cpu(mgt_frame->u.assoc_req.listen_interval) <
608 priv->listen_interval) {
609 pr_debug("Modified Listen Interval to %d from %d\n",
610 priv->listen_interval,
611 mgt_frame->u.assoc_req.listen_interval);
612 /* Replace listen interval derieved from
613 * the one read from SDD
614 */
615 mgt_frame->u.assoc_req.listen_interval = cpu_to_le16(priv->listen_interval);
616 }
617 }
618
619 if (!priority) {
620 if (ieee80211_is_action(t->hdr->frame_control))
621 priority = WSM_EPTA_PRIORITY_ACTION;
622 else if (ieee80211_is_mgmt(t->hdr->frame_control))
623 priority = WSM_EPTA_PRIORITY_MGT;
624 else if (wsm->queue_id == WSM_QUEUE_VOICE)
625 priority = WSM_EPTA_PRIORITY_VOICE;
626 else if (wsm->queue_id == WSM_QUEUE_VIDEO)
627 priority = WSM_EPTA_PRIORITY_VIDEO;
628 else
629 priority = WSM_EPTA_PRIORITY_DATA;
630 }
631
632 pr_debug("[TX] EPTA priority %d.\n", priority);
633
634 wsm->flags |= priority << 1;
635 }
636
637 static int
cw1200_tx_h_rate_policy(struct cw1200_common * priv,struct cw1200_txinfo * t,struct wsm_tx * wsm)638 cw1200_tx_h_rate_policy(struct cw1200_common *priv,
639 struct cw1200_txinfo *t,
640 struct wsm_tx *wsm)
641 {
642 bool tx_policy_renew = false;
643
644 t->txpriv.rate_id = tx_policy_get(priv,
645 t->tx_info->control.rates, IEEE80211_TX_MAX_RATES,
646 &tx_policy_renew);
647 if (t->txpriv.rate_id == CW1200_INVALID_RATE_ID)
648 return -EFAULT;
649
650 wsm->flags |= t->txpriv.rate_id << 4;
651
652 t->rate = cw1200_get_tx_rate(priv,
653 &t->tx_info->control.rates[0]);
654 wsm->max_tx_rate = t->rate->hw_value;
655 if (t->rate->flags & IEEE80211_TX_RC_MCS) {
656 if (cw1200_ht_greenfield(&priv->ht_info))
657 wsm->ht_tx_parameters |=
658 __cpu_to_le32(WSM_HT_TX_GREENFIELD);
659 else
660 wsm->ht_tx_parameters |=
661 __cpu_to_le32(WSM_HT_TX_MIXED);
662 }
663
664 if (tx_policy_renew) {
665 pr_debug("[TX] TX policy renew.\n");
666 /* It's not so optimal to stop TX queues every now and then.
667 * Better to reimplement task scheduling with
668 * a counter. TODO.
669 */
670 wsm_lock_tx_async(priv);
671 cw1200_tx_queues_lock(priv);
672 if (queue_work(priv->workqueue,
673 &priv->tx_policy_upload_work) <= 0) {
674 cw1200_tx_queues_unlock(priv);
675 wsm_unlock_tx(priv);
676 }
677 }
678 return 0;
679 }
680
681 static bool
cw1200_tx_h_pm_state(struct cw1200_common * priv,struct cw1200_txinfo * t)682 cw1200_tx_h_pm_state(struct cw1200_common *priv,
683 struct cw1200_txinfo *t)
684 {
685 int was_buffered = 1;
686
687 if (t->txpriv.link_id == CW1200_LINK_ID_AFTER_DTIM &&
688 !priv->buffered_multicasts) {
689 priv->buffered_multicasts = true;
690 if (priv->sta_asleep_mask)
691 queue_work(priv->workqueue,
692 &priv->multicast_start_work);
693 }
694
695 if (t->txpriv.raw_link_id && t->txpriv.tid < CW1200_MAX_TID)
696 was_buffered = priv->link_id_db[t->txpriv.raw_link_id - 1].buffered[t->txpriv.tid]++;
697
698 return !was_buffered;
699 }
700
701 /* ******************************************************************** */
702
cw1200_tx(struct ieee80211_hw * dev,struct ieee80211_tx_control * control,struct sk_buff * skb)703 void cw1200_tx(struct ieee80211_hw *dev,
704 struct ieee80211_tx_control *control,
705 struct sk_buff *skb)
706 {
707 struct cw1200_common *priv = dev->priv;
708 struct cw1200_txinfo t = {
709 .skb = skb,
710 .queue = skb_get_queue_mapping(skb),
711 .tx_info = IEEE80211_SKB_CB(skb),
712 .hdr = (struct ieee80211_hdr *)skb->data,
713 .txpriv.tid = CW1200_MAX_TID,
714 .txpriv.rate_id = CW1200_INVALID_RATE_ID,
715 };
716 struct ieee80211_sta *sta;
717 struct wsm_tx *wsm;
718 bool tid_update = false;
719 u8 flags = 0;
720 int ret;
721
722 if (priv->bh_error)
723 goto drop;
724
725 t.hdrlen = ieee80211_hdrlen(t.hdr->frame_control);
726 t.da = ieee80211_get_DA(t.hdr);
727 if (control) {
728 t.sta = control->sta;
729 t.sta_priv = (struct cw1200_sta_priv *)&t.sta->drv_priv;
730 }
731
732 if (WARN_ON(t.queue >= 4))
733 goto drop;
734
735 ret = cw1200_tx_h_calc_link_ids(priv, &t);
736 if (ret)
737 goto drop;
738
739 pr_debug("[TX] TX %d bytes (queue: %d, link_id: %d (%d)).\n",
740 skb->len, t.queue, t.txpriv.link_id,
741 t.txpriv.raw_link_id);
742
743 cw1200_tx_h_pm(priv, &t);
744 cw1200_tx_h_calc_tid(priv, &t);
745 ret = cw1200_tx_h_crypt(priv, &t);
746 if (ret)
747 goto drop;
748 ret = cw1200_tx_h_align(priv, &t, &flags);
749 if (ret)
750 goto drop;
751 ret = cw1200_tx_h_action(priv, &t);
752 if (ret)
753 goto drop;
754 wsm = cw1200_tx_h_wsm(priv, &t);
755 if (!wsm) {
756 ret = -ENOMEM;
757 goto drop;
758 }
759 wsm->flags |= flags;
760 cw1200_tx_h_bt(priv, &t, wsm);
761 ret = cw1200_tx_h_rate_policy(priv, &t, wsm);
762 if (ret)
763 goto drop;
764
765 sta = t.sta;
766
767 spin_lock_bh(&priv->ps_state_lock);
768 {
769 tid_update = cw1200_tx_h_pm_state(priv, &t);
770 BUG_ON(cw1200_queue_put(&priv->tx_queue[t.queue],
771 t.skb, &t.txpriv));
772 }
773 spin_unlock_bh(&priv->ps_state_lock);
774
775 if (tid_update && sta)
776 ieee80211_sta_set_buffered(sta, t.txpriv.tid, true);
777
778 cw1200_bh_wakeup(priv);
779
780 return;
781
782 drop:
783 cw1200_skb_dtor(priv, skb, &t.txpriv);
784 return;
785 }
786
787 /* ******************************************************************** */
788
cw1200_handle_action_rx(struct cw1200_common * priv,struct sk_buff * skb)789 static int cw1200_handle_action_rx(struct cw1200_common *priv,
790 struct sk_buff *skb)
791 {
792 struct ieee80211_mgmt *mgmt = (void *)skb->data;
793
794 /* Filter block ACK negotiation: fully controlled by firmware */
795 if (mgmt->u.action.category == WLAN_CATEGORY_BACK)
796 return 1;
797
798 return 0;
799 }
800
cw1200_handle_pspoll(struct cw1200_common * priv,struct sk_buff * skb)801 static int cw1200_handle_pspoll(struct cw1200_common *priv,
802 struct sk_buff *skb)
803 {
804 struct ieee80211_sta *sta;
805 struct ieee80211_pspoll *pspoll = (struct ieee80211_pspoll *)skb->data;
806 int link_id = 0;
807 u32 pspoll_mask = 0;
808 int drop = 1;
809 int i;
810
811 if (priv->join_status != CW1200_JOIN_STATUS_AP)
812 goto done;
813 if (memcmp(priv->vif->addr, pspoll->bssid, ETH_ALEN))
814 goto done;
815
816 rcu_read_lock();
817 sta = ieee80211_find_sta(priv->vif, pspoll->ta);
818 if (sta) {
819 struct cw1200_sta_priv *sta_priv;
820 sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
821 link_id = sta_priv->link_id;
822 pspoll_mask = BIT(sta_priv->link_id);
823 }
824 rcu_read_unlock();
825 if (!link_id)
826 goto done;
827
828 priv->pspoll_mask |= pspoll_mask;
829 drop = 0;
830
831 /* Do not report pspols if data for given link id is queued already. */
832 for (i = 0; i < 4; ++i) {
833 if (cw1200_queue_get_num_queued(&priv->tx_queue[i],
834 pspoll_mask)) {
835 cw1200_bh_wakeup(priv);
836 drop = 1;
837 break;
838 }
839 }
840 pr_debug("[RX] PSPOLL: %s\n", drop ? "local" : "fwd");
841 done:
842 return drop;
843 }
844
845 /* ******************************************************************** */
846
cw1200_tx_confirm_cb(struct cw1200_common * priv,int link_id,struct wsm_tx_confirm * arg)847 void cw1200_tx_confirm_cb(struct cw1200_common *priv,
848 int link_id,
849 struct wsm_tx_confirm *arg)
850 {
851 u8 queue_id = cw1200_queue_get_queue_id(arg->packet_id);
852 struct cw1200_queue *queue = &priv->tx_queue[queue_id];
853 struct sk_buff *skb;
854 const struct cw1200_txpriv *txpriv;
855
856 pr_debug("[TX] TX confirm: %d, %d.\n",
857 arg->status, arg->ack_failures);
858
859 if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
860 /* STA is stopped. */
861 return;
862 }
863
864 if (WARN_ON(queue_id >= 4))
865 return;
866
867 if (arg->status)
868 pr_debug("TX failed: %d.\n", arg->status);
869
870 if ((arg->status == WSM_REQUEUE) &&
871 (arg->flags & WSM_TX_STATUS_REQUEUE)) {
872 /* "Requeue" means "implicit suspend" */
873 struct wsm_suspend_resume suspend = {
874 .link_id = link_id,
875 .stop = 1,
876 .multicast = !link_id,
877 };
878 cw1200_suspend_resume(priv, &suspend);
879 wiphy_warn(priv->hw->wiphy, "Requeue for link_id %d (try %d). STAs asleep: 0x%.8X\n",
880 link_id,
881 cw1200_queue_get_generation(arg->packet_id) + 1,
882 priv->sta_asleep_mask);
883 cw1200_queue_requeue(queue, arg->packet_id);
884 spin_lock_bh(&priv->ps_state_lock);
885 if (!link_id) {
886 priv->buffered_multicasts = true;
887 if (priv->sta_asleep_mask) {
888 queue_work(priv->workqueue,
889 &priv->multicast_start_work);
890 }
891 }
892 spin_unlock_bh(&priv->ps_state_lock);
893 } else if (!cw1200_queue_get_skb(queue, arg->packet_id,
894 &skb, &txpriv)) {
895 struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
896 int tx_count = arg->ack_failures;
897 u8 ht_flags = 0;
898 int i;
899
900 if (cw1200_ht_greenfield(&priv->ht_info))
901 ht_flags |= IEEE80211_TX_RC_GREEN_FIELD;
902
903 spin_lock(&priv->bss_loss_lock);
904 if (priv->bss_loss_state &&
905 arg->packet_id == priv->bss_loss_confirm_id) {
906 if (arg->status) {
907 /* Recovery failed */
908 __cw1200_cqm_bssloss_sm(priv, 0, 0, 1);
909 } else {
910 /* Recovery succeeded */
911 __cw1200_cqm_bssloss_sm(priv, 0, 1, 0);
912 }
913 }
914 spin_unlock(&priv->bss_loss_lock);
915
916 if (!arg->status) {
917 tx->flags |= IEEE80211_TX_STAT_ACK;
918 ++tx_count;
919 cw1200_debug_txed(priv);
920 if (arg->flags & WSM_TX_STATUS_AGGREGATION) {
921 /* Do not report aggregation to mac80211:
922 * it confuses minstrel a lot.
923 */
924 /* tx->flags |= IEEE80211_TX_STAT_AMPDU; */
925 cw1200_debug_txed_agg(priv);
926 }
927 } else {
928 if (tx_count)
929 ++tx_count;
930 }
931
932 for (i = 0; i < IEEE80211_TX_MAX_RATES; ++i) {
933 if (tx->status.rates[i].count >= tx_count) {
934 tx->status.rates[i].count = tx_count;
935 break;
936 }
937 tx_count -= tx->status.rates[i].count;
938 if (tx->status.rates[i].flags & IEEE80211_TX_RC_MCS)
939 tx->status.rates[i].flags |= ht_flags;
940 }
941
942 for (++i; i < IEEE80211_TX_MAX_RATES; ++i) {
943 tx->status.rates[i].count = 0;
944 tx->status.rates[i].idx = -1;
945 }
946
947 /* Pull off any crypto trailers that we added on */
948 if (tx->control.hw_key) {
949 skb_trim(skb, skb->len - tx->control.hw_key->icv_len);
950 if (tx->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
951 skb_trim(skb, skb->len - 8); /* MIC space */
952 }
953 cw1200_queue_remove(queue, arg->packet_id);
954 }
955 /* XXX TODO: Only wake if there are pending transmits.. */
956 cw1200_bh_wakeup(priv);
957 }
958
cw1200_notify_buffered_tx(struct cw1200_common * priv,struct sk_buff * skb,int link_id,int tid)959 static void cw1200_notify_buffered_tx(struct cw1200_common *priv,
960 struct sk_buff *skb, int link_id, int tid)
961 {
962 struct ieee80211_sta *sta;
963 struct ieee80211_hdr *hdr;
964 u8 *buffered;
965 u8 still_buffered = 0;
966
967 if (link_id && tid < CW1200_MAX_TID) {
968 buffered = priv->link_id_db
969 [link_id - 1].buffered;
970
971 spin_lock_bh(&priv->ps_state_lock);
972 if (!WARN_ON(!buffered[tid]))
973 still_buffered = --buffered[tid];
974 spin_unlock_bh(&priv->ps_state_lock);
975
976 if (!still_buffered && tid < CW1200_MAX_TID) {
977 hdr = (struct ieee80211_hdr *)skb->data;
978 rcu_read_lock();
979 sta = ieee80211_find_sta(priv->vif, hdr->addr1);
980 if (sta)
981 ieee80211_sta_set_buffered(sta, tid, false);
982 rcu_read_unlock();
983 }
984 }
985 }
986
cw1200_skb_dtor(struct cw1200_common * priv,struct sk_buff * skb,const struct cw1200_txpriv * txpriv)987 void cw1200_skb_dtor(struct cw1200_common *priv,
988 struct sk_buff *skb,
989 const struct cw1200_txpriv *txpriv)
990 {
991 skb_pull(skb, txpriv->offset);
992 if (txpriv->rate_id != CW1200_INVALID_RATE_ID) {
993 cw1200_notify_buffered_tx(priv, skb,
994 txpriv->raw_link_id, txpriv->tid);
995 tx_policy_put(priv, txpriv->rate_id);
996 }
997 ieee80211_tx_status_skb(priv->hw, skb);
998 }
999
cw1200_rx_cb(struct cw1200_common * priv,struct wsm_rx * arg,int link_id,struct sk_buff ** skb_p)1000 void cw1200_rx_cb(struct cw1200_common *priv,
1001 struct wsm_rx *arg,
1002 int link_id,
1003 struct sk_buff **skb_p)
1004 {
1005 struct sk_buff *skb = *skb_p;
1006 struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
1007 struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
1008 struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
1009 struct cw1200_link_entry *entry = NULL;
1010 unsigned long grace_period;
1011
1012 bool early_data = false;
1013 bool p2p = priv->vif && priv->vif->p2p;
1014 size_t hdrlen;
1015 hdr->flag = 0;
1016
1017 if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
1018 /* STA is stopped. */
1019 goto drop;
1020 }
1021
1022 if (link_id && link_id <= CW1200_MAX_STA_IN_AP_MODE) {
1023 entry = &priv->link_id_db[link_id - 1];
1024 if (entry->status == CW1200_LINK_SOFT &&
1025 ieee80211_is_data(frame->frame_control))
1026 early_data = true;
1027 entry->timestamp = jiffies;
1028 } else if (p2p &&
1029 ieee80211_is_action(frame->frame_control) &&
1030 (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
1031 pr_debug("[RX] Going to MAP&RESET link ID\n");
1032 WARN_ON(work_pending(&priv->linkid_reset_work));
1033 memcpy(&priv->action_frame_sa[0],
1034 ieee80211_get_SA(frame), ETH_ALEN);
1035 priv->action_linkid = 0;
1036 schedule_work(&priv->linkid_reset_work);
1037 }
1038
1039 if (link_id && p2p &&
1040 ieee80211_is_action(frame->frame_control) &&
1041 (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
1042 /* Link ID already exists for the ACTION frame.
1043 * Reset and Remap
1044 */
1045 WARN_ON(work_pending(&priv->linkid_reset_work));
1046 memcpy(&priv->action_frame_sa[0],
1047 ieee80211_get_SA(frame), ETH_ALEN);
1048 priv->action_linkid = link_id;
1049 schedule_work(&priv->linkid_reset_work);
1050 }
1051 if (arg->status) {
1052 if (arg->status == WSM_STATUS_MICFAILURE) {
1053 pr_debug("[RX] MIC failure.\n");
1054 hdr->flag |= RX_FLAG_MMIC_ERROR;
1055 } else if (arg->status == WSM_STATUS_NO_KEY_FOUND) {
1056 pr_debug("[RX] No key found.\n");
1057 goto drop;
1058 } else {
1059 pr_debug("[RX] Receive failure: %d.\n",
1060 arg->status);
1061 goto drop;
1062 }
1063 }
1064
1065 if (skb->len < sizeof(struct ieee80211_pspoll)) {
1066 wiphy_warn(priv->hw->wiphy, "Malformed SDU rx'ed. Size is lesser than IEEE header.\n");
1067 goto drop;
1068 }
1069
1070 if (ieee80211_is_pspoll(frame->frame_control))
1071 if (cw1200_handle_pspoll(priv, skb))
1072 goto drop;
1073
1074 hdr->band = ((arg->channel_number & 0xff00) ||
1075 (arg->channel_number > 14)) ?
1076 NL80211_BAND_5GHZ : NL80211_BAND_2GHZ;
1077 hdr->freq = ieee80211_channel_to_frequency(
1078 arg->channel_number,
1079 hdr->band);
1080
1081 if (arg->rx_rate >= 14) {
1082 hdr->encoding = RX_ENC_HT;
1083 hdr->rate_idx = arg->rx_rate - 14;
1084 } else if (arg->rx_rate >= 4) {
1085 hdr->rate_idx = arg->rx_rate - 2;
1086 } else {
1087 hdr->rate_idx = arg->rx_rate;
1088 }
1089
1090 hdr->signal = (s8)arg->rcpi_rssi;
1091 hdr->antenna = 0;
1092
1093 hdrlen = ieee80211_hdrlen(frame->frame_control);
1094
1095 if (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
1096 size_t iv_len = 0, icv_len = 0;
1097
1098 hdr->flag |= RX_FLAG_DECRYPTED | RX_FLAG_IV_STRIPPED;
1099
1100 /* Oops... There is no fast way to ask mac80211 about
1101 * IV/ICV lengths. Even defineas are not exposed.
1102 */
1103 switch (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
1104 case WSM_RX_STATUS_WEP:
1105 iv_len = 4 /* WEP_IV_LEN */;
1106 icv_len = 4 /* WEP_ICV_LEN */;
1107 break;
1108 case WSM_RX_STATUS_TKIP:
1109 iv_len = 8 /* TKIP_IV_LEN */;
1110 icv_len = 4 /* TKIP_ICV_LEN */
1111 + 8 /*MICHAEL_MIC_LEN*/;
1112 hdr->flag |= RX_FLAG_MMIC_STRIPPED;
1113 break;
1114 case WSM_RX_STATUS_AES:
1115 iv_len = 8 /* CCMP_HDR_LEN */;
1116 icv_len = 8 /* CCMP_MIC_LEN */;
1117 break;
1118 case WSM_RX_STATUS_WAPI:
1119 iv_len = 18 /* WAPI_HDR_LEN */;
1120 icv_len = 16 /* WAPI_MIC_LEN */;
1121 break;
1122 default:
1123 pr_warn("Unknown encryption type %d\n",
1124 WSM_RX_STATUS_ENCRYPTION(arg->flags));
1125 goto drop;
1126 }
1127
1128 /* Firmware strips ICV in case of MIC failure. */
1129 if (arg->status == WSM_STATUS_MICFAILURE)
1130 icv_len = 0;
1131
1132 if (skb->len < hdrlen + iv_len + icv_len) {
1133 wiphy_warn(priv->hw->wiphy, "Malformed SDU rx'ed. Size is lesser than crypto headers.\n");
1134 goto drop;
1135 }
1136
1137 /* Remove IV, ICV and MIC */
1138 skb_trim(skb, skb->len - icv_len);
1139 memmove(skb->data + iv_len, skb->data, hdrlen);
1140 skb_pull(skb, iv_len);
1141 }
1142
1143 /* Remove TSF from the end of frame */
1144 if (arg->flags & WSM_RX_STATUS_TSF_INCLUDED) {
1145 hdr->mactime = get_unaligned_le64(skb->data + skb->len - 8);
1146 if (skb->len >= 8)
1147 skb_trim(skb, skb->len - 8);
1148 } else {
1149 hdr->mactime = 0;
1150 }
1151
1152 cw1200_debug_rxed(priv);
1153 if (arg->flags & WSM_RX_STATUS_AGGREGATE)
1154 cw1200_debug_rxed_agg(priv);
1155
1156 if (ieee80211_is_action(frame->frame_control) &&
1157 (arg->flags & WSM_RX_STATUS_ADDRESS1)) {
1158 if (cw1200_handle_action_rx(priv, skb))
1159 return;
1160 } else if (ieee80211_is_beacon(frame->frame_control) &&
1161 !arg->status && priv->vif &&
1162 ether_addr_equal(ieee80211_get_SA(frame), priv->vif->bss_conf.bssid)) {
1163 const u8 *tim_ie;
1164 u8 *ies = ((struct ieee80211_mgmt *)
1165 (skb->data))->u.beacon.variable;
1166 size_t ies_len = skb->len - (ies - (u8 *)(skb->data));
1167
1168 tim_ie = cfg80211_find_ie(WLAN_EID_TIM, ies, ies_len);
1169 if (tim_ie && tim_ie[1] >= sizeof(struct ieee80211_tim_ie)) {
1170 struct ieee80211_tim_ie *tim =
1171 (struct ieee80211_tim_ie *)&tim_ie[2];
1172
1173 if (priv->join_dtim_period != tim->dtim_period) {
1174 priv->join_dtim_period = tim->dtim_period;
1175 queue_work(priv->workqueue,
1176 &priv->set_beacon_wakeup_period_work);
1177 }
1178 }
1179
1180 /* Disable beacon filter once we're associated... */
1181 if (priv->disable_beacon_filter &&
1182 (priv->vif->cfg.assoc ||
1183 priv->vif->cfg.ibss_joined)) {
1184 priv->disable_beacon_filter = false;
1185 queue_work(priv->workqueue,
1186 &priv->update_filtering_work);
1187 }
1188 }
1189
1190 /* Stay awake after frame is received to give
1191 * userspace chance to react and acquire appropriate
1192 * wakelock.
1193 */
1194 if (ieee80211_is_auth(frame->frame_control))
1195 grace_period = 5 * HZ;
1196 else if (ieee80211_is_deauth(frame->frame_control))
1197 grace_period = 5 * HZ;
1198 else
1199 grace_period = 1 * HZ;
1200 cw1200_pm_stay_awake(&priv->pm_state, grace_period);
1201
1202 if (early_data) {
1203 spin_lock_bh(&priv->ps_state_lock);
1204 /* Double-check status with lock held */
1205 if (entry->status == CW1200_LINK_SOFT)
1206 skb_queue_tail(&entry->rx_queue, skb);
1207 else
1208 ieee80211_rx_irqsafe(priv->hw, skb);
1209 spin_unlock_bh(&priv->ps_state_lock);
1210 } else {
1211 ieee80211_rx_irqsafe(priv->hw, skb);
1212 }
1213 *skb_p = NULL;
1214
1215 return;
1216
1217 drop:
1218 /* TODO: update failure counters */
1219 return;
1220 }
1221
1222 /* ******************************************************************** */
1223 /* Security */
1224
cw1200_alloc_key(struct cw1200_common * priv)1225 int cw1200_alloc_key(struct cw1200_common *priv)
1226 {
1227 int idx;
1228
1229 idx = ffs(~priv->key_map) - 1;
1230 if (idx < 0 || idx > WSM_KEY_MAX_INDEX)
1231 return -1;
1232
1233 priv->key_map |= BIT(idx);
1234 priv->keys[idx].index = idx;
1235 return idx;
1236 }
1237
cw1200_free_key(struct cw1200_common * priv,int idx)1238 void cw1200_free_key(struct cw1200_common *priv, int idx)
1239 {
1240 BUG_ON(!(priv->key_map & BIT(idx)));
1241 memset(&priv->keys[idx], 0, sizeof(priv->keys[idx]));
1242 priv->key_map &= ~BIT(idx);
1243 }
1244
cw1200_free_keys(struct cw1200_common * priv)1245 void cw1200_free_keys(struct cw1200_common *priv)
1246 {
1247 memset(&priv->keys, 0, sizeof(priv->keys));
1248 priv->key_map = 0;
1249 }
1250
cw1200_upload_keys(struct cw1200_common * priv)1251 int cw1200_upload_keys(struct cw1200_common *priv)
1252 {
1253 int idx, ret = 0;
1254 for (idx = 0; idx <= WSM_KEY_MAX_INDEX; ++idx)
1255 if (priv->key_map & BIT(idx)) {
1256 ret = wsm_add_key(priv, &priv->keys[idx]);
1257 if (ret < 0)
1258 break;
1259 }
1260 return ret;
1261 }
1262
1263 /* Workaround for WFD test case 6.1.10 */
cw1200_link_id_reset(struct work_struct * work)1264 void cw1200_link_id_reset(struct work_struct *work)
1265 {
1266 struct cw1200_common *priv =
1267 container_of(work, struct cw1200_common, linkid_reset_work);
1268 int temp_linkid;
1269
1270 if (!priv->action_linkid) {
1271 /* In GO mode we can receive ACTION frames without a linkID */
1272 temp_linkid = cw1200_alloc_link_id(priv,
1273 &priv->action_frame_sa[0]);
1274 WARN_ON(!temp_linkid);
1275 if (temp_linkid) {
1276 /* Make sure we execute the WQ */
1277 flush_workqueue(priv->workqueue);
1278 /* Release the link ID */
1279 spin_lock_bh(&priv->ps_state_lock);
1280 priv->link_id_db[temp_linkid - 1].prev_status =
1281 priv->link_id_db[temp_linkid - 1].status;
1282 priv->link_id_db[temp_linkid - 1].status =
1283 CW1200_LINK_RESET;
1284 spin_unlock_bh(&priv->ps_state_lock);
1285 wsm_lock_tx_async(priv);
1286 if (queue_work(priv->workqueue,
1287 &priv->link_id_work) <= 0)
1288 wsm_unlock_tx(priv);
1289 }
1290 } else {
1291 spin_lock_bh(&priv->ps_state_lock);
1292 priv->link_id_db[priv->action_linkid - 1].prev_status =
1293 priv->link_id_db[priv->action_linkid - 1].status;
1294 priv->link_id_db[priv->action_linkid - 1].status =
1295 CW1200_LINK_RESET_REMAP;
1296 spin_unlock_bh(&priv->ps_state_lock);
1297 wsm_lock_tx_async(priv);
1298 if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
1299 wsm_unlock_tx(priv);
1300 flush_workqueue(priv->workqueue);
1301 }
1302 }
1303
cw1200_find_link_id(struct cw1200_common * priv,const u8 * mac)1304 int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac)
1305 {
1306 int i, ret = 0;
1307 spin_lock_bh(&priv->ps_state_lock);
1308 for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1309 if (!memcmp(mac, priv->link_id_db[i].mac, ETH_ALEN) &&
1310 priv->link_id_db[i].status) {
1311 priv->link_id_db[i].timestamp = jiffies;
1312 ret = i + 1;
1313 break;
1314 }
1315 }
1316 spin_unlock_bh(&priv->ps_state_lock);
1317 return ret;
1318 }
1319
cw1200_alloc_link_id(struct cw1200_common * priv,const u8 * mac)1320 int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
1321 {
1322 int i, ret = 0;
1323 unsigned long max_inactivity = 0;
1324 unsigned long now = jiffies;
1325
1326 spin_lock_bh(&priv->ps_state_lock);
1327 for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1328 if (!priv->link_id_db[i].status) {
1329 ret = i + 1;
1330 break;
1331 } else if (priv->link_id_db[i].status != CW1200_LINK_HARD &&
1332 !priv->tx_queue_stats.link_map_cache[i + 1]) {
1333 unsigned long inactivity =
1334 now - priv->link_id_db[i].timestamp;
1335 if (inactivity < max_inactivity)
1336 continue;
1337 max_inactivity = inactivity;
1338 ret = i + 1;
1339 }
1340 }
1341 if (ret) {
1342 struct cw1200_link_entry *entry = &priv->link_id_db[ret - 1];
1343 pr_debug("[AP] STA added, link_id: %d\n", ret);
1344 entry->status = CW1200_LINK_RESERVE;
1345 memcpy(&entry->mac, mac, ETH_ALEN);
1346 memset(&entry->buffered, 0, CW1200_MAX_TID);
1347 skb_queue_head_init(&entry->rx_queue);
1348 wsm_lock_tx_async(priv);
1349 if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
1350 wsm_unlock_tx(priv);
1351 } else {
1352 wiphy_info(priv->hw->wiphy,
1353 "[AP] Early: no more link IDs available.\n");
1354 }
1355
1356 spin_unlock_bh(&priv->ps_state_lock);
1357 return ret;
1358 }
1359
cw1200_link_id_work(struct work_struct * work)1360 void cw1200_link_id_work(struct work_struct *work)
1361 {
1362 struct cw1200_common *priv =
1363 container_of(work, struct cw1200_common, link_id_work);
1364 wsm_flush_tx(priv);
1365 cw1200_link_id_gc_work(&priv->link_id_gc_work.work);
1366 wsm_unlock_tx(priv);
1367 }
1368
cw1200_link_id_gc_work(struct work_struct * work)1369 void cw1200_link_id_gc_work(struct work_struct *work)
1370 {
1371 struct cw1200_common *priv =
1372 container_of(work, struct cw1200_common, link_id_gc_work.work);
1373 struct wsm_reset reset = {
1374 .reset_statistics = false,
1375 };
1376 struct wsm_map_link map_link = {
1377 .link_id = 0,
1378 };
1379 unsigned long now = jiffies;
1380 unsigned long next_gc = -1;
1381 long ttl;
1382 bool need_reset;
1383 u32 mask;
1384 int i;
1385
1386 if (priv->join_status != CW1200_JOIN_STATUS_AP)
1387 return;
1388
1389 wsm_lock_tx(priv);
1390 spin_lock_bh(&priv->ps_state_lock);
1391 for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1392 need_reset = false;
1393 mask = BIT(i + 1);
1394 if (priv->link_id_db[i].status == CW1200_LINK_RESERVE ||
1395 (priv->link_id_db[i].status == CW1200_LINK_HARD &&
1396 !(priv->link_id_map & mask))) {
1397 if (priv->link_id_map & mask) {
1398 priv->sta_asleep_mask &= ~mask;
1399 priv->pspoll_mask &= ~mask;
1400 need_reset = true;
1401 }
1402 priv->link_id_map |= mask;
1403 if (priv->link_id_db[i].status != CW1200_LINK_HARD)
1404 priv->link_id_db[i].status = CW1200_LINK_SOFT;
1405 memcpy(map_link.mac_addr, priv->link_id_db[i].mac,
1406 ETH_ALEN);
1407 spin_unlock_bh(&priv->ps_state_lock);
1408 if (need_reset) {
1409 reset.link_id = i + 1;
1410 wsm_reset(priv, &reset);
1411 }
1412 map_link.link_id = i + 1;
1413 wsm_map_link(priv, &map_link);
1414 next_gc = min(next_gc, CW1200_LINK_ID_GC_TIMEOUT);
1415 spin_lock_bh(&priv->ps_state_lock);
1416 } else if (priv->link_id_db[i].status == CW1200_LINK_SOFT) {
1417 ttl = priv->link_id_db[i].timestamp - now +
1418 CW1200_LINK_ID_GC_TIMEOUT;
1419 if (ttl <= 0) {
1420 need_reset = true;
1421 priv->link_id_db[i].status = CW1200_LINK_OFF;
1422 priv->link_id_map &= ~mask;
1423 priv->sta_asleep_mask &= ~mask;
1424 priv->pspoll_mask &= ~mask;
1425 eth_zero_addr(map_link.mac_addr);
1426 spin_unlock_bh(&priv->ps_state_lock);
1427 reset.link_id = i + 1;
1428 wsm_reset(priv, &reset);
1429 spin_lock_bh(&priv->ps_state_lock);
1430 } else {
1431 next_gc = min_t(unsigned long, next_gc, ttl);
1432 }
1433 } else if (priv->link_id_db[i].status == CW1200_LINK_RESET ||
1434 priv->link_id_db[i].status ==
1435 CW1200_LINK_RESET_REMAP) {
1436 int status = priv->link_id_db[i].status;
1437 priv->link_id_db[i].status =
1438 priv->link_id_db[i].prev_status;
1439 priv->link_id_db[i].timestamp = now;
1440 reset.link_id = i + 1;
1441 spin_unlock_bh(&priv->ps_state_lock);
1442 wsm_reset(priv, &reset);
1443 if (status == CW1200_LINK_RESET_REMAP) {
1444 memcpy(map_link.mac_addr,
1445 priv->link_id_db[i].mac,
1446 ETH_ALEN);
1447 map_link.link_id = i + 1;
1448 wsm_map_link(priv, &map_link);
1449 next_gc = min(next_gc,
1450 CW1200_LINK_ID_GC_TIMEOUT);
1451 }
1452 spin_lock_bh(&priv->ps_state_lock);
1453 }
1454 if (need_reset) {
1455 skb_queue_purge(&priv->link_id_db[i].rx_queue);
1456 pr_debug("[AP] STA removed, link_id: %d\n",
1457 reset.link_id);
1458 }
1459 }
1460 spin_unlock_bh(&priv->ps_state_lock);
1461 if (next_gc != -1)
1462 queue_delayed_work(priv->workqueue,
1463 &priv->link_id_gc_work, next_gc);
1464 wsm_unlock_tx(priv);
1465 }
1466