neard: nfctool: Add LLCP traffic sniffing feature

nfctool -d nfcX --sniffer
This dumps LLCP frames to stdout (Hex+ASCII display)

nfctool -d nfcX --sniffer --pcap-file FILENAME
This saves LLCP frames in pcap format to file FILENAME. This pcap file
can be opened in wireshark (v>=1.8.2) with the wireshark-nfc plugin
available at http://code.google.com/p/wireshark-nfc/

4 files changed, 421 insertions, 19 deletions

diff --git a/tools/nfctool/main.c b/tools/nfctool/main.c
index 2e1d493..2c7cc8f 100644
--- a/tools/nfctool/main.c
+++ b/tools/nfctool/main.c
@@ -29,6 +29,7 @@
 
 #include "nfctool.h"
 #include "netlink.h"
+#include "sniffer.h"
 
 #define LLCP_MAX_LTO  0xff
 #define LLCP_MAX_RW   0x0f
@@ -242,7 +243,8 @@ static int nfctool_tm_activated(void)
 {
 	printf("Target mode activated\n");
 
-	g_main_loop_quit(main_loop);
+	if (!opts.sniff)
+		g_main_loop_quit(main_loop);
 
 	return 0;
 }
@@ -285,7 +287,8 @@ static int nfctool_targets_found(guint32 adapter_idx)
 	}
 
 exit:
-	g_main_loop_quit(main_loop);
+	if (!opts.sniff)
+		g_main_loop_quit(main_loop);
 
 	return err;
 }
@@ -332,6 +335,10 @@ struct nfctool_options opts = {
 	.lto = -1,
 	.rw = -1,
 	.miux = -1,
+	.need_netlink = FALSE,
+	.sniff = FALSE,
+	.snap_len = 0,
+	.pcap_filename = NULL,
 };
 
 static gboolean opt_parse_poll_arg(const gchar *option_name, const gchar *value,
@@ -439,6 +446,13 @@ static GOptionEntry option_entries[] = {
 	  "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" },
+	{ "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,
+	  "packet snapshot length (in bytes); only relevant with -n", "1024" },
+	{ "pcap-file", 'f', 0, G_OPTION_ARG_STRING, &opts.pcap_filename,
+	  "specify a filename to save traffic in pcap format; "
+	  "only relevant with -n", "filename" },
 	{ NULL }
 };
 
@@ -479,13 +493,15 @@ static int nfctool_options_parse(int argc, char **argv)
 		}
 	}
 
-	if (!opts.poll && !opts.list && !opts.set_param) {
+	opts.need_netlink = opts.list || opts.poll || opts.set_param;
+
+	if (!opts.need_netlink && !opts.sniff) {
 		printf("%s", g_option_context_get_help(context, TRUE, NULL));
 
 		goto exit;
 	}
 
-	if ((opts.poll || opts.set_param) &&
+	if ((opts.poll || opts.set_param || opts.sniff) &&
 	    opts.adapter_idx == INVALID_ADAPTER_IDX) {
 		print_error("Please specify a device with -d nfcX option");
 
@@ -518,6 +534,9 @@ static void nfctool_options_cleanup(void)
 {
 	if (opts.device_name != NULL)
 		g_free(opts.device_name);
+
+	if (opts.pcap_filename != NULL)
+		g_free(opts.pcap_filename);
 }
 
 static void nfctool_main_loop_clean(void)
@@ -534,30 +553,43 @@ int main(int argc, char **argv)
 	if (err)
 		goto exit_err;
 
-	err = nl_init(nfc_event_cb);
-	if (err)
-		goto exit_err;
-
-	err = nfctool_get_devices();
-	if (err)
-		goto exit_err;
-
-	if (opts.list && !opts.set_param)
-		nfctool_list_adapters();
+	if (opts.need_netlink) {
+		err = nl_init(nfc_event_cb);
+		if (err)
+			goto exit_err;
 
-	if (opts.set_param) {
-		err = nfctool_set_params();
+		err = nfctool_get_devices();
 		if (err)
 			goto exit_err;
+
+		if (opts.list && !opts.set_param)
+			nfctool_list_adapters();
+
+		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.poll) {
-		err = nfctool_start_poll();
+	if (opts.sniff) {
+		err = sniffer_init();
 		if (err)
 			goto exit_err;
+	}
 
+	if (opts.poll || opts.sniff)
 		nfctool_main_loop_start();
-	}
 
 	err = 0;
 
@@ -568,6 +600,8 @@ exit_err:
 
 	nl_cleanup();
 
+	sniffer_cleanup();
+
 	nfctool_options_cleanup();
 
 	return err;
diff --git a/tools/nfctool/nfctool.h b/tools/nfctool/nfctool.h
index 9562962..ada81dc 100644
--- a/tools/nfctool/nfctool.h
+++ b/tools/nfctool/nfctool.h
@@ -67,6 +67,10 @@ struct nfctool_options {
 	gint32 lto;
 	gint32 rw;
 	gint32 miux;
+	gboolean need_netlink;
+	gboolean sniff;
+	gsize snap_len;
+	gchar *pcap_filename;
 };
 
 extern struct nfctool_options opts;
diff --git a/tools/nfctool/sniffer.c b/tools/nfctool/sniffer.c
new file mode 100644
index 0000000..8e667dc
--- /dev/null
+++ b/tools/nfctool/sniffer.c
@@ -0,0 +1,331 @@
+/*
+ *
+ *  Near Field Communication nfctool
+ *
+ *  Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <ctype.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <linux/tcp.h>
+#include <linux/ip.h>
+#include <linux/nfc.h>
+#include <errno.h>
+#include <glib.h>
+
+#include "nfctool.h"
+#include "sniffer.h"
+
+#define PCAP_MAGIC_NUMBER 0xa1b2c3d4
+#define PCAP_MAJOR_VER 2
+#define PCAP_MINOR_VER 4
+#define PCAP_SNAP_LEN 0xFFFF
+#define PCAP_NETWORK 0xF5
+
+#define SNAP_LEN 1024
+
+static GIOChannel *gio_channel = NULL;
+
+static FILE *pcap_file = NULL;
+static guint8 *buffer;
+
+static int pcap_file_write_packet(guint8 *data, guint32 len,
+				  struct timeval *timestamp)
+{
+	guint32 val32;
+	guint32 incl_len;
+
+	if (pcap_file == NULL || data == NULL || len == 0)
+		return -EINVAL;
+
+	val32 = timestamp->tv_sec;
+	if (fwrite(&val32, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	val32 = timestamp->tv_usec;
+	if (fwrite(&val32, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	if (len > PCAP_SNAP_LEN)
+		incl_len = PCAP_SNAP_LEN;
+	else
+		incl_len = len;
+
+	if (fwrite(&incl_len, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	if (fwrite(&len, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	if (fwrite(data, 1, incl_len, pcap_file) < incl_len)
+		goto exit_err;
+
+	return 0;
+
+exit_err:
+	return -errno;
+}
+
+static int pcap_file_init(char *pcap_filename)
+{
+	int err = 0;
+	guint16 value16;
+	guint32 value32;
+
+	pcap_file = fopen(pcap_filename, "w");
+
+	if (!pcap_file) {
+		err = errno;
+		print_error("Can't open file %s: %s",
+				pcap_filename, strerror(err));
+		return -err;
+	}
+
+	value32 = PCAP_MAGIC_NUMBER;
+	if (fwrite(&value32, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	value16 = PCAP_MAJOR_VER;
+	if (fwrite(&value16, 2, 1, pcap_file) < 1)
+		goto exit_err;
+
+	value16 = PCAP_MINOR_VER;
+	if (fwrite(&value16, 2, 1, pcap_file) < 1)
+		goto exit_err;
+
+	value32 = 0;
+	if (fwrite(&value32, 4, 1, pcap_file) < 1)
+		goto exit_err;
+	if (fwrite(&value32, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	value32 = PCAP_SNAP_LEN;
+	if (fwrite(&value32, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	value32 = PCAP_NETWORK;
+	if (fwrite(&value32, 4, 1, pcap_file) < 1)
+		goto exit_err;
+
+	return 0;
+
+exit_err:
+	return -errno;
+}
+
+static void pcap_file_cleanup(void)
+{
+	if (pcap_file != NULL) {
+		fclose(pcap_file);
+		pcap_file = NULL;
+	}
+}
+
+
+#define LINE_SIZE (10 + 3 * 16 + 2 + 18 + 1)
+#define HUMAN_READABLE_OFFSET 59
+
+/*
+ * Dumps data in Hex+ASCII format as:
+ *
+ * 00000000: 01 01 43 20 30 70 72 6F 70 65 72 74 69 65 73 20  |..C 0properties |
+ *
+ */
+static void sniffer_print_hexdump(FILE *file, unsigned char *data, int len,
+							char *line_prefix)
+{
+	int digits;
+	int offset;
+	int total;
+	char line[LINE_SIZE];
+	char *hexa = NULL, *human = NULL;
+
+	if (len <= 0)
+		return;
+
+	offset = 0;
+	digits = 0;
+	total = 0;
+
+	while (total < len) {
+		if (digits == 0) {
+			memset(line, ' ', HUMAN_READABLE_OFFSET);
+
+			sprintf(line, "%08X: ", offset);
+			offset += 16;
+
+			hexa = line + 8 + 2;
+
+			human = line + HUMAN_READABLE_OFFSET;
+			*human++ = '|';
+		}
+
+		sprintf(hexa, "%02hhX ", data[total]);
+		*human++ = isprint((int)data[total]) ? (char)data[total] : '.';
+		hexa += 3;
+
+		if (++digits >= 16) {
+			*hexa = ' ';
+			strcpy(human, "|");
+			if (line_prefix)
+				fprintf(file, "%s", line_prefix);
+			fprintf(file, "%s\n", line);
+
+			digits = 0;
+		}
+
+		total++;
+	}
+
+	if ((len & 0xF) != 0) {
+		*hexa = ' ';
+		strcpy(human, "|");
+		if (line_prefix)
+			fprintf(file, "%s", line_prefix);
+		fprintf(file, "%s\n", line);
+	}
+}
+
+static gboolean gio_handler(GIOChannel *channel,
+				 GIOCondition cond, gpointer data)
+{
+	struct msghdr msg;
+	struct iovec iov;
+	int sock;
+	int len;
+	guint8 ctrl[CMSG_SPACE(sizeof(struct timeval))];
+	struct cmsghdr *cmsg;
+	struct timeval msg_timestamp;
+
+	if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
+		return FALSE;
+
+	sock = g_io_channel_unix_get_fd(channel);
+
+	memset(&msg, 0, sizeof(struct msghdr));
+
+	msg.msg_control = &ctrl;
+	msg.msg_controllen = sizeof(ctrl);
+
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+	iov.iov_base = buffer;
+	iov.iov_len = opts.snap_len;
+
+	len = recvmsg(sock, &msg, 0);
+	if (len < 0) {
+		print_error("recv: %s", strerror(errno));
+		return FALSE;
+	}
+
+	cmsg = CMSG_FIRSTHDR(&msg);
+	if (cmsg && cmsg->cmsg_type == SCM_TIMESTAMP)
+		memcpy(&msg_timestamp, CMSG_DATA(cmsg), sizeof(struct timeval));
+	else
+		gettimeofday(&msg_timestamp, NULL);
+
+	sniffer_print_hexdump(stdout, buffer, len, NULL);
+	printf("\n");
+
+	pcap_file_write_packet(buffer, len, &msg_timestamp);
+
+	return TRUE;
+}
+
+void sniffer_cleanup(void)
+{
+	DBG("gio_channel: %p", gio_channel);
+
+	if (gio_channel) {
+		g_io_channel_shutdown(gio_channel, TRUE, NULL);
+		g_io_channel_unref(gio_channel);
+
+		gio_channel = NULL;
+	}
+
+	pcap_file_cleanup();
+}
+
+int sniffer_init(void)
+{
+	struct sockaddr_nfc_llcp sockaddr;
+	int sock = 0;
+	int err;
+	int one = 1;
+
+	if (opts.snap_len < SNAP_LEN)
+		opts.snap_len = SNAP_LEN;
+
+	buffer = g_malloc(opts.snap_len);
+
+	sock = socket(AF_NFC, SOCK_RAW, NFC_SOCKPROTO_LLCP);
+
+	if (sock < 0) {
+		print_error("socket: %s", strerror(errno));
+		return -1;
+	}
+
+	err = setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &one, sizeof(one));
+	if (err < 0)
+		print_error("setsockopt: %s", strerror(errno));
+
+	memset(&sockaddr, 0, sizeof(struct sockaddr_nfc_llcp));
+	sockaddr.sa_family = AF_NFC;
+	sockaddr.dev_idx = opts.adapter_idx;
+	sockaddr.nfc_protocol = NFC_PROTO_NFC_DEP;
+
+	err = bind(sock, (struct sockaddr *)&sockaddr,
+			sizeof(struct sockaddr_nfc_llcp));
+
+	if (err < 0) {
+		print_error("bind: %s", strerror(errno));
+		goto exit;
+	}
+
+	gio_channel = g_io_channel_unix_new(sock);
+	g_io_channel_set_close_on_unref(gio_channel, TRUE);
+
+	g_io_channel_set_encoding(gio_channel, NULL, NULL);
+	g_io_channel_set_buffered(gio_channel, FALSE);
+
+	g_io_add_watch(gio_channel,
+		       G_IO_IN | G_IO_NVAL | G_IO_HUP | G_IO_ERR,
+		       gio_handler, NULL);
+
+	if (opts.pcap_filename != NULL) {
+		err = pcap_file_init(opts.pcap_filename);
+		if (err)
+			goto exit;
+	}
+
+	printf("Start sniffer on nfc%d\n\n", opts.adapter_idx);
+
+exit:
+	if (err)
+		sniffer_cleanup();
+
+	return err;
+}
diff --git a/tools/nfctool/sniffer.h b/tools/nfctool/sniffer.h
new file mode 100644
index 0000000..1eb42d8
--- /dev/null
+++ b/tools/nfctool/sniffer.h
@@ -0,0 +1,33 @@
+/*
+ *
+ *  Near Field Communication nfctool
+ *
+ *  Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef __SNIFFER_H
+#define __SNIFFER_H
+
+#ifndef AF_NFC
+#define AF_NFC 39
+#endif
+
+int sniffer_init(void);
+
+void sniffer_cleanup(void);
+
+#endif /* __SNIFFER_H */