tools/nfctool/llcp-decode.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 #include <stdio.h>
  22 #include <glib.h>
  23 #include <errno.h>
  24 #include <string.h>
  25 #include <netdb.h>
  26 #include <sys/time.h>
  27
  28 #include <near/nfc_copy.h>
  29
  30 #include "nfctool.h"
  31 #include "sniffer.h"
  32 #include "snep-decode.h"
  33 #include "llcp-decode.h"
  34
  35 /* Raw socket + LLCP headers */
  36 #define RAW_LLCP_HEADERS_SIZE 4
  37
  38 #define LLCP_PTYPE_SYMM         0
  39 #define LLCP_PTYPE_PAX          1
  40 #define LLCP_PTYPE_AGF          2
  41 #define LLCP_PTYPE_UI           3
  42 #define LLCP_PTYPE_CONNECT      4
  43 #define LLCP_PTYPE_DISC         5
  44 #define LLCP_PTYPE_CC           6
  45 #define LLCP_PTYPE_DM           7
  46 #define LLCP_PTYPE_FRMR         8
  47 #define LLCP_PTYPE_SNL          9
  48 #define LLCP_PTYPE_I            12
  49 #define LLCP_PTYPE_RR           13
  50 #define LLCP_PTYPE_RNR          14
  51
  52 #define LLCP_DM_NORMAL                  0x00
  53 #define LLCP_DM_NO_ACTIVE_CONN          0x01
  54 #define LLCP_DM_NOT_BOUND               0x02
  55 #define LLCP_DM_REJECTED                0x03
  56 #define LLCP_DM_PERM_SAP_FAILURE        0x10
  57 #define LLCP_DM_PERM_ALL_SAP_FAILURE    0x11
  58 #define LLCP_DM_TMP_SAP_FAILURE         0x20
  59 #define LLCP_DM_TMP_ALL_SAP_FAILURE     0x21
  60
  61 enum llcp_param_t {
  62         LLCP_PARAM_VERSION = 1,
  63         LLCP_PARAM_MIUX,
  64         LLCP_PARAM_WKS,
  65         LLCP_PARAM_LTO,
  66         LLCP_PARAM_RW,
  67         LLCP_PARAM_SN,
  68         LLCP_PARAM_OPT,
  69         LLCP_PARAM_SDREQ,
  70         LLCP_PARAM_SDRES,
  71
  72         LLCP_PARAM_MIN = LLCP_PARAM_VERSION,
  73         LLCP_PARAM_MAX = LLCP_PARAM_SDRES
  74 };
  75
  76 static guint8 llcp_param_length[] = {
  77         0,
  78         1,
  79         2,
  80         2,
  81         1,
  82         1,
  83         0,
  84         1,
  85         0,
  86         2
  87 };
  88
  89 static char *llcp_ptype_str[] = {
  90         "Symmetry (SYMM)",
  91         "Parameter Exchange (PAX)",
  92         "Aggregated Frame (AGF)",
  93         "Unnumbered Information (UI)",
  94         "Connect (CONNECT)",
  95         "Disconnect (DISC)",
  96         "Connection Complete (CC)",
  97         "Disconnected Mode (DM)",
  98         "Frame Reject (FRMR)",
  99         "Service Name Lookup (SNL)",
 100         "reserved",
 101         "reserved",
 102         "Information (I)",
 103         "Receive Ready (RR)",
 104         "Receive Not Ready (RNR)",
 105         "reserved",
 106         "Unknown"
 107 };
 108
 109 static char *llcp_ptype_short_str[] = {
 110         "SYMM",
 111         "PAX",
 112         "AGF",
 113         "UI",
 114         "CONNECT",
 115         "DISC",
 116         "CC",
 117         "DM",
 118         "FRMR",
 119         "SNL",
 120         NULL,
 121         NULL,
 122         "I",
 123         "RR",
 124         "RNR",
 125         NULL,
 126         "Unknown"
 127 };
 128
 129 static const gchar *llcp_param_str[] = {
 130         "",
 131         "Version Number",
 132         "Maximum Information Unit Extensions",
 133         "Well-Known Service List",
 134         "Link Timeout",
 135         "Receive Window Size",
 136         "Service Name",
 137         "Option",
 138         "Service Discovery Request",
 139         "Service Discovery Response"
 140 };
 141
 142 #define llcp_printf_header(prefix, color, fmt, ...) \
 143                                 print_indent_prefix( LLCP_HEADER_INDENT,\
 144                                 color, prefix, \
 145                                 LLCP_COLOR, fmt, ## __VA_ARGS__)
 146
 147 #define llcp_printf_msg(fmt, ...) print_indent(LLCP_MSG_INDENT, \
 148                                                 LLCP_COLOR, fmt, ## __VA_ARGS__)
 149
 150 #define llcp_printf_error(fmt, ...) print_indent(LLCP_MSG_INDENT, \
 151                                                 COLOR_ERROR, fmt, ## __VA_ARGS__)
 152
 153 #define llcp_get_param_str(param_type) llcp_param_str[param_type]
 154
 155 #define llcp_get_param_len(param_type) llcp_param_length[param_type]
 156
 157 #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
 158
 159 static struct timeval start_timestamp;
 160
 161 static void llcp_print_params(struct sniffer_packet *packet)
 162 {
 163         guint8 major, minor;
 164         guint16 miux, wks, tid;
 165         guint32 offset = 0;
 166         guint32 rmng;
 167         guint8 *param;
 168         guint8 param_len;
 169         gchar *sn, param_str[64];
 170
 171         while (packet->llcp.data_len - offset >= 3) {
 172                 param = packet->llcp.data + offset;
 173                 rmng = packet->llcp.data_len - offset;
 174
 175                 if (param[0] < LLCP_PARAM_MIN || param[0] > LLCP_PARAM_MAX) {
 176                         llcp_printf_error("Error decoding params");
 177                         return;
 178                 }
 179
 180                 param_len = llcp_get_param_len(param[0]);
 181
 182                 if (param_len == 0)
 183                         param_len = param[1];
 184
 185                 if (param_len != param[1] || rmng < 2u + param_len) {
 186                         llcp_printf_error("Error decoding params");
 187                         return;
 188                 }
 189
 190                 switch ((enum llcp_param_t)param[0]) {
 191                 case LLCP_PARAM_VERSION:
 192                         major = (param[2] & 0xF0) >> 4;
 193                         minor = param[2] & 0x0F;
 194                         sprintf(param_str, "%d.%d", major, minor);
 195                         break;
 196
 197                 case LLCP_PARAM_MIUX:
 198                         miux = ((param[2] & 0x07) << 8) | param[3];
 199                         sprintf(param_str, "%d", miux);
 200                         break;
 201
 202                 case LLCP_PARAM_WKS:
 203                         wks = (param[2] << 8) | param[3];
 204                         sprintf(param_str, "0x%02hX", wks);
 205                         break;
 206
 207                 case LLCP_PARAM_LTO:
 208                         sprintf(param_str, "%d", param[2]);
 209                         break;
 210
 211                 case LLCP_PARAM_RW:
 212                         sprintf(param_str, "%d", param[2] & 0x0F);
 213                         break;
 214
 215                 case LLCP_PARAM_SN:
 216                         sn = g_strndup((gchar *)param + 2, param_len);
 217                         sprintf(param_str, "%s", sn);
 218                         g_free(sn);
 219                         break;
 220
 221                 case LLCP_PARAM_OPT:
 222                         sprintf(param_str, "0x%X", param[2] & 0x03);
 223                         break;
 224
 225                 case LLCP_PARAM_SDREQ:
 226                         tid = param[2];
 227                         sn = g_strndup((gchar *)param + 3, param_len - 1);
 228                         sprintf(param_str, "TID:%d, SN:%s", tid, sn);
 229                         g_free(sn);
 230                         break;
 231
 232                 case LLCP_PARAM_SDRES:
 233                         sprintf(param_str, "TID:%d, SAP:%d", param[2], param[3] & 0x3F);
 234                         break;
 235                 }
 236
 237                 llcp_printf_msg("%s: %s", llcp_get_param_str(param[0]),
 238                                 param_str);
 239
 240                 offset += 2 + param_len;
 241         }
 242 }
 243
 244 static int llcp_decode_packet(guint8 *data, guint32 data_len,
 245                               struct sniffer_packet *packet)
 246 {
 247         if (data_len < RAW_LLCP_HEADERS_SIZE)
 248                 return -EINVAL;
 249
 250         memset(packet, 0, sizeof(struct sniffer_packet));
 251
 252         /* LLCP raw socket header */
 253         packet->adapter_idx = data[0];
 254         packet->direction = data[1] & 0x01;
 255
 256         /* LLCP header */
 257         if (packet->direction == NFC_LLCP_DIRECTION_TX) {
 258                 packet->llcp.remote_sap = (data[2] & 0xFC) >> 2;
 259                 packet->llcp.local_sap = data[3] & 0x3F;
 260         } else {
 261                 packet->llcp.remote_sap = data[3] & 0x3F;
 262                 packet->llcp.local_sap = (data[2] & 0xFC) >> 2;
 263         }
 264
 265         packet->llcp.ptype = ((data[2] & 0x03) << 2) | ((data[3] & 0xC0) >> 6);
 266
 267         if (packet->llcp.ptype >= ARRAY_SIZE(llcp_ptype_str))
 268                 return -EINVAL;
 269
 270         packet->llcp.data = data + RAW_LLCP_HEADERS_SIZE;
 271         packet->llcp.data_len = data_len - RAW_LLCP_HEADERS_SIZE;
 272
 273         /* Sequence field */
 274         if (packet->llcp.ptype >= LLCP_PTYPE_I) {
 275                 if (packet->llcp.data_len == 0)
 276                         return -EINVAL;
 277
 278                 packet->llcp.send_seq = ((packet->llcp.data[0] & 0xF0) >> 4);
 279                 packet->llcp.recv_seq = packet->llcp.data[0] & 0x0F;
 280
 281                 packet->llcp.data++;
 282                 packet->llcp.data_len--;
 283         }
 284
 285         return 0;
 286 }
 287
 288 static void llcp_print_sequence(struct sniffer_packet *packet)
 289 {
 290         llcp_printf_msg("N(S):%d N(R):%d",
 291                         packet->llcp.send_seq, packet->llcp.recv_seq);
 292 }
 293
 294 static int llcp_print_agf(struct sniffer_packet *packet,
 295                           struct timeval *timestamp)
 296 {
 297         guint8 *pdu;
 298         gsize pdu_size;
 299         gsize size;
 300         guint16 offset;
 301         guint16 count;
 302         int err;
 303
 304         if (packet->llcp.data_len < 2) {
 305                 llcp_printf_error("Error parsing AGF PDU");
 306                 return -EINVAL;
 307         }
 308
 309         printf("\n");
 310
 311         pdu = NULL;
 312         pdu_size = 0;
 313         offset = 0;
 314         count = 0;
 315
 316         while (offset < packet->llcp.data_len - 2) {
 317                 size = (packet->llcp.data[offset] << 8) |
 318                         packet->llcp.data[offset + 1];
 319
 320                 offset += 2;
 321
 322                 if (size == 0 || offset + size > packet->llcp.data_len) {
 323                         llcp_printf_error("Error parsing AGF PDU");
 324                         err = -EINVAL;
 325                         goto exit;
 326                 }
 327
 328                 if (size + NFC_LLCP_RAW_HEADER_SIZE > pdu_size) {
 329                         pdu_size = size + NFC_LLCP_RAW_HEADER_SIZE;
 330                         pdu = g_realloc(pdu, pdu_size);
 331
 332                         pdu[0] = packet->adapter_idx;
 333                         pdu[1] = packet->direction;
 334                 }
 335
 336                 memcpy(pdu + NFC_LLCP_RAW_HEADER_SIZE,
 337                         packet->llcp.data + offset, size);
 338
 339                 llcp_printf_msg("-- AGF LLC PDU %02u:", count++);
 340
 341                 llcp_print_pdu(pdu, size + NFC_LLCP_RAW_HEADER_SIZE, timestamp);
 342
 343                 offset += size;
 344         }
 345
 346         llcp_printf_msg("-- End of AGF LLC PDUs");
 347
 348         err = 0;
 349 exit:
 350         g_free(pdu);
 351
 352         return err;
 353 }
 354
 355 static int llcp_print_dm(struct sniffer_packet *packet)
 356 {
 357         gchar *reason;
 358
 359         if (packet->llcp.data_len != 1)
 360                 return -EINVAL;
 361
 362         switch (packet->llcp.data[0]) {
 363         case LLCP_DM_NORMAL:
 364         default:
 365                 reason = "Normal disconnect";
 366                 break;
 367
 368         case LLCP_DM_NO_ACTIVE_CONN:
 369                 reason =
 370                       "No active connection for connection-oriented PDU at SAP";
 371                 break;
 372
 373         case LLCP_DM_NOT_BOUND:
 374                 reason = "No service bound to target SAP";
 375                 break;
 376
 377         case LLCP_DM_REJECTED:
 378                 reason = "CONNECT PDU rejected by service layer";
 379                 break;
 380
 381         case LLCP_DM_PERM_SAP_FAILURE:
 382                 reason = "Permanent failure for target SAP";
 383                 break;
 384
 385         case LLCP_DM_PERM_ALL_SAP_FAILURE:
 386                 reason = "Permanent failure for any target SAP";
 387                 break;
 388
 389         case LLCP_DM_TMP_SAP_FAILURE:
 390                 reason = "Temporary failure for target SAP";
 391                 break;
 392
 393         case LLCP_DM_TMP_ALL_SAP_FAILURE:
 394                 reason = "Temporary failure for any target SAP";
 395                 break;
 396         }
 397
 398         llcp_printf_msg("Reason: %d (%s)", packet->llcp.data[0], reason);
 399
 400         return 0;
 401 }
 402
 403 static int llcp_print_i(struct sniffer_packet *packet)
 404 {
 405         llcp_print_sequence(packet);
 406
 407         if (packet->llcp.local_sap == opts.snep_sap ||
 408             packet->llcp.remote_sap == opts.snep_sap) {
 409                 int err;
 410
 411                 err = snep_print_pdu(packet);
 412                 if (err != 0)
 413                         llcp_printf_error("Error decoding SNEP frame");
 414
 415                 return err;
 416         }
 417
 418         sniffer_print_hexdump(stdout, packet->llcp.data,  packet->llcp.data_len,
 419                                 LLCP_MSG_INDENT, TRUE);
 420
 421         return 0;
 422 }
 423
 424 static int llcp_print_frmr(struct sniffer_packet *packet)
 425 {
 426         guint8 info, ptype;
 427
 428         if (packet->llcp.data_len != 4)
 429                 return -EINVAL;
 430
 431         info = packet->llcp.data[0];
 432         ptype = info & 0x0F;
 433         if (ptype >= ARRAY_SIZE(llcp_ptype_str))
 434                 ptype = ARRAY_SIZE(llcp_ptype_str) - 1;
 435
 436         llcp_printf_msg("W:%d I:%d R:%d S:%d PTYPE:%s SEQ: %d V(S):"
 437                         " %d V(R): %d V(SA): %d V(RA): %d",
 438                         (info & 0x80) >> 7, (info & 0x40) >> 6,
 439                         (info & 0x20) >> 5, (info & 0x10) >> 4,
 440                         llcp_ptype_short_str[ptype], packet->llcp.data[1],
 441                         (packet->llcp.data[2] & 0xF0) >> 4,
 442                         packet->llcp.data[2] & 0x0F,
 443                         (packet->llcp.data[3] & 0xF0) >> 4,
 444                         packet->llcp.data[3] & 0x0F);
 445
 446         return 0;
 447 }
 448
 449 int llcp_print_pdu(guint8 *data, guint32 data_len, struct timeval *timestamp)
 450 {
 451         struct timeval msg_timestamp;
 452         struct sniffer_packet packet;
 453         gchar *direction_str, time_str[32];
 454         gchar *direction_color;
 455         int err;
 456
 457         if (timestamp == NULL)
 458                 return -EINVAL;
 459
 460         if (!timerisset(&start_timestamp))
 461                 start_timestamp = *timestamp;
 462
 463         err = llcp_decode_packet(data, data_len, &packet);
 464         if (err)
 465                 goto exit;
 466
 467         if (!opts.dump_symm && packet.llcp.ptype == LLCP_PTYPE_SYMM)
 468                 return 0;
 469
 470         if (packet.direction == NFC_LLCP_DIRECTION_RX) {
 471                 direction_str = ">>";
 472                 direction_color = COLOR_RED;
 473         } else {
 474                 direction_str = "<<";
 475                 direction_color = COLOR_GREEN;
 476         }
 477
 478         if (opts.show_timestamp != SNIFFER_SHOW_TIMESTAMP_NONE) {
 479                 char prefix = ' ';
 480
 481                 if (opts.show_timestamp == SNIFFER_SHOW_TIMESTAMP_ABS) {
 482                         msg_timestamp = *timestamp;
 483                 } else {
 484                         timersub(timestamp, &start_timestamp, &msg_timestamp);
 485                         prefix = '+';
 486                 }
 487
 488                 sprintf(time_str,  "%c%lu.%06lus", prefix, msg_timestamp.tv_sec,
 489                                                         msg_timestamp.tv_usec);
 490         }
 491
 492         llcp_printf_header(direction_str, direction_color,
 493                                 " nfc%d: local:0x%02x remote:0x%02x %s",
 494                                 packet.adapter_idx, packet.llcp.local_sap,
 495                                 packet.llcp.remote_sap, time_str);
 496
 497         llcp_printf_msg("%s", llcp_ptype_str[packet.llcp.ptype]);
 498
 499         switch (packet.llcp.ptype) {
 500         case LLCP_PTYPE_AGF:
 501                 llcp_print_agf(&packet, timestamp);
 502                 break;
 503
 504         case LLCP_PTYPE_I:
 505                 llcp_print_i(&packet);
 506                 break;
 507
 508         case LLCP_PTYPE_RR:
 509         case LLCP_PTYPE_RNR:
 510                 llcp_print_sequence(&packet);
 511                 break;
 512
 513         case LLCP_PTYPE_PAX:
 514         case LLCP_PTYPE_CONNECT:
 515         case LLCP_PTYPE_CC:
 516         case LLCP_PTYPE_SNL:
 517                 llcp_print_params(&packet);
 518                 break;
 519
 520         case LLCP_PTYPE_DM:
 521                 llcp_print_dm(&packet);
 522                 break;
 523
 524         case LLCP_PTYPE_FRMR:
 525                 llcp_print_frmr(&packet);
 526                 break;
 527
 528         default:
 529                 sniffer_print_hexdump(stdout, packet.llcp.data,
 530                                         packet.llcp.data_len,
 531                                         LLCP_MSG_INDENT, TRUE);
 532                 break;
 533         }
 534
 535         printf("\n");
 536
 537         err = 0;
 538
 539 exit:
 540         return err;
 541 }
 542
 543 void llcp_decode_cleanup(void)
 544 {
 545         timerclear(&start_timestamp);
 546
 547         snep_decode_cleanup();
 548 }
 549
 550 int llcp_decode_init(void)
 551 {
 552         int err;
 553
 554         timerclear(&start_timestamp);
 555
 556         err = snep_decode_init();
 557
 558         return err;
 559 }