cld80211-lib/cld80211_lib.c

/*
 * Driver interaction with Linux nl80211/cfg80211
 * Copyright (c) 2002-2015, Jouni Malinen <j@w1.fi>
 * Copyright (c) 2003-2004, Instant802 Networks, Inc.
 * Copyright (c) 2005-2006, Devicescape Software, Inc.
 * Copyright (c) 2007, Johannes Berg <johannes@sipsolutions.net>
 * Copyright (c) 2009-2010, Atheros Communications
 * Copyright (c) 2017-2018, The Linux Foundation. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *     * Redistributions of source code must retain the above copyright
 *     * Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *     * Neither the name of The Linux Foundation nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
 *
 */

#include <errno.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>
#include <linux/pkt_sched.h>
#include <unistd.h>
#include "cld80211_lib.h"

#ifndef LE_BUILD
 #include <log/log.h>
 #undef LOG_TAG
 #define LOG_TAG "CLD80211"
#else
 #include <stdlib.h>
 #include <syslog.h>
 #include <unistd.h>
 #define ALOGI(fmt, args...) syslog(LOG_INFO, fmt, ## args)
 #define ALOGE(fmt, args...) syslog(LOG_ERR, fmt, ## args)
 extern const char *__progname;
 const char *getprogname() { return (__progname); }
#endif

#define SOCK_BUF_SIZE (256*1024)

struct family_data {
	const char *group;
	int id;
};


static struct nl_sock * create_nl_socket(int protocol)
{
	struct nl_sock *sock;

	sock = nl_socket_alloc();
	if (sock == NULL) {
		ALOGE("%s: Failed to create NL socket, err: %d",
		      getprogname(), errno);
		return NULL;
	}

	if (nl_connect(sock, protocol)) {
		ALOGE("%s: Could not connect sock, err: %d",
		      getprogname(), errno);
		nl_socket_free(sock);
		return NULL;
	}

	return sock;
}


static int init_exit_sockets(struct cld80211_ctx *ctx)
{
	ctx->exit_sockets[0] = -1;
	ctx->exit_sockets[1] = -1;
	if (socketpair(AF_UNIX, SOCK_STREAM, 0, &ctx->exit_sockets[0]) == -1) {
		ALOGE("%s: Failed to create exit socket pair", getprogname());
		return -1;
	}
	ALOGI("%s: initialized exit socket pair", getprogname());

	return 0;
}


static void cleanup_exit_sockets(struct cld80211_ctx *ctx)
{
	if (ctx->exit_sockets[0] >= 0) {
		close(ctx->exit_sockets[0]);
		ctx->exit_sockets[0] = -1;
	}

	if (ctx->exit_sockets[1] >= 0) {
		close(ctx->exit_sockets[1]);
		ctx->exit_sockets[1] = -1;
	}
}


void exit_cld80211_recv(struct cld80211_ctx *ctx)
{
	if (!ctx) {
		ALOGE("%s: ctx is NULL: %s", getprogname(), __func__);
		return;
	}
	TEMP_FAILURE_RETRY(write(ctx->exit_sockets[0], "E", 1));
	ALOGI("%s: Sent msg on exit sock to unblock poll()", getprogname());
}


/* Event handlers */
static int response_handler(struct nl_msg *msg, void *arg)
{
	UNUSED(msg);
	UNUSED(arg);
	ALOGI("%s: Received nlmsg response: no callback registered;drop it",
	      getprogname());

	return NL_SKIP;
}


static int ack_handler(struct nl_msg *msg, void *arg)
{
	int *err = (int *)arg;
	*err = 0;
	UNUSED(msg);
	return NL_STOP;
}


static int finish_handler(struct nl_msg *msg, void *arg)
{
	int *ret = (int *)arg;
	*ret = 0;
	UNUSED(msg);
	return NL_SKIP;
}


static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
			 void *arg)
{
	int *ret = (int *)arg;
	*ret = err->error;

	UNUSED(nla);
	ALOGE("%s: error_handler received : %d", getprogname(), err->error);
	return NL_SKIP;
}


static int no_seq_check(struct nl_msg *msg, void *arg)
{
	UNUSED(msg);
	UNUSED(arg);
	return NL_OK;
}


int cld80211_recv_msg(struct nl_sock *sock, struct nl_cb *cb)
{
	if (!sock || !cb) {
		ALOGE("%s: %s is NULL", getprogname(), sock?"cb":"sock");
		return -EINVAL;
	}

	int res = nl_recvmsgs(sock, cb);
	if(res)
		ALOGE("%s: Error :%d while reading nl msg , err: %d",
		      getprogname(), res, errno);
	return res;
}


static void cld80211_handle_event(int events, struct nl_sock *sock,
				  struct nl_cb *cb)
{
	if (events & POLLERR) {
		ALOGE("%s: Error reading from socket", getprogname());
		cld80211_recv_msg(sock, cb);
	} else if (events & POLLHUP) {
		ALOGE("%s: Remote side hung up", getprogname());
	} else if (events & POLLIN) {
		cld80211_recv_msg(sock, cb);
	} else {
		ALOGE("%s: Unknown event - %0x", getprogname(), events);
	}
}


static int family_handler(struct nl_msg *msg, void *arg)
{
	struct family_data *res = arg;
	struct nlattr *tb[CTRL_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nlattr *mcgrp;
	int i;

	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
			genlmsg_attrlen(gnlh, 0), NULL);
	if (!tb[CTRL_ATTR_MCAST_GROUPS])
		return NL_SKIP;

	nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], i) {
		struct nlattr *tb2[CTRL_ATTR_MCAST_GRP_MAX + 1];
		nla_parse(tb2, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp),
				nla_len(mcgrp), NULL);

		if (!tb2[CTRL_ATTR_MCAST_GRP_NAME] ||
			!tb2[CTRL_ATTR_MCAST_GRP_ID] ||
			strncmp(nla_data(tb2[CTRL_ATTR_MCAST_GRP_NAME]),
				   res->group,
				   nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0)
			continue;
		res->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]);
		break;
	};

	return NL_SKIP;
}


static int get_multicast_id(struct cld80211_ctx *ctx, const char *group, bool sync_driver)
{
	struct family_data res = { group, -ENOENT };
	struct nl_msg *nlmsg = nlmsg_alloc();

	if (!nlmsg) {
		return -1;
	}

	genlmsg_put(nlmsg, 0, 0, ctx->nlctrl_familyid, 0, 0,
	            CTRL_CMD_GETFAMILY, 0);
	nla_put_string(nlmsg, CTRL_ATTR_FAMILY_NAME, "cld80211");

	if (sync_driver == true) {
		cld80211_send_recv_msg(ctx, nlmsg, family_handler, &res);
		ALOGI("%s: nlctrl family id: %d group: %s mcast_id: %d", getprogname(),
				ctx->nlctrl_familyid, group, res.id);
	}
	nlmsg_free(nlmsg);
	return res.id;
}


int cld80211_add_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup)
{
	if (!ctx || !mcgroup) {
		ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__);
		return 0;
	}
	int id = get_multicast_id(ctx, mcgroup, true);
	if (id < 0) {
		ALOGE("%s: Could not find group %s, errno: %d id: %d",
		      getprogname(), mcgroup, errno, id);
		return id;
	}

	int ret = nl_socket_add_membership(ctx->sock, id);
	if (ret < 0) {
		ALOGE("%s: Could not add membership to group %s, errno: %d",
		      getprogname(), mcgroup, errno);
	}

	return ret;
}


int cld80211_remove_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup)
{
	if (!ctx || !mcgroup) {
		ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__);
		return 0;
	}
	int id = get_multicast_id(ctx, mcgroup, false);
	if (id < 0) {
		ALOGE("%s: Could not find group %s, errno: %d id: %d",
		      getprogname(), mcgroup, errno, id);
		return id;
	}

	int ret = nl_socket_drop_membership(ctx->sock, id);
	if (ret < 0) {
		ALOGE("%s: Could not drop membership from group %s, errno: %d,"
		      " ret: %d", getprogname(), mcgroup, errno, ret);
		return ret;
	}

	return 0;
}


struct nl_msg *cld80211_msg_alloc(struct cld80211_ctx *ctx, int cmd,
				  struct nlattr **nla_data, int pid)
{
	struct nl_msg *nlmsg;

	if (!ctx || !nla_data) {
		ALOGE("%s: ctx is null: %s", getprogname(), __func__);
		return NULL;
	}

	nlmsg = nlmsg_alloc();
	if (nlmsg == NULL) {
		ALOGE("%s: Out of memory", getprogname());
		return NULL;
	}

	genlmsg_put(nlmsg, pid, /* seq = */ 0, ctx->netlink_familyid,
			0, 0, cmd, /* version = */ 0);

	*nla_data = nla_nest_start(nlmsg, CLD80211_ATTR_VENDOR_DATA);
	if (!nla_data)
		goto cleanup;

	return nlmsg;

cleanup:
	if (nlmsg)
		nlmsg_free(nlmsg);
	return NULL;
}


int cld80211_send_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg)
{
	int err;

	if (!ctx || !ctx->sock || !nlmsg) {
		ALOGE("%s: Invalid data from client", getprogname());
		return -EINVAL;
	}

	err = nl_send_auto_complete(ctx->sock, nlmsg);  /* send message */
	if (err < 0) {
		ALOGE("%s: failed to send msg: %d", getprogname(), err);
		return err;
	}

	return 0;
}


int cld80211_send_recv_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg,
			   int (*valid_handler)(struct nl_msg *, void *),
			   void *valid_data)
{
	int err;

	if (!ctx || !ctx->sock || !nlmsg) {
		ALOGE("%s: Invalid data from client", getprogname());
		return -EINVAL;
	}

	struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT);
	if (!cb)
		return -ENOMEM;

	err = nl_send_auto_complete(ctx->sock, nlmsg);  /* send message */
	if (err < 0)
		goto out;

	err = 1;

	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);

	if (valid_handler)
		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
			  valid_handler, valid_data);
	else
		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
			  response_handler, valid_data);

	while (err > 0) {    /* wait for reply */
		int res = nl_recvmsgs(ctx->sock, cb);
		if (res) {
			ALOGE("%s: cld80211: nl_recvmsgs failed: %d",
			      getprogname(), res);
		}
	}
out:
	nl_cb_put(cb);
	return err;
}


int cld80211_recv(struct cld80211_ctx *ctx, int timeout, bool recv_multi_msg,
		  int (*valid_handler)(struct nl_msg *, void *),
		  void *cbctx)
{
	struct pollfd pfd[2];
	struct nl_cb *cb;
	int err;

	if (!ctx || !ctx->sock || !valid_handler) {
		ALOGE("%s: Invalid data from client", getprogname());
		return -EINVAL;
	}

	cb = nl_cb_alloc(NL_CB_DEFAULT);
	if (!cb)
		return -ENOMEM;

	memset(&pfd[0], 0, 2*sizeof(struct pollfd));

	err = 1;

	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler, cbctx);

	pfd[0].fd = nl_socket_get_fd(ctx->sock);
	pfd[0].events = POLLIN;

	pfd[1].fd = ctx->exit_sockets[1];
	pfd[1].events = POLLIN;

	do {
		pfd[0].revents = 0;
		pfd[1].revents = 0;
		int result = poll(pfd, 2, timeout);
		if (result < 0) {
			ALOGE("%s: Error polling socket", getprogname());
		} else if (pfd[0].revents & (POLLIN | POLLHUP | POLLERR)) {
			cld80211_handle_event(pfd[0].revents, ctx->sock, cb);
			if (!recv_multi_msg)
				break;
		} else {
			ALOGI("%s: Exiting poll", getprogname());
			break;
		}
	} while (1);

	nl_cb_put(cb);
	return 0;
}


struct cld80211_ctx * cld80211_init(void)
{
	struct cld80211_ctx *ctx;

	ctx = (struct cld80211_ctx *)malloc(sizeof(struct cld80211_ctx));
	if (ctx == NULL) {
		ALOGE("%s: Failed to alloc cld80211_ctx", getprogname());
		return NULL;
	}
	memset(ctx, 0, sizeof(struct cld80211_ctx));

	ctx->sock = create_nl_socket(NETLINK_GENERIC);
	if (ctx->sock == NULL) {
		ALOGE("%s: Failed to create socket port", getprogname());
		goto cleanup;
	}

	/* Set the socket buffer size */
	if (nl_socket_set_buffer_size(ctx->sock, SOCK_BUF_SIZE , 0) < 0) {
		ALOGE("%s: Could not set nl_socket RX buffer size for sock: %s",
		      getprogname(), strerror(errno));
		/* continue anyway with the default (smaller) buffer */
	}

	ctx->netlink_familyid = genl_ctrl_resolve(ctx->sock, "cld80211");
	if (ctx->netlink_familyid < 0) {
		ALOGE("%s: Could not resolve cld80211 familty id",
		      getprogname());
		goto cleanup;
	}

	ctx->nlctrl_familyid = genl_ctrl_resolve(ctx->sock, "nlctrl");
	if (ctx->nlctrl_familyid < 0) {
		ALOGE("%s: net link family nlctrl is not present: %d err:%d",
			getprogname(), ctx->nlctrl_familyid, errno);
		goto cleanup;
	}


	if (init_exit_sockets(ctx) != 0) {
		ALOGE("%s: Failed to initialize exit sockets", getprogname());
		goto cleanup;
	}

	return ctx;
cleanup:
	if (ctx->sock) {
		nl_socket_free(ctx->sock);
	}
	free (ctx);
	return NULL;
}


void cld80211_deinit(struct cld80211_ctx *ctx)
{
	if (!ctx || !ctx->sock) {
		ALOGE("%s: ctx/sock is NULL", getprogname());
		return;
	}
	nl_socket_free(ctx->sock);
	cleanup_exit_sockets(ctx);
	free (ctx);
}