armsrc/appmain.c

   1 //-----------------------------------------------------------------------------
   2 // Jonathan Westhues, Mar 2006
   3 // Edits by Gerhard de Koning Gans, Sep 2007 (##)
   4 //
   5 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   6 // at your option, any later version. See the LICENSE.txt file for the text of
   7 // the license.
   8 //-----------------------------------------------------------------------------
   9 // The main application code. This is the first thing called after start.c
  10 // executes.
  11 //-----------------------------------------------------------------------------
  12
  13 #include "usb_cdc.h"
  14 #include "cmd.h"
  15
  16 #include "proxmark3.h"
  17 #include "apps.h"
  18 #include "util.h"
  19 #include "printf.h"
  20 #include "string.h"
  21
  22 #include <stdarg.h>
  23
  24 #include "legicrf.h"
  25 #include <hitag2.h>
  26 #include "lfsampling.h"
  27 #include "BigBuf.h"
  28 #ifdef WITH_LCD
  29  #include "LCD.h"
  30 #endif
  31
  32 #define abs(x) ( ((x)<0) ? -(x) : (x) )
  33
  34 //=============================================================================
  35 // A buffer where we can queue things up to be sent through the FPGA, for
  36 // any purpose (fake tag, as reader, whatever). We go MSB first, since that
  37 // is the order in which they go out on the wire.
  38 //=============================================================================
  39
  40 #define TOSEND_BUFFER_SIZE (9*MAX_FRAME_SIZE + 1 + 1 + 2)  // 8 data bits and 1 parity bit per payload byte, 1 correction bit, 1 SOC bit, 2 EOC bits
  41 uint8_t ToSend[TOSEND_BUFFER_SIZE];
  42 int ToSendMax;
  43 static int ToSendBit;
  44 struct common_area common_area __attribute__((section(".commonarea")));
  45
  46 void ToSendReset(void)
  47 {
  48         ToSendMax = -1;
  49         ToSendBit = 8;
  50 }
  51
  52 void ToSendStuffBit(int b)
  53 {
  54         if(ToSendBit >= 8) {
  55                 ToSendMax++;
  56                 ToSend[ToSendMax] = 0;
  57                 ToSendBit = 0;
  58         }
  59
  60         if(b) {
  61                 ToSend[ToSendMax] |= (1 << (7 - ToSendBit));
  62         }
  63
  64         ToSendBit++;
  65
  66         if(ToSendMax >= sizeof(ToSend)) {
  67                 ToSendBit = 0;
  68                 DbpString("ToSendStuffBit overflowed!");
  69         }
  70 }
  71
  72 //=============================================================================
  73 // Debug print functions, to go out over USB, to the usual PC-side client.
  74 //=============================================================================
  75
  76 void DbpString(char *str)
  77 {
  78   byte_t len = strlen(str);
  79   cmd_send(CMD_DEBUG_PRINT_STRING,len,0,0,(byte_t*)str,len);
  80 }
  81
  82 #if 0
  83 void DbpIntegers(int x1, int x2, int x3)
  84 {
  85   cmd_send(CMD_DEBUG_PRINT_INTEGERS,x1,x2,x3,0,0);
  86 }
  87 #endif
  88
  89 void Dbprintf(const char *fmt, ...) {
  90 // should probably limit size here; oh well, let's just use a big buffer
  91         char output_string[128];
  92         va_list ap;
  93
  94         va_start(ap, fmt);
  95         kvsprintf(fmt, output_string, 10, ap);
  96         va_end(ap);
  97
  98         DbpString(output_string);
  99 }
 100
 101 // prints HEX & ASCII
 102 void Dbhexdump(int len, uint8_t *d, bool bAsci) {
 103         int l=0,i;
 104         char ascii[9];
 105
 106         while (len>0) {
 107                 if (len>8) l=8;
 108                 else l=len;
 109
 110                 memcpy(ascii,d,l);
 111                 ascii[l]=0;
 112
 113                 // filter safe ascii
 114                 for (i=0;i<l;i++)
 115                         if (ascii[i]<32 || ascii[i]>126) ascii[i]='.';
 116
 117                 if (bAsci) {
 118                         Dbprintf("%-8s %*D",ascii,l,d," ");
 119                 } else {
 120                         Dbprintf("%*D",l,d," ");
 121                 }
 122
 123                 len-=8;
 124                 d+=8;
 125         }
 126 }
 127
 128 //-----------------------------------------------------------------------------
 129 // Read an ADC channel and block till it completes, then return the result
 130 // in ADC units (0 to 1023). Also a routine to average 32 samples and
 131 // return that.
 132 //-----------------------------------------------------------------------------
 133 static int ReadAdc(int ch)
 134 {
 135         uint32_t d;
 136
 137         AT91C_BASE_ADC->ADC_CR = AT91C_ADC_SWRST;
 138         AT91C_BASE_ADC->ADC_MR =
 139                 ADC_MODE_PRESCALE(63  /* was 32 */) |                                                   // ADC_CLK = MCK / ((63+1) * 2) = 48MHz / 128 = 375kHz
 140                 ADC_MODE_STARTUP_TIME(1  /* was 16 */) |                                                // Startup Time = (1+1) * 8 / ADC_CLK = 16 / 375kHz = 42,7us     Note: must be > 20us
 141                 ADC_MODE_SAMPLE_HOLD_TIME(15  /* was 8 */);                                     // Sample & Hold Time SHTIM = 15 / ADC_CLK = 15 / 375kHz = 40us
 142
 143         // Note: ADC_MODE_PRESCALE and ADC_MODE_SAMPLE_HOLD_TIME are set to the maximum allowed value.
 144         // Both AMPL_LO and AMPL_HI are very high impedance (10MOhm) outputs, the input capacitance of the ADC is 12pF (typical). This results in a time constant
 145         // of RC = 10MOhm * 12pF = 120us. Even after the maximum configurable sample&hold time of 40us the input capacitor will not be fully charged.
 146         //
 147         // The maths are:
 148         // If there is a voltage v_in at the input, the voltage v_cap at the capacitor (this is what we are measuring) will be
 149         //
 150         //       v_cap = v_in * (1 - exp(-RC/SHTIM))  =   v_in * (1 - exp(-3))  =  v_in * 0,95                   (i.e. an error of 5%)
 151         //
 152         // Note: with the "historic" values in the comments above, the error was 34%  !!!
 153
 154         AT91C_BASE_ADC->ADC_CHER = ADC_CHANNEL(ch);
 155
 156         AT91C_BASE_ADC->ADC_CR = AT91C_ADC_START;
 157
 158         while(!(AT91C_BASE_ADC->ADC_SR & ADC_END_OF_CONVERSION(ch)))
 159                 ;
 160         d = AT91C_BASE_ADC->ADC_CDR[ch];
 161
 162         return d;
 163 }
 164
 165 int AvgAdc(int ch) // was static - merlok
 166 {
 167         int i;
 168         int a = 0;
 169
 170         for(i = 0; i < 32; i++) {
 171                 a += ReadAdc(ch);
 172         }
 173
 174         return (a + 15) >> 5;
 175 }
 176
 177 void MeasureAntennaTuning(void)
 178 {
 179         uint8_t LF_Results[256];
 180         int i, adcval = 0, peak = 0, peakv = 0, peakf = 0; //ptr = 0
 181         int vLf125 = 0, vLf134 = 0, vHf = 0;    // in mV
 182
 183         LED_B_ON();
 184
 185 /*
 186  * Sweeps the useful LF range of the proxmark from
 187  * 46.8kHz (divisor=255) to 600kHz (divisor=19) and
 188  * read the voltage in the antenna, the result left
 189  * in the buffer is a graph which should clearly show
 190  * the resonating frequency of your LF antenna
 191  * ( hopefully around 95 if it is tuned to 125kHz!)
 192  */
 193
 194         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 195         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 196         for (i=255; i>=19; i--) {
 197     WDT_HIT();
 198                 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, i);
 199                 SpinDelay(20);
 200                 adcval = ((MAX_ADC_LF_VOLTAGE * AvgAdc(ADC_CHAN_LF)) >> 10);
 201                 if (i==95)      vLf125 = adcval; // voltage at 125Khz
 202                 if (i==89)      vLf134 = adcval; // voltage at 134Khz
 203
 204                 LF_Results[i] = adcval>>8; // scale int to fit in byte for graphing purposes
 205                 if(LF_Results[i] > peak) {
 206                         peakv = adcval;
 207                         peak = LF_Results[i];
 208                         peakf = i;
 209                         //ptr = i;
 210                 }
 211         }
 212
 213         for (i=18; i >= 0; i--) LF_Results[i] = 0;
 214
 215         LED_A_ON();
 216         // Let the FPGA drive the high-frequency antenna around 13.56 MHz.
 217         FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
 218         FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR);
 219         SpinDelay(20);
 220         vHf = (MAX_ADC_HF_VOLTAGE * AvgAdc(ADC_CHAN_HF)) >> 10;
 221
 222         cmd_send(CMD_MEASURED_ANTENNA_TUNING, vLf125 | (vLf134<<16), vHf, peakf | (peakv<<16), LF_Results, 256);
 223         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 224         LED_A_OFF();
 225         LED_B_OFF();
 226         return;
 227 }
 228
 229 void MeasureAntennaTuningHf(void)
 230 {
 231         int vHf = 0;    // in mV
 232
 233         DbpString("Measuring HF antenna, press button to exit");
 234
 235         // Let the FPGA drive the high-frequency antenna around 13.56 MHz.
 236         FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
 237         FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR);
 238
 239         for (;;) {
 240                 SpinDelay(20);
 241                 vHf = (MAX_ADC_HF_VOLTAGE * AvgAdc(ADC_CHAN_HF)) >> 10;
 242
 243                 Dbprintf("%d mV",vHf);
 244                 if (BUTTON_PRESS()) break;
 245         }
 246         DbpString("cancelled");
 247
 248         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 249
 250 }
 251
 252
 253 void SimulateTagHfListen(void)
 254 {
 255         // ToDo: historically this used the free buffer, which was 2744 Bytes long.
 256         // There might be a better size to be defined:
 257         #define HF_14B_SNOOP_BUFFER_SIZE 2744
 258         uint8_t *dest = BigBuf_malloc(HF_14B_SNOOP_BUFFER_SIZE);
 259         uint8_t v = 0;
 260         int i;
 261         int p = 0;
 262
 263         // We're using this mode just so that I can test it out; the simulated
 264         // tag mode would work just as well and be simpler.
 265         FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
 266         FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR | FPGA_HF_READER_RX_XCORR_848_KHZ | FPGA_HF_READER_RX_XCORR_SNOOP);
 267
 268         // We need to listen to the high-frequency, peak-detected path.
 269         SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 270
 271         FpgaSetupSsc();
 272
 273         i = 0;
 274         for(;;) {
 275                 if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
 276                         AT91C_BASE_SSC->SSC_THR = 0xff;
 277                 }
 278                 if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
 279                         uint8_t r = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
 280
 281                         v <<= 1;
 282                         if(r & 1) {
 283                                 v |= 1;
 284                         }
 285                         p++;
 286
 287                         if(p >= 8) {
 288                                 dest[i] = v;
 289                                 v = 0;
 290                                 p = 0;
 291                                 i++;
 292
 293                                 if(i >= HF_14B_SNOOP_BUFFER_SIZE) {
 294                                         break;
 295                                 }
 296                         }
 297                 }
 298         }
 299         DbpString("simulate tag (now type bitsamples)");
 300 }
 301
 302 void ReadMem(int addr)
 303 {
 304         const uint8_t *data = ((uint8_t *)addr);
 305
 306         Dbprintf("%x: %02x %02x %02x %02x %02x %02x %02x %02x",
 307                 addr, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
 308 }
 309
 310 /* osimage version information is linked in */
 311 extern struct version_information version_information;
 312 /* bootrom version information is pointed to from _bootphase1_version_pointer */
 313 extern char *_bootphase1_version_pointer, _flash_start, _flash_end;
 314 void SendVersion(void)
 315 {
 316         char temp[512]; /* Limited data payload in USB packets */
 317         DbpString("Prox/RFID mark3 RFID instrument");
 318
 319         /* Try to find the bootrom version information. Expect to find a pointer at
 320          * symbol _bootphase1_version_pointer, perform slight sanity checks on the
 321          * pointer, then use it.
 322          */
 323         char *bootrom_version = *(char**)&_bootphase1_version_pointer;
 324         if( bootrom_version < &_flash_start || bootrom_version >= &_flash_end ) {
 325                 DbpString("bootrom version information appears invalid");
 326         } else {
 327                 FormatVersionInformation(temp, sizeof(temp), "bootrom: ", bootrom_version);
 328                 DbpString(temp);
 329         }
 330
 331         FormatVersionInformation(temp, sizeof(temp), "os: ", &version_information);
 332         DbpString(temp);
 333
 334         FpgaGatherVersion(temp, sizeof(temp));
 335         DbpString(temp);
 336         // Send Chip ID
 337         cmd_send(CMD_ACK,*(AT91C_DBGU_CIDR),0,0,NULL,0);
 338 }
 339
 340 #ifdef WITH_LF
 341 // samy's sniff and repeat routine
 342 void SamyRun()
 343 {
 344         DbpString("Stand-alone mode! No PC necessary.");
 345         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 346
 347         // 3 possible options? no just 2 for now
 348 #define OPTS 2
 349
 350         int high[OPTS], low[OPTS];
 351
 352         // Oooh pretty -- notify user we're in elite samy mode now
 353         LED(LED_RED,    200);
 354         LED(LED_ORANGE, 200);
 355         LED(LED_GREEN,  200);
 356         LED(LED_ORANGE, 200);
 357         LED(LED_RED,    200);
 358         LED(LED_ORANGE, 200);
 359         LED(LED_GREEN,  200);
 360         LED(LED_ORANGE, 200);
 361         LED(LED_RED,    200);
 362
 363         int selected = 0;
 364         int playing = 0;
 365         int cardRead = 0;
 366
 367         // Turn on selected LED
 368         LED(selected + 1, 0);
 369
 370         for (;;)
 371         {
 372                 usb_poll();
 373     WDT_HIT();
 374
 375                 // Was our button held down or pressed?
 376                 int button_pressed = BUTTON_HELD(1000);
 377                 SpinDelay(300);
 378
 379                 // Button was held for a second, begin recording
 380                 if (button_pressed > 0 && cardRead == 0)
 381                 {
 382                         LEDsoff();
 383                         LED(selected + 1, 0);
 384                         LED(LED_RED2, 0);
 385
 386                         // record
 387                         DbpString("Starting recording");
 388
 389                         // wait for button to be released
 390                         while(BUTTON_PRESS())
 391                                 WDT_HIT();
 392
 393                         /* need this delay to prevent catching some weird data */
 394                         SpinDelay(500);
 395
 396                         CmdHIDdemodFSK(1, &high[selected], &low[selected], 0);
 397                         Dbprintf("Recorded %x %x %x", selected, high[selected], low[selected]);
 398
 399                         LEDsoff();
 400                         LED(selected + 1, 0);
 401                         // Finished recording
 402
 403                         // If we were previously playing, set playing off
 404                         // so next button push begins playing what we recorded
 405                         playing = 0;
 406
 407                         cardRead = 1;
 408
 409                 }
 410
 411                 else if (button_pressed > 0 && cardRead == 1)
 412                 {
 413                                         LEDsoff();
 414                                         LED(selected + 1, 0);
 415                                         LED(LED_ORANGE, 0);
 416
 417                                         // record
 418                                         Dbprintf("Cloning %x %x %x", selected, high[selected], low[selected]);
 419
 420                                         // wait for button to be released
 421                                         while(BUTTON_PRESS())
 422                                                 WDT_HIT();
 423
 424                                         /* need this delay to prevent catching some weird data */
 425                                         SpinDelay(500);
 426
 427                                         CopyHIDtoT55x7(high[selected], low[selected], 0, 0);
 428                                         Dbprintf("Cloned %x %x %x", selected, high[selected], low[selected]);
 429
 430                                         LEDsoff();
 431                                         LED(selected + 1, 0);
 432                                         // Finished recording
 433
 434                                         // If we were previously playing, set playing off
 435                                         // so next button push begins playing what we recorded
 436                                         playing = 0;
 437
 438                                         cardRead = 0;
 439
 440                 }
 441
 442                 // Change where to record (or begin playing)
 443                 else if (button_pressed)
 444                 {
 445                         // Next option if we were previously playing
 446                         if (playing)
 447                                 selected = (selected + 1) % OPTS;
 448                         playing = !playing;
 449
 450                         LEDsoff();
 451                         LED(selected + 1, 0);
 452
 453                         // Begin transmitting
 454                         if (playing)
 455                         {
 456                                 LED(LED_GREEN, 0);
 457                                 DbpString("Playing");
 458                                 // wait for button to be released
 459                                 while(BUTTON_PRESS())
 460                                         WDT_HIT();
 461                                 Dbprintf("%x %x %x", selected, high[selected], low[selected]);
 462                                 CmdHIDsimTAG(high[selected], low[selected], 0);
 463                                 DbpString("Done playing");
 464                                 if (BUTTON_HELD(1000) > 0)
 465                                         {
 466                                         DbpString("Exiting");
 467                                         LEDsoff();
 468                                         return;
 469                                         }
 470
 471                                 /* We pressed a button so ignore it here with a delay */
 472                                 SpinDelay(300);
 473
 474                                 // when done, we're done playing, move to next option
 475                                 selected = (selected + 1) % OPTS;
 476                                 playing = !playing;
 477                                 LEDsoff();
 478                                 LED(selected + 1, 0);
 479                         }
 480                         else
 481                                 while(BUTTON_PRESS())
 482                                         WDT_HIT();
 483                 }
 484         }
 485 }
 486 #endif
 487
 488 /*
 489 OBJECTIVE
 490 Listen and detect an external reader. Determine the best location
 491 for the antenna.
 492
 493 INSTRUCTIONS:
 494 Inside the ListenReaderField() function, there is two mode.
 495 By default, when you call the function, you will enter mode 1.
 496 If you press the PM3 button one time, you will enter mode 2.
 497 If you press the PM3 button a second time, you will exit the function.
 498
 499 DESCRIPTION OF MODE 1:
 500 This mode just listens for an external reader field and lights up green
 501 for HF and/or red for LF. This is the original mode of the detectreader
 502 function.
 503
 504 DESCRIPTION OF MODE 2:
 505 This mode will visually represent, using the LEDs, the actual strength of the
 506 current compared to the maximum current detected. Basically, once you know
 507 what kind of external reader is present, it will help you spot the best location to place
 508 your antenna. You will probably not get some good results if there is a LF and a HF reader
 509 at the same place! :-)
 510
 511 LIGHT SCHEME USED:
 512 */
 513 static const char LIGHT_SCHEME[] = {
 514                 0x0, /* ----     | No field detected */
 515                 0x1, /* X---     | 14% of maximum current detected */
 516                 0x2, /* -X--     | 29% of maximum current detected */
 517                 0x4, /* --X-     | 43% of maximum current detected */
 518                 0x8, /* ---X     | 57% of maximum current detected */
 519                 0xC, /* --XX     | 71% of maximum current detected */
 520                 0xE, /* -XXX     | 86% of maximum current detected */
 521                 0xF, /* XXXX     | 100% of maximum current detected */
 522 };
 523 static const int LIGHT_LEN = sizeof(LIGHT_SCHEME)/sizeof(LIGHT_SCHEME[0]);
 524
 525 void ListenReaderField(int limit)
 526 {
 527         int lf_av, lf_av_new, lf_baseline= 0, lf_max;
 528         int hf_av, hf_av_new,  hf_baseline= 0, hf_max;
 529         int mode=1, display_val, display_max, i;
 530
 531 #define LF_ONLY                                         1
 532 #define HF_ONLY                                         2
 533 #define REPORT_CHANGE                           10    // report new values only if they have changed at least by REPORT_CHANGE
 534
 535
 536         // switch off FPGA - we don't want to measure our own signal
 537         FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
 538         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 539
 540         LEDsoff();
 541
 542         lf_av = lf_max = AvgAdc(ADC_CHAN_LF);
 543
 544         if(limit != HF_ONLY) {
 545                 Dbprintf("LF 125/134kHz Baseline: %dmV", (MAX_ADC_LF_VOLTAGE * lf_av) >> 10);
 546                 lf_baseline = lf_av;
 547         }
 548
 549         hf_av = hf_max = AvgAdc(ADC_CHAN_HF);
 550
 551         if (limit != LF_ONLY) {
 552                 Dbprintf("HF 13.56MHz Baseline: %dmV", (MAX_ADC_HF_VOLTAGE * hf_av) >> 10);
 553                 hf_baseline = hf_av;
 554         }
 555
 556         for(;;) {
 557                 if (BUTTON_PRESS()) {
 558                         SpinDelay(500);
 559                         switch (mode) {
 560                                 case 1:
 561                                         mode=2;
 562                                         DbpString("Signal Strength Mode");
 563                                         break;
 564                                 case 2:
 565                                 default:
 566                                         DbpString("Stopped");
 567                                         LEDsoff();
 568                                         return;
 569                                         break;
 570                         }
 571                 }
 572                 WDT_HIT();
 573
 574                 if (limit != HF_ONLY) {
 575                         if(mode == 1) {
 576                                 if (abs(lf_av - lf_baseline) > REPORT_CHANGE)
 577                                         LED_D_ON();
 578                                 else
 579                                         LED_D_OFF();
 580                         }
 581
 582                         lf_av_new = AvgAdc(ADC_CHAN_LF);
 583                         // see if there's a significant change
 584                         if(abs(lf_av - lf_av_new) > REPORT_CHANGE) {
 585                                 Dbprintf("LF 125/134kHz Field Change: %5dmV", (MAX_ADC_LF_VOLTAGE * lf_av_new) >> 10);
 586                                 lf_av = lf_av_new;
 587                                 if (lf_av > lf_max)
 588                                         lf_max = lf_av;
 589                         }
 590                 }
 591
 592                 if (limit != LF_ONLY) {
 593                         if (mode == 1){
 594                                 if (abs(hf_av - hf_baseline) > REPORT_CHANGE)
 595                                         LED_B_ON();
 596                                 else
 597                                         LED_B_OFF();
 598                         }
 599
 600                         hf_av_new = AvgAdc(ADC_CHAN_HF);
 601                         // see if there's a significant change
 602                         if(abs(hf_av - hf_av_new) > REPORT_CHANGE) {
 603                                 Dbprintf("HF 13.56MHz Field Change: %5dmV", (MAX_ADC_HF_VOLTAGE * hf_av_new) >> 10);
 604                                 hf_av = hf_av_new;
 605                                 if (hf_av > hf_max)
 606                                         hf_max = hf_av;
 607                         }
 608                 }
 609
 610                 if(mode == 2) {
 611                         if (limit == LF_ONLY) {
 612                                 display_val = lf_av;
 613                                 display_max = lf_max;
 614                         } else if (limit == HF_ONLY) {
 615                                 display_val = hf_av;
 616                                 display_max = hf_max;
 617                         } else { /* Pick one at random */
 618                                 if( (hf_max - hf_baseline) > (lf_max - lf_baseline) ) {
 619                                         display_val = hf_av;
 620                                         display_max = hf_max;
 621                                 } else {
 622                                         display_val = lf_av;
 623                                         display_max = lf_max;
 624                                 }
 625                         }
 626                         for (i=0; i<LIGHT_LEN; i++) {
 627                                 if (display_val >= ((display_max/LIGHT_LEN)*i) && display_val <= ((display_max/LIGHT_LEN)*(i+1))) {
 628                                         if (LIGHT_SCHEME[i] & 0x1) LED_C_ON(); else LED_C_OFF();
 629                                         if (LIGHT_SCHEME[i] & 0x2) LED_A_ON(); else LED_A_OFF();
 630                                         if (LIGHT_SCHEME[i] & 0x4) LED_B_ON(); else LED_B_OFF();
 631                                         if (LIGHT_SCHEME[i] & 0x8) LED_D_ON(); else LED_D_OFF();
 632                                         break;
 633                                 }
 634                         }
 635                 }
 636         }
 637 }
 638
 639 void UsbPacketReceived(uint8_t *packet, int len)
 640 {
 641         UsbCommand *c = (UsbCommand *)packet;
 642
 643 //  Dbprintf("received %d bytes, with command: 0x%04x and args: %d %d %d",len,c->cmd,c->arg[0],c->arg[1],c->arg[2]);
 644
 645         switch(c->cmd) {
 646 #ifdef WITH_LF
 647                 case CMD_SET_LF_SAMPLING_CONFIG:
 648                         setSamplingConfig((sample_config *) c->d.asBytes);
 649                         break;
 650                 case CMD_ACQUIRE_RAW_ADC_SAMPLES_125K:
 651                         cmd_send(CMD_ACK,SampleLF(c->arg[0]),0,0,0,0);
 652                         break;
 653                 case CMD_MOD_THEN_ACQUIRE_RAW_ADC_SAMPLES_125K:
 654                         ModThenAcquireRawAdcSamples125k(c->arg[0],c->arg[1],c->arg[2],c->d.asBytes);
 655                         break;
 656                 case CMD_LF_SNOOP_RAW_ADC_SAMPLES:
 657                         cmd_send(CMD_ACK,SnoopLF(),0,0,0,0);
 658                         break;
 659                 case CMD_HID_DEMOD_FSK:
 660                         CmdHIDdemodFSK(c->arg[0], 0, 0, 1);
 661                         break;
 662                 case CMD_HID_SIM_TAG:
 663                         CmdHIDsimTAG(c->arg[0], c->arg[1], 1);
 664                         break;
 665                 case CMD_FSK_SIM_TAG:
 666                         CmdFSKsimTAG(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 667                         break;
 668                 case CMD_ASK_SIM_TAG:
 669                         CmdASKsimTag(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 670                         break;
 671                 case CMD_PSK_SIM_TAG:
 672                         CmdPSKsimTag(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 673                         break;
 674                 case CMD_HID_CLONE_TAG:
 675                         CopyHIDtoT55x7(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes[0]);
 676                         break;
 677                 case CMD_IO_DEMOD_FSK:
 678                         CmdIOdemodFSK(c->arg[0], 0, 0, 1);
 679                         break;
 680                 case CMD_IO_CLONE_TAG:
 681                         CopyIOtoT55x7(c->arg[0], c->arg[1], c->d.asBytes[0]);
 682                         break;
 683                 case CMD_EM410X_DEMOD:
 684                         CmdEM410xdemod(c->arg[0], 0, 0, 1);
 685                         break;
 686                 case CMD_EM410X_WRITE_TAG:
 687                         WriteEM410x(c->arg[0], c->arg[1], c->arg[2]);
 688                         break;
 689                 case CMD_READ_TI_TYPE:
 690                         ReadTItag();
 691                         break;
 692                 case CMD_WRITE_TI_TYPE:
 693                         WriteTItag(c->arg[0],c->arg[1],c->arg[2]);
 694                         break;
 695                 case CMD_SIMULATE_TAG_125K:
 696                         LED_A_ON();
 697                         SimulateTagLowFrequency(c->arg[0], c->arg[1], 1);
 698                         LED_A_OFF();
 699                         break;
 700                 case CMD_LF_SIMULATE_BIDIR:
 701                         SimulateTagLowFrequencyBidir(c->arg[0], c->arg[1]);
 702                         break;
 703                 case CMD_INDALA_CLONE_TAG:
 704                         CopyIndala64toT55x7(c->arg[0], c->arg[1]);
 705                         break;
 706                 case CMD_INDALA_CLONE_TAG_L:
 707                         CopyIndala224toT55x7(c->d.asDwords[0], c->d.asDwords[1], c->d.asDwords[2], c->d.asDwords[3], c->d.asDwords[4], c->d.asDwords[5], c->d.asDwords[6]);
 708                         break;
 709                 case CMD_T55XX_READ_BLOCK:
 710                         T55xxReadBlock(c->arg[1], c->arg[2],c->d.asBytes[0]);
 711                         break;
 712                 case CMD_T55XX_WRITE_BLOCK:
 713                         T55xxWriteBlock(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes[0]);
 714                         break;
 715                 case CMD_T55XX_READ_TRACE:
 716                         T55xxReadTrace();
 717                         break;
 718                 case CMD_PCF7931_READ:
 719                         ReadPCF7931();
 720                         cmd_send(CMD_ACK,0,0,0,0,0);
 721                         break;
 722                 case CMD_EM4X_READ_WORD:
 723                         EM4xReadWord(c->arg[1], c->arg[2],c->d.asBytes[0]);
 724                         break;
 725                 case CMD_EM4X_WRITE_WORD:
 726                         EM4xWriteWord(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes[0]);
 727                         break;
 728 #endif
 729
 730 #ifdef WITH_HITAG
 731                 case CMD_SNOOP_HITAG: // Eavesdrop Hitag tag, args = type
 732                         SnoopHitag(c->arg[0]);
 733                         break;
 734                 case CMD_SIMULATE_HITAG: // Simulate Hitag tag, args = memory content
 735                         SimulateHitagTag((bool)c->arg[0],(byte_t*)c->d.asBytes);
 736                         break;
 737                 case CMD_READER_HITAG: // Reader for Hitag tags, args = type and function
 738                         ReaderHitag((hitag_function)c->arg[0],(hitag_data*)c->d.asBytes);
 739                         break;
 740 #endif
 741
 742 #ifdef WITH_ISO15693
 743                 case CMD_ACQUIRE_RAW_ADC_SAMPLES_ISO_15693:
 744                         AcquireRawAdcSamplesIso15693();
 745                         break;
 746                 case CMD_RECORD_RAW_ADC_SAMPLES_ISO_15693:
 747                         RecordRawAdcSamplesIso15693();
 748                         break;
 749
 750                 case CMD_ISO_15693_COMMAND:
 751                         DirectTag15693Command(c->arg[0],c->arg[1],c->arg[2],c->d.asBytes);
 752                         break;
 753
 754                 case CMD_ISO_15693_FIND_AFI:
 755                         BruteforceIso15693Afi(c->arg[0]);
 756                         break;
 757
 758                 case CMD_ISO_15693_DEBUG:
 759                         SetDebugIso15693(c->arg[0]);
 760                         break;
 761
 762                 case CMD_READER_ISO_15693:
 763                         ReaderIso15693(c->arg[0]);
 764                         break;
 765                 case CMD_SIMTAG_ISO_15693:
 766                         SimTagIso15693(c->arg[0], c->d.asBytes);
 767                         break;
 768 #endif
 769
 770 #ifdef WITH_LEGICRF
 771                 case CMD_SIMULATE_TAG_LEGIC_RF:
 772                         LegicRfSimulate(c->arg[0], c->arg[1], c->arg[2]);
 773                         break;
 774
 775                 case CMD_WRITER_LEGIC_RF:
 776                         LegicRfWriter(c->arg[1], c->arg[0]);
 777                         break;
 778
 779                 case CMD_READER_LEGIC_RF:
 780                         LegicRfReader(c->arg[0], c->arg[1]);
 781                         break;
 782 #endif
 783
 784 #ifdef WITH_ISO14443b
 785                 case CMD_ACQUIRE_RAW_ADC_SAMPLES_ISO_14443:
 786                         AcquireRawAdcSamplesIso14443(c->arg[0]);
 787                         break;
 788                 case CMD_READ_SRI512_TAG:
 789                         ReadSTMemoryIso14443(0x0F);
 790                         break;
 791                 case CMD_READ_SRIX4K_TAG:
 792                         ReadSTMemoryIso14443(0x7F);
 793                         break;
 794                 case CMD_SNOOP_ISO_14443:
 795                         SnoopIso14443();
 796                         break;
 797                 case CMD_SIMULATE_TAG_ISO_14443:
 798                         SimulateIso14443Tag();
 799                         break;
 800                 case CMD_ISO_14443B_COMMAND:
 801                         SendRawCommand14443B(c->arg[0],c->arg[1],c->arg[2],c->d.asBytes);
 802                         break;
 803 #endif
 804
 805 #ifdef WITH_ISO14443a
 806                 case CMD_SNOOP_ISO_14443a:
 807                         SnoopIso14443a(c->arg[0]);
 808                         break;
 809                 case CMD_READER_ISO_14443a:
 810                         ReaderIso14443a(c);
 811                         break;
 812                 case CMD_SIMULATE_TAG_ISO_14443a:
 813                         SimulateIso14443aTag(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);  // ## Simulate iso14443a tag - pass tag type & UID
 814                         break;
 815
 816                 case CMD_EPA_PACE_COLLECT_NONCE:
 817                         EPA_PACE_Collect_Nonce(c);
 818                         break;
 819
 820                 case CMD_READER_MIFARE:
 821                         ReaderMifare(c->arg[0]);
 822                         break;
 823                 case CMD_MIFARE_READBL:
 824                         MifareReadBlock(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 825                         break;
 826                 case CMD_MIFAREU_READBL:
 827                         MifareUReadBlock(c->arg[0],c->arg[1], c->d.asBytes);
 828                         break;
 829                 case CMD_MIFAREUC_AUTH:
 830                         MifareUC_Auth(c->arg[0],c->d.asBytes);
 831                         break;
 832                 case CMD_MIFAREU_READCARD:
 833                         MifareUReadCard(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 834                         break;
 835                 case CMD_MIFAREUC_SETPWD:
 836                         MifareUSetPwd(c->arg[0], c->d.asBytes);
 837                         break;
 838                 case CMD_MIFARE_READSC:
 839                         MifareReadSector(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 840                         break;
 841                 case CMD_MIFARE_WRITEBL:
 842                         MifareWriteBlock(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 843                         break;
 844                 //case CMD_MIFAREU_WRITEBL_COMPAT:
 845                         //MifareUWriteBlockCompat(c->arg[0], c->d.asBytes);
 846                         //break;
 847                 case CMD_MIFAREU_WRITEBL:
 848                         MifareUWriteBlock(c->arg[0], c->arg[1], c->d.asBytes);
 849                         break;
 850                 case CMD_MIFARE_NESTED:
 851                         MifareNested(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 852                         break;
 853                 case CMD_MIFARE_CHKKEYS:
 854                         MifareChkKeys(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 855                         break;
 856                 case CMD_SIMULATE_MIFARE_CARD:
 857                         Mifare1ksim(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 858                         break;
 859
 860                 // emulator
 861                 case CMD_MIFARE_SET_DBGMODE:
 862                         MifareSetDbgLvl(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 863                         break;
 864                 case CMD_MIFARE_EML_MEMCLR:
 865                         MifareEMemClr(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 866                         break;
 867                 case CMD_MIFARE_EML_MEMSET:
 868                         MifareEMemSet(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 869                         break;
 870                 case CMD_MIFARE_EML_MEMGET:
 871                         MifareEMemGet(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 872                         break;
 873                 case CMD_MIFARE_EML_CARDLOAD:
 874                         MifareECardLoad(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 875                         break;
 876
 877                 // Work with "magic Chinese" card
 878                 case CMD_MIFARE_CSETBLOCK:
 879                         MifareCSetBlock(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 880                         break;
 881                 case CMD_MIFARE_CGETBLOCK:
 882                         MifareCGetBlock(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 883                         break;
 884                 case CMD_MIFARE_CIDENT:
 885                         MifareCIdent();
 886                         break;
 887
 888                 // mifare sniffer
 889                 case CMD_MIFARE_SNIFFER:
 890                         SniffMifare(c->arg[0]);
 891                         break;
 892
 893 #endif
 894
 895 #ifdef WITH_ICLASS
 896                 // Makes use of ISO14443a FPGA Firmware
 897                 case CMD_SNOOP_ICLASS:
 898                         SnoopIClass();
 899                         break;
 900                 case CMD_SIMULATE_TAG_ICLASS:
 901                         SimulateIClass(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
 902                         break;
 903                 case CMD_READER_ICLASS:
 904                         ReaderIClass(c->arg[0]);
 905                         break;
 906                 case CMD_READER_ICLASS_REPLAY:
 907                     ReaderIClass_Replay(c->arg[0], c->d.asBytes);
 908                         break;
 909         case CMD_ICLASS_EML_MEMSET:
 910                         emlSet(c->d.asBytes,c->arg[0], c->arg[1]);
 911                         break;
 912 #endif
 913
 914                 case CMD_SIMULATE_TAG_HF_LISTEN:
 915                         SimulateTagHfListen();
 916                         break;
 917
 918                 case CMD_BUFF_CLEAR:
 919                         BigBuf_Clear();
 920                         break;
 921
 922                 case CMD_MEASURE_ANTENNA_TUNING:
 923                         MeasureAntennaTuning();
 924                         break;
 925
 926                 case CMD_MEASURE_ANTENNA_TUNING_HF:
 927                         MeasureAntennaTuningHf();
 928                         break;
 929
 930                 case CMD_LISTEN_READER_FIELD:
 931                         ListenReaderField(c->arg[0]);
 932                         break;
 933
 934                 case CMD_FPGA_MAJOR_MODE_OFF:           // ## FPGA Control
 935                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 936                         SpinDelay(200);
 937                         LED_D_OFF(); // LED D indicates field ON or OFF
 938                         break;
 939
 940                 case CMD_DOWNLOAD_RAW_ADC_SAMPLES_125K:
 941
 942                         LED_B_ON();
 943                         uint8_t *BigBuf = BigBuf_get_addr();
 944                         for(size_t i=0; i<c->arg[1]; i += USB_CMD_DATA_SIZE) {
 945                                 size_t len = MIN((c->arg[1] - i),USB_CMD_DATA_SIZE);
 946                                 cmd_send(CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K,i,len,BigBuf_get_traceLen(),BigBuf+c->arg[0]+i,len);
 947                         }
 948                         // Trigger a finish downloading signal with an ACK frame
 949                         cmd_send(CMD_ACK,1,0,BigBuf_get_traceLen(),getSamplingConfig(),sizeof(sample_config));
 950                         LED_B_OFF();
 951                         break;
 952
 953                 case CMD_DOWNLOADED_SIM_SAMPLES_125K: {
 954                         uint8_t *b = BigBuf_get_addr();
 955                         memcpy(b+c->arg[0], c->d.asBytes, USB_CMD_DATA_SIZE);
 956                         cmd_send(CMD_ACK,0,0,0,0,0);
 957                         break;
 958                 }
 959                 case CMD_READ_MEM:
 960                         ReadMem(c->arg[0]);
 961                         break;
 962
 963                 case CMD_SET_LF_DIVISOR:
 964                         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 965                         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, c->arg[0]);
 966                         break;
 967
 968                 case CMD_SET_ADC_MUX:
 969                         switch(c->arg[0]) {
 970                                 case 0: SetAdcMuxFor(GPIO_MUXSEL_LOPKD); break;
 971                                 case 1: SetAdcMuxFor(GPIO_MUXSEL_LORAW); break;
 972                                 case 2: SetAdcMuxFor(GPIO_MUXSEL_HIPKD); break;
 973                                 case 3: SetAdcMuxFor(GPIO_MUXSEL_HIRAW); break;
 974                         }
 975                         break;
 976
 977                 case CMD_VERSION:
 978                         SendVersion();
 979                         break;
 980
 981 #ifdef WITH_LCD
 982                 case CMD_LCD_RESET:
 983                         LCDReset();
 984                         break;
 985                 case CMD_LCD:
 986                         LCDSend(c->arg[0]);
 987                         break;
 988 #endif
 989                 case CMD_SETUP_WRITE:
 990                 case CMD_FINISH_WRITE:
 991                 case CMD_HARDWARE_RESET:
 992                         usb_disable();
 993                         SpinDelay(1000);
 994                         SpinDelay(1000);
 995                         AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST;
 996                         for(;;) {
 997                                 // We're going to reset, and the bootrom will take control.
 998                         }
 999                         break;
1000
1001                 case CMD_START_FLASH:
1002                         if(common_area.flags.bootrom_present) {
1003                                 common_area.command = COMMON_AREA_COMMAND_ENTER_FLASH_MODE;
1004                         }
1005                         usb_disable();
1006                         AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST;
1007                         for(;;);
1008                         break;
1009
1010                 case CMD_DEVICE_INFO: {
1011                         uint32_t dev_info = DEVICE_INFO_FLAG_OSIMAGE_PRESENT | DEVICE_INFO_FLAG_CURRENT_MODE_OS;
1012                         if(common_area.flags.bootrom_present) dev_info |= DEVICE_INFO_FLAG_BOOTROM_PRESENT;
1013                         cmd_send(CMD_DEVICE_INFO,dev_info,0,0,0,0);
1014                         break;
1015                 }
1016                 default:
1017                         Dbprintf("%s: 0x%04x","unknown command:",c->cmd);
1018                         break;
1019         }
1020 }
1021
1022 void  __attribute__((noreturn)) AppMain(void)
1023 {
1024         SpinDelay(100);
1025         clear_trace();
1026         if(common_area.magic != COMMON_AREA_MAGIC || common_area.version != 1) {
1027                 /* Initialize common area */
1028                 memset(&common_area, 0, sizeof(common_area));
1029                 common_area.magic = COMMON_AREA_MAGIC;
1030                 common_area.version = 1;
1031         }
1032         common_area.flags.osimage_present = 1;
1033
1034         LED_D_OFF();
1035         LED_C_OFF();
1036         LED_B_OFF();
1037         LED_A_OFF();
1038
1039         // Init USB device
1040   usb_enable();
1041
1042         // The FPGA gets its clock from us from PCK0 output, so set that up.
1043         AT91C_BASE_PIOA->PIO_BSR = GPIO_PCK0;
1044         AT91C_BASE_PIOA->PIO_PDR = GPIO_PCK0;
1045         AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_PCK0;
1046         // PCK0 is PLL clock / 4 = 96Mhz / 4 = 24Mhz
1047         AT91C_BASE_PMC->PMC_PCKR[0] = AT91C_PMC_CSS_PLL_CLK |
1048                 AT91C_PMC_PRES_CLK_4;
1049         AT91C_BASE_PIOA->PIO_OER = GPIO_PCK0;
1050
1051         // Reset SPI
1052         AT91C_BASE_SPI->SPI_CR = AT91C_SPI_SWRST;
1053         // Reset SSC
1054         AT91C_BASE_SSC->SSC_CR = AT91C_SSC_SWRST;
1055
1056         // Load the FPGA image, which we have stored in our flash.
1057         // (the HF version by default)
1058         FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
1059
1060         StartTickCount();
1061
1062 #ifdef WITH_LCD
1063         LCDInit();
1064 #endif
1065
1066   byte_t rx[sizeof(UsbCommand)];
1067         size_t rx_len;
1068
1069         for(;;) {
1070     if (usb_poll()) {
1071       rx_len = usb_read(rx,sizeof(UsbCommand));
1072       if (rx_len) {
1073         UsbPacketReceived(rx,rx_len);
1074       }
1075     }
1076                 WDT_HIT();
1077
1078 #ifdef WITH_LF
1079                 if (BUTTON_HELD(1000) > 0)
1080                         SamyRun();
1081 #endif
1082         }
1083 }