rt2x00queue.c 33 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289
  1. /*
  2. Copyright (C) 2010 Willow Garage <http://www.willowgarage.com>
  3. Copyright (C) 2004 - 2010 Ivo van Doorn <IvDoorn@gmail.com>
  4. Copyright (C) 2004 - 2009 Gertjan van Wingerde <gwingerde@gmail.com>
  5. <http://rt2x00.serialmonkey.com>
  6. This program is free software; you can redistribute it and/or modify
  7. it under the terms of the GNU General Public License as published by
  8. the Free Software Foundation; either version 2 of the License, or
  9. (at your option) any later version.
  10. This program is distributed in the hope that it will be useful,
  11. but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13. GNU General Public License for more details.
  14. You should have received a copy of the GNU General Public License
  15. along with this program; if not, write to the
  16. Free Software Foundation, Inc.,
  17. 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18. */
  19. /*
  20. Module: rt2x00lib
  21. Abstract: rt2x00 queue specific routines.
  22. */
  23. #include <linux/slab.h>
  24. #include <linux/kernel.h>
  25. #include <linux/module.h>
  26. #include <linux/dma-mapping.h>
  27. #include "rt2x00.h"
  28. #include "rt2x00lib.h"
  29. struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry)
  30. {
  31. struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
  32. struct sk_buff *skb;
  33. struct skb_frame_desc *skbdesc;
  34. unsigned int frame_size;
  35. unsigned int head_size = 0;
  36. unsigned int tail_size = 0;
  37. /*
  38. * The frame size includes descriptor size, because the
  39. * hardware directly receive the frame into the skbuffer.
  40. */
  41. frame_size = entry->queue->data_size + entry->queue->desc_size;
  42. /*
  43. * The payload should be aligned to a 4-byte boundary,
  44. * this means we need at least 3 bytes for moving the frame
  45. * into the correct offset.
  46. */
  47. head_size = 4;
  48. /*
  49. * For IV/EIV/ICV assembly we must make sure there is
  50. * at least 8 bytes bytes available in headroom for IV/EIV
  51. * and 8 bytes for ICV data as tailroon.
  52. */
  53. if (test_bit(CAPABILITY_HW_CRYPTO, &rt2x00dev->cap_flags)) {
  54. head_size += 8;
  55. tail_size += 8;
  56. }
  57. /*
  58. * Allocate skbuffer.
  59. */
  60. skb = dev_alloc_skb(frame_size + head_size + tail_size);
  61. if (!skb)
  62. return NULL;
  63. /*
  64. * Make sure we not have a frame with the requested bytes
  65. * available in the head and tail.
  66. */
  67. skb_reserve(skb, head_size);
  68. skb_put(skb, frame_size);
  69. /*
  70. * Populate skbdesc.
  71. */
  72. skbdesc = get_skb_frame_desc(skb);
  73. memset(skbdesc, 0, sizeof(*skbdesc));
  74. skbdesc->entry = entry;
  75. if (test_bit(REQUIRE_DMA, &rt2x00dev->cap_flags)) {
  76. skbdesc->skb_dma = dma_map_single(rt2x00dev->dev,
  77. skb->data,
  78. skb->len,
  79. DMA_FROM_DEVICE);
  80. skbdesc->flags |= SKBDESC_DMA_MAPPED_RX;
  81. }
  82. return skb;
  83. }
  84. void rt2x00queue_map_txskb(struct queue_entry *entry)
  85. {
  86. struct device *dev = entry->queue->rt2x00dev->dev;
  87. struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
  88. skbdesc->skb_dma =
  89. dma_map_single(dev, entry->skb->data, entry->skb->len, DMA_TO_DEVICE);
  90. skbdesc->flags |= SKBDESC_DMA_MAPPED_TX;
  91. }
  92. EXPORT_SYMBOL_GPL(rt2x00queue_map_txskb);
  93. void rt2x00queue_unmap_skb(struct queue_entry *entry)
  94. {
  95. struct device *dev = entry->queue->rt2x00dev->dev;
  96. struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
  97. if (skbdesc->flags & SKBDESC_DMA_MAPPED_RX) {
  98. dma_unmap_single(dev, skbdesc->skb_dma, entry->skb->len,
  99. DMA_FROM_DEVICE);
  100. skbdesc->flags &= ~SKBDESC_DMA_MAPPED_RX;
  101. } else if (skbdesc->flags & SKBDESC_DMA_MAPPED_TX) {
  102. dma_unmap_single(dev, skbdesc->skb_dma, entry->skb->len,
  103. DMA_TO_DEVICE);
  104. skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
  105. }
  106. }
  107. EXPORT_SYMBOL_GPL(rt2x00queue_unmap_skb);
  108. void rt2x00queue_free_skb(struct queue_entry *entry)
  109. {
  110. if (!entry->skb)
  111. return;
  112. rt2x00queue_unmap_skb(entry);
  113. dev_kfree_skb_any(entry->skb);
  114. entry->skb = NULL;
  115. }
  116. void rt2x00queue_align_frame(struct sk_buff *skb)
  117. {
  118. unsigned int frame_length = skb->len;
  119. unsigned int align = ALIGN_SIZE(skb, 0);
  120. if (!align)
  121. return;
  122. skb_push(skb, align);
  123. memmove(skb->data, skb->data + align, frame_length);
  124. skb_trim(skb, frame_length);
  125. }
  126. void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int header_length)
  127. {
  128. unsigned int payload_length = skb->len - header_length;
  129. unsigned int header_align = ALIGN_SIZE(skb, 0);
  130. unsigned int payload_align = ALIGN_SIZE(skb, header_length);
  131. unsigned int l2pad = payload_length ? L2PAD_SIZE(header_length) : 0;
  132. /*
  133. * Adjust the header alignment if the payload needs to be moved more
  134. * than the header.
  135. */
  136. if (payload_align > header_align)
  137. header_align += 4;
  138. /* There is nothing to do if no alignment is needed */
  139. if (!header_align)
  140. return;
  141. /* Reserve the amount of space needed in front of the frame */
  142. skb_push(skb, header_align);
  143. /*
  144. * Move the header.
  145. */
  146. memmove(skb->data, skb->data + header_align, header_length);
  147. /* Move the payload, if present and if required */
  148. if (payload_length && payload_align)
  149. memmove(skb->data + header_length + l2pad,
  150. skb->data + header_length + l2pad + payload_align,
  151. payload_length);
  152. /* Trim the skb to the correct size */
  153. skb_trim(skb, header_length + l2pad + payload_length);
  154. }
  155. void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int header_length)
  156. {
  157. /*
  158. * L2 padding is only present if the skb contains more than just the
  159. * IEEE 802.11 header.
  160. */
  161. unsigned int l2pad = (skb->len > header_length) ?
  162. L2PAD_SIZE(header_length) : 0;
  163. if (!l2pad)
  164. return;
  165. memmove(skb->data + l2pad, skb->data, header_length);
  166. skb_pull(skb, l2pad);
  167. }
  168. static void rt2x00queue_create_tx_descriptor_seq(struct rt2x00_dev *rt2x00dev,
  169. struct sk_buff *skb,
  170. struct txentry_desc *txdesc)
  171. {
  172. struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
  173. struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
  174. struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
  175. if (!(tx_info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ))
  176. return;
  177. __set_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
  178. if (!test_bit(REQUIRE_SW_SEQNO, &rt2x00dev->cap_flags))
  179. return;
  180. /*
  181. * The hardware is not able to insert a sequence number. Assign a
  182. * software generated one here.
  183. *
  184. * This is wrong because beacons are not getting sequence
  185. * numbers assigned properly.
  186. *
  187. * A secondary problem exists for drivers that cannot toggle
  188. * sequence counting per-frame, since those will override the
  189. * sequence counter given by mac80211.
  190. */
  191. spin_lock(&intf->seqlock);
  192. if (test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags))
  193. intf->seqno += 0x10;
  194. hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
  195. hdr->seq_ctrl |= cpu_to_le16(intf->seqno);
  196. spin_unlock(&intf->seqlock);
  197. }
  198. static void rt2x00queue_create_tx_descriptor_plcp(struct rt2x00_dev *rt2x00dev,
  199. struct sk_buff *skb,
  200. struct txentry_desc *txdesc,
  201. const struct rt2x00_rate *hwrate)
  202. {
  203. struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
  204. struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
  205. unsigned int data_length;
  206. unsigned int duration;
  207. unsigned int residual;
  208. /*
  209. * Determine with what IFS priority this frame should be send.
  210. * Set ifs to IFS_SIFS when the this is not the first fragment,
  211. * or this fragment came after RTS/CTS.
  212. */
  213. if (test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags))
  214. txdesc->u.plcp.ifs = IFS_BACKOFF;
  215. else
  216. txdesc->u.plcp.ifs = IFS_SIFS;
  217. /* Data length + CRC + Crypto overhead (IV/EIV/ICV/MIC) */
  218. data_length = skb->len + 4;
  219. data_length += rt2x00crypto_tx_overhead(rt2x00dev, skb);
  220. /*
  221. * PLCP setup
  222. * Length calculation depends on OFDM/CCK rate.
  223. */
  224. txdesc->u.plcp.signal = hwrate->plcp;
  225. txdesc->u.plcp.service = 0x04;
  226. if (hwrate->flags & DEV_RATE_OFDM) {
  227. txdesc->u.plcp.length_high = (data_length >> 6) & 0x3f;
  228. txdesc->u.plcp.length_low = data_length & 0x3f;
  229. } else {
  230. /*
  231. * Convert length to microseconds.
  232. */
  233. residual = GET_DURATION_RES(data_length, hwrate->bitrate);
  234. duration = GET_DURATION(data_length, hwrate->bitrate);
  235. if (residual != 0) {
  236. duration++;
  237. /*
  238. * Check if we need to set the Length Extension
  239. */
  240. if (hwrate->bitrate == 110 && residual <= 30)
  241. txdesc->u.plcp.service |= 0x80;
  242. }
  243. txdesc->u.plcp.length_high = (duration >> 8) & 0xff;
  244. txdesc->u.plcp.length_low = duration & 0xff;
  245. /*
  246. * When preamble is enabled we should set the
  247. * preamble bit for the signal.
  248. */
  249. if (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
  250. txdesc->u.plcp.signal |= 0x08;
  251. }
  252. }
  253. static void rt2x00queue_create_tx_descriptor_ht(struct rt2x00_dev *rt2x00dev,
  254. struct sk_buff *skb,
  255. struct txentry_desc *txdesc,
  256. const struct rt2x00_rate *hwrate)
  257. {
  258. struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
  259. struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
  260. struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
  261. struct rt2x00_sta *sta_priv = NULL;
  262. if (tx_info->control.sta) {
  263. txdesc->u.ht.mpdu_density =
  264. tx_info->control.sta->ht_cap.ampdu_density;
  265. sta_priv = sta_to_rt2x00_sta(tx_info->control.sta);
  266. txdesc->u.ht.wcid = sta_priv->wcid;
  267. }
  268. /*
  269. * If IEEE80211_TX_RC_MCS is set txrate->idx just contains the
  270. * mcs rate to be used
  271. */
  272. if (txrate->flags & IEEE80211_TX_RC_MCS) {
  273. txdesc->u.ht.mcs = txrate->idx;
  274. /*
  275. * MIMO PS should be set to 1 for STA's using dynamic SM PS
  276. * when using more then one tx stream (>MCS7).
  277. */
  278. if (tx_info->control.sta && txdesc->u.ht.mcs > 7 &&
  279. ((tx_info->control.sta->ht_cap.cap &
  280. IEEE80211_HT_CAP_SM_PS) >>
  281. IEEE80211_HT_CAP_SM_PS_SHIFT) ==
  282. WLAN_HT_CAP_SM_PS_DYNAMIC)
  283. __set_bit(ENTRY_TXD_HT_MIMO_PS, &txdesc->flags);
  284. } else {
  285. txdesc->u.ht.mcs = rt2x00_get_rate_mcs(hwrate->mcs);
  286. if (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
  287. txdesc->u.ht.mcs |= 0x08;
  288. }
  289. if (test_bit(CONFIG_HT_DISABLED, &rt2x00dev->flags)) {
  290. if (!(tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT))
  291. txdesc->u.ht.txop = TXOP_SIFS;
  292. else
  293. txdesc->u.ht.txop = TXOP_BACKOFF;
  294. /* Left zero on all other settings. */
  295. return;
  296. }
  297. txdesc->u.ht.ba_size = 7; /* FIXME: What value is needed? */
  298. /*
  299. * Only one STBC stream is supported for now.
  300. */
  301. if (tx_info->flags & IEEE80211_TX_CTL_STBC)
  302. txdesc->u.ht.stbc = 1;
  303. /*
  304. * This frame is eligible for an AMPDU, however, don't aggregate
  305. * frames that are intended to probe a specific tx rate.
  306. */
  307. if (tx_info->flags & IEEE80211_TX_CTL_AMPDU &&
  308. !(tx_info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE))
  309. __set_bit(ENTRY_TXD_HT_AMPDU, &txdesc->flags);
  310. /*
  311. * Set 40Mhz mode if necessary (for legacy rates this will
  312. * duplicate the frame to both channels).
  313. */
  314. if (txrate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH ||
  315. txrate->flags & IEEE80211_TX_RC_DUP_DATA)
  316. __set_bit(ENTRY_TXD_HT_BW_40, &txdesc->flags);
  317. if (txrate->flags & IEEE80211_TX_RC_SHORT_GI)
  318. __set_bit(ENTRY_TXD_HT_SHORT_GI, &txdesc->flags);
  319. /*
  320. * Determine IFS values
  321. * - Use TXOP_BACKOFF for management frames except beacons
  322. * - Use TXOP_SIFS for fragment bursts
  323. * - Use TXOP_HTTXOP for everything else
  324. *
  325. * Note: rt2800 devices won't use CTS protection (if used)
  326. * for frames not transmitted with TXOP_HTTXOP
  327. */
  328. if (ieee80211_is_mgmt(hdr->frame_control) &&
  329. !ieee80211_is_beacon(hdr->frame_control))
  330. txdesc->u.ht.txop = TXOP_BACKOFF;
  331. else if (!(tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT))
  332. txdesc->u.ht.txop = TXOP_SIFS;
  333. else
  334. txdesc->u.ht.txop = TXOP_HTTXOP;
  335. }
  336. static void rt2x00queue_create_tx_descriptor(struct rt2x00_dev *rt2x00dev,
  337. struct sk_buff *skb,
  338. struct txentry_desc *txdesc)
  339. {
  340. struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
  341. struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
  342. struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
  343. struct ieee80211_rate *rate;
  344. const struct rt2x00_rate *hwrate = NULL;
  345. memset(txdesc, 0, sizeof(*txdesc));
  346. /*
  347. * Header and frame information.
  348. */
  349. txdesc->length = skb->len;
  350. txdesc->header_length = ieee80211_get_hdrlen_from_skb(skb);
  351. /*
  352. * Check whether this frame is to be acked.
  353. */
  354. if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
  355. __set_bit(ENTRY_TXD_ACK, &txdesc->flags);
  356. /*
  357. * Check if this is a RTS/CTS frame
  358. */
  359. if (ieee80211_is_rts(hdr->frame_control) ||
  360. ieee80211_is_cts(hdr->frame_control)) {
  361. __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
  362. if (ieee80211_is_rts(hdr->frame_control))
  363. __set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
  364. else
  365. __set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
  366. if (tx_info->control.rts_cts_rate_idx >= 0)
  367. rate =
  368. ieee80211_get_rts_cts_rate(rt2x00dev->hw, tx_info);
  369. }
  370. /*
  371. * Determine retry information.
  372. */
  373. txdesc->retry_limit = tx_info->control.rates[0].count - 1;
  374. if (txdesc->retry_limit >= rt2x00dev->long_retry)
  375. __set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);
  376. /*
  377. * Check if more fragments are pending
  378. */
  379. if (ieee80211_has_morefrags(hdr->frame_control)) {
  380. __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
  381. __set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
  382. }
  383. /*
  384. * Check if more frames (!= fragments) are pending
  385. */
  386. if (tx_info->flags & IEEE80211_TX_CTL_MORE_FRAMES)
  387. __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
  388. /*
  389. * Beacons and probe responses require the tsf timestamp
  390. * to be inserted into the frame.
  391. */
  392. if (ieee80211_is_beacon(hdr->frame_control) ||
  393. ieee80211_is_probe_resp(hdr->frame_control))
  394. __set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags);
  395. if ((tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) &&
  396. !test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags))
  397. __set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
  398. /*
  399. * Determine rate modulation.
  400. */
  401. if (txrate->flags & IEEE80211_TX_RC_GREEN_FIELD)
  402. txdesc->rate_mode = RATE_MODE_HT_GREENFIELD;
  403. else if (txrate->flags & IEEE80211_TX_RC_MCS)
  404. txdesc->rate_mode = RATE_MODE_HT_MIX;
  405. else {
  406. rate = ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
  407. hwrate = rt2x00_get_rate(rate->hw_value);
  408. if (hwrate->flags & DEV_RATE_OFDM)
  409. txdesc->rate_mode = RATE_MODE_OFDM;
  410. else
  411. txdesc->rate_mode = RATE_MODE_CCK;
  412. }
  413. /*
  414. * Apply TX descriptor handling by components
  415. */
  416. rt2x00crypto_create_tx_descriptor(rt2x00dev, skb, txdesc);
  417. rt2x00queue_create_tx_descriptor_seq(rt2x00dev, skb, txdesc);
  418. if (test_bit(REQUIRE_HT_TX_DESC, &rt2x00dev->cap_flags))
  419. rt2x00queue_create_tx_descriptor_ht(rt2x00dev, skb, txdesc,
  420. hwrate);
  421. else
  422. rt2x00queue_create_tx_descriptor_plcp(rt2x00dev, skb, txdesc,
  423. hwrate);
  424. }
  425. static int rt2x00queue_write_tx_data(struct queue_entry *entry,
  426. struct txentry_desc *txdesc)
  427. {
  428. struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
  429. /*
  430. * This should not happen, we already checked the entry
  431. * was ours. When the hardware disagrees there has been
  432. * a queue corruption!
  433. */
  434. if (unlikely(rt2x00dev->ops->lib->get_entry_state &&
  435. rt2x00dev->ops->lib->get_entry_state(entry))) {
  436. ERROR(rt2x00dev,
  437. "Corrupt queue %d, accessing entry which is not ours.\n"
  438. "Please file bug report to %s.\n",
  439. entry->queue->qid, DRV_PROJECT);
  440. return -EINVAL;
  441. }
  442. /*
  443. * Add the requested extra tx headroom in front of the skb.
  444. */
  445. skb_push(entry->skb, rt2x00dev->ops->extra_tx_headroom);
  446. memset(entry->skb->data, 0, rt2x00dev->ops->extra_tx_headroom);
  447. /*
  448. * Call the driver's write_tx_data function, if it exists.
  449. */
  450. if (rt2x00dev->ops->lib->write_tx_data)
  451. rt2x00dev->ops->lib->write_tx_data(entry, txdesc);
  452. /*
  453. * Map the skb to DMA.
  454. */
  455. if (test_bit(REQUIRE_DMA, &rt2x00dev->cap_flags))
  456. rt2x00queue_map_txskb(entry);
  457. return 0;
  458. }
  459. static void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
  460. struct txentry_desc *txdesc)
  461. {
  462. struct data_queue *queue = entry->queue;
  463. queue->rt2x00dev->ops->lib->write_tx_desc(entry, txdesc);
  464. /*
  465. * All processing on the frame has been completed, this means
  466. * it is now ready to be dumped to userspace through debugfs.
  467. */
  468. rt2x00debug_dump_frame(queue->rt2x00dev, DUMP_FRAME_TX, entry->skb);
  469. }
  470. static void rt2x00queue_kick_tx_queue(struct data_queue *queue,
  471. struct txentry_desc *txdesc)
  472. {
  473. /*
  474. * Check if we need to kick the queue, there are however a few rules
  475. * 1) Don't kick unless this is the last in frame in a burst.
  476. * When the burst flag is set, this frame is always followed
  477. * by another frame which in some way are related to eachother.
  478. * This is true for fragments, RTS or CTS-to-self frames.
  479. * 2) Rule 1 can be broken when the available entries
  480. * in the queue are less then a certain threshold.
  481. */
  482. if (rt2x00queue_threshold(queue) ||
  483. !test_bit(ENTRY_TXD_BURST, &txdesc->flags))
  484. queue->rt2x00dev->ops->lib->kick_queue(queue);
  485. }
  486. int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
  487. bool local)
  488. {
  489. struct ieee80211_tx_info *tx_info;
  490. struct queue_entry *entry;
  491. struct txentry_desc txdesc;
  492. struct skb_frame_desc *skbdesc;
  493. u8 rate_idx, rate_flags;
  494. int ret = 0;
  495. /*
  496. * Copy all TX descriptor information into txdesc,
  497. * after that we are free to use the skb->cb array
  498. * for our information.
  499. */
  500. rt2x00queue_create_tx_descriptor(queue->rt2x00dev, skb, &txdesc);
  501. /*
  502. * All information is retrieved from the skb->cb array,
  503. * now we should claim ownership of the driver part of that
  504. * array, preserving the bitrate index and flags.
  505. */
  506. tx_info = IEEE80211_SKB_CB(skb);
  507. rate_idx = tx_info->control.rates[0].idx;
  508. rate_flags = tx_info->control.rates[0].flags;
  509. skbdesc = get_skb_frame_desc(skb);
  510. memset(skbdesc, 0, sizeof(*skbdesc));
  511. skbdesc->tx_rate_idx = rate_idx;
  512. skbdesc->tx_rate_flags = rate_flags;
  513. if (local)
  514. skbdesc->flags |= SKBDESC_NOT_MAC80211;
  515. /*
  516. * When hardware encryption is supported, and this frame
  517. * is to be encrypted, we should strip the IV/EIV data from
  518. * the frame so we can provide it to the driver separately.
  519. */
  520. if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc.flags) &&
  521. !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc.flags)) {
  522. if (test_bit(REQUIRE_COPY_IV, &queue->rt2x00dev->cap_flags))
  523. rt2x00crypto_tx_copy_iv(skb, &txdesc);
  524. else
  525. rt2x00crypto_tx_remove_iv(skb, &txdesc);
  526. }
  527. /*
  528. * When DMA allocation is required we should guarantee to the
  529. * driver that the DMA is aligned to a 4-byte boundary.
  530. * However some drivers require L2 padding to pad the payload
  531. * rather then the header. This could be a requirement for
  532. * PCI and USB devices, while header alignment only is valid
  533. * for PCI devices.
  534. */
  535. if (test_bit(REQUIRE_L2PAD, &queue->rt2x00dev->cap_flags))
  536. rt2x00queue_insert_l2pad(skb, txdesc.header_length);
  537. else if (test_bit(REQUIRE_DMA, &queue->rt2x00dev->cap_flags))
  538. rt2x00queue_align_frame(skb);
  539. /*
  540. * That function must be called with bh disabled.
  541. */
  542. spin_lock(&queue->tx_lock);
  543. if (unlikely(rt2x00queue_full(queue))) {
  544. ERROR(queue->rt2x00dev,
  545. "Dropping frame due to full tx queue %d.\n", queue->qid);
  546. ret = -ENOBUFS;
  547. goto out;
  548. }
  549. entry = rt2x00queue_get_entry(queue, Q_INDEX);
  550. if (unlikely(test_and_set_bit(ENTRY_OWNER_DEVICE_DATA,
  551. &entry->flags))) {
  552. ERROR(queue->rt2x00dev,
  553. "Arrived at non-free entry in the non-full queue %d.\n"
  554. "Please file bug report to %s.\n",
  555. queue->qid, DRV_PROJECT);
  556. ret = -EINVAL;
  557. goto out;
  558. }
  559. skbdesc->entry = entry;
  560. entry->skb = skb;
  561. /*
  562. * It could be possible that the queue was corrupted and this
  563. * call failed. Since we always return NETDEV_TX_OK to mac80211,
  564. * this frame will simply be dropped.
  565. */
  566. if (unlikely(rt2x00queue_write_tx_data(entry, &txdesc))) {
  567. clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
  568. entry->skb = NULL;
  569. ret = -EIO;
  570. goto out;
  571. }
  572. set_bit(ENTRY_DATA_PENDING, &entry->flags);
  573. rt2x00queue_index_inc(entry, Q_INDEX);
  574. rt2x00queue_write_tx_descriptor(entry, &txdesc);
  575. rt2x00queue_kick_tx_queue(queue, &txdesc);
  576. out:
  577. spin_unlock(&queue->tx_lock);
  578. return ret;
  579. }
  580. int rt2x00queue_clear_beacon(struct rt2x00_dev *rt2x00dev,
  581. struct ieee80211_vif *vif)
  582. {
  583. struct rt2x00_intf *intf = vif_to_intf(vif);
  584. if (unlikely(!intf->beacon))
  585. return -ENOBUFS;
  586. mutex_lock(&intf->beacon_skb_mutex);
  587. /*
  588. * Clean up the beacon skb.
  589. */
  590. rt2x00queue_free_skb(intf->beacon);
  591. /*
  592. * Clear beacon (single bssid devices don't need to clear the beacon
  593. * since the beacon queue will get stopped anyway).
  594. */
  595. if (rt2x00dev->ops->lib->clear_beacon)
  596. rt2x00dev->ops->lib->clear_beacon(intf->beacon);
  597. mutex_unlock(&intf->beacon_skb_mutex);
  598. return 0;
  599. }
  600. int rt2x00queue_update_beacon_locked(struct rt2x00_dev *rt2x00dev,
  601. struct ieee80211_vif *vif)
  602. {
  603. struct rt2x00_intf *intf = vif_to_intf(vif);
  604. struct skb_frame_desc *skbdesc;
  605. struct txentry_desc txdesc;
  606. if (unlikely(!intf->beacon))
  607. return -ENOBUFS;
  608. /*
  609. * Clean up the beacon skb.
  610. */
  611. rt2x00queue_free_skb(intf->beacon);
  612. intf->beacon->skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
  613. if (!intf->beacon->skb)
  614. return -ENOMEM;
  615. /*
  616. * Copy all TX descriptor information into txdesc,
  617. * after that we are free to use the skb->cb array
  618. * for our information.
  619. */
  620. rt2x00queue_create_tx_descriptor(rt2x00dev, intf->beacon->skb, &txdesc);
  621. /*
  622. * Fill in skb descriptor
  623. */
  624. skbdesc = get_skb_frame_desc(intf->beacon->skb);
  625. memset(skbdesc, 0, sizeof(*skbdesc));
  626. skbdesc->entry = intf->beacon;
  627. /*
  628. * Send beacon to hardware.
  629. */
  630. rt2x00dev->ops->lib->write_beacon(intf->beacon, &txdesc);
  631. return 0;
  632. }
  633. int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
  634. struct ieee80211_vif *vif)
  635. {
  636. struct rt2x00_intf *intf = vif_to_intf(vif);
  637. int ret;
  638. mutex_lock(&intf->beacon_skb_mutex);
  639. ret = rt2x00queue_update_beacon_locked(rt2x00dev, vif);
  640. mutex_unlock(&intf->beacon_skb_mutex);
  641. return ret;
  642. }
  643. bool rt2x00queue_for_each_entry(struct data_queue *queue,
  644. enum queue_index start,
  645. enum queue_index end,
  646. void *data,
  647. bool (*fn)(struct queue_entry *entry,
  648. void *data))
  649. {
  650. unsigned long irqflags;
  651. unsigned int index_start;
  652. unsigned int index_end;
  653. unsigned int i;
  654. if (unlikely(start >= Q_INDEX_MAX || end >= Q_INDEX_MAX)) {
  655. ERROR(queue->rt2x00dev,
  656. "Entry requested from invalid index range (%d - %d)\n",
  657. start, end);
  658. return true;
  659. }
  660. /*
  661. * Only protect the range we are going to loop over,
  662. * if during our loop a extra entry is set to pending
  663. * it should not be kicked during this run, since it
  664. * is part of another TX operation.
  665. */
  666. spin_lock_irqsave(&queue->index_lock, irqflags);
  667. index_start = queue->index[start];
  668. index_end = queue->index[end];
  669. spin_unlock_irqrestore(&queue->index_lock, irqflags);
  670. /*
  671. * Start from the TX done pointer, this guarantees that we will
  672. * send out all frames in the correct order.
  673. */
  674. if (index_start < index_end) {
  675. for (i = index_start; i < index_end; i++) {
  676. if (fn(&queue->entries[i], data))
  677. return true;
  678. }
  679. } else {
  680. for (i = index_start; i < queue->limit; i++) {
  681. if (fn(&queue->entries[i], data))
  682. return true;
  683. }
  684. for (i = 0; i < index_end; i++) {
  685. if (fn(&queue->entries[i], data))
  686. return true;
  687. }
  688. }
  689. return false;
  690. }
  691. EXPORT_SYMBOL_GPL(rt2x00queue_for_each_entry);
  692. struct queue_entry *rt2x00queue_get_entry(struct data_queue *queue,
  693. enum queue_index index)
  694. {
  695. struct queue_entry *entry;
  696. unsigned long irqflags;
  697. if (unlikely(index >= Q_INDEX_MAX)) {
  698. ERROR(queue->rt2x00dev,
  699. "Entry requested from invalid index type (%d)\n", index);
  700. return NULL;
  701. }
  702. spin_lock_irqsave(&queue->index_lock, irqflags);
  703. entry = &queue->entries[queue->index[index]];
  704. spin_unlock_irqrestore(&queue->index_lock, irqflags);
  705. return entry;
  706. }
  707. EXPORT_SYMBOL_GPL(rt2x00queue_get_entry);
  708. void rt2x00queue_index_inc(struct queue_entry *entry, enum queue_index index)
  709. {
  710. struct data_queue *queue = entry->queue;
  711. unsigned long irqflags;
  712. if (unlikely(index >= Q_INDEX_MAX)) {
  713. ERROR(queue->rt2x00dev,
  714. "Index change on invalid index type (%d)\n", index);
  715. return;
  716. }
  717. spin_lock_irqsave(&queue->index_lock, irqflags);
  718. queue->index[index]++;
  719. if (queue->index[index] >= queue->limit)
  720. queue->index[index] = 0;
  721. entry->last_action = jiffies;
  722. if (index == Q_INDEX) {
  723. queue->length++;
  724. } else if (index == Q_INDEX_DONE) {
  725. queue->length--;
  726. queue->count++;
  727. }
  728. spin_unlock_irqrestore(&queue->index_lock, irqflags);
  729. }
  730. void rt2x00queue_pause_queue(struct data_queue *queue)
  731. {
  732. if (!test_bit(DEVICE_STATE_PRESENT, &queue->rt2x00dev->flags) ||
  733. !test_bit(QUEUE_STARTED, &queue->flags) ||
  734. test_and_set_bit(QUEUE_PAUSED, &queue->flags))
  735. return;
  736. switch (queue->qid) {
  737. case QID_AC_VO:
  738. case QID_AC_VI:
  739. case QID_AC_BE:
  740. case QID_AC_BK:
  741. /*
  742. * For TX queues, we have to disable the queue
  743. * inside mac80211.
  744. */
  745. ieee80211_stop_queue(queue->rt2x00dev->hw, queue->qid);
  746. break;
  747. default:
  748. break;
  749. }
  750. }
  751. EXPORT_SYMBOL_GPL(rt2x00queue_pause_queue);
  752. void rt2x00queue_unpause_queue(struct data_queue *queue)
  753. {
  754. if (!test_bit(DEVICE_STATE_PRESENT, &queue->rt2x00dev->flags) ||
  755. !test_bit(QUEUE_STARTED, &queue->flags) ||
  756. !test_and_clear_bit(QUEUE_PAUSED, &queue->flags))
  757. return;
  758. switch (queue->qid) {
  759. case QID_AC_VO:
  760. case QID_AC_VI:
  761. case QID_AC_BE:
  762. case QID_AC_BK:
  763. /*
  764. * For TX queues, we have to enable the queue
  765. * inside mac80211.
  766. */
  767. ieee80211_wake_queue(queue->rt2x00dev->hw, queue->qid);
  768. break;
  769. case QID_RX:
  770. /*
  771. * For RX we need to kick the queue now in order to
  772. * receive frames.
  773. */
  774. queue->rt2x00dev->ops->lib->kick_queue(queue);
  775. default:
  776. break;
  777. }
  778. }
  779. EXPORT_SYMBOL_GPL(rt2x00queue_unpause_queue);
  780. void rt2x00queue_start_queue(struct data_queue *queue)
  781. {
  782. mutex_lock(&queue->status_lock);
  783. if (!test_bit(DEVICE_STATE_PRESENT, &queue->rt2x00dev->flags) ||
  784. test_and_set_bit(QUEUE_STARTED, &queue->flags)) {
  785. mutex_unlock(&queue->status_lock);
  786. return;
  787. }
  788. set_bit(QUEUE_PAUSED, &queue->flags);
  789. queue->rt2x00dev->ops->lib->start_queue(queue);
  790. rt2x00queue_unpause_queue(queue);
  791. mutex_unlock(&queue->status_lock);
  792. }
  793. EXPORT_SYMBOL_GPL(rt2x00queue_start_queue);
  794. void rt2x00queue_stop_queue(struct data_queue *queue)
  795. {
  796. mutex_lock(&queue->status_lock);
  797. if (!test_and_clear_bit(QUEUE_STARTED, &queue->flags)) {
  798. mutex_unlock(&queue->status_lock);
  799. return;
  800. }
  801. rt2x00queue_pause_queue(queue);
  802. queue->rt2x00dev->ops->lib->stop_queue(queue);
  803. mutex_unlock(&queue->status_lock);
  804. }
  805. EXPORT_SYMBOL_GPL(rt2x00queue_stop_queue);
  806. void rt2x00queue_flush_queue(struct data_queue *queue, bool drop)
  807. {
  808. bool started;
  809. bool tx_queue =
  810. (queue->qid == QID_AC_VO) ||
  811. (queue->qid == QID_AC_VI) ||
  812. (queue->qid == QID_AC_BE) ||
  813. (queue->qid == QID_AC_BK);
  814. mutex_lock(&queue->status_lock);
  815. /*
  816. * If the queue has been started, we must stop it temporarily
  817. * to prevent any new frames to be queued on the device. If
  818. * we are not dropping the pending frames, the queue must
  819. * only be stopped in the software and not the hardware,
  820. * otherwise the queue will never become empty on its own.
  821. */
  822. started = test_bit(QUEUE_STARTED, &queue->flags);
  823. if (started) {
  824. /*
  825. * Pause the queue
  826. */
  827. rt2x00queue_pause_queue(queue);
  828. /*
  829. * If we are not supposed to drop any pending
  830. * frames, this means we must force a start (=kick)
  831. * to the queue to make sure the hardware will
  832. * start transmitting.
  833. */
  834. if (!drop && tx_queue)
  835. queue->rt2x00dev->ops->lib->kick_queue(queue);
  836. }
  837. /*
  838. * Check if driver supports flushing, if that is the case we can
  839. * defer the flushing to the driver. Otherwise we must use the
  840. * alternative which just waits for the queue to become empty.
  841. */
  842. if (likely(queue->rt2x00dev->ops->lib->flush_queue))
  843. queue->rt2x00dev->ops->lib->flush_queue(queue, drop);
  844. /*
  845. * The queue flush has failed...
  846. */
  847. if (unlikely(!rt2x00queue_empty(queue)))
  848. WARNING(queue->rt2x00dev, "Queue %d failed to flush\n", queue->qid);
  849. /*
  850. * Restore the queue to the previous status
  851. */
  852. if (started)
  853. rt2x00queue_unpause_queue(queue);
  854. mutex_unlock(&queue->status_lock);
  855. }
  856. EXPORT_SYMBOL_GPL(rt2x00queue_flush_queue);
  857. void rt2x00queue_start_queues(struct rt2x00_dev *rt2x00dev)
  858. {
  859. struct data_queue *queue;
  860. /*
  861. * rt2x00queue_start_queue will call ieee80211_wake_queue
  862. * for each queue after is has been properly initialized.
  863. */
  864. tx_queue_for_each(rt2x00dev, queue)
  865. rt2x00queue_start_queue(queue);
  866. rt2x00queue_start_queue(rt2x00dev->rx);
  867. }
  868. EXPORT_SYMBOL_GPL(rt2x00queue_start_queues);
  869. void rt2x00queue_stop_queues(struct rt2x00_dev *rt2x00dev)
  870. {
  871. struct data_queue *queue;
  872. /*
  873. * rt2x00queue_stop_queue will call ieee80211_stop_queue
  874. * as well, but we are completely shutting doing everything
  875. * now, so it is much safer to stop all TX queues at once,
  876. * and use rt2x00queue_stop_queue for cleaning up.
  877. */
  878. ieee80211_stop_queues(rt2x00dev->hw);
  879. tx_queue_for_each(rt2x00dev, queue)
  880. rt2x00queue_stop_queue(queue);
  881. rt2x00queue_stop_queue(rt2x00dev->rx);
  882. }
  883. EXPORT_SYMBOL_GPL(rt2x00queue_stop_queues);
  884. void rt2x00queue_flush_queues(struct rt2x00_dev *rt2x00dev, bool drop)
  885. {
  886. struct data_queue *queue;
  887. tx_queue_for_each(rt2x00dev, queue)
  888. rt2x00queue_flush_queue(queue, drop);
  889. rt2x00queue_flush_queue(rt2x00dev->rx, drop);
  890. }
  891. EXPORT_SYMBOL_GPL(rt2x00queue_flush_queues);
  892. static void rt2x00queue_reset(struct data_queue *queue)
  893. {
  894. unsigned long irqflags;
  895. unsigned int i;
  896. spin_lock_irqsave(&queue->index_lock, irqflags);
  897. queue->count = 0;
  898. queue->length = 0;
  899. for (i = 0; i < Q_INDEX_MAX; i++)
  900. queue->index[i] = 0;
  901. spin_unlock_irqrestore(&queue->index_lock, irqflags);
  902. }
  903. void rt2x00queue_init_queues(struct rt2x00_dev *rt2x00dev)
  904. {
  905. struct data_queue *queue;
  906. unsigned int i;
  907. queue_for_each(rt2x00dev, queue) {
  908. rt2x00queue_reset(queue);
  909. for (i = 0; i < queue->limit; i++)
  910. rt2x00dev->ops->lib->clear_entry(&queue->entries[i]);
  911. }
  912. }
  913. static int rt2x00queue_alloc_entries(struct data_queue *queue,
  914. const struct data_queue_desc *qdesc)
  915. {
  916. struct queue_entry *entries;
  917. unsigned int entry_size;
  918. unsigned int i;
  919. rt2x00queue_reset(queue);
  920. queue->limit = qdesc->entry_num;
  921. queue->threshold = DIV_ROUND_UP(qdesc->entry_num, 10);
  922. queue->data_size = qdesc->data_size;
  923. queue->desc_size = qdesc->desc_size;
  924. /*
  925. * Allocate all queue entries.
  926. */
  927. entry_size = sizeof(*entries) + qdesc->priv_size;
  928. entries = kcalloc(queue->limit, entry_size, GFP_KERNEL);
  929. if (!entries)
  930. return -ENOMEM;
  931. #define QUEUE_ENTRY_PRIV_OFFSET(__base, __index, __limit, __esize, __psize) \
  932. (((char *)(__base)) + ((__limit) * (__esize)) + \
  933. ((__index) * (__psize)))
  934. for (i = 0; i < queue->limit; i++) {
  935. entries[i].flags = 0;
  936. entries[i].queue = queue;
  937. entries[i].skb = NULL;
  938. entries[i].entry_idx = i;
  939. entries[i].priv_data =
  940. QUEUE_ENTRY_PRIV_OFFSET(entries, i, queue->limit,
  941. sizeof(*entries), qdesc->priv_size);
  942. }
  943. #undef QUEUE_ENTRY_PRIV_OFFSET
  944. queue->entries = entries;
  945. return 0;
  946. }
  947. static void rt2x00queue_free_skbs(struct data_queue *queue)
  948. {
  949. unsigned int i;
  950. if (!queue->entries)
  951. return;
  952. for (i = 0; i < queue->limit; i++) {
  953. rt2x00queue_free_skb(&queue->entries[i]);
  954. }
  955. }
  956. static int rt2x00queue_alloc_rxskbs(struct data_queue *queue)
  957. {
  958. unsigned int i;
  959. struct sk_buff *skb;
  960. for (i = 0; i < queue->limit; i++) {
  961. skb = rt2x00queue_alloc_rxskb(&queue->entries[i]);
  962. if (!skb)
  963. return -ENOMEM;
  964. queue->entries[i].skb = skb;
  965. }
  966. return 0;
  967. }
  968. int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
  969. {
  970. struct data_queue *queue;
  971. int status;
  972. status = rt2x00queue_alloc_entries(rt2x00dev->rx, rt2x00dev->ops->rx);
  973. if (status)
  974. goto exit;
  975. tx_queue_for_each(rt2x00dev, queue) {
  976. status = rt2x00queue_alloc_entries(queue, rt2x00dev->ops->tx);
  977. if (status)
  978. goto exit;
  979. }
  980. status = rt2x00queue_alloc_entries(rt2x00dev->bcn, rt2x00dev->ops->bcn);
  981. if (status)
  982. goto exit;
  983. if (test_bit(REQUIRE_ATIM_QUEUE, &rt2x00dev->cap_flags)) {
  984. status = rt2x00queue_alloc_entries(rt2x00dev->atim,
  985. rt2x00dev->ops->atim);
  986. if (status)
  987. goto exit;
  988. }
  989. status = rt2x00queue_alloc_rxskbs(rt2x00dev->rx);
  990. if (status)
  991. goto exit;
  992. return 0;
  993. exit:
  994. ERROR(rt2x00dev, "Queue entries allocation failed.\n");
  995. rt2x00queue_uninitialize(rt2x00dev);
  996. return status;
  997. }
  998. void rt2x00queue_uninitialize(struct rt2x00_dev *rt2x00dev)
  999. {
  1000. struct data_queue *queue;
  1001. rt2x00queue_free_skbs(rt2x00dev->rx);
  1002. queue_for_each(rt2x00dev, queue) {
  1003. kfree(queue->entries);
  1004. queue->entries = NULL;
  1005. }
  1006. }
  1007. static void rt2x00queue_init(struct rt2x00_dev *rt2x00dev,
  1008. struct data_queue *queue, enum data_queue_qid qid)
  1009. {
  1010. mutex_init(&queue->status_lock);
  1011. spin_lock_init(&queue->tx_lock);
  1012. spin_lock_init(&queue->index_lock);
  1013. queue->rt2x00dev = rt2x00dev;
  1014. queue->qid = qid;
  1015. queue->txop = 0;
  1016. queue->aifs = 2;
  1017. queue->cw_min = 5;
  1018. queue->cw_max = 10;
  1019. }
  1020. int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
  1021. {
  1022. struct data_queue *queue;
  1023. enum data_queue_qid qid;
  1024. unsigned int req_atim =
  1025. !!test_bit(REQUIRE_ATIM_QUEUE, &rt2x00dev->cap_flags);
  1026. /*
  1027. * We need the following queues:
  1028. * RX: 1
  1029. * TX: ops->tx_queues
  1030. * Beacon: 1
  1031. * Atim: 1 (if required)
  1032. */
  1033. rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
  1034. queue = kcalloc(rt2x00dev->data_queues, sizeof(*queue), GFP_KERNEL);
  1035. if (!queue) {
  1036. ERROR(rt2x00dev, "Queue allocation failed.\n");
  1037. return -ENOMEM;
  1038. }
  1039. /*
  1040. * Initialize pointers
  1041. */
  1042. rt2x00dev->rx = queue;
  1043. rt2x00dev->tx = &queue[1];
  1044. rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
  1045. rt2x00dev->atim = req_atim ? &queue[2 + rt2x00dev->ops->tx_queues] : NULL;
  1046. /*
  1047. * Initialize queue parameters.
  1048. * RX: qid = QID_RX
  1049. * TX: qid = QID_AC_VO + index
  1050. * TX: cw_min: 2^5 = 32.
  1051. * TX: cw_max: 2^10 = 1024.
  1052. * BCN: qid = QID_BEACON
  1053. * ATIM: qid = QID_ATIM
  1054. */
  1055. rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
  1056. qid = QID_AC_VO;
  1057. tx_queue_for_each(rt2x00dev, queue)
  1058. rt2x00queue_init(rt2x00dev, queue, qid++);
  1059. rt2x00queue_init(rt2x00dev, rt2x00dev->bcn, QID_BEACON);
  1060. if (req_atim)
  1061. rt2x00queue_init(rt2x00dev, rt2x00dev->atim, QID_ATIM);
  1062. return 0;
  1063. }
  1064. void rt2x00queue_free(struct rt2x00_dev *rt2x00dev)
  1065. {
  1066. kfree(rt2x00dev->rx);
  1067. rt2x00dev->rx = NULL;
  1068. rt2x00dev->tx = NULL;
  1069. rt2x00dev->bcn = NULL;
  1070. }