/* pretty hex print a pkt buffer chain */
void prpkt(char *msg, void *drv, void *p0)
{
void *p;
if (msg && (msg[0] != '\0'))
printf("%s: ", msg);
for (p = p0; p; p = PKTNEXT(drv, p))
prhex(NULL, PKTDATA(drv, p), PKTLEN(drv, p));
}
/* pretty hex print a pkt buffer chain */
void prpkt(const char *msg, struct osl_info *osh, struct sk_buff *p0)
{
struct sk_buff *p;
if (msg && (msg[0] != '\0'))
printf("%s:\n", msg);
for (p = p0; p; p = p->next)
prhex(NULL, p->data, p->len);
}
void
wpa_calc_tpk(struct ether_addr *init_ea, struct ether_addr *resp_ea,
struct ether_addr *bssid, uint8 *anonce, uint8* snonce,
uint8 *tpk, uint tpk_len)
{
uchar key_input[SHA256_DIGEST_LENGTH];
uchar data[128], tpk_buff[160]; /* TK_bits + 128, where TK_bits is 16 bytes for CCMP */
char prefix[] = "TDLS PMK";
uint data_len = 0;
/* Generate TPK-Key-Input = SHA-256(min(SN, AN) || max(SN, AN)) first */
bcopy(wpa_array_cmp(MIN_ARRAY, snonce, anonce,
EAPOL_WPA_KEY_NONCE_LEN),
(char *)&data[0], EAPOL_WPA_KEY_NONCE_LEN);
prhex("min(sn,an):", data, EAPOL_WPA_KEY_NONCE_LEN);
data_len += EAPOL_WPA_KEY_NONCE_LEN;
bcopy(wpa_array_cmp(MAX_ARRAY, snonce, anonce,
EAPOL_WPA_KEY_NONCE_LEN),
(char *)&data[data_len], EAPOL_WPA_KEY_NONCE_LEN);
prhex("max(sn,an):", &data[data_len], EAPOL_WPA_KEY_NONCE_LEN);
data_len += EAPOL_WPA_KEY_NONCE_LEN;
prhex("data:", &data[data_len], 2*EAPOL_WPA_KEY_NONCE_LEN);
sha256(data, data_len, key_input, SHA256_DIGEST_LENGTH);
prhex("input_key", key_input, SHA256_DIGEST_LENGTH);
/* Create the the data portion:
* the lesser of the EAs, followed by the greater of the EAs,
* followed by BSSID
*/
data_len = 0;
bcopy(prefix, data, strlen(prefix));
prhex("prefix:", data, strlen(prefix));
data_len += strlen(prefix);
bcopy(wpa_array_cmp(MIN_ARRAY, (uint8 *)init_ea, (uint8 *)resp_ea,
ETHER_ADDR_LEN),
(char *)&data[data_len], ETHER_ADDR_LEN);
prhex("min(init_ea, resp_ea:", &data[data_len], ETHER_ADDR_LEN);
data_len += ETHER_ADDR_LEN;
bcopy(wpa_array_cmp(MAX_ARRAY, (uint8 *)init_ea, (uint8 *)resp_ea,
ETHER_ADDR_LEN),
(char *)&data[data_len], ETHER_ADDR_LEN);
prhex("min(init_ea, resp_ea:", &data[data_len], ETHER_ADDR_LEN);
data_len += ETHER_ADDR_LEN;
bcopy((char*)bssid, (char *)&data[data_len], ETHER_ADDR_LEN);
data_len += ETHER_ADDR_LEN;
prhex("data", data, data_len);
/* generate the TPK */
hmac_sha256_n(key_input, SHA256_DIGEST_LENGTH, data, data_len, tpk_buff, tpk_len);
bcopy(tpk_buff, (char*)tpk, tpk_len);
}
int
wl_cfgnan_status_handler(struct net_device *ndev,
struct bcm_cfg80211 *cfg, char *cmd, nan_cmd_data_t *cmd_data)
{
wl_nan_ioc_t *nanioc = NULL;
void *pxtlv;
char *ptr = cmd;
wl_nan_tlv_data_t tlv_data;
s32 ret = BCME_OK;
u16 kflags = in_atomic() ? GFP_ATOMIC : GFP_KERNEL;
uint16 nanioc_size = sizeof(wl_nan_ioc_t) + NAN_IOCTL_BUF_SIZE;
nanioc = kzalloc(nanioc_size, kflags);
if (!nanioc) {
WL_ERR((" memory allocation failed \n"));
return -ENOMEM;
}
/*
* command to test
*
* wl: wl nan status
*
* wpa_cli: DRIVER NAN_STATUS
*/
/* nan status */
nanioc->version = htod16(WL_NAN_IOCTL_VERSION);
nanioc->id = htod16(WL_NAN_CMD_STATUS);
pxtlv = nanioc->data;
nanioc->len = NAN_IOCTL_BUF_SIZE;
nanioc_size = sizeof(wl_nan_ioc_t) + sizeof(bcm_xtlv_t);
ret = wldev_iovar_getbuf(ndev, "nan", nanioc, nanioc_size,
cfg->ioctl_buf, WLC_IOCTL_MEDLEN, NULL);
if (unlikely(ret)) {
WL_ERR((" nan status failed, error = %d \n", ret));
goto fail;
} else {
WL_DBG((" nan status successful \n"));
}
/* unpack the tlvs */
memset(&tlv_data, 0, sizeof(tlv_data));
nanioc = (wl_nan_ioc_t *)cfg->ioctl_buf;
if (g_nan_debug) {
prhex(" nanioc->data: ", (uint8 *)nanioc->data, nanioc->len);
}
bcm_unpack_xtlv_buf(&tlv_data, nanioc->data, nanioc->len,
wl_cfgnan_set_vars_cbfn);
ptr += sprintf(ptr, CLUS_ID_PREFIX MACF, ETHER_TO_MACF(tlv_data.clus_id));
ptr += sprintf(ptr, " " ROLE_PREFIX"%d", tlv_data.dev_role);
ptr += sprintf(ptr, " " AMR_PREFIX);
ptr += bcm_format_hex(ptr, tlv_data.amr, NAN_MASTER_RANK_LEN);
ptr += sprintf(ptr, " " AMBTT_PREFIX"0x%x", tlv_data.ambtt);
ptr += sprintf(ptr, " " HOP_COUNT_PREFIX"%d", tlv_data.hop_count);
WL_DBG((" formatted string for userspace: %s, len: %zu \n",
cmd, strlen(cmd)));
fail:
if (nanioc) {
kfree(nanioc);
}
if (tlv_data.svc_info.data) {
kfree(tlv_data.svc_info.data);
tlv_data.svc_info.data = NULL;
tlv_data.svc_info.dlen = 0;
}
if (tlv_data.vend_info.data) {
kfree(tlv_data.vend_info.data);
tlv_data.vend_info.data = NULL;
tlv_data.vend_info.dlen = 0;
}
return ret;
}
s32
wl_cfgnan_notify_nan_status(struct bcm_cfg80211 *cfg,
bcm_struct_cfgdev *cfgdev, const wl_event_msg_t *event, void *data)
{
s32 ret = BCME_OK;
u16 data_len;
u32 event_num;
s32 event_type;
nan_event_hdr_t nan_hdr;
wl_nan_tlv_data_t tlv_data;
u8 *buf = NULL;
u32 buf_len;
u8 *ptr;
u16 kflags = in_atomic() ? GFP_ATOMIC : GFP_KERNEL;
if (!event || !data) {
WL_ERR((" event data is NULL \n"));
return -EINVAL;
}
event_type = ntoh32(event->event_type);
event_num = ntoh32(event->reason);
data_len = ntoh32(event->datalen);
memset(&nan_hdr, 0, sizeof(nan_event_hdr_t));
nan_hdr.event_subtype = event_num;
WL_DBG((" nan event: type: %d num: %d len: %d \n",
event_type, event_num, data_len));
if (INVALID_NAN_EVENT(event_num)) {
WL_ERR((" unsupported event, num: %d \n", event_num));
return -EINVAL;
}
if (g_nan_debug) {
WL_DBG((" event name: %s \n", nan_event_name[event_num]));
WL_DBG((" event data: \n"));
prhex(NULL, data, data_len);
}
/* unpack the tlvs */
memset(&tlv_data, 0, sizeof(wl_nan_tlv_data_t));
bcm_unpack_xtlv_buf(&tlv_data, data, data_len,
wl_cfgnan_set_vars_cbfn);
/*
* send as preformatted hex string
*
* EVENT_NAN <event_type> <tlv_hex_string>
*/
buf_len = NAN_IOCTL_BUF_SIZE;
buf = ptr = kzalloc(buf_len, kflags);
if (!buf) {
WL_ERR((" memory allocation failed \n"));
ret = -ENOMEM;
goto fail;
}
switch (event_num) {
case WL_NAN_EVENT_START:
case WL_NAN_EVENT_JOIN:
case WL_NAN_EVENT_STOP:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " CLUS_ID_PREFIX MACF,
nan_event_str[event_num], ETHER_TO_MACF(tlv_data.nstatus.cid));
break;
case WL_NAN_EVENT_ROLE:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s "ROLE_PREFIX "%d "
CLUS_ID_PREFIX MACF, nan_event_str[event_num],
tlv_data.nstatus.role, ETHER_TO_MACF(tlv_data.nstatus.cid));
break;
case WL_NAN_EVENT_DISCOVERY_RESULT:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " PUB_ID_PREFIX"%d "
SUB_ID_PREFIX"%d " MAC_ADDR_PREFIX MACF,
nan_event_str[event_num], tlv_data.pub_id, tlv_data.sub_id,
ETHER_TO_MACF(tlv_data.mac_addr));
if (tlv_data.svc_info.data && tlv_data.svc_info.dlen) {
WL_DBG((" service info present \n"));
if ((strlen(ptr) + tlv_data.svc_info.dlen) >= buf_len) {
WL_ERR((" service info length = %d\n",
tlv_data.svc_info.dlen));
WL_ERR((" insufficent buffer to copy service info \n"));
ret = -EOVERFLOW;
goto fail;
}
ptr += sprintf(ptr, " %s", SVC_INFO_PREFIX);
ptr += bcm_format_hex(ptr, tlv_data.svc_info.data,
tlv_data.svc_info.dlen);
} else {
WL_DBG((" service info not present \n"));
}
if (tlv_data.vend_info.data && tlv_data.vend_info.dlen) {
struct ether_addr *ea;
u8 *data = tlv_data.vend_info.data;
uint32 bitmap;
u16 dlen = tlv_data.vend_info.dlen;
chanspec_t chanspec;
uint8 mapcontrol;
uint8 proto;
WL_DBG((" vendor info present \n"));
if ((*data != NAN_ATTR_VENDOR_SPECIFIC) ||
(dlen < NAN_VENDOR_HDR_SIZE)) {
WL_ERR((" error in vendor info attribute \n"));
ret = -EINVAL;
goto fail;
} else {
WL_DBG((" vendor info not present \n"));
}
if (*(data + 6) == NAN_VENDOR_TYPE_RTT) {
data += NAN_VENDOR_HDR_SIZE;
ea = (struct ether_addr *)data;
data += ETHER_ADDR_LEN;
mapcontrol = *data++;
proto = *data++;
bitmap = *(uint32 *)data;
data += 4;
chanspec = *(chanspec_t *)data;
ptr += sprintf(ptr, " "BITMAP_PREFIX"0x%x "CHAN_PREFIX"%d/%s",
bitmap, wf_chspec_ctlchan(chanspec),
wf_chspec_to_bw_str(chanspec));
WL_DBG((" bitmap: 0x%x channel: %d bandwidth: %s \n", bitmap,
wf_chspec_ctlchan(chanspec),
wf_chspec_to_bw_str(chanspec)));
}
}
break;
case WL_NAN_EVENT_REPLIED:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " PUB_ID_PREFIX"%d "
MAC_ADDR_PREFIX MACF, nan_event_str[event_num],
tlv_data.pub_id, ETHER_TO_MACF(tlv_data.mac_addr));
break;
case WL_NAN_EVENT_TERMINATED:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " PUB_ID_PREFIX"%d ",
nan_event_str[event_num], tlv_data.pub_id);
break;
case WL_NAN_EVENT_RECEIVE:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " PUB_ID_PREFIX"%d "
MAC_ADDR_PREFIX MACF, nan_event_str[event_num],
tlv_data.pub_id, ETHER_TO_MACF(tlv_data.mac_addr));
if (tlv_data.svc_info.data && tlv_data.svc_info.dlen) {
WL_DBG((" service info present \n"));
if ((strlen(ptr) + tlv_data.svc_info.dlen) >= buf_len) {
WL_ERR((" service info length = %d\n",
tlv_data.svc_info.dlen));
WL_ERR((" insufficent buffer to copy service info \n"));
ret = -EOVERFLOW;
goto fail;
}
ptr += sprintf(ptr, " %s", SVC_INFO_PREFIX);
ptr += bcm_format_hex(ptr, tlv_data.svc_info.data,
tlv_data.svc_info.dlen);
} else {
WL_DBG((" service info not present \n"));
}
break;
case WL_NAN_EVENT_SCAN_COMPLETE:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " CLUS_ID_PREFIX MACF,
nan_event_str[event_num], ETHER_TO_MACF(tlv_data.nstatus.cid));
break;
case WL_NAN_EVENT_STATUS_CHG:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " CLUS_ID_PREFIX MACF,
nan_event_str[event_num], ETHER_TO_MACF(tlv_data.nstatus.cid));
break;
case WL_NAN_EVENT_MERGE:
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " CLUS_ID_PREFIX MACF,
nan_event_str[event_num], ETHER_TO_MACF(tlv_data.nstatus.cid));
break;
default:
WL_ERR((" unknown event \n"));
break;
}
#ifdef WL_GENL
/* send the preformatted string to the upper layer as event */
WL_DBG((" formatted string for userspace: %s, len: %zu \n",
buf, strlen(buf)));
wl_genl_send_msg(bcmcfg_to_prmry_ndev(cfg), 0, buf, strlen(buf), 0, 0);
#endif /* WL_GENL */
fail:
if (buf) {
kfree(buf);
}
if (tlv_data.svc_info.data) {
kfree(tlv_data.svc_info.data);
tlv_data.svc_info.data = NULL;
tlv_data.svc_info.dlen = 0;
}
if (tlv_data.vend_info.data) {
kfree(tlv_data.vend_info.data);
tlv_data.vend_info.data = NULL;
tlv_data.vend_info.dlen = 0;
}
return ret;
}
s32
wl_cfgnan_notify_proxd_status(struct bcm_cfg80211 *cfg,
bcm_struct_cfgdev *cfgdev, const wl_event_msg_t *event, void *data)
{
s32 ret = BCME_OK;
wl_nan_ranging_event_data_t *rdata;
s32 status;
u16 data_len;
s32 event_type;
s32 event_num;
u8 *buf = NULL;
u32 buf_len;
u8 *ptr;
u16 kflags = in_atomic() ? GFP_ATOMIC : GFP_KERNEL;
s32 i;
if (!event || !data) {
WL_ERR((" event data is NULL \n"));
return -EINVAL;
}
status = ntoh32(event->reason);
event_type = ntoh32(event->event_type);
event_num = ntoh32(event->reason);
data_len = ntoh32(event->datalen);
WL_DBG((" proxd event: type: %d num: %d len: %d \n",
event_type, event_num, data_len));
if (INVALID_PROXD_EVENT(event_num)) {
WL_ERR((" unsupported event, num: %d \n", event_num));
return -EINVAL;
}
if (g_nan_debug) {
WL_DBG((" event name: WLC_E_PROXD_NAN_EVENT \n"));
WL_DBG((" event data: \n"));
prhex(NULL, data, data_len);
}
if (data_len < sizeof(wl_nan_ranging_event_data_t)) {
WL_ERR((" wrong data len \n"));
return -EINVAL;
}
rdata = (wl_nan_ranging_event_data_t *)data;
WL_DBG((" proxd event: count:%d success_count:%d mode:%d \n",
rdata->count, rdata->success_count, rdata->mode));
if (g_nan_debug) {
prhex(" event data: ", data, data_len);
}
buf_len = NAN_IOCTL_BUF_SIZE;
buf = kzalloc(buf_len, kflags);
if (!buf) {
WL_ERR((" memory allocation failed \n"));
return -ENOMEM;
}
for (i = 0; i < rdata->count; i++) {
if (&rdata->rr[i] == NULL) {
ret = -EINVAL;
goto fail;
}
ptr = buf;
WL_DBG((" ranging data for mac:"MACDBG" \n",
MAC2STRDBG(rdata->rr[i].ea.octet)));
ptr += sprintf(buf, SUPP_EVENT_PREFIX"%s " MAC_ADDR_PREFIX MACF
" "STATUS_PREFIX"%s", EVENT_RTT_STATUS_STR,
ETHER_TO_MACF(rdata->rr[i].ea), (rdata->rr[i].status == 1) ?
"success" : "fail");
if (rdata->rr[i].status == 1) {
/* add tsf and distance only if status is success */
ptr += sprintf(ptr, " "TIMESTAMP_PREFIX"0x%x "
DISTANCE_PREFIX"%d.%04d", rdata->rr[i].timestamp,
rdata->rr[i].distance >> 4,
((rdata->rr[i].distance & 0x0f) * 625));
}
#ifdef WL_GENL
/* send the preformatted string to the upper layer as event */
WL_DBG((" formatted string for userspace: %s, len: %zu \n",
buf, strlen(buf)));
wl_genl_send_msg(bcmcfg_to_prmry_ndev(cfg), 0, buf, strlen(buf), 0, 0);
#endif /* WL_GENL */
}