Snap for 10155831 from 39f4a55f7028e50122f0de50fac36374173c2a08 to udc-d1-release

Change-Id: I4d97b942ef8dbf5f3428a7f717fce5e70ca170ef

2 files changed, 277 insertions, 30 deletions

diff --git a/bcmdhd/wifi_hal/common.h b/bcmdhd/wifi_hal/common.h
index 29e0e97..2ec2bc0 100644
--- a/bcmdhd/wifi_hal/common.h
+++ b/bcmdhd/wifi_hal/common.h
@@ -62,6 +62,7 @@ const uint32_t BRCM_OUI =  0x001018;
 #define SAR_CONFIG_SCENARIO_COUNT      100
 #define MAX_NUM_RADIOS 3
 #define MAX_CMD_RESP_BUF_LEN 8192
+#define MAX_MLO_LINK 3
 
 /*
  This enum defines ranges for various commands; commands themselves
diff --git a/bcmdhd/wifi_hal/link_layer_stats.cpp b/bcmdhd/wifi_hal/link_layer_stats.cpp
index 541cbce..f3460c2 100644
--- a/bcmdhd/wifi_hal/link_layer_stats.cpp
+++ b/bcmdhd/wifi_hal/link_layer_stats.cpp
@@ -94,12 +94,14 @@ public:
 protected:
     virtual int handleResponse(WifiEvent& reply) {
         bool ml_data = false;
-        void *data = NULL;
         wifi_radio_stat *radio_stat_ptr = NULL;
-        u8 *iface_stat = NULL, *iface_ml_stat = NULL;
-        u8 *radioStatsBuf = NULL, *output = NULL, *data_ptr = NULL;
-        uint32_t total_size = 0, per_radio_size = 0, data_len = 0, rem_len = 0;
-        int num_radios = 0, id = 0, subcmd = 0, len = 0;
+        wifi_iface_ml_stat *iface_ml_stat_ptr = NULL;
+        u8 *radioStatsBuf = NULL, *ifaceMlStatsBuf = NULL, *outbuf = NULL, *data_ptr = NULL;
+        u8 *data = NULL, *iface_stat = NULL;
+        uint32_t offset = 0, per_radio_size = 0, data_len = 0, outbuf_rem_len = 0, data_rem_len = 0;
+        int ret = 0, num_radios = 0, id = 0, subcmd = 0, len = 0;
+        u32 fixed_iface_ml_stat_size = 0, all_links_stat_size = 0;
+        u8 num_links = 0;
 
         ALOGI("In GetLinkStatsCommand::handleResponse");
 
@@ -123,10 +125,10 @@ protected:
             if (it.get_type() == ANDR_WIFI_ATTRIBUTE_NUM_RADIO) {
                 num_radios = it.get_u32();
             } else if (it.get_type() == ANDR_WIFI_ATTRIBUTE_STATS_INFO) {
-                data = it.get_data();
+                data = (u8 *)it.get_data();
                 data_len = it.get_len();
             } else if (it.get_type() == ANDR_WIFI_ATTRIBUTE_ML_STATS_INFO) {
-                data = it.get_data();
+                data = (u8 *)it.get_data();
                 data_len = it.get_len();
                 ml_data = true;
             } else {
@@ -136,27 +138,28 @@ protected:
         }
 
         if (num_radios) {
-            rem_len = MAX_CMD_RESP_BUF_LEN;
+            outbuf_rem_len = MAX_CMD_RESP_BUF_LEN;
             radioStatsBuf = (u8 *)malloc(MAX_CMD_RESP_BUF_LEN);
             if (!radioStatsBuf) {
                 ALOGE("No memory\n");
                 return NL_SKIP;
             }
             memset(radioStatsBuf, 0, MAX_CMD_RESP_BUF_LEN);
-            output = radioStatsBuf;
+            outbuf = radioStatsBuf;
 
             if (!data || !data_len) {
                 ALOGE("%s: null data\n", __func__);
                 goto exit;
             }
-            data_ptr = (u8*)data;
+            data_ptr = data;
             for (int i = 0; i < num_radios; i++) {
-                rem_len -= per_radio_size;
-                if (rem_len < per_radio_size) {
+                outbuf_rem_len -= per_radio_size;
+                if (outbuf_rem_len < per_radio_size) {
                     ALOGE("No data left for radio %d\n", i);
                     goto exit;
                 }
-                data_ptr = (u8*)data + total_size;
+
+                data_ptr = data + offset;
                 if (!data_ptr) {
                     ALOGE("Invalid data for radio index = %d\n", i);
                     goto exit;
@@ -168,32 +171,89 @@ protected:
                     ALOGE("No data for radio %d\n", i);
                     continue;
                 }
-                memcpy(output, radio_stat_ptr, per_radio_size);
-                output += per_radio_size;
-                total_size += per_radio_size;
+                memcpy(outbuf, radio_stat_ptr, per_radio_size);
+                outbuf += per_radio_size;
+                /* Accounted size of all the radio data len */
+                offset += per_radio_size;
             }
 
-            if (ml_data && (data_len >= (total_size + sizeof(wifi_iface_ml_stat)))) {
-                iface_ml_stat = ((u8*)data + total_size);
-                if (!iface_ml_stat) {
-                    ALOGE("No iface ml stats data!!, data_len = %d, total_size = %d\n",
-                        data_len, total_size);
+            if (ml_data) {
+                outbuf = NULL;
+                data_rem_len = data_len - offset;
+                fixed_iface_ml_stat_size = offsetof(wifi_iface_ml_stat, links);
+
+                /* Allocate vendor hal buffer, instead of using the nlmsg allocated buffer */
+                ifaceMlStatsBuf = (u8 *)malloc(MAX_CMD_RESP_BUF_LEN);
+                if (!ifaceMlStatsBuf) {
+                    ALOGE("No memory\n");
                     goto exit;
                 }
-                (*mHandler.on_multi_link_stats_results)(id, (wifi_iface_ml_stat *)iface_ml_stat,
-                    num_radios, (wifi_radio_stat *)radioStatsBuf);
-            } else if ((data_len >= (total_size + sizeof(wifi_iface_stat)))) {
-                iface_stat = ((u8*)data + total_size);
+                outbuf_rem_len = MAX_CMD_RESP_BUF_LEN;
+                memset(ifaceMlStatsBuf, 0, MAX_CMD_RESP_BUF_LEN);
+                outbuf = ifaceMlStatsBuf;
+
+                if (data_rem_len >= fixed_iface_ml_stat_size) {
+                    data_ptr = (data + offset);
+                    if (!data_ptr) {
+                        ALOGE("No iface ml stats fixed data!!, data_len = %d, offset = %d\n",
+                            data_len, offset);
+                        ret = WIFI_ERROR_INVALID_ARGS;
+                        goto exit;
+                    }
+
+                    if (outbuf_rem_len < fixed_iface_ml_stat_size) {
+                        ALOGE("No space to copy fixed iface ml stats!, rem_len %d, req_len %d\n",
+                            outbuf_rem_len, fixed_iface_ml_stat_size);
+                        ret = WIFI_ERROR_OUT_OF_MEMORY;
+                        goto exit;
+                    }
+
+                    memcpy(outbuf, data_ptr, fixed_iface_ml_stat_size);
+                    data_rem_len -= fixed_iface_ml_stat_size;
+                    outbuf_rem_len -= fixed_iface_ml_stat_size;
+                    outbuf += fixed_iface_ml_stat_size;
+                    offset += fixed_iface_ml_stat_size;
+
+                    iface_ml_stat_ptr = (wifi_iface_ml_stat *)ifaceMlStatsBuf;
+                    if (!iface_ml_stat_ptr) {
+                        ALOGE("No iface ml stats data!!");
+                        goto exit;
+                    }
+
+                    num_links = iface_ml_stat_ptr->num_links;
+                    all_links_stat_size = (num_links * offsetof(wifi_link_stat, peer_info));
+
+                    if (num_links >= MAX_MLO_LINK) {
+                        ALOGE("Invalid num links :%d\n", num_links);
+                        goto exit;
+                    }
+                    if (num_links && (data_rem_len >= all_links_stat_size)) {
+                        ret = convertToExternalIfaceMlstatStructure(&data, &offset, &outbuf,
+                            &data_rem_len, num_links, &outbuf_rem_len);
+                        if (ret < 0) {
+                            ALOGE(("Failed to map data to iface ml struct\n"));
+                            goto exit;
+                        }
+                    } else {
+                        ALOGE("num_links %d, Required data not found: expected len %d,"
+                            " data_rem_len %d\n", num_links, all_links_stat_size, data_rem_len);
+                    }
+                    (*mHandler.on_multi_link_stats_results)(id,
+                        (wifi_iface_ml_stat *)ifaceMlStatsBuf, num_radios,
+                        (wifi_radio_stat *)radioStatsBuf);
+                }
+            } else if ((data_len >= (offset + sizeof(wifi_iface_stat)))) {
+                iface_stat = (data + offset);
                 if (!iface_stat) {
-                    ALOGE("No data for legacy iface stats!!, data_len = %d, total_size = %d\n",
-                        data_len, total_size);
+                    ALOGE("No data for legacy iface stats!!, data_len = %d, offset = %d\n",
+                        data_len, offset);
                     goto exit;
                 }
                 (*mHandler.on_link_stats_results)(id, (wifi_iface_stat *)iface_stat,
                     num_radios, (wifi_radio_stat *)radioStatsBuf);
             } else {
-                ALOGE("No data for iface stats!!, data_len = %d, total_size = %d\n",
-                    data_len, total_size);
+                ALOGE("No data for iface stats!!, data_len = %d, offset = %d\n",
+                    data_len, offset);
                 goto exit;
             }
         }
@@ -206,12 +266,17 @@ exit:
             free(radioStatsBuf);
             radioStatsBuf = NULL;
         }
+        if (ifaceMlStatsBuf) {
+            free(ifaceMlStatsBuf);
+            ifaceMlStatsBuf = NULL;
+        }
         return NL_OK;
     }
 
 private:
     wifi_radio_stat *convertToExternalRadioStatStructure(wifi_radio_stat *internal_stat_ptr,
-        uint32_t *per_radio_size) {
+        uint32_t *per_radio_size)
+    {
         wifi_radio_stat *external_stat_ptr = NULL;
         if (!internal_stat_ptr) {
             ALOGE("Incoming data is null\n");
@@ -241,6 +306,187 @@ private:
         }
         return external_stat_ptr;
     }
+
+    int convertToExternalRatestatsStructure(u8 **data, u32 *offset, u8 **outbuf,
+        u32 *data_rem_len, wifi_peer_info *peer_info, u8 num_rate, u32 *outbuf_rem_len)
+    {
+        u8 k = 0, num_peers = 0;
+        int ret = 0;
+        u8 *data_ptr = NULL;
+        u32 all_rates_size = 0, per_rate_size = 0;
+
+        per_rate_size = sizeof(wifi_rate_stat);
+        all_rates_size = num_rate * per_rate_size;
+        if (!peer_info && (*data_rem_len != all_rates_size)) {
+            ALOGE("Insufficient data for rate_stats, data_rem_len %d,"
+                "required data size %d \n", *data_rem_len, all_rates_size);
+            ret = WIFI_ERROR_INVALID_ARGS;
+            goto exit;
+        }
+
+        for (k = 0; k < num_rate; k++) {
+            data_ptr = ((*data) + (*offset));
+            if (!data_ptr) {
+                ALOGE("rate_stats not found!! num_rate %d, *data_rem_len = %d, *offset = %d\n",
+                    num_rate, *data_rem_len, *offset);
+                ret = WIFI_ERROR_OUT_OF_MEMORY;
+                goto exit;
+            }
+
+            if (*data_rem_len < per_rate_size) {
+                ALOGE("no rate_stats!!, data_rem_len %d, rate_stat size %d\n",
+                    *data_rem_len, per_rate_size);
+                ret = WIFI_ERROR_INVALID_ARGS;
+                goto exit;
+            }
+
+            if (*outbuf_rem_len < per_rate_size) {
+                ALOGE("No space to copy rate_stats of index [%d]!, rem_len %d, req_len %d\n",
+                    k, *outbuf_rem_len, per_rate_size);
+                ret = WIFI_ERROR_OUT_OF_MEMORY;
+                goto exit;
+            }
+
+            memcpy(*outbuf, data_ptr, per_rate_size);
+            *data_rem_len -= per_rate_size;
+            *outbuf_rem_len -= per_rate_size;
+            *outbuf += per_rate_size;
+            *offset += per_rate_size;
+        }
+    exit:
+        return ret;
+    }
+
+    int convertToExternalIfaceLinkstatStructure(u8 **data, uint32_t *offset, u8 **outbuf,
+            u32 *data_rem_len, u8 num_peers, wifi_link_stat *links, u32 *outbuf_rem_len)
+    {
+        int ret = 0, j = 0, num_rate = 0;
+        u32 all_rate_stats_per_peer_per_link_size = 0, fixed_peer_info_size = 0;
+        u8 *data_ptr = NULL;
+        wifi_peer_info *peer_info_ptr = NULL;
+
+        for (j = 0; j < num_peers; j++) {
+            data_ptr = ((*data) + (*offset));
+            if (!data_ptr || (!*data_rem_len)) {
+                ALOGE("no peer_info data!! num_peers %d, data_rem_len = %d, *offset = %d\n",
+                    num_peers, *data_rem_len, *offset);
+                ret = WIFI_ERROR_OUT_OF_MEMORY;
+                goto exit;
+            }
+
+            fixed_peer_info_size = offsetof(wifi_peer_info, rate_stats);
+            if (*data_rem_len < fixed_peer_info_size) {
+                ALOGE("no fixed peer_info data!!, data_rem_len %d, fixed peer info %d\n",
+                    *data_rem_len, fixed_peer_info_size);
+                ret = WIFI_ERROR_INVALID_ARGS;
+                goto exit;
+            }
+
+            if (*outbuf_rem_len < fixed_peer_info_size) {
+                ALOGE("No space to copy fixed peer_info of index[%d]!, rem_len %d, req_len %d\n",
+                    j, *outbuf_rem_len, fixed_peer_info_size);
+                ret = WIFI_ERROR_OUT_OF_MEMORY;
+                goto exit;
+            }
+
+            memcpy(*outbuf, data_ptr, fixed_peer_info_size);
+            *data_rem_len -= fixed_peer_info_size;
+            *outbuf_rem_len -= fixed_peer_info_size;
+            *outbuf += fixed_peer_info_size;
+            *offset += fixed_peer_info_size;
+
+            peer_info_ptr = (wifi_peer_info *)data_ptr;
+            if (!peer_info_ptr) {
+                ALOGE("no peer_info data!!");
+                ret = WIFI_ERROR_INVALID_ARGS;
+                goto exit;
+            }
+
+            num_rate = peer_info_ptr->num_rate;
+            all_rate_stats_per_peer_per_link_size = num_rate*sizeof(wifi_rate_stat);
+            if (num_rate && (*data_rem_len >= all_rate_stats_per_peer_per_link_size)) {
+                ret = convertToExternalRatestatsStructure(data, offset, outbuf, data_rem_len,
+                    peer_info_ptr, num_rate, outbuf_rem_len);
+                if (ret != WIFI_SUCCESS) {
+                    ALOGE(("Failed to convert it to rate stats\n"));
+                    goto exit;
+                }
+            } else {
+                ALOGI("num_rate %d, Required rate_stats not found: expected len %d,"
+                    " data_rem_len %d\n", num_rate, all_rate_stats_per_peer_per_link_size,
+                    *data_rem_len);
+                continue;
+            }
+        }
+
+    exit:
+        return ret;
+    }
+
+    int convertToExternalIfaceMlstatStructure(u8 **data, u32 *offset, u8 **outbuf,
+        u32 *data_rem_len, u8 num_links, u32 *outbuf_rem_len)
+    {
+        int ret = 0, i = 0;
+        u32 all_peers_per_link_size = 0, fixed_link_stat_size = 0;
+        u8 *data_ptr = NULL;
+        u8 num_peers = 0;
+        wifi_link_stat *links_ptr = NULL;
+
+        for (i = 0; i < num_links; i++) {
+            data_ptr = ((*data) + (*offset));
+            if (!data_ptr || !(*data_rem_len)) {
+                ALOGE("no variable links data!! num_links %d, data_rem_len = %d, offset = %d\n",
+                    num_links, *data_rem_len, *offset);
+                ret = WIFI_ERROR_OUT_OF_MEMORY;
+                goto exit;
+            }
+
+            fixed_link_stat_size = offsetof(wifi_link_stat, peer_info);
+            if (*data_rem_len < fixed_link_stat_size) {
+                ALOGE("no fixed wifi_link_stat data!!, data_rem_len %d, fixed link stat data %d\n",
+                    *data_rem_len, fixed_link_stat_size);
+                ret = WIFI_ERROR_INVALID_ARGS;
+                goto exit;
+            }
+
+            if (*outbuf_rem_len < fixed_link_stat_size) {
+                ALOGE("No space to copy fixed link stats of index[%d]!, rem_len %d, req_len %d\n",
+                    i, *outbuf_rem_len, fixed_link_stat_size);
+                ret = WIFI_ERROR_OUT_OF_MEMORY;
+                goto exit;
+            }
+
+            memcpy(*outbuf, data_ptr, fixed_link_stat_size);
+            *outbuf_rem_len -= fixed_link_stat_size;
+            *data_rem_len -= fixed_link_stat_size;
+            *outbuf += fixed_link_stat_size;
+            *offset += fixed_link_stat_size;
+
+            links_ptr = (wifi_link_stat *)data_ptr;
+            if (!links_ptr) {
+                ALOGE("no link_stat data!!");
+                ret = WIFI_ERROR_INVALID_ARGS;
+                goto exit;
+            }
+
+            num_peers = links_ptr->num_peers;
+            all_peers_per_link_size = num_peers * offsetof(wifi_peer_info, rate_stats);
+            if (num_peers && (*data_rem_len >= all_peers_per_link_size)) {
+                ret = convertToExternalIfaceLinkstatStructure(data, offset, outbuf,
+                    data_rem_len, num_peers, links_ptr, outbuf_rem_len);
+                if (ret != WIFI_SUCCESS) {
+                    ALOGE(("Failed to convert it to iface link stats\n"));
+                    goto exit;
+                }
+            } else {
+                ALOGI("num_peers %d, Required data not found: expected len %d, data_rem_len %d\n",
+                    num_peers, all_peers_per_link_size, *data_rem_len);
+                continue;
+            }
+        }
+    exit:
+        return ret;
+    }
 };
 
 wifi_error wifi_get_link_stats(wifi_request_id id,