qca-cld:lim: Send change OP mode message to FW

For HT20/40 mode switch, check associated station's HT capability
before sending change OP mode message to FW.

Change-Id: Iad4f7fde03eb8399db9e17087b9720b8221c7fb8
CRs-fixed: 697592

2 files changed, 64 insertions, 21 deletions

diff --git a/CORE/HDD/src/wlan_hdd_cfg80211.c b/CORE/HDD/src/wlan_hdd_cfg80211.c
index fcb7ff4232f8..3584a6e958b7 100644
--- a/CORE/HDD/src/wlan_hdd_cfg80211.c
+++ b/CORE/HDD/src/wlan_hdd_cfg80211.c
@@ -13888,7 +13888,6 @@ int wlan_hdd_cfg80211_set_ap_channel_width(struct wiphy *wiphy,
     hdd_context_t *pHddCtx;
     VOS_STATUS status;
     tSmeConfigParams smeConfig;
-    int i;
     bool cbModeChange;
 
     if (NULL == wiphy) {
@@ -13913,16 +13912,24 @@ int wlan_hdd_cfg80211_set_ap_channel_width(struct wiphy *wiphy,
     sme_GetConfigParam(pHddCtx->hHal, &smeConfig);
     switch (chandef->width) {
     case NL80211_CHAN_WIDTH_20:
-        if (smeConfig.csrConfig.channelBondingMode24GHz != 0) {
-            smeConfig.csrConfig.channelBondingMode24GHz = 0;
+        if (smeConfig.csrConfig.channelBondingMode24GHz !=
+                      eCSR_INI_SINGLE_CHANNEL_CENTERED) {
+            smeConfig.csrConfig.channelBondingMode24GHz =
+                      eCSR_INI_SINGLE_CHANNEL_CENTERED;
             sme_UpdateConfig(pHddCtx->hHal, &smeConfig);
             cbModeChange = TRUE;
         }
         break;
 
     case NL80211_CHAN_WIDTH_40:
-        if (smeConfig.csrConfig.channelBondingMode24GHz != 1) {
-            smeConfig.csrConfig.channelBondingMode24GHz = 1;
+        if (smeConfig.csrConfig.channelBondingMode24GHz ==
+                      eCSR_INI_SINGLE_CHANNEL_CENTERED) {
+            if ( NL80211_CHAN_HT40MINUS == cfg80211_get_chandef_type(chandef))
+                smeConfig.csrConfig.channelBondingMode24GHz =
+                      eCSR_INI_DOUBLE_CHANNEL_HIGH_PRIMARY;
+            else
+                smeConfig.csrConfig.channelBondingMode24GHz =
+                      eCSR_INI_DOUBLE_CHANNEL_LOW_PRIMARY;
             sme_UpdateConfig(pHddCtx->hHal, &smeConfig);
             cbModeChange = TRUE;
         }
@@ -13954,22 +13961,6 @@ int wlan_hdd_cfg80211_set_ap_channel_width(struct wiphy *wiphy,
         return -EINVAL;
     }
 
-    for (i = 0; i < WLAN_MAX_STA_COUNT; i++) {
-        if (!pAdapter->aStaInfo[i].isUsed)
-            continue;
-
-        status = hdd_wlan_set_ht2040_mode(pAdapter,
-                                          pAdapter->aStaInfo[i].ucSTAId,
-                                          pAdapter->aStaInfo[i].macAddrSTA,
-                                          cfg80211_get_chandef_type(chandef));
-        if (status != VOS_STATUS_SUCCESS) {
-                    hddLog(VOS_TRACE_LEVEL_ERROR,
-                    "%s:Error!!! Cannot set HT20/40 mode for STA %d!",
-                    __func__, pAdapter->aStaInfo[i].ucSTAId);
-            return -EINVAL;
-        }
-    }
-
     return 0;
 }
 #endif
diff --git a/CORE/MAC/src/pe/lim/limProcessSmeReqMessages.c b/CORE/MAC/src/pe/lim/limProcessSmeReqMessages.c
index 341f9ec6a827..8fc67f4288f8 100644
--- a/CORE/MAC/src/pe/lim/limProcessSmeReqMessages.c
+++ b/CORE/MAC/src/pe/lim/limProcessSmeReqMessages.c
@@ -4810,6 +4810,10 @@ static void __limProcessSmeSetHT2040Mode(tpAniSirGlobal pMac, tANI_U32 *pMsgBuf)
     tpSirSetHT2040Mode     pSetHT2040Mode;
     tpPESession             psessionEntry;
     tANI_U8  sessionId = 0;
+    vos_msg_t msg;
+    tUpdateVHTOpMode *pHtOpMode = NULL;
+    tANI_U16                staId = 0;
+    tpDphHashNode pStaDs = NULL;
 
     PELOG1(limLog(pMac, LOG1,
            FL("received Set HT 20/40 mode message")););
@@ -4851,6 +4855,54 @@ static void __limProcessSmeSetHT2040Mode(tpAniSirGlobal pMac, tANI_U32 *pMsgBuf)
     /* Update beacon */
     schSetFixedBeaconFields(pMac, psessionEntry);
     limSendBeaconInd(pMac, psessionEntry);
+
+    /* update OP Mode for each associated peer */
+    for (staId = 0; staId < psessionEntry->dph.dphHashTable.size; staId++)
+    {
+        pStaDs = dphGetHashEntry(pMac, staId, &psessionEntry->dph.dphHashTable);
+        if (NULL == pStaDs)
+            continue;
+
+        if (pStaDs->valid && pStaDs->htSupportedChannelWidthSet)
+        {
+            pHtOpMode = vos_mem_malloc(sizeof(tUpdateVHTOpMode));
+            if ( NULL == pHtOpMode )
+            {
+                limLog(pMac, LOGE,
+                      FL("%s: Not able to allocate memory for setting OP mode"),
+                      __func__);
+                return;
+            }
+            pHtOpMode->opMode = (psessionEntry->htSecondaryChannelOffset ==
+                          PHY_SINGLE_CHANNEL_CENTERED)?
+                          eHT_CHANNEL_WIDTH_20MHZ:eHT_CHANNEL_WIDTH_40MHZ;
+            pHtOpMode->staId = staId;
+            vos_mem_copy(pHtOpMode->peer_mac, &pStaDs->staAddr,
+                 sizeof(tSirMacAddr));
+            pHtOpMode->smesessionId = sessionId;
+
+            msg.type     = WDA_UPDATE_OP_MODE;
+            msg.reserved = 0;
+            msg.bodyptr  = pHtOpMode;
+            if (!VOS_IS_STATUS_SUCCESS(
+                     vos_mq_post_message(VOS_MODULE_ID_WDA, &msg)))
+            {
+                 limLog(pMac, LOGE,
+                   FL("%s: Not able to post WDA_UPDATE_OP_MODE message to WDA"),
+                   __func__);
+                 vos_mem_free(pHtOpMode);
+                 return;
+            }
+            limLog(pMac, LOG1,
+                        FL("%s: Notifed FW about OP mode: %d for staId=%d"),
+                        __func__, pHtOpMode->opMode, staId);
+
+         }
+         else
+            limLog(pMac, LOG1, FL("%s: station %d does not support HT40\n"),
+                        __func__, staId);
+    }
+
     return;
 }
 #endif