119 lines · c
1// SPDX-License-Identifier: ISC2/*3 * Copyright (c) 2014 Broadcom Corporation4 */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 19static 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 else71 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 101exit:102 vfree(dcmd_buf);103 104 return ret;105}106 107const struct wiphy_vendor_command brcmf_vendor_cmds[] = {108 {109 {110 .vendor_id = BROADCOM_OUI,111 .subcmd = BRCMF_VNDR_CMDS_DCMD112 },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_handler117 },118};119