armsrc/Standalone/lf_nedap_sim.c

   1 //-----------------------------------------------------------------------------
   2 // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details.
   3 //
   4 // This program is free software: you can redistribute it and/or modify
   5 // it under the terms of the GNU General Public License as published by
   6 // the Free Software Foundation, either version 3 of the License, or
   7 // (at your option) any later version.
   8 //
   9 // This program is distributed in the hope that it will be useful,
  10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 // GNU General Public License for more details.
  13 //
  14 // See LICENSE.txt for the text of the license.
  15 //-----------------------------------------------------------------------------
  16 // This simple mode encode, then emulate a Nedap identificator until button pressed
  17 // lots of code from client side, cmdlfnedap, util, etc.
  18 //-----------------------------------------------------------------------------
  19 #include "standalone.h" // standalone definitions
  20 #include "proxmark3_arm.h"
  21 #include "appmain.h"
  22 #include "fpgaloader.h"
  23 #include "lfops.h"
  24 #include "util.h"
  25 #include "dbprint.h"
  26 #include "string.h"
  27 #include "BigBuf.h"
  28 #include "crc16.h"
  29
  30 #define MODULE_LONG_NAME    "LF Nedap simple simulator"
  31
  32 typedef struct _NEDAP_TAG {
  33     uint8_t subType;
  34     uint16_t customerCode;
  35     uint32_t id;
  36
  37     uint8_t bIsLong;
  38 } NEDAP_TAG, *PNEDAP_TAG;
  39
  40 const NEDAP_TAG Tag = {.subType = 0x5, .customerCode = 0x123, .id = 42424, .bIsLong = 1};
  41
  42 static int NedapPrepareBigBuffer(const NEDAP_TAG *pTag);
  43 static void biphaseSimBitInverted(uint8_t c, int *n, uint8_t *phase);
  44 static void NedapGen(uint8_t subType, uint16_t customerCode, uint32_t id, bool isLong, uint8_t *data);
  45 static uint8_t isEven_64_63(const uint8_t *data);
  46 static inline uint32_t bitcount32(uint32_t a);
  47 static void bytes_to_bytebits(const void *src, const size_t srclen, void *dest);
  48
  49 void ModInfo(void) {
  50     DbpString("  " MODULE_LONG_NAME);
  51 }
  52
  53 void RunMod(void) {
  54     int n;
  55
  56     StandAloneMode();
  57
  58     Dbprintf("[=] " MODULE_LONG_NAME " -- started");
  59     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  60     Dbprintf("[=] NEDAP (%s) - ID: " _GREEN_("%05u") " subtype: " _GREEN_("%1u") " customer code: " _GREEN_("%u / 0x%03X"), Tag.bIsLong ? "128b" : "64b", Tag.id, Tag.subType, Tag.customerCode, Tag.customerCode);
  61
  62     n = NedapPrepareBigBuffer(&Tag);
  63     do {
  64         WDT_HIT();
  65
  66         if (data_available())
  67             break;
  68
  69         SimulateTagLowFrequency(n, 0, true);
  70
  71     } while (BUTTON_HELD(1000) == BUTTON_NO_CLICK);
  72
  73     Dbprintf("[=] " MODULE_LONG_NAME " -- exiting");
  74
  75     LEDsoff();
  76 }
  77
  78 static int NedapPrepareBigBuffer(const NEDAP_TAG *pTag) {
  79     int ret = 0;
  80     uint8_t data[16], bitStream[sizeof(data) * 8], phase = 0;
  81     uint16_t i, size = pTag->bIsLong ? sizeof(data) : (sizeof(data) / 2);
  82
  83     NedapGen(pTag->subType, pTag->customerCode, pTag->id, pTag->bIsLong, data);
  84     bytes_to_bytebits(data, size, bitStream);
  85     size <<= 3;
  86
  87     for (i = 0; i < size; i++) {
  88         biphaseSimBitInverted(!bitStream[i], &ret, &phase);
  89     }
  90     if (phase == 1) { //run a second set inverted to keep phase in check
  91         for (i = 0; i < size; i++) {
  92             biphaseSimBitInverted(!bitStream[i], &ret, &phase);
  93         }
  94     }
  95
  96     return ret;
  97 }
  98
  99 static void biphaseSimBitInverted(uint8_t c, int *n, uint8_t *phase) {
 100     uint8_t *dest = BigBuf_get_addr();
 101
 102     if (c) {
 103         memset(dest + (*n), c ^ 1 ^ *phase, 32);
 104         memset(dest + (*n) + 32, c ^ *phase, 32);
 105     } else {
 106         memset(dest + (*n), c ^ *phase, 64);
 107         *phase ^= 1;
 108     }
 109     *n += 64;
 110 }
 111
 112 #define FIXED_71    0x71
 113 #define FIXED_40    0x40
 114 #define UNKNOWN_A   0x00
 115 #define UNKNOWN_B   0x00
 116 static const uint8_t translateTable[10] = {8, 2, 1, 12, 4, 5, 10, 13, 0, 9};
 117 static void NedapGen(uint8_t subType, uint16_t customerCode, uint32_t id, bool isLong, uint8_t *data) { // 8 or 16
 118     uint8_t buffer[7];
 119
 120     uint8_t r1 = (uint8_t)(id / 10000);
 121     uint8_t r2 = (uint8_t)((id % 10000) / 1000);
 122     uint8_t r3 = (uint8_t)((id % 1000) / 100);
 123     uint8_t r4 = (uint8_t)((id % 100) / 10);
 124     uint8_t r5 = (uint8_t)(id % 10);
 125
 126     // first part
 127     uint8_t idxC1 = r1;
 128     uint8_t idxC2 = (idxC1 + 1 + r2) % 10;
 129     uint8_t idxC3 = (idxC2 + 1 + r3) % 10;
 130     uint8_t idxC4 = (idxC3 + 1 + r4) % 10;
 131     uint8_t idxC5 = (idxC4 + 1 + r5) % 10;
 132
 133     buffer[0] = 0xc0 | (subType & 0x0F);
 134     buffer[1] = (customerCode & 0x0FF0) >> 4;
 135     buffer[2] = ((customerCode & 0x000F) << 4) | translateTable[idxC1];
 136     buffer[3] = (translateTable[idxC2] << 4) | translateTable[idxC3];
 137     buffer[4] = (translateTable[idxC4] << 4) | translateTable[idxC5];
 138
 139     // checksum
 140     init_table(CRC_XMODEM);
 141     uint16_t checksum = crc16_xmodem(buffer, 5);
 142
 143     buffer[6] = ((checksum & 0x000F) << 4) | (buffer[4] & 0x0F);
 144     buffer[5] = (checksum & 0x00F0) | ((buffer[4] & 0xF0) >> 4);
 145     buffer[4] = ((checksum & 0x0F00) >> 4) | (buffer[3] & 0x0F);
 146     buffer[3] = ((checksum & 0xF000) >> 8) | ((buffer[3] & 0xF0) >> 4);
 147
 148     // carry calc
 149     uint8_t carry = 0;
 150     for (uint8_t i = 0; i < sizeof(buffer); i++) {
 151         uint8_t tmp = buffer[sizeof(buffer) - 1 - i];
 152         data[7 - i] = ((tmp & 0x7F) << 1) | carry;
 153         carry = (tmp & 0x80) >> 7;
 154     }
 155     data[0] = 0xFE | carry;
 156     data[7] |= isEven_64_63(data);
 157
 158     // second part
 159     if (isLong) {
 160         uint8_t id0 = r1;
 161         uint8_t id1 = (r2 << 4) | r3;
 162         uint8_t id2 = (r4 << 4) | r5;
 163
 164         data[8] = (id2 >> 1);
 165         data[9] = ((id2 & 0x01) << 7) | (id1 >> 2);
 166         data[10] = ((id1 & 0x03) << 6) | (id0 >> 3);
 167         data[11] = ((id0 & 0x07) << 5) | (FIXED_71 >> 4);
 168         data[12] = ((FIXED_71 & 0x0F) << 4) | (FIXED_40 >> 5);
 169         data[13] = ((FIXED_40 & 0x1F) << 3) | (UNKNOWN_A >> 6);
 170         data[14] = ((UNKNOWN_A & 0x3F) << 2) | (UNKNOWN_B >> 7);
 171         data[15] = ((UNKNOWN_B & 0x7F) << 1);
 172         data[15] |= isEven_64_63(data + 8);
 173     }
 174 }
 175
 176 static uint8_t isEven_64_63(const uint8_t *data) { // 8
 177     uint32_t tmp[2];
 178     memcpy(tmp, data, 8);
 179     return (bitcount32(tmp[0]) + (bitcount32(tmp[1] & 0xfeffffff))) & 1;
 180 }
 181
 182 static void bytes_to_bytebits(const void *src, const size_t srclen, void *dest) {
 183     uint8_t *s = (uint8_t *)src, *d = (uint8_t *)dest;
 184     size_t i = srclen * 8, j = srclen;
 185
 186     while (j--) {
 187         uint8_t b = s[j];
 188         d[--i] = (b >> 0) & 1;
 189         d[--i] = (b >> 1) & 1;
 190         d[--i] = (b >> 2) & 1;
 191         d[--i] = (b >> 3) & 1;
 192         d[--i] = (b >> 4) & 1;
 193         d[--i] = (b >> 5) & 1;
 194         d[--i] = (b >> 6) & 1;
 195         d[--i] = (b >> 7) & 1;
 196     }
 197 }
 198
 199 static inline uint32_t bitcount32(uint32_t a) {
 200 #if defined __GNUC__
 201     return __builtin_popcountl(a);
 202 #else
 203     a = a - ((a >> 1) & 0x55555555);
 204     a = (a & 0x33333333) + ((a >> 2) & 0x33333333);
 205     return (((a + (a >> 4)) & 0x0f0f0f0f) * 0x01010101) >> 24;
 206 #endif
 207 }