828 lines
24 KiB
C
828 lines
24 KiB
C
/*
|
|
* Broadcom Dongle Host Driver (DHD), Channel State information Module
|
|
*
|
|
* Copyright (C) 2023, Broadcom.
|
|
*
|
|
* Unless you and Broadcom execute a separate written software license
|
|
* agreement governing use of this software, this software is licensed to you
|
|
* under the terms of the GNU General Public License version 2 (the "GPL"),
|
|
* available at http://www.broadcom.com/licenses/GPLv2.php, with the
|
|
* following added to such license:
|
|
*
|
|
* As a special exception, the copyright holders of this software give you
|
|
* permission to link this software with independent modules, and to copy and
|
|
* distribute the resulting executable under terms of your choice, provided that
|
|
* you also meet, for each linked independent module, the terms and conditions of
|
|
* the license of that module. An independent module is a module which is not
|
|
* derived from this software. The special exception does not apply to any
|
|
* modifications of the software.
|
|
*
|
|
*
|
|
* <<Broadcom-WL-IPTag/Open:>>
|
|
*
|
|
* $Id$
|
|
*/
|
|
|
|
#ifdef CSI_SUPPORT
|
|
#include <osl.h>
|
|
|
|
#include <bcmutils.h>
|
|
|
|
#include <bcmendian.h>
|
|
#include <linuxver.h>
|
|
#include <linux/list.h>
|
|
#include <linux/sort.h>
|
|
#include <dngl_stats.h>
|
|
#include <wlioctl.h>
|
|
|
|
#include <bcmevent.h>
|
|
#include <dhd.h>
|
|
#include <dhd_dbg.h>
|
|
#include <dhd_csi.h>
|
|
#include <wldev_common.h>
|
|
#include <dhd_linux.h>
|
|
|
|
#define SYNA_ALIGN_BYTES (sizeof(uint32))
|
|
#define SYNA_ALIGN_PADDING (SYNA_ALIGN_BYTES -1LL)
|
|
#define SYNA_ALIGN_MASK (~SYNA_ALIGN_PADDING)
|
|
#define SYNA_ALIGN(value) \
|
|
(\
|
|
(SYNA_ALIGN_PADDING + ((long long int)(value))) \
|
|
& SYNA_ALIGN_MASK \
|
|
)
|
|
|
|
static const uint8 _gpMAC_zero[6] = {0, 0, 0, 0, 0, 0};
|
|
|
|
static int dhd_csi_data_append(uint32 remain_length, uint32 data_length,
|
|
const uint8 *pData, struct csi_cfr_node *ptr)
|
|
{
|
|
struct syna_csi_header *pEntry = NULL;
|
|
int error_status = 0;
|
|
int ret = BCME_OK;
|
|
int append_len = 0;
|
|
|
|
if ((!pData) || (!ptr) || (ptr->pNode != ptr)) {
|
|
DHD_ERROR(("%s-%d: *Error, invalid parameter, "
|
|
"pData=0x%px, ptr=0x%px, pNode=0x%px\n",
|
|
__func__, __LINE__,
|
|
pData, ptr, ptr?(ptr->pNode):(NULL)));
|
|
ret = BCME_BADARG;
|
|
goto done;
|
|
} else {
|
|
pEntry = (struct syna_csi_header *)&(ptr->entry);
|
|
error_status = SYNA_CSI_FLAG_ERROR_MASK & pEntry->flags;
|
|
}
|
|
/* find the append length */
|
|
if (remain_length) {
|
|
append_len = data_length
|
|
- pEntry->data_length
|
|
- remain_length;
|
|
} else if (pEntry->remain_length) {
|
|
append_len = pEntry->remain_length;
|
|
} else if (SYNA_CSI_CFR_NODE_FREE_LEN(ptr) >= data_length) {
|
|
append_len = data_length;
|
|
}
|
|
|
|
DHD_TRACE(("%s-%d: data_length=%d, remain=%d, append=%d "
|
|
"pNode: pEntry=0x%px, global_id=%d, flags=0x%x, "
|
|
"data_len=%d, remain_len=%d, copied_len=%d\n",
|
|
__func__, __LINE__,
|
|
data_length, remain_length, append_len,
|
|
pEntry, pEntry->global_id, pEntry->flags,
|
|
pEntry->data_length,
|
|
pEntry->remain_length, pEntry->copied_length));
|
|
|
|
/* copy and update */
|
|
if (append_len) {
|
|
memcpy((pEntry->data_length + ((uint8 *)pEntry->data)),
|
|
pData, append_len);
|
|
pEntry->data_length += append_len;
|
|
|
|
if ((!error_status)
|
|
&& (!remain_length)
|
|
&& (pEntry->checksum)) {
|
|
uint16 checksum = 0;
|
|
uint i;
|
|
pData = (uint8 *)pEntry->data;
|
|
for (i = 0; i < data_length; i++) {
|
|
checksum += pData[i];
|
|
}
|
|
if (checksum != pEntry->checksum) {
|
|
DHD_ERROR(("%s-%d: *Error, global_id=%d, "
|
|
"checksum=0x%X mismatch to FW=0x%X, "
|
|
"data_length=%d, remain_length=%d, "
|
|
"pData=0x%px\n",
|
|
__func__, __LINE__,
|
|
pEntry->global_id,
|
|
checksum, pEntry->checksum,
|
|
data_length, remain_length, pData));
|
|
pEntry->flags |= SYNA_CSI_FLAG_ERROR_CHECKSUM;
|
|
}
|
|
}
|
|
} else if (!error_status) {
|
|
DHD_ERROR(("%s-%d: *Error, no data append, "
|
|
"total_size=%d, free_len=%d, "
|
|
"in_data_length=%d, in_remain_length=%d, "
|
|
" Node: global_id=%d, flags=0x%X, "
|
|
"data_length=%d, remain_length=%d\n",
|
|
__func__, __LINE__,
|
|
ptr->total_size,
|
|
(int)SYNA_CSI_CFR_NODE_FREE_LEN(ptr),
|
|
data_length, remain_length,
|
|
pEntry->global_id, pEntry->flags,
|
|
pEntry->data_length, pEntry->remain_length));
|
|
} else {
|
|
DHD_ERROR(("%s-%d: *Warning, error report frame, "
|
|
"size=%d, free=%d, "
|
|
"in_data_length=%d, in_remain_length=%d, "
|
|
" Node: global_id=%d, flags=0x%X%s, "
|
|
"data_length=%d, remain_length=%d\n",
|
|
__func__, __LINE__,
|
|
ptr->total_size,
|
|
(int)SYNA_CSI_CFR_NODE_FREE_LEN(ptr),
|
|
data_length, remain_length,
|
|
pEntry->global_id, pEntry->flags,
|
|
(SYNA_CSI_FLAG_ERROR_CHECKSUM & pEntry->flags)
|
|
? "(error_checksum)"
|
|
: (SYNA_CSI_FLAG_ERROR_NO_ACK & pEntry->flags)
|
|
? "(error_no_ack)"
|
|
: (SYNA_CSI_FLAG_ERROR_READ & pEntry->flags)
|
|
? "(error_read)"
|
|
: (SYNA_CSI_FLAG_ERROR_PS & pEntry->flags)
|
|
? "(error_ps)"
|
|
: (SYNA_CSI_FLAG_ERROR_GENERIC & pEntry->flags)
|
|
? "(error_generic)"
|
|
: "",
|
|
pEntry->data_length, pEntry->remain_length));
|
|
}
|
|
|
|
/* make this as final step to avoid packet early retrieved */
|
|
pEntry->copied_length = 0;
|
|
pEntry->remain_length = remain_length;
|
|
|
|
ret = append_len;
|
|
|
|
done:
|
|
return ret;
|
|
}
|
|
|
|
static int dhd_csi_data_new_v0(dhd_pub_t *dhdp, const wl_event_msg_t *event,
|
|
struct dhd_cfr_header_v0 *pEvent)
|
|
{
|
|
struct csi_cfr_node *ptr = NULL;
|
|
struct syna_csi_header *pEntry = NULL;
|
|
struct timespec64 ts;
|
|
dhd_if_t *ifp = NULL;
|
|
int ret = BCME_ERROR;
|
|
uint total_size = 0;
|
|
u32 chanspec = 0;
|
|
uint32 remain_length = 0;
|
|
uint32 data_length = 0;
|
|
uint8 *pData = NULL;
|
|
|
|
ktime_get_real_ts64(&ts);
|
|
total_size = SYNA_CSI_CFR_NODE_LEN + ltoh32_ua(&(pEvent->cfr_dump_length));
|
|
|
|
ptr = (struct csi_cfr_node *)MALLOCZ(dhdp->osh, total_size);
|
|
if (!ptr) {
|
|
DHD_ERROR(("%s-%d: *Error, malloc %d for cfr dump list error\n",
|
|
__func__, __LINE__, total_size));
|
|
return BCME_NOMEM;
|
|
}
|
|
ptr->pNode = ptr; /* Not useful */
|
|
ptr->total_size = total_size;
|
|
pEntry = &(ptr->entry);
|
|
|
|
DHD_TRACE(("%s-%d: ptr=0x%p, ptr=0x%px, pEntry=0x%px, "
|
|
"delta=%d, header_size=%d\n",
|
|
__func__, __LINE__, ptr, ptr, pEntry,
|
|
(int)((uintptr)pEntry - (uintptr)ptr),
|
|
(int)SYNA_CSI_CFR_NODE_LEN));
|
|
|
|
/* process one by one for alignment consideration */
|
|
pEntry->magic_flag = CONST_SYNA_CSI_MAGIC_FLAG;
|
|
pEntry->version = CONST_SYNA_CSI_COMMON_HEADER_VERSION;
|
|
pEntry->format_type = SYNA_CSI_FORMAT_Q8;
|
|
pEntry->fc = 0;
|
|
if (pEvent->status) {
|
|
pEntry->flags |= SYNA_CSI_FLAG_ERROR_GENERIC;
|
|
}
|
|
memcpy(pEntry->client_ea, pEvent->peer_macaddr, ETHER_ADDR_LEN);
|
|
|
|
ifp = dhd_get_ifp(dhdp, event->ifidx);
|
|
if (!ifp) {
|
|
DHD_ERROR(("%s-%d: *Error, get ifp error\n",
|
|
__func__, __LINE__));
|
|
} else {
|
|
memcpy(pEntry->bsscfg_ea, ifp->mac_addr, ETHER_ADDR_LEN);
|
|
}
|
|
|
|
if ((ret = dhd_iovar(dhdp, event->ifidx, "chanspec",
|
|
NULL, 0, (char*)&chanspec,
|
|
sizeof(chanspec), FALSE) != BCME_OK)) {
|
|
pEntry->band = SYNA_CSI_BAND_UNKNOWN;
|
|
pEntry->bandwidth = SYNA_CSI_BW_UNKNOWN;
|
|
pEntry->channel = 0;
|
|
} else {
|
|
switch (CHSPEC_BAND(chanspec)) {
|
|
case WL_CHANSPEC_BAND_2G:
|
|
pEntry->band = SYNA_CSI_BAND_2G;
|
|
break;
|
|
case WL_CHANSPEC_BAND_5G:
|
|
pEntry->band = SYNA_CSI_BAND_5G;
|
|
break;
|
|
case WL_CHANSPEC_BAND_6G:
|
|
pEntry->band = SYNA_CSI_BAND_6G;
|
|
break;
|
|
default:
|
|
pEntry->band = SYNA_CSI_BAND_UNKNOWN;
|
|
break;
|
|
}
|
|
|
|
switch (CHSPEC_BW(chanspec)) {
|
|
case WL_CHANSPEC_BW_20:
|
|
pEntry->bandwidth = SYNA_CSI_BW_20MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_40:
|
|
pEntry->bandwidth = SYNA_CSI_BW_40MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_80:
|
|
pEntry->bandwidth = SYNA_CSI_BW_80MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_160:
|
|
pEntry->bandwidth = SYNA_CSI_BW_160MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_320:
|
|
pEntry->bandwidth = SYNA_CSI_BW_320MHz;
|
|
break;
|
|
default:
|
|
pEntry->bandwidth = SYNA_CSI_BAND_UNKNOWN;
|
|
break;
|
|
}
|
|
|
|
pEntry->channel = CHSPEC_CHANNEL(chanspec);
|
|
}
|
|
|
|
pEntry->num_txstream = pEvent->sts;
|
|
pEntry->num_rxchain = pEvent->num_rx;
|
|
pEntry->num_subcarrier = ltoh16_ua(&(pEvent->num_carrier));
|
|
pEntry->report_tsf = (uint64)TIMESPEC64_TO_US(ts);
|
|
pEntry->rssi = pEvent->rssi;
|
|
pEntry->data_length = 0;
|
|
pEntry->remain_length = 0;
|
|
pEntry->copied_length = 0;
|
|
|
|
DHD_TRACE(("%s-%d: Entry data_size=%d, remain_size=%d, @%llu\n",
|
|
__func__, __LINE__,
|
|
ltoh32_ua(&(pEvent->cfr_dump_length)),
|
|
ltoh32_ua(&(pEvent->remain_length)),
|
|
ltoh64_ua(&(pEntry->report_tsf))));
|
|
|
|
remain_length = ltoh32_ua(&(pEvent->remain_length));
|
|
data_length = ltoh32_ua(&(pEvent->cfr_dump_length));
|
|
pData = (uint8 *)pEvent->data;
|
|
ret = dhd_csi_data_append(remain_length, data_length, pData, ptr);
|
|
if (0 <= ret) {
|
|
INIT_LIST_HEAD(&(ptr->list));
|
|
mutex_lock(&dhdp->csi_lock);
|
|
list_add_tail(&(ptr->list), &dhdp->csi_list);
|
|
dhdp->csi_count++;
|
|
mutex_unlock(&dhdp->csi_lock);
|
|
} else {
|
|
MFREE(dhdp->osh, ptr, total_size);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int dhd_csi_data_new_v1(dhd_pub_t *dhdp, const wl_event_msg_t *event,
|
|
struct dhd_cfr_header_v1 *pEvent)
|
|
{
|
|
struct csi_cfr_node *ptr = NULL;
|
|
struct syna_csi_header *pEntry = NULL;
|
|
dhd_if_t *ifp = NULL;
|
|
int ret = BCME_ERROR;
|
|
uint total_size = 0;
|
|
u32 chanspec = 0;
|
|
uint32 remain_length = 0;
|
|
uint32 data_length = 0;
|
|
uint8 *pData = NULL;
|
|
|
|
total_size = SYNA_CSI_CFR_NODE_LEN + ltoh32_ua(&(pEvent->cfr_dump_length));
|
|
|
|
ptr = (struct csi_cfr_node *)MALLOCZ(dhdp->osh, total_size);
|
|
if (!ptr) {
|
|
DHD_ERROR(("%s-%d: *Error, malloc %d for cfr dump list error\n",
|
|
__func__, __LINE__, total_size));
|
|
return BCME_NOMEM;
|
|
}
|
|
ptr->pNode = ptr; /* Not useful */
|
|
ptr->total_size = total_size;
|
|
pEntry = &(ptr->entry);
|
|
|
|
DHD_TRACE(("%s-%d: ptr=0x%p, ptr=0x%px, pEntry=0x%px, "
|
|
"delta=%d, header_size=%d\n",
|
|
__func__, __LINE__, ptr, ptr, pEntry,
|
|
(int)((uintptr)pEntry - (uintptr)ptr),
|
|
(int)SYNA_CSI_CFR_NODE_LEN));
|
|
|
|
/* process one by one for alignment consideration */
|
|
pEntry->magic_flag = CONST_SYNA_CSI_MAGIC_FLAG;
|
|
pEntry->version = CONST_SYNA_CSI_COMMON_HEADER_VERSION;
|
|
pEntry->format_type = SYNA_CSI_FORMAT_Q8;
|
|
pEntry->fc = pEvent->fc;
|
|
if (pEvent->status) {
|
|
pEntry->flags |= SYNA_CSI_FLAG_ERROR_GENERIC;
|
|
}
|
|
|
|
memcpy(pEntry->client_ea, pEvent->peer_macaddr, ETHER_ADDR_LEN);
|
|
|
|
ifp = dhd_get_ifp(dhdp, event->ifidx);
|
|
|
|
if (!ifp) {
|
|
DHD_ERROR(("%s-%d: *Error, get ifp error\n",
|
|
__func__, __LINE__));
|
|
} else {
|
|
memcpy(pEntry->bsscfg_ea, ifp->mac_addr, ETHER_ADDR_LEN);
|
|
}
|
|
|
|
if ((ret = dhd_iovar(dhdp, event->ifidx, "chanspec",
|
|
NULL, 0, (char*)&chanspec, sizeof(chanspec), FALSE) != BCME_OK)) {
|
|
pEntry->band = SYNA_CSI_BAND_UNKNOWN;
|
|
pEntry->bandwidth = SYNA_CSI_BW_UNKNOWN;
|
|
pEntry->channel = 0;
|
|
} else {
|
|
switch (CHSPEC_BAND(chanspec)) {
|
|
case WL_CHANSPEC_BAND_2G:
|
|
pEntry->band = SYNA_CSI_BAND_2G;
|
|
break;
|
|
case WL_CHANSPEC_BAND_5G:
|
|
pEntry->band = SYNA_CSI_BAND_5G;
|
|
break;
|
|
case WL_CHANSPEC_BAND_6G:
|
|
pEntry->band = SYNA_CSI_BAND_6G;
|
|
break;
|
|
default:
|
|
pEntry->band = SYNA_CSI_BAND_UNKNOWN;
|
|
break;
|
|
}
|
|
|
|
switch (CHSPEC_BW(chanspec)) {
|
|
case WL_CHANSPEC_BW_20:
|
|
pEntry->bandwidth = SYNA_CSI_BW_20MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_40:
|
|
pEntry->bandwidth = SYNA_CSI_BW_40MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_80:
|
|
pEntry->bandwidth = SYNA_CSI_BW_80MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_160:
|
|
pEntry->bandwidth = SYNA_CSI_BW_160MHz;
|
|
break;
|
|
case WL_CHANSPEC_BW_320:
|
|
pEntry->bandwidth = SYNA_CSI_BW_320MHz;
|
|
break;
|
|
default:
|
|
pEntry->bandwidth = SYNA_CSI_BAND_UNKNOWN;
|
|
break;
|
|
}
|
|
|
|
pEntry->channel = CHSPEC_CHANNEL(chanspec);
|
|
}
|
|
|
|
pEntry->num_txstream = pEvent->sts;
|
|
pEntry->num_rxchain = pEvent->num_rx;
|
|
pEntry->num_subcarrier = ltoh16_ua(&(pEvent->num_carrier));
|
|
pEntry->report_tsf = ltoh64_ua(&(pEvent->cfr_timestamp));
|
|
pEntry->rssi = pEvent->rssi;
|
|
pEntry->data_length = 0;
|
|
pEntry->remain_length = 0;
|
|
pEntry->copied_length = 0;
|
|
|
|
DHD_TRACE(("%s-%d: Entry data_size=%d, remain_size=%d, @%llu\n",
|
|
__func__, __LINE__,
|
|
ltoh32_ua(&(pEvent->cfr_dump_length)),
|
|
ltoh32_ua(&(pEvent->remain_length)),
|
|
ltoh64_ua(&(pEntry->report_tsf))));
|
|
|
|
remain_length = ltoh32_ua(&(pEvent->remain_length));
|
|
data_length = ltoh32_ua(&(pEvent->cfr_dump_length));
|
|
pData = (uint8 *)pEvent->data;
|
|
ret = dhd_csi_data_append(remain_length, data_length, pData, ptr);
|
|
if (0 <= ret) {
|
|
INIT_LIST_HEAD(&(ptr->list));
|
|
mutex_lock(&dhdp->csi_lock);
|
|
list_add_tail(&(ptr->list), &dhdp->csi_list);
|
|
dhdp->csi_count++;
|
|
mutex_unlock(&dhdp->csi_lock);
|
|
} else {
|
|
MFREE(dhdp->osh, ptr, total_size);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int dhd_csi_data_new_v2(dhd_pub_t *dhdp, const wl_event_msg_t *event,
|
|
struct dhd_cfr_header_v2 *pEvent)
|
|
{
|
|
struct csi_cfr_node *ptr = NULL;
|
|
struct syna_csi_header *pEntry = NULL;
|
|
int ret = BCME_ERROR;
|
|
uint total_size = 0;
|
|
uint32 remain_length = 0;
|
|
uint32 data_length = 0;
|
|
uint8 *pData = NULL;
|
|
|
|
if (SYNA_ALIGN_PADDING & (uintptr)pEvent) {
|
|
DHD_ERROR(("%s-%d: *Error, pEvent=0x%px is not aligned!",
|
|
__func__, __LINE__, pEvent));
|
|
}
|
|
|
|
total_size = SYNA_CSI_CFR_NODE_LEN + pEvent->data_length;
|
|
|
|
ptr = (struct csi_cfr_node *)MALLOCZ(dhdp->osh, total_size);
|
|
if (!ptr) {
|
|
DHD_ERROR(("%s-%d: *Error, malloc %d for cfr dump list error\n",
|
|
__func__, __LINE__, total_size));
|
|
return BCME_NOMEM;
|
|
}
|
|
ptr->pNode = ptr; /* Not useful */
|
|
ptr->total_size = total_size;
|
|
pEntry = &(ptr->entry);
|
|
|
|
DHD_TRACE(("%s-%d: ptr=0x%p, ptr=0x%px, pEntry=0x%px, "
|
|
"delta=%d, header_size=%d\n",
|
|
__func__, __LINE__, ptr, ptr, pEntry,
|
|
(int)((uintptr)pEntry - (uintptr)ptr),
|
|
(int)SYNA_CSI_CFR_NODE_LEN));
|
|
|
|
/* process one by one for alignment consideration */
|
|
memcpy(pEntry, pEvent, sizeof(struct syna_csi_header));
|
|
pEntry->magic_flag = CONST_SYNA_CSI_MAGIC_FLAG;
|
|
pEntry->version = CONST_SYNA_CSI_COMMON_HEADER_VERSION;
|
|
|
|
if ((pEvent->status_compat)
|
|
&& (!(SYNA_CSI_FLAG_ERROR_MASK & pEntry->flags))) {
|
|
pEntry->flags |= SYNA_CSI_FLAG_ERROR_GENERIC;
|
|
DHD_ERROR(("%s-%d: *Warning, status=0x%X, "
|
|
" global_id=%d!\n",
|
|
__func__, __LINE__,
|
|
pEvent->status_compat,
|
|
pEvent->global_id));
|
|
}
|
|
if (!pEntry->format_type) {
|
|
pEntry->format_type = SYNA_CSI_FORMAT_Q8;
|
|
}
|
|
if (!memcmp(_gpMAC_zero, pEntry->bsscfg_ea, ETHER_ADDR_LEN)) {
|
|
dhd_if_t *ifp = dhd_get_ifp(dhdp, event->ifidx);
|
|
if (!ifp) {
|
|
DHD_ERROR(("%s-%d: *Error, get ifp error\n",
|
|
__func__, __LINE__));
|
|
} else {
|
|
memcpy(pEntry->bsscfg_ea, ifp->mac_addr, ETHER_ADDR_LEN);
|
|
}
|
|
}
|
|
|
|
if (!pEntry->report_tsf) {
|
|
struct timespec64 ts;
|
|
ktime_get_real_ts64(&ts);
|
|
pEntry->report_tsf = (uint64)TIMESPEC64_TO_US(ts);
|
|
}
|
|
|
|
pEntry->data_length = 0;
|
|
pEntry->remain_length = 0;
|
|
pEntry->copied_length = 0;
|
|
pEntry->checksum = pEvent->checksum;
|
|
|
|
DHD_TRACE(("%s-%d: Entry data_size=%d, remain_size=%d, @%llu\n",
|
|
__func__, __LINE__,
|
|
pEvent->data_length,
|
|
pEvent->remain_length,
|
|
pEntry->report_tsf));
|
|
|
|
remain_length = pEvent->remain_length;
|
|
data_length = pEvent->data_length;
|
|
pData = (uint8 *)pEvent->data;
|
|
ret = dhd_csi_data_append(remain_length, data_length, pData, ptr);
|
|
if (0 <= ret) {
|
|
INIT_LIST_HEAD(&(ptr->list));
|
|
mutex_lock(&dhdp->csi_lock);
|
|
list_add_tail(&(ptr->list), &dhdp->csi_list);
|
|
dhdp->csi_count++;
|
|
mutex_unlock(&dhdp->csi_lock);
|
|
} else {
|
|
MFREE(dhdp->osh, ptr, total_size);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int dhd_csi_event_handler(dhd_pub_t *dhdp, const wl_event_msg_t *event, void *event_data)
|
|
{
|
|
union dhd_cfr_header *pEvent = NULL;
|
|
struct csi_cfr_node *ptr = NULL, *next = NULL, *save_ptr = NULL;
|
|
const uint8 *pMAC = NULL, *pData = NULL;
|
|
uint8 version = 0xff;
|
|
uint32 data_length = 0, remain_length = 0, header_length = 0;
|
|
int ret = BCME_OK;
|
|
|
|
NULL_CHECK(dhdp, "dhdp is NULL", ret);
|
|
|
|
DHD_TRACE(("%s-%d: Enter\n", __func__, __LINE__));
|
|
|
|
if (!event_data) {
|
|
DHD_ERROR(("%s-%d: event_data is NULL\n",
|
|
__func__, __LINE__));
|
|
ret = BCME_BADARG;
|
|
goto done;
|
|
}
|
|
|
|
pEvent = (union dhd_cfr_header *)event_data;
|
|
if (pEvent->header_v0.status) {
|
|
DHD_ERROR(("%s-%d: *Warning, status=0x%X "
|
|
" may indicate error!\n",
|
|
__func__, __LINE__,
|
|
pEvent->header_v0.status));
|
|
/* TODO: do need to abandon this scenario? */
|
|
}
|
|
|
|
version = pEvent->header_v0.version;
|
|
switch (version) {
|
|
case CSI_VERSION_V0:
|
|
pMAC = pEvent->header_v0.peer_macaddr;
|
|
header_length = sizeof(struct dhd_cfr_header_v0);
|
|
remain_length = ltoh32_ua(&(pEvent->header_v0.remain_length));
|
|
data_length = ltoh32_ua(&(pEvent->header_v0.cfr_dump_length));
|
|
pData = (uint8 *)pEvent->header_v0.data;
|
|
break;
|
|
case CSI_VERSION_V1:
|
|
pMAC = pEvent->header_v1.peer_macaddr;
|
|
header_length = sizeof(struct dhd_cfr_header_v1);
|
|
remain_length = ltoh32_ua(&(pEvent->header_v1.remain_length));
|
|
data_length = ltoh32_ua(&(pEvent->header_v1.cfr_dump_length));
|
|
pData = (uint8 *)pEvent->header_v1.data;
|
|
break;
|
|
case CSI_VERSION_V2:
|
|
pMAC = pEvent->header_v2.client_ea;
|
|
header_length = sizeof(struct dhd_cfr_header_v2);
|
|
remain_length = pEvent->header_v2.remain_length;
|
|
data_length = pEvent->header_v2.data_length;
|
|
pData = (uint8 *)pEvent->header_v2.data;
|
|
break;
|
|
default:
|
|
DHD_ERROR(("%s-%d: CSI version=%d error\n",
|
|
__func__, __LINE__, version));
|
|
ret = BCME_BADOPTION;
|
|
goto done;
|
|
break;
|
|
}
|
|
|
|
if (header_length <= remain_length) {
|
|
remain_length -= header_length;
|
|
}
|
|
if (CONST_CSI_MAXIMUM_DATA_BYTES < remain_length) {
|
|
DHD_ERROR(("%s-%d: *Error, drop too big length, "
|
|
"global_id=%d, version=%d, status=%d, "
|
|
"data_length=%d(0x%X), remain_length=%d(0x%X) > %d\n",
|
|
__func__, __LINE__,
|
|
pEvent->header_v2.global_id,
|
|
version, pEvent->header_v2.status_compat,
|
|
data_length, data_length,
|
|
remain_length, remain_length,
|
|
(int)CONST_CSI_MAXIMUM_DATA_BYTES));
|
|
prhex("TOO_BIG_LENGTH", (uchar *)pEvent, 128);
|
|
ret = BCME_BADLEN;
|
|
goto done;
|
|
}
|
|
|
|
mutex_lock(&dhdp->csi_lock);
|
|
/* check if this addr exist */
|
|
if (!list_empty(&dhdp->csi_list)) {
|
|
list_for_each_entry_safe(ptr, next, &dhdp->csi_list, list) {
|
|
struct syna_csi_header *pEntry = &(ptr->entry);
|
|
/* check if is new */
|
|
if (bcmp(&pEntry->client_ea, pMAC, ETHER_ADDR_LEN) == 0) {
|
|
if (pEntry->remain_length > 0) {
|
|
save_ptr = ptr;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
/* check if need to drop head packet */
|
|
if ((!save_ptr) && (dhdp->csi_count >= MAX_CSI_BUF_NUM)) {
|
|
DHD_ERROR(("%s-%d: CSI data is full, drop pkt \n",
|
|
__func__, __LINE__));
|
|
ptr = list_first_entry(&dhdp->csi_list, struct csi_cfr_node, list);
|
|
list_del_init(&ptr->list);
|
|
MFREE(dhdp->osh, ptr, ptr->total_size);
|
|
dhdp->csi_count--;
|
|
}
|
|
mutex_unlock(&dhdp->csi_lock);
|
|
|
|
/* try to append new node if needs */
|
|
DHD_TRACE(("%s-%d: save_ptr=0x%px, version=%d\n",
|
|
__func__, __LINE__, save_ptr, version));
|
|
if (!save_ptr) {
|
|
switch (version) {
|
|
case CSI_VERSION_V0:
|
|
ret = dhd_csi_data_new_v0(dhdp, event, &(pEvent->header_v0));
|
|
break;
|
|
case CSI_VERSION_V1:
|
|
ret = dhd_csi_data_new_v1(dhdp, event, &(pEvent->header_v1));
|
|
break;
|
|
case CSI_VERSION_V2:
|
|
ret = dhd_csi_data_new_v2(dhdp, event, &(pEvent->header_v2));
|
|
break;
|
|
default:
|
|
DHD_ERROR(("%s-%d: CSI version=%d error\n",
|
|
__func__, __LINE__, version));
|
|
ret = BCME_BADOPTION;
|
|
goto done;
|
|
break;
|
|
}
|
|
} else {
|
|
ret = dhd_csi_data_append(remain_length, data_length, pData, save_ptr);
|
|
}
|
|
|
|
done:
|
|
return ret;
|
|
}
|
|
|
|
static void
|
|
dhd_csi_clean_list(dhd_pub_t *dhdp)
|
|
{
|
|
struct csi_cfr_node *ptr, *next;
|
|
int num = 0;
|
|
|
|
if (!dhdp) {
|
|
DHD_ERROR(("%s-%d: *Error, NULL POINTER\n",
|
|
__func__, __LINE__));
|
|
return;
|
|
}
|
|
|
|
mutex_lock(&dhdp->csi_lock);
|
|
if (!list_empty(&dhdp->csi_list)) {
|
|
list_for_each_entry_safe(ptr, next, &dhdp->csi_list, list) {
|
|
list_del(&ptr->list);
|
|
num++;
|
|
MFREE(dhdp->osh, ptr, ptr->total_size);
|
|
}
|
|
}
|
|
dhdp->csi_count = 0;
|
|
mutex_unlock(&dhdp->csi_lock);
|
|
|
|
DHD_TRACE(("%s-%d: Clean up %d record\n",
|
|
__func__, __LINE__, num));
|
|
}
|
|
|
|
int dhd_csi_config(dhd_pub_t *dhdp, char *pBuf, uint length, bool is_set)
|
|
{
|
|
int ret = BCME_OK;
|
|
|
|
if ((!dhdp) || (!pBuf) || (sizeof(uint32) > length)) {
|
|
DHD_ERROR(("%s-%d: *Error, Bad argument\n",
|
|
__func__, __LINE__));
|
|
ret = BCME_BADARG;
|
|
goto done;
|
|
}
|
|
|
|
if (is_set) {
|
|
uint value = *((uint *)pBuf);
|
|
if (SYNA_CSI_DATA_MODE_LAST <= value) {
|
|
ret = BCME_BADOPTION;
|
|
goto done;
|
|
} else if (SYNA_CSI_DATA_MODE_NONE != value) {
|
|
dhdp->csi_data_send_manner = value;
|
|
dhd_csi_clean_list(dhdp);
|
|
}
|
|
} else {
|
|
memcpy(pBuf, &dhdp->csi_data_send_manner, sizeof(uint32));
|
|
}
|
|
|
|
done:
|
|
return ret;
|
|
}
|
|
|
|
int dhd_csi_init(dhd_pub_t *dhdp)
|
|
{
|
|
int err = BCME_OK;
|
|
|
|
mutex_init(&dhdp->csi_lock);
|
|
mutex_lock(&dhdp->csi_lock);
|
|
NULL_CHECK(dhdp, "dhdp is NULL", err);
|
|
INIT_LIST_HEAD(&dhdp->csi_list);
|
|
dhdp->csi_count = 0;
|
|
mutex_unlock(&dhdp->csi_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
int
|
|
dhd_csi_deinit(dhd_pub_t *dhdp)
|
|
{
|
|
int err = BCME_OK;
|
|
|
|
NULL_CHECK(dhdp, "dhdp is NULL", err);
|
|
|
|
dhd_csi_clean_list(dhdp);
|
|
|
|
mutex_destroy(&dhdp->csi_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
int dhd_csi_retrieve_queue_data(dhd_pub_t *dhdp, char *buf, uint count)
|
|
{
|
|
struct csi_cfr_node *ptr = NULL, *next = NULL;
|
|
int ret = BCME_OK;
|
|
char *pbuf = buf;
|
|
int num = 0;
|
|
int copy_len = 0;
|
|
int left_data = 0;
|
|
int total_copy_len = 0;
|
|
|
|
NULL_CHECK(dhdp, "dhdp is NULL", ret);
|
|
|
|
mutex_lock(&dhdp->csi_lock);
|
|
if (!list_empty(&dhdp->csi_list)) {
|
|
list_for_each_entry_safe(ptr, next, &dhdp->csi_list, list) {
|
|
struct syna_csi_header *pEntry = &(ptr->entry);
|
|
if (pEntry->remain_length) {
|
|
DHD_ERROR(("%s-%d: *Warning, data not ready "
|
|
"for %02X:%02X:%02X:%02X:%02X:%02X,"
|
|
"global_id=%d, data_length=%d, "
|
|
"remain_length=%d\n",
|
|
__func__, __LINE__,
|
|
pEntry->client_ea[0],
|
|
pEntry->client_ea[1],
|
|
pEntry->client_ea[2],
|
|
pEntry->client_ea[3],
|
|
pEntry->client_ea[4],
|
|
pEntry->client_ea[5],
|
|
pEntry->global_id,
|
|
pEntry->data_length,
|
|
pEntry->remain_length));
|
|
continue;
|
|
}
|
|
|
|
left_data = SYNA_CSI_PKT_TOTAL_LEN(pEntry)
|
|
- pEntry->copied_length;
|
|
copy_len = count;
|
|
|
|
if (0 > left_data) {
|
|
DHD_ERROR(("%s-%d: *Error, invalid case, "
|
|
"total_len=%d, data_len=%d, "
|
|
"remain_len=%d, copied_len=%d\n",
|
|
__func__, __LINE__,
|
|
(int)SYNA_CSI_PKT_TOTAL_LEN(pEntry),
|
|
pEntry->data_length,
|
|
pEntry->remain_length,
|
|
pEntry->copied_length));
|
|
copy_len = 0;
|
|
} else if (copy_len > left_data) {
|
|
copy_len = left_data;
|
|
}
|
|
|
|
DHD_TRACE(("%s-%d: Packet[%d]: "
|
|
"left_room=%d, copy_len=%d, "
|
|
"pNode_data_len=%d, pNode_reamin_len=%d, "
|
|
"pNode_copied_len=%d, csi_count=%d\n",
|
|
__func__, __LINE__,
|
|
num, count, copy_len,
|
|
pEntry->data_length,
|
|
pEntry->remain_length,
|
|
pEntry->copied_length,
|
|
dhdp->csi_count));
|
|
|
|
if (0 < copy_len) {
|
|
memcpy(pbuf,
|
|
pEntry->copied_length + (uint8 *)pEntry,
|
|
copy_len);
|
|
pEntry->copied_length += copy_len;
|
|
count -= copy_len;
|
|
pbuf += copy_len;
|
|
num++;
|
|
}
|
|
|
|
if (pEntry->copied_length >= SYNA_CSI_PKT_TOTAL_LEN(pEntry)) {
|
|
list_del(&ptr->list);
|
|
MFREE(dhdp->osh, ptr, ptr->total_size);
|
|
dhdp->csi_count--;
|
|
}
|
|
|
|
if (0 >= copy_len) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
mutex_unlock(&dhdp->csi_lock);
|
|
total_copy_len = pbuf - buf;
|
|
DHD_TRACE(("%s-%d: dump %d record %d bytes\n",
|
|
__func__, __LINE__, num, total_copy_len));
|
|
|
|
return total_copy_len;
|
|
}
|
|
#endif /* CSI_SUPPORT */
|