0001
0002
0003
0004
0005
0006
0007
0008 #include "mesh.h"
0009 #include "wme.h"
0010
0011
0012
0013
0014
0015
0016
0017
0018 static struct sk_buff *mps_qos_null_get(struct sta_info *sta)
0019 {
0020 struct ieee80211_sub_if_data *sdata = sta->sdata;
0021 struct ieee80211_local *local = sdata->local;
0022 struct ieee80211_hdr *nullfunc;
0023 struct sk_buff *skb;
0024 int size = sizeof(*nullfunc);
0025 __le16 fc;
0026
0027 skb = dev_alloc_skb(local->hw.extra_tx_headroom + size + 2);
0028 if (!skb)
0029 return NULL;
0030 skb_reserve(skb, local->hw.extra_tx_headroom);
0031
0032 nullfunc = skb_put(skb, size);
0033 fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_NULLFUNC);
0034 ieee80211_fill_mesh_addresses(nullfunc, &fc, sta->sta.addr,
0035 sdata->vif.addr);
0036 nullfunc->frame_control = fc;
0037 nullfunc->duration_id = 0;
0038 nullfunc->seq_ctrl = 0;
0039
0040 memcpy(nullfunc->addr1, sta->sta.addr, ETH_ALEN);
0041 skb_put_zero(skb, 2);
0042 ieee80211_mps_set_frame_flags(sdata, sta, nullfunc);
0043
0044 return skb;
0045 }
0046
0047
0048
0049
0050
0051 static void mps_qos_null_tx(struct sta_info *sta)
0052 {
0053 struct sk_buff *skb;
0054
0055 skb = mps_qos_null_get(sta);
0056 if (!skb)
0057 return;
0058
0059 mps_dbg(sta->sdata, "announcing peer-specific power mode to %pM\n",
0060 sta->sta.addr);
0061
0062
0063 if (!test_sta_flag(sta, WLAN_STA_PS_STA)) {
0064 u8 *qc = ieee80211_get_qos_ctl((void *) skb->data);
0065
0066 qc[0] |= IEEE80211_QOS_CTL_EOSP;
0067 }
0068
0069 ieee80211_tx_skb(sta->sdata, skb);
0070 }
0071
0072
0073
0074
0075
0076
0077
0078
0079
0080 u32 ieee80211_mps_local_status_update(struct ieee80211_sub_if_data *sdata)
0081 {
0082 struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
0083 struct sta_info *sta;
0084 bool peering = false;
0085 int light_sleep_cnt = 0;
0086 int deep_sleep_cnt = 0;
0087 u32 changed = 0;
0088 enum nl80211_mesh_power_mode nonpeer_pm;
0089
0090 rcu_read_lock();
0091 list_for_each_entry_rcu(sta, &sdata->local->sta_list, list) {
0092 if (sdata != sta->sdata)
0093 continue;
0094
0095 switch (sta->mesh->plink_state) {
0096 case NL80211_PLINK_OPN_SNT:
0097 case NL80211_PLINK_OPN_RCVD:
0098 case NL80211_PLINK_CNF_RCVD:
0099 peering = true;
0100 break;
0101 case NL80211_PLINK_ESTAB:
0102 if (sta->mesh->local_pm == NL80211_MESH_POWER_LIGHT_SLEEP)
0103 light_sleep_cnt++;
0104 else if (sta->mesh->local_pm == NL80211_MESH_POWER_DEEP_SLEEP)
0105 deep_sleep_cnt++;
0106 break;
0107 default:
0108 break;
0109 }
0110 }
0111 rcu_read_unlock();
0112
0113
0114
0115
0116
0117
0118
0119
0120 if (peering) {
0121 mps_dbg(sdata, "setting non-peer PM to active for peering\n");
0122 nonpeer_pm = NL80211_MESH_POWER_ACTIVE;
0123 } else if (light_sleep_cnt || deep_sleep_cnt) {
0124 mps_dbg(sdata, "setting non-peer PM to deep sleep\n");
0125 nonpeer_pm = NL80211_MESH_POWER_DEEP_SLEEP;
0126 } else {
0127 mps_dbg(sdata, "setting non-peer PM to user value\n");
0128 nonpeer_pm = ifmsh->mshcfg.power_mode;
0129 }
0130
0131
0132 if (ifmsh->nonpeer_pm != nonpeer_pm ||
0133 !ifmsh->ps_peers_light_sleep != !light_sleep_cnt ||
0134 !ifmsh->ps_peers_deep_sleep != !deep_sleep_cnt)
0135 changed = BSS_CHANGED_BEACON;
0136
0137 ifmsh->nonpeer_pm = nonpeer_pm;
0138 ifmsh->ps_peers_light_sleep = light_sleep_cnt;
0139 ifmsh->ps_peers_deep_sleep = deep_sleep_cnt;
0140
0141 return changed;
0142 }
0143
0144
0145
0146
0147
0148
0149
0150
0151 u32 ieee80211_mps_set_sta_local_pm(struct sta_info *sta,
0152 enum nl80211_mesh_power_mode pm)
0153 {
0154 struct ieee80211_sub_if_data *sdata = sta->sdata;
0155
0156 if (sta->mesh->local_pm == pm)
0157 return 0;
0158
0159 mps_dbg(sdata, "local STA operates in mode %d with %pM\n",
0160 pm, sta->sta.addr);
0161
0162 sta->mesh->local_pm = pm;
0163
0164
0165
0166
0167
0168 if (sta->mesh->plink_state == NL80211_PLINK_ESTAB)
0169 mps_qos_null_tx(sta);
0170
0171 return ieee80211_mps_local_status_update(sdata);
0172 }
0173
0174
0175
0176
0177
0178
0179
0180
0181
0182
0183
0184
0185
0186 void ieee80211_mps_set_frame_flags(struct ieee80211_sub_if_data *sdata,
0187 struct sta_info *sta,
0188 struct ieee80211_hdr *hdr)
0189 {
0190 enum nl80211_mesh_power_mode pm;
0191 u8 *qc;
0192
0193 if (WARN_ON(is_unicast_ether_addr(hdr->addr1) &&
0194 ieee80211_is_data_qos(hdr->frame_control) &&
0195 !sta))
0196 return;
0197
0198 if (is_unicast_ether_addr(hdr->addr1) &&
0199 ieee80211_is_data_qos(hdr->frame_control) &&
0200 sta->mesh->plink_state == NL80211_PLINK_ESTAB)
0201 pm = sta->mesh->local_pm;
0202 else
0203 pm = sdata->u.mesh.nonpeer_pm;
0204
0205 if (pm == NL80211_MESH_POWER_ACTIVE)
0206 hdr->frame_control &= cpu_to_le16(~IEEE80211_FCTL_PM);
0207 else
0208 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
0209
0210 if (!ieee80211_is_data_qos(hdr->frame_control))
0211 return;
0212
0213 qc = ieee80211_get_qos_ctl(hdr);
0214
0215 if ((is_unicast_ether_addr(hdr->addr1) &&
0216 pm == NL80211_MESH_POWER_DEEP_SLEEP) ||
0217 (is_multicast_ether_addr(hdr->addr1) &&
0218 sdata->u.mesh.ps_peers_deep_sleep > 0))
0219 qc[1] |= (IEEE80211_QOS_CTL_MESH_PS_LEVEL >> 8);
0220 else
0221 qc[1] &= ~(IEEE80211_QOS_CTL_MESH_PS_LEVEL >> 8);
0222 }
0223
0224
0225
0226
0227
0228
0229
0230
0231 void ieee80211_mps_sta_status_update(struct sta_info *sta)
0232 {
0233 enum nl80211_mesh_power_mode pm;
0234 bool do_buffer;
0235
0236
0237 if (sta->sta_state < IEEE80211_STA_ASSOC)
0238 return;
0239
0240
0241
0242
0243
0244 if (sta->mesh->plink_state == NL80211_PLINK_ESTAB &&
0245 sta->mesh->peer_pm != NL80211_MESH_POWER_UNKNOWN)
0246 pm = sta->mesh->peer_pm;
0247 else
0248 pm = sta->mesh->nonpeer_pm;
0249
0250 do_buffer = (pm != NL80211_MESH_POWER_ACTIVE);
0251
0252
0253 if (sta->mesh->plink_state != NL80211_PLINK_ESTAB) {
0254 clear_sta_flag(sta, WLAN_STA_MPSP_OWNER);
0255 clear_sta_flag(sta, WLAN_STA_MPSP_RECIPIENT);
0256 } else if (!do_buffer) {
0257 clear_sta_flag(sta, WLAN_STA_MPSP_OWNER);
0258 }
0259
0260
0261 if (test_sta_flag(sta, WLAN_STA_PS_STA) == do_buffer)
0262 return;
0263
0264 if (do_buffer) {
0265 set_sta_flag(sta, WLAN_STA_PS_STA);
0266 atomic_inc(&sta->sdata->u.mesh.ps.num_sta_ps);
0267 mps_dbg(sta->sdata, "start PS buffering frames towards %pM\n",
0268 sta->sta.addr);
0269 } else {
0270 ieee80211_sta_ps_deliver_wakeup(sta);
0271 }
0272 }
0273
0274 static void mps_set_sta_peer_pm(struct sta_info *sta,
0275 struct ieee80211_hdr *hdr)
0276 {
0277 enum nl80211_mesh_power_mode pm;
0278 u8 *qc = ieee80211_get_qos_ctl(hdr);
0279
0280
0281
0282
0283
0284
0285
0286
0287
0288
0289
0290 if (ieee80211_has_pm(hdr->frame_control)) {
0291 if (qc[1] & (IEEE80211_QOS_CTL_MESH_PS_LEVEL >> 8))
0292 pm = NL80211_MESH_POWER_DEEP_SLEEP;
0293 else
0294 pm = NL80211_MESH_POWER_LIGHT_SLEEP;
0295 } else {
0296 pm = NL80211_MESH_POWER_ACTIVE;
0297 }
0298
0299 if (sta->mesh->peer_pm == pm)
0300 return;
0301
0302 mps_dbg(sta->sdata, "STA %pM enters mode %d\n",
0303 sta->sta.addr, pm);
0304
0305 sta->mesh->peer_pm = pm;
0306
0307 ieee80211_mps_sta_status_update(sta);
0308 }
0309
0310 static void mps_set_sta_nonpeer_pm(struct sta_info *sta,
0311 struct ieee80211_hdr *hdr)
0312 {
0313 enum nl80211_mesh_power_mode pm;
0314
0315 if (ieee80211_has_pm(hdr->frame_control))
0316 pm = NL80211_MESH_POWER_DEEP_SLEEP;
0317 else
0318 pm = NL80211_MESH_POWER_ACTIVE;
0319
0320 if (sta->mesh->nonpeer_pm == pm)
0321 return;
0322
0323 mps_dbg(sta->sdata, "STA %pM sets non-peer mode to %d\n",
0324 sta->sta.addr, pm);
0325
0326 sta->mesh->nonpeer_pm = pm;
0327
0328 ieee80211_mps_sta_status_update(sta);
0329 }
0330
0331
0332
0333
0334
0335
0336
0337 void ieee80211_mps_rx_h_sta_process(struct sta_info *sta,
0338 struct ieee80211_hdr *hdr)
0339 {
0340 if (is_unicast_ether_addr(hdr->addr1) &&
0341 ieee80211_is_data_qos(hdr->frame_control)) {
0342
0343
0344
0345
0346 mps_set_sta_peer_pm(sta, hdr);
0347
0348
0349 ieee80211_mpsp_trigger_process(ieee80211_get_qos_ctl(hdr),
0350 sta, false, false);
0351 } else {
0352
0353
0354
0355
0356 mps_set_sta_nonpeer_pm(sta, hdr);
0357 }
0358 }
0359
0360
0361
0362
0363 static void mpsp_trigger_send(struct sta_info *sta, bool rspi, bool eosp)
0364 {
0365 struct ieee80211_sub_if_data *sdata = sta->sdata;
0366 struct sk_buff *skb;
0367 struct ieee80211_hdr *nullfunc;
0368 struct ieee80211_tx_info *info;
0369 u8 *qc;
0370
0371 skb = mps_qos_null_get(sta);
0372 if (!skb)
0373 return;
0374
0375 nullfunc = (struct ieee80211_hdr *) skb->data;
0376 if (!eosp)
0377 nullfunc->frame_control |=
0378 cpu_to_le16(IEEE80211_FCTL_MOREDATA);
0379
0380
0381
0382
0383
0384
0385
0386
0387 qc = ieee80211_get_qos_ctl(nullfunc);
0388 if (rspi)
0389 qc[1] |= (IEEE80211_QOS_CTL_RSPI >> 8);
0390 if (eosp)
0391 qc[0] |= IEEE80211_QOS_CTL_EOSP;
0392
0393 info = IEEE80211_SKB_CB(skb);
0394
0395 info->flags |= IEEE80211_TX_CTL_NO_PS_BUFFER |
0396 IEEE80211_TX_CTL_REQ_TX_STATUS;
0397
0398 mps_dbg(sdata, "sending MPSP trigger%s%s to %pM\n",
0399 rspi ? " RSPI" : "", eosp ? " EOSP" : "", sta->sta.addr);
0400
0401 ieee80211_tx_skb(sdata, skb);
0402 }
0403
0404
0405
0406
0407
0408
0409
0410
0411
0412
0413 static void mpsp_qos_null_append(struct sta_info *sta,
0414 struct sk_buff_head *frames)
0415 {
0416 struct ieee80211_sub_if_data *sdata = sta->sdata;
0417 struct sk_buff *new_skb, *skb = skb_peek_tail(frames);
0418 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
0419 struct ieee80211_tx_info *info;
0420
0421 if (ieee80211_is_data_qos(hdr->frame_control))
0422 return;
0423
0424 new_skb = mps_qos_null_get(sta);
0425 if (!new_skb)
0426 return;
0427
0428 mps_dbg(sdata, "appending QoS Null in MPSP towards %pM\n",
0429 sta->sta.addr);
0430
0431
0432
0433
0434 new_skb->priority = 1;
0435 skb_set_queue_mapping(new_skb, IEEE80211_AC_BK);
0436 ieee80211_set_qos_hdr(sdata, new_skb);
0437
0438 info = IEEE80211_SKB_CB(new_skb);
0439 info->control.vif = &sdata->vif;
0440 info->control.flags |= IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
0441
0442 __skb_queue_tail(frames, new_skb);
0443 }
0444
0445
0446
0447
0448
0449
0450
0451 static void mps_frame_deliver(struct sta_info *sta, int n_frames)
0452 {
0453 struct ieee80211_local *local = sta->sdata->local;
0454 int ac;
0455 struct sk_buff_head frames;
0456 struct sk_buff *skb;
0457 bool more_data = false;
0458
0459 skb_queue_head_init(&frames);
0460
0461
0462 for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
0463 while (n_frames != 0) {
0464 skb = skb_dequeue(&sta->tx_filtered[ac]);
0465 if (!skb) {
0466 skb = skb_dequeue(
0467 &sta->ps_tx_buf[ac]);
0468 if (skb)
0469 local->total_ps_buffered--;
0470 }
0471 if (!skb)
0472 break;
0473 n_frames--;
0474 __skb_queue_tail(&frames, skb);
0475 }
0476
0477 if (!skb_queue_empty(&sta->tx_filtered[ac]) ||
0478 !skb_queue_empty(&sta->ps_tx_buf[ac]))
0479 more_data = true;
0480 }
0481
0482
0483 if (skb_queue_empty(&frames)) {
0484 mpsp_trigger_send(sta, false, true);
0485 return;
0486 }
0487
0488
0489 if (test_sta_flag(sta, WLAN_STA_MPSP_OWNER))
0490 mpsp_qos_null_append(sta, &frames);
0491
0492 mps_dbg(sta->sdata, "sending %d frames to PS STA %pM\n",
0493 skb_queue_len(&frames), sta->sta.addr);
0494
0495
0496 skb_queue_walk(&frames, skb) {
0497 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
0498 struct ieee80211_hdr *hdr = (void *) skb->data;
0499
0500
0501
0502
0503
0504
0505 info->flags |= IEEE80211_TX_CTL_NO_PS_BUFFER;
0506
0507 if (more_data || !skb_queue_is_last(&frames, skb))
0508 hdr->frame_control |=
0509 cpu_to_le16(IEEE80211_FCTL_MOREDATA);
0510 else
0511 hdr->frame_control &=
0512 cpu_to_le16(~IEEE80211_FCTL_MOREDATA);
0513
0514 if (skb_queue_is_last(&frames, skb) &&
0515 ieee80211_is_data_qos(hdr->frame_control)) {
0516 u8 *qoshdr = ieee80211_get_qos_ctl(hdr);
0517
0518
0519 *qoshdr |= IEEE80211_QOS_CTL_EOSP;
0520 info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
0521 }
0522 }
0523
0524 ieee80211_add_pending_skbs(local, &frames);
0525 sta_info_recalc_tim(sta);
0526 }
0527
0528
0529
0530
0531
0532
0533
0534
0535
0536
0537
0538 void ieee80211_mpsp_trigger_process(u8 *qc, struct sta_info *sta,
0539 bool tx, bool acked)
0540 {
0541 u8 rspi = qc[1] & (IEEE80211_QOS_CTL_RSPI >> 8);
0542 u8 eosp = qc[0] & IEEE80211_QOS_CTL_EOSP;
0543
0544 if (tx) {
0545 if (rspi && acked)
0546 set_sta_flag(sta, WLAN_STA_MPSP_RECIPIENT);
0547
0548 if (eosp)
0549 clear_sta_flag(sta, WLAN_STA_MPSP_OWNER);
0550 else if (acked &&
0551 test_sta_flag(sta, WLAN_STA_PS_STA) &&
0552 !test_and_set_sta_flag(sta, WLAN_STA_MPSP_OWNER))
0553 mps_frame_deliver(sta, -1);
0554 } else {
0555 if (eosp)
0556 clear_sta_flag(sta, WLAN_STA_MPSP_RECIPIENT);
0557 else if (sta->mesh->local_pm != NL80211_MESH_POWER_ACTIVE)
0558 set_sta_flag(sta, WLAN_STA_MPSP_RECIPIENT);
0559
0560 if (rspi && !test_and_set_sta_flag(sta, WLAN_STA_MPSP_OWNER))
0561 mps_frame_deliver(sta, -1);
0562 }
0563 }
0564
0565
0566
0567
0568
0569
0570
0571
0572
0573
0574
0575
0576 void ieee80211_mps_frame_release(struct sta_info *sta,
0577 struct ieee802_11_elems *elems)
0578 {
0579 int ac, buffer_local = 0;
0580 bool has_buffered = false;
0581
0582 if (sta->mesh->plink_state == NL80211_PLINK_ESTAB)
0583 has_buffered = ieee80211_check_tim(elems->tim, elems->tim_len,
0584 sta->mesh->aid);
0585
0586 if (has_buffered)
0587 mps_dbg(sta->sdata, "%pM indicates buffered frames\n",
0588 sta->sta.addr);
0589
0590
0591 if (test_sta_flag(sta, WLAN_STA_PS_STA) &&
0592 (!elems->awake_window || !get_unaligned_le16(elems->awake_window)))
0593 return;
0594
0595 if (!test_sta_flag(sta, WLAN_STA_MPSP_OWNER))
0596 for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
0597 buffer_local += skb_queue_len(&sta->ps_tx_buf[ac]) +
0598 skb_queue_len(&sta->tx_filtered[ac]);
0599
0600 if (!has_buffered && !buffer_local)
0601 return;
0602
0603 if (sta->mesh->plink_state == NL80211_PLINK_ESTAB)
0604 mpsp_trigger_send(sta, has_buffered, !buffer_local);
0605 else
0606 mps_frame_deliver(sta, 1);
0607 }