Merge pull request #138 from nbd168/nl80211

nl80211: add support for registering an uloop based listener

2 files changed, 269 insertions, 50 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6506b1a..6d76f3a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -154,13 +154,14 @@ ENDIF()
 
 IF(NL80211_SUPPORT)
   FIND_LIBRARY(nl NAMES nl-tiny)
+  FIND_LIBRARY(ubox NAMES ubox)
   FIND_PATH(nl_include_dir NAMES netlink/msg.h PATH_SUFFIXES libnl-tiny)
   INCLUDE_DIRECTORIES(${nl_include_dir})
   SET(LIBRARIES ${LIBRARIES} nl80211_lib)
   ADD_LIBRARY(nl80211_lib MODULE lib/nl80211.c)
   SET_TARGET_PROPERTIES(nl80211_lib PROPERTIES OUTPUT_NAME nl80211 PREFIX "")
   TARGET_LINK_OPTIONS(nl80211_lib PRIVATE ${UCODE_MODULE_LINK_OPTIONS})
-  TARGET_LINK_LIBRARIES(nl80211_lib ${nl})
+  TARGET_LINK_LIBRARIES(nl80211_lib ${nl} ${ubox})
 ENDIF()
 
 IF(RESOLV_SUPPORT)
diff --git a/lib/nl80211.c b/lib/nl80211.c
index d182d7a..f3e63bb 100644
--- a/lib/nl80211.c
+++ b/lib/nl80211.c
@@ -40,9 +40,12 @@ limitations under the License.
 
 #include <linux/nl80211.h>
 #include <linux/ieee80211.h>
+#include <libubox/uloop.h>
 
 #include "ucode/module.h"
 
+#define DIV_ROUND_UP(n, d)      (((n) + (d) - 1) / (d))
+
 #define err_return(code, ...) do { set_error(code, __VA_ARGS__); return NULL; } while(0)
 
 /* Modified downstream nl80211.h headers may disable certain unsupported
@@ -51,6 +54,8 @@ limitations under the License.
 
 #define NL80211_ATTR_NOT_IMPLEMENTED 0x10000
 
+#define NL80211_CMDS_BITMAP_SIZE	DIV_ROUND_UP(NL80211_CMD_MAX + 1, 32)
+
 static struct {
 	int code;
 	char *msg;
@@ -72,6 +77,15 @@ set_error(int errcode, const char *fmt, ...) {
 	}
 }
 
+static uc_resource_type_t *listener_type;
+static uc_value_t *listener_registry;
+static uc_vm_t *listener_vm;
+
+typedef struct {
+	uint32_t cmds[NL80211_CMDS_BITMAP_SIZE];
+	size_t index;
+} uc_nl_listener_t;
+
 static bool
 uc_nl_parse_u32(uc_value_t *val, uint32_t *n)
 {
@@ -1813,6 +1827,8 @@ static struct {
 	struct nl_cache *cache;
 	struct genl_family *nl80211;
 	struct genl_family *nlctrl;
+	struct uloop_fd evsock_fd;
+	struct nl_cb *evsock_cb;
 } nl80211_conn;
 
 typedef enum {
@@ -2149,37 +2165,95 @@ struct waitfor_ctx {
 	uint8_t cmd;
 	uc_vm_t *vm;
 	uc_value_t *res;
-	uint32_t cmds[8];
+	uint32_t cmds[NL80211_CMDS_BITMAP_SIZE];
 };
 
+static uc_value_t *
+uc_nl_prepare_event(uc_vm_t *vm, struct nl_msg *msg)
+{
+	struct nlmsghdr *hdr = nlmsg_hdr(msg);
+	struct genlmsghdr *gnlh = nlmsg_data(hdr);
+	uc_value_t *o = ucv_object_new(vm);
+
+	if (!uc_nl_convert_attrs(msg, genlmsg_attrdata(gnlh, 0),
+		genlmsg_attrlen(gnlh, 0), 0,
+		nl80211_msg.attrs, nl80211_msg.nattrs, vm, o)) {
+		ucv_put(o);
+		return NULL;
+	}
+
+	return o;
+}
+
+static int
+cb_listener_event(struct nl_msg *msg, void *arg)
+{
+	struct nlmsghdr *hdr = nlmsg_hdr(msg);
+	struct genlmsghdr *gnlh = nlmsg_data(hdr);
+	uc_vm_t *vm = listener_vm;
+
+	if (!nl80211_conn.evsock_fd.registered || !vm)
+		return NL_SKIP;
+
+	for (size_t i = 0; i < ucv_array_length(listener_registry); i += 2) {
+		uc_value_t *this = ucv_array_get(listener_registry, i);
+		uc_value_t *func = ucv_array_get(listener_registry, i + 1);
+		uc_nl_listener_t *l;
+		uc_value_t *o, *data;
+
+		l = ucv_resource_data(this, "nl80211.listener");
+		if (!l)
+			continue;
+
+		if (gnlh->cmd > NL80211_CMD_MAX ||
+			!(l->cmds[gnlh->cmd / 32] & (1 << (gnlh->cmd % 32))))
+			continue;
+
+		if (!ucv_is_callable(func))
+			continue;
+
+		data = uc_nl_prepare_event(vm, msg);
+		if (!data)
+			return NL_SKIP;
+
+		o = ucv_object_new(vm);
+		ucv_object_add(o, "cmd", ucv_int64_new(gnlh->cmd));
+		ucv_object_add(o, "msg", data);
+
+		uc_vm_stack_push(vm, ucv_get(this));
+		uc_vm_stack_push(vm, ucv_get(func));
+		uc_vm_stack_push(vm, o);
+
+		if (uc_vm_call(vm, true, 1) != EXCEPTION_NONE) {
+			uloop_end();
+			return NL_STOP;
+		}
+
+		ucv_put(uc_vm_stack_pop(vm));
+	}
+
+	return NL_SKIP;
+}
+
 static int
 cb_event(struct nl_msg *msg, void *arg)
 {
 	struct nlmsghdr *hdr = nlmsg_hdr(msg);
 	struct genlmsghdr *gnlh = nlmsg_data(hdr);
 	struct waitfor_ctx *s = arg;
-	bool rv, match = true;
 	uc_value_t *o;
 
-	if (s->cmds[0] || s->cmds[1] || s->cmds[2] || s->cmds[3] ||
-	    s->cmds[4] || s->cmds[5] || s->cmds[6] || s->cmds[7]) {
-		match = (s->cmds[gnlh->cmd / 32] & (1 << (gnlh->cmd % 32)));
-	}
-
-	if (match) {
-		o = ucv_object_new(s->vm);
+	cb_listener_event(msg, arg);
 
-		rv = uc_nl_convert_attrs(msg,
-			genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0),
-			0, nl80211_msg.attrs, nl80211_msg.nattrs, s->vm, o);
+	if (gnlh->cmd > NL80211_CMD_MAX ||
+	    !(s->cmds[gnlh->cmd / 32] & (1 << (gnlh->cmd % 32))))
+		return NL_SKIP;
 
-		if (rv)
-			s->res = o;
-		else
-			ucv_put(o);
+	o = uc_nl_prepare_event(s->vm, msg);
+	if (o)
+		s->res = o;
 
-		s->cmd = gnlh->cmd;
-	}
+	s->cmd = gnlh->cmd;
 
 	return NL_SKIP;
 }
@@ -2190,6 +2264,58 @@ cb_seq(struct nl_msg *msg, void *arg)
 	return NL_OK;
 }
 
+static bool
+uc_nl_fill_cmds(uint32_t *cmd_bits, uc_value_t *cmds)
+{
+	if (ucv_type(cmds) == UC_ARRAY) {
+		for (size_t i = 0; i < ucv_array_length(cmds); i++) {
+			int64_t n = ucv_int64_get(ucv_array_get(cmds, i));
+
+			if (errno || n < 0 || n > NL80211_CMD_MAX)
+				return false;
+
+			cmd_bits[n / 32] |= (1 << (n % 32));
+		}
+	}
+	else if (ucv_type(cmds) == UC_INTEGER) {
+		int64_t n = ucv_int64_get(cmds);
+
+		if (errno || n < 0 || n > 255)
+			return false;
+
+		cmd_bits[n / 32] |= (1 << (n % 32));
+	}
+	else if (!cmds)
+		memset(cmd_bits, 0xff, NL80211_CMDS_BITMAP_SIZE * sizeof(*cmd_bits));
+	else
+		return false;
+
+	return true;
+}
+
+static bool
+uc_nl_evsock_init(void)
+{
+	if (nl80211_conn.evsock)
+		return true;
+
+	if (!uc_nl_connect_sock(&nl80211_conn.evsock, true))
+		return false;
+
+	if (!uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "config") ||
+	    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "scan") ||
+	    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "regulatory") ||
+	    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "mlme") ||
+	    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "vendor") ||
+	    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "nan")) {
+		nl_socket_free(nl80211_conn.evsock);
+		nl80211_conn.evsock = NULL;
+		return false;
+	}
+
+	return true;
+}
+
 static uc_value_t *
 uc_nl_waitfor(uc_vm_t *vm, size_t nargs)
 {
@@ -2200,11 +2326,9 @@ uc_nl_waitfor(uc_vm_t *vm, size_t nargs)
 	struct waitfor_ctx ctx = { .vm = vm };
 	struct nl_cb *cb;
 	int ms = -1, err;
-	int64_t n;
-	size_t i;
 
 	if (timeout) {
-		n = ucv_int64_get(timeout);
+		int64_t n = ucv_int64_get(timeout);
 
 		if (ucv_type(timeout) != UC_INTEGER || n < INT32_MIN || n > INT32_MAX)
 			err_return(NLE_INVAL, "Invalid timeout specified");
@@ -2212,35 +2336,11 @@ uc_nl_waitfor(uc_vm_t *vm, size_t nargs)
 		ms = (int)n;
 	}
 
-	if (ucv_type(cmds) == UC_ARRAY) {
-		for (i = 0; i < ucv_array_length(cmds); i++) {
-			n = ucv_int64_get(ucv_array_get(cmds, i));
-
-			if (n < 0 || n > 255)
-				err_return(NLE_INVAL, "Invalid command ID specified");
-
-			ctx.cmds[n / 32] |= (1 << (n % 32));
-		}
-	}
-	else if (ucv_type(cmds) == UC_INTEGER) {
-		n = ucv_int64_get(cmds);
+	if (!uc_nl_fill_cmds(ctx.cmds, cmds))
+		err_return(NLE_INVAL, "Invalid command ID specified");
 
-		if (n < 0 || n > 255)
-			err_return(NLE_INVAL, "Invalid command ID specified");
-
-		ctx.cmds[n / 32] |= (1 << (n % 32));
-	}
-
-	if (!nl80211_conn.evsock) {
-		if (!uc_nl_connect_sock(&nl80211_conn.evsock, true) ||
-		    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "config") ||
-		    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "scan") ||
-		    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "regulatory") ||
-		    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "mlme") ||
-		    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "vendor") ||
-		    !uc_nl_subscribe(nl80211_conn.evsock, "nl80211", "nan"))
-			return NULL;
-	}
+	if (!uc_nl_evsock_init())
+		return NULL;
 
 	cb = nl_cb_alloc(NL_CB_DEFAULT);
 
@@ -2368,6 +2468,113 @@ uc_nl_request(uc_vm_t *vm, size_t nargs)
 	}
 }
 
+static void
+uc_nl_listener_cb(struct uloop_fd *fd, unsigned int events)
+{
+	nl_recvmsgs(nl80211_conn.evsock, nl80211_conn.evsock_cb);
+}
+
+static uc_value_t *
+uc_nl_listener(uc_vm_t *vm, size_t nargs)
+{
+	struct uloop_fd *fd = &nl80211_conn.evsock_fd;
+	uc_nl_listener_t *l;
+	uc_value_t *cb_func = uc_fn_arg(0);
+	uc_value_t *cmds = uc_fn_arg(1);
+	uc_value_t *rv;
+	size_t i;
+
+	if (!ucv_is_callable(cb_func)) {
+		uc_vm_raise_exception(vm, EXCEPTION_TYPE, "Invalid callback");
+		return NULL;
+	}
+
+	if (!uc_nl_evsock_init())
+		return NULL;
+
+	if (!fd->registered) {
+		fd->fd = nl_socket_get_fd(nl80211_conn.evsock);
+		fd->cb = uc_nl_listener_cb;
+		uloop_fd_add(fd, ULOOP_READ);
+	}
+
+	if (!nl80211_conn.evsock_cb) {
+		struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT);
+
+		if (!cb)
+			err_return(NLE_NOMEM, NULL);
+
+		nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, cb_seq, NULL);
+		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, cb_listener_event, NULL);
+		nl80211_conn.evsock_cb = cb;
+	}
+
+	for (i = 0; i < ucv_array_length(listener_registry); i += 2) {
+		if (!ucv_array_get(listener_registry, i))
+			break;
+	}
+
+	ucv_array_set(listener_registry, i + 1, cb_func);
+	l = xalloc(sizeof(*l));
+	l->index = i;
+	if (!uc_nl_fill_cmds(l->cmds, cmds)) {
+		uc_vm_raise_exception(vm, EXCEPTION_TYPE, "Invalid command ID");
+		free(l);
+		return NULL;
+	}
+
+	rv = uc_resource_new(listener_type, l);
+	ucv_array_set(listener_registry, i, rv);
+	listener_vm = vm;
+
+	return rv;
+}
+
+static void
+uc_nl_listener_free(void *arg)
+{
+	uc_nl_listener_t *l = arg;
+
+	ucv_array_set(listener_registry, l->index, NULL);
+	ucv_array_set(listener_registry, l->index + 1, NULL);
+	free(l);
+}
+
+static uc_value_t *
+uc_nl_listener_set_commands(uc_vm_t *vm, size_t nargs)
+{
+	uc_nl_listener_t *l = uc_fn_thisval("nl80211.listener");
+	uc_value_t *cmds = uc_fn_arg(0);
+
+	if (!l)
+		return NULL;
+
+	memset(l->cmds, 0, sizeof(l->cmds));
+	if (!uc_nl_fill_cmds(l->cmds, cmds))
+		uc_vm_raise_exception(vm, EXCEPTION_TYPE, "Invalid command ID");
+
+	return NULL;
+}
+
+static uc_value_t *
+uc_nl_listener_close(uc_vm_t *vm, size_t nargs)
+{
+	uc_nl_listener_t **lptr = uc_fn_this("nl80211.listener");
+	uc_nl_listener_t *l;
+
+	if (!lptr)
+		return NULL;
+
+	l = *lptr;
+	if (!l)
+		return NULL;
+
+	*lptr = NULL;
+	uc_nl_listener_free(l);
+
+	return NULL;
+}
+
 
 static void
 register_constants(uc_vm_t *vm, uc_value_t *scope)
@@ -2518,12 +2725,23 @@ static const uc_function_list_t global_fns[] = {
 	{ "error",		uc_nl_error },
 	{ "request",	uc_nl_request },
 	{ "waitfor",	uc_nl_waitfor },
+	{ "listener",	uc_nl_listener },
 };
 
 
+static const uc_function_list_t listener_fns[] = {
+	{ "set_commands",	uc_nl_listener_set_commands },
+	{ "close",			uc_nl_listener_close },
+};
+
 void uc_module_init(uc_vm_t *vm, uc_value_t *scope)
 {
 	uc_function_list_register(scope, global_fns);
 
+	listener_type = uc_type_declare(vm, "nl80211.listener", listener_fns, uc_nl_listener_free);
+	listener_registry = ucv_array_new(vm);
+
+	uc_vm_registry_set(vm, "nl80211.registry", listener_registry);
+
 	register_constants(vm, scope);
 }