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