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
brcmf_cfg80211_vndr_cmds_dcmd_handler(struct wiphy * wiphy,struct wireless_dev * wdev,const void * data,int len)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 #if defined(__linux__)
64 memcpy(dcmd_buf, (void *)cmdhdr + cmdhdr->offset, len);
65 *(char *)(dcmd_buf + len) = '\0';
66 #elif defined(__FreeBSD__)
67 memcpy(dcmd_buf, (void *)((uintptr_t)cmdhdr + cmdhdr->offset), len);
68 *(char *)((uintptr_t)dcmd_buf + len) = '\0';
69 #endif
70 }
71
72 if (cmdhdr->set)
73 ret = brcmf_fil_cmd_data_set(ifp, cmdhdr->cmd, dcmd_buf,
74 ret_len);
75 else
76 ret = brcmf_fil_cmd_data_get(ifp, cmdhdr->cmd, dcmd_buf,
77 ret_len);
78 if (ret != 0)
79 goto exit;
80
81 wr_pointer = dcmd_buf;
82 while (ret_len > 0) {
83 msglen = ret_len > maxmsglen ? maxmsglen : ret_len;
84 ret_len -= msglen;
85 payload = msglen + sizeof(msglen);
86 reply = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, payload);
87 if (NULL == reply) {
88 ret = -ENOMEM;
89 break;
90 }
91
92 if (nla_put(reply, BRCMF_NLATTR_DATA, msglen, wr_pointer) ||
93 nla_put_u16(reply, BRCMF_NLATTR_LEN, msglen)) {
94 kfree_skb(reply);
95 ret = -ENOBUFS;
96 break;
97 }
98
99 ret = cfg80211_vendor_cmd_reply(reply);
100 if (ret)
101 break;
102
103 #if defined(__linux__)
104 wr_pointer += msglen;
105 #elif defined(__FreeBSD__)
106 wr_pointer = (void *)((uintptr_t)wr_pointer + msglen);
107 #endif
108 }
109
110 exit:
111 vfree(dcmd_buf);
112
113 return ret;
114 }
115
116 const struct wiphy_vendor_command brcmf_vendor_cmds[] = {
117 {
118 {
119 .vendor_id = BROADCOM_OUI,
120 .subcmd = BRCMF_VNDR_CMDS_DCMD
121 },
122 .flags = WIPHY_VENDOR_CMD_NEED_WDEV |
123 WIPHY_VENDOR_CMD_NEED_NETDEV,
124 .policy = VENDOR_CMD_RAW_DATA,
125 .doit = brcmf_cfg80211_vndr_cmds_dcmd_handler
126 },
127 };
128