| 1 |
// SPDX-License-Identifier: ISC |
| 2 |
/* |
| 3 |
* Copyright (c) 2014 Broadcom Corporation |
| 4 |
*/ |
| 5 |
|
| 6 |
#include <linux/vmalloc.h> |
| 7 |
#include <net/cfg80211.h> |
| 8 |
#include <net/netlink.h> |
| 9 |
|
| 10 |
#include <brcmu_wifi.h> |
| 11 |
#include "fwil_types.h" |
| 12 |
#include "core.h" |
| 13 |
#include "p2p.h" |
| 14 |
#include "debug.h" |
| 15 |
#include "cfg80211.h" |
| 16 |
#include "vendor.h" |
| 17 |
#include "fwil.h" |
| 18 |
|
| 19 |
static int brcmf_cfg80211_vndr_cmds_dcmd_handler(struct wiphy *wiphy, |
| 20 |
struct wireless_dev *wdev, |
| 21 |
const void *data, int len) |
| 22 |
{ |
| 23 |
struct brcmf_cfg80211_vif *vif; |
| 24 |
struct brcmf_if *ifp; |
| 25 |
const struct brcmf_vndr_dcmd_hdr *cmdhdr = data; |
| 26 |
struct sk_buff *reply; |
| 27 |
unsigned int payload, ret_len; |
| 28 |
void *dcmd_buf = NULL, *wr_pointer; |
| 29 |
u16 msglen, maxmsglen = PAGE_SIZE - 0x100; |
| 30 |
int ret; |
| 31 |
|
| 32 |
if (len < sizeof(*cmdhdr)) { |
| 33 |
brcmf_err("vendor command too short: %d\n", len); |
| 34 |
return -EINVAL; |
| 35 |
} |
| 36 |
|
| 37 |
vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev); |
| 38 |
ifp = vif->ifp; |
| 39 |
|
| 40 |
brcmf_dbg(TRACE, "ifidx=%d, cmd=%d\n", ifp->ifidx, cmdhdr->cmd); |
| 41 |
|
| 42 |
if (cmdhdr->offset > len) { |
| 43 |
brcmf_err("bad buffer offset %d > %d\n", cmdhdr->offset, len); |
| 44 |
return -EINVAL; |
| 45 |
} |
| 46 |
|
| 47 |
len -= cmdhdr->offset; |
| 48 |
ret_len = cmdhdr->len; |
| 49 |
if (ret_len > 0 || len > 0) { |
| 50 |
if (len > BRCMF_DCMD_MAXLEN) { |
| 51 |
brcmf_err("oversize input buffer %d\n", len); |
| 52 |
len = BRCMF_DCMD_MAXLEN; |
| 53 |
} |
| 54 |
if (ret_len > BRCMF_DCMD_MAXLEN) { |
| 55 |
brcmf_err("oversize return buffer %d\n", ret_len); |
| 56 |
ret_len = BRCMF_DCMD_MAXLEN; |
| 57 |
} |
| 58 |
payload = max_t(unsigned int, ret_len, len) + 1; |
| 59 |
dcmd_buf = vzalloc(payload); |
| 60 |
if (NULL == dcmd_buf) |
| 61 |
return -ENOMEM; |
| 62 |
|
| 63 |
memcpy(dcmd_buf, (void *)cmdhdr + cmdhdr->offset, len); |
| 64 |
*(char *)(dcmd_buf + len) = '\0'; |
| 65 |
} |
| 66 |
|
| 67 |
if (cmdhdr->set) |
| 68 |
ret = brcmf_fil_cmd_data_set(ifp, cmdhdr->cmd, dcmd_buf, |
| 69 |
ret_len); |
| 70 |
else |
| 71 |
ret = brcmf_fil_cmd_data_get(ifp, cmdhdr->cmd, dcmd_buf, |
| 72 |
ret_len); |
| 73 |
if (ret != 0) |
| 74 |
goto exit; |
| 75 |
|
| 76 |
wr_pointer = dcmd_buf; |
| 77 |
while (ret_len > 0) { |
| 78 |
msglen = ret_len > maxmsglen ? maxmsglen : ret_len; |
| 79 |
ret_len -= msglen; |
| 80 |
payload = msglen + sizeof(msglen); |
| 81 |
reply = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, payload); |
| 82 |
if (NULL == reply) { |
| 83 |
ret = -ENOMEM; |
| 84 |
break; |
| 85 |
} |
| 86 |
|
| 87 |
if (nla_put(reply, BRCMF_NLATTR_DATA, msglen, wr_pointer) || |
| 88 |
nla_put_u16(reply, BRCMF_NLATTR_LEN, msglen)) { |
| 89 |
kfree_skb(reply); |
| 90 |
ret = -ENOBUFS; |
| 91 |
break; |
| 92 |
} |
| 93 |
|
| 94 |
ret = cfg80211_vendor_cmd_reply(reply); |
| 95 |
if (ret) |
| 96 |
break; |
| 97 |
|
| 98 |
wr_pointer += msglen; |
| 99 |
} |
| 100 |
|
| 101 |
exit: |
| 102 |
vfree(dcmd_buf); |
| 103 |
|
| 104 |
return ret; |
| 105 |
} |
| 106 |
|
| 107 |
const struct wiphy_vendor_command brcmf_vendor_cmds[] = { |
| 108 |
{ |
| 109 |
{ |
| 110 |
.vendor_id = BROADCOM_OUI, |
| 111 |
.subcmd = BRCMF_VNDR_CMDS_DCMD |
| 112 |
}, |
| 113 |
.flags = WIPHY_VENDOR_CMD_NEED_WDEV | |
| 114 |
WIPHY_VENDOR_CMD_NEED_NETDEV, |
| 115 |
.policy = VENDOR_CMD_RAW_DATA, |
| 116 |
.doit = brcmf_cfg80211_vndr_cmds_dcmd_handler |
| 117 |
}, |
| 118 |
}; |