tools/nfctool/sniffer.c

   1 /*
   2  *
   3  *  Near Field Communication nfctool
   4  *
   5  *  Copyright (C) 2012  Intel Corporation. All rights reserved.
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License version 2 as
   9  *  published by the Free Software Foundation.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <stdlib.h>
  24 #include <unistd.h>
  25 #include <string.h>
  26 #include <ctype.h>
  27 #include <sys/types.h>
  28 #include <sys/socket.h>
  29 #include <sys/time.h>
  30 #include <netdb.h>
  31 #include <netinet/in.h>
  32 #include <arpa/inet.h>
  33 #include <linux/tcp.h>
  34 #include <linux/ip.h>
  35 #include <errno.h>
  36 #include <glib.h>
  37
  38 #include <near/nfc_copy.h>
  39
  40 #include "nfctool.h"
  41 #include "llcp-decode.h"
  42 #include "sniffer.h"
  43
  44 #define PCAP_MAGIC_NUMBER 0xa1b2c3d4
  45 #define PCAP_MAJOR_VER 2
  46 #define PCAP_MINOR_VER 4
  47 #define PCAP_SNAP_LEN 0xFFFF
  48 #define PCAP_NETWORK 0xF5
  49
  50 #define SNAP_LEN 1024
  51
  52 static GIOChannel *gio_channel = NULL;
  53 guint watch = 0;
  54
  55 static FILE *pcap_file = NULL;
  56 static guint8 *buffer;
  57
  58 static int pcap_file_write_packet(guint8 *data, guint32 len,
  59                                   struct timeval *timestamp)
  60 {
  61         guint32 val32;
  62         guint32 incl_len;
  63
  64         if (pcap_file == NULL || data == NULL || len == 0)
  65                 return -EINVAL;
  66
  67         val32 = timestamp->tv_sec;
  68         if (fwrite(&val32, 4, 1, pcap_file) < 1)
  69                 goto exit_err;
  70
  71         val32 = timestamp->tv_usec;
  72         if (fwrite(&val32, 4, 1, pcap_file) < 1)
  73                 goto exit_err;
  74
  75         if (len > PCAP_SNAP_LEN)
  76                 incl_len = PCAP_SNAP_LEN;
  77         else
  78                 incl_len = len;
  79
  80         if (fwrite(&incl_len, 4, 1, pcap_file) < 1)
  81                 goto exit_err;
  82
  83         if (fwrite(&len, 4, 1, pcap_file) < 1)
  84                 goto exit_err;
  85
  86         if (fwrite(data, 1, incl_len, pcap_file) < incl_len)
  87                 goto exit_err;
  88
  89         return 0;
  90
  91 exit_err:
  92         return -errno;
  93 }
  94
  95 static int pcap_file_init(char *pcap_filename)
  96 {
  97         int err = 0;
  98         guint16 value16;
  99         guint32 value32;
 100
 101         pcap_file = fopen(pcap_filename, "w");
 102
 103         if (!pcap_file) {
 104                 err = errno;
 105                 print_error("Can't open file %s: %s",
 106                                 pcap_filename, strerror(err));
 107                 return -err;
 108         }
 109
 110         value32 = PCAP_MAGIC_NUMBER;
 111         if (fwrite(&value32, 4, 1, pcap_file) < 1)
 112                 goto exit_err;
 113
 114         value16 = PCAP_MAJOR_VER;
 115         if (fwrite(&value16, 2, 1, pcap_file) < 1)
 116                 goto exit_err;
 117
 118         value16 = PCAP_MINOR_VER;
 119         if (fwrite(&value16, 2, 1, pcap_file) < 1)
 120                 goto exit_err;
 121
 122         value32 = 0;
 123         if (fwrite(&value32, 4, 1, pcap_file) < 1)
 124                 goto exit_err;
 125         if (fwrite(&value32, 4, 1, pcap_file) < 1)
 126                 goto exit_err;
 127
 128         value32 = PCAP_SNAP_LEN;
 129         if (fwrite(&value32, 4, 1, pcap_file) < 1)
 130                 goto exit_err;
 131
 132         value32 = PCAP_NETWORK;
 133         if (fwrite(&value32, 4, 1, pcap_file) < 1)
 134                 goto exit_err;
 135
 136         return 0;
 137
 138 exit_err:
 139         return -errno;
 140 }
 141
 142 static void pcap_file_cleanup(void)
 143 {
 144         if (pcap_file != NULL) {
 145                 fclose(pcap_file);
 146                 pcap_file = NULL;
 147         }
 148 }
 149
 150
 151 #define LINE_SIZE (10 + 3 * 16 + 2 + 18 + 1)
 152 #define HUMAN_READABLE_OFFSET 59
 153
 154 /*
 155  * Dumps data in Hex+ASCII format as:
 156  *
 157  * 00000000: 01 01 43 20 30 70 72 6F 70 65 72 74 69 65 73 20  |..C 0properties |
 158  *
 159  */
 160 void sniffer_print_hexdump(FILE *file, guint8 *data, guint32 len,
 161                            gchar *line_prefix, gboolean print_len)
 162 {
 163         guint8 digits;
 164         guint32 offset;
 165         guint32 total;
 166         gchar line[LINE_SIZE];
 167         gchar *hexa = NULL, *human = NULL;
 168
 169         if (len <= 0)
 170                 return;
 171
 172         offset = 0;
 173         digits = 0;
 174         total = 0;
 175
 176         if (print_len) {
 177                 if (line_prefix)
 178                         fprintf(file, "%s", line_prefix);
 179
 180                 fprintf(file, "Total length: %u bytes\n", len);
 181         }
 182
 183         while (total < len) {
 184                 if (digits == 0) {
 185                         memset(line, ' ', HUMAN_READABLE_OFFSET);
 186
 187                         sprintf(line, "%08X: ", offset);
 188                         offset += 16;
 189
 190                         hexa = line + 8 + 2;
 191
 192                         human = line + HUMAN_READABLE_OFFSET;
 193                         *human++ = '|';
 194                 }
 195
 196                 sprintf(hexa, "%02hhX ", data[total]);
 197                 *human++ = isprint((int)data[total]) ? (char)data[total] : '.';
 198                 hexa += 3;
 199
 200                 if (++digits >= 16) {
 201                         *hexa = ' ';
 202                         strcpy(human, "|");
 203                         if (line_prefix)
 204                                 fprintf(file, "%s", line_prefix);
 205                         fprintf(file, "%s\n", line);
 206
 207                         digits = 0;
 208                 }
 209
 210                 total++;
 211         }
 212
 213         if ((len & 0xF) != 0) {
 214                 *hexa = ' ';
 215                 strcpy(human, "|");
 216                 if (line_prefix)
 217                         fprintf(file, "%s", line_prefix);
 218                 fprintf(file, "%s\n", line);
 219         }
 220 }
 221
 222 static gboolean gio_handler(GIOChannel *channel,
 223                                  GIOCondition cond, gpointer data)
 224 {
 225         struct msghdr msg;
 226         struct iovec iov;
 227         int sock;
 228         int len;
 229         guint8 ctrl[CMSG_SPACE(sizeof(struct timeval))];
 230         struct cmsghdr *cmsg;
 231         struct timeval msg_timestamp;
 232
 233         if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR)) {
 234                 print_error("Sniffer IO error 0x%x\n", cond);
 235
 236                 if (watch > 0)
 237                         g_source_remove(watch);
 238                 watch = 0;
 239                 gio_channel = NULL;
 240
 241                 return FALSE;
 242         }
 243
 244         sock = g_io_channel_unix_get_fd(channel);
 245
 246         memset(&msg, 0, sizeof(struct msghdr));
 247
 248         msg.msg_control = &ctrl;
 249         msg.msg_controllen = sizeof(ctrl);
 250
 251         msg.msg_iov = &iov;
 252         msg.msg_iovlen = 1;
 253         iov.iov_base = buffer;
 254         iov.iov_len = opts.snap_len;
 255
 256         len = recvmsg(sock, &msg, 0);
 257         if (len < 0) {
 258                 print_error("recv: %s", strerror(errno));
 259                 return FALSE;
 260         }
 261
 262         cmsg = CMSG_FIRSTHDR(&msg);
 263         if (cmsg && cmsg->cmsg_type == SCM_TIMESTAMP)
 264                 memcpy(&msg_timestamp, CMSG_DATA(cmsg), sizeof(struct timeval));
 265         else
 266                 gettimeofday(&msg_timestamp, NULL);
 267
 268         llcp_print_pdu(buffer, len, &msg_timestamp);
 269
 270         pcap_file_write_packet(buffer, len, &msg_timestamp);
 271
 272         return TRUE;
 273 }
 274
 275 void sniffer_cleanup(void)
 276 {
 277         DBG("gio_channel: %p", gio_channel);
 278
 279         if (gio_channel) {
 280                 g_io_channel_shutdown(gio_channel, TRUE, NULL);
 281                 g_io_channel_unref(gio_channel);
 282
 283                 gio_channel = NULL;
 284         }
 285
 286         pcap_file_cleanup();
 287
 288         llcp_decode_cleanup();
 289 }
 290
 291 int sniffer_init(void)
 292 {
 293         struct sockaddr_nfc_llcp sockaddr;
 294         int sock = 0;
 295         int err;
 296         int one = 1;
 297
 298         if (opts.snap_len < SNAP_LEN)
 299                 opts.snap_len = SNAP_LEN;
 300
 301         buffer = g_malloc(opts.snap_len);
 302
 303         sock = socket(AF_NFC, SOCK_RAW, NFC_SOCKPROTO_LLCP);
 304
 305         if (sock < 0) {
 306                 print_error("socket: %s", strerror(errno));
 307                 return -1;
 308         }
 309
 310         err = setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &one, sizeof(one));
 311         if (err < 0)
 312                 print_error("setsockopt: %s", strerror(errno));
 313
 314         memset(&sockaddr, 0, sizeof(struct sockaddr_nfc_llcp));
 315         sockaddr.sa_family = AF_NFC;
 316         sockaddr.dev_idx = opts.adapter_idx;
 317         sockaddr.nfc_protocol = NFC_PROTO_NFC_DEP;
 318
 319         err = bind(sock, (struct sockaddr *)&sockaddr,
 320                         sizeof(struct sockaddr_nfc_llcp));
 321
 322         if (err < 0) {
 323                 print_error("bind: %s", strerror(errno));
 324                 goto exit;
 325         }
 326
 327         gio_channel = g_io_channel_unix_new(sock);
 328         g_io_channel_set_close_on_unref(gio_channel, TRUE);
 329
 330         g_io_channel_set_encoding(gio_channel, NULL, NULL);
 331         g_io_channel_set_buffered(gio_channel, FALSE);
 332
 333         watch = g_io_add_watch(gio_channel,
 334                        G_IO_IN | G_IO_NVAL | G_IO_HUP | G_IO_ERR,
 335                        gio_handler, NULL);
 336
 337         g_io_channel_unref(gio_channel);
 338
 339         if (opts.pcap_filename != NULL) {
 340                 err = pcap_file_init(opts.pcap_filename);
 341                 if (err)
 342                         goto exit;
 343         }
 344
 345         err = llcp_decode_init();
 346         if (err)
 347                 goto exit;
 348
 349         printf("Start sniffer on nfc%d\n\n", opts.adapter_idx);
 350
 351 exit:
 352         if (err)
 353                 sniffer_cleanup();
 354
 355         return err;
 356 }