nfctool: sniffer: Add SNEP decoding

This adds support for decoding Simple NDF Message Protocol. The SNEP decoder
supports aggregation of fragmented messages.

7 files changed, 435 insertions, 3 deletions

diff --git a/Makefile.am b/Makefile.am
index c4f1346..fca5caf 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -107,7 +107,9 @@ tools_nfctool_nfctool_SOURCES = tools/nfctool/main.c \
 					tools/nfctool/sniffer.h \
 					tools/nfctool/sniffer.c \
 					tools/nfctool/llcp-decode.h \
-					tools/nfctool/llcp-decode.c
+					tools/nfctool/llcp-decode.c \
+					tools/nfctool/snep-decode.h \
+					tools/nfctool/snep-decode.c
 
 tools_nfctool_nfctool_LDADD = @GLIB_LIBS@ @NETLINK_LIBS@
 
diff --git a/tools/nfctool/llcp-decode.c b/tools/nfctool/llcp-decode.c
index 7cadd0f..bb5dcfd 100644
--- a/tools/nfctool/llcp-decode.c
+++ b/tools/nfctool/llcp-decode.c
@@ -29,6 +29,7 @@
 
 #include "nfctool.h"
 #include "sniffer.h"
+#include "snep-decode.h"
 #include "llcp-decode.h"
 
 /* Raw socket + LLCP headers */
@@ -393,7 +394,18 @@ static int llcp_print_i(struct sniffer_packet *packet)
 {
 	llcp_print_sequence(packet);
 
-	sniffer_print_hexdump(stdout, packet->llcp.data, packet->llcp.data_len,
+	if (packet->llcp.local_sap == opts.snep_sap ||
+	    packet->llcp.remote_sap == opts.snep_sap) {
+		int err;
+
+		err = snep_print_pdu(packet);
+		if (err != 0)
+			printf("    Error decoding SNEP frame\n");
+
+		return err;
+	}
+
+	sniffer_print_hexdump(stdout, packet->llcp.data,  packet->llcp.data_len,
 				"  ", TRUE);
 
 	return 0;
@@ -517,11 +529,17 @@ exit:
 void llcp_decode_cleanup(void)
 {
 	timerclear(&start_timestamp);
+
+	snep_decode_cleanup();
 }
 
 int llcp_decode_init(void)
 {
+	int err;
+
 	timerclear(&start_timestamp);
 
-	return 0;
+	err = snep_decode_init();
+
+	return err;
 }
diff --git a/tools/nfctool/main.c b/tools/nfctool/main.c
index b0c2a36..f11d800 100644
--- a/tools/nfctool/main.c
+++ b/tools/nfctool/main.c
@@ -350,6 +350,7 @@ struct nfctool_options opts = {
 	.snap_len = 0,
 	.dump_symm = FALSE,
 	.show_timestamp = SNIFFER_SHOW_TIMESTAMP_NONE,
+	.snep_sap = 0x04,
 	.pcap_filename = NULL,
 };
 
@@ -495,6 +496,9 @@ static GOptionEntry option_entries[] = {
 	{ "dump-symm", 'y', 0, G_OPTION_ARG_NONE, &opts.dump_symm,
 	  "dump SYMM packets to stdout (flooding); only relevant with -n",
 	  NULL },
+	{ "snep-sap", 'e', 0, G_OPTION_ARG_INT, &opts.snep_sap,
+	  "Specify the sap number to be used for snep decoding; "
+	  "only relevant with -n", "0x04" },
 	{ "show-timestamp", 't', G_OPTION_FLAG_OPTIONAL_ARG,
 	  G_OPTION_ARG_CALLBACK, opt_parse_show_timestamp_arg,
 	  "show packet timestamp as the delta from first frame (default) "
diff --git a/tools/nfctool/nfctool.h b/tools/nfctool/nfctool.h
index 2b208f7..1fcbbb9 100644
--- a/tools/nfctool/nfctool.h
+++ b/tools/nfctool/nfctool.h
@@ -61,6 +61,7 @@ struct nfctool_options {
 	gsize snap_len;
 	gboolean dump_symm;
 	guint8 show_timestamp;
+	guint8 snep_sap;
 	gchar *pcap_filename;
 };
 
diff --git a/tools/nfctool/snep-decode.c b/tools/nfctool/snep-decode.c
new file mode 100644
index 0000000..7c89937
--- /dev/null
+++ b/tools/nfctool/snep-decode.c
@@ -0,0 +1,365 @@
+/*
+ *
+ *  Near Field Communication nfctool
+ *
+ *  Copyright (C) 2013  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 <glib.h>
+#include <errno.h>
+#include <string.h>
+#include <netdb.h>
+#include <sys/time.h>
+
+#include <near/nfc_copy.h>
+
+#include "nfctool.h"
+#include "sniffer.h"
+#include "snep-decode.h"
+
+#define HEADER_INDENT	"    "
+#define MSG_INDENT	"      "
+
+#define SNEP_HEADER_LEN 6
+
+#define SNEP_REQUEST_CONTINUE	0x00
+#define SNEP_REQUEST_GET	0x01
+#define SNEP_REQUEST_PUT	0x02
+#define SNEP_REQUEST_REJECT	0x7f
+
+#define SNEP_RESPONSE_CONTINUE		0x80
+#define SNEP_RESPONSE_SUCCESS		0x81
+#define SNEP_RESPONSE_NOT_FOUND		0xc0
+#define SNEP_RESPONSE_EXCESS_DATA	0xc1
+#define SNEP_RESPONSE_BAD_REQUEST	0xc2
+#define SNEP_RESPONSE_NOT_IMPLEMENTED	0xe0
+#define SNEP_RESPONSE_UNSUPPORTED	0xe1
+#define SNEP_RESPONSE_REJECT		0xff
+
+#define snep_make_frag_index(idx, dir, lsap, rsap) \
+				((((idx) << 24) & 0xFF000000) |	\
+				(((dir)  << 16) & 0x00FF0000) |	\
+				(((lsap) << 8)  & 0x0000FF00) |	\
+				((rsap)         & 0x000000FF))
+
+#define snep_get_frag_index(p)	\
+		snep_make_frag_index((p)->adapter_idx, (p)->direction, \
+				     (p)->llcp.local_sap, (p)->llcp.remote_sap)
+
+#define snep_printf(fmt, ...) printf(MSG_INDENT fmt, ## __VA_ARGS__)
+
+struct snep_frag {
+	guint32 index;
+	guint16 count;
+	guint32 received;
+	guint32 buffer_size;
+	guint8 *buffer;
+};
+
+static GHashTable *snep_frag_hash = NULL;
+
+static void snep_frag_free(struct snep_frag *frag)
+{
+	g_free(frag->buffer);
+	g_free(frag);
+}
+
+static void snep_frag_delete(guint32 frag_index)
+{
+	DBG("Deleting frag with index 0x%x", frag_index);
+
+	g_hash_table_remove(snep_frag_hash, GINT_TO_POINTER(frag_index));
+}
+
+static void snep_frag_rejected(struct sniffer_packet *packet)
+{
+	guint32 index;
+	guint8 direction;
+
+	/* reverse direction to delete the corresponding fragment */
+	if (packet->direction == NFC_LLCP_DIRECTION_RX)
+		direction = NFC_LLCP_DIRECTION_TX;
+	else
+		direction = NFC_LLCP_DIRECTION_RX;
+
+	index = snep_make_frag_index(packet->adapter_idx,
+				     direction,
+				     packet->llcp.local_sap,
+				     packet->llcp.remote_sap);
+
+	snep_frag_delete(index);
+}
+
+static int snep_frag_append(struct snep_frag *frag,
+			    struct sniffer_packet *packet)
+{
+	snep_printf("Ongoing fragmented message\n");
+
+	if (frag->received + packet->llcp.data_len > frag->buffer_size) {
+		snep_printf("Too many bytes received\n");
+		return -EINVAL;
+	}
+
+	memcpy(frag->buffer + frag->received, packet->llcp.data,
+	       packet->llcp.data_len);
+
+	frag->received += packet->llcp.data_len;
+
+	frag->count++;
+
+	snep_printf("Received fragment #%hu of %u bytes (total %u/%u)\n",
+		    frag->count, packet->llcp.data_len,
+		    frag->received, frag->buffer_size);
+
+	if (frag->received == frag->buffer_size) {
+		snep_printf("End of fragmented message\n");
+
+		sniffer_print_hexdump(stdout, frag->buffer, frag->buffer_size,
+				      MSG_INDENT, TRUE);
+
+		snep_frag_delete(frag->index);
+	}
+
+	return 0;
+}
+
+static int snep_decode_info(struct sniffer_packet *packet)
+{
+	struct snep_frag *frag;
+
+	if (packet->snep.data_len <= packet->snep.real_len) {
+		/* Message is not fragmented */
+		sniffer_print_hexdump(stdout, packet->snep.data,
+				      packet->snep.data_len, MSG_INDENT, TRUE);
+
+		return 0;
+	}
+
+	frag = g_malloc(sizeof(struct snep_frag));
+
+	frag->count = 1;
+
+	frag->buffer_size = packet->snep.data_len;
+
+	frag->received = packet->snep.real_len;
+
+	frag->buffer = g_malloc0(frag->buffer_size);
+
+	memcpy(frag->buffer, packet->snep.data, packet->snep.real_len);
+
+	frag->index = snep_get_frag_index(packet);
+
+	snep_printf("Start receiving fragmented message of %u bytes\n",
+		    frag->buffer_size);
+
+	snep_printf("Received fragment #%hu of %u bytes\n", frag->count,
+		    frag->received);
+
+	DBG("Adding frag with index 0x%x", frag->index);
+
+	g_hash_table_replace(snep_frag_hash, GINT_TO_POINTER(frag->index),
+			     frag);
+
+	return 0;
+}
+
+static int snep_decode_req_get(struct sniffer_packet *packet)
+{
+	guint32 acceptable_len;
+
+	if (packet->snep.real_len < 4)
+		return -EINVAL;
+
+	memcpy(&acceptable_len, packet->snep.data, 4);
+	packet->snep.acceptable_len = GUINT_FROM_BE(acceptable_len);
+
+	packet->snep.data += 4;
+	packet->snep.data_len -= 4;
+	packet->snep.real_len -= 4;
+
+	return 0;
+}
+
+static int snep_decode_header(struct sniffer_packet *packet)
+{
+	guint32 data_len;
+	guint8 *data = packet->llcp.data;
+
+	if (packet->llcp.data_len < SNEP_HEADER_LEN)
+		return -EINVAL;
+
+	packet->snep.version = data[0];
+
+	packet->snep.rcode = data[1];
+
+	memcpy(&data_len, data + 2, 4);
+
+	packet->snep.data_len = GUINT_FROM_BE(data_len);
+
+	packet->snep.real_len = packet->llcp.data_len - SNEP_HEADER_LEN;
+
+	if (packet->snep.data_len != 0)
+		packet->snep.data = data + SNEP_HEADER_LEN;
+
+	return 0;
+}
+
+static void snep_print_version(struct sniffer_packet *packet)
+{
+	snep_printf("Version: %d.%d\n", (packet->snep.version & 0xF0) >> 4,
+		    packet->snep.version & 0x0F);
+}
+
+static void print_request(struct sniffer_packet *packet, gchar *msg)
+{
+	snep_print_version(packet);
+
+	snep_printf("Request: %s\n", msg);
+}
+
+static void snep_print_response(struct sniffer_packet *packet, gchar *msg)
+{
+	snep_print_version(packet);
+
+	snep_printf("Response: %s\n", msg);
+}
+
+int snep_print_pdu(struct sniffer_packet *packet)
+{
+	int err = 0;
+	guint32 frag_index;
+	struct snep_frag *frag;
+
+	printf(HEADER_INDENT "Simple NDEF Messaging Protocol\n");
+
+	frag_index = snep_get_frag_index(packet);
+
+	frag = g_hash_table_lookup(snep_frag_hash, GINT_TO_POINTER(frag_index));
+
+	if (frag != NULL) {
+		/* Incoming or outgoing fragmented message */
+		err = snep_frag_append(frag, packet);
+
+		if (err != 0) {
+			snep_printf("Error receiving fragmented message\n");
+
+			snep_frag_delete(frag_index);
+		}
+
+		goto exit;
+	}
+
+	err = snep_decode_header(packet);
+	if (err != 0) {
+		snep_printf("Error decoding message header\n");
+
+		goto exit;
+	}
+
+	switch (packet->snep.rcode) {
+	/* Requests */
+	case SNEP_REQUEST_CONTINUE:
+		print_request(packet, "Continue");
+		break;
+
+	case SNEP_REQUEST_GET:
+		print_request(packet, "Get");
+
+		err = snep_decode_req_get(packet);
+		if (err != 0)
+			goto exit;
+
+		snep_printf("Acceptable length: %u\n",
+			    packet->snep.acceptable_len);
+
+		snep_decode_info(packet);
+		break;
+
+	case SNEP_REQUEST_PUT:
+		print_request(packet, "Put");
+
+		snep_decode_info(packet);
+		break;
+
+	case SNEP_REQUEST_REJECT:
+		print_request(packet, "Reject");
+
+		snep_frag_rejected(packet);
+		break;
+
+	/* Responses */
+	case SNEP_RESPONSE_CONTINUE:
+		snep_print_response(packet, "Continue");
+		break;
+
+	case SNEP_RESPONSE_SUCCESS:
+		snep_print_response(packet, "Success");
+
+		if (packet->snep.data_len > 0)
+			snep_decode_info(packet);
+		break;
+
+	case SNEP_RESPONSE_NOT_FOUND:
+		snep_print_response(packet, "Not Found");
+		break;
+
+	case SNEP_RESPONSE_EXCESS_DATA:
+		snep_print_response(packet, "Excess Data");
+		break;
+
+	case SNEP_RESPONSE_BAD_REQUEST:
+		snep_print_response(packet, "Bad Request");
+		break;
+
+	case SNEP_RESPONSE_NOT_IMPLEMENTED:
+		snep_print_response(packet, "Not Implemented");
+		break;
+
+	case SNEP_RESPONSE_UNSUPPORTED:
+		snep_print_response(packet, "Unsupported");
+		break;
+
+	case SNEP_RESPONSE_REJECT:
+		snep_print_response(packet, "Reject");
+
+		snep_frag_rejected(packet);
+		break;
+
+	default:
+		snep_printf("Invalid request or response code: %d\n",
+			    packet->snep.rcode);
+		break;
+	}
+
+exit:
+	return err;
+}
+
+void snep_decode_cleanup(void)
+{
+	if (snep_frag_hash != NULL)
+		g_hash_table_destroy(snep_frag_hash);
+}
+
+int snep_decode_init(void)
+{
+	snep_frag_hash =
+		g_hash_table_new_full(g_direct_hash, g_direct_equal, NULL,
+				      (GDestroyNotify)snep_frag_free);
+
+	return 0;
+}
diff --git a/tools/nfctool/snep-decode.h b/tools/nfctool/snep-decode.h
new file mode 100644
index 0000000..352ca7c
--- /dev/null
+++ b/tools/nfctool/snep-decode.h
@@ -0,0 +1,30 @@
+/*
+ *
+ *  Near Field Communication nfctool
+ *
+ *  Copyright (C) 2013  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 __SNEP_DECODE_H
+#define __SNEP_DECODE_H
+
+int snep_decode_init(void);
+
+void snep_decode_cleanup(void);
+
+int snep_print_pdu(struct sniffer_packet *packet);
+
+#endif /* __SNEP_DECODE_H */
diff --git a/tools/nfctool/sniffer.h b/tools/nfctool/sniffer.h
index 4de0b3f..6aa5ca5 100644
--- a/tools/nfctool/sniffer.h
+++ b/tools/nfctool/sniffer.h
@@ -41,6 +41,18 @@ struct sniffer_packet {
 		guint8 *data;
 		guint32 data_len;
 	} llcp;
+
+	struct {
+		guint8 version;
+
+		guint8 rcode;
+
+		guint32 acceptable_len;
+
+		guint8 *data;
+		guint32 data_len;
+		guint32 real_len;
+	} snep;
 };
 
 int sniffer_init(void);