wl/src/wl/exe/wlu_nl80211.c

/*
 * nl80211 linux driver interface.
 *
 * Copyright (C) 2017, Broadcom. All Rights Reserved.
 * 
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *
 * <<Broadcom-WL-IPTag/Open:>>
 *
 * $Id: $
 */
#include <errno.h>
#include <linux/nl80211.h>
#include <wlioctl.h>
#include <brcm_nl80211.h>
#include "wlu_nl80211.h"

static struct nla_policy wl_nl_policy[BCM_NLATTR_MAX + 1] = {
	[BCM_NLATTR_LEN] = { .type = NLA_U16 },
	[BCM_NLATTR_DATA] = { .type = NLA_UNSPEC },
};

/* libnl 1.x compatibility code */
#if !defined(CONFIG_LIBNL20) && !defined(CONFIG_LIBNL30)
static inline struct nl_handle *nl_socket_alloc(void)
{
	return nl_handle_alloc();
}

static inline void nl_socket_free(struct nl_sock *h)
{
	nl_handle_destroy(h);
}
#endif /* CONFIG_LIBNL20 && CONFIG_LIBNL30 */

static int wlnl_error_handler(struct sockaddr_nl * nla,	struct nlmsgerr *err, void *arg)
{
	struct nl_prv_data *prv = arg;
	UNUSED_PARAMETER(nla);

	if (err->error) {
		prv->err = err->error;
		return NL_STOP;
	}
	return NL_SKIP;
}

static int wlnl_finish_handler(struct nl_msg *msg, void *arg)
{
	UNUSED_PARAMETER(msg);
	UNUSED_PARAMETER(arg);
	return NL_SKIP;
}

static int wlnl_ack_handler(struct nl_msg *msg, void *arg)
{
	struct nl_prv_data *prv = arg;
	UNUSED_PARAMETER(msg);

	prv->err = 0;
	return NL_STOP;
}

static int wlnl_valid_handler(struct nl_msg *msg, void *arg)
{
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nlattr *attrs[NL80211_ATTR_MAX + 1];
	struct nlattr *bcmnl[BCM_NLATTR_MAX + 1];
	struct nl_prv_data *prv = arg;
	int ret;
	uint payload;

	nla_parse(attrs, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		genlmsg_attrlen(gnlh, 0), NULL);
	if (!attrs[NL80211_ATTR_VENDOR_DATA]) {
		fprintf(stderr, "Could not find attrs NL80211_ATTR_VENDOR_DATA\n");
		return NL_SKIP;
	}

	ret = nla_parse_nested(bcmnl, BCM_NLATTR_MAX,
		attrs[NL80211_ATTR_VENDOR_DATA], wl_nl_policy);
	if (ret != 0 || !bcmnl[BCM_NLATTR_LEN] || !bcmnl[BCM_NLATTR_DATA]) {
		fprintf(stderr, "nla_parse_nested error\n");
		return NL_SKIP;
	}

	payload = nla_get_u16(bcmnl[BCM_NLATTR_LEN]);
	if (payload > prv->len)
		payload = prv->len;
	memcpy(prv->data, nla_data(bcmnl[BCM_NLATTR_DATA]), payload);
	prv->data += payload;
	prv->len -= payload;

	return NL_SKIP;
}

int wlnl_sock_connect(struct wl_netlink_info *wl_nli)
{
	wl_nli->nl = nl_socket_alloc();
	if (wl_nli->nl == NULL)
		return -1;

	if (genl_connect(wl_nli->nl) < 0) {
		fprintf(stderr, "netlink connection failed\n");
		goto err;
	}

	wl_nli->nl_id = genl_ctrl_resolve(wl_nli->nl, "nl80211");
	if (wl_nli->nl_id < 0) {
		fprintf(stderr, "'nl80211' netlink not found\n");
		goto err;
	}

	wl_nli->cb = nl_cb_alloc(NL_CB_DEFAULT);
	if (wl_nli->cb == NULL)
		goto err;

	nl_socket_set_cb(wl_nli->nl, wl_nli->cb);
	return 0;

err:
	nl_cb_put(wl_nli->cb);
	nl_socket_free(wl_nli->nl);
	fprintf(stderr, "nl80211 connection failed\n");
	return -1;
}

void wlnl_sock_disconnect(struct wl_netlink_info *wl_nli)
{
	nl_cb_put(wl_nli->cb);
	nl_socket_free(wl_nli->nl);
}

int wlnl_do_vndr_cmd(struct wl_netlink_info *wl_nli, wl_ioctl_t *ioc)
{
	struct nl_msg *msg;
	struct nl_prv_data prv_dat;
	struct bcm_nlmsg_hdr *nlioc;
	uint msglen = ioc->len;

	msg = nlmsg_alloc();
	if (msg == NULL)
		return -ENOMEM;

	/* nlmsg_alloc() can only allocate default_pagesize packet, cap
	 * any buffer send down to 1536 bytes
	 * DO NOT switch to nlmsg_alloc_size because Android doesn't support it
	 */
	if (msglen > 0x600)
		msglen = 0x600;
	msglen += sizeof(struct bcm_nlmsg_hdr);
	nlioc = malloc(msglen);
	if (nlioc == NULL) {
		nlmsg_free(msg);
		return -ENOMEM;
	}
	nlioc->cmd = ioc->cmd;
	nlioc->len = ioc->len;
	nlioc->offset = sizeof(struct bcm_nlmsg_hdr);
	nlioc->set = ioc->set;
	nlioc->magic = 0;
	memcpy(((void *)nlioc) + nlioc->offset, ioc->buf, msglen - nlioc->offset);

	/* fill testmode message */
	genlmsg_put(msg, 0, 0, wl_nli->nl_id, 0, 0, NL80211_CMD_VENDOR, 0);
	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, wl_nli->ifidx);
	NLA_PUT_U32(msg, NL80211_ATTR_VENDOR_ID, OUI_BRCM);
	NLA_PUT_U32(msg, NL80211_ATTR_VENDOR_SUBCMD, BRCM_VENDOR_SCMD_PRIV_STR);
	NLA_PUT(msg, NL80211_ATTR_VENDOR_DATA, msglen, nlioc);

	prv_dat.err = nl_send_auto_complete(wl_nli->nl, msg);
	if (prv_dat.err < 0) {
		goto out;
	}

	prv_dat.err = 1;
	prv_dat.nlioc = nlioc;
	prv_dat.data = ioc->buf;
	prv_dat.len = ioc->len;
	nl_cb_err(wl_nli->cb, NL_CB_CUSTOM, wlnl_error_handler, &prv_dat);
	nl_cb_set(wl_nli->cb, NL_CB_ACK, NL_CB_CUSTOM,
		wlnl_ack_handler, &prv_dat);
	nl_cb_set(wl_nli->cb, NL_CB_FINISH, NL_CB_CUSTOM,
		wlnl_finish_handler, &prv_dat);
	nl_cb_set(wl_nli->cb, NL_CB_VALID, NL_CB_CUSTOM,
		wlnl_valid_handler, &prv_dat);
	while (prv_dat.err > 0)
		nl_recvmsgs(wl_nli->nl, wl_nli->cb);

out:
	free(nlioc);
	nlmsg_free(msg);
	return prv_dat.err;

nla_put_failure:
	fprintf(stderr, "setting netlink attribute failed\n");
	prv_dat.err = -EFAULT;
	goto out;
}