nfc-relay-picc.c 16.2 KB
Newer Older
1
/*-
2
 * Free/Libre Near Field Communication (NFC) library
3
 *
4 5 6 7 8 9 10
 * Libnfc historical contributors:
 * Copyright (C) 2009      Roel Verdult
 * Copyright (C) 2009-2013 Romuald Conty
 * Copyright (C) 2010-2012 Romain Tartière
 * Copyright (C) 2010-2013 Philippe Teuwen
 * Copyright (C) 2012-2013 Ludovic Rousseau
 * Additional contributors of this file:
11
 *
12 13 14
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *  1) Redistributions of source code must retain the above copyright notice,
15
 *  this list of conditions and the following disclaimer.
16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
 *  2 )Redistributions in binary form must reproduce the above copyright
 *  notice, this list of conditions and the following disclaimer in the
 *  documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
31
 *
32
 * Note that this license only applies on the examples, NFC library itself is under LGPL
33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50
 *
 */

/**
 * @file nfc-relay-picc.c
 * @brief Relay example using two PN532 devices.
 */

// Notes & differences with nfc-relay:
// - This example only works with PN532 because it relies on
//   its internal handling of ISO14443-4 specificities.
// - Thanks to this internal handling & injection of WTX frames,
//   this example works on readers very strict on timing

#ifdef HAVE_CONFIG_H
#  include "config.h"
#endif /* HAVE_CONFIG_H */

51
#include <inttypes.h>
52 53 54 55 56 57
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <signal.h>

58 59
#include <unistd.h>

60 61 62 63 64 65 66
#include <nfc/nfc.h>

#include "nfc-utils.h"

#define MAX_FRAME_LEN 264
#define MAX_DEVICE_COUNT 2

67
static uint8_t abtCapdu[MAX_FRAME_LEN];
68
static size_t szCapduLen;
69
static uint8_t abtRapdu[MAX_FRAME_LEN];
70
static size_t szRapduLen;
71 72
static nfc_device *pndInitiator;
static nfc_device *pndTarget;
73 74 75 76
static bool quitting = false;
static bool quiet_output = false;
static bool initiator_only_mode = false;
static bool target_only_mode = false;
77
static bool swap_devices = false;
78
static int waiting_time = 0;
79 80
FILE *fd3;
FILE *fd4;
81

82
static void
83
intr_hdlr(int sig)
84
{
85
  (void) sig;
86 87
  printf("\nQuitting...\n");
  printf("Please send a last command to the emulator to quit properly.\n");
88 89 90 91
  quitting = true;
  return;
}

92
static void
93
print_usage(char *argv[])
94
{
95 96 97 98 99 100 101
  printf("Usage: %s [OPTIONS]\n", argv[0]);
  printf("Options:\n");
  printf("\t-h\tHelp. Print this message.\n");
  printf("\t-q\tQuiet mode. Suppress printing of relayed data (improves timing).\n");
  printf("\t-t\tTarget mode only (the one on reader side). Data expected from FD3 to FD4.\n");
  printf("\t-i\tInitiator mode only (the one on tag side). Data expected from FD3 to FD4.\n");
  printf("\t-n N\tAdds a waiting time of N seconds (integer) in the relay to mimic long distance.\n");
102 103
}

104
static int print_hex_fd4(const uint8_t *pbtData, const size_t szBytes, const char *pchPrefix)
105 106 107
{
  size_t  szPos;
  if (szBytes > MAX_FRAME_LEN) {
108
    return -1;
109
  }
110
  if (fprintf(fd4, "#%s %04" PRIxPTR ": ", pchPrefix, szBytes) < 0) {
111
    return -1;
112 113 114
  }

  for (szPos = 0; szPos < szBytes; szPos++) {
115
    if (fprintf(fd4, "%02x ", pbtData[szPos]) < 0) {
116
      return -1;
117 118
    }
  }
119
  if (fprintf(fd4, "\n") < 0) {
120
    return -1;
121 122
  }
  fflush(fd4);
123
  return 0;
124 125
}

126
static int scan_hex_fd3(uint8_t *pbtData, size_t *pszBytes, const char *pchPrefix)
127 128 129 130 131 132 133
{
  size_t  szPos;
  unsigned int uiBytes;
  unsigned int uiData;
  char pchScan[256];
  int c;
  // Look for our next sync marker
134
  while ((c = fgetc(fd3)) != '#') {
135
    if (c == EOF) {
136
      return -1;
137 138 139
    }
  }
  strncpy(pchScan, pchPrefix, 250);
140
  pchScan[sizeof(pchScan) - 1] = '\0';
141
  strcat(pchScan, " %04x:");
142
  if (fscanf(fd3, pchScan, &uiBytes) < 1) {
143
    return -1;
144
  }
145
  *pszBytes = uiBytes;
146
  if (*pszBytes > MAX_FRAME_LEN) {
147
    return -1;
148 149
  }
  for (szPos = 0; szPos < *pszBytes; szPos++) {
150
    if (fscanf(fd3, "%02x", &uiData) < 1) {
151
      return -1;
152
    }
153
    pbtData[szPos] = uiData;
154
  }
155
  return 0;
156 157 158
}

int
159
main(int argc, char *argv[])
160 161
{
  int     arg;
162
  const char *acLibnfcVersion = nfc_version();
163
  nfc_target ntRealTarget;
164 165 166

  // Get commandline options
  for (arg = 1; arg < argc; arg++) {
167 168
    if (0 == strcmp(argv[arg], "-h")) {
      print_usage(argv);
169
      exit(EXIT_SUCCESS);
170
    } else if (0 == strcmp(argv[arg], "-q")) {
171
      quiet_output = true;
172 173
    } else if (0 == strcmp(argv[arg], "-t")) {
      printf("INFO: %s\n", "Target mode only.");
174 175
      initiator_only_mode = false;
      target_only_mode = true;
176 177
    } else if (0 == strcmp(argv[arg], "-i")) {
      printf("INFO: %s\n", "Initiator mode only.");
178 179
      initiator_only_mode = true;
      target_only_mode = false;
180 181 182
    } else if (0 == strcmp(argv[arg], "-s")) {
      printf("INFO: %s\n", "Swapping devices.");
      swap_devices = true;
183
    } else if (0 == strcmp(argv[arg], "-n")) {
184
      if (++arg == argc || (sscanf(argv[arg], "%10i", &waiting_time) < 1)) {
185 186
        ERR("Missing or wrong waiting time value: %s.", argv[arg]);
        print_usage(argv);
187
        exit(EXIT_FAILURE);
188
      }
189
      printf("Waiting time: %i secs.\n", waiting_time);
190
    } else {
191 192
      ERR("%s is not supported option.", argv[arg]);
      print_usage(argv);
193
      exit(EXIT_FAILURE);
194 195 196 197
    }
  }

  // Display libnfc version
198
  printf("%s uses libnfc %s\n", argv[0], acLibnfcVersion);
199 200

#ifdef WIN32
201
  signal(SIGINT, (void (__cdecl *)(int)) intr_hdlr);
202
#else
203
  signal(SIGINT, intr_hdlr);
204 205
#endif

206 207
  nfc_context *context;
  nfc_init(&context);
208 209 210 211
  if (context == NULL) {
    ERR("Unable to init libnfc (malloc)");
    exit(EXIT_FAILURE);
  }
212

213
  nfc_connstring connstrings[MAX_DEVICE_COUNT];
214
  // List available devices
215
  size_t szFound = nfc_list_devices(context, connstrings, MAX_DEVICE_COUNT);
216 217 218

  if (initiator_only_mode || target_only_mode) {
    if (szFound < 1) {
219
      ERR("No device found");
220 221
      nfc_exit(context);
      exit(EXIT_FAILURE);
222
    }
223 224
    if ((fd3 = fdopen(3, "r")) == NULL) {
      ERR("Could not open file descriptor 3");
225 226
      nfc_exit(context);
      exit(EXIT_FAILURE);
227 228 229
    }
    if ((fd4 = fdopen(4, "r")) == NULL) {
      ERR("Could not open file descriptor 4");
230 231
      nfc_exit(context);
      exit(EXIT_FAILURE);
232
    }
233
  } else {
234
    if (szFound < 2) {
235
      ERR("%" PRIdPTR " device found but two opened devices are needed to relay NFC.", szFound);
236 237
      nfc_exit(context);
      exit(EXIT_FAILURE);
238 239 240 241 242 243 244
    }
  }

  if (!target_only_mode) {
    // Try to open the NFC reader used as initiator
    // Little hack to allow using initiator no matter if
    // there is already a target used locally or not on the same machine:
245
    // if there is more than one readers opened we open the second reader
246
    // (we hope they're always detected in the same order)
247
    if ((szFound == 1) || swap_devices) {
248
      pndInitiator = nfc_open(context, connstrings[0]);
249
    } else {
250
      pndInitiator = nfc_open(context, connstrings[1]);
251 252
    }

253
    if (pndInitiator == NULL) {
254
      printf("Error opening NFC reader\n");
255
      nfc_exit(context);
256 257 258
      exit(EXIT_FAILURE);
    }

259
    printf("NFC reader device: %s opened\n", nfc_device_get_name(pndInitiator));
260

261 262 263
    if (nfc_initiator_init(pndInitiator) < 0) {
      printf("Error: fail initializing initiator\n");
      nfc_close(pndInitiator);
264
      nfc_exit(context);
265
      exit(EXIT_FAILURE);
266 267
    }

268
    // Try to find a ISO 14443-4A tag
269
    nfc_modulation nm = {
270 271 272
      .nmt = NMT_ISO14443A,
      .nbr = NBR_106,
    };
273
    if (nfc_initiator_select_passive_target(pndInitiator, nm, NULL, 0, &ntRealTarget) <= 0) {
274 275
      printf("Error: no tag was found\n");
      nfc_close(pndInitiator);
276
      nfc_exit(context);
277
      exit(EXIT_FAILURE);
278 279 280
    }

    printf("Found tag:\n");
281
    print_nfc_target(&ntRealTarget, false);
282
    if (initiator_only_mode) {
283
      if (print_hex_fd4(ntRealTarget.nti.nai.abtUid, ntRealTarget.nti.nai.szUidLen, "UID") < 0) {
284 285
        fprintf(stderr, "Error while printing UID to FD4\n");
        nfc_close(pndInitiator);
286
        nfc_exit(context);
287 288
        exit(EXIT_FAILURE);
      }
289
      if (print_hex_fd4(ntRealTarget.nti.nai.abtAtqa, 2, "ATQA") < 0) {
290 291
        fprintf(stderr, "Error while printing ATQA to FD4\n");
        nfc_close(pndInitiator);
292
        nfc_exit(context);
293 294
        exit(EXIT_FAILURE);
      }
295
      if (print_hex_fd4(&(ntRealTarget.nti.nai.btSak), 1, "SAK") < 0) {
296 297
        fprintf(stderr, "Error while printing SAK to FD4\n");
        nfc_close(pndInitiator);
298
        nfc_exit(context);
299 300
        exit(EXIT_FAILURE);
      }
301
      if (print_hex_fd4(ntRealTarget.nti.nai.abtAts, ntRealTarget.nti.nai.szAtsLen, "ATS") < 0) {
302 303
        fprintf(stderr, "Error while printing ATS to FD4\n");
        nfc_close(pndInitiator);
304
        nfc_exit(context);
305 306
        exit(EXIT_FAILURE);
      }
307
    }
308 309
  }
  if (initiator_only_mode) {
310
    printf("Hint: tag <---> *INITIATOR* (relay) <-FD3/FD4-> target (relay) <---> original reader\n\n");
311
  } else if (target_only_mode) {
312
    printf("Hint: tag <---> initiator (relay) <-FD3/FD4-> *TARGET* (relay) <---> original reader\n\n");
313
  } else {
314
    printf("Hint: tag <---> initiator (relay) <---> target (relay) <---> original reader\n\n");
315 316
  }
  if (!initiator_only_mode) {
317
    nfc_target ntEmulatedTarget = {
318 319 320 321
      .nm = {
        .nmt = NMT_ISO14443A,
        .nbr = NBR_106,
      },
322 323 324
    };
    if (target_only_mode) {
      size_t foo;
325
      if (scan_hex_fd3(ntEmulatedTarget.nti.nai.abtUid, &(ntEmulatedTarget.nti.nai.szUidLen), "UID") < 0) {
326 327
        fprintf(stderr, "Error while scanning UID from FD3\n");
        nfc_close(pndInitiator);
328
        nfc_exit(context);
329 330
        exit(EXIT_FAILURE);
      }
331
      if (scan_hex_fd3(ntEmulatedTarget.nti.nai.abtAtqa, &foo, "ATQA") < 0) {
332 333
        fprintf(stderr, "Error while scanning ATQA from FD3\n");
        nfc_close(pndInitiator);
334
        nfc_exit(context);
335 336
        exit(EXIT_FAILURE);
      }
337
      if (scan_hex_fd3(&(ntEmulatedTarget.nti.nai.btSak), &foo, "SAK") < 0) {
338 339
        fprintf(stderr, "Error while scanning SAK from FD3\n");
        nfc_close(pndInitiator);
340
        nfc_exit(context);
341 342
        exit(EXIT_FAILURE);
      }
343
      if (scan_hex_fd3(ntEmulatedTarget.nti.nai.abtAts, &(ntEmulatedTarget.nti.nai.szAtsLen), "ATS") < 0) {
344 345
        fprintf(stderr, "Error while scanning ATS from FD3\n");
        nfc_close(pndInitiator);
346
        nfc_exit(context);
347 348 349 350 351 352 353
        exit(EXIT_FAILURE);
      }
    } else {
      ntEmulatedTarget.nti = ntRealTarget.nti;
    }
    // We can only emulate a short UID, so fix length & ATQA bit:
    ntEmulatedTarget.nti.nai.szUidLen = 4;
354
    ntEmulatedTarget.nti.nai.abtAtqa[1] &= (0xFF - 0x40);
355 356 357 358 359 360 361 362 363 364
    // First byte of UID is always automatically replaced by 0x08 in this mode anyway
    ntEmulatedTarget.nti.nai.abtUid[0] = 0x08;
    // ATS is always automatically replaced by PN532, we've no control on it:
    // ATS = (05) 75 33 92 03
    //       (TL) T0 TA TB TC
    //             |  |  |  +-- CID supported, NAD supported
    //             |  |  +----- FWI=9 SFGI=2 => FWT=154ms, SFGT=1.21ms
    //             |  +-------- DR=2,4 DS=2,4 => supports 106, 212 & 424bps in both directions
    //             +----------- TA,TB,TC, FSCI=5 => FSC=64
    // It seems hazardous to tell we support NAD if the tag doesn't support NAD but I don't know how to disable it
365 366 367
    // PC/SC pseudo-ATR = 3B 80 80 01 01 if there is no historical bytes

    // Creates ATS and copy max 48 bytes of Tk:
368
    uint8_t *pbtTk;
369
    size_t szTk;
370
    pbtTk = iso14443a_locate_historical_bytes(ntEmulatedTarget.nti.nai.abtAts, ntEmulatedTarget.nti.nai.szAtsLen, &szTk);
371
    szTk = (szTk > 48) ? 48 : szTk;
372
    uint8_t pbtTkt[48];
373
    memcpy(pbtTkt, pbtTk, szTk);
374 375 376 377
    ntEmulatedTarget.nti.nai.abtAts[0] = 0x75;
    ntEmulatedTarget.nti.nai.abtAts[1] = 0x33;
    ntEmulatedTarget.nti.nai.abtAts[2] = 0x92;
    ntEmulatedTarget.nti.nai.abtAts[3] = 0x03;
378 379
    ntEmulatedTarget.nti.nai.szAtsLen = 4 + szTk;
    memcpy(&(ntEmulatedTarget.nti.nai.abtAts[4]), pbtTkt, szTk);
380 381

    printf("We will emulate:\n");
382
    print_nfc_target(&ntEmulatedTarget, false);
383

384
    // Try to open the NFC emulator device
385
    if (swap_devices) {
386
      pndTarget = nfc_open(context, connstrings[1]);
387
    } else {
388
      pndTarget = nfc_open(context, connstrings[0]);
389
    }
390
    if (pndTarget == NULL) {
391
      printf("Error opening NFC emulator device\n");
392
      if (!target_only_mode) {
393
        nfc_close(pndInitiator);
394
      }
395
      nfc_exit(context);
396
      exit(EXIT_FAILURE);
397 398
    }

399
    printf("NFC emulator device: %s opened\n", nfc_device_get_name(pndTarget));
400
    if (nfc_target_init(pndTarget, &ntEmulatedTarget, abtCapdu, sizeof(abtCapdu), 0) < 0) {
401
      ERR("%s", "Initialization of NFC emulator failed");
402
      if (!target_only_mode) {
403
        nfc_close(pndInitiator);
404
      }
405
      nfc_close(pndTarget);
406
      nfc_exit(context);
407 408
      exit(EXIT_FAILURE);
    }
409
    printf("%s\n", "Done, relaying frames now!");
410 411 412 413
  }

  while (!quitting) {
    bool ret;
414
    int res = 0;
415 416
    if (!initiator_only_mode) {
      // Receive external reader command through target
417 418
      if ((res = nfc_target_receive_bytes(pndTarget, abtCapdu, sizeof(abtCapdu), 0)) < 0) {
        nfc_perror(pndTarget, "nfc_target_receive_bytes");
419
        if (!target_only_mode) {
420
          nfc_close(pndInitiator);
421
        }
422
        nfc_close(pndTarget);
423
        nfc_exit(context);
424 425
        exit(EXIT_FAILURE);
      }
426
      szCapduLen = (size_t) res;
427
      if (target_only_mode) {
428
        if (print_hex_fd4(abtCapdu, szCapduLen, "C-APDU") < 0) {
429 430
          fprintf(stderr, "Error while printing C-APDU to FD4\n");
          nfc_close(pndTarget);
431
          nfc_exit(context);
432 433 434 435
          exit(EXIT_FAILURE);
        }
      }
    } else {
436
      if (scan_hex_fd3(abtCapdu, &szCapduLen, "C-APDU") < 0) {
437 438
        fprintf(stderr, "Error while scanning C-APDU from FD3\n");
        nfc_close(pndInitiator);
439
        nfc_exit(context);
440 441 442 443 444
        exit(EXIT_FAILURE);
      }
    }
    // Show transmitted response
    if (!quiet_output) {
445 446
      printf("Forwarding C-APDU: ");
      print_hex(abtCapdu, szCapduLen);
447 448 449 450
    }

    if (!target_only_mode) {
      // Forward the frame to the original tag
451
      if ((res = nfc_initiator_transceive_bytes(pndInitiator, abtCapdu, szCapduLen, abtRapdu, sizeof(abtRapdu), -1)) < 0) {
452 453
        ret = false;
      } else {
454
        szRapduLen = (size_t) res;
455 456
        ret = true;
      }
457
    } else {
458
      if (scan_hex_fd3(abtRapdu, &szRapduLen, "R-APDU") < 0) {
459 460
        fprintf(stderr, "Error while scanning R-APDU from FD3\n");
        nfc_close(pndTarget);
461
        nfc_exit(context);
462 463 464 465 466 467 468 469
        exit(EXIT_FAILURE);
      }
      ret = true;
    }
    if (ret) {
      // Redirect the answer back to the external reader
      if (waiting_time > 0) {
        if (!quiet_output) {
470
          printf("Waiting %is to simulate longer relay...\n", waiting_time);
471
        }
472
        sleep(waiting_time);
473 474 475
      }
      // Show transmitted response
      if (!quiet_output) {
476 477
        printf("Forwarding R-APDU: ");
        print_hex(abtRapdu, szRapduLen);
478 479 480
      }
      if (!initiator_only_mode) {
        // Transmit the response bytes
481
        if (nfc_target_send_bytes(pndTarget, abtRapdu, szRapduLen, 0) < 0) {
482
          nfc_perror(pndTarget, "nfc_target_send_bytes");
483
          if (!target_only_mode) {
484
            nfc_close(pndInitiator);
485 486
          }
          if (!initiator_only_mode) {
487
            nfc_close(pndTarget);
488
            nfc_exit(context);
489
          }
490
          nfc_exit(context);
491 492 493
          exit(EXIT_FAILURE);
        }
      } else {
494
        if (print_hex_fd4(abtRapdu, szRapduLen, "R-APDU") < 0) {
495 496
          fprintf(stderr, "Error while printing R-APDU to FD4\n");
          nfc_close(pndInitiator);
497
          nfc_exit(context);
498 499 500 501 502 503 504
          exit(EXIT_FAILURE);
        }
      }
    }
  }

  if (!target_only_mode) {
505
    nfc_close(pndInitiator);
506 507
  }
  if (!initiator_only_mode) {
508
    nfc_close(pndTarget);
509
  }
510
  nfc_exit(context);
511
  exit(EXIT_SUCCESS);
512 513
}