neard: nfctool: Add start_poll command

nfctool --device nfcX --poll [Initiator|Target|Both]
This starts polling on device nfcX as initiator, target, or both.

4 files changed, 401 insertions, 5 deletions

diff --git a/tools/nfctool/main.c b/tools/nfctool/main.c
index fc8e0a3..930d25e 100644
--- a/tools/nfctool/main.c
+++ b/tools/nfctool/main.c
@@ -32,6 +32,33 @@
 
 GSList *adapters = NULL;
 
+static GMainLoop *main_loop = NULL;
+
+static gint nfctool_compare_adapter_idx(struct nfc_adapter *adapter,
+								guint32 idx)
+{
+	if (adapter->idx < idx)
+		return -1;
+
+	if (adapter->idx > idx)
+		return 1;
+
+	return 0;
+}
+
+static struct nfc_adapter *nfctool_find_adapter(guint32 adapter_idx)
+{
+	GSList *elem;
+
+	elem = g_slist_find_custom(adapters, GINT_TO_POINTER(adapter_idx),
+				(GCompareFunc)nfctool_compare_adapter_idx);
+
+	if (elem)
+		return elem->data;
+
+	return NULL;
+}
+
 static void nfctool_print_target(guint32 idx, gchar *type)
 {
 	printf("%s%d ", type, idx);
@@ -55,6 +82,8 @@ static void nfctool_print_targets(struct nfc_adapter *adapter, gchar *prefix)
 
 static void nfctool_print_adapter_info(struct nfc_adapter *adapter)
 {
+	gchar *rf_mode_str;
+
 	printf("nfc%d:\n", adapter->idx);
 
 	nfctool_print_targets(adapter, "          ");
@@ -81,6 +110,15 @@ static void nfctool_print_adapter_info(struct nfc_adapter *adapter)
 	printf("          Powered: %s\n",
 		adapter->powered ? "Yes" : "No");
 
+	if (adapter->rf_mode == NFC_RF_INITIATOR)
+		rf_mode_str = "Initiator";
+	else if (adapter->rf_mode == NFC_RF_TARGET)
+		rf_mode_str = "Target";
+	else
+		rf_mode_str = "None";
+
+	printf("          RF Mode: %s\n", rf_mode_str);
+
 	printf("\n");
 }
 
@@ -104,9 +142,54 @@ static void nfctool_adapter_free(struct nfc_adapter *adapter)
 	g_free(adapter);
 }
 
+static gchar *nfctool_poll_mode_str(int mode)
+{
+	if (mode == POLLING_MODE_TARGET)
+		return "target";
+
+	if (mode == POLLING_MODE_BOTH)
+		return "both initiator and target";
+
+	return "initiator";
+}
+
+static int nfctool_start_poll(void)
+{
+	int err;
+
+	struct nfc_adapter *adapter;
+
+	adapter = nfctool_find_adapter(opts.adapter_idx);
+
+	if (adapter == NULL) {
+		print_error("Invalid adapter index: %d", opts.adapter_idx);
+
+		return -ENODEV;
+	}
+
+	err = nl_start_poll(adapter, opts.poll_mode);
+
+	if (err == 0) {
+		printf("Start polling on nfc%d as %s\n\n",
+			adapter->idx, nfctool_poll_mode_str(opts.poll_mode));
+		return 0;
+	}
+
+	if (err != -EBUSY)
+		return err;
+
+	if (adapter->rf_mode == NFC_RF_NONE)
+		printf("nfc%d already in polling mode\n\n", adapter->idx);
+	else
+		printf("nfc%d already activated\n\n", adapter->idx);
+
+	return err;
+}
+
 static void nfctool_get_device(struct nfc_adapter *adapter)
 {
-	nl_get_targets(adapter);
+	if (adapter->rf_mode == NFC_RF_INITIATOR)
+		nl_get_targets(adapter);
 }
 
 static int nfctool_get_devices(void)
@@ -122,17 +205,123 @@ static int nfctool_get_devices(void)
 	return 0;
 }
 
+static int nfctool_tm_activated(void)
+{
+	printf("Target mode activated\n");
+
+	g_main_loop_quit(main_loop);
+
+	return 0;
+}
+
+static void nfctool_send_dep_link_up(guint32 target_idx, guint32 adapter_idx)
+{
+	nl_send_dep_link_up(adapter_idx, target_idx);
+}
+
+static int nfctool_targets_found(guint32 adapter_idx)
+{
+	int err;
+	struct nfc_adapter *adapter;
+
+	DBG("adapter_idx: %d", adapter_idx);
+
+	if (adapter_idx == INVALID_ADAPTER_IDX)
+		return -ENODEV;
+
+	adapter = nfctool_find_adapter(adapter_idx);
+
+	if (adapter == NULL)
+		return -ENODEV;
+
+	err = nl_get_targets(adapter);
+	if (err) {
+		print_error("Error getting targets\n");
+		goto exit;
+	}
+
+	printf("Targets found for nfc%d\n", adapter_idx);
+	nfctool_print_targets(adapter, "  ");
+
+	if (adapter->polling) {
+		g_slist_foreach(adapter->devices,
+				(GFunc)nfctool_send_dep_link_up,
+				GINT_TO_POINTER(adapter_idx));
+
+		adapter->polling = FALSE;
+	}
+
+exit:
+	g_main_loop_quit(main_loop);
+
+	return err;
+}
+
+static int nfc_event_cb(guint8 cmd, guint32 idx)
+{
+	int err = 0;
+
+	switch (cmd) {
+	case NFC_EVENT_TARGETS_FOUND:
+		DBG("Targets found");
+		err = nfctool_targets_found(idx);
+		break;
+	case NFC_EVENT_TM_ACTIVATED:
+		DBG("Target mode activated");
+		err = nfctool_tm_activated();
+		break;
+	}
+
+	return err;
+}
+
+static volatile sig_atomic_t __terminated = 0;
+
+static void sig_term(int sig)
+{
+	if (__terminated > 0)
+		return;
+
+	__terminated = 1;
+
+	DBG("Terminating");
+
+	g_main_loop_quit(main_loop);
+}
+
 struct nfctool_options opts = {
 	.list = FALSE,
+	.poll = FALSE,
+	.poll_mode = POLLING_MODE_INITIATOR,
 	.device_name = NULL,
 	.adapter_idx = INVALID_ADAPTER_IDX,
 };
 
+static gboolean opt_parse_poll_arg(const gchar *option_name, const gchar *value,
+				   gpointer data, GError **error)
+{
+	opts.poll = TRUE;
+
+	opts.poll_mode = POLLING_MODE_INITIATOR;
+
+	if (value != NULL) {
+		if (*value == 't' || *value == 'T')
+			opts.poll_mode = POLLING_MODE_TARGET;
+		else if (*value == 'b' || *value == 'B')
+			opts.poll_mode = POLLING_MODE_BOTH;
+	}
+
+	return TRUE;
+}
+
 static GOptionEntry option_entries[] = {
 	{ "list", 'l', 0, G_OPTION_ARG_NONE, &opts.list,
 	  "list attached NFC devices", NULL },
 	{ "device", 'd', 0, G_OPTION_ARG_STRING, &opts.device_name,
 	  "specify a nfc device", "nfcX" },
+	{ "poll", 'p', G_OPTION_FLAG_OPTIONAL_ARG, G_OPTION_ARG_CALLBACK,
+	  opt_parse_poll_arg, "start polling as initiator, target, or both; "
+	  "default mode is initiator", "[Initiator|Target|Both]" },
 	{ NULL }
 };
 
@@ -173,12 +362,18 @@ static int nfctool_options_parse(int argc, char **argv)
 		}
 	}
 
-	if (!opts.list) {
+	if (!opts.poll && !opts.list) {
 		printf("%s", g_option_context_get_help(context, TRUE, NULL));
 
 		goto exit;
 	}
 
+	if (opts.poll && opts.adapter_idx == INVALID_ADAPTER_IDX) {
+		print_error("Please specify a device with -d nfcX option");
+
+		goto exit;
+	}
+
 	err = 0;
 
 exit:
@@ -187,12 +382,32 @@ exit:
 	return err;
 }
 
+static void nfctool_main_loop_start(void)
+{
+	struct sigaction sa;
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_handler = sig_term;
+	sigaction(SIGINT, &sa, NULL);
+	sigaction(SIGTERM, &sa, NULL);
+
+	main_loop = g_main_loop_new(NULL, FALSE);
+
+	g_main_loop_run(main_loop);
+}
+
 static void nfctool_options_cleanup(void)
 {
 	if (opts.device_name != NULL)
 		g_free(opts.device_name);
 }
 
+static void nfctool_main_loop_clean(void)
+{
+	if (main_loop != NULL)
+		g_main_loop_unref(main_loop);
+}
+
 int main(int argc, char **argv)
 {
 	int err;
@@ -201,7 +416,7 @@ int main(int argc, char **argv)
 	if (err)
 		goto exit_err;
 
-	err = nl_init();
+	err = nl_init(nfc_event_cb);
 	if (err)
 		goto exit_err;
 
@@ -212,9 +427,19 @@ int main(int argc, char **argv)
 	if (opts.list)
 		nfctool_list_adapters();
 
+	if (opts.poll) {
+		err = nfctool_start_poll();
+		if (err)
+			goto exit_err;
+
+		nfctool_main_loop_start();
+	}
+
 	err = 0;
 
 exit_err:
+	nfctool_main_loop_clean();
+
 	g_slist_free_full(adapters, (GDestroyNotify)nfctool_adapter_free);
 
 	nl_cleanup();
diff --git a/tools/nfctool/netlink.c b/tools/nfctool/netlink.c
index c7bf07a..67f1d10 100644
--- a/tools/nfctool/netlink.c
+++ b/tools/nfctool/netlink.c
@@ -43,6 +43,10 @@ struct nlnfc_state {
 
 static struct nlnfc_state *nfc_state = NULL;
 
+static GIOChannel *nl_gio_channel = NULL;
+
+static nfc_event_cb_t nfc_event_cb = NULL;
+
 static void adapter_add_target(struct nfc_adapter *adapter,
 				guint8 type, guint32 idx)
 {
@@ -202,6 +206,13 @@ nla_put_failure:
 	return err;
 }
 
+static int nl_no_seq_check_cb(struct nl_msg *n, void *arg)
+{
+	DBG("");
+
+	return NL_OK;
+}
+
 static int nl_get_targets_handler(struct nl_msg *n, void *arg)
 {
 	struct nlmsghdr *nlh = nlmsg_hdr(n);
@@ -305,6 +316,7 @@ static int nl_get_devices_handler(struct nl_msg *n, void *arg)
 	adapter->idx = idx;
 	adapter->protocols = protocols;
 	adapter->powered = powered;
+	adapter->rf_mode = rf_mode;
 
 	if (rf_mode == NFC_RF_TARGET)
 		adapter_add_target(adapter, TARGET_TYPE_DEVICE, 0);
@@ -347,8 +359,139 @@ out:
 	return err;
 }
 
+int nl_send_dep_link_up(guint32 idx, guint32 target_idx)
+{
+	struct nl_msg *msg;
+	void *hdr;
+	int err;
+
+	DBG("");
+
+	msg = nlmsg_alloc();
+	if (msg == NULL)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
+			  NLM_F_REQUEST, NFC_CMD_DEP_LINK_UP, NFC_GENL_VERSION);
+	if (hdr == NULL) {
+		err = -EINVAL;
+		goto nla_put_failure;
+	}
+
+	err = -EMSGSIZE;
+
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
+	NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx);
+	NLA_PUT_U8(msg, NFC_ATTR_COMM_MODE, NFC_COMM_ACTIVE);
+	NLA_PUT_U8(msg, NFC_ATTR_RF_MODE, NFC_RF_INITIATOR);
+
+	err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
+
+nla_put_failure:
+	nlmsg_free(msg);
+
+	return err;
+}
+
+int nl_start_poll(struct nfc_adapter *adapter, guint8 mode)
+{
+	struct nl_msg *msg;
+	void *hdr;
+	int err;
+	guint32 im_protocols = 0;
+	guint32 tm_protocols = 0;
+
+	if ((mode & POLLING_MODE_INITIATOR) == POLLING_MODE_INITIATOR)
+		im_protocols = adapter->protocols;
+
+	if ((mode & POLLING_MODE_TARGET) == POLLING_MODE_TARGET)
+		tm_protocols = adapter->protocols;
+
+	DBG("IM protos 0x%x TM protos 0x%x", im_protocols, tm_protocols);
+
+	msg = nlmsg_alloc();
+	if (msg == NULL)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
+			  NLM_F_REQUEST, NFC_CMD_START_POLL, NFC_GENL_VERSION);
+	if (hdr == NULL) {
+		err = -EINVAL;
+		goto nla_put_failure;
+	}
+
+	err = -EMSGSIZE;
+
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);
+	if (im_protocols != 0) {
+		NLA_PUT_U32(msg, NFC_ATTR_IM_PROTOCOLS, im_protocols);
+		NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, im_protocols);
+	}
+	if (tm_protocols != 0)
+		NLA_PUT_U32(msg, NFC_ATTR_TM_PROTOCOLS, tm_protocols);
+
+	err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
+
+	if (err == 0)
+		adapter->polling = TRUE;
+
+nla_put_failure:
+	nlmsg_free(msg);
+
+	return err;
+}
+
+static int nl_nfc_event_cb(struct nl_msg *n, void *arg)
+{
+	guint32 idx = INVALID_ADAPTER_IDX;
+	struct nlattr *attr[NFC_ATTR_MAX + 1];
+	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(n));
+
+	if (nfc_event_cb == NULL)
+		return NL_SKIP;
+
+	nla_parse(attr, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+		genlmsg_attrlen(gnlh, 0), NULL);
+
+	if (attr[NFC_ATTR_DEVICE_INDEX] != NULL)
+		idx = nla_get_u32(attr[NFC_ATTR_DEVICE_INDEX]);
+
+	nfc_event_cb(gnlh->cmd, idx);
+
+	return NL_SKIP;
+}
+
+static gboolean nl_gio_handler(GIOChannel *channel,
+			       GIOCondition cond, gpointer data)
+{
+	struct nl_cb *cb;
+	struct nlnfc_state *state = data;
+
+	if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
+		return FALSE;
+
+	cb = nl_cb_alloc(NL_CB_VERBOSE);
+	if (cb == NULL)
+		return TRUE;
+
+	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_no_seq_check_cb, NULL);
+	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, nl_nfc_event_cb, data);
+
+	nl_recvmsgs(state->event_sock, cb);
+
+	nl_cb_put(cb);
+
+	return TRUE;
+}
+
 void nl_cleanup(void)
 {
+	if (nl_gio_channel) {
+		g_io_channel_shutdown(nl_gio_channel, TRUE, NULL);
+		g_io_channel_unref(nl_gio_channel);
+		nl_gio_channel = NULL;
+	}
+
 	if (nfc_state) {
 		if (nfc_state->cmd_sock)
 			nl_socket_free(nfc_state->cmd_sock);
@@ -361,9 +504,10 @@ void nl_cleanup(void)
 	}
 }
 
-int nl_init(void)
+int nl_init(nfc_event_cb_t cb)
 {
 	int err;
+	int fd;
 
 	DBG("");
 
@@ -417,6 +561,19 @@ int nl_init(void)
 		goto exit_err;
 	}
 
+	fd = nl_socket_get_fd(nfc_state->event_sock);
+	nl_gio_channel = g_io_channel_unix_new(fd);
+	g_io_channel_set_close_on_unref(nl_gio_channel, TRUE);
+
+	g_io_channel_set_encoding(nl_gio_channel, NULL, NULL);
+	g_io_channel_set_buffered(nl_gio_channel, FALSE);
+
+	g_io_add_watch(nl_gio_channel,
+		       G_IO_IN | G_IO_NVAL | G_IO_HUP | G_IO_ERR,
+		       nl_gio_handler, nfc_state);
+
+	nfc_event_cb = cb;
+
 	return 0;
 
 exit_err:
diff --git a/tools/nfctool/netlink.h b/tools/nfctool/netlink.h
index 99e4e91..6b5174e 100644
--- a/tools/nfctool/netlink.h
+++ b/tools/nfctool/netlink.h
@@ -21,9 +21,11 @@
 #ifndef __NETLINK_H
 #define __NETLINK_H
 
+typedef int (*nfc_event_cb_t)(guint8 cmd, guint32 adapter_idx);
+
 struct nfc_adapter;
 
-int nl_init(void);
+int nl_init(nfc_event_cb_t cb);
 
 void nl_cleanup(void);
 
@@ -31,4 +33,8 @@ int nl_get_devices(void);
 
 int nl_get_targets(struct nfc_adapter *adapter);
 
+int nl_send_dep_link_up(guint32 idx, guint32 target_idx);
+
+int nl_start_poll(struct nfc_adapter *adapter, guint8 mode);
+
 #endif /* __NETLINK_H */
diff --git a/tools/nfctool/nfctool.h b/tools/nfctool/nfctool.h
index 35d985b..7507eea 100644
--- a/tools/nfctool/nfctool.h
+++ b/tools/nfctool/nfctool.h
@@ -30,6 +30,10 @@
 
 #define print_error(fmt, ...) fprintf(stderr, fmt"\n", ## __VA_ARGS__)
 
+#define POLLING_MODE_INITIATOR	0x01
+#define POLLING_MODE_TARGET	0x02
+#define POLLING_MODE_BOTH	0x03
+
 #define INVALID_ADAPTER_IDX 0xFFFFFFFF
 
 #define TARGET_TYPE_TAG		0
@@ -44,12 +48,16 @@ struct nfc_adapter {
 	guint32 idx;
 	guint32 protocols;
 	guint8 powered;
+	guint8 polling;
+	guint8 rf_mode;
 	GSList *tags;
 	GSList *devices;
 };
 
 struct nfctool_options {
 	gboolean list;
+	gboolean poll;
+	guint8 poll_mode;
 	gchar *device_name;
 	guint32 adapter_idx;
 };