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