From 4006662f681f955785eba0d717a3d3b07e7087d2 Mon Sep 17 00:00:00 2001 From: Thierry Escande Date: Fri, 14 Dec 2012 15:34:01 +0100 Subject: [PATCH] neard: nfctool: Add LLCP frame decoding support nfctool now dumps LLCP frames in a human readable manner --- Makefile.am | 3 +- tools/nfctool/llcp-decode.c | 489 ++++++++++++++++++++++++++++++++++++++++++++ tools/nfctool/llcp-decode.h | 30 +++ tools/nfctool/main.c | 21 ++ tools/nfctool/nfctool.h | 6 + tools/nfctool/sniffer.c | 12 +- tools/nfctool/sniffer.h | 3 + 7 files changed, 560 insertions(+), 4 deletions(-) create mode 100644 tools/nfctool/llcp-decode.c create mode 100644 tools/nfctool/llcp-decode.h diff --git a/Makefile.am b/Makefile.am index f2ca340..3c1c187 100644 --- a/Makefile.am +++ b/Makefile.am @@ -87,7 +87,8 @@ tools_snep_send_SOURCES = $(gdbus_sources) src/log.c src/dbus.c \ tools_snep_send_LDADD = @GLIB_LIBS@ @DBUS_LIBS@ tools_nfctool_nfctool_SOURCES = tools/nfctool/main.c tools/nfctool/netlink.c \ - tools/nfctool/sniffer.c + tools/nfctool/sniffer.c tools/nfctool/llcp-decode.c + tools_nfctool_nfctool_LDADD = @GLIB_LIBS@ @NETLINK_LIBS@ endif diff --git a/tools/nfctool/llcp-decode.c b/tools/nfctool/llcp-decode.c new file mode 100644 index 0000000..9ffc218 --- /dev/null +++ b/tools/nfctool/llcp-decode.c @@ -0,0 +1,489 @@ +/* + * + * 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 +#include +#include +#include +#include +#include +#include + +#include "nfctool.h" +#include "sniffer.h" +#include "llcp-decode.h" + +#define LLCP_MIN_HEADER_SIZE 2 + +#define LLCP_PTYPE_SYMM 0 +#define LLCP_PTYPE_PAX 1 +#define LLCP_PTYPE_AGF 2 +#define LLCP_PTYPE_UI 3 +#define LLCP_PTYPE_CONNECT 4 +#define LLCP_PTYPE_DISC 5 +#define LLCP_PTYPE_CC 6 +#define LLCP_PTYPE_DM 7 +#define LLCP_PTYPE_FRMR 8 +#define LLCP_PTYPE_SNL 9 +#define LLCP_PTYPE_I 12 +#define LLCP_PTYPE_RR 13 +#define LLCP_PTYPE_RNR 14 + +#define LLCP_DM_NORMAL 0x00 +#define LLCP_DM_NO_ACTIVE_CONN 0x01 +#define LLCP_DM_NOT_BOUND 0x02 +#define LLCP_DM_REJECTED 0x03 +#define LLCP_DM_PERM_SAP_FAILURE 0x10 +#define LLCP_DM_PERM_ALL_SAP_FAILURE 0x11 +#define LLCP_DM_TMP_SAP_FAILURE 0x20 +#define LLCP_DM_TMP_ALL_SAP_FAILURE 0x21 + +struct llcp_info { + guint8 ptype; + guint8 ssap; + guint8 dsap; + + guint8 *data; + guint32 data_len; +}; + +enum llcp_param_t { + LLCP_PARAM_VERSION = 1, + LLCP_PARAM_MIUX, + LLCP_PARAM_WKS, + LLCP_PARAM_LTO, + LLCP_PARAM_RW, + LLCP_PARAM_SN, + LLCP_PARAM_OPT, + LLCP_PARAM_SDREQ, + LLCP_PARAM_SDRES, + + LLCP_PARAM_MIN = LLCP_PARAM_VERSION, + LLCP_PARAM_MAX = LLCP_PARAM_SDRES +}; + +static guint8 llcp_param_length[] = { + 0, + 1, + 2, + 2, + 1, + 1, + 0, + 1, + 0, + 2 +}; + +static char *llcp_ptype_str[] = { + "Symmetry (SYMM)", + "Parameter Exchange (PAX)", + "Aggregated Frame (AGF)", + "Unnumbered Information (UI)", + "Connect (CONNECT)", + "Disconnect (DISC)", + "Connection Complete (CC)", + "Disconnected Mode (DM)", + "Frame Reject (FRMR)", + "Service Name Lookup (SNL)", + "reserved", + "reserved", + "Information (I)", + "Receive Ready (RR)", + "Receive Not Ready (RNR)", + "reserved", + "Unknown" +}; + +static char *llcp_ptype_short_str[] = { + "SYMM", + "PAX", + "AGF", + "UI", + "CONNECT", + "DISC", + "CC", + "DM", + "FRMR", + "SNL", + NULL, + NULL, + "I", + "RR", + "RNR", + NULL, + "Unknown" +}; + +static const gchar *llcp_param_str[] = { + "", + "Version Number", + "Maximum Information Unit Extensions", + "Well-Known Service List", + "Link Timeout", + "Receive Window Size", + "Service Name", + "Option", + "Service Discovery Request", + "Service Discovery Response" +}; + +#define llcp_get_param_str(param_type) llcp_param_str[param_type] + +#define llcp_get_param_len(param_type) llcp_param_length[param_type] + +#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) + +static struct timeval start_timestamp; + +static void llcp_print_params(guint8 *params, guint32 len) +{ + guint8 major, minor; + guint16 miux, wks, tid; + guint32 offset = 0; + guint32 rmng; + guint8 *param; + guint8 param_len; + gchar *sn; + + while (len - offset >= 3) { + param = params + offset; + rmng = len - offset; + + if (param[0] < LLCP_PARAM_MIN || param[0] > LLCP_PARAM_MAX) { + print_error("Error decoding params"); + return; + } + + param_len = llcp_get_param_len(param[0]); + + if (param_len == 0) + param_len = param[1]; + + if (param_len != param[1] || rmng < 2u + param_len) { + print_error("Error decoding params"); + return; + } + + printf(" %s: ", llcp_get_param_str(param[0])); + + switch ((enum llcp_param_t)param[0]) { + case LLCP_PARAM_VERSION: + major = (param[2] & 0xF0) >> 4; + minor = param[2] & 0x0F; + printf("%d.%d", major, minor); + break; + + case LLCP_PARAM_MIUX: + miux = ((param[2] & 0x07) << 8) | param[3]; + printf("%d", miux); + break; + + case LLCP_PARAM_WKS: + wks = (param[2] << 8) | param[3]; + printf("0x%02hX", wks); + break; + + case LLCP_PARAM_LTO: + printf("%d", param[2]); + break; + + case LLCP_PARAM_RW: + printf("%d", param[2] & 0x0F); + break; + + case LLCP_PARAM_SN: + sn = g_strndup((gchar *)param + 2, param_len); + printf("%s", sn); + g_free(sn); + break; + + case LLCP_PARAM_OPT: + printf("0x%X", param[2] & 0x03); + break; + + case LLCP_PARAM_SDREQ: + tid = param[2]; + sn = g_strndup((gchar *)param + 3, param_len - 1); + printf("TID:%d, SN:%s", tid, sn); + g_free(sn); + break; + + case LLCP_PARAM_SDRES: + printf("TID:%d, SAP:%d", param[2], param[3] & 0x3F); + break; + } + + printf("\n"); + + offset += 2 + param_len; + } +} + +static int llcp_decode_header(guint8 *data, guint32 data_len, + struct llcp_info *llcp) +{ + if (data_len < LLCP_MIN_HEADER_SIZE) + return -EINVAL; + + memset(llcp, 0, sizeof(struct llcp_info)); + + llcp->dsap = (data[0] & 0xFC) >> 2; + llcp->ssap = data[1] & 0x3F; + llcp->ptype = ((data[0] & 0x03) << 2) | ((data[1] & 0xC0) >> 6); + + if (llcp->ptype >= ARRAY_SIZE(llcp_ptype_str)) + return -EINVAL; + + llcp->data = data + 2; + llcp->data_len = data_len - 2; + + return 0; +} + +static int llcp_print_sequence(guint8 *data, guint32 data_len) +{ + guint8 send_seq; + guint8 recv_seq; + + if (data_len < 1) + return -EINVAL; + + send_seq = ((data[0] & 0xF0) >> 4); + recv_seq = data[0] & 0x0F; + + printf(" N(S):%d N(R):%d\n", send_seq, recv_seq); + + return 0; +} + +static int llcp_print_agf(guint8 *data, guint8 data_len, + struct timeval *timestamp) +{ + guint16 len; + guint16 offset = 0; + + if (data_len < 2) + return -EINVAL; + + while (offset < data_len - 2) { + len = (data[offset] << 8) | data[offset + 1]; + offset += 2; + + if (offset + len > data_len) + return -EINVAL; + + llcp_print_pdu(data + offset, len, timestamp); + + offset += len; + } + + return 0; +} + +static int llcp_print_dm(guint8 *data, guint8 data_len) +{ + gchar *reason; + + if (data_len != 1) + return -EINVAL; + + switch (data[0]) { + case LLCP_DM_NORMAL: + default: + reason = "Normal disconnect"; + break; + + case LLCP_DM_NO_ACTIVE_CONN: + reason = + "No active connection for connection-oriented PDU at SAP"; + break; + + case LLCP_DM_NOT_BOUND: + reason = "No service bound to target SAP"; + break; + + case LLCP_DM_REJECTED: + reason = "CONNECT PDU rejected by service layer"; + break; + + case LLCP_DM_PERM_SAP_FAILURE: + reason = "Permanent failure for target SAP"; + break; + + case LLCP_DM_PERM_ALL_SAP_FAILURE: + reason = "Permanent failure for any target SAP"; + break; + + case LLCP_DM_TMP_SAP_FAILURE: + reason = "Temporary failure for target SAP"; + break; + + case LLCP_DM_TMP_ALL_SAP_FAILURE: + reason = "Temporary failure for any target SAP"; + break; + } + + printf(" Reason: %d (%s)\n", data[0], reason); + + return 0; +} + +static int llcp_print_i(guint8 *data, guint8 data_len) +{ + if (llcp_print_sequence(data, data_len)) + return -EINVAL; + + sniffer_print_hexdump(stdout, data + 1, data_len - 1, " "); + + return 0; +} + +static int llcp_print_frmr(guint8 *data, guint8 data_len) +{ + guint8 val; + + if (data_len != 4) + return -EINVAL; + + val = data[0]; + printf("W:%d ", (val & 0x80) >> 7); + printf("I:%d ", (val & 0x40) >> 6); + printf("R:%d ", (val & 0x20) >> 5); + printf("S:%d ", (val & 0x10) >> 4); + val = val & 0x0F; + if (val >= ARRAY_SIZE(llcp_ptype_str)) + val = ARRAY_SIZE(llcp_ptype_str) - 1; + printf("PTYPE:%s ", llcp_ptype_short_str[val]); + printf("SEQ: %d ", data[1]); + printf("V(S): %d ", (data[2] & 0xF0) >> 4); + printf("V(R): %d ", data[2] & 0x0F); + printf("V(SA): %d ", (data[3] & 0xF0) >> 4); + printf("V(RA): %d\n", data[3] & 0x0F); + + return 0; +} + +int llcp_print_pdu(guint8 *data, guint32 data_len, struct timeval *timestamp) +{ + struct timeval msg_timestamp; + struct llcp_info llcp; + guint8 adapter_idx, direction; + gchar *direction_str; + int err; + + if (data_len < NFC_LLCP_RAW_HEADER_SIZE || timestamp == NULL) + return -EINVAL; + + if (!timerisset(&start_timestamp)) + start_timestamp = *timestamp; + + /* LLCP raw socket pseudo-header */ + adapter_idx = data[0]; + direction = data[1] & 0x01; + + err = llcp_decode_header(data + 2, data_len - 2, &llcp); + if (err) + goto exit; + + if (!opts.dump_symm && llcp.ptype == LLCP_PTYPE_SYMM) + return 0; + + /* LLCP header */ + if (direction == NFC_LLCP_DIRECTION_RX) + direction_str = ">>"; + else + direction_str = "<<"; + + printf("%s nfc%d: local:0x%02x remote:0x%02x", + direction_str, adapter_idx, llcp.ssap, llcp.dsap); + + if (opts.show_timestamp != SNIFFER_SHOW_TIMESTAMP_NONE) { + printf(" time: "); + + if (opts.show_timestamp == SNIFFER_SHOW_TIMESTAMP_ABS) { + msg_timestamp = *timestamp; + } else { + timersub(timestamp, &start_timestamp, &msg_timestamp); + printf("+"); + } + + printf("%lu.%06lu", msg_timestamp.tv_sec, + msg_timestamp.tv_usec); + } + + printf("\n"); + + printf(" %s\n", llcp_ptype_str[llcp.ptype]); + + switch (llcp.ptype) { + case LLCP_PTYPE_AGF: + llcp_print_agf(llcp.data, llcp.data_len, timestamp); + printf(" End of AGF frame\n"); + break; + + case LLCP_PTYPE_I: + llcp_print_i(llcp.data, llcp.data_len); + break; + + case LLCP_PTYPE_RR: + case LLCP_PTYPE_RNR: + llcp_print_sequence(llcp.data, llcp.data_len); + break; + + case LLCP_PTYPE_PAX: + case LLCP_PTYPE_CONNECT: + case LLCP_PTYPE_CC: + case LLCP_PTYPE_SNL: + llcp_print_params(llcp.data, llcp.data_len); + break; + + case LLCP_PTYPE_DM: + llcp_print_dm(llcp.data, llcp.data_len); + break; + + case LLCP_PTYPE_FRMR: + llcp_print_frmr(llcp.data, llcp.data_len); + break; + + default: + sniffer_print_hexdump(stdout, llcp.data, llcp.data_len, " "); + break; + } + + printf("\n"); + + err = 0; + +exit: + return err; +} + +void llcp_decode_cleanup(void) +{ + timerclear(&start_timestamp); +} + +int llcp_decode_init(void) +{ + timerclear(&start_timestamp); + + return 0; +} diff --git a/tools/nfctool/llcp-decode.h b/tools/nfctool/llcp-decode.h new file mode 100644 index 0000000..d822769 --- /dev/null +++ b/tools/nfctool/llcp-decode.h @@ -0,0 +1,30 @@ +/* + * + * 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 __LLCP_DECODE_H +#define __LLCP_DECODE_H + +int llcp_decode_init(void); + +void llcp_decode_cleanup(void); + +int llcp_print_pdu(guint8 *buffer, guint32 len, struct timeval *timestamp); + +#endif /* __LLCP_DECODE_H */ diff --git a/tools/nfctool/main.c b/tools/nfctool/main.c index 2c7cc8f..8906508 100644 --- a/tools/nfctool/main.c +++ b/tools/nfctool/main.c @@ -338,6 +338,8 @@ struct nfctool_options opts = { .need_netlink = FALSE, .sniff = FALSE, .snap_len = 0, + .dump_symm = FALSE, + .show_timestamp = SNIFFER_SHOW_TIMESTAMP_NONE, .pcap_filename = NULL, }; @@ -436,6 +438,18 @@ exit: return result; } +static gboolean opt_parse_show_timestamp_arg(const gchar *option_name, + const gchar *value, + gpointer data, GError **error) +{ + if (value != NULL && (*value == 'a' || *value == 'A')) + opts.show_timestamp = SNIFFER_SHOW_TIMESTAMP_ABS; + else + opts.show_timestamp = SNIFFER_SHOW_TIMESTAMP_DELTA; + + return TRUE; +} + static GOptionEntry option_entries[] = { { "list", 'l', 0, G_OPTION_ARG_NONE, &opts.list, "list attached NFC devices", NULL }, @@ -450,6 +464,13 @@ static GOptionEntry option_entries[] = { "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" }, + { "dump-symm", 'y', 0, G_OPTION_ARG_NONE, &opts.dump_symm, + "dump SYMM packets to stdout (flooding); only relevant with -n", + NULL }, + { "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) " + "or absolute value; only relevant with -n", "[delta|abs]" }, { "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" }, diff --git a/tools/nfctool/nfctool.h b/tools/nfctool/nfctool.h index ada81dc..40c6cec 100644 --- a/tools/nfctool/nfctool.h +++ b/tools/nfctool/nfctool.h @@ -39,6 +39,10 @@ #define TARGET_TYPE_TAG 0 #define TARGET_TYPE_DEVICE 1 +#define SNIFFER_SHOW_TIMESTAMP_NONE 0 +#define SNIFFER_SHOW_TIMESTAMP_DELTA 1 +#define SNIFFER_SHOW_TIMESTAMP_ABS 2 + struct nfc_target { guint32 idx; guint8 type; @@ -70,6 +74,8 @@ struct nfctool_options { gboolean need_netlink; gboolean sniff; gsize snap_len; + gboolean dump_symm; + guint8 show_timestamp; gchar *pcap_filename; }; diff --git a/tools/nfctool/sniffer.c b/tools/nfctool/sniffer.c index 8e667dc..2a8d176 100644 --- a/tools/nfctool/sniffer.c +++ b/tools/nfctool/sniffer.c @@ -37,6 +37,7 @@ #include #include "nfctool.h" +#include "llcp-decode.h" #include "sniffer.h" #define PCAP_MAGIC_NUMBER 0xa1b2c3d4 @@ -154,7 +155,7 @@ static void pcap_file_cleanup(void) * 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, +void sniffer_print_hexdump(FILE *file, unsigned char *data, int len, char *line_prefix) { int digits; @@ -247,8 +248,7 @@ static gboolean gio_handler(GIOChannel *channel, else gettimeofday(&msg_timestamp, NULL); - sniffer_print_hexdump(stdout, buffer, len, NULL); - printf("\n"); + llcp_print_pdu(buffer, len, &msg_timestamp); pcap_file_write_packet(buffer, len, &msg_timestamp); @@ -267,6 +267,8 @@ void sniffer_cleanup(void) } pcap_file_cleanup(); + + llcp_decode_cleanup(); } int sniffer_init(void) @@ -321,6 +323,10 @@ int sniffer_init(void) goto exit; } + err = llcp_decode_init(); + if (err) + goto exit; + printf("Start sniffer on nfc%d\n\n", opts.adapter_idx); exit: diff --git a/tools/nfctool/sniffer.h b/tools/nfctool/sniffer.h index 1eb42d8..cfa0eb4 100644 --- a/tools/nfctool/sniffer.h +++ b/tools/nfctool/sniffer.h @@ -30,4 +30,7 @@ int sniffer_init(void); void sniffer_cleanup(void); +void sniffer_print_hexdump(FILE *file, unsigned char *data, int len, + char *line_prefix); + #endif /* __SNIFFER_H */ -- 2.7.4