armsrc/util.c

   1 //-----------------------------------------------------------------------------
   2 // Jonathan Westhues, Sept 2005
   3 //
   4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   5 // at your option, any later version. See the LICENSE.txt file for the text of
   6 // the license.
   7 //-----------------------------------------------------------------------------
   8 // Utility functions used in many places, not specific to any piece of code.
   9 //-----------------------------------------------------------------------------
  10
  11 #include "proxmark3.h"
  12 #include "util.h"
  13 #include "string.h"
  14 #include "apps.h"
  15 #include "BigBuf.h"
  16
  17
  18
  19 void print_result(char *name, uint8_t *buf, size_t len) {
  20    uint8_t *p = buf;
  21
  22    if ( len % 16 == 0 ) {
  23            for(; p-buf < len; p += 16)
  24        Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
  25                                 name,
  26                                 p-buf,
  27                                 len,
  28                                 p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7],p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15]
  29            );
  30    }
  31    else {
  32    for(; p-buf < len; p += 8)
  33        Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x", name, p-buf, len, p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7]);
  34    }
  35 }
  36
  37 size_t nbytes(size_t nbits) {
  38         return (nbits >> 3)+((nbits % 8) > 0);
  39 }
  40
  41 uint32_t SwapBits(uint32_t value, int nrbits) {
  42         int i;
  43         uint32_t newvalue = 0;
  44         for(i = 0; i < nrbits; i++) {
  45                 newvalue ^= ((value >> i) & 1) << (nrbits - 1 - i);
  46         }
  47         return newvalue;
  48 }
  49
  50 void num_to_bytes(uint64_t n, size_t len, uint8_t* dest)
  51 {
  52         while (len--) {
  53                 dest[len] = (uint8_t) n;
  54                 n >>= 8;
  55         }
  56 }
  57
  58 uint64_t bytes_to_num(uint8_t* src, size_t len)
  59 {
  60         uint64_t num = 0;
  61         while (len--)
  62         {
  63                 num = (num << 8) | (*src);
  64                 src++;
  65         }
  66         return num;
  67 }
  68
  69 // RotateLeft - Ultralight, Desfire
  70 void rol(uint8_t *data, const size_t len){
  71     uint8_t first = data[0];
  72     for (size_t i = 0; i < len-1; i++) {
  73         data[i] = data[i+1];
  74     }
  75     data[len-1] = first;
  76 }
  77 void lsl (uint8_t *data, size_t len) {
  78     for (size_t n = 0; n < len - 1; n++) {
  79         data[n] = (data[n] << 1) | (data[n+1] >> 7);
  80     }
  81     data[len - 1] <<= 1;
  82 }
  83
  84 int32_t le24toh (uint8_t data[3])
  85 {
  86     return (data[2] << 16) | (data[1] << 8) | data[0];
  87 }
  88
  89 void LEDsoff()
  90 {
  91         LED_A_OFF();
  92         LED_B_OFF();
  93         LED_C_OFF();
  94         LED_D_OFF();
  95 }
  96
  97 // LEDs: R(C) O(A) G(B) -- R(D) [1, 2, 4 and 8]
  98 void LED(int led, int ms)
  99 {
 100         if (led & LED_RED)
 101                 LED_C_ON();
 102         if (led & LED_ORANGE)
 103                 LED_A_ON();
 104         if (led & LED_GREEN)
 105                 LED_B_ON();
 106         if (led & LED_RED2)
 107                 LED_D_ON();
 108
 109         if (!ms)
 110                 return;
 111
 112         SpinDelay(ms);
 113
 114         if (led & LED_RED)
 115                 LED_C_OFF();
 116         if (led & LED_ORANGE)
 117                 LED_A_OFF();
 118         if (led & LED_GREEN)
 119                 LED_B_OFF();
 120         if (led & LED_RED2)
 121                 LED_D_OFF();
 122 }
 123
 124
 125 // Determine if a button is double clicked, single clicked,
 126 // not clicked, or held down (for ms || 1sec)
 127 // In general, don't use this function unless you expect a
 128 // double click, otherwise it will waste 500ms -- use BUTTON_HELD instead
 129 int BUTTON_CLICKED(int ms)
 130 {
 131         // Up to 500ms in between clicks to mean a double click
 132         int ticks = (48000 * (ms ? ms : 1000)) >> 10;
 133
 134         // If we're not even pressed, forget about it!
 135         if (!BUTTON_PRESS())
 136                 return BUTTON_NO_CLICK;
 137
 138         // Borrow a PWM unit for my real-time clock
 139         AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0);
 140         // 48 MHz / 1024 gives 46.875 kHz
 141         AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10);
 142         AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0;
 143         AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff;
 144
 145         uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 146
 147         int letoff = 0;
 148         for(;;)
 149         {
 150                 uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 151
 152                 // We haven't let off the button yet
 153                 if (!letoff)
 154                 {
 155                         // We just let it off!
 156                         if (!BUTTON_PRESS())
 157                         {
 158                                 letoff = 1;
 159
 160                                 // reset our timer for 500ms
 161                                 start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 162                                 ticks = (48000 * (500)) >> 10;
 163                         }
 164
 165                         // Still haven't let it off
 166                         else
 167                                 // Have we held down a full second?
 168                                 if (now == (uint16_t)(start + ticks))
 169                                         return BUTTON_HOLD;
 170                 }
 171
 172                 // We already let off, did we click again?
 173                 else
 174                         // Sweet, double click!
 175                         if (BUTTON_PRESS())
 176                                 return BUTTON_DOUBLE_CLICK;
 177
 178                         // Have we ran out of time to double click?
 179                         else
 180                                 if (now == (uint16_t)(start + ticks))
 181                                         // At least we did a single click
 182                                         return BUTTON_SINGLE_CLICK;
 183
 184                 WDT_HIT();
 185         }
 186
 187         // We should never get here
 188         return BUTTON_ERROR;
 189 }
 190
 191 // Determine if a button is held down
 192 int BUTTON_HELD(int ms)
 193 {
 194         // If button is held for one second
 195         int ticks = (48000 * (ms ? ms : 1000)) >> 10;
 196
 197         // If we're not even pressed, forget about it!
 198         if (!BUTTON_PRESS())
 199                 return BUTTON_NO_CLICK;
 200
 201         // Borrow a PWM unit for my real-time clock
 202         AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0);
 203         // 48 MHz / 1024 gives 46.875 kHz
 204         AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10);
 205         AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0;
 206         AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff;
 207
 208         uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 209
 210         for(;;)
 211         {
 212                 uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 213
 214                 // As soon as our button let go, we didn't hold long enough
 215                 if (!BUTTON_PRESS())
 216                         return BUTTON_SINGLE_CLICK;
 217
 218                 // Have we waited the full second?
 219                 else
 220                         if (now == (uint16_t)(start + ticks))
 221                                 return BUTTON_HOLD;
 222
 223                 WDT_HIT();
 224         }
 225
 226         // We should never get here
 227         return BUTTON_ERROR;
 228 }
 229
 230 // attempt at high resolution microsecond timer
 231 // beware: timer counts in 21.3uS increments (1024/48Mhz)
 232 void SpinDelayUs(int us)
 233 {
 234         int ticks = (48*us) >> 10;
 235
 236         // Borrow a PWM unit for my real-time clock
 237         AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0);
 238         // 48 MHz / 1024 gives 46.875 kHz
 239         AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10);
 240         AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0;
 241         AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff;
 242
 243         uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 244
 245         for(;;) {
 246                 uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 247                 if (now == (uint16_t)(start + ticks))
 248                         return;
 249
 250                 WDT_HIT();
 251         }
 252 }
 253
 254 void SpinDelay(int ms)
 255 {
 256   // convert to uS and call microsecond delay function
 257         SpinDelayUs(ms*1000);
 258 }
 259
 260 /* Similar to FpgaGatherVersion this formats stored version information
 261  * into a string representation. It takes a pointer to the struct version_information,
 262  * verifies the magic properties, then stores a formatted string, prefixed by
 263  * prefix in dst.
 264  */
 265 void FormatVersionInformation(char *dst, int len, const char *prefix, void *version_information)
 266 {
 267         struct version_information *v = (struct version_information*)version_information;
 268         dst[0] = 0;
 269         strncat(dst, prefix, len-1);
 270         if(v->magic != VERSION_INFORMATION_MAGIC) {
 271                 strncat(dst, "Missing/Invalid version information", len - strlen(dst) - 1);
 272                 return;
 273         }
 274         if(v->versionversion != 1) {
 275                 strncat(dst, "Version information not understood", len - strlen(dst) - 1);
 276                 return;
 277         }
 278         if(!v->present) {
 279                 strncat(dst, "Version information not available", len - strlen(dst) - 1);
 280                 return;
 281         }
 282
 283         strncat(dst, v->gitversion, len - strlen(dst) - 1);
 284         if(v->clean == 0) {
 285                 strncat(dst, "-unclean", len - strlen(dst) - 1);
 286         } else if(v->clean == 2) {
 287                 strncat(dst, "-suspect", len - strlen(dst) - 1);
 288         }
 289
 290         strncat(dst, " ", len - strlen(dst) - 1);
 291         strncat(dst, v->buildtime, len - strlen(dst) - 1);
 292 }
 293
 294 //  -------------------------------------------------------------------------
 295 //  timer lib
 296 //  -------------------------------------------------------------------------
 297 //  test procedure:
 298 //
 299 //      ti = GetTickCount();
 300 //      SpinDelay(1000);
 301 //      ti = GetTickCount() - ti;
 302 //      Dbprintf("timer(1s): %d t=%d", ti, GetTickCount());
 303
 304 void StartTickCount()
 305 {
 306 //  must be 0x40, but on my cpu - included divider is optimal
 307 //  0x20 - 1 ms / bit
 308 //  0x40 - 2 ms / bit
 309
 310         AT91C_BASE_RTTC->RTTC_RTMR = AT91C_RTTC_RTTRST + 0x001D; // was 0x003B
 311 }
 312
 313 /*
 314 * Get the current count.
 315 */
 316 uint32_t RAMFUNC GetTickCount(){
 317         return AT91C_BASE_RTTC->RTTC_RTVR;// was * 2;
 318 }
 319
 320 //  -------------------------------------------------------------------------
 321 //  microseconds timer
 322 //  -------------------------------------------------------------------------
 323 void StartCountUS()
 324 {
 325         AT91C_BASE_PMC->PMC_PCER |= (0x1 << 12) | (0x1 << 13) | (0x1 << 14);
 326 //      AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC1XC1S_TIOA0;
 327         AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_NONE | AT91C_TCB_TC1XC1S_TIOA0 | AT91C_TCB_TC2XC2S_NONE;
 328
 329         // fast clock
 330         AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS; // timer disable
 331         AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | // MCK(48MHz)/32 -- tick=1.5mks
 332                                                                                                                 AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_ACPA_CLEAR |
 333                                                                                                                 AT91C_TC_ACPC_SET | AT91C_TC_ASWTRG_SET;
 334         AT91C_BASE_TC0->TC_RA = 1;
 335         AT91C_BASE_TC0->TC_RC = 0xBFFF + 1; // 0xC000
 336
 337         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // timer disable
 338         AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_XC1; // from timer 0
 339
 340         AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN;
 341         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN;
 342         AT91C_BASE_TCB->TCB_BCR = 1;
 343         }
 344
 345 uint32_t RAMFUNC GetCountUS(){
 346         return (AT91C_BASE_TC1->TC_CV * 0x8000) + ((AT91C_BASE_TC0->TC_CV / 15) * 10);
 347 }
 348
 349 static uint32_t GlobalUsCounter = 0;
 350
 351 uint32_t RAMFUNC GetDeltaCountUS(){
 352         uint32_t g_cnt = GetCountUS();
 353         uint32_t g_res = g_cnt - GlobalUsCounter;
 354         GlobalUsCounter = g_cnt;
 355         return g_res;
 356 }
 357
 358
 359 //  -------------------------------------------------------------------------
 360 //  Timer for iso14443 commands. Uses ssp_clk from FPGA
 361 //  -------------------------------------------------------------------------
 362 void StartCountSspClk()
 363 {
 364         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0) | (1 << AT91C_ID_TC1) | (1 << AT91C_ID_TC2);  // Enable Clock to all timers
 365         AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_TIOA1               // XC0 Clock = TIOA1
 366                                                         | AT91C_TCB_TC1XC1S_NONE                // XC1 Clock = none
 367                                                         | AT91C_TCB_TC2XC2S_TIOA0;              // XC2 Clock = TIOA0
 368
 369         // configure TC1 to create a short pulse on TIOA1 when a rising edge on TIOB1 (= ssp_clk from FPGA) occurs:
 370         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;                               // disable TC1
 371         AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK // TC1 Clock = MCK(48MHz)/2 = 24MHz
 372                                                         | AT91C_TC_CPCSTOP                              // Stop clock on RC compare
 373                                                         | AT91C_TC_EEVTEDG_RISING               // Trigger on rising edge of Event
 374                                                         | AT91C_TC_EEVT_TIOB                    // Event-Source: TIOB1 (= ssp_clk from FPGA = 13,56MHz/16)
 375                                                         | AT91C_TC_ENETRG                               // Enable external trigger event
 376                                                         | AT91C_TC_WAVESEL_UP                   // Upmode without automatic trigger on RC compare
 377                                                         | AT91C_TC_WAVE                                 // Waveform Mode
 378                                                         | AT91C_TC_AEEVT_SET                    // Set TIOA1 on external event
 379                                                         | AT91C_TC_ACPC_CLEAR;                  // Clear TIOA1 on RC Compare
 380         AT91C_BASE_TC1->TC_RC = 0x04;                                                   // RC Compare value = 0x04
 381
 382         // use TC0 to count TIOA1 pulses
 383         AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;                               // disable TC0
 384         AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_XC0                              // TC0 clock = XC0 clock = TIOA1
 385                                                         | AT91C_TC_WAVE                                 // Waveform Mode
 386                                                         | AT91C_TC_WAVESEL_UP                   // just count
 387                                                         | AT91C_TC_ACPA_CLEAR                   // Clear TIOA0 on RA Compare
 388                                                         | AT91C_TC_ACPC_SET;                    // Set TIOA0 on RC Compare
 389         AT91C_BASE_TC0->TC_RA = 1;                                                              // RA Compare value = 1; pulse width to TC2
 390         AT91C_BASE_TC0->TC_RC = 0;                                                              // RC Compare value = 0; increment TC2 on overflow
 391
 392         // use TC2 to count TIOA0 pulses (giving us a 32bit counter (TC0/TC2) clocked by ssp_clk)
 393         AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKDIS;                               // disable TC2
 394         AT91C_BASE_TC2->TC_CMR = AT91C_TC_CLKS_XC2                              // TC2 clock = XC2 clock = TIOA0
 395                                                         | AT91C_TC_WAVE                                 // Waveform Mode
 396                                                         | AT91C_TC_WAVESEL_UP;                  // just count
 397
 398         AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN;                                // enable TC0
 399         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN;                                // enable TC1
 400         AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKEN;                                // enable TC2
 401
 402         //
 403         // synchronize the counter with the ssp_frame signal. Note: FPGA must be in any iso14446 mode, otherwise the frame signal would not be present
 404         //
 405         while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME));   // wait for ssp_frame to go high (start of frame)
 406         while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME);              // wait for ssp_frame to be low
 407         while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK));     // wait for ssp_clk to go high
 408         // note: up to now two ssp_clk rising edges have passed since the rising edge of ssp_frame
 409         // it is now safe to assert a sync signal. This sets all timers to 0 on next active clock edge
 410         AT91C_BASE_TCB->TCB_BCR = 1;                                                    // assert Sync (set all timers to 0 on next active clock edge)
 411         // at the next (3rd) ssp_clk rising edge, TC1 will be reset (and not generate a clock signal to TC0)
 412         // at the next (4th) ssp_clk rising edge, TC0 (the low word of our counter) will be reset. From now on,
 413         // whenever the last three bits of our counter go 0, we can be sure to be in the middle of a frame transfer.
 414         // (just started with the transfer of the 4th Bit).
 415         // The high word of the counter (TC2) will not reset until the low word (TC0) overflows. Therefore need to wait quite some time before
 416         // we can use the counter.
 417         while (AT91C_BASE_TC0->TC_CV < 0xFFF0);
 418 }
 419
 420
 421 uint32_t RAMFUNC GetCountSspClk(){
 422         uint32_t tmp_count;
 423         tmp_count = (AT91C_BASE_TC2->TC_CV << 16) | AT91C_BASE_TC0->TC_CV;
 424         if ((tmp_count & 0x0000ffff) == 0) { //small chance that we may have missed an increment in TC2
 425                 return (AT91C_BASE_TC2->TC_CV << 16);
 426         }
 427         else {
 428                 return tmp_count;
 429         }
 430 }
 431