| 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 | |