utils_mlo.c 121 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667366836693670367136723673367436753676367736783679368036813682368336843685368636873688368936903691369236933694369536963697369836993700370137023703370437053706370737083709371037113712371337143715371637173718371937203721372237233724372537263727372837293730373137323733373437353736373737383739374037413742374337443745374637473748374937503751375237533754375537563757375837593760376137623763376437653766376737683769377037713772377337743775377637773778377937803781378237833784378537863787378837893790379137923793379437953796379737983799380038013802380338043805380638073808380938103811381238133814381538163817381838193820382138223823382438253826382738283829383038313832383338343835383638373838383938403841384238433844384538463847384838493850385138523853385438553856385738583859386038613862386338643865386638673868386938703871387238733874387538763877387838793880388138823883388438853886388738883889389038913892389338943895389638973898389939003901390239033904390539063907390839093910
  1. /*
  2. * Copyright (c) 2021, The Linux Foundation. All rights reserved.
  3. * Copyright (c) 2021-2022 Qualcomm Innovation Center, Inc. All rights reserved.
  4. *
  5. * Permission to use, copy, modify, and/or distribute this software for any
  6. * purpose with or without fee is hereby granted, provided that the above
  7. * copyright notice and this permission notice appear in all copies.
  8. *
  9. * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
  10. * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  11. * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
  12. * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  13. * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  14. * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  15. * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  16. */
  17. /*
  18. * DOC: contains MLO manager util api's
  19. */
  20. #include <wlan_cmn.h>
  21. #include <wlan_mlo_mgr_sta.h>
  22. #include <wlan_cm_public_struct.h>
  23. #include <wlan_mlo_mgr_main.h>
  24. #include <wlan_cm_api.h>
  25. #include "wlan_scan_api.h"
  26. #include "qdf_types.h"
  27. #include "utils_mlo.h"
  28. #include "wlan_mlo_mgr_cmn.h"
  29. #include "wlan_utility.h"
  30. #ifdef WLAN_FEATURE_11BE_MLO
  31. static uint8_t *util_find_eid(uint8_t eid, uint8_t *frame, qdf_size_t len)
  32. {
  33. if (!frame)
  34. return NULL;
  35. while (len >= MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) {
  36. if (frame[ID_POS] == eid)
  37. return frame;
  38. len -= frame[TAG_LEN_POS] + MIN_IE_LEN;
  39. frame += frame[TAG_LEN_POS] + MIN_IE_LEN;
  40. }
  41. return NULL;
  42. }
  43. static
  44. uint8_t *util_find_extn_eid(uint8_t eid, uint8_t extn_eid,
  45. uint8_t *frame, qdf_size_t len)
  46. {
  47. if (!frame)
  48. return NULL;
  49. while (len > MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) {
  50. if ((frame[ID_POS] == eid) &&
  51. (frame[ELEM_ID_EXTN_POS] == extn_eid))
  52. return frame;
  53. len -= frame[TAG_LEN_POS] + MIN_IE_LEN;
  54. frame += frame[TAG_LEN_POS] + MIN_IE_LEN;
  55. }
  56. return NULL;
  57. }
  58. static QDF_STATUS
  59. util_parse_multi_link_ctrl(uint8_t *mlieseqpayload,
  60. qdf_size_t mlieseqpayloadlen,
  61. uint8_t **link_info,
  62. qdf_size_t *link_info_len)
  63. {
  64. qdf_size_t parsed_payload_len;
  65. uint16_t mlcontrol;
  66. uint16_t presence_bm;
  67. uint16_t cinfo_len = 0;
  68. uint16_t exp_cinfo_len = 0;
  69. /* This helper returns the location(s) and length(s) of (sub)field(s)
  70. * inferable after parsing the Multi Link element Control field. These
  71. * location(s) and length(s) is/are in reference to the payload section
  72. * of the Multi Link element (after defragmentation, if applicable).
  73. * Here, the payload is the point after the element ID extension of the
  74. * Multi Link element, and includes the payloads of all subsequent
  75. * fragments (if any) but not the headers of those fragments.
  76. *
  77. * Currently, the helper returns the location and length of the Link
  78. * Info field in the Multi Link element sequence. Other (sub)field(s)
  79. * can be added later as required.
  80. */
  81. if (!mlieseqpayload) {
  82. mlo_err("ML seq payload pointer is NULL");
  83. return QDF_STATUS_E_NULL_VALUE;
  84. }
  85. if (!mlieseqpayloadlen) {
  86. mlo_err("ML seq payload len is 0");
  87. return QDF_STATUS_E_INVAL;
  88. }
  89. if (mlieseqpayloadlen < WLAN_ML_CTRL_SIZE) {
  90. mlo_err_rl("ML seq payload len %zu < ML Control size %u",
  91. mlieseqpayloadlen, WLAN_ML_CTRL_SIZE);
  92. return QDF_STATUS_E_PROTO;
  93. }
  94. parsed_payload_len = 0;
  95. qdf_mem_copy(&mlcontrol, mlieseqpayload, WLAN_ML_CTRL_SIZE);
  96. mlcontrol = qdf_le16_to_cpu(mlcontrol);
  97. parsed_payload_len += WLAN_ML_CTRL_SIZE;
  98. presence_bm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  99. WLAN_ML_CTRL_PBM_BITS);
  100. if (mlieseqpayloadlen <
  101. (parsed_payload_len + WLAN_ML_BV_CINFO_LENGTH_SIZE)) {
  102. mlo_err_rl("ML seq payload len %zu insufficient for common info length size %u after parsed payload len %zu.",
  103. mlieseqpayloadlen,
  104. WLAN_ML_BV_CINFO_LENGTH_SIZE,
  105. parsed_payload_len);
  106. return QDF_STATUS_E_PROTO;
  107. }
  108. cinfo_len = *(mlieseqpayload + parsed_payload_len);
  109. parsed_payload_len += WLAN_ML_BV_CINFO_LENGTH_SIZE;
  110. if (mlieseqpayloadlen <
  111. (parsed_payload_len + QDF_MAC_ADDR_SIZE)) {
  112. mlo_err_rl("ML seq payload len %zu insufficient for MAC address size %u after parsed payload len %zu.",
  113. mlieseqpayloadlen,
  114. QDF_MAC_ADDR_SIZE,
  115. parsed_payload_len);
  116. return QDF_STATUS_E_PROTO;
  117. }
  118. parsed_payload_len += QDF_MAC_ADDR_SIZE;
  119. /* Check if Link ID info is present */
  120. if (presence_bm & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
  121. if (mlieseqpayloadlen <
  122. (parsed_payload_len +
  123. WLAN_ML_BV_CINFO_LINKIDINFO_SIZE)) {
  124. mlo_err_rl("ML seq payload len %zu insufficient for Link ID info size %u after parsed payload len %zu.",
  125. mlieseqpayloadlen,
  126. WLAN_ML_BV_CINFO_LINKIDINFO_SIZE,
  127. parsed_payload_len);
  128. return QDF_STATUS_E_PROTO;
  129. }
  130. parsed_payload_len += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
  131. }
  132. /* Check if BSS parameter change count is present */
  133. if (presence_bm & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) {
  134. if (mlieseqpayloadlen <
  135. (parsed_payload_len +
  136. WLAN_ML_BSSPARAMCHNGCNT_SIZE)) {
  137. mlo_err_rl("ML seq payload len %zu insufficient for BSS parameter change count size %u after parsed payload len %zu.",
  138. mlieseqpayloadlen,
  139. WLAN_ML_BSSPARAMCHNGCNT_SIZE,
  140. parsed_payload_len);
  141. return QDF_STATUS_E_PROTO;
  142. }
  143. parsed_payload_len += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
  144. }
  145. /* Check if Medium Sync Delay Info is present */
  146. if (presence_bm & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) {
  147. if (mlieseqpayloadlen <
  148. (parsed_payload_len +
  149. WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE)) {
  150. mlo_err_rl("ML seq payload len %zu insufficient for Medium Sync Delay Info size %u after parsed payload len %zu.",
  151. mlieseqpayloadlen,
  152. WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE,
  153. parsed_payload_len);
  154. return QDF_STATUS_E_PROTO;
  155. }
  156. parsed_payload_len += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE;
  157. }
  158. /* Check if EML cap is present */
  159. if (presence_bm & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) {
  160. if (mlieseqpayloadlen <
  161. (parsed_payload_len +
  162. WLAN_ML_BV_CINFO_EMLCAP_SIZE)) {
  163. mlo_err_rl("ML seq payload len %zu insufficient for EML cap size %u after parsed payload len %zu.",
  164. mlieseqpayloadlen,
  165. WLAN_ML_BV_CINFO_EMLCAP_SIZE,
  166. parsed_payload_len);
  167. return QDF_STATUS_E_PROTO;
  168. }
  169. parsed_payload_len += WLAN_ML_BV_CINFO_EMLCAP_SIZE;
  170. }
  171. /* Check if MLD cap is present */
  172. if (presence_bm & WLAN_ML_BV_CTRL_PBM_MLDCAPANDOP_P) {
  173. if (mlieseqpayloadlen <
  174. (parsed_payload_len +
  175. WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE)) {
  176. mlo_err_rl("ML seq payload len %zu insufficient for MLD cap size %u after parsed payload len %zu.",
  177. mlieseqpayloadlen,
  178. WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE,
  179. parsed_payload_len);
  180. return QDF_STATUS_E_PROTO;
  181. }
  182. parsed_payload_len += WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE;
  183. }
  184. /* Check if MLD ID is present */
  185. if (presence_bm & WLAN_ML_BV_CTRL_PBM_MLDID_P) {
  186. if (mlieseqpayloadlen <
  187. (parsed_payload_len +
  188. WLAN_ML_BV_CINFO_MLDID_SIZE)) {
  189. mlo_err_rl("ML seq payload len %zu insufficient for MLD ID size %u after parsed payload len %zu.",
  190. mlieseqpayloadlen,
  191. WLAN_ML_BV_CINFO_MLDID_SIZE,
  192. parsed_payload_len);
  193. return QDF_STATUS_E_PROTO;
  194. }
  195. parsed_payload_len += WLAN_ML_BV_CINFO_MLDID_SIZE;
  196. }
  197. exp_cinfo_len = parsed_payload_len - WLAN_ML_CTRL_SIZE;
  198. if (cinfo_len != exp_cinfo_len) {
  199. mlo_err_rl("ML seq common info len %u doesn't match with expected common info len %u",
  200. cinfo_len, exp_cinfo_len);
  201. return QDF_STATUS_E_PROTO;
  202. }
  203. if (link_info_len) {
  204. *link_info_len = mlieseqpayloadlen - parsed_payload_len;
  205. mlo_debug("link_info_len:%zu, parsed_payload_len:%zu",
  206. *link_info_len, parsed_payload_len);
  207. }
  208. if (mlieseqpayloadlen == parsed_payload_len) {
  209. mlo_debug("No Link Info field present");
  210. if (link_info)
  211. *link_info = NULL;
  212. return QDF_STATUS_SUCCESS;
  213. }
  214. if (link_info)
  215. *link_info = mlieseqpayload + parsed_payload_len;
  216. return QDF_STATUS_SUCCESS;
  217. }
  218. static QDF_STATUS
  219. util_parse_prv_multi_link_ctrl(uint8_t *mlieseqpayload,
  220. qdf_size_t mlieseqpayloadlen,
  221. uint8_t **link_info,
  222. qdf_size_t *link_info_len)
  223. {
  224. qdf_size_t parsed_payload_len;
  225. uint16_t mlcontrol;
  226. uint16_t presence_bm;
  227. uint16_t cinfo_len = 0;
  228. uint16_t exp_cinfo_len = 0;
  229. /* This helper returns the location(s) and length(s) of (sub)field(s)
  230. * inferable after parsing the Multi Link element Control field. These
  231. * location(s) and length(s) is/are in reference to the payload section
  232. * of the Multi Link element (after defragmentation, if applicable).
  233. * Here, the payload is the point after the element ID extension of the
  234. * Multi Link element, and includes the payloads of all subsequent
  235. * fragments (if any) but not the headers of those fragments.
  236. *
  237. * Currently, the helper returns the location and length of the Link
  238. * Info field in the Multi Link element sequence. Other (sub)field(s)
  239. * can be added later as required.
  240. */
  241. if (!mlieseqpayload) {
  242. mlo_err("ML seq payload pointer is NULL");
  243. return QDF_STATUS_E_NULL_VALUE;
  244. }
  245. if (!mlieseqpayloadlen) {
  246. mlo_err("ML seq payload len is 0");
  247. return QDF_STATUS_E_INVAL;
  248. }
  249. if (mlieseqpayloadlen < WLAN_ML_CTRL_SIZE) {
  250. mlo_err_rl("ML seq payload len %zu < ML Control size %u",
  251. mlieseqpayloadlen, WLAN_ML_CTRL_SIZE);
  252. return QDF_STATUS_E_PROTO;
  253. }
  254. parsed_payload_len = 0;
  255. qdf_mem_copy(&mlcontrol, mlieseqpayload, WLAN_ML_CTRL_SIZE);
  256. mlcontrol = qdf_le16_to_cpu(mlcontrol);
  257. parsed_payload_len += WLAN_ML_CTRL_SIZE;
  258. presence_bm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  259. WLAN_ML_CTRL_PBM_BITS);
  260. if (mlieseqpayloadlen <
  261. (parsed_payload_len + WLAN_ML_PRV_CINFO_LENGTH_SIZE)) {
  262. mlo_err_rl("ML seq payload len %zu insufficient for common info length size %u after parsed payload len %zu.",
  263. mlieseqpayloadlen,
  264. WLAN_ML_PRV_CINFO_LENGTH_SIZE,
  265. parsed_payload_len);
  266. return QDF_STATUS_E_PROTO;
  267. }
  268. cinfo_len = *(mlieseqpayload + parsed_payload_len);
  269. parsed_payload_len += WLAN_ML_PRV_CINFO_LENGTH_SIZE;
  270. /* Check if MLD ID is present */
  271. if (presence_bm & WLAN_ML_PRV_CTRL_PBM_MLDID_P) {
  272. if (mlieseqpayloadlen <
  273. (parsed_payload_len +
  274. WLAN_ML_PRV_CINFO_MLDID_SIZE)) {
  275. mlo_err_rl("ML seq payload len %zu insufficient for MLD ID size %u after parsed payload len %zu.",
  276. mlieseqpayloadlen,
  277. WLAN_ML_PRV_CINFO_MLDID_SIZE,
  278. parsed_payload_len);
  279. return QDF_STATUS_E_PROTO;
  280. }
  281. parsed_payload_len += WLAN_ML_PRV_CINFO_MLDID_SIZE;
  282. }
  283. exp_cinfo_len = parsed_payload_len - WLAN_ML_CTRL_SIZE;
  284. if (cinfo_len != exp_cinfo_len) {
  285. mlo_err_rl("ML seq common info len %u doesn't match with expected common info len %u",
  286. cinfo_len, exp_cinfo_len);
  287. return QDF_STATUS_E_PROTO;
  288. }
  289. if (link_info_len) {
  290. *link_info_len = mlieseqpayloadlen - parsed_payload_len;
  291. mlo_debug("link_info_len:%zu, parsed_payload_len:%zu",
  292. *link_info_len, parsed_payload_len);
  293. }
  294. if (mlieseqpayloadlen == parsed_payload_len) {
  295. mlo_debug("No Link Info field present");
  296. if (link_info)
  297. *link_info = NULL;
  298. return QDF_STATUS_SUCCESS;
  299. }
  300. if (link_info)
  301. *link_info = mlieseqpayload + parsed_payload_len;
  302. return QDF_STATUS_SUCCESS;
  303. }
  304. static QDF_STATUS
  305. util_parse_bvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
  306. qdf_size_t subelempayloadlen,
  307. uint8_t *linkid,
  308. uint16_t *beaconinterval,
  309. bool *is_beaconinterval_valid,
  310. uint64_t *tsfoffset,
  311. bool *is_tsfoffset_valid,
  312. bool *is_complete_profile,
  313. bool *is_macaddr_valid,
  314. struct qdf_mac_addr *macaddr,
  315. bool is_staprof_reqd,
  316. uint8_t **staprof,
  317. qdf_size_t *staprof_len)
  318. {
  319. qdf_size_t parsed_payload_len = 0;
  320. uint16_t stacontrol;
  321. uint8_t completeprofile;
  322. uint8_t nstrlppresent;
  323. enum wlan_ml_bv_linfo_perstaprof_stactrl_nstrbmsz nstrbmsz;
  324. /* This helper returns the location(s) and where required, the length(s)
  325. * of (sub)field(s) inferable after parsing the STA Control field in the
  326. * per-STA profile subelement. These location(s) and length(s) is/are in
  327. * reference to the payload section of the per-STA profile subelement
  328. * (after defragmentation, if applicable). Here, the payload is the
  329. * point after the subelement length in the subelement, and includes the
  330. * payloads of all subsequent fragments (if any) but not the headers of
  331. * those fragments.
  332. *
  333. * Currently, the helper returns the link ID, MAC address, and STA
  334. * profile. More (sub)fields can be added when required.
  335. */
  336. if (!subelempayload) {
  337. mlo_err("Pointer to subelement payload is NULL");
  338. return QDF_STATUS_E_NULL_VALUE;
  339. }
  340. if (!subelempayloadlen) {
  341. mlo_err("Length of subelement payload is zero");
  342. return QDF_STATUS_E_INVAL;
  343. }
  344. if (subelempayloadlen < WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE) {
  345. mlo_err_rl("Subelement payload length %zu octets is smaller than STA control field of per-STA profile subelement %u octets",
  346. subelempayloadlen,
  347. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE);
  348. return QDF_STATUS_E_PROTO;
  349. }
  350. parsed_payload_len = 0;
  351. qdf_mem_copy(&stacontrol,
  352. subelempayload,
  353. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE);
  354. stacontrol = le16toh(stacontrol);
  355. parsed_payload_len += WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE;
  356. if (linkid) {
  357. *linkid = QDF_GET_BITS(stacontrol,
  358. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
  359. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS);
  360. }
  361. /* Check if this a complete profile */
  362. completeprofile = QDF_GET_BITS(stacontrol,
  363. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
  364. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS);
  365. if (completeprofile && is_complete_profile)
  366. *is_complete_profile = true;
  367. /* Check STA Info Length */
  368. if (subelempayloadlen <
  369. parsed_payload_len + WLAN_ML_BV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE) {
  370. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain STA Info Length of size %u octets after parsed payload length of %zu octets.",
  371. subelempayloadlen,
  372. WLAN_ML_BV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE,
  373. parsed_payload_len);
  374. return QDF_STATUS_E_PROTO;
  375. }
  376. parsed_payload_len += WLAN_ML_BV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE;
  377. if (is_macaddr_valid)
  378. *is_macaddr_valid = false;
  379. /* Check STA MAC address present bit */
  380. if (QDF_GET_BITS(stacontrol,
  381. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_IDX,
  382. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_BITS)) {
  383. if (subelempayloadlen <
  384. (parsed_payload_len + QDF_MAC_ADDR_SIZE)) {
  385. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain MAC address of size %u octets after parsed payload length of %zu octets.",
  386. subelempayloadlen,
  387. QDF_MAC_ADDR_SIZE,
  388. parsed_payload_len);
  389. return QDF_STATUS_E_PROTO;
  390. }
  391. if (macaddr) {
  392. qdf_mem_copy(macaddr->bytes,
  393. subelempayload + parsed_payload_len,
  394. QDF_MAC_ADDR_SIZE);
  395. mlo_nofl_debug("Copied MAC address: " QDF_MAC_ADDR_FMT,
  396. subelempayload + parsed_payload_len);
  397. if (is_macaddr_valid)
  398. *is_macaddr_valid = true;
  399. }
  400. parsed_payload_len += QDF_MAC_ADDR_SIZE;
  401. }
  402. /* Check Beacon Interval present bit */
  403. if (QDF_GET_BITS(stacontrol,
  404. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_IDX,
  405. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_BITS)) {
  406. if (subelempayloadlen <
  407. (parsed_payload_len +
  408. WLAN_BEACONINTERVAL_LEN)) {
  409. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain Beacon Interval of size %u octets after parsed payload length of %zu octets.",
  410. subelempayloadlen,
  411. WLAN_BEACONINTERVAL_LEN,
  412. parsed_payload_len);
  413. return QDF_STATUS_E_PROTO;
  414. }
  415. if (beaconinterval) {
  416. qdf_mem_copy(beaconinterval,
  417. subelempayload + parsed_payload_len,
  418. WLAN_BEACONINTERVAL_LEN);
  419. *beaconinterval = qdf_le16_to_cpu(*beaconinterval);
  420. if (is_beaconinterval_valid)
  421. *is_beaconinterval_valid = true;
  422. }
  423. parsed_payload_len += WLAN_BEACONINTERVAL_LEN;
  424. }
  425. /* Check TSF Offset present bit */
  426. if (QDF_GET_BITS(stacontrol,
  427. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_TSFOFFSETP_IDX,
  428. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_TSFOFFSETP_BITS)) {
  429. if (!completeprofile) {
  430. mlo_err_rl("TSF offset is expected only for complete profiles");
  431. return QDF_STATUS_E_PROTO;
  432. }
  433. if (subelempayloadlen <
  434. (parsed_payload_len +
  435. WLAN_ML_TSF_OFFSET_SIZE)) {
  436. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain TSF Offset of size %u octets after parsed payload length of %zu octets.",
  437. subelempayloadlen,
  438. WLAN_ML_TSF_OFFSET_SIZE,
  439. parsed_payload_len);
  440. return QDF_STATUS_E_PROTO;
  441. }
  442. if (tsfoffset) {
  443. qdf_mem_copy(tsfoffset,
  444. subelempayload + parsed_payload_len,
  445. WLAN_TIMESTAMP_LEN);
  446. *tsfoffset = qdf_le64_to_cpu(*tsfoffset);
  447. if (is_tsfoffset_valid)
  448. *is_tsfoffset_valid = true;
  449. }
  450. parsed_payload_len += WLAN_ML_TSF_OFFSET_SIZE;
  451. }
  452. /* Check DTIM Info present bit */
  453. if (QDF_GET_BITS(stacontrol,
  454. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_IDX,
  455. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_BITS)) {
  456. if (subelempayloadlen <
  457. (parsed_payload_len +
  458. sizeof(struct wlan_ml_bv_linfo_perstaprof_stainfo_dtiminfo))) {
  459. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain DTIM Info of size %zu octets after parsed payload length of %zu octets.",
  460. subelempayloadlen,
  461. sizeof(struct wlan_ml_bv_linfo_perstaprof_stainfo_dtiminfo),
  462. parsed_payload_len);
  463. return QDF_STATUS_E_PROTO;
  464. }
  465. parsed_payload_len +=
  466. sizeof(struct wlan_ml_bv_linfo_perstaprof_stainfo_dtiminfo);
  467. }
  468. /* Check NTSR Link pair present bit */
  469. nstrlppresent =
  470. QDF_GET_BITS(stacontrol,
  471. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_IDX,
  472. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_BITS);
  473. if (completeprofile && nstrlppresent) {
  474. /* Check NTSR Bitmap Size bit */
  475. nstrbmsz =
  476. QDF_GET_BITS(stacontrol,
  477. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_IDX,
  478. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_BITS);
  479. if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_1_OCTET) {
  480. if (subelempayloadlen <
  481. (parsed_payload_len + 1)) {
  482. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain NTSR Bitmap of size 1 octet after parsed payload length of %zu octets.",
  483. subelempayloadlen,
  484. parsed_payload_len);
  485. return QDF_STATUS_E_PROTO;
  486. }
  487. parsed_payload_len += 1;
  488. } else if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_2_OCTETS) {
  489. if (subelempayloadlen <
  490. (parsed_payload_len + 2)) {
  491. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain NTSR Bitmap of size 2 octets after parsed payload length of %zu octets.",
  492. subelempayloadlen,
  493. parsed_payload_len);
  494. return QDF_STATUS_E_PROTO;
  495. }
  496. parsed_payload_len += 2;
  497. } else {
  498. /* Though an invalid value cannot occur if only 1 bit is
  499. * used, we check for it in a generic manner in case the
  500. * number of bits is increased in the future.
  501. */
  502. mlo_err_rl("Invalid NSTR Bitmap size %u", nstrbmsz);
  503. return QDF_STATUS_E_PROTO;
  504. }
  505. }
  506. /* Check BSS Parameters Change Count Present bit */
  507. if (QDF_GET_BITS(stacontrol,
  508. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BSSPARAMCHNGCNTP_IDX,
  509. WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BSSPARAMCHNGCNTP_BITS)) {
  510. if (subelempayloadlen <
  511. (parsed_payload_len +
  512. WLAN_ML_BSSPARAMCHNGCNT_SIZE)) {
  513. mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain BSS Parameters Change Count of size %u octets after parsed payload length of %zu octets.",
  514. subelempayloadlen,
  515. WLAN_ML_BSSPARAMCHNGCNT_SIZE,
  516. parsed_payload_len);
  517. return QDF_STATUS_E_PROTO;
  518. }
  519. parsed_payload_len += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
  520. }
  521. /* Note: Some implementation versions of hostapd/wpa_supplicant may
  522. * provide a per-STA profile without STA profile. Let the caller
  523. * indicate whether a STA profile is required to be found. This may be
  524. * revisited as upstreaming progresses.
  525. */
  526. if (!is_staprof_reqd)
  527. return QDF_STATUS_SUCCESS;
  528. if (subelempayloadlen == parsed_payload_len) {
  529. mlo_err_rl("Subelement payload length %zu == parsed payload length %zu. Unable to get STA profile.",
  530. subelempayloadlen,
  531. parsed_payload_len);
  532. return QDF_STATUS_E_PROTO;
  533. }
  534. if (staprof_len)
  535. *staprof_len = subelempayloadlen - parsed_payload_len;
  536. if (staprof)
  537. *staprof = subelempayload + parsed_payload_len;
  538. return QDF_STATUS_SUCCESS;
  539. }
  540. static QDF_STATUS
  541. util_parse_prvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
  542. qdf_size_t subelempayloadlen,
  543. uint8_t *linkid,
  544. bool is_staprof_reqd,
  545. uint8_t **staprof,
  546. qdf_size_t *staprof_len)
  547. {
  548. qdf_size_t parsed_payload_len = 0;
  549. uint16_t stacontrol;
  550. uint8_t completeprofile;
  551. /* This helper returns the location(s) and where required, the length(s)
  552. * of (sub)field(s) inferable after parsing the STA Control field in the
  553. * per-STA profile subelement. These location(s) and length(s) is/are in
  554. * reference to the payload section of the per-STA profile subelement
  555. * (after defragmentation, if applicable). Here, the payload is the
  556. * point after the subelement length in the subelement, and includes the
  557. * payloads of all subsequent fragments (if any) but not the headers of
  558. * those fragments.
  559. *
  560. * Currently, the helper returns the link ID, MAC address, and STA
  561. * profile. More (sub)fields can be added when required.
  562. */
  563. if (!subelempayload) {
  564. mlo_err("Pointer to subelement payload is NULL");
  565. return QDF_STATUS_E_NULL_VALUE;
  566. }
  567. if (!subelempayloadlen) {
  568. mlo_err("Length of subelement payload is zero");
  569. return QDF_STATUS_E_INVAL;
  570. }
  571. if (subelempayloadlen < WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE) {
  572. mlo_err_rl("Subelement payload length %zu octets is smaller than STA control field of per-STA profile subelement %u octets",
  573. subelempayloadlen,
  574. WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE);
  575. return QDF_STATUS_E_PROTO;
  576. }
  577. parsed_payload_len = 0;
  578. qdf_mem_copy(&stacontrol,
  579. subelempayload,
  580. WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE);
  581. stacontrol = qdf_le16_to_cpu(stacontrol);
  582. parsed_payload_len += WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE;
  583. if (linkid) {
  584. *linkid = QDF_GET_BITS(stacontrol,
  585. WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
  586. WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS);
  587. }
  588. /* Check if this a complete profile */
  589. completeprofile = QDF_GET_BITS(stacontrol,
  590. WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
  591. WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS);
  592. /* Note: Some implementation versions of hostapd/wpa_supplicant may
  593. * provide a per-STA profile without STA profile. Let the caller
  594. * indicate whether a STA profile is required to be found. This may be
  595. * revisited as upstreaming progresses.
  596. */
  597. if (!is_staprof_reqd)
  598. return QDF_STATUS_SUCCESS;
  599. if (subelempayloadlen == parsed_payload_len) {
  600. mlo_err_rl("Subelement payload length %zu == parsed payload length %zu. Unable to get STA profile.",
  601. subelempayloadlen,
  602. parsed_payload_len);
  603. return QDF_STATUS_E_PROTO;
  604. }
  605. if (staprof_len)
  606. *staprof_len = subelempayloadlen - parsed_payload_len;
  607. if (staprof)
  608. *staprof = subelempayload + parsed_payload_len;
  609. return QDF_STATUS_SUCCESS;
  610. }
  611. static
  612. uint8_t *util_get_successorfrag(uint8_t *currie, uint8_t *frame, qdf_size_t len)
  613. {
  614. uint8_t *nextie;
  615. if (!currie || !frame || !len)
  616. return NULL;
  617. if ((currie + MIN_IE_LEN) > (frame + len))
  618. return NULL;
  619. /* Check whether there is sufficient space in the frame for the current
  620. * IE, plus at least another MIN_IE_LEN bytes for the IE header of a
  621. * fragment (if present) that would come just after the current IE.
  622. */
  623. if ((currie + MIN_IE_LEN + currie[TAG_LEN_POS] + MIN_IE_LEN) >
  624. (frame + len))
  625. return NULL;
  626. nextie = currie + currie[TAG_LEN_POS] + MIN_IE_LEN;
  627. /* Check whether there is sufficient space in the frame for the next IE
  628. */
  629. if ((nextie + MIN_IE_LEN + nextie[TAG_LEN_POS]) > (frame + len))
  630. return NULL;
  631. if (nextie[ID_POS] != WLAN_ELEMID_FRAGMENT)
  632. return NULL;
  633. return nextie;
  634. }
  635. static
  636. QDF_STATUS util_parse_partner_info_from_linkinfo(uint8_t *linkinfo,
  637. qdf_size_t linkinfo_len,
  638. struct mlo_partner_info *partner_info)
  639. {
  640. uint8_t linkid;
  641. struct qdf_mac_addr macaddr;
  642. bool is_macaddr_valid;
  643. uint8_t *linkinfo_currpos;
  644. qdf_size_t linkinfo_remlen;
  645. bool is_subelemfragseq;
  646. uint8_t subelemid;
  647. qdf_size_t subelemseqtotallen;
  648. qdf_size_t subelemseqpayloadlen;
  649. qdf_size_t defragpayload_len;
  650. QDF_STATUS ret;
  651. /* This helper function parses partner info from the per-STA profiles
  652. * present (if any) in the Link Info field in the payload of a Multi
  653. * Link element (after defragmentation if required). The caller should
  654. * pass a copy of the payload so that inline defragmentation of
  655. * subelements can be carried out if required. The subelement
  656. * defragmentation (if applicable) in this Control Path helper is
  657. * required for maintainability, accuracy and eliminating current and
  658. * future per-field-access multi-level fragment boundary checks and
  659. * adjustments, given the complex format of Multi Link elements. It is
  660. * also most likely to be required mainly at the client side.
  661. */
  662. if (!linkinfo) {
  663. mlo_err("linkinfo is NULL");
  664. return QDF_STATUS_E_NULL_VALUE;
  665. }
  666. if (!linkinfo_len) {
  667. mlo_err("linkinfo_len is zero");
  668. return QDF_STATUS_E_NULL_VALUE;
  669. }
  670. if (!partner_info) {
  671. mlo_err("ML partner info is NULL");
  672. return QDF_STATUS_E_NULL_VALUE;
  673. }
  674. partner_info->num_partner_links = 0;
  675. linkinfo_currpos = linkinfo;
  676. linkinfo_remlen = linkinfo_len;
  677. while (linkinfo_remlen) {
  678. if (linkinfo_remlen < sizeof(struct subelem_header)) {
  679. mlo_err_rl("Remaining length in link info %zu octets is smaller than subelement header length %zu octets",
  680. linkinfo_remlen,
  681. sizeof(struct subelem_header));
  682. return QDF_STATUS_E_PROTO;
  683. }
  684. subelemid = linkinfo_currpos[ID_POS];
  685. is_subelemfragseq = false;
  686. subelemseqtotallen = 0;
  687. subelemseqpayloadlen = 0;
  688. ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
  689. linkinfo_currpos,
  690. linkinfo_remlen,
  691. &is_subelemfragseq,
  692. &subelemseqtotallen,
  693. &subelemseqpayloadlen);
  694. if (QDF_IS_STATUS_ERROR(ret))
  695. return ret;
  696. if (is_subelemfragseq) {
  697. if (!subelemseqpayloadlen) {
  698. mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
  699. return QDF_STATUS_E_FAILURE;
  700. }
  701. mlo_debug("Subelement fragment sequence found with payload len %zu",
  702. subelemseqpayloadlen);
  703. ret = wlan_defrag_subelem_fragseq(true,
  704. WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
  705. linkinfo_currpos,
  706. linkinfo_remlen,
  707. NULL,
  708. 0,
  709. &defragpayload_len);
  710. if (QDF_IS_STATUS_ERROR(ret))
  711. return ret;
  712. if (defragpayload_len != subelemseqpayloadlen) {
  713. mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
  714. defragpayload_len,
  715. subelemseqpayloadlen);
  716. return QDF_STATUS_E_FAILURE;
  717. }
  718. /* Adjust linkinfo_remlen to reflect removal of all
  719. * subelement headers except the header of the lead
  720. * subelement.
  721. */
  722. linkinfo_remlen -= (subelemseqtotallen -
  723. subelemseqpayloadlen -
  724. sizeof(struct subelem_header));
  725. } else {
  726. if (linkinfo_remlen <
  727. (sizeof(struct subelem_header) +
  728. linkinfo_currpos[TAG_LEN_POS])) {
  729. mlo_err_rl("Remaining length in link info %zu octets is smaller than total size of current subelement %zu octets",
  730. linkinfo_remlen,
  731. sizeof(struct subelem_header) +
  732. linkinfo_currpos[TAG_LEN_POS]);
  733. return QDF_STATUS_E_PROTO;
  734. }
  735. subelemseqpayloadlen = linkinfo_currpos[TAG_LEN_POS];
  736. }
  737. if (subelemid == WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE) {
  738. is_macaddr_valid = false;
  739. ret = util_parse_bvmlie_perstaprofile_stactrl(linkinfo_currpos +
  740. sizeof(struct subelem_header),
  741. subelemseqpayloadlen,
  742. &linkid,
  743. NULL,
  744. NULL,
  745. NULL,
  746. NULL,
  747. NULL,
  748. &is_macaddr_valid,
  749. &macaddr,
  750. false,
  751. NULL,
  752. NULL);
  753. if (QDF_IS_STATUS_ERROR(ret)) {
  754. return ret;
  755. }
  756. if (is_macaddr_valid) {
  757. if (partner_info->num_partner_links >=
  758. QDF_ARRAY_SIZE(partner_info->partner_link_info)) {
  759. mlo_err_rl("Insufficient size %zu of array for partner link info",
  760. QDF_ARRAY_SIZE(partner_info->partner_link_info));
  761. return QDF_STATUS_E_NOMEM;
  762. }
  763. partner_info->partner_link_info[partner_info->num_partner_links].link_id =
  764. linkid;
  765. qdf_mem_copy(&partner_info->partner_link_info[partner_info->num_partner_links].link_addr,
  766. &macaddr,
  767. sizeof(partner_info->partner_link_info[partner_info->num_partner_links].link_addr));
  768. partner_info->num_partner_links++;
  769. } else {
  770. mlo_warn_rl("MAC address not found in STA Info field of per-STA profile with link ID %u",
  771. linkid);
  772. }
  773. }
  774. linkinfo_remlen -= (sizeof(struct subelem_header) +
  775. subelemseqpayloadlen);
  776. linkinfo_currpos += (sizeof(struct subelem_header) +
  777. subelemseqpayloadlen);
  778. }
  779. mlo_debug("Number of ML partner links found=%u",
  780. partner_info->num_partner_links);
  781. return QDF_STATUS_SUCCESS;
  782. }
  783. static QDF_STATUS
  784. util_parse_probereq_info_from_linkinfo(uint8_t *linkinfo,
  785. qdf_size_t linkinfo_len,
  786. struct mlo_probereq_info *probereq_info)
  787. {
  788. uint8_t linkid;
  789. uint8_t *linkinfo_currpos;
  790. qdf_size_t linkinfo_remlen;
  791. bool is_subelemfragseq;
  792. uint8_t subelemid;
  793. qdf_size_t subelemseqtotallen;
  794. qdf_size_t subelemseqpayloadlen;
  795. qdf_size_t defragpayload_len;
  796. QDF_STATUS ret;
  797. /* This helper function parses probe request info from the per-STA prof
  798. * present (if any) in the Link Info field in the payload of a Multi
  799. * Link element (after defragmentation if required). The caller should
  800. * pass a copy of the payload so that inline defragmentation of
  801. * subelements can be carried out if required. The subelement
  802. * defragmentation (if applicable) in this Control Path helper is
  803. * required for maintainability, accuracy and eliminating current and
  804. * future per-field-access multi-level fragment boundary checks and
  805. * adjustments, given the complex format of Multi Link elements. It is
  806. * also most likely to be required mainly at the client side.
  807. */
  808. if (!linkinfo) {
  809. mlo_err("linkinfo is NULL");
  810. return QDF_STATUS_E_NULL_VALUE;
  811. }
  812. if (!linkinfo_len) {
  813. mlo_err("linkinfo_len is zero");
  814. return QDF_STATUS_E_NULL_VALUE;
  815. }
  816. if (!probereq_info) {
  817. mlo_err("ML probe req info is NULL");
  818. return QDF_STATUS_E_NULL_VALUE;
  819. }
  820. probereq_info->num_links = 0;
  821. linkinfo_currpos = linkinfo;
  822. linkinfo_remlen = linkinfo_len;
  823. while (linkinfo_remlen) {
  824. if (linkinfo_remlen < sizeof(struct subelem_header)) {
  825. mlo_err_rl("Remaining length in link info %zu octets is smaller than subelement header length %zu octets",
  826. linkinfo_remlen,
  827. sizeof(struct subelem_header));
  828. return QDF_STATUS_E_PROTO;
  829. }
  830. subelemid = linkinfo_currpos[ID_POS];
  831. is_subelemfragseq = false;
  832. subelemseqtotallen = 0;
  833. subelemseqpayloadlen = 0;
  834. ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
  835. linkinfo_currpos,
  836. linkinfo_remlen,
  837. &is_subelemfragseq,
  838. &subelemseqtotallen,
  839. &subelemseqpayloadlen);
  840. if (QDF_IS_STATUS_ERROR(ret))
  841. return ret;
  842. if (is_subelemfragseq) {
  843. if (!subelemseqpayloadlen) {
  844. mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
  845. return QDF_STATUS_E_FAILURE;
  846. }
  847. mlo_debug("Subelement fragment sequence found with payload len %zu",
  848. subelemseqpayloadlen);
  849. ret = wlan_defrag_subelem_fragseq(true,
  850. WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
  851. linkinfo_currpos,
  852. linkinfo_remlen,
  853. NULL,
  854. 0,
  855. &defragpayload_len);
  856. if (QDF_IS_STATUS_ERROR(ret))
  857. return ret;
  858. if (defragpayload_len != subelemseqpayloadlen) {
  859. mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
  860. defragpayload_len,
  861. subelemseqpayloadlen);
  862. return QDF_STATUS_E_FAILURE;
  863. }
  864. /* Adjust linkinfo_remlen to reflect removal of all
  865. * subelement headers except the header of the lead
  866. * subelement.
  867. */
  868. linkinfo_remlen -= (subelemseqtotallen -
  869. subelemseqpayloadlen -
  870. sizeof(struct subelem_header));
  871. } else {
  872. if (linkinfo_remlen <
  873. (sizeof(struct subelem_header) +
  874. linkinfo_currpos[TAG_LEN_POS])) {
  875. mlo_err_rl("Remaining length in link info %zu octets is smaller than total size of current subelement %zu octets",
  876. linkinfo_remlen,
  877. sizeof(struct subelem_header) +
  878. linkinfo_currpos[TAG_LEN_POS]);
  879. return QDF_STATUS_E_PROTO;
  880. }
  881. subelemseqpayloadlen = linkinfo_currpos[TAG_LEN_POS];
  882. }
  883. if (subelemid == WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE) {
  884. ret = util_parse_prvmlie_perstaprofile_stactrl(linkinfo_currpos +
  885. sizeof(struct subelem_header),
  886. subelemseqpayloadlen,
  887. &linkid,
  888. false,
  889. NULL,
  890. NULL);
  891. if (QDF_IS_STATUS_ERROR(ret))
  892. return ret;
  893. if (probereq_info->num_links >=
  894. QDF_ARRAY_SIZE(probereq_info->link_id)) {
  895. mlo_err_rl("Insufficient size %zu of array for probe req link id",
  896. QDF_ARRAY_SIZE(probereq_info->link_id));
  897. return QDF_STATUS_E_NOMEM;
  898. }
  899. probereq_info->link_id[probereq_info->num_links] = linkid;
  900. probereq_info->num_links++;
  901. mlo_debug("LINK ID requested is = %u", linkid);
  902. }
  903. linkinfo_remlen -= (sizeof(struct subelem_header) +
  904. subelemseqpayloadlen);
  905. linkinfo_currpos += (sizeof(struct subelem_header) +
  906. subelemseqpayloadlen);
  907. }
  908. mlo_debug("Number of ML probe request links found=%u",
  909. probereq_info->num_links);
  910. return QDF_STATUS_SUCCESS;
  911. }
  912. static
  913. QDF_STATUS util_get_noninheritlists(uint8_t *buff, qdf_size_t buff_len,
  914. uint8_t **ninherit_elemlist,
  915. qdf_size_t *ninherit_elemlist_len,
  916. uint8_t **ninherit_elemextlist,
  917. qdf_size_t *ninherit_elemextlist_len)
  918. {
  919. uint8_t *ninherit_ie;
  920. qdf_size_t unparsed_len;
  921. /* Note: This functionality provided by this helper may be combined with
  922. * other, older non-inheritance parsing helper functionality and exposed
  923. * as a common API as part of future efforts once the older
  924. * functionality can be made generic.
  925. */
  926. if (!buff) {
  927. mlo_err("Pointer to buffer for IEs is NULL");
  928. return QDF_STATUS_E_NULL_VALUE;
  929. }
  930. if (!buff_len) {
  931. mlo_err("IE buffer length is zero");
  932. return QDF_STATUS_E_INVAL;
  933. }
  934. if (!ninherit_elemlist) {
  935. mlo_err("Pointer to Non-Inheritance element ID list array is NULL");
  936. return QDF_STATUS_E_NULL_VALUE;
  937. }
  938. if (!ninherit_elemlist_len) {
  939. mlo_err("Pointer to Non-Inheritance element ID list array length is NULL");
  940. return QDF_STATUS_E_NULL_VALUE;
  941. }
  942. if (!ninherit_elemextlist) {
  943. mlo_err("Pointer to Non-Inheritance element ID extension list array is NULL");
  944. return QDF_STATUS_E_NULL_VALUE;
  945. }
  946. if (!ninherit_elemextlist_len) {
  947. mlo_err("Pointer to Non-Inheritance element ID extension list array length is NULL");
  948. return QDF_STATUS_E_NULL_VALUE;
  949. }
  950. ninherit_ie = NULL;
  951. *ninherit_elemlist_len = 0;
  952. *ninherit_elemlist = NULL;
  953. *ninherit_elemextlist_len = 0;
  954. *ninherit_elemextlist = NULL;
  955. ninherit_ie =
  956. (uint8_t *)util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM,
  957. WLAN_EXTN_ELEMID_NONINHERITANCE,
  958. buff,
  959. buff_len);
  960. if (ninherit_ie) {
  961. if ((ninherit_ie + TAG_LEN_POS) > (buff + buff_len - 1)) {
  962. mlo_err_rl("Position of length field of Non-Inheritance element would exceed IE buffer boundary");
  963. return QDF_STATUS_E_PROTO;
  964. }
  965. if ((ninherit_ie + ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN) >
  966. (buff + buff_len)) {
  967. mlo_err_rl("Non-Inheritance element with total length %u would exceed IE buffer boundary",
  968. ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN);
  969. return QDF_STATUS_E_PROTO;
  970. }
  971. if ((ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN) <
  972. MIN_NONINHERITANCEELEM_LEN) {
  973. mlo_err_rl("Non-Inheritance element size %u is smaller than the minimum required %u",
  974. ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN,
  975. MIN_NONINHERITANCEELEM_LEN);
  976. return QDF_STATUS_E_PROTO;
  977. }
  978. /* Track the number of unparsed octets, excluding the IE header.
  979. */
  980. unparsed_len = ninherit_ie[TAG_LEN_POS];
  981. /* Mark the element ID extension as parsed */
  982. unparsed_len--;
  983. *ninherit_elemlist_len = ninherit_ie[ELEM_ID_LIST_LEN_POS];
  984. unparsed_len--;
  985. /* While checking if the Non-Inheritance element ID list length
  986. * exceeds the remaining unparsed IE space, we factor in one
  987. * octet for the element extension ID list length and subtract
  988. * this from the unparsed IE space.
  989. */
  990. if (*ninherit_elemlist_len > (unparsed_len - 1)) {
  991. mlo_err_rl("Non-Inheritance element ID list length %zu exceeds remaining unparsed IE space, minus an octet for element extension ID list length %zu",
  992. *ninherit_elemlist_len, unparsed_len - 1);
  993. return QDF_STATUS_E_PROTO;
  994. }
  995. if (*ninherit_elemlist_len != 0) {
  996. *ninherit_elemlist = ninherit_ie + ELEM_ID_LIST_POS;
  997. unparsed_len -= *ninherit_elemlist_len;
  998. }
  999. *ninherit_elemextlist_len =
  1000. ninherit_ie[ELEM_ID_LIST_LEN_POS + *ninherit_elemlist_len + 1];
  1001. unparsed_len--;
  1002. if (*ninherit_elemextlist_len > unparsed_len) {
  1003. mlo_err_rl("Non-Inheritance element ID extension list length %zu exceeds remaining unparsed IE space %zu",
  1004. *ninherit_elemextlist_len, unparsed_len);
  1005. return QDF_STATUS_E_PROTO;
  1006. }
  1007. if (*ninherit_elemextlist_len != 0) {
  1008. *ninherit_elemextlist = ninherit_ie +
  1009. ELEM_ID_LIST_LEN_POS + (*ninherit_elemlist_len)
  1010. + 2;
  1011. unparsed_len -= *ninherit_elemextlist_len;
  1012. }
  1013. if (unparsed_len > 0) {
  1014. mlo_err_rl("Unparsed length is %zu, expected 0",
  1015. unparsed_len);
  1016. return QDF_STATUS_E_PROTO;
  1017. }
  1018. }
  1019. /* If Non-Inheritance element is not found, we still return success,
  1020. * with the list lengths kept at zero.
  1021. */
  1022. mlo_debug("Non-Inheritance element ID list array length=%zu",
  1023. *ninherit_elemlist_len);
  1024. mlo_debug("Non-Inheritance element ID extension list array length=%zu",
  1025. *ninherit_elemextlist_len);
  1026. return QDF_STATUS_SUCCESS;
  1027. }
  1028. static
  1029. QDF_STATUS util_eval_ie_in_noninheritlist(uint8_t *ie, qdf_size_t total_ie_len,
  1030. uint8_t *ninherit_elemlist,
  1031. qdf_size_t ninherit_elemlist_len,
  1032. uint8_t *ninherit_elemextlist,
  1033. qdf_size_t ninherit_elemextlist_len,
  1034. bool *is_in_noninheritlist)
  1035. {
  1036. int i;
  1037. /* Evaluate whether the given IE is in the given Non-Inheritance element
  1038. * ID list or Non-Inheritance element ID extension list, and update the
  1039. * result into is_in_noninheritlist. If any list is empty, then the IE
  1040. * is considered to not be present in that list. Both lists can be
  1041. * empty.
  1042. *
  1043. * If QDF_STATUS_SUCCESS is returned, it means that the evaluation is
  1044. * successful, and that is_in_noninheritlist contains a valid value
  1045. * (which could be true or false). If a QDF_STATUS error value is
  1046. * returned, the value in is_in_noninheritlist is invalid and the caller
  1047. * should ignore it.
  1048. */
  1049. /* Note: The functionality provided by this helper may be combined with
  1050. * other, older non-inheritance parsing helper functionality and exposed
  1051. * as a common API as part of future efforts once the older
  1052. * functionality can be made generic.
  1053. */
  1054. /* Except for is_in_noninheritlist and ie, other pointer arguments are
  1055. * permitted to be NULL if they are inapplicable. If they are
  1056. * applicable, they will be checked to ensure they are not NULL.
  1057. */
  1058. if (!is_in_noninheritlist) {
  1059. mlo_err("NULL pointer to flag that indicates if element is in a Non-Inheritance list");
  1060. return QDF_STATUS_E_NULL_VALUE;
  1061. }
  1062. /* If ninherit_elemlist_len and ninherit_elemextlist_len are both zero
  1063. * as checked soon in this function, we won't be accessing the IE.
  1064. * However, we still check right-away if the pointer to the IE is
  1065. * non-NULL and whether the total IE length is sane enough to access the
  1066. * element ID and if applicable, the element ID extension, since it
  1067. * doesn't make sense to set the flag in is_in_noninheritlist for a NULL
  1068. * IE pointer or an IE whose total length is not sane enough to
  1069. * distinguish the identity of the IE.
  1070. */
  1071. if (!ie) {
  1072. mlo_err("NULL pointer to IE");
  1073. return QDF_STATUS_E_NULL_VALUE;
  1074. }
  1075. if (total_ie_len < (ID_POS + 1)) {
  1076. mlo_err("Total IE length %zu is smaller than minimum required to access element ID %u",
  1077. total_ie_len, ID_POS + 1);
  1078. return QDF_STATUS_E_INVAL;
  1079. }
  1080. if ((ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
  1081. (total_ie_len < (IDEXT_POS + 1))) {
  1082. mlo_err("Total IE length %zu is smaller than minimum required to access element ID extension %u",
  1083. total_ie_len, IDEXT_POS + 1);
  1084. return QDF_STATUS_E_INVAL;
  1085. }
  1086. *is_in_noninheritlist = false;
  1087. /* If both the Non-Inheritance element list and Non-Inheritance element
  1088. * ID extension list are empty, then return success since we can
  1089. * conclude immediately that the given element does not occur in any
  1090. * Non-Inheritance list. The is_in_noninheritlist remains set to false
  1091. * as required.
  1092. */
  1093. if (!ninherit_elemlist_len && !ninherit_elemextlist_len)
  1094. return QDF_STATUS_SUCCESS;
  1095. if (ie[ID_POS] != WLAN_ELEMID_EXTN_ELEM) {
  1096. if (!ninherit_elemlist_len)
  1097. return QDF_STATUS_SUCCESS;
  1098. if (!ninherit_elemlist) {
  1099. mlo_err("NULL pointer to Non-Inheritance element ID list though length of element ID list is %zu",
  1100. ninherit_elemlist_len);
  1101. return QDF_STATUS_E_NULL_VALUE;
  1102. }
  1103. for (i = 0; i < ninherit_elemlist_len; i++) {
  1104. if (ie[ID_POS] == ninherit_elemlist[i]) {
  1105. *is_in_noninheritlist = true;
  1106. return QDF_STATUS_SUCCESS;
  1107. }
  1108. }
  1109. } else {
  1110. if (!ninherit_elemextlist_len)
  1111. return QDF_STATUS_SUCCESS;
  1112. if (!ninherit_elemextlist) {
  1113. mlo_err("NULL pointer to Non-Inheritance element ID extension list though length of element ID extension list is %zu",
  1114. ninherit_elemextlist_len);
  1115. return QDF_STATUS_E_NULL_VALUE;
  1116. }
  1117. for (i = 0; i < ninherit_elemextlist_len; i++) {
  1118. if (ie[IDEXT_POS] == ninherit_elemextlist[i]) {
  1119. *is_in_noninheritlist = true;
  1120. return QDF_STATUS_SUCCESS;
  1121. }
  1122. }
  1123. }
  1124. return QDF_STATUS_SUCCESS;
  1125. }
  1126. static inline
  1127. QDF_STATUS util_validate_reportingsta_ie(const uint8_t *reportingsta_ie,
  1128. const uint8_t *frame_iesection,
  1129. const qdf_size_t frame_iesection_len)
  1130. {
  1131. qdf_size_t reportingsta_ie_size;
  1132. if (!reportingsta_ie) {
  1133. mlo_err("Pointer to reporting STA IE is NULL");
  1134. return QDF_STATUS_E_NULL_VALUE;
  1135. }
  1136. if (!frame_iesection) {
  1137. mlo_err("Pointer to start of IE section in reporting frame is NULL");
  1138. return QDF_STATUS_E_NULL_VALUE;
  1139. }
  1140. if (!frame_iesection_len) {
  1141. mlo_err("Length of IE section in reporting frame is zero");
  1142. return QDF_STATUS_E_INVAL;
  1143. }
  1144. if ((reportingsta_ie + ID_POS) > (frame_iesection +
  1145. frame_iesection_len - 1)) {
  1146. mlo_err_rl("Position of element ID field of element for reporting STA would exceed frame IE section boundary");
  1147. return QDF_STATUS_E_PROTO;
  1148. }
  1149. if ((reportingsta_ie + TAG_LEN_POS) > (frame_iesection +
  1150. frame_iesection_len - 1)) {
  1151. mlo_err_rl("Position of length field of element with element ID %u for reporting STA would exceed frame IE section boundary",
  1152. reportingsta_ie[ID_POS]);
  1153. return QDF_STATUS_E_PROTO;
  1154. }
  1155. if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
  1156. ((reportingsta_ie + IDEXT_POS) > (frame_iesection +
  1157. frame_iesection_len - 1))) {
  1158. mlo_err_rl("Position of element ID extension field of element would exceed frame IE section boundary");
  1159. return QDF_STATUS_E_PROTO;
  1160. }
  1161. reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] + MIN_IE_LEN;
  1162. if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
  1163. (reportingsta_ie_size < (IDEXT_POS + 1))) {
  1164. mlo_err_rl("Total length %zu of element for reporting STA is smaller than minimum required to access element ID extension %u",
  1165. reportingsta_ie_size, IDEXT_POS + 1);
  1166. return QDF_STATUS_E_PROTO;
  1167. }
  1168. if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_VENDOR) &&
  1169. (reportingsta_ie_size < (PAYLOAD_START_POS + OUI_LEN))) {
  1170. mlo_err_rl("Total length %zu of element for reporting STA is smaller than minimum required to access vendor EID %u",
  1171. reportingsta_ie_size, PAYLOAD_START_POS + OUI_LEN);
  1172. return QDF_STATUS_E_PROTO;
  1173. }
  1174. if ((reportingsta_ie + reportingsta_ie_size) >
  1175. (frame_iesection + frame_iesection_len)) {
  1176. if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
  1177. mlo_err_rl("Total size %zu octets of element with element ID %u element ID extension %u for reporting STA would exceed frame IE section boundary",
  1178. reportingsta_ie_size,
  1179. reportingsta_ie[ID_POS],
  1180. reportingsta_ie[IDEXT_POS]);
  1181. } else {
  1182. mlo_err_rl("Total size %zu octets of element with element ID %u for reporting STA would exceed frame IE section boundary",
  1183. reportingsta_ie_size,
  1184. reportingsta_ie[ID_POS]);
  1185. }
  1186. return QDF_STATUS_E_PROTO;
  1187. }
  1188. return QDF_STATUS_SUCCESS;
  1189. }
  1190. static inline
  1191. QDF_STATUS util_validate_sta_prof_ie(const uint8_t *sta_prof_ie,
  1192. const uint8_t *sta_prof_iesection,
  1193. const qdf_size_t sta_prof_iesection_len)
  1194. {
  1195. qdf_size_t sta_prof_ie_size;
  1196. if (!sta_prof_ie) {
  1197. mlo_err("Pointer to STA profile IE is NULL");
  1198. return QDF_STATUS_E_NULL_VALUE;
  1199. }
  1200. if (!sta_prof_iesection) {
  1201. mlo_err("Pointer to start of IE section in STA profile is NULL");
  1202. return QDF_STATUS_E_NULL_VALUE;
  1203. }
  1204. if (!sta_prof_iesection_len) {
  1205. mlo_err("Length of IE section in STA profile is zero");
  1206. return QDF_STATUS_E_INVAL;
  1207. }
  1208. if ((sta_prof_ie + ID_POS) > (sta_prof_iesection +
  1209. sta_prof_iesection_len - 1)) {
  1210. mlo_err_rl("Position of element ID field of STA profile element would exceed STA profile IE section boundary");
  1211. return QDF_STATUS_E_PROTO;
  1212. }
  1213. if ((sta_prof_ie + TAG_LEN_POS) > (sta_prof_iesection +
  1214. sta_prof_iesection_len - 1)) {
  1215. mlo_err_rl("Position of length field of element with element ID %u in STA profile would exceed STA profile IE section boundary",
  1216. sta_prof_ie[ID_POS]);
  1217. return QDF_STATUS_E_PROTO;
  1218. }
  1219. if ((sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
  1220. ((sta_prof_ie + IDEXT_POS) > (sta_prof_iesection +
  1221. sta_prof_iesection_len - 1))) {
  1222. mlo_err_rl("Position of element ID extension field of element would exceed STA profile IE section boundary");
  1223. return QDF_STATUS_E_PROTO;
  1224. }
  1225. sta_prof_ie_size = sta_prof_ie[TAG_LEN_POS] + MIN_IE_LEN;
  1226. if ((sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
  1227. (sta_prof_ie_size < (IDEXT_POS + 1))) {
  1228. mlo_err_rl("Total length %zu of STA profile element is smaller than minimum required to access element ID extension %u",
  1229. sta_prof_ie_size, IDEXT_POS + 1);
  1230. return QDF_STATUS_E_PROTO;
  1231. }
  1232. if ((sta_prof_ie + sta_prof_ie_size) >
  1233. (sta_prof_iesection + sta_prof_iesection_len)) {
  1234. if (sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
  1235. mlo_err_rl("Total size %zu octets of element with element ID %u element ID extension %u in STA profile would exceed STA profile IE section boundary",
  1236. sta_prof_ie_size,
  1237. sta_prof_ie[ID_POS],
  1238. sta_prof_ie[IDEXT_POS]);
  1239. } else {
  1240. mlo_err_rl("Total size %zu octets of element with element ID %u in STA profile would exceed STA profile IE section boundary",
  1241. sta_prof_ie_size,
  1242. sta_prof_ie[ID_POS]);
  1243. }
  1244. return QDF_STATUS_E_PROTO;
  1245. }
  1246. return QDF_STATUS_SUCCESS;
  1247. }
  1248. #ifdef CONN_MGR_ADV_FEATURE
  1249. /**
  1250. * util_add_mlie_for_prb_rsp_gen - Add the basic variant Multi-Link element
  1251. * when generating link specific probe response.
  1252. * @reportingsta_ie: Pointer to the reportingsta ie
  1253. * @reportingsta_ie_len: Length for reporting sta ie
  1254. * @plink_frame_currpos: Pointer to Link frame current pos
  1255. * @plink_frame_currlen: Current length of link frame.
  1256. * @linkid: Link Id value
  1257. *
  1258. * Add the basic variant Multi-Link element when
  1259. * generating link specific probe response.
  1260. *
  1261. * Return: QDF_STATUS_SUCCESS in the case of success, QDF_STATUS value giving
  1262. * the reason for error in the case of failure
  1263. */
  1264. static QDF_STATUS
  1265. util_add_mlie_for_prb_rsp_gen(const uint8_t *reportingsta_ie,
  1266. qdf_size_t reportingsta_ie_len,
  1267. uint8_t **plink_frame_currpos,
  1268. qdf_size_t *plink_frame_currlen,
  1269. uint8_t linkid)
  1270. {
  1271. uint8_t mlie_len = 0;
  1272. uint8_t common_info_len = 0;
  1273. struct wlan_ie_multilink ml_ie_ff;
  1274. uint16_t mlcontrol;
  1275. uint16_t presencebm;
  1276. uint8_t *mlie_frame = NULL;
  1277. uint8_t link_id_offset = sizeof(struct wlan_ie_multilink) +
  1278. QDF_MAC_ADDR_SIZE +
  1279. WLAN_ML_BV_CINFO_LENGTH_SIZE;
  1280. uint8_t *link_frame_currpos = *plink_frame_currpos;
  1281. qdf_size_t link_frame_currlen = *plink_frame_currlen;
  1282. QDF_STATUS status = QDF_STATUS_SUCCESS;
  1283. status = util_get_mlie_common_info_len((uint8_t *)reportingsta_ie,
  1284. reportingsta_ie_len,
  1285. &common_info_len);
  1286. if (QDF_IS_STATUS_ERROR(status)) {
  1287. mlo_err("Failed while parsing the common info length");
  1288. return status;
  1289. }
  1290. /* common info len + bvmlie fixed fields */
  1291. mlie_len = common_info_len + sizeof(struct wlan_ie_multilink);
  1292. mlo_debug_rl("mlie_len %d, common_info_len %d, link_id_offset %d",
  1293. mlie_len,
  1294. common_info_len,
  1295. link_id_offset);
  1296. mlie_frame = qdf_mem_malloc(mlie_len);
  1297. if (!mlie_frame)
  1298. return QDF_STATUS_E_NOMEM;
  1299. /* Copy ml ie fixed fields */
  1300. qdf_mem_copy(&ml_ie_ff,
  1301. reportingsta_ie,
  1302. sizeof(struct wlan_ie_multilink));
  1303. ml_ie_ff.elem_len = mlie_len - sizeof(struct ie_header);
  1304. mlcontrol = qdf_le16_to_cpu(ml_ie_ff.mlcontrol);
  1305. presencebm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  1306. WLAN_ML_CTRL_PBM_BITS);
  1307. qdf_set_bit(WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P,
  1308. (unsigned long *)&presencebm);
  1309. QDF_SET_BITS(ml_ie_ff.mlcontrol,
  1310. WLAN_ML_CTRL_PBM_IDX,
  1311. WLAN_ML_CTRL_PBM_BITS,
  1312. presencebm);
  1313. qdf_mem_copy(mlie_frame,
  1314. &ml_ie_ff,
  1315. sizeof(struct wlan_ie_multilink));
  1316. qdf_mem_copy(mlie_frame + sizeof(struct wlan_ie_multilink),
  1317. reportingsta_ie + sizeof(struct wlan_ie_multilink),
  1318. mlie_len - sizeof(struct wlan_ie_multilink));
  1319. if (linkid == 0xFF) {
  1320. qdf_mem_free(mlie_frame);
  1321. mlo_err("Link id is invalid");
  1322. return QDF_STATUS_E_INVAL;
  1323. }
  1324. mlie_frame[link_id_offset] = (mlie_frame[link_id_offset] & ~0x0f) |
  1325. (linkid & 0x0f);
  1326. qdf_mem_copy(link_frame_currpos,
  1327. mlie_frame,
  1328. mlie_len);
  1329. mlo_debug("Add mlie for link id %d", linkid);
  1330. QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_PE, QDF_TRACE_LEVEL_DEBUG,
  1331. mlie_frame, mlie_len);
  1332. link_frame_currpos += mlie_len;
  1333. link_frame_currlen += mlie_len;
  1334. *plink_frame_currpos = link_frame_currpos;
  1335. *plink_frame_currlen = link_frame_currlen;
  1336. qdf_mem_free(mlie_frame);
  1337. return QDF_STATUS_SUCCESS;
  1338. }
  1339. #else
  1340. static QDF_STATUS
  1341. util_add_mlie_for_prb_rsp_gen(const uint8_t *reportingsta_ie,
  1342. qdf_size_t reportingsta_ie_len,
  1343. uint8_t **plink_frame_currpos,
  1344. qdf_size_t *plink_frame_currlen,
  1345. uint8_t linkid)
  1346. {
  1347. return QDF_STATUS_SUCCESS;
  1348. }
  1349. #endif
  1350. #define MLO_LINKSPECIFIC_ASSOC_REQ_FC0 0x00
  1351. #define MLO_LINKSPECIFIC_ASSOC_REQ_FC1 0x00
  1352. #define MLO_LINKSPECIFIC_ASSOC_RESP_FC0 0x10
  1353. #define MLO_LINKSPECIFIC_ASSOC_RESP_FC1 0x00
  1354. #define MLO_LINKSPECIFIC_PROBE_RESP_FC0 0x50
  1355. #define MLO_LINKSPECIFIC_PROBE_RESP_FC1 0x00
  1356. static
  1357. QDF_STATUS util_gen_link_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
  1358. uint8_t subtype,
  1359. struct qdf_mac_addr link_addr,
  1360. uint8_t *link_frame,
  1361. qdf_size_t link_frame_maxsize,
  1362. qdf_size_t *link_frame_len)
  1363. {
  1364. /* Please see documentation for util_gen_link_assoc_req() and
  1365. * util_gen_link_assoc_resp() for information on the inputs to and
  1366. * output from this helper, since those APIs are essentially wrappers
  1367. * over this helper.
  1368. */
  1369. /* Pointer to Multi-Link element/Multi-Link element fragment sequence */
  1370. uint8_t *mlieseq;
  1371. /* Total length of Multi-Link element sequence (including fragments if
  1372. * any)
  1373. */
  1374. qdf_size_t mlieseqlen;
  1375. /* Variant (i.e. type) of the Multi-Link element */
  1376. enum wlan_ml_variant variant;
  1377. /* Length of the payload of the Multi-Link element (inclusive of
  1378. * fragment payloads if any) without IE headers and element ID extension
  1379. */
  1380. qdf_size_t mlieseqpayloadlen;
  1381. /* Pointer to copy of the payload of the Multi-Link element (inclusive
  1382. * of fragment payloads if any) without IE headers and element ID
  1383. * extension
  1384. */
  1385. uint8_t *mlieseqpayload_copy;
  1386. /* Pointer to start of Link Info within the copy of the payload of the
  1387. * Multi-Link element
  1388. */
  1389. uint8_t *link_info;
  1390. /* Length of the Link Info */
  1391. qdf_size_t link_info_len;
  1392. /* Pointer to the IE section that occurs after the fixed fields in the
  1393. * original frame for the reporting STA.
  1394. */
  1395. uint8_t *frame_iesection;
  1396. /* Offset to the start of the IE section in the original frame for the
  1397. * reporting STA.
  1398. */
  1399. qdf_size_t frame_iesection_offset;
  1400. /* Total length of the IE section in the original frame for the
  1401. * reporting STA.
  1402. */
  1403. qdf_size_t frame_iesection_len;
  1404. /* Pointer to the IEEE802.11 frame header in the link specific frame
  1405. * being generated for the reported STA.
  1406. */
  1407. struct wlan_frame_hdr *link_frame_hdr;
  1408. /* Current position in the link specific frame being generated for the
  1409. * reported STA.
  1410. */
  1411. uint8_t *link_frame_currpos;
  1412. /* Current length of the link specific frame being generated for the
  1413. * reported STA.
  1414. */
  1415. qdf_size_t link_frame_currlen;
  1416. /* Pointer to IE for reporting STA */
  1417. const uint8_t *reportingsta_ie;
  1418. /* Total size of IE for reporting STA, inclusive of the element header
  1419. */
  1420. qdf_size_t reportingsta_ie_size;
  1421. /* Pointer to current position in STA profile */
  1422. uint8_t *sta_prof_currpos;
  1423. /* Remaining length of STA profile */
  1424. qdf_size_t sta_prof_remlen;
  1425. /* Pointer to start of IE section in STA profile that occurs after fixed
  1426. * fields.
  1427. */
  1428. uint8_t *sta_prof_iesection;
  1429. /* Total length of IE section in STA profile */
  1430. qdf_size_t sta_prof_iesection_len;
  1431. /* Pointer to current position being processed in IE section in STA
  1432. * profile.
  1433. */
  1434. uint8_t *sta_prof_iesection_currpos;
  1435. /* Remaining length of IE section in STA profile */
  1436. qdf_size_t sta_prof_iesection_remlen;
  1437. /* Pointer to IE in STA profile, that occurs within IE section */
  1438. uint8_t *sta_prof_ie;
  1439. /* Total size of IE in STA profile, inclusive of the element header */
  1440. qdf_size_t sta_prof_ie_size;
  1441. /* Pointer to element ID list in Non-Inheritance IE */
  1442. uint8_t *ninherit_elemlist;
  1443. /* Length of element ID list in Non-Inheritance IE */
  1444. qdf_size_t ninherit_elemlist_len;
  1445. /* Pointer to element ID extension list in Non-Inheritance IE */
  1446. uint8_t *ninherit_elemextlist;
  1447. /* Length of element ID extension list in Non-Inheritance IE */
  1448. qdf_size_t ninherit_elemextlist_len;
  1449. /* Whether a given IE is in a non-inheritance list */
  1450. bool is_in_noninheritlist;
  1451. /* Whether MAC address of reported STA is valid */
  1452. bool is_reportedmacaddr_valid;
  1453. /* MAC address of reported STA */
  1454. struct qdf_mac_addr reportedmacaddr;
  1455. /* Pointer to per-STA profile */
  1456. uint8_t *persta_prof;
  1457. /* Length of the containing buffer which starts with the per-STA profile
  1458. */
  1459. qdf_size_t persta_prof_bufflen;
  1460. /* Other variables for temporary purposes */
  1461. /* Variable into which API for determining fragment information will
  1462. * indicate whether the element is the start of a fragment sequence or
  1463. * not.
  1464. */
  1465. bool is_elemfragseq;
  1466. /* De-fragmented payload length returned by API for element
  1467. * defragmentation.
  1468. */
  1469. qdf_size_t defragpayload_len;
  1470. /* Variable into which API for determining fragment information will
  1471. * indicate whether the subelement is the start of a fragment sequence
  1472. * or not.
  1473. */
  1474. bool is_subelemfragseq;
  1475. /* Total length of the subelement fragment sequence, inclusive of
  1476. * subelement header and the headers of fragments if any.
  1477. */
  1478. qdf_size_t subelemseqtotallen;
  1479. /* Total length of the subelement fragment sequence payload, excluding
  1480. * subelement header and fragment headers if any.
  1481. */
  1482. qdf_size_t subelemseqpayloadlen;
  1483. /* Pointer to Beacon interval in STA info field */
  1484. uint16_t beaconinterval;
  1485. /* Whether Beacon interval value valid */
  1486. bool is_beaconinterval_valid;
  1487. /* TSF timer of the reporting AP */
  1488. uint64_t tsf;
  1489. /* TSF offset of the reproted AP */
  1490. uint64_t tsfoffset;
  1491. /* TSF offset value valid */
  1492. bool is_tsfoffset_valid;
  1493. /* If Complete Profile or not*/
  1494. bool is_completeprofile;
  1495. qdf_size_t tmplen;
  1496. QDF_STATUS ret;
  1497. uint8_t linkid = 0xFF;
  1498. if (!frame) {
  1499. mlo_err("Pointer to original frame is NULL");
  1500. return QDF_STATUS_E_NULL_VALUE;
  1501. }
  1502. if (!frame_len) {
  1503. mlo_err("Length of original frame is zero");
  1504. return QDF_STATUS_E_INVAL;
  1505. }
  1506. if ((subtype != WLAN_FC0_STYPE_ASSOC_REQ) &&
  1507. (subtype != WLAN_FC0_STYPE_REASSOC_REQ) &&
  1508. (subtype != WLAN_FC0_STYPE_ASSOC_RESP) &&
  1509. (subtype != WLAN_FC0_STYPE_REASSOC_RESP) &&
  1510. (subtype != WLAN_FC0_STYPE_PROBE_RESP)) {
  1511. mlo_err("802.11 frame subtype %u is invalid", subtype);
  1512. return QDF_STATUS_E_INVAL;
  1513. }
  1514. if (!link_frame) {
  1515. mlo_err("Pointer to secondary link specific frame is NULL");
  1516. return QDF_STATUS_E_NULL_VALUE;
  1517. }
  1518. if (!link_frame_maxsize) {
  1519. mlo_err("Maximum size of secondary link specific frame is zero");
  1520. return QDF_STATUS_E_INVAL;
  1521. }
  1522. if (!link_frame_len) {
  1523. mlo_err("Pointer to populated length of secondary link specific frame is NULL");
  1524. return QDF_STATUS_E_NULL_VALUE;
  1525. }
  1526. frame_iesection_offset = 0;
  1527. if (subtype == WLAN_FC0_STYPE_ASSOC_REQ) {
  1528. frame_iesection_offset = WLAN_ASSOC_REQ_IES_OFFSET;
  1529. } else if (subtype == WLAN_FC0_STYPE_REASSOC_REQ) {
  1530. frame_iesection_offset = WLAN_REASSOC_REQ_IES_OFFSET;
  1531. } else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
  1532. frame_iesection_offset = WLAN_PROBE_RESP_IES_OFFSET;
  1533. qdf_mem_copy(&tsf, frame, WLAN_TIMESTAMP_LEN);
  1534. tsf = qdf_le64_to_cpu(tsf);
  1535. } else {
  1536. /* This is a (re)association response */
  1537. frame_iesection_offset = WLAN_ASSOC_RSP_IES_OFFSET;
  1538. }
  1539. if (frame_len < frame_iesection_offset) {
  1540. /* The caller is supposed to have confirmed that this is a valid
  1541. * frame containing a Multi-Link element. Hence we treat this as
  1542. * a case of invalid argument being passed to us.
  1543. */
  1544. mlo_err("Frame length %zu is smaller than the IE section offset %zu for subtype %u",
  1545. frame_len, frame_iesection_offset, subtype);
  1546. return QDF_STATUS_E_INVAL;
  1547. }
  1548. frame_iesection_len = frame_len - frame_iesection_offset;
  1549. if (frame_iesection_len == 0) {
  1550. /* The caller is supposed to have confirmed that this is a valid
  1551. * frame containing a Multi-Link element. Hence we treat this as
  1552. * a case of invalid argument being passed to us.
  1553. */
  1554. mlo_err("No space left in frame for IE section");
  1555. return QDF_STATUS_E_INVAL;
  1556. }
  1557. frame_iesection = frame + frame_iesection_offset;
  1558. mlieseq = NULL;
  1559. mlieseqlen = 0;
  1560. ret = util_find_mlie(frame_iesection, frame_iesection_len, &mlieseq,
  1561. &mlieseqlen);
  1562. if (QDF_IS_STATUS_ERROR(ret))
  1563. return ret;
  1564. if (!mlieseq) {
  1565. /* The caller is supposed to have confirmed that a Multi-Link
  1566. * element is present in the frame. Hence we treat this as a
  1567. * case of invalid argument being passed to us.
  1568. */
  1569. mlo_err("Invalid original frame since no Multi-Link element found");
  1570. return QDF_STATUS_E_INVAL;
  1571. }
  1572. /* Sanity check the Multi-Link element sequence length */
  1573. if (!mlieseqlen) {
  1574. mlo_err("Length of Multi-Link element sequence is zero. Investigate.");
  1575. return QDF_STATUS_E_FAILURE;
  1576. }
  1577. if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
  1578. mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
  1579. mlieseqlen, sizeof(struct wlan_ie_multilink));
  1580. return QDF_STATUS_E_PROTO;
  1581. }
  1582. ret = util_get_mlie_variant(mlieseq, mlieseqlen, (int *)&variant);
  1583. if (QDF_IS_STATUS_ERROR(ret))
  1584. return ret;
  1585. if (variant != WLAN_ML_VARIANT_BASIC) {
  1586. mlo_err_rl("Unexpected variant %u of Multi-Link element.",
  1587. variant);
  1588. return QDF_STATUS_E_PROTO;
  1589. }
  1590. mlieseqpayloadlen = 0;
  1591. tmplen = 0;
  1592. is_elemfragseq = false;
  1593. ret = wlan_get_elem_fragseq_info(mlieseq,
  1594. mlieseqlen,
  1595. &is_elemfragseq,
  1596. &tmplen,
  1597. &mlieseqpayloadlen);
  1598. if (QDF_IS_STATUS_ERROR(ret))
  1599. return ret;
  1600. if (is_elemfragseq) {
  1601. if (tmplen != mlieseqlen) {
  1602. mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val per Multi-Link element search: %zu octets",
  1603. tmplen, mlieseqlen);
  1604. return QDF_STATUS_E_FAILURE;
  1605. }
  1606. if (!mlieseqpayloadlen) {
  1607. mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
  1608. return QDF_STATUS_E_FAILURE;
  1609. }
  1610. mlo_debug("Multi-Link element fragment sequence found with payload len %zu",
  1611. mlieseqpayloadlen);
  1612. } else {
  1613. if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
  1614. mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
  1615. mlieseqlen,
  1616. sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
  1617. return QDF_STATUS_E_FAILURE;
  1618. }
  1619. mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
  1620. }
  1621. mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
  1622. if (!mlieseqpayload_copy) {
  1623. mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
  1624. return QDF_STATUS_E_NOMEM;
  1625. }
  1626. if (is_elemfragseq) {
  1627. ret = wlan_defrag_elem_fragseq(false,
  1628. mlieseq,
  1629. mlieseqlen,
  1630. mlieseqpayload_copy,
  1631. mlieseqpayloadlen,
  1632. &defragpayload_len);
  1633. if (QDF_IS_STATUS_ERROR(ret)) {
  1634. qdf_mem_free(mlieseqpayload_copy);
  1635. return ret;
  1636. }
  1637. if (defragpayload_len != mlieseqpayloadlen) {
  1638. mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
  1639. defragpayload_len, mlieseqpayloadlen);
  1640. qdf_mem_free(mlieseqpayload_copy);
  1641. return QDF_STATUS_E_FAILURE;
  1642. }
  1643. } else {
  1644. qdf_mem_copy(mlieseqpayload_copy,
  1645. mlieseq + sizeof(struct ie_header) + 1,
  1646. mlieseqpayloadlen);
  1647. }
  1648. link_info = NULL;
  1649. link_info_len = 0;
  1650. ret = util_parse_multi_link_ctrl(mlieseqpayload_copy,
  1651. mlieseqpayloadlen,
  1652. &link_info,
  1653. &link_info_len);
  1654. if (QDF_IS_STATUS_ERROR(ret)) {
  1655. qdf_mem_free(mlieseqpayload_copy);
  1656. return ret;
  1657. }
  1658. /* As per the standard, the sender must include Link Info for
  1659. * association request/response. Throw an error if we are unable to
  1660. * obtain this.
  1661. */
  1662. if (!link_info) {
  1663. mlo_err_rl("Unable to successfully obtain Link Info");
  1664. qdf_mem_free(mlieseqpayload_copy);
  1665. return QDF_STATUS_E_PROTO;
  1666. }
  1667. mlo_debug("Dumping hex of link info after parsing Multi-Link element control");
  1668. QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_MLO, QDF_TRACE_LEVEL_DEBUG,
  1669. link_info, link_info_len);
  1670. /* Note: We may have a future change to skip subelements which are not
  1671. * Per-STA Profile, handle more than two links in MLO, handle cases
  1672. * where we unexpectedly find more Per-STA Profiles than expected, etc.
  1673. */
  1674. persta_prof = link_info;
  1675. persta_prof_bufflen = link_info_len;
  1676. is_subelemfragseq = false;
  1677. subelemseqtotallen = 0;
  1678. subelemseqpayloadlen = 0;
  1679. ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
  1680. persta_prof,
  1681. persta_prof_bufflen,
  1682. &is_subelemfragseq,
  1683. &subelemseqtotallen,
  1684. &subelemseqpayloadlen);
  1685. if (QDF_IS_STATUS_ERROR(ret)) {
  1686. qdf_mem_free(mlieseqpayload_copy);
  1687. return ret;
  1688. }
  1689. if (is_subelemfragseq) {
  1690. if (!subelemseqpayloadlen) {
  1691. mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
  1692. qdf_mem_free(mlieseqpayload_copy);
  1693. return QDF_STATUS_E_FAILURE;
  1694. }
  1695. mlo_debug("Subelement fragment sequence found with payload len %zu",
  1696. subelemseqpayloadlen);
  1697. ret = wlan_defrag_subelem_fragseq(true,
  1698. WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
  1699. persta_prof,
  1700. persta_prof_bufflen,
  1701. NULL,
  1702. 0,
  1703. &defragpayload_len);
  1704. if (QDF_IS_STATUS_ERROR(ret)) {
  1705. qdf_mem_free(mlieseqpayload_copy);
  1706. return ret;
  1707. }
  1708. if (defragpayload_len != subelemseqpayloadlen) {
  1709. mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
  1710. defragpayload_len,
  1711. subelemseqpayloadlen);
  1712. qdf_mem_free(mlieseqpayload_copy);
  1713. return QDF_STATUS_E_FAILURE;
  1714. }
  1715. } else {
  1716. if (persta_prof_bufflen <
  1717. (sizeof(struct subelem_header) +
  1718. persta_prof[TAG_LEN_POS])) {
  1719. mlo_err_rl("Length of buffer containing per-STA profile %zu octets is smaller than total size of current subelement %zu octets",
  1720. persta_prof_bufflen,
  1721. sizeof(struct subelem_header) +
  1722. persta_prof[TAG_LEN_POS]);
  1723. return QDF_STATUS_E_PROTO;
  1724. }
  1725. subelemseqpayloadlen = persta_prof[TAG_LEN_POS];
  1726. }
  1727. sta_prof_remlen = 0;
  1728. sta_prof_currpos = NULL;
  1729. is_reportedmacaddr_valid = false;
  1730. is_beaconinterval_valid = false;
  1731. is_completeprofile = false;
  1732. is_tsfoffset_valid = false;
  1733. /* Parse per-STA profile */
  1734. ret = util_parse_bvmlie_perstaprofile_stactrl(persta_prof +
  1735. sizeof(struct subelem_header),
  1736. subelemseqpayloadlen,
  1737. &linkid,
  1738. &beaconinterval,
  1739. &is_beaconinterval_valid,
  1740. &tsfoffset,
  1741. &is_tsfoffset_valid,
  1742. &is_completeprofile,
  1743. &is_reportedmacaddr_valid,
  1744. &reportedmacaddr,
  1745. true,
  1746. &sta_prof_currpos,
  1747. &sta_prof_remlen);
  1748. if (QDF_IS_STATUS_ERROR(ret)) {
  1749. qdf_mem_free(mlieseqpayload_copy);
  1750. return ret;
  1751. }
  1752. if (subtype == WLAN_FC0_STYPE_PROBE_RESP && !is_completeprofile) {
  1753. mlo_err("Complete profile information is not present in per-STA profile of probe response frame");
  1754. return QDF_STATUS_E_NOSUPPORT;
  1755. }
  1756. /* We double check for a NULL STA Profile, though the helper function
  1757. * above would have taken care of this. We need to get a non-NULL STA
  1758. * profile, because we need to get at least the expected fixed fields,
  1759. * even if there is an (improbable) total inheritance.
  1760. */
  1761. if (!sta_prof_currpos) {
  1762. mlo_err_rl("STA profile is NULL");
  1763. qdf_mem_free(mlieseqpayload_copy);
  1764. return QDF_STATUS_E_PROTO;
  1765. }
  1766. /* As per the standard, the sender sets the MAC address in the per-STA
  1767. * profile in association request/response. Without this, we cannot
  1768. * generate the link specific frame.
  1769. */
  1770. if (!is_reportedmacaddr_valid) {
  1771. mlo_err_rl("Unable to get MAC address from per-STA profile");
  1772. qdf_mem_free(mlieseqpayload_copy);
  1773. return QDF_STATUS_E_PROTO;
  1774. }
  1775. link_frame_currpos = link_frame;
  1776. *link_frame_len = 0;
  1777. link_frame_currlen = 0;
  1778. if (link_frame_maxsize < WLAN_MAC_HDR_LEN_3A) {
  1779. mlo_err("Insufficient space in link specific frame for 802.11 header. Required: %u octets, available: %zu octets",
  1780. WLAN_MAC_HDR_LEN_3A, link_frame_maxsize);
  1781. qdf_mem_free(mlieseqpayload_copy);
  1782. return QDF_STATUS_E_NOMEM;
  1783. }
  1784. link_frame_currpos += WLAN_MAC_HDR_LEN_3A;
  1785. link_frame_currlen += WLAN_MAC_HDR_LEN_3A;
  1786. if ((subtype == WLAN_FC0_STYPE_ASSOC_REQ) ||
  1787. (subtype == WLAN_FC0_STYPE_REASSOC_REQ)) {
  1788. mlo_debug("Populating fixed fields for (re)assoc req in link specific frame");
  1789. if (sta_prof_remlen < WLAN_CAPABILITYINFO_LEN) {
  1790. mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Capability Info %u",
  1791. sta_prof_remlen,
  1792. WLAN_CAPABILITYINFO_LEN);
  1793. qdf_mem_free(mlieseqpayload_copy);
  1794. return QDF_STATUS_E_PROTO;
  1795. }
  1796. /* Capability information is specific to the link. Copy this
  1797. * from the STA profile.
  1798. */
  1799. if ((link_frame_maxsize - link_frame_currlen) <
  1800. WLAN_CAPABILITYINFO_LEN) {
  1801. mlo_err("Insufficient space in link specific frame for Capability Info field. Required: %u octets, available: %zu octets",
  1802. WLAN_CAPABILITYINFO_LEN,
  1803. (link_frame_maxsize - link_frame_currlen));
  1804. qdf_mem_free(mlieseqpayload_copy);
  1805. return QDF_STATUS_E_NOMEM;
  1806. }
  1807. qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
  1808. WLAN_CAPABILITYINFO_LEN);
  1809. link_frame_currpos += WLAN_CAPABILITYINFO_LEN;
  1810. link_frame_currlen += WLAN_CAPABILITYINFO_LEN;
  1811. mlo_debug("Added Capability Info field (%u octets) to link specific frame",
  1812. WLAN_CAPABILITYINFO_LEN);
  1813. sta_prof_currpos += WLAN_CAPABILITYINFO_LEN;
  1814. sta_prof_remlen -= WLAN_CAPABILITYINFO_LEN;
  1815. /* Listen Interval is common between all links. Copy this from
  1816. * the reporting section of the frame.
  1817. */
  1818. if ((link_frame_maxsize - link_frame_currlen) <
  1819. WLAN_LISTENINTERVAL_LEN) {
  1820. mlo_err("Insufficient space in link specific frame for Listen Interval field. Required: %u octets, available: %zu octets",
  1821. WLAN_LISTENINTERVAL_LEN,
  1822. (link_frame_maxsize - link_frame_currlen));
  1823. qdf_mem_free(mlieseqpayload_copy);
  1824. return QDF_STATUS_E_NOMEM;
  1825. }
  1826. qdf_mem_copy(link_frame_currpos,
  1827. frame + WLAN_CAPABILITYINFO_LEN,
  1828. WLAN_LISTENINTERVAL_LEN);
  1829. link_frame_currpos += WLAN_LISTENINTERVAL_LEN;
  1830. link_frame_currlen += WLAN_LISTENINTERVAL_LEN;
  1831. mlo_debug("Added Listen Interval field (%u octets) to link specific frame",
  1832. WLAN_LISTENINTERVAL_LEN);
  1833. if (subtype == WLAN_FC0_STYPE_REASSOC_REQ) {
  1834. /* Current AP address is common between all links. Copy
  1835. * this from the reporting section of the frame.
  1836. */
  1837. if ((link_frame_maxsize - link_frame_currlen) <
  1838. QDF_MAC_ADDR_SIZE) {
  1839. mlo_err("Insufficient space in link specific frame for current AP address. Required: %u octets, available: %zu octets",
  1840. QDF_MAC_ADDR_SIZE,
  1841. (link_frame_maxsize -
  1842. link_frame_currlen));
  1843. qdf_mem_free(mlieseqpayload_copy);
  1844. return QDF_STATUS_E_NOMEM;
  1845. }
  1846. qdf_mem_copy(link_frame_currpos,
  1847. frame + WLAN_CAPABILITYINFO_LEN +
  1848. WLAN_LISTENINTERVAL_LEN,
  1849. QDF_MAC_ADDR_SIZE);
  1850. link_frame_currpos += QDF_MAC_ADDR_SIZE;
  1851. link_frame_currlen += QDF_MAC_ADDR_SIZE;
  1852. mlo_debug("Reassoc req: Added Current AP address field (%u octets) to link specific frame",
  1853. QDF_MAC_ADDR_SIZE);
  1854. }
  1855. } else if (subtype == WLAN_FC0_STYPE_ASSOC_RESP ||
  1856. subtype == WLAN_FC0_STYPE_REASSOC_RESP) {
  1857. /* This is a (re)association response */
  1858. mlo_debug("Populating fixed fields for (re)assoc resp in link specific frame");
  1859. if (sta_prof_remlen <
  1860. (WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN)) {
  1861. mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Capability Info + length of Status Code %u",
  1862. sta_prof_remlen,
  1863. WLAN_CAPABILITYINFO_LEN +
  1864. WLAN_STATUSCODE_LEN);
  1865. qdf_mem_free(mlieseqpayload_copy);
  1866. return QDF_STATUS_E_PROTO;
  1867. }
  1868. /* Capability information and Status Code are specific to the
  1869. * link. Copy these from the STA profile.
  1870. */
  1871. if ((link_frame_maxsize - link_frame_currlen) <
  1872. (WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN)) {
  1873. mlo_err("Insufficient space in link specific frame for Capability Info and Status Code fields. Required: %u octets, available: %zu octets",
  1874. WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN,
  1875. (link_frame_maxsize - link_frame_currlen));
  1876. qdf_mem_free(mlieseqpayload_copy);
  1877. return QDF_STATUS_E_NOMEM;
  1878. }
  1879. qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
  1880. (WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN));
  1881. link_frame_currpos += (WLAN_CAPABILITYINFO_LEN +
  1882. WLAN_STATUSCODE_LEN);
  1883. link_frame_currlen += (WLAN_CAPABILITYINFO_LEN +
  1884. WLAN_STATUSCODE_LEN);
  1885. mlo_debug("Added Capability Info and Status Code fields (%u octets) to link specific frame",
  1886. WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN);
  1887. sta_prof_currpos += (WLAN_CAPABILITYINFO_LEN +
  1888. WLAN_STATUSCODE_LEN);
  1889. sta_prof_remlen -= (WLAN_CAPABILITYINFO_LEN +
  1890. WLAN_STATUSCODE_LEN);
  1891. /* AID is common between all links. Copy this from the original
  1892. * frame.
  1893. */
  1894. if ((link_frame_maxsize - link_frame_currlen) < WLAN_AID_LEN) {
  1895. mlo_err("Insufficient space in link specific frame for AID field. Required: %u octets, available: %zu octets",
  1896. WLAN_AID_LEN,
  1897. (link_frame_maxsize - link_frame_currlen));
  1898. qdf_mem_free(mlieseqpayload_copy);
  1899. return QDF_STATUS_E_NOMEM;
  1900. }
  1901. qdf_mem_copy(link_frame_currpos,
  1902. frame + WLAN_CAPABILITYINFO_LEN +
  1903. WLAN_STATUSCODE_LEN,
  1904. WLAN_AID_LEN);
  1905. link_frame_currpos += WLAN_AID_LEN;
  1906. link_frame_currlen += WLAN_AID_LEN;
  1907. mlo_debug("Added AID field (%u octets) to link specific frame",
  1908. WLAN_AID_LEN);
  1909. } else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
  1910. /* This is a probe response */
  1911. mlo_debug("Populating fixed fields for probe response in link specific frame");
  1912. if ((link_frame_maxsize - link_frame_currlen) <
  1913. WLAN_TIMESTAMP_LEN) {
  1914. mlo_err("Insufficient space in link specific frame for Timestamp Info field. Required: %u octets, available: %zu octets",
  1915. WLAN_TIMESTAMP_LEN,
  1916. (link_frame_maxsize - link_frame_currlen));
  1917. qdf_mem_free(mlieseqpayload_copy);
  1918. return QDF_STATUS_E_NOMEM;
  1919. }
  1920. /* Per spec 11be_D2.1.1, the TSF Offset subfield of the STA Info
  1921. * field indicates the offset (Toffset)between the TSF timer of
  1922. * the reported AP (TA) and the TSF timer of the reporting
  1923. * AP (TB) and is encoded as a 2s complement signed integer
  1924. * with units of 2 µs. Toffset is calculated as
  1925. * Toffset= Floor((TA – TB)/2).
  1926. */
  1927. if (is_tsfoffset_valid)
  1928. tsf += tsfoffset * 2;
  1929. qdf_mem_copy(link_frame_currpos, &tsf, WLAN_TIMESTAMP_LEN);
  1930. link_frame_currpos += WLAN_TIMESTAMP_LEN;
  1931. link_frame_currlen += WLAN_TIMESTAMP_LEN;
  1932. mlo_debug("Added Timestamp Info field (%u octets) to link specific frame",
  1933. WLAN_TIMESTAMP_LEN);
  1934. if (!is_beaconinterval_valid) {
  1935. mlo_err_rl("Beacon interval information not present in STA info field of per-STA profile");
  1936. qdf_mem_free(mlieseqpayload_copy);
  1937. return QDF_STATUS_E_PROTO;
  1938. }
  1939. /* Beacon Interval information copy this from
  1940. * the STA info field.
  1941. */
  1942. if ((link_frame_maxsize - link_frame_currlen) <
  1943. WLAN_BEACONINTERVAL_LEN) {
  1944. mlo_err("Insufficient space in link specific frame for Beacon Interval Info field. Required: %u octets, available: %zu octets",
  1945. WLAN_BEACONINTERVAL_LEN,
  1946. (link_frame_maxsize - link_frame_currlen));
  1947. qdf_mem_free(mlieseqpayload_copy);
  1948. return QDF_STATUS_E_NOMEM;
  1949. }
  1950. qdf_mem_copy(link_frame_currpos, &beaconinterval,
  1951. WLAN_BEACONINTERVAL_LEN);
  1952. link_frame_currpos += WLAN_BEACONINTERVAL_LEN;
  1953. link_frame_currlen += WLAN_BEACONINTERVAL_LEN;
  1954. mlo_debug("Added Beacon Interval Info field (%u octets) to link specific frame",
  1955. WLAN_BEACONINTERVAL_LEN);
  1956. if (sta_prof_remlen < WLAN_CAPABILITYINFO_LEN) {
  1957. mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Capability Info %u",
  1958. sta_prof_remlen,
  1959. WLAN_CAPABILITYINFO_LEN);
  1960. qdf_mem_free(mlieseqpayload_copy);
  1961. return QDF_STATUS_E_PROTO;
  1962. }
  1963. /* Capability information is specific to the link. Copy this
  1964. * from the STA profile.
  1965. */
  1966. if ((link_frame_maxsize - link_frame_currlen) <
  1967. WLAN_CAPABILITYINFO_LEN) {
  1968. mlo_err("Insufficient space in link specific frame for Capability Info field. Required: %u octets, available: %zu octets",
  1969. WLAN_CAPABILITYINFO_LEN,
  1970. (link_frame_maxsize - link_frame_currlen));
  1971. qdf_mem_free(mlieseqpayload_copy);
  1972. return QDF_STATUS_E_NOMEM;
  1973. }
  1974. qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
  1975. WLAN_CAPABILITYINFO_LEN);
  1976. link_frame_currpos += WLAN_CAPABILITYINFO_LEN;
  1977. link_frame_currlen += WLAN_CAPABILITYINFO_LEN;
  1978. mlo_debug("Added Capability Info field (%u octets) to link specific frame",
  1979. WLAN_CAPABILITYINFO_LEN);
  1980. sta_prof_currpos += WLAN_CAPABILITYINFO_LEN;
  1981. sta_prof_remlen -= WLAN_CAPABILITYINFO_LEN;
  1982. }
  1983. sta_prof_iesection = sta_prof_currpos;
  1984. sta_prof_iesection_len = sta_prof_remlen;
  1985. /* Populate non-inheritance lists if applicable */
  1986. ninherit_elemlist_len = 0;
  1987. ninherit_elemlist = NULL;
  1988. ninherit_elemextlist_len = 0;
  1989. ninherit_elemextlist = NULL;
  1990. ret = util_get_noninheritlists(sta_prof_iesection,
  1991. sta_prof_iesection_len,
  1992. &ninherit_elemlist,
  1993. &ninherit_elemlist_len,
  1994. &ninherit_elemextlist,
  1995. &ninherit_elemextlist_len);
  1996. if (QDF_IS_STATUS_ERROR(ret)) {
  1997. qdf_mem_free(mlieseqpayload_copy);
  1998. return ret;
  1999. }
  2000. /* Go through IEs of the reporting STA, and those in STA profile, merge
  2001. * them into link_frame (except for elements in the Non-Inheritance
  2002. * list).
  2003. *
  2004. * Note: Currently, only 2-link MLO is supported here. We may have a
  2005. * future change to expand to more links.
  2006. */
  2007. reportingsta_ie = util_find_eid(WLAN_ELEMID_SSID, frame_iesection,
  2008. frame_iesection_len);
  2009. if ((subtype == WLAN_FC0_STYPE_ASSOC_REQ) ||
  2010. (subtype == WLAN_FC0_STYPE_REASSOC_REQ) ||
  2011. (subtype == WLAN_FC0_STYPE_PROBE_RESP)) {
  2012. /* Sanity check that the SSID element is present for the
  2013. * reporting STA. There is no stipulation in the standard for
  2014. * the STA profile in this regard, so we do not check the STA
  2015. * profile for the SSID element.
  2016. */
  2017. if (!reportingsta_ie) {
  2018. mlo_err_rl("SSID element not found in reporting STA of the frame.");
  2019. qdf_mem_free(mlieseqpayload_copy);
  2020. return QDF_STATUS_E_PROTO;
  2021. }
  2022. } else {
  2023. /* This is a (re)association response. Sanity check that the
  2024. * SSID element is present neither for the reporting STA nor in
  2025. * the STA profile.
  2026. */
  2027. if (reportingsta_ie) {
  2028. mlo_err_rl("SSID element found for reporting STA for (re)association response. It should not be present.");
  2029. qdf_mem_free(mlieseqpayload_copy);
  2030. return QDF_STATUS_E_PROTO;
  2031. }
  2032. sta_prof_ie = util_find_eid(WLAN_ELEMID_SSID,
  2033. sta_prof_iesection,
  2034. sta_prof_iesection_len);
  2035. if (sta_prof_ie) {
  2036. mlo_err_rl("SSID element found in STA profile for (re)association response. It should not be present.");
  2037. qdf_mem_free(mlieseqpayload_copy);
  2038. return QDF_STATUS_E_PROTO;
  2039. }
  2040. }
  2041. reportingsta_ie = reportingsta_ie ? reportingsta_ie : frame_iesection;
  2042. ret = util_validate_reportingsta_ie(reportingsta_ie, frame_iesection,
  2043. frame_iesection_len);
  2044. if (QDF_IS_STATUS_ERROR(ret)) {
  2045. qdf_mem_free(mlieseqpayload_copy);
  2046. return ret;
  2047. }
  2048. reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] + MIN_IE_LEN;
  2049. while (((reportingsta_ie + reportingsta_ie_size) - frame_iesection)
  2050. <= frame_iesection_len) {
  2051. /* Skip Multi-Link element */
  2052. if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
  2053. (reportingsta_ie[IDEXT_POS] ==
  2054. WLAN_EXTN_ELEMID_MULTI_LINK)) {
  2055. if (((reportingsta_ie + reportingsta_ie_size) -
  2056. frame_iesection) == frame_iesection_len)
  2057. break;
  2058. /* Add BV ML IE for link specific probe response */
  2059. if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
  2060. ret = util_add_mlie_for_prb_rsp_gen(reportingsta_ie,
  2061. reportingsta_ie[TAG_LEN_POS],
  2062. &link_frame_currpos,
  2063. &link_frame_currlen,
  2064. linkid);
  2065. if (QDF_IS_STATUS_ERROR(ret)) {
  2066. qdf_mem_free(mlieseqpayload_copy);
  2067. return ret;
  2068. }
  2069. }
  2070. reportingsta_ie += reportingsta_ie_size;
  2071. ret = util_validate_reportingsta_ie(reportingsta_ie,
  2072. frame_iesection,
  2073. frame_iesection_len);
  2074. if (QDF_IS_STATUS_ERROR(ret)) {
  2075. qdf_mem_free(mlieseqpayload_copy);
  2076. return ret;
  2077. }
  2078. reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] +
  2079. MIN_IE_LEN;
  2080. continue;
  2081. }
  2082. sta_prof_ie = NULL;
  2083. sta_prof_ie_size = 0;
  2084. if (sta_prof_iesection_len) {
  2085. if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
  2086. sta_prof_ie = (uint8_t *)util_find_extn_eid(reportingsta_ie[ID_POS],
  2087. reportingsta_ie[IDEXT_POS],
  2088. sta_prof_iesection,
  2089. sta_prof_iesection_len);
  2090. } else {
  2091. sta_prof_ie = (uint8_t *)util_find_eid(reportingsta_ie[ID_POS],
  2092. sta_prof_iesection,
  2093. sta_prof_iesection_len);
  2094. }
  2095. }
  2096. if (!sta_prof_ie) {
  2097. /* IE is present for reporting STA, but not in STA
  2098. * profile.
  2099. */
  2100. is_in_noninheritlist = false;
  2101. ret = util_eval_ie_in_noninheritlist((uint8_t *)reportingsta_ie,
  2102. reportingsta_ie_size,
  2103. ninherit_elemlist,
  2104. ninherit_elemlist_len,
  2105. ninherit_elemextlist,
  2106. ninherit_elemextlist_len,
  2107. &is_in_noninheritlist);
  2108. if (QDF_IS_STATUS_ERROR(ret)) {
  2109. qdf_mem_free(mlieseqpayload_copy);
  2110. return ret;
  2111. }
  2112. if (!is_in_noninheritlist) {
  2113. if ((link_frame_currpos +
  2114. reportingsta_ie_size) <=
  2115. (link_frame + link_frame_maxsize)) {
  2116. qdf_mem_copy(link_frame_currpos,
  2117. reportingsta_ie,
  2118. reportingsta_ie_size);
  2119. link_frame_currpos +=
  2120. reportingsta_ie_size;
  2121. link_frame_currlen +=
  2122. reportingsta_ie_size;
  2123. if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
  2124. mlo_debug("IE with element ID : %u extension element ID : %u (%zu octets) present for reporting STA but not in STA profile. Copied IE from reporting frame to link specific frame",
  2125. reportingsta_ie[ID_POS],
  2126. reportingsta_ie[IDEXT_POS],
  2127. reportingsta_ie_size);
  2128. } else {
  2129. mlo_debug("IE with element ID : %u (%zu octets) present for reporting STA but not in STA profile. Copied IE from reporting frame to link specific frame",
  2130. reportingsta_ie[ID_POS],
  2131. reportingsta_ie_size);
  2132. }
  2133. } else {
  2134. if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
  2135. mlo_err_rl("Insufficient space in link specific frame for IE with element ID : %u extension element ID : %u. Required: %zu octets, available: %zu octets",
  2136. reportingsta_ie[ID_POS],
  2137. reportingsta_ie[IDEXT_POS],
  2138. reportingsta_ie_size,
  2139. link_frame_maxsize -
  2140. link_frame_currlen);
  2141. } else {
  2142. mlo_err_rl("Insufficient space in link specific frame for IE with element ID : %u. Required: %zu octets, available: %zu octets",
  2143. reportingsta_ie[ID_POS],
  2144. reportingsta_ie_size,
  2145. link_frame_maxsize -
  2146. link_frame_currlen);
  2147. }
  2148. qdf_mem_free(mlieseqpayload_copy);
  2149. return QDF_STATUS_E_NOMEM;
  2150. }
  2151. } else {
  2152. if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
  2153. mlo_debug("IE with element ID : %u extension element ID : %u (%zu octets) present for reporting STA but not in STA profile. However it is in Non-Inheritance list, hence ignoring.",
  2154. reportingsta_ie[ID_POS],
  2155. reportingsta_ie[IDEXT_POS],
  2156. reportingsta_ie_size);
  2157. } else {
  2158. mlo_debug("IE with element ID : %u (%zu octets) present for reporting STA but not in STA profile. However it is in Non-Inheritance list, hence ignoring.",
  2159. reportingsta_ie[ID_POS],
  2160. reportingsta_ie_size);
  2161. }
  2162. }
  2163. } else {
  2164. /* IE is present for reporting STA and also in STA
  2165. * profile, copy from STA profile and flag the IE in STA
  2166. * profile as copied (by setting EID field to 0). The
  2167. * SSID element (with EID 0) is processed first to
  2168. * enable this. For vendor IE, compare OUI + type +
  2169. * subType to determine if they are the same IE.
  2170. */
  2171. /* Note: This may be revisited in a future change, to
  2172. * adhere to provisions in the standard for multiple
  2173. * occurrences of a given element ID/extension element
  2174. * ID.
  2175. */
  2176. ret = util_validate_sta_prof_ie(sta_prof_ie,
  2177. sta_prof_iesection,
  2178. sta_prof_iesection_len);
  2179. if (QDF_IS_STATUS_ERROR(ret)) {
  2180. qdf_mem_free(mlieseqpayload_copy);
  2181. return ret;
  2182. }
  2183. sta_prof_ie_size = sta_prof_ie[TAG_LEN_POS] +
  2184. MIN_IE_LEN;
  2185. sta_prof_iesection_remlen =
  2186. sta_prof_iesection_len -
  2187. (sta_prof_ie - sta_prof_iesection);
  2188. if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_VENDOR) &&
  2189. (sta_prof_iesection_remlen >= MIN_VENDOR_TAG_LEN)) {
  2190. /* If Vendor IE also presents in STA profile,
  2191. * then ignore the Vendor IE which is for
  2192. * reporting STA. It only needs to copy Vendor
  2193. * IE from STA profile to link specific frame.
  2194. * The copy happens when going through the
  2195. * remaining IEs.
  2196. */
  2197. ;
  2198. } else {
  2199. /* Copy IE from STA profile into link specific
  2200. * frame.
  2201. */
  2202. if ((link_frame_currpos + sta_prof_ie_size) <=
  2203. (link_frame + link_frame_maxsize)) {
  2204. qdf_mem_copy(link_frame_currpos,
  2205. sta_prof_ie,
  2206. sta_prof_ie_size);
  2207. link_frame_currpos += sta_prof_ie_size;
  2208. link_frame_currlen +=
  2209. sta_prof_ie_size;
  2210. if (reportingsta_ie[ID_POS] ==
  2211. WLAN_ELEMID_EXTN_ELEM) {
  2212. mlo_debug("IE with element ID : %u extension element ID : %u (%zu octets) for reporting STA also present in STA profile. Copied IE from STA profile to link specific frame",
  2213. sta_prof_ie[ID_POS],
  2214. sta_prof_ie[IDEXT_POS],
  2215. sta_prof_ie_size);
  2216. } else {
  2217. mlo_debug("IE with element ID : %u (%zu octets) for reporting STA also present in STA profile. Copied IE from STA profile to link specific frame",
  2218. sta_prof_ie[ID_POS],
  2219. sta_prof_ie_size);
  2220. }
  2221. sta_prof_ie[0] = 0;
  2222. } else {
  2223. if (sta_prof_ie[ID_POS] ==
  2224. WLAN_ELEMID_EXTN_ELEM) {
  2225. mlo_err_rl("Insufficient space in link specific frame for IE with element ID : %u extension element ID : %u. Required: %zu octets, available: %zu octets",
  2226. sta_prof_ie[ID_POS],
  2227. sta_prof_ie[IDEXT_POS],
  2228. sta_prof_ie_size,
  2229. link_frame_maxsize -
  2230. link_frame_currlen);
  2231. } else {
  2232. mlo_err_rl("Insufficient space in link specific frame for IE with element ID : %u. Required: %zu octets, available: %zu octets",
  2233. sta_prof_ie[ID_POS],
  2234. sta_prof_ie_size,
  2235. link_frame_maxsize -
  2236. link_frame_currlen);
  2237. }
  2238. qdf_mem_free(mlieseqpayload_copy);
  2239. return QDF_STATUS_E_NOMEM;
  2240. }
  2241. }
  2242. }
  2243. if (((reportingsta_ie + reportingsta_ie_size) -
  2244. frame_iesection) == frame_iesection_len)
  2245. break;
  2246. reportingsta_ie += reportingsta_ie_size;
  2247. ret = util_validate_reportingsta_ie(reportingsta_ie,
  2248. frame_iesection,
  2249. frame_iesection_len);
  2250. if (QDF_IS_STATUS_ERROR(ret)) {
  2251. qdf_mem_free(mlieseqpayload_copy);
  2252. return ret;
  2253. }
  2254. reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] +
  2255. MIN_IE_LEN;
  2256. }
  2257. /* Go through the remaining unprocessed IEs in STA profile and copy them
  2258. * to the link specific frame. The processed ones are marked with 0 in
  2259. * the first octet. The first octet corresponds to the element ID. In
  2260. * the case of (re)association request, the element with actual ID
  2261. * WLAN_ELEMID_SSID(0) has already been copied to the link specific
  2262. * frame. In the case of (re)association response, it has been verified
  2263. * that the element with actual ID WLAN_ELEMID_SSID(0) is present
  2264. * neither for the reporting STA nor in the STA profile.
  2265. */
  2266. sta_prof_iesection_currpos = sta_prof_iesection;
  2267. sta_prof_iesection_remlen = sta_prof_iesection_len;
  2268. while (sta_prof_iesection_remlen > 0) {
  2269. sta_prof_ie = sta_prof_iesection_currpos;
  2270. ret = util_validate_sta_prof_ie(sta_prof_ie,
  2271. sta_prof_iesection_currpos,
  2272. sta_prof_iesection_remlen);
  2273. if (QDF_IS_STATUS_ERROR(ret)) {
  2274. qdf_mem_free(mlieseqpayload_copy);
  2275. return ret;
  2276. }
  2277. sta_prof_ie_size = sta_prof_ie[TAG_LEN_POS] + MIN_IE_LEN;
  2278. if (!sta_prof_ie[0]) {
  2279. /* Skip this, since it has already been processed */
  2280. sta_prof_iesection_currpos += sta_prof_ie_size;
  2281. sta_prof_iesection_remlen -= sta_prof_ie_size;
  2282. continue;
  2283. }
  2284. /* Copy IE from STA profile into link specific frame. */
  2285. if ((link_frame_currpos + sta_prof_ie_size) <=
  2286. (link_frame + link_frame_maxsize)) {
  2287. qdf_mem_copy(link_frame_currpos,
  2288. sta_prof_ie,
  2289. sta_prof_ie_size);
  2290. link_frame_currpos += sta_prof_ie_size;
  2291. link_frame_currlen +=
  2292. sta_prof_ie_size;
  2293. if (reportingsta_ie[ID_POS] ==
  2294. WLAN_ELEMID_EXTN_ELEM) {
  2295. mlo_debug("IE with element ID : %u extension element ID : %u (%zu octets) is present only in STA profile. Copied IE from STA profile to link specific frame",
  2296. sta_prof_ie[ID_POS],
  2297. sta_prof_ie[IDEXT_POS],
  2298. sta_prof_ie_size);
  2299. } else {
  2300. mlo_debug("IE with element ID : %u (%zu octets) is present only in STA profile. Copied IE from STA profile to link specific frame",
  2301. sta_prof_ie[ID_POS],
  2302. sta_prof_ie_size);
  2303. }
  2304. sta_prof_ie[0] = 0;
  2305. } else {
  2306. if (sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
  2307. mlo_err_rl("Insufficient space in link specific frame for IE with element ID : %u extension element ID : %u. Required: %zu octets, available: %zu octets",
  2308. sta_prof_ie[ID_POS],
  2309. sta_prof_ie[IDEXT_POS],
  2310. sta_prof_ie_size,
  2311. link_frame_maxsize -
  2312. link_frame_currlen);
  2313. } else {
  2314. mlo_err_rl("Insufficient space in link specific frame for IE with element ID : %u. Required: %zu octets, available: %zu octets",
  2315. sta_prof_ie[ID_POS],
  2316. sta_prof_ie_size,
  2317. link_frame_maxsize -
  2318. link_frame_currlen);
  2319. }
  2320. qdf_mem_free(mlieseqpayload_copy);
  2321. return QDF_STATUS_E_NOMEM;
  2322. }
  2323. sta_prof_iesection_currpos += sta_prof_ie_size;
  2324. sta_prof_iesection_remlen -= sta_prof_ie_size;
  2325. }
  2326. /* Copy the link MAC addr */
  2327. link_frame_hdr = (struct wlan_frame_hdr *)link_frame;
  2328. if ((subtype == WLAN_FC0_STYPE_ASSOC_REQ) ||
  2329. (subtype == WLAN_FC0_STYPE_REASSOC_REQ)) {
  2330. qdf_mem_copy(link_frame_hdr->i_addr3, &link_addr,
  2331. QDF_MAC_ADDR_SIZE);
  2332. qdf_mem_copy(link_frame_hdr->i_addr2, reportedmacaddr.bytes,
  2333. QDF_MAC_ADDR_SIZE);
  2334. qdf_mem_copy(link_frame_hdr->i_addr1, &link_addr,
  2335. QDF_MAC_ADDR_SIZE);
  2336. link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_ASSOC_REQ_FC0;
  2337. link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_ASSOC_REQ_FC1;
  2338. } else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
  2339. qdf_mem_copy(link_frame_hdr->i_addr3, reportedmacaddr.bytes,
  2340. QDF_MAC_ADDR_SIZE);
  2341. qdf_mem_copy(link_frame_hdr->i_addr2, reportedmacaddr.bytes,
  2342. QDF_MAC_ADDR_SIZE);
  2343. qdf_mem_copy(link_frame_hdr->i_addr1, &link_addr,
  2344. QDF_MAC_ADDR_SIZE);
  2345. link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_PROBE_RESP_FC0;
  2346. link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_PROBE_RESP_FC1;
  2347. } else {
  2348. /* This is a (re)association response */
  2349. qdf_mem_copy(link_frame_hdr->i_addr3, reportedmacaddr.bytes,
  2350. QDF_MAC_ADDR_SIZE);
  2351. qdf_mem_copy(link_frame_hdr->i_addr2, reportedmacaddr.bytes,
  2352. QDF_MAC_ADDR_SIZE);
  2353. qdf_mem_copy(link_frame_hdr->i_addr1, &link_addr,
  2354. QDF_MAC_ADDR_SIZE);
  2355. link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_ASSOC_RESP_FC0;
  2356. link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_ASSOC_RESP_FC1;
  2357. }
  2358. mlo_debug("subtype:%u addr3:" QDF_MAC_ADDR_FMT " addr2:"
  2359. QDF_MAC_ADDR_FMT " addr1:" QDF_MAC_ADDR_FMT,
  2360. subtype,
  2361. QDF_MAC_ADDR_REF(link_frame_hdr->i_addr3),
  2362. QDF_MAC_ADDR_REF(link_frame_hdr->i_addr2),
  2363. QDF_MAC_ADDR_REF(link_frame_hdr->i_addr1));
  2364. /* Seq num not used so not populated */
  2365. qdf_mem_free(mlieseqpayload_copy);
  2366. *link_frame_len = link_frame_currlen;
  2367. return QDF_STATUS_SUCCESS;
  2368. }
  2369. QDF_STATUS
  2370. util_gen_link_assoc_req(uint8_t *frame, qdf_size_t frame_len, bool isreassoc,
  2371. struct qdf_mac_addr link_addr,
  2372. uint8_t *link_frame,
  2373. qdf_size_t link_frame_maxsize,
  2374. qdf_size_t *link_frame_len)
  2375. {
  2376. return util_gen_link_reqrsp_cmn(frame, frame_len,
  2377. (isreassoc ? WLAN_FC0_STYPE_REASSOC_REQ :
  2378. WLAN_FC0_STYPE_ASSOC_REQ),
  2379. link_addr, link_frame, link_frame_maxsize,
  2380. link_frame_len);
  2381. }
  2382. QDF_STATUS
  2383. util_gen_link_assoc_rsp(uint8_t *frame, qdf_size_t frame_len, bool isreassoc,
  2384. struct qdf_mac_addr link_addr,
  2385. uint8_t *link_frame,
  2386. qdf_size_t link_frame_maxsize,
  2387. qdf_size_t *link_frame_len)
  2388. {
  2389. return util_gen_link_reqrsp_cmn(frame, frame_len,
  2390. (isreassoc ? WLAN_FC0_STYPE_REASSOC_RESP :
  2391. WLAN_FC0_STYPE_ASSOC_RESP),
  2392. link_addr, link_frame, link_frame_maxsize,
  2393. link_frame_len);
  2394. }
  2395. QDF_STATUS
  2396. util_gen_link_probe_rsp(uint8_t *frame, qdf_size_t frame_len,
  2397. struct qdf_mac_addr link_addr,
  2398. uint8_t *link_frame,
  2399. qdf_size_t link_frame_maxsize,
  2400. qdf_size_t *link_frame_len)
  2401. {
  2402. return util_gen_link_reqrsp_cmn(frame, frame_len,
  2403. WLAN_FC0_STYPE_PROBE_RESP,
  2404. link_addr, link_frame, link_frame_maxsize,
  2405. link_frame_len);
  2406. }
  2407. QDF_STATUS
  2408. util_find_mlie(uint8_t *buf, qdf_size_t buflen, uint8_t **mlieseq,
  2409. qdf_size_t *mlieseqlen)
  2410. {
  2411. uint8_t *bufboundary;
  2412. uint8_t *ieseq;
  2413. qdf_size_t ieseqlen;
  2414. uint8_t *currie;
  2415. uint8_t *successorfrag;
  2416. if (!buf || !buflen || !mlieseq || !mlieseqlen)
  2417. return QDF_STATUS_E_NULL_VALUE;
  2418. *mlieseq = NULL;
  2419. *mlieseqlen = 0;
  2420. /* Find Multi-Link element. In case a fragment sequence is present,
  2421. * this element will be the leading fragment.
  2422. */
  2423. ieseq = util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM,
  2424. WLAN_EXTN_ELEMID_MULTI_LINK, buf,
  2425. buflen);
  2426. /* Even if the element is not found, we have successfully examined the
  2427. * buffer. The caller will be provided a NULL value for the starting of
  2428. * the Multi-Link element. Hence, we return success.
  2429. */
  2430. if (!ieseq)
  2431. return QDF_STATUS_SUCCESS;
  2432. bufboundary = buf + buflen;
  2433. if ((ieseq + MIN_IE_LEN) > bufboundary)
  2434. return QDF_STATUS_E_INVAL;
  2435. ieseqlen = MIN_IE_LEN + ieseq[TAG_LEN_POS];
  2436. if (ieseqlen < sizeof(struct wlan_ie_multilink))
  2437. return QDF_STATUS_E_PROTO;
  2438. if ((ieseq + ieseqlen) > bufboundary)
  2439. return QDF_STATUS_E_INVAL;
  2440. /* In the next sequence of checks, if there is no space in the buffer
  2441. * for another element after the Multi-Link element/element fragment
  2442. * sequence, it could indicate an issue since non-MLO EHT elements
  2443. * would be expected to follow the Multi-Link element/element fragment
  2444. * sequence. However, this is outside of the purview of this function,
  2445. * hence we ignore it.
  2446. */
  2447. currie = ieseq;
  2448. successorfrag = util_get_successorfrag(currie, buf, buflen);
  2449. /* Fragmentation definitions as of IEEE802.11be D1.0 and
  2450. * IEEE802.11REVme D0.2 are applied. Only the case where Multi-Link
  2451. * element is present in a buffer from the core frame is considered.
  2452. * Future changes to fragmentation, cases where the Multi-Link element
  2453. * is present in a subelement, etc. to be reflected here if applicable
  2454. * as and when the rules evolve.
  2455. */
  2456. while (successorfrag) {
  2457. /* We should not be seeing a successor fragment if the length
  2458. * of the current IE is lesser than the max.
  2459. */
  2460. if (currie[TAG_LEN_POS] != WLAN_MAX_IE_LEN)
  2461. return QDF_STATUS_E_PROTO;
  2462. if (successorfrag[TAG_LEN_POS] == 0)
  2463. return QDF_STATUS_E_PROTO;
  2464. ieseqlen += (MIN_IE_LEN + successorfrag[TAG_LEN_POS]);
  2465. currie = successorfrag;
  2466. successorfrag = util_get_successorfrag(currie, buf, buflen);
  2467. }
  2468. *mlieseq = ieseq;
  2469. *mlieseqlen = ieseqlen;
  2470. return QDF_STATUS_SUCCESS;
  2471. }
  2472. QDF_STATUS
  2473. util_find_mlie_by_variant(uint8_t *buf, qdf_size_t buflen, uint8_t **mlieseq,
  2474. qdf_size_t *mlieseqlen, int variant)
  2475. {
  2476. uint8_t *ieseq;
  2477. qdf_size_t ieseqlen;
  2478. QDF_STATUS status;
  2479. int ml_variant;
  2480. qdf_size_t buf_parsed_len;
  2481. if (!buf || !buflen || !mlieseq || !mlieseqlen)
  2482. return QDF_STATUS_E_NULL_VALUE;
  2483. if (variant >= WLAN_ML_VARIANT_INVALIDSTART)
  2484. return QDF_STATUS_E_PROTO;
  2485. ieseq = NULL;
  2486. ieseqlen = 0;
  2487. *mlieseq = NULL;
  2488. *mlieseqlen = 0;
  2489. buf_parsed_len = 0;
  2490. while (buflen > buf_parsed_len) {
  2491. status = util_find_mlie(buf + buf_parsed_len,
  2492. buflen - buf_parsed_len,
  2493. &ieseq, &ieseqlen);
  2494. if (QDF_IS_STATUS_ERROR(status))
  2495. return status;
  2496. /* Even if the element is not found, we have successfully
  2497. * examined the buffer. The caller will be provided a NULL value
  2498. * for the starting of the Multi-Link element. Hence, we return
  2499. * success.
  2500. */
  2501. if (!ieseq)
  2502. return QDF_STATUS_SUCCESS;
  2503. status = util_get_mlie_variant(ieseq, ieseqlen,
  2504. &ml_variant);
  2505. if (QDF_IS_STATUS_ERROR(status)) {
  2506. mlo_err("Unable to get Multi-link element variant");
  2507. return status;
  2508. }
  2509. if (ml_variant == variant) {
  2510. *mlieseq = ieseq;
  2511. *mlieseqlen = ieseqlen;
  2512. return QDF_STATUS_SUCCESS;
  2513. }
  2514. buf_parsed_len = ieseq + ieseqlen - buf;
  2515. }
  2516. return QDF_STATUS_E_INVAL;
  2517. }
  2518. QDF_STATUS
  2519. util_get_mlie_common_info_len(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2520. uint8_t *commoninfo_len)
  2521. {
  2522. struct wlan_ie_multilink *mlie_fixed;
  2523. enum wlan_ml_variant variant;
  2524. uint16_t mlcontrol;
  2525. if (!mlieseq || !mlieseqlen || !commoninfo_len)
  2526. return QDF_STATUS_E_NULL_VALUE;
  2527. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2528. return QDF_STATUS_E_INVAL;
  2529. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2530. if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
  2531. mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
  2532. return QDF_STATUS_E_INVAL;
  2533. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  2534. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2535. WLAN_ML_CTRL_TYPE_BITS);
  2536. if (variant != WLAN_ML_VARIANT_BASIC)
  2537. return QDF_STATUS_E_INVAL;
  2538. /* Common Info starts at mlieseq + sizeof(struct wlan_ie_multilink).
  2539. * Check if there is sufficient space in the buffer for the Common Info
  2540. * Length and MLD MAC address.
  2541. */
  2542. if ((sizeof(struct wlan_ie_multilink) + WLAN_ML_BV_CINFO_LENGTH_SIZE +
  2543. QDF_MAC_ADDR_SIZE) > mlieseqlen)
  2544. return QDF_STATUS_E_PROTO;
  2545. *commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
  2546. return QDF_STATUS_SUCCESS;
  2547. }
  2548. QDF_STATUS
  2549. util_get_bvmlie_bssparamchangecnt(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2550. bool *bssparamchangecntfound,
  2551. uint8_t *bssparamchangecnt)
  2552. {
  2553. struct wlan_ie_multilink *mlie_fixed;
  2554. enum wlan_ml_variant variant;
  2555. uint16_t mlcontrol;
  2556. uint16_t presencebitmap;
  2557. uint8_t *commoninfo;
  2558. qdf_size_t commoninfolen;
  2559. if (!mlieseq || !mlieseqlen || !bssparamchangecntfound ||
  2560. !bssparamchangecnt)
  2561. return QDF_STATUS_E_NULL_VALUE;
  2562. *bssparamchangecntfound = false;
  2563. *bssparamchangecnt = 0;
  2564. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2565. return QDF_STATUS_E_INVAL;
  2566. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2567. if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
  2568. mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
  2569. return QDF_STATUS_E_INVAL;
  2570. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  2571. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2572. WLAN_ML_CTRL_TYPE_BITS);
  2573. if (variant != WLAN_ML_VARIANT_BASIC)
  2574. return QDF_STATUS_E_NOSUPPORT;
  2575. presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  2576. WLAN_ML_CTRL_PBM_BITS);
  2577. commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
  2578. commoninfolen = WLAN_ML_BV_CINFO_LENGTH_SIZE;
  2579. commoninfolen += QDF_MAC_ADDR_SIZE;
  2580. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
  2581. commoninfolen += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
  2582. if ((sizeof(struct wlan_ie_multilink) + commoninfolen) >
  2583. mlieseqlen)
  2584. return QDF_STATUS_E_PROTO;
  2585. }
  2586. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) {
  2587. *bssparamchangecntfound = true;
  2588. *bssparamchangecnt = *(commoninfo + commoninfolen);
  2589. }
  2590. return QDF_STATUS_SUCCESS;
  2591. }
  2592. QDF_STATUS
  2593. util_get_mlie_variant(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2594. int *variant)
  2595. {
  2596. struct wlan_ie_multilink *mlie_fixed;
  2597. enum wlan_ml_variant var;
  2598. uint16_t mlcontrol;
  2599. if (!mlieseq || !mlieseqlen || !variant)
  2600. return QDF_STATUS_E_NULL_VALUE;
  2601. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2602. return QDF_STATUS_E_INVAL;
  2603. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2604. if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
  2605. (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
  2606. return QDF_STATUS_E_INVAL;
  2607. mlcontrol = le16toh(mlie_fixed->mlcontrol);
  2608. var = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2609. WLAN_ML_CTRL_TYPE_BITS);
  2610. if (var >= WLAN_ML_VARIANT_INVALIDSTART)
  2611. return QDF_STATUS_E_PROTO;
  2612. *variant = var;
  2613. return QDF_STATUS_SUCCESS;
  2614. }
  2615. QDF_STATUS
  2616. util_get_bvmlie_eml_cap(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2617. bool *eml_cap_found,
  2618. uint16_t *eml_cap)
  2619. {
  2620. struct wlan_ie_multilink *mlie_fixed;
  2621. enum wlan_ml_variant variant;
  2622. uint16_t mlcontrol;
  2623. uint8_t eml_cap_offset;
  2624. uint8_t commoninfo_len;
  2625. uint16_t presencebitmap;
  2626. if (!mlieseq || !mlieseqlen || !eml_cap_found || !eml_cap)
  2627. return QDF_STATUS_E_NULL_VALUE;
  2628. *eml_cap = 0;
  2629. *eml_cap_found = false;
  2630. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2631. return QDF_STATUS_E_INVAL;
  2632. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2633. if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
  2634. (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
  2635. return QDF_STATUS_E_INVAL;
  2636. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  2637. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2638. WLAN_ML_CTRL_TYPE_BITS);
  2639. if (variant != WLAN_ML_VARIANT_BASIC)
  2640. return QDF_STATUS_E_INVAL;
  2641. presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  2642. WLAN_ML_CTRL_PBM_BITS);
  2643. /* eml_cap_offset stores the offset of EML Capabilities within
  2644. * Common Info
  2645. */
  2646. eml_cap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE + QDF_MAC_ADDR_SIZE;
  2647. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P)
  2648. eml_cap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
  2649. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P)
  2650. eml_cap_offset += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
  2651. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P)
  2652. eml_cap_offset += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE;
  2653. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) {
  2654. /* Common Info starts at
  2655. * mlieseq + sizeof(struct wlan_ie_multilink).
  2656. * Check if there is sufficient space in the buffer for
  2657. * the Common Info Length.
  2658. */
  2659. if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
  2660. WLAN_ML_BV_CINFO_LENGTH_SIZE))
  2661. return QDF_STATUS_E_PROTO;
  2662. /* Check if the value indicated in the Common Info Length
  2663. * subfield is sufficient to access the EML capabilities.
  2664. */
  2665. commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
  2666. if (commoninfo_len < (eml_cap_offset +
  2667. WLAN_ML_BV_CINFO_EMLCAP_SIZE))
  2668. return QDF_STATUS_E_PROTO;
  2669. /* Common Info starts at mlieseq + sizeof(struct
  2670. * wlan_ie_multilink). Check if there is sufficient space in
  2671. * Common Info for the EML capability.
  2672. */
  2673. if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
  2674. eml_cap_offset +
  2675. WLAN_ML_BV_CINFO_EMLCAP_SIZE))
  2676. return QDF_STATUS_E_PROTO;
  2677. *eml_cap_found = true;
  2678. *eml_cap = qdf_le16_to_cpu(*(uint16_t *)(mlieseq +
  2679. sizeof(struct wlan_ie_multilink) +
  2680. eml_cap_offset));
  2681. }
  2682. return QDF_STATUS_SUCCESS;
  2683. }
  2684. QDF_STATUS
  2685. util_get_bvmlie_msd_cap(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2686. bool *msd_cap_found,
  2687. uint16_t *msd_cap)
  2688. {
  2689. struct wlan_ie_multilink *mlie_fixed;
  2690. enum wlan_ml_variant variant;
  2691. uint16_t mlcontrol;
  2692. uint8_t msd_cap_offset;
  2693. uint8_t commoninfo_len;
  2694. uint16_t presencebitmap;
  2695. if (!mlieseq || !mlieseqlen || !msd_cap_found || !msd_cap)
  2696. return QDF_STATUS_E_NULL_VALUE;
  2697. *msd_cap = 0;
  2698. *msd_cap_found = false;
  2699. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2700. return QDF_STATUS_E_INVAL;
  2701. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2702. if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
  2703. (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
  2704. return QDF_STATUS_E_INVAL;
  2705. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  2706. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2707. WLAN_ML_CTRL_TYPE_BITS);
  2708. if (variant != WLAN_ML_VARIANT_BASIC)
  2709. return QDF_STATUS_E_INVAL;
  2710. presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  2711. WLAN_ML_CTRL_PBM_BITS);
  2712. /* msd_cap_offset stores the offset of MSD capabilities within
  2713. * Common Info
  2714. */
  2715. msd_cap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE + QDF_MAC_ADDR_SIZE;
  2716. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P)
  2717. msd_cap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
  2718. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P)
  2719. msd_cap_offset += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
  2720. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) {
  2721. /* Common Info starts at
  2722. * mlieseq + sizeof(struct wlan_ie_multilink).
  2723. * Check if there is sufficient space in the buffer for
  2724. * the Common Info Length.
  2725. */
  2726. if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
  2727. WLAN_ML_BV_CINFO_LENGTH_SIZE))
  2728. return QDF_STATUS_E_PROTO;
  2729. /* Check if the value indicated in the Common Info Length
  2730. * subfield is sufficient to access the MSD capabilities.
  2731. */
  2732. commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
  2733. if (commoninfo_len < (msd_cap_offset +
  2734. WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE))
  2735. return QDF_STATUS_E_PROTO;
  2736. /* Common Info starts at mlieseq + sizeof(struct
  2737. * wlan_ie_multilink). Check if there is sufficient space in
  2738. * Common Info for the MSD capability.
  2739. */
  2740. if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
  2741. msd_cap_offset +
  2742. WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE))
  2743. return QDF_STATUS_E_PROTO;
  2744. *msd_cap_found = true;
  2745. *msd_cap = qdf_le16_to_cpu(*(uint16_t *)(mlieseq +
  2746. sizeof(struct wlan_ie_multilink) +
  2747. msd_cap_offset));
  2748. } else {
  2749. mlo_debug("MSD caps not found in assoc rsp");
  2750. }
  2751. return QDF_STATUS_SUCCESS;
  2752. }
  2753. QDF_STATUS
  2754. util_get_bvmlie_mldmacaddr(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2755. struct qdf_mac_addr *mldmacaddr)
  2756. {
  2757. struct wlan_ie_multilink *mlie_fixed;
  2758. enum wlan_ml_variant variant;
  2759. uint16_t mlcontrol;
  2760. uint8_t commoninfo_len;
  2761. if (!mlieseq || !mlieseqlen || !mldmacaddr)
  2762. return QDF_STATUS_E_NULL_VALUE;
  2763. qdf_mem_zero(mldmacaddr, sizeof(*mldmacaddr));
  2764. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2765. return QDF_STATUS_E_INVAL;
  2766. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2767. if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
  2768. (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
  2769. return QDF_STATUS_E_INVAL;
  2770. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  2771. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2772. WLAN_ML_CTRL_TYPE_BITS);
  2773. if (variant != WLAN_ML_VARIANT_BASIC)
  2774. return QDF_STATUS_E_INVAL;
  2775. /* Common Info starts at mlieseq + sizeof(struct wlan_ie_multilink).
  2776. * Check if there is sufficient space in the buffer for the Common Info
  2777. * Length and MLD MAC address.
  2778. */
  2779. if ((sizeof(struct wlan_ie_multilink) + WLAN_ML_BV_CINFO_LENGTH_SIZE +
  2780. QDF_MAC_ADDR_SIZE) > mlieseqlen)
  2781. return QDF_STATUS_E_PROTO;
  2782. /* Check if the value indicated in the Common Info Length subfield is
  2783. * sufficient to access the MLD MAC address.
  2784. */
  2785. commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
  2786. if (commoninfo_len < (WLAN_ML_BV_CINFO_LENGTH_SIZE + QDF_MAC_ADDR_SIZE))
  2787. return QDF_STATUS_E_PROTO;
  2788. qdf_mem_copy(mldmacaddr->bytes,
  2789. mlieseq + sizeof(struct wlan_ie_multilink) +
  2790. WLAN_ML_BV_CINFO_LENGTH_SIZE,
  2791. QDF_MAC_ADDR_SIZE);
  2792. return QDF_STATUS_SUCCESS;
  2793. }
  2794. QDF_STATUS
  2795. util_get_bvmlie_primary_linkid(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2796. bool *linkidfound, uint8_t *linkid)
  2797. {
  2798. struct wlan_ie_multilink *mlie_fixed;
  2799. enum wlan_ml_variant variant;
  2800. uint16_t mlcontrol;
  2801. uint16_t presencebitmap;
  2802. uint8_t *commoninfo;
  2803. qdf_size_t commoninfolen;
  2804. uint8_t *linkidinfo;
  2805. if (!mlieseq || !mlieseqlen || !linkidfound || !linkid)
  2806. return QDF_STATUS_E_NULL_VALUE;
  2807. *linkidfound = false;
  2808. *linkid = 0;
  2809. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2810. return QDF_STATUS_E_INVAL;
  2811. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2812. if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
  2813. (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
  2814. return QDF_STATUS_E_INVAL;
  2815. mlcontrol = le16toh(mlie_fixed->mlcontrol);
  2816. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2817. WLAN_ML_CTRL_TYPE_BITS);
  2818. if (variant != WLAN_ML_VARIANT_BASIC)
  2819. return QDF_STATUS_E_INVAL;
  2820. presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  2821. WLAN_ML_CTRL_PBM_BITS);
  2822. commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
  2823. commoninfolen = 0;
  2824. commoninfolen += WLAN_ML_BV_CINFO_LENGTH_SIZE;
  2825. if ((sizeof(struct wlan_ie_multilink) + commoninfolen) >
  2826. mlieseqlen)
  2827. return QDF_STATUS_E_PROTO;
  2828. commoninfolen += QDF_MAC_ADDR_SIZE;
  2829. if ((sizeof(struct wlan_ie_multilink) + commoninfolen) >
  2830. mlieseqlen)
  2831. return QDF_STATUS_E_PROTO;
  2832. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
  2833. linkidinfo = commoninfo + commoninfolen;
  2834. commoninfolen += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
  2835. if ((sizeof(struct wlan_ie_multilink) + commoninfolen) >
  2836. mlieseqlen)
  2837. return QDF_STATUS_E_PROTO;
  2838. *linkidfound = true;
  2839. *linkid = QDF_GET_BITS(linkidinfo[0],
  2840. WLAN_ML_BV_CINFO_LINKIDINFO_LINKID_IDX,
  2841. WLAN_ML_BV_CINFO_LINKIDINFO_LINKID_BITS);
  2842. }
  2843. return QDF_STATUS_SUCCESS;
  2844. }
  2845. QDF_STATUS
  2846. util_get_bvmlie_mldcap(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  2847. bool *mldcapfound, uint16_t *mldcap)
  2848. {
  2849. struct wlan_ie_multilink *mlie_fixed;
  2850. enum wlan_ml_variant variant;
  2851. uint16_t mlcontrol;
  2852. uint16_t presencebitmap;
  2853. uint8_t *commoninfo;
  2854. uint8_t commoninfo_len;
  2855. qdf_size_t mldcap_offset;
  2856. if (!mlieseq || !mlieseqlen || !mldcapfound || !mldcap)
  2857. return QDF_STATUS_E_NULL_VALUE;
  2858. *mldcapfound = false;
  2859. *mldcap = 0;
  2860. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  2861. return QDF_STATUS_E_INVAL;
  2862. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2863. if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
  2864. mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
  2865. return QDF_STATUS_E_INVAL;
  2866. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  2867. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2868. WLAN_ML_CTRL_TYPE_BITS);
  2869. if (variant != WLAN_ML_VARIANT_BASIC)
  2870. return QDF_STATUS_E_NOSUPPORT;
  2871. presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  2872. WLAN_ML_CTRL_PBM_BITS);
  2873. commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
  2874. commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
  2875. /* mldcap_offset stores the offset of MLD Capabilities within
  2876. * Common Info
  2877. */
  2878. mldcap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE;
  2879. mldcap_offset += QDF_MAC_ADDR_SIZE;
  2880. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
  2881. mldcap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
  2882. if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
  2883. mlieseqlen)
  2884. return QDF_STATUS_E_PROTO;
  2885. }
  2886. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) {
  2887. mldcap_offset += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
  2888. if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
  2889. mlieseqlen)
  2890. return QDF_STATUS_E_PROTO;
  2891. }
  2892. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) {
  2893. mldcap_offset += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE;
  2894. if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
  2895. mlieseqlen)
  2896. return QDF_STATUS_E_PROTO;
  2897. }
  2898. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) {
  2899. mldcap_offset += WLAN_ML_BV_CINFO_EMLCAP_SIZE;
  2900. if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
  2901. mlieseqlen)
  2902. return QDF_STATUS_E_PROTO;
  2903. }
  2904. if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MLDCAPANDOP_P) {
  2905. /* Check if the value indicated in the Common Info Length
  2906. * subfield is sufficient to access the MLD capabilities.
  2907. */
  2908. if (commoninfo_len < (mldcap_offset +
  2909. WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE))
  2910. return QDF_STATUS_E_PROTO;
  2911. if ((sizeof(struct wlan_ie_multilink) + mldcap_offset +
  2912. WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE) >
  2913. mlieseqlen)
  2914. return QDF_STATUS_E_PROTO;
  2915. *mldcap = qdf_le16_to_cpu(*((uint16_t *)(commoninfo + mldcap_offset)));
  2916. *mldcapfound = true;
  2917. }
  2918. return QDF_STATUS_SUCCESS;
  2919. }
  2920. QDF_STATUS
  2921. util_get_bvmlie_persta_partner_info(uint8_t *mlieseq,
  2922. qdf_size_t mlieseqlen,
  2923. struct mlo_partner_info *partner_info)
  2924. {
  2925. struct wlan_ie_multilink *mlie_fixed;
  2926. uint16_t mlcontrol;
  2927. enum wlan_ml_variant variant;
  2928. uint8_t *linkinfo;
  2929. qdf_size_t linkinfo_len;
  2930. struct mlo_partner_info pinfo = {0};
  2931. qdf_size_t mlieseqpayloadlen;
  2932. uint8_t *mlieseqpayload_copy;
  2933. bool is_elemfragseq;
  2934. qdf_size_t defragpayload_len;
  2935. qdf_size_t tmplen;
  2936. QDF_STATUS ret;
  2937. if (!mlieseq) {
  2938. mlo_err("Pointer to Multi-Link element sequence is NULL");
  2939. return QDF_STATUS_E_NULL_VALUE;
  2940. }
  2941. if (!mlieseqlen) {
  2942. mlo_err("Length of Multi-Link element sequence is zero");
  2943. return QDF_STATUS_E_INVAL;
  2944. }
  2945. if (!partner_info) {
  2946. mlo_err("partner_info is NULL");
  2947. return QDF_STATUS_E_NULL_VALUE;
  2948. }
  2949. partner_info->num_partner_links = 0;
  2950. if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
  2951. mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
  2952. mlieseqlen, sizeof(struct wlan_ie_multilink));
  2953. return QDF_STATUS_E_INVAL;
  2954. }
  2955. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  2956. if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
  2957. (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) {
  2958. mlo_err("The element is not a Multi-Link element");
  2959. return QDF_STATUS_E_INVAL;
  2960. }
  2961. mlcontrol = le16toh(mlie_fixed->mlcontrol);
  2962. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  2963. WLAN_ML_CTRL_TYPE_BITS);
  2964. if (variant != WLAN_ML_VARIANT_BASIC) {
  2965. mlo_err("The variant value %u does not correspond to Basic Variant value %u",
  2966. variant, WLAN_ML_VARIANT_BASIC);
  2967. return QDF_STATUS_E_INVAL;
  2968. }
  2969. mlieseqpayloadlen = 0;
  2970. tmplen = 0;
  2971. is_elemfragseq = false;
  2972. ret = wlan_get_elem_fragseq_info(mlieseq,
  2973. mlieseqlen,
  2974. &is_elemfragseq,
  2975. &tmplen,
  2976. &mlieseqpayloadlen);
  2977. if (QDF_IS_STATUS_ERROR(ret))
  2978. return ret;
  2979. if (is_elemfragseq) {
  2980. if (tmplen != mlieseqlen) {
  2981. mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val passed as arg: %zu octets",
  2982. tmplen, mlieseqlen);
  2983. return QDF_STATUS_E_INVAL;
  2984. }
  2985. if (!mlieseqpayloadlen) {
  2986. mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
  2987. return QDF_STATUS_E_FAILURE;
  2988. }
  2989. mlo_debug("Multi-Link element fragment sequence found with payload len %zu",
  2990. mlieseqpayloadlen);
  2991. } else {
  2992. if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
  2993. mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
  2994. mlieseqlen,
  2995. sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
  2996. return QDF_STATUS_E_FAILURE;
  2997. }
  2998. mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
  2999. }
  3000. mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
  3001. if (!mlieseqpayload_copy) {
  3002. mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
  3003. return QDF_STATUS_E_NOMEM;
  3004. }
  3005. if (is_elemfragseq) {
  3006. ret = wlan_defrag_elem_fragseq(false,
  3007. mlieseq,
  3008. mlieseqlen,
  3009. mlieseqpayload_copy,
  3010. mlieseqpayloadlen,
  3011. &defragpayload_len);
  3012. if (QDF_IS_STATUS_ERROR(ret)) {
  3013. qdf_mem_free(mlieseqpayload_copy);
  3014. return ret;
  3015. }
  3016. if (defragpayload_len != mlieseqpayloadlen) {
  3017. mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
  3018. defragpayload_len, mlieseqpayloadlen);
  3019. qdf_mem_free(mlieseqpayload_copy);
  3020. return QDF_STATUS_E_FAILURE;
  3021. }
  3022. } else {
  3023. qdf_mem_copy(mlieseqpayload_copy,
  3024. mlieseq + sizeof(struct ie_header) + 1,
  3025. mlieseqpayloadlen);
  3026. }
  3027. linkinfo = NULL;
  3028. linkinfo_len = 0;
  3029. ret = util_parse_multi_link_ctrl(mlieseqpayload_copy,
  3030. mlieseqpayloadlen,
  3031. &linkinfo,
  3032. &linkinfo_len);
  3033. if (QDF_IS_STATUS_ERROR(ret)) {
  3034. qdf_mem_free(mlieseqpayload_copy);
  3035. return ret;
  3036. }
  3037. /*
  3038. * If Probe Request variant Multi-Link element in the Multi-Link probe
  3039. * request does not include any per-STA profile, then all APs affiliated
  3040. * with the same AP MLD as the AP identified in the Addr 1 or Addr 3
  3041. * field or AP MLD ID of the Multi-Link probe request are requested
  3042. * APs return success here
  3043. */
  3044. if (!linkinfo) {
  3045. qdf_mem_free(mlieseqpayload_copy);
  3046. return QDF_STATUS_SUCCESS;
  3047. }
  3048. ret = util_parse_partner_info_from_linkinfo(linkinfo,
  3049. linkinfo_len,
  3050. &pinfo);
  3051. if (QDF_IS_STATUS_ERROR(ret)) {
  3052. qdf_mem_free(mlieseqpayload_copy);
  3053. return ret;
  3054. }
  3055. qdf_mem_copy(partner_info, &pinfo, sizeof(*partner_info));
  3056. qdf_mem_free(mlieseqpayload_copy);
  3057. return QDF_STATUS_SUCCESS;
  3058. }
  3059. QDF_STATUS
  3060. util_get_prvmlie_persta_link_id(uint8_t *mlieseq,
  3061. qdf_size_t mlieseqlen,
  3062. struct mlo_probereq_info *probereq_info)
  3063. {
  3064. struct wlan_ie_multilink *mlie_fixed;
  3065. uint16_t mlcontrol;
  3066. enum wlan_ml_variant variant;
  3067. uint8_t *linkinfo;
  3068. qdf_size_t linkinfo_len;
  3069. qdf_size_t mlieseqpayloadlen;
  3070. uint8_t *mlieseqpayload_copy;
  3071. bool is_elemfragseq;
  3072. qdf_size_t defragpayload_len;
  3073. qdf_size_t tmplen;
  3074. QDF_STATUS ret;
  3075. if (!mlieseq) {
  3076. mlo_err("Pointer to Multi-Link element sequence is NULL");
  3077. return QDF_STATUS_E_NULL_VALUE;
  3078. }
  3079. if (!mlieseqlen) {
  3080. mlo_err("Length of Multi-Link element sequence is zero");
  3081. return QDF_STATUS_E_INVAL;
  3082. }
  3083. if (!probereq_info) {
  3084. mlo_err("probe request_info is NULL");
  3085. return QDF_STATUS_E_NULL_VALUE;
  3086. }
  3087. probereq_info->num_links = 0;
  3088. if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
  3089. mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
  3090. mlieseqlen, sizeof(struct wlan_ie_multilink));
  3091. return QDF_STATUS_E_INVAL;
  3092. }
  3093. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  3094. if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
  3095. (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) {
  3096. mlo_err("The element is not a Multi-Link element");
  3097. return QDF_STATUS_E_INVAL;
  3098. }
  3099. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  3100. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  3101. WLAN_ML_CTRL_TYPE_BITS);
  3102. if (variant != WLAN_ML_VARIANT_PROBEREQ) {
  3103. mlo_err("The variant value %u does not correspond to Probe Request Variant value %u",
  3104. variant, WLAN_ML_VARIANT_PROBEREQ);
  3105. return QDF_STATUS_E_INVAL;
  3106. }
  3107. mlieseqpayloadlen = 0;
  3108. tmplen = 0;
  3109. is_elemfragseq = false;
  3110. ret = wlan_get_elem_fragseq_info(mlieseq,
  3111. mlieseqlen,
  3112. &is_elemfragseq,
  3113. &tmplen,
  3114. &mlieseqpayloadlen);
  3115. if (QDF_IS_STATUS_ERROR(ret))
  3116. return ret;
  3117. if (is_elemfragseq) {
  3118. if (tmplen != mlieseqlen) {
  3119. mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val passed as arg: %zu octets",
  3120. tmplen, mlieseqlen);
  3121. return QDF_STATUS_E_INVAL;
  3122. }
  3123. if (!mlieseqpayloadlen) {
  3124. mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
  3125. return QDF_STATUS_E_FAILURE;
  3126. }
  3127. mlo_debug("Multi-Link element fragment sequence found with payload len %zu",
  3128. mlieseqpayloadlen);
  3129. } else {
  3130. if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
  3131. mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
  3132. mlieseqlen,
  3133. sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
  3134. return QDF_STATUS_E_FAILURE;
  3135. }
  3136. mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
  3137. }
  3138. mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
  3139. if (!mlieseqpayload_copy) {
  3140. mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
  3141. return QDF_STATUS_E_NOMEM;
  3142. }
  3143. if (is_elemfragseq) {
  3144. ret = wlan_defrag_elem_fragseq(false,
  3145. mlieseq,
  3146. mlieseqlen,
  3147. mlieseqpayload_copy,
  3148. mlieseqpayloadlen,
  3149. &defragpayload_len);
  3150. if (QDF_IS_STATUS_ERROR(ret)) {
  3151. qdf_mem_free(mlieseqpayload_copy);
  3152. return ret;
  3153. }
  3154. if (defragpayload_len != mlieseqpayloadlen) {
  3155. mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
  3156. defragpayload_len, mlieseqpayloadlen);
  3157. qdf_mem_free(mlieseqpayload_copy);
  3158. return QDF_STATUS_E_FAILURE;
  3159. }
  3160. } else {
  3161. qdf_mem_copy(mlieseqpayload_copy,
  3162. mlieseq + sizeof(struct ie_header) + 1,
  3163. mlieseqpayloadlen);
  3164. }
  3165. linkinfo = NULL;
  3166. linkinfo_len = 0;
  3167. ret = util_parse_prv_multi_link_ctrl(mlieseqpayload_copy,
  3168. mlieseqpayloadlen,
  3169. &linkinfo,
  3170. &linkinfo_len);
  3171. if (QDF_IS_STATUS_ERROR(ret)) {
  3172. qdf_mem_free(mlieseqpayload_copy);
  3173. return ret;
  3174. }
  3175. /* In case Link Info is absent, the number of links will remain
  3176. * zero.
  3177. */
  3178. if (!linkinfo) {
  3179. mlo_debug("No link info present");
  3180. qdf_mem_free(mlieseqpayload_copy);
  3181. return QDF_STATUS_SUCCESS;
  3182. }
  3183. ret = util_parse_probereq_info_from_linkinfo(linkinfo,
  3184. linkinfo_len,
  3185. probereq_info);
  3186. if (QDF_IS_STATUS_ERROR(ret)) {
  3187. qdf_mem_free(mlieseqpayload_copy);
  3188. return ret;
  3189. }
  3190. qdf_mem_free(mlieseqpayload_copy);
  3191. return QDF_STATUS_SUCCESS;
  3192. }
  3193. QDF_STATUS
  3194. util_get_prvmlie_mldid(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  3195. bool *mldidfound, uint8_t *mldid)
  3196. {
  3197. struct wlan_ie_multilink *mlie_fixed;
  3198. enum wlan_ml_variant variant;
  3199. uint16_t mlcontrol;
  3200. uint16_t presencebitmap;
  3201. uint8_t *commoninfo;
  3202. qdf_size_t commoninfolen;
  3203. if (!mlieseq || !mlieseqlen || !mldidfound || !mldid)
  3204. return QDF_STATUS_E_NULL_VALUE;
  3205. *mldidfound = false;
  3206. *mldid = 0;
  3207. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  3208. return QDF_STATUS_E_INVAL;
  3209. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  3210. if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
  3211. mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
  3212. return QDF_STATUS_E_INVAL;
  3213. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  3214. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  3215. WLAN_ML_CTRL_TYPE_BITS);
  3216. if (variant != WLAN_ML_VARIANT_PROBEREQ)
  3217. return QDF_STATUS_E_NOSUPPORT;
  3218. presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  3219. WLAN_ML_CTRL_PBM_BITS);
  3220. commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
  3221. commoninfolen = WLAN_ML_PRV_CINFO_LENGTH_SIZE;
  3222. if (presencebitmap & WLAN_ML_PRV_CTRL_PBM_MLDID_P) {
  3223. if ((sizeof(struct wlan_ie_multilink) + commoninfolen +
  3224. WLAN_ML_PRV_CINFO_MLDID_SIZE) >
  3225. mlieseqlen)
  3226. return QDF_STATUS_E_PROTO;
  3227. *mldid = *((uint8_t *)(commoninfo + commoninfolen));
  3228. commoninfolen += WLAN_ML_PRV_CINFO_MLDID_SIZE;
  3229. *mldidfound = true;
  3230. }
  3231. return QDF_STATUS_SUCCESS;
  3232. }
  3233. QDF_STATUS util_get_rvmlie_mldmacaddr(uint8_t *mlieseq, qdf_size_t mlieseqlen,
  3234. struct qdf_mac_addr *mldmacaddr)
  3235. {
  3236. struct wlan_ie_multilink *mlie_fixed;
  3237. enum wlan_ml_variant variant;
  3238. uint16_t mlcontrol;
  3239. uint16_t presencebitmap;
  3240. if (!mlieseq || !mlieseqlen || !mldmacaddr)
  3241. return QDF_STATUS_E_NULL_VALUE;
  3242. qdf_mem_zero(mldmacaddr, sizeof(*mldmacaddr));
  3243. if (mlieseqlen < sizeof(struct wlan_ie_multilink))
  3244. return QDF_STATUS_E_INVAL;
  3245. mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
  3246. if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
  3247. mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
  3248. return QDF_STATUS_E_INVAL;
  3249. mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
  3250. variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
  3251. WLAN_ML_CTRL_TYPE_BITS);
  3252. if (variant != WLAN_ML_VARIANT_RECONFIG)
  3253. return QDF_STATUS_E_INVAL;
  3254. presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
  3255. WLAN_ML_CTRL_PBM_BITS);
  3256. /* Check if MLD mac address is present */
  3257. if (presencebitmap & WLAN_ML_RV_CTRL_PBM_MLDMACADDR_P) {
  3258. if ((sizeof(struct wlan_ie_multilink) + QDF_MAC_ADDR_SIZE) >
  3259. mlieseqlen)
  3260. return QDF_STATUS_E_PROTO;
  3261. qdf_mem_copy(mldmacaddr->bytes,
  3262. mlieseq + sizeof(struct wlan_ie_multilink),
  3263. QDF_MAC_ADDR_SIZE);
  3264. }
  3265. return QDF_STATUS_SUCCESS;
  3266. }
  3267. #endif