android13/external/wifi_driver/bcmdhd/wl_ext_genl.c

#ifdef WL_EXT_GENL
#include <bcmendian.h>
#include <wl_android.h>
#include <dhd_config.h>
#include <net/genetlink.h>

#define AGENL_ERROR(name, arg1, args...) \
	do { \
		if (android_msg_level & ANDROID_ERROR_LEVEL) { \
			printf("[%s] AGENL-ERROR) %s : " arg1, name, __func__, ## args); \
		} \
	} while (0)
#define AGENL_TRACE(name, arg1, args...) \
	do { \
		if (android_msg_level & ANDROID_TRACE_LEVEL) { \
			printf("[%s] AGENL-TRACE) %s : " arg1, name, __func__, ## args); \
		} \
	} while (0)
#define AGENL_INFO(name, arg1, args...) \
	do { \
		if (android_msg_level & ANDROID_INFO_LEVEL) { \
			printf("[%s] AGENL-INFO) %s : " arg1, name, __func__, ## args); \
		} \
	} while (0)

#define htod32(i) i
#define htod16(i) i
#define dtoh32(i) i
#define dtoh16(i) i

#ifdef SENDPROB
#define MGMT_PROBE_REQ 0x40
#define MGMT_PROBE_RES 0x50
#endif

enum {
	__GENL_CUSTOM_ATTR_INVALID,
	GENL_CUSTOM_ATTR_MSG,	/* message */
	__GENL_CUSTOM_ATTR_MAX,
};

enum {
	__GENLL_CUSTOM_COMMAND_INVALID,
	GENL_CUSTOM_COMMAND_BIND,	/* bind */
	GENL_CUSTOM_COMMAND_SEND,	/* user -> kernel */
	GENL_CUSTOM_COMMAND_RECV,	/* kernel -> user */
	__GENL_CUSTOM_COMMAND_MAX,
};

#if defined(ALIBABA_ZEROCONFIG)
#define GENL_FAMILY_NAME	"WIFI_NL_CUSTOM"
#define PROBE_RSP_DST_MAC_OFFSET	4
#define PROBE_RSP_VNDR_ID_OFFSET	55
#else
#define GENL_FAMILY_NAME	"WLAN_NL_CUSTOM"
#define PROBE_RSP_DST_MAC_OFFSET	4
#define PROBE_RSP_VNDR_ID_OFFSET	DOT11_MGMT_HDR_LEN
#endif
#define PROBE_RSP_VNDR_LEN_OFFSET	(PROBE_RSP_VNDR_ID_OFFSET+1)
#define PROBE_RSP_VNDR_OUI_OFFSET	(PROBE_RSP_VNDR_ID_OFFSET+2)
#define MAX_CUSTOM_PKT_LENGTH	2048
#define GENL_CUSTOM_ATTR_MAX	(__GENL_CUSTOM_ATTR_MAX - 1)
#define GENLMSG_UNICAST_RETRY_LIMIT 5

typedef struct genl_params {
	struct net_device *dev;
	bool bind;
	int pm;
	int bind_pid;
	int send_retry_cnt;
} genl_params_t;

struct genl_params *g_zconf = NULL;

static int wl_ext_genl_bind(struct sk_buff *skb, struct genl_info *info);
static int wl_ext_genl_recv(struct sk_buff *skb, struct genl_info *info);
static int wl_ext_genl_send(struct genl_params *zconf, struct net_device *dev,
	char* buf, int buf_len);

static struct nla_policy wl_ext_genl_policy[GENL_CUSTOM_ATTR_MAX + 1] = {
	[GENL_CUSTOM_ATTR_MSG] = {.type = NLA_NUL_STRING},
};

static struct genl_ops wl_ext_genl_ops[] = {
	{
		.cmd = GENL_CUSTOM_COMMAND_BIND,
		.flags = 0,
		.policy = wl_ext_genl_policy,
		.doit = wl_ext_genl_bind,
		.dumpit = NULL,
	},
	{
		.cmd = GENL_CUSTOM_COMMAND_SEND,
		.flags = 0,
		.policy = wl_ext_genl_policy,
		.doit = wl_ext_genl_recv,
		.dumpit = NULL,
	},
};

static struct genl_family wl_ext_genl_family = {
	.id = GENL_ID_GENERATE,
	.hdrsize = 0,
	.name = GENL_FAMILY_NAME,
	.version = 1,
	.maxattr = GENL_CUSTOM_ATTR_MAX,
};

#ifdef SENDPROB
static int
wl_ext_add_del_ie_hex(struct net_device *dev, uint pktflag,
	char *ie_data, int ie_len, const char* add_del_cmd)
{
	vndr_ie_setbuf_t *vndr_ie = NULL;
	char iovar_buf[WLC_IOCTL_SMLEN]="\0";
	int tot_len = 0, iecount;
	int err = -1;

	if (!ie_len) {
		AGENL_ERROR(dev->name, "wrong ie_len %d\n", ie_len);
		goto exit;
	}

	tot_len = (int)(sizeof(vndr_ie_setbuf_t) + (ie_len));
	vndr_ie = (vndr_ie_setbuf_t *) kzalloc(tot_len, GFP_KERNEL);
	if (!vndr_ie) {
		AGENL_ERROR(dev->name, "IE memory alloc failed\n");
		err = -ENOMEM;
		goto exit;
	}

	/* Copy the vndr_ie SET command ("add"/"del") to the buffer */
	strncpy(vndr_ie->cmd, add_del_cmd, VNDR_IE_CMD_LEN - 1);
	vndr_ie->cmd[VNDR_IE_CMD_LEN - 1] = '\0';

	/* Set the IE count - the buffer contains only 1 IE */
	iecount = htod32(1);
	memcpy((void *)&vndr_ie->vndr_ie_buffer.iecount, &iecount, sizeof(s32));

	/* Set packet flag to indicate that BEACON's will contain this IE */
	pktflag = htod32(pktflag);
	memcpy((void *)&vndr_ie->vndr_ie_buffer.vndr_ie_list[0].pktflag, &pktflag,
		sizeof(u32));

	/* Set the IE ID */
	vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.id = (uchar)DOT11_MNG_VS_ID;

	/* Set the IE LEN */
	vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.len = ie_len;

	/* Set the IE OUI and DATA */
	memcpy((char *)vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.oui, ie_data, ie_len);

	err = wldev_iovar_setbuf(dev, "vndr_ie", vndr_ie, tot_len,
		iovar_buf, sizeof(iovar_buf), NULL);
	if (err != 0)
		AGENL_ERROR(dev->name, "vndr_ie, ret=%d\n", err);

exit:
	if (vndr_ie) {
		kfree(vndr_ie);
	}
	return err;
}

static int
wl_ext_send_probersp(struct net_device *dev, char* buf, int buf_len)
{
	char addr[ETHER_ADDR_LEN], *pVndrOUI;
	char iovar_buf[WLC_IOCTL_SMLEN]="\0";
	int err = -1, ie_len;

	if (buf == NULL || buf_len <= 0){
		AGENL_ERROR(dev->name, "buf is NULL or buf_len <= 0\n");
		return -1;
	}

	AGENL_TRACE(dev->name, "Enter\n");

	memcpy(addr, (buf+PROBE_RSP_DST_MAC_OFFSET), ETHER_ADDR_LEN);
	pVndrOUI = (buf+PROBE_RSP_VNDR_OUI_OFFSET);
	ie_len = *(buf+PROBE_RSP_VNDR_LEN_OFFSET);

	if (ie_len > (buf_len-PROBE_RSP_VNDR_OUI_OFFSET)) {
		AGENL_ERROR(dev->name, "wrong vendor ie len %d\n", ie_len);
		return -1;
	}

	err = wl_ext_add_del_ie_hex(dev, VNDR_IE_PRBRSP_FLAG, pVndrOUI, ie_len, "add");
	if (err)
		goto exit;

	err = wldev_iovar_setbuf(dev, "send_probresp", addr, ETHER_ADDR_LEN,
		iovar_buf, sizeof(iovar_buf), NULL);
	if (err != 0)
		AGENL_ERROR(dev->name, "vndr_ie, ret=%d\n", err);

	OSL_SLEEP(100);
	wl_ext_add_del_ie_hex(dev, VNDR_IE_PRBRSP_FLAG, pVndrOUI, ie_len, "del");

exit:
	return err;
}

static int
wl_ext_set_probreq(struct net_device *dev, bool set)
{
	int bytes_written = 0;
	char recv_probreq[32];

	AGENL_TRACE(dev->name, "Enter\n");

	if (set) {
		sprintf(recv_probreq, "wl recv_probreq 1");
		wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
	} else {
		sprintf(recv_probreq, "wl recv_probreq 0");
		wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
	}

	return 0;
}

void
wl_ext_probreq_event(struct net_device *dev, void *argu,
	const wl_event_msg_t *e, void *data)
{
	struct genl_params *zconf = (struct genl_params *)argu;
	int i, ret = 0, num_ie = 0, totlen;
	uint32 event_len = 0;
	char *buf, *pbuf;
	uint rem_len, buflen = MAX_CUSTOM_PKT_LENGTH;
	uint32 event_id[] = {DOT11_MNG_VS_ID};
	uint32 datalen = ntoh32(e->datalen);
	bcm_tlv_t *ie;

	AGENL_TRACE(dev->name, "Enter\n");

	rem_len = buflen;
	buf = kzalloc(MAX_CUSTOM_PKT_LENGTH, GFP_KERNEL);
	if (unlikely(!buf)) {
		AGENL_ERROR(dev->name, "Could not allocate buf\n");
		return;
	}

	// copy mgmt header
	pbuf = buf;
	memcpy(pbuf, data, DOT11_MGMT_HDR_LEN);
	rem_len -= (DOT11_MGMT_HDR_LEN+1);
	datalen -= DOT11_MGMT_HDR_LEN;
	data += DOT11_MGMT_HDR_LEN;

	// copy IEs
	pbuf = buf + DOT11_MGMT_HDR_LEN;
#if 1 // non-sort by id
	ie = (bcm_tlv_t*)data;
	totlen = datalen;
	while (ie && totlen >= TLV_HDR_LEN) {
		int ie_id = -1;
		int ie_len = ie->len + TLV_HDR_LEN;
		for (i=0; i<sizeof(event_id)/sizeof(event_id[0]); i++) {
			if (ie->id == event_id[i]) {
				ie_id = ie->id;
				break;
			}
		}
		if ((ie->id == ie_id) && (totlen >= ie_len) && (rem_len >= ie_len)) {
			memcpy(pbuf, ie, ie_len);
			pbuf += ie_len;
			rem_len -= ie_len;
			num_ie++;
		}
		ie = (bcm_tlv_t*)((uint8*)ie + ie_len);
		totlen -= ie_len;
	}
#else // sort by id
	for (i = 0; i < sizeof(event_id)/sizeof(event_id[0]); i++) {
		void *pdata = data;
		int data_len = datalen;
		while (rem_len > 0) {
			ie = bcm_parse_tlvs(pdata, data_len, event_id[i]);
			if (!ie)
				break;
			if (rem_len < (ie->len+TLV_HDR_LEN)) {
				ANDROID_TRACE(("%s: buffer is not enough\n", __FUNCTION__));
				break;
			}
			memcpy(pbuf, ie, min(ie->len+TLV_HDR_LEN, rem_len));
			pbuf += (ie->len+TLV_HDR_LEN);
			rem_len -= (ie->len+TLV_HDR_LEN);
			data_len -= (((void *)ie-pdata) + (ie->len+TLV_HDR_LEN));
			pdata = (char *)ie + (ie->len+TLV_HDR_LEN);
			num_ie++;
		}
	}
#endif
	if (num_ie) {
		event_len = buflen - rem_len;
		AGENL_INFO(dev->name, "num_ie=%d\n", num_ie);
		if (android_msg_level & ANDROID_INFO_LEVEL)
			prhex("buf", buf, event_len);
		ret = wl_ext_genl_send(zconf, dev, buf, event_len);
	}

	if(buf)
		kfree(buf);
	return;
}
#endif

static int
wl_ext_genl_recv(struct sk_buff *skb, struct genl_info *info)
{
	struct genl_params *zconf = g_zconf;
	struct net_device *dev;
	struct nlattr *na;
	char* pData = NULL;
	int DataLen = 0;

	if (info == NULL) {
		AGENL_ERROR(dev->name, "genl_info is NULL\n");
		return -1;
	}

	if (zconf == NULL) {
		AGENL_ERROR("wlan", "g_zconf is NULL\n");
		return -1;
	}
	dev = zconf->dev;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
	AGENL_TRACE(dev->name, "Enter snd_portid=%d\n", info->snd_portid);
#else
	AGENL_TRACE(dev->name, "Enter\n");
#endif
	na = info->attrs[GENL_CUSTOM_ATTR_MSG];

	if (na) {
		pData = (char*) nla_data(na);
		DataLen = nla_len(na);
		AGENL_INFO(dev->name, "nla_len(na) : %d\n", DataLen);
		if (android_msg_level & ANDROID_INFO_LEVEL)
			prhex("nla_data(na)", pData, DataLen);
	}

#ifdef SENDPROB
	if(*pData == MGMT_PROBE_RES) {
		wl_ext_send_probersp(dev, pData, DataLen);
	} else if(*pData == MGMT_PROBE_REQ) {
		AGENL_ERROR(dev->name, "probe req\n");
	} else {
		AGENL_ERROR(dev->name, "Unexpected pkt %d\n", *pData);
		if (android_msg_level & ANDROID_INFO_LEVEL)
			prhex("nla_data(na)", pData, DataLen);
	}
#endif

	return 0;
}

static int
wl_ext_genl_send(struct genl_params *zconf, struct net_device *dev,
	char* buf, int buf_len)
{
	struct sk_buff *skb = NULL;
	char* msg_head = NULL;
	int ret = -1;
	int bytes_written = 0;
	char recv_probreq[32];

	if (zconf->bind_pid == -1) {
		AGENL_ERROR(dev->name, "There is no binded process\n");
		return -1;
	}

	if(buf == NULL || buf_len <= 0) {
		AGENL_ERROR(dev->name, "buf is NULL or buf_len : %d\n", buf_len);
		return -1;
	}

	skb = genlmsg_new(MAX_CUSTOM_PKT_LENGTH, GFP_KERNEL);

	if (skb) {
		msg_head = genlmsg_put(skb, 0, 0, &wl_ext_genl_family, 0, GENL_CUSTOM_COMMAND_RECV);
		if (msg_head == NULL) {
			nlmsg_free(skb);
			AGENL_ERROR(dev->name, "genlmsg_put fail\n");
			return -1;
		}

		ret = nla_put(skb, GENL_CUSTOM_ATTR_MSG, buf_len, buf);
		if (ret != 0) {
			nlmsg_free(skb);
			AGENL_ERROR(dev->name, "nla_put fail : %d\n", ret);
			return ret;
		}

		genlmsg_end(skb, msg_head);

		/* sending message */
		AGENL_TRACE(dev->name, "send to process %d\n", zconf->bind_pid);
		ret = genlmsg_unicast(&init_net, skb, zconf->bind_pid);
		if (ret != 0) {
			AGENL_ERROR(dev->name, "genlmsg_unicast fail : %d\n", ret);
			zconf->send_retry_cnt++;
			if(zconf->send_retry_cnt >= GENLMSG_UNICAST_RETRY_LIMIT) {
				AGENL_ERROR(dev->name, "Exceeding retry cnt %d, Unbind pid : %d\n",
					zconf->send_retry_cnt, zconf->bind_pid);
				zconf->bind_pid = -1;
				sprintf(recv_probreq, "wl recv_probreq 0");
				wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
			}
			return ret;
		}
	} else {
		AGENL_ERROR(dev->name, "genlmsg_new fail\n");
		return -1;
	}

	zconf->send_retry_cnt = 0;

	return 0;
}

static int
wl_ext_genl_bind(struct sk_buff *skb, struct genl_info *info)
{
	struct genl_params *zconf = g_zconf;
	struct net_device *dev;
	struct dhd_pub *dhd;
	struct nlattr *na;
	bool bind;
	char* pData = NULL;
	int DataLen = 0;

	if (info == NULL) {
		AGENL_ERROR("wlan", "genl_info is NULL\n");
		return -1;
	}

	if (zconf == NULL) {
		AGENL_ERROR("wlan", "zconf is NULL\n");
		return -1;
	}
	dev = zconf->dev;
	dhd = dhd_get_pub(dev);

	AGENL_TRACE(dev->name, "Enter\n");

	na = info->attrs[GENL_CUSTOM_ATTR_MSG];
	if (na) {
		pData = (char*) nla_data(na);
		DataLen = nla_len(na);
		AGENL_INFO(dev->name, "nla_len(na) : %d\n", DataLen);
		if (android_msg_level & ANDROID_INFO_LEVEL)
			prhex("nla_data(na)", pData, DataLen);
	}

	if (strcmp(pData, "BIND") == 0) {
		bind = TRUE;
	} else if (strcmp(pData, "UNBIND") == 0) {
		bind = FALSE;
	} else {
		AGENL_ERROR(dev->name, "Unknown cmd %s\n", pData);
		return -1;
	}

	if (bind == zconf->bind) {
		AGENL_TRACE(dev->name, "Already %s\n", bind?"BIND":"UNBIND");
		return 0;
	}

	if (bind) {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
		zconf->bind_pid = info->snd_portid;
#endif
		AGENL_TRACE(dev->name, "BIND pid = %d\n", zconf->bind_pid);
#ifdef SENDPROB
		wl_ext_set_probreq(dev, TRUE);
#endif
		zconf->bind = TRUE;
		zconf->pm = dhd->conf->pm;
		dhd->conf->pm = PM_OFF;
	} else {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
		AGENL_TRACE(dev->name, "UNBIND snd_portid = %d\n", info->snd_portid);
#else
		AGENL_TRACE(dev->name, "UNBIND pid = %d\n", zconf->bind_pid);
#endif
		zconf->bind_pid = -1;
#ifdef SENDPROB
		wl_ext_set_probreq(dev, FALSE);
#endif
		dhd->conf->pm = zconf->pm;
		zconf->bind = FALSE;
	}

	return 0;
}

int
wl_ext_genl_init(struct net_device *net)
{
	struct dhd_pub *dhd = dhd_get_pub(net);
	struct genl_params *zconf = dhd->zconf;
	int ret = 0;

	AGENL_TRACE(net->name, "Enter falimy name: \"%s\"\n", wl_ext_genl_family.name);

	zconf = kzalloc(sizeof(struct genl_params), GFP_KERNEL);
	if (unlikely(!zconf)) {
		AGENL_ERROR(net->name, "Could not allocate zconf\n");
		return -ENOMEM;
	}
	dhd->zconf = (void *)zconf;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 10, 0)
	ret = genl_register_family(&wl_ext_genl_family);
	//fix me: how to attach wl_ext_genl_ops
	ret = -1;
#elif LINUX_VERSION_CODE >= KERNEL_VERSION(3, 13, 0)
	ret = genl_register_family_with_ops(&wl_ext_genl_family, wl_ext_genl_ops);
#else
	ret = genl_register_family_with_ops(&wl_ext_genl_family, wl_ext_genl_ops,
		ARRAY_SIZE(wl_ext_genl_ops));
#endif
	if (ret != 0) {
		AGENL_ERROR(net->name, "GE_NELINK family registration fail\n");
		goto err;
	}
	zconf->bind_pid = -1;
#ifdef SENDPROB
	ret = wl_ext_event_register(net, dhd, WLC_E_PROBREQ_MSG, wl_ext_probreq_event,
		zconf, PRIO_EVENT_IAPSTA);
	if (ret)
		goto err;
#endif
	zconf->dev = net;
	g_zconf = zconf;

	return ret;
err:
	if(zconf)
		kfree(zconf);
	return ret;
}

void
wl_ext_genl_deinit(struct net_device *net)
{
	struct dhd_pub *dhd = dhd_get_pub(net);
	struct genl_params *zconf = dhd->zconf;

	AGENL_TRACE(net->name, "Enter\n");

#ifdef SENDPROB
	wl_ext_event_deregister(net, dhd, WLC_E_PROBREQ_MSG, wl_ext_probreq_event);
#endif

	genl_unregister_family(&wl_ext_genl_family);
	if(zconf != NULL) {
		kfree(dhd->zconf);
		dhd->zconf = NULL;
	}
	g_zconf = NULL;

}
#endif