From 994489ccbdf7fb9543ea9595e0752c3fce9bbc60 Mon Sep 17 00:00:00 2001 From: Thierry Escande Date: Fri, 14 Dec 2012 15:34:00 +0100 Subject: 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/ --- Makefile.am | 3 +- tools/nfctool/main.c | 72 ++++++++--- tools/nfctool/nfctool.h | 4 + tools/nfctool/sniffer.c | 331 ++++++++++++++++++++++++++++++++++++++++++++++++ tools/nfctool/sniffer.h | 33 +++++ 5 files changed, 423 insertions(+), 20 deletions(-) create mode 100644 tools/nfctool/sniffer.c create mode 100644 tools/nfctool/sniffer.h diff --git a/Makefile.am b/Makefile.am index 79110a5..f2ca340 100644 --- a/Makefile.am +++ b/Makefile.am @@ -86,7 +86,8 @@ tools_snep_send_SOURCES = $(gdbus_sources) src/log.c src/dbus.c \ src/bluetooth.c src/ndef.c tools/snep-send.c src/agent.c tools_snep_send_LDADD = @GLIB_LIBS@ @DBUS_LIBS@ -tools_nfctool_nfctool_SOURCES = tools/nfctool/main.c tools/nfctool/netlink.c +tools_nfctool_nfctool_SOURCES = tools/nfctool/main.c tools/nfctool/netlink.c \ + tools/nfctool/sniffer.c tools_nfctool_nfctool_LDADD = @GLIB_LIBS@ @NETLINK_LIBS@ endif 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 */ -- cgit v1.2.3