brcm80211/brcmfmac/vendor.c

0001 // SPDX-License-Identifier: ISC
0002 /*
0003  * Copyright (c) 2014 Broadcom Corporation
0004  */
0005
0006 #include <linux/vmalloc.h>
0007 #include <net/cfg80211.h>
0008 #include <net/netlink.h>
0009
0010 #include <brcmu_wifi.h>
0011 #include "fwil_types.h"
0012 #include "core.h"
0013 #include "p2p.h"
0014 #include "debug.h"
0015 #include "cfg80211.h"
0016 #include "vendor.h"
0017 #include "fwil.h"
0018
0019 static int brcmf_cfg80211_vndr_cmds_dcmd_handler(struct wiphy *wiphy,
0020                          struct wireless_dev *wdev,
0021                          const void *data, int len)
0022 {
0023     struct brcmf_cfg80211_vif *vif;
0024     struct brcmf_if *ifp;
0025     const struct brcmf_vndr_dcmd_hdr *cmdhdr = data;
0026     struct sk_buff *reply;
0027     unsigned int payload, ret_len;
0028     void *dcmd_buf = NULL, *wr_pointer;
0029     u16 msglen, maxmsglen = PAGE_SIZE - 0x100;
0030     int ret;
0031
0032     if (len < sizeof(*cmdhdr)) {
0033         brcmf_err("vendor command too short: %d\n", len);
0034         return -EINVAL;
0035     }
0036
0037     vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
0038     ifp = vif->ifp;
0039
0040     brcmf_dbg(TRACE, "ifidx=%d, cmd=%d\n", ifp->ifidx, cmdhdr->cmd);
0041
0042     if (cmdhdr->offset > len) {
0043         brcmf_err("bad buffer offset %d > %d\n", cmdhdr->offset, len);
0044         return -EINVAL;
0045     }
0046
0047     len -= cmdhdr->offset;
0048     ret_len = cmdhdr->len;
0049     if (ret_len > 0 || len > 0) {
0050         if (len > BRCMF_DCMD_MAXLEN) {
0051             brcmf_err("oversize input buffer %d\n", len);
0052             len = BRCMF_DCMD_MAXLEN;
0053         }
0054         if (ret_len > BRCMF_DCMD_MAXLEN) {
0055             brcmf_err("oversize return buffer %d\n", ret_len);
0056             ret_len = BRCMF_DCMD_MAXLEN;
0057         }
0058         payload = max_t(unsigned int, ret_len, len) + 1;
0059         dcmd_buf = vzalloc(payload);
0060         if (NULL == dcmd_buf)
0061             return -ENOMEM;
0062
0063         memcpy(dcmd_buf, (void *)cmdhdr + cmdhdr->offset, len);
0064         *(char *)(dcmd_buf + len)  = '\0';
0065     }
0066
0067     if (cmdhdr->set)
0068         ret = brcmf_fil_cmd_data_set(ifp, cmdhdr->cmd, dcmd_buf,
0069                          ret_len);
0070     else
0071         ret = brcmf_fil_cmd_data_get(ifp, cmdhdr->cmd, dcmd_buf,
0072                          ret_len);
0073     if (ret != 0)
0074         goto exit;
0075
0076     wr_pointer = dcmd_buf;
0077     while (ret_len > 0) {
0078         msglen = ret_len > maxmsglen ? maxmsglen : ret_len;
0079         ret_len -= msglen;
0080         payload = msglen + sizeof(msglen);
0081         reply = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, payload);
0082         if (NULL == reply) {
0083             ret = -ENOMEM;
0084             break;
0085         }
0086
0087         if (nla_put(reply, BRCMF_NLATTR_DATA, msglen, wr_pointer) ||
0088             nla_put_u16(reply, BRCMF_NLATTR_LEN, msglen)) {
0089             kfree_skb(reply);
0090             ret = -ENOBUFS;
0091             break;
0092         }
0093
0094         ret = cfg80211_vendor_cmd_reply(reply);
0095         if (ret)
0096             break;
0097
0098         wr_pointer += msglen;
0099     }
0100
0101 exit:
0102     vfree(dcmd_buf);
0103
0104     return ret;
0105 }
0106
0107 const struct wiphy_vendor_command brcmf_vendor_cmds[] = {
0108     {
0109         {
0110             .vendor_id = BROADCOM_OUI,
0111             .subcmd = BRCMF_VNDR_CMDS_DCMD
0112         },
0113         .flags = WIPHY_VENDOR_CMD_NEED_WDEV |
0114              WIPHY_VENDOR_CMD_NEED_NETDEV,
0115         .policy = VENDOR_CMD_RAW_DATA,
0116         .doit = brcmf_cfg80211_vndr_cmds_dcmd_handler
0117     },
0118 };