nfctool: sniffer: Add NDEF decoding support

This adds support for decoding NDEF records embedded in SNEP packets

4 files changed, 230 insertions, 6 deletions

diff --git a/tools/nfctool/ndef-decode.c b/tools/nfctool/ndef-decode.c
new file mode 100644
index 0000000..5fa9c02
--- /dev/null
+++ b/tools/nfctool/ndef-decode.c
@@ -0,0 +1,189 @@
+/*
+ *
+ *  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 "nfctool.h"
+#include "sniffer.h"
+#include "ndef-decode.h"
+
+#define bool_str(b) (b) ? "True" : "False"
+
+#define ndef_printf_header(fmt, ...) print_indent(NDEF_HEADER_INDENT, \
+						NDEF_COLOR, fmt, ## __VA_ARGS__)
+
+#define ndef_printf_msg(fmt, ...) print_indent(NDEF_MSG_INDENT, NDEF_COLOR, \
+						fmt, ## __VA_ARGS__)
+
+#define ndef_printf_error(fmt, ...) print_indent(NDEF_MSG_INDENT, COLOR_ERROR, \
+						fmt, ## __VA_ARGS__)
+
+static gchar *tnf_str[] = {
+	"Empty (0x00)",
+	"NFC Forum well-known type [NFC RTD] (0x01)",
+	"Media-type as defined in RFC 2046 [RFC 2046] (0x02)",
+	"Absolute URI as defined in RFC 3986 [RFC 3986] (0x03)",
+	"NFC Forum external type [NFC RTD] (0x04)",
+	"Unknown (0x05)",
+	"Unchanged (0x06)",
+	"Reserved (0x07)"
+};
+
+int ndef_print_records(guint8 *data, guint32 data_len)
+{
+	gboolean mb, me, cf, sr, il;
+	guint8 tnf;
+	guint8 type_len;
+	guint8 *type;
+	guint32 payload_len;
+	guint8 *payload;
+	guint8 id_len;
+	guint8 *id;
+	guint32 offset;
+	guint8 *record;
+	int err = 0;
+
+#define CHECK_OFFSET(s)							\
+	do {								\
+		if (data_len - offset < (s)) {				\
+			ndef_printf_error("Malformed NDEF record");	\
+			sniffer_print_hexdump(stdout, data, data_len,	\
+					      NDEF_HEX_INDENT, TRUE);	\
+			err = -EINVAL;					\
+			goto exit;					\
+		}							\
+	} while (0)
+
+	offset = 0;
+
+	while (offset < data_len) {
+
+		ndef_printf_header("NDEF Record");
+
+		record = data + offset;
+		id_len = 0;
+		type = NULL;
+		id = NULL;
+		payload = NULL;
+
+		CHECK_OFFSET(2);
+
+		mb  = (record[0] & 0x80) != 0;
+		me  = (record[0] & 0x40) != 0;
+		cf  = (record[0] & 0x20) != 0;
+		sr  = (record[0] & 0x10) != 0;
+		il  = (record[0] & 0x08) != 0;
+		tnf = (record[0] & 0x07) != 0;
+
+		type_len = record[1];
+
+		offset += 2;
+
+		if (sr) {
+			CHECK_OFFSET(1);
+
+			payload_len = record[offset];
+
+			offset++;
+		} else {
+			CHECK_OFFSET(4);
+
+			memcpy(&payload_len, record + offset, 4);
+
+			payload_len = GUINT_FROM_BE(payload_len);
+
+			offset += 4;
+		}
+
+		if (il) {
+			CHECK_OFFSET(1);
+
+			id_len = record[offset];
+
+			offset++;
+		}
+
+		if (type_len > 0) {
+			CHECK_OFFSET(type_len);
+
+			type = record + offset;
+
+			offset += type_len;
+		}
+
+		if (id_len > 0) {
+			CHECK_OFFSET(id_len);
+
+			id = record + offset;
+
+			offset += id_len;
+		}
+
+		if (payload_len) {
+			CHECK_OFFSET(payload_len);
+
+			payload = record + offset;
+
+			offset += payload_len;
+		}
+
+		ndef_printf_msg("Message Begin: %s",     bool_str(mb));
+		ndef_printf_msg("Message End: %s",       bool_str(me));
+		ndef_printf_msg("Chunk Flag: %s",        bool_str(cf));
+		ndef_printf_msg("Short Record: %s",      bool_str(sr));
+		ndef_printf_msg("ID Length present: %s", bool_str(il));
+		ndef_printf_msg("Type Name Format: %s",  tnf_str[tnf]);
+		ndef_printf_msg("Type Length: %u",       type_len);
+		ndef_printf_msg("Payload Length: %u",    payload_len);
+
+		if (il)
+			ndef_printf_msg("ID Length: %u", id_len);
+
+		if (type) {
+			ndef_printf_msg("Type:");
+
+			sniffer_print_hexdump(stdout, type, type_len,
+					      NDEF_HEX_INDENT, FALSE);
+		}
+
+		if (id) {
+			ndef_printf_msg("ID:");
+
+			sniffer_print_hexdump(stdout, id, id_len,
+					      NDEF_HEX_INDENT, FALSE);
+		}
+
+		if (payload) {
+			ndef_printf_msg("Payload:");
+
+			sniffer_print_hexdump(stdout, payload, payload_len,
+					      NDEF_HEX_INDENT, FALSE);
+		}
+	}
+
+exit:
+	return err;
+
+#undef CHECK_OFFSET
+}
diff --git a/tools/nfctool/ndef-decode.h b/tools/nfctool/ndef-decode.h
new file mode 100644
index 0000000..5ef8b84
--- /dev/null
+++ b/tools/nfctool/ndef-decode.h
@@ -0,0 +1,26 @@
+/*
+ *
+ *  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 __NDEF_DECODE_H
+#define __NDEF_DECODE_H
+
+int ndef_print_records(guint8 *data, guint32 data_len);
+
+#endif /* __NDEF_DECODE_H */
diff --git a/tools/nfctool/nfctool.h b/tools/nfctool/nfctool.h
index 9c4cf60..53d8f35 100644
--- a/tools/nfctool/nfctool.h
+++ b/tools/nfctool/nfctool.h
@@ -38,6 +38,11 @@
 #define SNEP_HEADER_INDENT 4
 #define SNEP_MSG_INDENT    6
 
+#define NDEF_COLOR COLOR_BLUE
+#define NDEF_HEADER_INDENT 6
+#define NDEF_MSG_INDENT    8
+#define NDEF_HEX_INDENT    9
+
 #define print_error(fmt, ...) fprintf(stderr, fmt"\n", ## __VA_ARGS__)
 
 #define POLLING_MODE_INITIATOR	0x01
diff --git a/tools/nfctool/snep-decode.c b/tools/nfctool/snep-decode.c
index 03e941c..1a7fbc5 100644
--- a/tools/nfctool/snep-decode.c
+++ b/tools/nfctool/snep-decode.c
@@ -29,6 +29,7 @@
 
 #include "nfctool.h"
 #include "sniffer.h"
+#include "ndef-decode.h"
 #include "snep-decode.h"
 
 #define SNEP_HEADER_LEN 6
@@ -111,6 +112,8 @@ static void snep_frag_rejected(struct sniffer_packet *packet)
 static int snep_frag_append(struct snep_frag *frag,
 			    struct sniffer_packet *packet)
 {
+	int err = 0;
+
 	snep_printf_msg("Ongoing fragmented message");
 
 	if (frag->received + packet->llcp.data_len > frag->buffer_size) {
@@ -132,25 +135,26 @@ static int snep_frag_append(struct snep_frag *frag,
 	if (frag->received == frag->buffer_size) {
 		snep_printf_msg("End of fragmented message");
 
-		sniffer_print_hexdump(stdout, frag->buffer, frag->buffer_size,
-				      6, TRUE);
+		err = ndef_print_records(packet->snep.data,
+					 packet->snep.data_len);
 
 		snep_frag_delete(frag->index);
 	}
 
-	return 0;
+	return err;
 }
 
 static int snep_decode_info(struct sniffer_packet *packet)
 {
 	struct snep_frag *frag;
+	int err;
 
 	if (packet->snep.data_len <= packet->snep.real_len) {
 		/* Message is not fragmented */
-		sniffer_print_hexdump(stdout, packet->snep.data,
-				      packet->snep.data_len, 6, TRUE);
+		err = ndef_print_records(packet->snep.data,
+					 packet->snep.data_len);
 
-		return 0;
+		return err;
 	}
 
 	frag = g_malloc(sizeof(struct snep_frag));