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 #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