@@ -2966,6 +2966,51 @@ mwl8k_cmd_rf_antenna(struct ieee80211_hw *hw, int antenna, int mask)
29662966/*
29672967 * CMD_SET_BEACON.
29682968 */
2969+
2970+ static bool mwl8k_beacon_has_ds_params (const u8 * buf , int len )
2971+ {
2972+ const struct ieee80211_mgmt * mgmt = (const void * )buf ;
2973+ int ies_len ;
2974+
2975+ if (len <= offsetof(struct ieee80211_mgmt , u .beacon .variable ))
2976+ return false;
2977+
2978+ ies_len = len - offsetof(struct ieee80211_mgmt , u .beacon .variable );
2979+
2980+ return cfg80211_find_ie (WLAN_EID_DS_PARAMS , mgmt -> u .beacon .variable ,
2981+ ies_len ) != NULL ;
2982+ }
2983+
2984+ static void mwl8k_beacon_copy_inject_ds_params (struct ieee80211_hw * hw ,
2985+ u8 * buf_dst , const u8 * buf_src ,
2986+ int src_len )
2987+ {
2988+ const struct ieee80211_mgmt * mgmt = (const void * )buf_src ;
2989+ static const u8 before_ds_params [] = {
2990+ WLAN_EID_SSID ,
2991+ WLAN_EID_SUPP_RATES ,
2992+ };
2993+ const u8 * ies ;
2994+ int hdr_len , left , offs , pos ;
2995+
2996+ ies = mgmt -> u .beacon .variable ;
2997+ hdr_len = offsetof(struct ieee80211_mgmt , u .beacon .variable );
2998+
2999+ offs = ieee80211_ie_split (ies , src_len - hdr_len , before_ds_params ,
3000+ ARRAY_SIZE (before_ds_params ), 0 );
3001+
3002+ pos = hdr_len + offs ;
3003+ left = src_len - pos ;
3004+
3005+ memcpy (buf_dst , buf_src , pos );
3006+
3007+ /* Inject a DSSS Parameter Set after SSID + Supp Rates */
3008+ buf_dst [pos + 0 ] = WLAN_EID_DS_PARAMS ;
3009+ buf_dst [pos + 1 ] = 1 ;
3010+ buf_dst [pos + 2 ] = hw -> conf .chandef .chan -> hw_value ;
3011+
3012+ memcpy (buf_dst + pos + 3 , buf_src + pos , left );
3013+ }
29693014struct mwl8k_cmd_set_beacon {
29703015 struct mwl8k_cmd_pkt_hdr header ;
29713016 __le16 beacon_len ;
@@ -2975,17 +3020,33 @@ struct mwl8k_cmd_set_beacon {
29753020static int mwl8k_cmd_set_beacon (struct ieee80211_hw * hw ,
29763021 struct ieee80211_vif * vif , u8 * beacon , int len )
29773022{
3023+ bool ds_params_present = mwl8k_beacon_has_ds_params (beacon , len );
29783024 struct mwl8k_cmd_set_beacon * cmd ;
2979- int rc ;
3025+ int rc , final_len = len ;
29803026
2981- cmd = kzalloc (sizeof (* cmd ) + len , GFP_KERNEL );
3027+ if (!ds_params_present ) {
3028+ /*
3029+ * mwl8k firmware requires a DS Params IE with the current
3030+ * channel in AP beacons. If mac80211/hostapd does not
3031+ * include it, inject one here. IE ID + length + channel
3032+ * number = 3 bytes.
3033+ */
3034+ final_len += 3 ;
3035+ }
3036+
3037+ cmd = kzalloc (sizeof (* cmd ) + final_len , GFP_KERNEL );
29823038 if (cmd == NULL )
29833039 return - ENOMEM ;
29843040
29853041 cmd -> header .code = cpu_to_le16 (MWL8K_CMD_SET_BEACON );
2986- cmd -> header .length = cpu_to_le16 (sizeof (* cmd ) + len );
2987- cmd -> beacon_len = cpu_to_le16 (len );
2988- memcpy (cmd -> beacon , beacon , len );
3042+ cmd -> header .length = cpu_to_le16 (sizeof (* cmd ) + final_len );
3043+ cmd -> beacon_len = cpu_to_le16 (final_len );
3044+
3045+ if (ds_params_present )
3046+ memcpy (cmd -> beacon , beacon , len );
3047+ else
3048+ mwl8k_beacon_copy_inject_ds_params (hw , cmd -> beacon , beacon ,
3049+ len );
29893050
29903051 rc = mwl8k_post_pervif_cmd (hw , vif , & cmd -> header );
29913052 kfree (cmd );
0 commit comments