From 8c96349bd66387a5ea62e01d64c22990ef773fac Mon Sep 17 00:00:00 2001 From: Thierry Escande Date: Thu, 2 May 2013 18:19:29 +0200 Subject: [PATCH] nfctool: sniffer: Add NDEF decoding support This adds support for decoding NDEF records embedded in SNEP packets --- Makefile.am | 2 + tools/nfctool/ndef-decode.c | 189 ++++++++++++++++++++++++++++++++++++++++++++ tools/nfctool/ndef-decode.h | 26 ++++++ tools/nfctool/nfctool.h | 5 ++ tools/nfctool/snep-decode.c | 16 ++-- 5 files changed, 232 insertions(+), 6 deletions(-) create mode 100644 tools/nfctool/ndef-decode.c create mode 100644 tools/nfctool/ndef-decode.h diff --git a/Makefile.am b/Makefile.am index 67699be..175cf07 100644 --- a/Makefile.am +++ b/Makefile.am @@ -110,6 +110,8 @@ tools_nfctool_nfctool_SOURCES = tools/nfctool/main.c \ tools/nfctool/llcp-decode.c \ tools/nfctool/snep-decode.h \ tools/nfctool/snep-decode.c \ + tools/nfctool/ndef-decode.h \ + tools/nfctool/ndef-decode.c \ tools/nfctool/display.h \ tools/nfctool/display.c 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 +#include +#include +#include + +#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)); -- 2.7.4