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