nfctool: Add Service Name Lookup support
This adds a --snl option to get the sap number for the passed service name
(i.e. urn:nfc:sn:snep). nfctool will wait until it receives a result for
all the requested services before exiting.
diff --git a/tools/nfctool/main.c b/tools/nfctool/main.c
index d0fe128..5bdd5fc 100644
--- a/tools/nfctool/main.c
+++ b/tools/nfctool/main.c
@@ -40,6 +40,7 @@
static GMainLoop *main_loop = NULL;
static int nfctool_poll_cb(guint8 cmd, guint32 idx, gpointer data);
+static int nfctool_snl_cb(guint8 cmd, guint32 idx, gpointer data);
static void nfctool_quit(gboolean force);
@@ -87,6 +88,10 @@
else
printf("nfc%d already activated\n\n", adapter->idx);
+ /* Don't fail if there is a pending SNL request */
+ if (opts.snl)
+ return 0;
+
return err;
}
@@ -113,6 +118,81 @@
return err;
}
+static int nfctool_snl_send_request(struct nfc_adapter *adapter)
+{
+ int err;
+
+ nl_add_event_handler(NFC_EVENT_LLC_SDRES, nfctool_snl_cb);
+
+ err = nl_send_sdreq(adapter, opts.snl_list);
+ if (err)
+ print_error("Can't send SNL request: %s", strerror(-err));
+
+ return err;
+}
+
+static int nfctool_dep_link_up_cb(guint8 cmd, guint32 idx, gpointer data)
+{
+ struct nfc_adapter *adapter;
+
+ printf("Link is UP for adapter nfc%d\n\n", idx);
+
+ if (idx != opts.adapter_idx)
+ return -ENODEV;
+
+ adapter = adapter_get(idx);
+ if (!adapter)
+ return -ENODEV;
+
+ nfctool_snl_send_request(adapter);
+
+ return 0;
+}
+
+static int nfctool_dep_link_down_cb(guint8 cmd, guint32 idx, gpointer data)
+{
+ if (idx != opts.adapter_idx)
+ return -ENODEV;
+
+ printf("Link is DOWN for adapter nfc%d\n\n", idx);
+
+ opts.snl = FALSE;
+
+ nfctool_quit(FALSE);
+
+ return 0;
+}
+
+static int nfctool_snl(void)
+{
+ struct nfc_adapter *adapter;
+
+ adapter = adapter_get(opts.adapter_idx);
+ if (!adapter)
+ return -ENODEV;
+
+ if (adapter->polling) {
+ /* Delay netlink message until the link is established */
+ nl_add_event_handler(NFC_CMD_DEP_LINK_UP,
+ nfctool_dep_link_up_cb);
+
+ nl_add_event_handler(NFC_CMD_DEP_LINK_DOWN,
+ nfctool_dep_link_down_cb);
+
+ return 0;
+ }
+
+ if (adapter->rf_mode == NFC_RF_NONE) {
+ print_error("Can't send SNL request: No active link");
+
+ opts.snl = FALSE;
+
+ return -ENOLINK;
+ }
+
+ return nfctool_snl_send_request(adapter);
+}
+
static void nfctool_send_dep_link_up(guint32 target_idx, guint32 adapter_idx)
{
nl_send_dep_link_up(adapter_idx, target_idx);
@@ -141,6 +221,7 @@
printf("Targets found for nfc%d\n", adapter_idx);
adpater_print_targets(adapter, " ");
+ printf("\n");
if (adapter->polling) {
g_slist_foreach(adapter->devices,
@@ -174,6 +255,65 @@
return err;
}
+static void nfctool_print_and_remove_snl(struct nfc_snl *sdres,
+ guint32 adapter_idx)
+{
+ GSList *elem;
+
+ printf(" uri: %s - sap: %d\n", sdres->uri, sdres->sap);
+
+ if (adapter_idx == opts.adapter_idx) {
+ elem = g_slist_find_custom(opts.snl_list, sdres->uri,
+ (GCompareFunc)g_strcmp0);
+
+ if (elem != NULL) {
+ g_free(elem->data);
+ opts.snl_list = g_slist_delete_link(opts.snl_list,
+ elem);
+ }
+ }
+}
+
+static int nfctool_snl_cb(guint8 cmd, guint32 idx, gpointer data)
+{
+ GSList *sdres_list = (GSList *)data;
+
+ printf("nfc%d: Service Name lookup:\n", idx);
+
+ g_slist_foreach(sdres_list, (GFunc)nfctool_print_and_remove_snl,
+ GINT_TO_POINTER(idx));
+
+ printf("\n");
+
+ if (opts.snl_list == NULL) {
+ opts.snl = FALSE;
+ nfctool_quit(FALSE);
+ }
+
+ return 0;
+}
+
+struct nfc_snl *nfctool_snl_alloc(gsize uri_size)
+{
+ struct nfc_snl *snl;
+
+ snl = g_malloc(sizeof(struct nfc_snl));
+
+ snl->uri = g_malloc0(uri_size);
+ snl->uri_size = uri_size;
+ snl->sap = 0;
+
+ return snl;
+}
+
+void nfctool_sdres_free(struct nfc_snl *snl)
+{
+ if (snl->uri_size)
+ g_free(snl->uri);
+
+ g_free(snl);
+}
+
static volatile sig_atomic_t __terminated = 0;
static void sig_term(int sig)
@@ -199,6 +339,8 @@
.rw = -1,
.miux = -1,
.need_netlink = FALSE,
+ .snl = FALSE,
+ .snl_list = NULL,
.sniff = FALSE,
.snap_len = 0,
.dump_symm = FALSE,
@@ -313,6 +455,20 @@
return TRUE;
}
+static gboolean opt_parse_snl_arg(const gchar *option_name, const gchar *value,
+ gpointer data, GError **error)
+{
+ gchar *uri;
+
+ opts.snl = TRUE;
+
+ uri = g_strdup(value);
+
+ opts.snl_list = g_slist_prepend(opts.snl_list, uri);
+
+ return TRUE;
+}
+
static GOptionEntry option_entries[] = {
{ "list", 'l', 0, G_OPTION_ARG_NONE, &opts.list,
"list attached NFC devices", NULL },
@@ -323,6 +479,8 @@
"default mode is initiator", "[Initiator|Target|Both]" },
{ "set-param", 's', 0, G_OPTION_ARG_CALLBACK, opt_parse_set_param_arg,
"set lto, rw, and/or miux parameters", "lto=150,rw=1,miux=100" },
+ { "snl", 'k', 0, G_OPTION_ARG_CALLBACK, &opt_parse_snl_arg,
+ "Send a Service Name Lookup request", "urn:nfc:sn:snep"},
{ "sniff", 'n', 0, G_OPTION_ARG_NONE, &opts.sniff,
"start LLCP sniffer on the specified device", NULL },
{ "snapshot-len", 'a', 0, G_OPTION_ARG_INT, &opts.snap_len,
@@ -377,7 +535,8 @@
}
}
- opts.need_netlink = opts.list || opts.poll || opts.set_param;
+ opts.need_netlink = opts.list || opts.poll ||
+ opts.set_param || opts.snl;
if (!opts.need_netlink && !opts.sniff) {
printf("%s", g_option_context_get_help(context, TRUE, NULL));
@@ -385,7 +544,7 @@
goto exit;
}
- if ((opts.poll || opts.set_param || opts.sniff) &&
+ if ((opts.poll || opts.set_param || opts.sniff || opts.snl) &&
opts.adapter_idx == INVALID_ADAPTER_IDX) {
print_error("Please specify a device with -d nfcX option");
@@ -421,6 +580,8 @@
if (opts.pcap_filename != NULL)
g_free(opts.pcap_filename);
+
+ g_slist_free_full(opts.snl_list, g_free);
}
static void nfctool_main_loop_clean(void)
@@ -431,7 +592,7 @@
static void nfctool_quit(gboolean force)
{
- if (force || !opts.sniff)
+ if (force || (!opts.sniff && !opts.snl))
g_main_loop_quit(main_loop);
}
@@ -453,25 +614,6 @@
err = adapter_all_get_devices();
if (err)
goto exit_err;
-
- if (opts.list && !opts.set_param)
- adapter_idx_print_info(opts.adapter_idx);
-
- if (opts.set_param) {
- err = nfctool_set_params();
- if (err)
- goto exit_err;
- }
-
- if (opts.poll) {
- err = nfctool_start_poll();
-
- if (err == -EBUSY && opts.sniff)
- err = 0;
-
- if (err)
- goto exit_err;
- }
}
if (opts.sniff) {
@@ -480,7 +622,29 @@
goto exit_err;
}
- if (opts.poll || opts.sniff)
+ if (opts.list && !opts.set_param)
+ adapter_idx_print_info(opts.adapter_idx);
+
+ if (opts.set_param) {
+ err = nfctool_set_params();
+ if (err)
+ goto exit_err;
+ }
+
+ if (opts.poll) {
+ err = nfctool_start_poll();
+
+ if (err == -EBUSY && opts.sniff)
+ err = 0;
+
+ if (err)
+ goto exit_err;
+ }
+
+ if (opts.snl)
+ nfctool_snl();
+
+ if (opts.poll || opts.sniff || opts.snl)
nfctool_main_loop_start();
err = 0;
diff --git a/tools/nfctool/netlink.c b/tools/nfctool/netlink.c
index f27deeb..dd0bef6 100644
--- a/tools/nfctool/netlink.c
+++ b/tools/nfctool/netlink.c
@@ -556,6 +556,116 @@
return err;
}
+int nl_send_sdreq(struct nfc_adapter *adapter, GSList *uris)
+{
+ struct nl_msg *msg;
+ void *hdr;
+ GSList *uri;
+ struct nlattr *sdp_attr, *uri_attr;
+ int err = 0, i;
+
+ DBG("");
+
+ if (nfc_state == NULL || nfc_state->nfc_id < 0)
+ return -ENODEV;
+
+ 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_LLC_SDREQ, NFC_GENL_VERSION);
+ if (hdr == NULL) {
+ err = -EINVAL;
+ goto nla_put_failure;
+ }
+
+ NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);
+
+ sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP);
+ if (sdp_attr == NULL) {
+ err = -ENOMEM;
+ goto nla_put_failure;
+ }
+
+ i = 1;
+ uri = uris;
+ while (uri != NULL) {
+ uri_attr = nla_nest_start(msg, i);
+
+ if (uri_attr == NULL) {
+ err = -ENOMEM;
+ goto nla_put_failure;
+ }
+
+ NLA_PUT_STRING(msg, NFC_SDP_ATTR_URI, uri->data);
+
+ nla_nest_end(msg, uri_attr);
+
+ uri = g_slist_next(uri);
+
+ i++;
+ }
+
+ nla_nest_end(msg, sdp_attr);
+
+ err = nl_send_msg(nfc_state->event_sock, msg, NULL, NULL);
+
+ DBG("nl_send_msg returns %d", err);
+
+nla_put_failure:
+ nlmsg_free(msg);
+
+ return err;
+}
+
+static int nl_nfc_send_sdres_event(guint32 idx, struct nlattr *sdres_attr,
+ nfc_event_cb_t handler)
+{
+ GSList *sdres_list = NULL;
+ struct nfc_snl *sdres;
+ struct nlattr *attr;
+ struct nlattr *sdp_attrs[NFC_SDP_ATTR_MAX + 1];
+ int rem, err = 0;
+ size_t uri_len;
+
+ nla_for_each_nested(attr, sdres_attr, rem) {
+ err = nla_parse_nested(sdp_attrs, NFC_SDP_ATTR_MAX,
+ attr, NULL);
+
+ if (err != 0) {
+ DBG("nla_parse_nested returned %d\n", err);
+ continue;
+ }
+
+ if (sdp_attrs[NFC_SDP_ATTR_URI] == NULL ||
+ sdp_attrs[NFC_SDP_ATTR_SAP] == NULL) {
+ print_error("Missing attribute in reply");
+ continue;
+ }
+
+ uri_len = nla_strlcpy(NULL, sdp_attrs[NFC_SDP_ATTR_URI], 0);
+
+ uri_len++;
+
+ sdres = nfctool_snl_alloc(uri_len);
+
+ nla_strlcpy(sdres->uri, sdp_attrs[NFC_SDP_ATTR_URI], uri_len);
+
+ sdres->sap = nla_get_u8(sdp_attrs[NFC_SDP_ATTR_SAP]);
+
+ DBG("URI: %s, sap: %d\n", sdres->uri, sdres->sap);
+
+ sdres_list = g_slist_append(sdres_list, sdres);
+ }
+
+ handler(NFC_EVENT_LLC_SDRES, idx, sdres_list);
+
+ g_slist_free_full(sdres_list, (GDestroyNotify)nfctool_sdres_free);
+
+ return err;
+}
+
static int nl_nfc_event_cb(struct nl_msg *n, void *arg)
{
guint32 idx = INVALID_ADAPTER_IDX;
@@ -574,8 +684,19 @@
if (handlers != NULL)
cb = g_hash_table_lookup(handlers, GINT_TO_POINTER(gnlh->cmd));
- if (cb != NULL)
+ if (cb == NULL)
+ return NL_SKIP;
+
+ switch (gnlh->cmd) {
+ case NFC_EVENT_LLC_SDRES:
+ DBG("Received NFC_EVENT_LLC_SDRES\n");
+ nl_nfc_send_sdres_event(idx, attr[NFC_ATTR_LLC_SDP], cb);
+ break;
+
+ default:
cb(gnlh->cmd, idx, NULL);
+ break;
+ }
return NL_SKIP;
}
diff --git a/tools/nfctool/netlink.h b/tools/nfctool/netlink.h
index c8dc66a..e1b8bd7 100644
--- a/tools/nfctool/netlink.h
+++ b/tools/nfctool/netlink.h
@@ -44,4 +44,6 @@
int nl_get_params(struct nfc_adapter *adapter);
+int nl_send_sdreq(struct nfc_adapter *adapter, GSList *uris);
+
#endif /* __NETLINK_H */
diff --git a/tools/nfctool/nfctool.h b/tools/nfctool/nfctool.h
index f9e2918..b11e584 100644
--- a/tools/nfctool/nfctool.h
+++ b/tools/nfctool/nfctool.h
@@ -54,6 +54,8 @@
gint32 rw;
gint32 miux;
gboolean need_netlink;
+ gboolean snl;
+ GSList *snl_list;
gboolean sniff;
gsize snap_len;
gboolean dump_symm;
@@ -61,6 +63,16 @@
gchar *pcap_filename;
};
+struct nfc_snl {
+ gchar *uri;
+ gsize uri_size;
+ guint8 sap;
+};
+
+struct nfc_snl *nfctool_snl_alloc(gsize uri_size);
+
+void nfctool_sdres_free(struct nfc_snl *snl);
+
extern struct nfctool_options opts;
