mt76x02_beacon.c 5.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213
  1. // SPDX-License-Identifier: ISC
  2. /*
  3. * Copyright (C) 2016 Felix Fietkau <nbd@nbd.name>
  4. * Copyright (C) 2018 Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
  5. * Copyright (C) 2018 Stanislaw Gruszka <stf_xl@wp.pl>
  6. */
  7. #include "mt76x02.h"
  8. static void mt76x02_set_beacon_offsets(struct mt76x02_dev *dev)
  9. {
  10. u32 regs[4] = {};
  11. u16 val;
  12. int i;
  13. for (i = 0; i < dev->beacon_ops->nslots; i++) {
  14. val = i * dev->beacon_ops->slot_size;
  15. regs[i / 4] |= (val / 64) << (8 * (i % 4));
  16. }
  17. for (i = 0; i < 4; i++)
  18. mt76_wr(dev, MT_BCN_OFFSET(i), regs[i]);
  19. }
  20. static int
  21. mt76x02_write_beacon(struct mt76x02_dev *dev, int offset, struct sk_buff *skb)
  22. {
  23. int beacon_len = dev->beacon_ops->slot_size;
  24. if (WARN_ON_ONCE(beacon_len < skb->len + sizeof(struct mt76x02_txwi)))
  25. return -ENOSPC;
  26. /* USB devices already reserve enough skb headroom for txwi's. This
  27. * helps to save slow copies over USB.
  28. */
  29. if (mt76_is_usb(&dev->mt76)) {
  30. struct mt76x02_txwi *txwi;
  31. txwi = (struct mt76x02_txwi *)(skb->data - sizeof(*txwi));
  32. mt76x02_mac_write_txwi(dev, txwi, skb, NULL, NULL, skb->len);
  33. skb_push(skb, sizeof(*txwi));
  34. } else {
  35. struct mt76x02_txwi txwi;
  36. mt76x02_mac_write_txwi(dev, &txwi, skb, NULL, NULL, skb->len);
  37. mt76_wr_copy(dev, offset, &txwi, sizeof(txwi));
  38. offset += sizeof(txwi);
  39. }
  40. mt76_wr_copy(dev, offset, skb->data, skb->len);
  41. return 0;
  42. }
  43. void mt76x02_mac_set_beacon(struct mt76x02_dev *dev,
  44. struct sk_buff *skb)
  45. {
  46. int bcn_len = dev->beacon_ops->slot_size;
  47. int bcn_addr = MT_BEACON_BASE + (bcn_len * dev->beacon_data_count);
  48. if (!mt76x02_write_beacon(dev, bcn_addr, skb))
  49. dev->beacon_data_count++;
  50. dev_kfree_skb(skb);
  51. }
  52. EXPORT_SYMBOL_GPL(mt76x02_mac_set_beacon);
  53. void mt76x02_mac_set_beacon_enable(struct mt76x02_dev *dev,
  54. struct ieee80211_vif *vif, bool enable)
  55. {
  56. struct mt76x02_vif *mvif = (struct mt76x02_vif *)vif->drv_priv;
  57. u8 old_mask = dev->mt76.beacon_mask;
  58. mt76x02_pre_tbtt_enable(dev, false);
  59. if (!dev->mt76.beacon_mask)
  60. dev->tbtt_count = 0;
  61. if (enable) {
  62. dev->mt76.beacon_mask |= BIT(mvif->idx);
  63. } else {
  64. dev->mt76.beacon_mask &= ~BIT(mvif->idx);
  65. }
  66. if (!!old_mask == !!dev->mt76.beacon_mask)
  67. goto out;
  68. if (dev->mt76.beacon_mask)
  69. mt76_set(dev, MT_BEACON_TIME_CFG,
  70. MT_BEACON_TIME_CFG_BEACON_TX |
  71. MT_BEACON_TIME_CFG_TBTT_EN |
  72. MT_BEACON_TIME_CFG_TIMER_EN);
  73. else
  74. mt76_clear(dev, MT_BEACON_TIME_CFG,
  75. MT_BEACON_TIME_CFG_BEACON_TX |
  76. MT_BEACON_TIME_CFG_TBTT_EN |
  77. MT_BEACON_TIME_CFG_TIMER_EN);
  78. mt76x02_beacon_enable(dev, !!dev->mt76.beacon_mask);
  79. out:
  80. mt76x02_pre_tbtt_enable(dev, true);
  81. }
  82. void
  83. mt76x02_resync_beacon_timer(struct mt76x02_dev *dev)
  84. {
  85. u32 timer_val = dev->mt76.beacon_int << 4;
  86. dev->tbtt_count++;
  87. /*
  88. * Beacon timer drifts by 1us every tick, the timer is configured
  89. * in 1/16 TU (64us) units.
  90. */
  91. if (dev->tbtt_count < 63)
  92. return;
  93. /*
  94. * The updated beacon interval takes effect after two TBTT, because
  95. * at this point the original interval has already been loaded into
  96. * the next TBTT_TIMER value
  97. */
  98. if (dev->tbtt_count == 63)
  99. timer_val -= 1;
  100. mt76_rmw_field(dev, MT_BEACON_TIME_CFG,
  101. MT_BEACON_TIME_CFG_INTVAL, timer_val);
  102. if (dev->tbtt_count >= 64)
  103. dev->tbtt_count = 0;
  104. }
  105. EXPORT_SYMBOL_GPL(mt76x02_resync_beacon_timer);
  106. void
  107. mt76x02_update_beacon_iter(void *priv, u8 *mac, struct ieee80211_vif *vif)
  108. {
  109. struct mt76x02_dev *dev = (struct mt76x02_dev *)priv;
  110. struct mt76x02_vif *mvif = (struct mt76x02_vif *)vif->drv_priv;
  111. struct sk_buff *skb = NULL;
  112. if (!(dev->mt76.beacon_mask & BIT(mvif->idx)))
  113. return;
  114. skb = ieee80211_beacon_get(mt76_hw(dev), vif);
  115. if (!skb)
  116. return;
  117. mt76x02_mac_set_beacon(dev, skb);
  118. }
  119. EXPORT_SYMBOL_GPL(mt76x02_update_beacon_iter);
  120. static void
  121. mt76x02_add_buffered_bc(void *priv, u8 *mac, struct ieee80211_vif *vif)
  122. {
  123. struct beacon_bc_data *data = priv;
  124. struct mt76x02_dev *dev = data->dev;
  125. struct mt76x02_vif *mvif = (struct mt76x02_vif *)vif->drv_priv;
  126. struct ieee80211_tx_info *info;
  127. struct sk_buff *skb;
  128. if (!(dev->mt76.beacon_mask & BIT(mvif->idx)))
  129. return;
  130. skb = ieee80211_get_buffered_bc(mt76_hw(dev), vif);
  131. if (!skb)
  132. return;
  133. info = IEEE80211_SKB_CB(skb);
  134. info->control.vif = vif;
  135. info->flags |= IEEE80211_TX_CTL_ASSIGN_SEQ;
  136. mt76_skb_set_moredata(skb, true);
  137. __skb_queue_tail(&data->q, skb);
  138. data->tail[mvif->idx] = skb;
  139. }
  140. void
  141. mt76x02_enqueue_buffered_bc(struct mt76x02_dev *dev,
  142. struct beacon_bc_data *data,
  143. int max_nframes)
  144. {
  145. int i, nframes;
  146. data->dev = dev;
  147. __skb_queue_head_init(&data->q);
  148. do {
  149. nframes = skb_queue_len(&data->q);
  150. ieee80211_iterate_active_interfaces_atomic(mt76_hw(dev),
  151. IEEE80211_IFACE_ITER_RESUME_ALL,
  152. mt76x02_add_buffered_bc, data);
  153. } while (nframes != skb_queue_len(&data->q) &&
  154. skb_queue_len(&data->q) < max_nframes);
  155. if (!skb_queue_len(&data->q))
  156. return;
  157. for (i = 0; i < ARRAY_SIZE(data->tail); i++) {
  158. if (!data->tail[i])
  159. continue;
  160. mt76_skb_set_moredata(data->tail[i], false);
  161. }
  162. }
  163. EXPORT_SYMBOL_GPL(mt76x02_enqueue_buffered_bc);
  164. void mt76x02_init_beacon_config(struct mt76x02_dev *dev)
  165. {
  166. mt76_clear(dev, MT_BEACON_TIME_CFG, (MT_BEACON_TIME_CFG_TIMER_EN |
  167. MT_BEACON_TIME_CFG_TBTT_EN |
  168. MT_BEACON_TIME_CFG_BEACON_TX));
  169. mt76_set(dev, MT_BEACON_TIME_CFG, MT_BEACON_TIME_CFG_SYNC_MODE);
  170. mt76_wr(dev, MT_BCN_BYPASS_MASK, 0xffff);
  171. mt76x02_set_beacon_offsets(dev);
  172. }
  173. EXPORT_SYMBOL_GPL(mt76x02_init_beacon_config);