Quelle  vendor.c   Sprache: C

// SPDX-License-Identifier: ISC
/*
 * Copyright (c) 2014 Broadcom Corporation
 */

#include <linux/vmalloc.h>
#include <net/cfg80211.h>
#include <net/netlink.h>

#include <brcmu_wifi.h>
#include "fwil_types.h"
#include "core.h"
#include "p2p.h"
#include "debug.h"
#include "cfg80211.h"
#include "vendor.h"
#include "fwil.h"

static int brcmf_cfg80211_vndr_cmds_dcmd_handler(struct wiphy *wiphy,
       struct wireless_dev *wdev,
       const void *data, int len)
{
 struct brcmf_cfg80211_vif *vif;
 struct brcmf_if *ifp;
 const struct brcmf_vndr_dcmd_hdr *cmdhdr = data;
 struct sk_buff *reply;
 unsigned int payload, ret_len;
 void *dcmd_buf = NULL, *wr_pointer;
 u16 msglen, maxmsglen = PAGE_SIZE - 0x100;
 int ret;

 if (len < sizeof(*cmdhdr)) {
  brcmf_err("vendor command too short: %d\n", len);
  return -EINVAL;
 }

 vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
 ifp = vif->ifp;

 brcmf_dbg(TRACE, "ifidx=%d, cmd=%d\n", ifp->ifidx, cmdhdr->cmd);

 if (cmdhdr->offset > len) {
  brcmf_err("bad buffer offset %d > %d\n", cmdhdr->offset, len);
  return -EINVAL;
 }

 len -= cmdhdr->offset;
 ret_len = cmdhdr->len;
 if (ret_len > 0 || len > 0) {
  if (len > BRCMF_DCMD_MAXLEN) {
   brcmf_err("oversize input buffer %d\n", len);
   len = BRCMF_DCMD_MAXLEN;
  }
  if (ret_len > BRCMF_DCMD_MAXLEN) {
   brcmf_err("oversize return buffer %d\n", ret_len);
   ret_len = BRCMF_DCMD_MAXLEN;
  }
  payload = max_t(unsigned int, ret_len, len) + 1;
  dcmd_buf = vzalloc(payload);
  if (NULL == dcmd_buf)
   return -ENOMEM;

  memcpy(dcmd_buf, (void *)cmdhdr + cmdhdr->offset, len);
  *(char *)(dcmd_buf + len)  = '\0';
 }

 if (cmdhdr->set)
  ret = brcmf_fil_cmd_data_set(ifp, cmdhdr->cmd, dcmd_buf,
          ret_len);
 else
  ret = brcmf_fil_cmd_data_get(ifp, cmdhdr->cmd, dcmd_buf,
          ret_len);
 if (ret != 0)
  goto exit;

 wr_pointer = dcmd_buf;
 while (ret_len > 0) {
  msglen = ret_len > maxmsglen ? maxmsglen : ret_len;
  ret_len -= msglen;
  payload = msglen + sizeof(msglen);
  reply = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, payload);
  if (NULL == reply) {
   ret = -ENOMEM;
   break;
  }

  if (nla_put(reply, BRCMF_NLATTR_DATA, msglen, wr_pointer) ||
      nla_put_u16(reply, BRCMF_NLATTR_LEN, msglen)) {
   kfree_skb(reply);
   ret = -ENOBUFS;
   break;
  }

  ret = cfg80211_vendor_cmd_reply(reply);
  if (ret)
   break;

  wr_pointer += msglen;
 }

exit:
 vfree(dcmd_buf);

 return ret;
}

const struct wiphy_vendor_command brcmf_vendor_cmds[] = {
 {
  {
   .vendor_id = BROADCOM_OUI,
   .subcmd = BRCMF_VNDR_CMDS_DCMD
  },
  .flags = WIPHY_VENDOR_CMD_NEED_WDEV |
    WIPHY_VENDOR_CMD_NEED_NETDEV,
  .policy = VENDOR_CMD_RAW_DATA,
  .doit = brcmf_cfg80211_vndr_cmds_dcmd_handler
 },
};