]> git.zerfleddert.de Git - proxmark3-svn/blob - armsrc/lfops.c
projects / proxmark3-svn / blob
? search:


11fc8c50a7864ee1f8a28d442dee66e7ac1a7af3
[proxmark3-svn] / armsrc / lfops.c
1 //-----------------------------------------------------------------------------
2 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
3 // at your option, any later version. See the LICENSE.txt file for the text of
4 // the license.
5 //-----------------------------------------------------------------------------
6 // Miscellaneous routines for low frequency tag operations.
7 // Tags supported here so far are Texas Instruments (TI), HID
8 // Also routines for raw mode reading/simulating of LF waveform
9 //-----------------------------------------------------------------------------
10
11 #include "../include/proxmark3.h"
12 #include "apps.h"
13 #include "util.h"
14 #include "../include/hitag2.h"
15 #include "../common/crc16.h"
16 #include "string.h"
17 #include "crapto1.h"
18 #include "mifareutil.h"
19
20 void LFSetupFPGAForADC(int divisor, bool lf_field)
21 {
22 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
23 if ( (divisor == 1) || (divisor < 0) || (divisor > 255) )
24 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
25 else if (divisor == 0)
26 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
27 else
28 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, divisor);
29
30 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | (lf_field ? FPGA_LF_ADC_READER_FIELD : 0));
31
32 // Connect the A/D to the peak-detected low-frequency path.
33 SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
34 // Give it a bit of time for the resonant antenna to settle.
35 SpinDelay(50);
36 // Now set up the SSC to get the ADC samples that are now streaming at us.
37 FpgaSetupSsc();
38 }
39
40 void AcquireRawAdcSamples125k(int divisor)
41 {
42 LFSetupFPGAForADC(divisor, true);
43 DoAcquisition125k(-1);
44 }
45
46 void SnoopLFRawAdcSamples(int divisor, int trigger_threshold)
47 {
48 LFSetupFPGAForADC(divisor, false);
49 DoAcquisition125k(trigger_threshold);
50 }
51
52 // split into two routines so we can avoid timing issues after sending commands //
53 void DoAcquisition125k(int trigger_threshold)
54 {
55 uint8_t *dest = (uint8_t *)BigBuf;
56 int n = sizeof(BigBuf);
57 int i;
58
59 memset(dest, 0, n);
60 i = 0;
61 for(;;) {
62 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
63 AT91C_BASE_SSC->SSC_THR = 0x43;
64 LED_D_ON();
65 }
66 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
67 dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
68 LED_D_OFF();
69 if (trigger_threshold != -1 && dest[i] < trigger_threshold)
70 continue;
71 else
72 trigger_threshold = -1;
73 if (++i >= n) break;
74 }
75 }
76 Dbprintf("buffer samples: %02x %02x %02x %02x %02x %02x %02x %02x ...",
77 dest[0], dest[1], dest[2], dest[3], dest[4], dest[5], dest[6], dest[7]);
78 }
79
80 void ModThenAcquireRawAdcSamples125k(int delay_off, int period_0, int period_1, uint8_t *command)
81 {
82 int at134khz;
83
84 /* Make sure the tag is reset */
85 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
86 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
87 SpinDelay(2500);
88
89 // see if 'h' was specified
90 if (command[strlen((char *) command) - 1] == 'h')
91 at134khz = TRUE;
92 else
93 at134khz = FALSE;
94
95 if (at134khz)
96 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
97 else
98 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
99
100 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
101
102 // Give it a bit of time for the resonant antenna to settle.
103 SpinDelay(50);
104 // And a little more time for the tag to fully power up
105 SpinDelay(2000);
106
107 // Now set up the SSC to get the ADC samples that are now streaming at us.
108 FpgaSetupSsc();
109
110 // now modulate the reader field
111 while(*command != '\0' && *command != ' ') {
112 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
113 LED_D_OFF();
114 SpinDelayUs(delay_off);
115 if (at134khz)
116 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
117 else
118 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
119
120 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
121 LED_D_ON();
122 if(*(command++) == '0')
123 SpinDelayUs(period_0);
124 else
125 SpinDelayUs(period_1);
126 }
127 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
128 LED_D_OFF();
129 SpinDelayUs(delay_off);
130 if (at134khz)
131 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
132 else
133 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
134
135 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
136
137 // now do the read
138 DoAcquisition125k(-1);
139 }
140
141 /* blank r/w tag data stream
142 ...0000000000000000 01111111
143 1010101010101010101010101010101010101010101010101010101010101010
144 0011010010100001
145 01111111
146 101010101010101[0]000...
147
148 [5555fe852c5555555555555555fe0000]
149 */
150 void ReadTItag(void)
151 {
152 // some hardcoded initial params
153 // when we read a TI tag we sample the zerocross line at 2Mhz
154 // TI tags modulate a 1 as 16 cycles of 123.2Khz
155 // TI tags modulate a 0 as 16 cycles of 134.2Khz
156 #define FSAMPLE 2000000
157 #define FREQLO 123200
158 #define FREQHI 134200
159
160 signed char *dest = (signed char *)BigBuf;
161 int n = sizeof(BigBuf);
162 // int *dest = GraphBuffer;
163 // int n = GraphTraceLen;
164
165 // 128 bit shift register [shift3:shift2:shift1:shift0]
166 uint32_t shift3 = 0, shift2 = 0, shift1 = 0, shift0 = 0;
167
168 int i, cycles=0, samples=0;
169 // how many sample points fit in 16 cycles of each frequency
170 uint32_t sampleslo = (FSAMPLE<<4)/FREQLO, sampleshi = (FSAMPLE<<4)/FREQHI;
171 // when to tell if we're close enough to one freq or another
172 uint32_t threshold = (sampleslo - sampleshi + 1)>>1;
173
174 // TI tags charge at 134.2Khz
175 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
176 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
177
178 // Place FPGA in passthrough mode, in this mode the CROSS_LO line
179 // connects to SSP_DIN and the SSP_DOUT logic level controls
180 // whether we're modulating the antenna (high)
181 // or listening to the antenna (low)
182 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU);
183
184 // get TI tag data into the buffer
185 AcquireTiType();
186
187 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
188
189 for (i=0; i<n-1; i++) {
190 // count cycles by looking for lo to hi zero crossings
191 if ( (dest[i]<0) && (dest[i+1]>0) ) {
192 cycles++;
193 // after 16 cycles, measure the frequency
194 if (cycles>15) {
195 cycles=0;
196 samples=i-samples; // number of samples in these 16 cycles
197
198 // TI bits are coming to us lsb first so shift them
199 // right through our 128 bit right shift register
200 shift0 = (shift0>>1) | (shift1 << 31);
201 shift1 = (shift1>>1) | (shift2 << 31);
202 shift2 = (shift2>>1) | (shift3 << 31);
203 shift3 >>= 1;
204
205 // check if the cycles fall close to the number
206 // expected for either the low or high frequency
207 if ( (samples>(sampleslo-threshold)) && (samples<(sampleslo+threshold)) ) {
208 // low frequency represents a 1
209 shift3 |= (1<<31);
210 } else if ( (samples>(sampleshi-threshold)) && (samples<(sampleshi+threshold)) ) {
211 // high frequency represents a 0
212 } else {
213 // probably detected a gay waveform or noise
214 // use this as gaydar or discard shift register and start again
215 shift3 = shift2 = shift1 = shift0 = 0;
216 }
217 samples = i;
218
219 // for each bit we receive, test if we've detected a valid tag
220
221 // if we see 17 zeroes followed by 6 ones, we might have a tag
222 // remember the bits are backwards
223 if ( ((shift0 & 0x7fffff) == 0x7e0000) ) {
224 // if start and end bytes match, we have a tag so break out of the loop
225 if ( ((shift0>>16)&0xff) == ((shift3>>8)&0xff) ) {
226 cycles = 0xF0B; //use this as a flag (ugly but whatever)
227 break;
228 }
229 }
230 }
231 }
232 }
233
234 // if flag is set we have a tag
235 if (cycles!=0xF0B) {
236 DbpString("Info: No valid tag detected.");
237 } else {
238 // put 64 bit data into shift1 and shift0
239 shift0 = (shift0>>24) | (shift1 << 8);
240 shift1 = (shift1>>24) | (shift2 << 8);
241
242 // align 16 bit crc into lower half of shift2
243 shift2 = ((shift2>>24) | (shift3 << 8)) & 0x0ffff;
244
245 // if r/w tag, check ident match
246 if ( shift3&(1<<15) ) {
247 DbpString("Info: TI tag is rewriteable");
248 // only 15 bits compare, last bit of ident is not valid
249 if ( ((shift3>>16)^shift0)&0x7fff ) {
250 DbpString("Error: Ident mismatch!");
251 } else {
252 DbpString("Info: TI tag ident is valid");
253 }
254 } else {
255 DbpString("Info: TI tag is readonly");
256 }
257
258 // WARNING the order of the bytes in which we calc crc below needs checking
259 // i'm 99% sure the crc algorithm is correct, but it may need to eat the
260 // bytes in reverse or something
261 // calculate CRC
262 uint32_t crc=0;
263
264 crc = update_crc16(crc, (shift0)&0xff);
265 crc = update_crc16(crc, (shift0>>8)&0xff);
266 crc = update_crc16(crc, (shift0>>16)&0xff);
267 crc = update_crc16(crc, (shift0>>24)&0xff);
268 crc = update_crc16(crc, (shift1)&0xff);
269 crc = update_crc16(crc, (shift1>>8)&0xff);
270 crc = update_crc16(crc, (shift1>>16)&0xff);
271 crc = update_crc16(crc, (shift1>>24)&0xff);
272
273 Dbprintf("Info: Tag data: %x%08x, crc=%x",
274 (unsigned int)shift1, (unsigned int)shift0, (unsigned int)shift2 & 0xFFFF);
275 if (crc != (shift2&0xffff)) {
276 Dbprintf("Error: CRC mismatch, expected %x", (unsigned int)crc);
277 } else {
278 DbpString("Info: CRC is good");
279 }
280 }
281 }
282
283 void WriteTIbyte(uint8_t b)
284 {
285 int i = 0;
286
287 // modulate 8 bits out to the antenna
288 for (i=0; i<8; i++)
289 {
290 if (b&(1<<i)) {
291 // stop modulating antenna
292 LOW(GPIO_SSC_DOUT);
293 SpinDelayUs(1000);
294 // modulate antenna
295 HIGH(GPIO_SSC_DOUT);
296 SpinDelayUs(1000);
297 } else {
298 // stop modulating antenna
299 LOW(GPIO_SSC_DOUT);
300 SpinDelayUs(300);
301 // modulate antenna
302 HIGH(GPIO_SSC_DOUT);
303 SpinDelayUs(1700);
304 }
305 }
306 }
307
308 void AcquireTiType(void)
309 {
310 int i, j, n;
311 // tag transmission is <20ms, sampling at 2M gives us 40K samples max
312 // each sample is 1 bit stuffed into a uint32_t so we need 1250 uint32_t
313 #define TIBUFLEN 1250
314
315 // clear buffer
316 memset(BigBuf,0,sizeof(BigBuf));
317
318 // Set up the synchronous serial port
319 AT91C_BASE_PIOA->PIO_PDR = GPIO_SSC_DIN;
320 AT91C_BASE_PIOA->PIO_ASR = GPIO_SSC_DIN;
321
322 // steal this pin from the SSP and use it to control the modulation
323 AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
324 AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
325
326 AT91C_BASE_SSC->SSC_CR = AT91C_SSC_SWRST;
327 AT91C_BASE_SSC->SSC_CR = AT91C_SSC_RXEN | AT91C_SSC_TXEN;
328
329 // Sample at 2 Mbit/s, so TI tags are 16.2 vs. 14.9 clocks long
330 // 48/2 = 24 MHz clock must be divided by 12
331 AT91C_BASE_SSC->SSC_CMR = 12;
332
333 AT91C_BASE_SSC->SSC_RCMR = SSC_CLOCK_MODE_SELECT(0);
334 AT91C_BASE_SSC->SSC_RFMR = SSC_FRAME_MODE_BITS_IN_WORD(32) | AT91C_SSC_MSBF;
335 AT91C_BASE_SSC->SSC_TCMR = 0;
336 AT91C_BASE_SSC->SSC_TFMR = 0;
337
338 LED_D_ON();
339
340 // modulate antenna
341 HIGH(GPIO_SSC_DOUT);
342
343 // Charge TI tag for 50ms.
344 SpinDelay(50);
345
346 // stop modulating antenna and listen
347 LOW(GPIO_SSC_DOUT);
348
349 LED_D_OFF();
350
351 i = 0;
352 for(;;) {
353 if(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
354 BigBuf[i] = AT91C_BASE_SSC->SSC_RHR; // store 32 bit values in buffer
355 i++; if(i >= TIBUFLEN) break;
356 }
357 WDT_HIT();
358 }
359
360 // return stolen pin to SSP
361 AT91C_BASE_PIOA->PIO_PDR = GPIO_SSC_DOUT;
362 AT91C_BASE_PIOA->PIO_ASR = GPIO_SSC_DIN | GPIO_SSC_DOUT;
363
364 char *dest = (char *)BigBuf;
365 n = TIBUFLEN*32;
366 // unpack buffer
367 for (i=TIBUFLEN-1; i>=0; i--) {
368 for (j=0; j<32; j++) {
369 if(BigBuf[i] & (1 << j)) {
370 dest[--n] = 1;
371 } else {
372 dest[--n] = -1;
373 }
374 }
375 }
376 }
377
378 // arguments: 64bit data split into 32bit idhi:idlo and optional 16bit crc
379 // if crc provided, it will be written with the data verbatim (even if bogus)
380 // if not provided a valid crc will be computed from the data and written.
381 void WriteTItag(uint32_t idhi, uint32_t idlo, uint16_t crc)
382 {
383 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
384 if(crc == 0) {
385 crc = update_crc16(crc, (idlo)&0xff);
386 crc = update_crc16(crc, (idlo>>8)&0xff);
387 crc = update_crc16(crc, (idlo>>16)&0xff);
388 crc = update_crc16(crc, (idlo>>24)&0xff);
389 crc = update_crc16(crc, (idhi)&0xff);
390 crc = update_crc16(crc, (idhi>>8)&0xff);
391 crc = update_crc16(crc, (idhi>>16)&0xff);
392 crc = update_crc16(crc, (idhi>>24)&0xff);
393 }
394 Dbprintf("Writing to tag: %x%08x, crc=%x",
395 (unsigned int) idhi, (unsigned int) idlo, crc);
396
397 // TI tags charge at 134.2Khz
398 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
399 // Place FPGA in passthrough mode, in this mode the CROSS_LO line
400 // connects to SSP_DIN and the SSP_DOUT logic level controls
401 // whether we're modulating the antenna (high)
402 // or listening to the antenna (low)
403 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU);
404 LED_A_ON();
405
406 // steal this pin from the SSP and use it to control the modulation
407 AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
408 AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
409
410 // writing algorithm:
411 // a high bit consists of a field off for 1ms and field on for 1ms
412 // a low bit consists of a field off for 0.3ms and field on for 1.7ms
413 // initiate a charge time of 50ms (field on) then immediately start writing bits
414 // start by writing 0xBB (keyword) and 0xEB (password)
415 // then write 80 bits of data (or 64 bit data + 16 bit crc if you prefer)
416 // finally end with 0x0300 (write frame)
417 // all data is sent lsb firts
418 // finish with 15ms programming time
419
420 // modulate antenna
421 HIGH(GPIO_SSC_DOUT);
422 SpinDelay(50); // charge time
423
424 WriteTIbyte(0xbb); // keyword
425 WriteTIbyte(0xeb); // password
426 WriteTIbyte( (idlo )&0xff );
427 WriteTIbyte( (idlo>>8 )&0xff );
428 WriteTIbyte( (idlo>>16)&0xff );
429 WriteTIbyte( (idlo>>24)&0xff );
430 WriteTIbyte( (idhi )&0xff );
431 WriteTIbyte( (idhi>>8 )&0xff );
432 WriteTIbyte( (idhi>>16)&0xff );
433 WriteTIbyte( (idhi>>24)&0xff ); // data hi to lo
434 WriteTIbyte( (crc )&0xff ); // crc lo
435 WriteTIbyte( (crc>>8 )&0xff ); // crc hi
436 WriteTIbyte(0x00); // write frame lo
437 WriteTIbyte(0x03); // write frame hi
438 HIGH(GPIO_SSC_DOUT);
439 SpinDelay(50); // programming time
440
441 LED_A_OFF();
442
443 // get TI tag data into the buffer
444 AcquireTiType();
445
446 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
447 DbpString("Now use tiread to check");
448 }
449
450 void SimulateTagLowFrequency(int period, int gap, int ledcontrol)
451 {
452 int i;
453 uint8_t *tab = (uint8_t *)BigBuf;
454
455 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
456 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT);
457
458 AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT | GPIO_SSC_CLK;
459
460 AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
461 AT91C_BASE_PIOA->PIO_ODR = GPIO_SSC_CLK;
462
463 #define SHORT_COIL() LOW(GPIO_SSC_DOUT)
464 #define OPEN_COIL() HIGH(GPIO_SSC_DOUT)
465
466 i = 0;
467 for(;;) {
468 while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)) {
469 if(BUTTON_PRESS()) {
470 DbpString("Stopped");
471 return;
472 }
473 WDT_HIT();
474 }
475
476 if (ledcontrol)
477 LED_D_ON();
478
479 if(tab[i])
480 OPEN_COIL();
481 else
482 SHORT_COIL();
483
484 if (ledcontrol)
485 LED_D_OFF();
486
487 while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK) {
488 if(BUTTON_PRESS()) {
489 DbpString("Stopped");
490 return;
491 }
492 WDT_HIT();
493 }
494
495 i++;
496 if(i == period) {
497 i = 0;
498 if (gap) {
499 SHORT_COIL();
500 SpinDelayUs(gap);
501 }
502 }
503 }
504 }
505
506 #define DEBUG_FRAME_CONTENTS 1
507 void SimulateTagLowFrequencyBidir(int divisor, int t0)
508 {
509 }
510
511 // compose fc/8 fc/10 waveform
512 static void fc(int c, int *n) {
513 uint8_t *dest = (uint8_t *)BigBuf;
514 int idx;
515
516 // for when we want an fc8 pattern every 4 logical bits
517 if(c==0) {
518 dest[((*n)++)]=1;
519 dest[((*n)++)]=1;
520 dest[((*n)++)]=0;
521 dest[((*n)++)]=0;
522 dest[((*n)++)]=0;
523 dest[((*n)++)]=0;
524 dest[((*n)++)]=0;
525 dest[((*n)++)]=0;
526 }
527 // an fc/8 encoded bit is a bit pattern of 11000000 x6 = 48 samples
528 if(c==8) {
529 for (idx=0; idx<6; idx++) {
530 dest[((*n)++)]=1;
531 dest[((*n)++)]=1;
532 dest[((*n)++)]=0;
533 dest[((*n)++)]=0;
534 dest[((*n)++)]=0;
535 dest[((*n)++)]=0;
536 dest[((*n)++)]=0;
537 dest[((*n)++)]=0;
538 }
539 }
540
541 // an fc/10 encoded bit is a bit pattern of 1110000000 x5 = 50 samples
542 if(c==10) {
543 for (idx=0; idx<5; idx++) {
544 dest[((*n)++)]=1;
545 dest[((*n)++)]=1;
546 dest[((*n)++)]=1;
547 dest[((*n)++)]=0;
548 dest[((*n)++)]=0;
549 dest[((*n)++)]=0;
550 dest[((*n)++)]=0;
551 dest[((*n)++)]=0;
552 dest[((*n)++)]=0;
553 dest[((*n)++)]=0;
554 }
555 }
556 }
557
558 // prepare a waveform pattern in the buffer based on the ID given then
559 // simulate a HID tag until the button is pressed
560 void CmdHIDsimTAG(int hi, int lo, int ledcontrol)
561 {
562 int n=0, i=0;
563 /*
564 HID tag bitstream format
565 The tag contains a 44bit unique code. This is sent out MSB first in sets of 4 bits
566 A 1 bit is represented as 6 fc8 and 5 fc10 patterns
567 A 0 bit is represented as 5 fc10 and 6 fc8 patterns
568 A fc8 is inserted before every 4 bits
569 A special start of frame pattern is used consisting a0b0 where a and b are neither 0
570 nor 1 bits, they are special patterns (a = set of 12 fc8 and b = set of 10 fc10)
571 */
572
573 if (hi>0xFFF) {
574 DbpString("Tags can only have 44 bits.");
575 return;
576 }
577 fc(0,&n);
578 // special start of frame marker containing invalid bit sequences
579 fc(8, &n); fc(8, &n); // invalid
580 fc(8, &n); fc(10, &n); // logical 0
581 fc(10, &n); fc(10, &n); // invalid
582 fc(8, &n); fc(10, &n); // logical 0
583
584 WDT_HIT();
585 // manchester encode bits 43 to 32
586 for (i=11; i>=0; i--) {
587 if ((i%4)==3) fc(0,&n);
588 if ((hi>>i)&1) {
589 fc(10, &n); fc(8, &n); // low-high transition
590 } else {
591 fc(8, &n); fc(10, &n); // high-low transition
592 }
593 }
594
595 WDT_HIT();
596 // manchester encode bits 31 to 0
597 for (i=31; i>=0; i--) {
598 if ((i%4)==3) fc(0,&n);
599 if ((lo>>i)&1) {
600 fc(10, &n); fc(8, &n); // low-high transition
601 } else {
602 fc(8, &n); fc(10, &n); // high-low transition
603 }
604 }
605
606 if (ledcontrol)
607 LED_A_ON();
608 SimulateTagLowFrequency(n, 0, ledcontrol);
609
610 if (ledcontrol)
611 LED_A_OFF();
612 }
613
614
615 // loop to capture raw HID waveform then FSK demodulate the TAG ID from it
616 void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
617 {
618 uint8_t *dest = (uint8_t *)BigBuf;
619 int m=0, n=0, i=0, idx=0, found=0, lastval=0;
620 uint32_t hi2=0, hi=0, lo=0;
621
622 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
623 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
624 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
625
626 // Connect the A/D to the peak-detected low-frequency path.
627 SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
628
629 // Give it a bit of time for the resonant antenna to settle.
630 SpinDelay(50);
631
632 // Now set up the SSC to get the ADC samples that are now streaming at us.
633 FpgaSetupSsc();
634
635 for(;;) {
636 WDT_HIT();
637 if (ledcontrol)
638 LED_A_ON();
639 if(BUTTON_PRESS()) {
640 DbpString("Stopped");
641 if (ledcontrol)
642 LED_A_OFF();
643 return;
644 }
645
646 i = 0;
647 m = sizeof(BigBuf);
648 memset(dest,128,m);
649 for(;;) {
650 if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
651 AT91C_BASE_SSC->SSC_THR = 0x43;
652 if (ledcontrol)
653 LED_D_ON();
654 }
655 if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
656 dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
657 // we don't care about actual value, only if it's more or less than a
658 // threshold essentially we capture zero crossings for later analysis
659 if(dest[i] < 127) dest[i] = 0; else dest[i] = 1;
660 i++;
661 if (ledcontrol)
662 LED_D_OFF();
663 if(i >= m) {
664 break;
665 }
666 }
667 }
668
669 // FSK demodulator
670
671 // sync to first lo-hi transition
672 for( idx=1; idx<m; idx++) {
673 if (dest[idx-1]<dest[idx])
674 lastval=idx;
675 break;
676 }
677 WDT_HIT();
678
679 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
680 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
681 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
682 for( i=0; idx<m; idx++) {
683 if (dest[idx-1]<dest[idx]) {
684 dest[i]=idx-lastval;
685 if (dest[i] <= 8) {
686 dest[i]=1;
687 } else {
688 dest[i]=0;
689 }
690
691 lastval=idx;
692 i++;
693 }
694 }
695 m=i;
696 WDT_HIT();
697
698 // we now have a set of cycle counts, loop over previous results and aggregate data into bit patterns
699 lastval=dest[0];
700 idx=0;
701 i=0;
702 n=0;
703 for( idx=0; idx<m; idx++) {
704 if (dest[idx]==lastval) {
705 n++;
706 } else {
707 // a bit time is five fc/10 or six fc/8 cycles so figure out how many bits a pattern width represents,
708 // an extra fc/8 pattern preceeds every 4 bits (about 200 cycles) just to complicate things but it gets
709 // swallowed up by rounding
710 // expected results are 1 or 2 bits, any more and it's an invalid manchester encoding
711 // special start of frame markers use invalid manchester states (no transitions) by using sequences
712 // like 111000
713 if (dest[idx-1]) {
714 n=(n+1)/6; // fc/8 in sets of 6
715 } else {
716 n=(n+1)/5; // fc/10 in sets of 5
717 }
718 switch (n) { // stuff appropriate bits in buffer
719 case 0:
720 case 1: // one bit
721 dest[i++]=dest[idx-1];
722 break;
723 case 2: // two bits
724 dest[i++]=dest[idx-1];
725 dest[i++]=dest[idx-1];
726 break;
727 case 3: // 3 bit start of frame markers
728 dest[i++]=dest[idx-1];
729 dest[i++]=dest[idx-1];
730 dest[i++]=dest[idx-1];
731 break;
732 // When a logic 0 is immediately followed by the start of the next transmisson
733 // (special pattern) a pattern of 4 bit duration lengths is created.
734 case 4:
735 dest[i++]=dest[idx-1];
736 dest[i++]=dest[idx-1];
737 dest[i++]=dest[idx-1];
738 dest[i++]=dest[idx-1];
739 break;
740 default: // this shouldn't happen, don't stuff any bits
741 break;
742 }
743 n=0;
744 lastval=dest[idx];
745 }
746 }
747 m=i;
748 WDT_HIT();
749
750 // final loop, go over previously decoded manchester data and decode into usable tag ID
751 // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
752 for( idx=0; idx<m-6; idx++) {
753 // search for a start of frame marker
754 if ( dest[idx] && dest[idx+1] && dest[idx+2] && (!dest[idx+3]) && (!dest[idx+4]) && (!dest[idx+5]) )
755 {
756 found=1;
757 idx+=6;
758 if (found && (hi2|hi|lo)) {
759 if (hi2 != 0){
760 Dbprintf("TAG ID: %x%08x%08x (%d)",
761 (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
762 }
763 else {
764 Dbprintf("TAG ID: %x%08x (%d)",
765 (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
766 }
767 /* if we're only looking for one tag */
768 if (findone)
769 {
770 *high = hi;
771 *low = lo;
772 return;
773 }
774 hi2=0;
775 hi=0;
776 lo=0;
777 found=0;
778 }
779 }
780 if (found) {
781 if (dest[idx] && (!dest[idx+1]) ) {
782 hi2=(hi2<<1)|(hi>>31);
783 hi=(hi<<1)|(lo>>31);
784 lo=(lo<<1)|0;
785 } else if ( (!dest[idx]) && dest[idx+1]) {
786 hi2=(hi2<<1)|(hi>>31);
787 hi=(hi<<1)|(lo>>31);
788 lo=(lo<<1)|1;
789 } else {
790 found=0;
791 hi2=0;
792 hi=0;
793 lo=0;
794 }
795 idx++;
796 }
797 if ( dest[idx] && dest[idx+1] && dest[idx+2] && (!dest[idx+3]) && (!dest[idx+4]) && (!dest[idx+5]) )
798 {
799 found=1;
800 idx+=6;
801 if (found && (hi|lo)) {
802 if (hi2 != 0){
803 Dbprintf("TAG ID: %x%08x%08x (%d)",
804 (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
805 }
806 else {
807 Dbprintf("TAG ID: %x%08x (%d)",
808 (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
809 }
810 /* if we're only looking for one tag */
811 if (findone)
812 {
813 *high = hi;
814 *low = lo;
815 return;
816 }
817 hi2=0;
818 hi=0;
819 lo=0;
820 found=0;
821 }
822 }
823 }
824 WDT_HIT();
825 }
826 }
827
828 void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
829 {
830 uint8_t *dest = (uint8_t *)BigBuf;
831 int m=0, n=0, i=0, idx=0, lastval=0;
832 int found=0;
833 uint32_t code=0, code2=0;
834 //uint32_t hi2=0, hi=0, lo=0;
835
836 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
837 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
838 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
839
840 // Connect the A/D to the peak-detected low-frequency path.
841 SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
842
843 // Give it a bit of time for the resonant antenna to settle.
844 SpinDelay(50);
845
846 // Now set up the SSC to get the ADC samples that are now streaming at us.
847 FpgaSetupSsc();
848
849 for(;;) {
850 WDT_HIT();
851 if (ledcontrol)
852 LED_A_ON();
853 if(BUTTON_PRESS()) {
854 DbpString("Stopped");
855 if (ledcontrol)
856 LED_A_OFF();
857 return;
858 }
859
860 i = 0;
861 m = sizeof(BigBuf);
862 memset(dest,128,m);
863 for(;;) {
864 if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
865 AT91C_BASE_SSC->SSC_THR = 0x43;
866 if (ledcontrol)
867 LED_D_ON();
868 }
869 if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
870 dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
871 // we don't care about actual value, only if it's more or less than a
872 // threshold essentially we capture zero crossings for later analysis
873 if(dest[i] < 127) dest[i] = 0; else dest[i] = 1;
874 i++;
875 if (ledcontrol)
876 LED_D_OFF();
877 if(i >= m) {
878 break;
879 }
880 }
881 }
882
883 // FSK demodulator
884
885 // sync to first lo-hi transition
886 for( idx=1; idx<m; idx++) {
887 if (dest[idx-1]<dest[idx])
888 lastval=idx;
889 break;
890 }
891 WDT_HIT();
892
893 // count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
894 // or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
895 // between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
896 for( i=0; idx<m; idx++) {
897 if (dest[idx-1]<dest[idx]) {
898 dest[i]=idx-lastval;
899 if (dest[i] <= 8) {
900 dest[i]=1;
901 } else {
902 dest[i]=0;
903 }
904
905 lastval=idx;
906 i++;
907 }
908 }
909 m=i;
910 WDT_HIT();
911
912 // we now have a set of cycle counts, loop over previous results and aggregate data into bit patterns
913 lastval=dest[0];
914 idx=0;
915 i=0;
916 n=0;
917 for( idx=0; idx<m; idx++) {
918 if (dest[idx]==lastval) {
919 n++;
920 } else {
921 // a bit time is five fc/10 or six fc/8 cycles so figure out how many bits a pattern width represents,
922 // an extra fc/8 pattern preceeds every 4 bits (about 200 cycles) just to complicate things but it gets
923 // swallowed up by rounding
924 // expected results are 1 or 2 bits, any more and it's an invalid manchester encoding
925 // special start of frame markers use invalid manchester states (no transitions) by using sequences
926 // like 111000
927 if (dest[idx-1]) {
928 n=(n+1)/7; // fc/8 in sets of 7
929 } else {
930 n=(n+1)/6; // fc/10 in sets of 6
931 }
932 switch (n) { // stuff appropriate bits in buffer
933 case 0:
934 case 1: // one bit
935 dest[i++]=dest[idx-1]^1;
936 //Dbprintf("%d",dest[idx-1]);
937 break;
938 case 2: // two bits
939 dest[i++]=dest[idx-1]^1;
940 dest[i++]=dest[idx-1]^1;
941 //Dbprintf("%d",dest[idx-1]);
942 //Dbprintf("%d",dest[idx-1]);
943 break;
944 case 3: // 3 bit start of frame markers
945 for(int j=0; j<3; j++){
946 dest[i++]=dest[idx-1]^1;
947 // Dbprintf("%d",dest[idx-1]);
948 }
949 break;
950 case 4:
951 for(int j=0; j<4; j++){
952 dest[i++]=dest[idx-1]^1;
953 // Dbprintf("%d",dest[idx-1]);
954 }
955 break;
956 case 5:
957 for(int j=0; j<5; j++){
958 dest[i++]=dest[idx-1]^1;
959 // Dbprintf("%d",dest[idx-1]);
960 }
961 break;
962 case 6:
963 for(int j=0; j<6; j++){
964 dest[i++]=dest[idx-1]^1;
965 // Dbprintf("%d",dest[idx-1]);
966 }
967 break;
968 case 7:
969 for(int j=0; j<7; j++){
970 dest[i++]=dest[idx-1]^1;
971 // Dbprintf("%d",dest[idx-1]);
972 }
973 break;
974 case 8:
975 for(int j=0; j<8; j++){
976 dest[i++]=dest[idx-1]^1;
977 // Dbprintf("%d",dest[idx-1]);
978 }
979 break;
980 case 9:
981 for(int j=0; j<9; j++){
982 dest[i++]=dest[idx-1]^1;
983 // Dbprintf("%d",dest[idx-1]);
984 }
985 break;
986 case 10:
987 for(int j=0; j<10; j++){
988 dest[i++]=dest[idx-1]^1;
989 // Dbprintf("%d",dest[idx-1]);
990 }
991 break;
992 case 11:
993 for(int j=0; j<11; j++){
994 dest[i++]=dest[idx-1]^1;
995 // Dbprintf("%d",dest[idx-1]);
996 }
997 break;
998 case 12:
999 for(int j=0; j<12; j++){
1000 dest[i++]=dest[idx-1]^1;
1001 // Dbprintf("%d",dest[idx-1]);
1002 }
1003 break;
1004 default: // this shouldn't happen, don't stuff any bits
1005 //Dbprintf("%d",dest[idx-1]);
1006 break;
1007 }
1008 n=0;
1009 lastval=dest[idx];
1010 }
1011 }//end for
1012 /*for(int j=0; j<64;j+=8){
1013 Dbprintf("%d%d%d%d%d%d%d%d",dest[j],dest[j+1],dest[j+2],dest[j+3],dest[j+4],dest[j+5],dest[j+6],dest[j+7]);
1014 }
1015 Dbprintf("\n");*/
1016 m=i;
1017 WDT_HIT();
1018
1019 for( idx=0; idx<m-9; idx++) {
1020 if ( !(dest[idx]) && !(dest[idx+1]) && !(dest[idx+2]) && !(dest[idx+3]) && !(dest[idx+4]) && !(dest[idx+5]) && !(dest[idx+6]) && !(dest[idx+7]) && !(dest[idx+8])&& (dest[idx+9])){
1021 found=1;
1022 //idx+=9;
1023 if (found) {
1024 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx], dest[idx+1], dest[idx+2],dest[idx+3],dest[idx+4],dest[idx+5],dest[idx+6],dest[idx+7]);
1025 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx+8], dest[idx+9], dest[idx+10],dest[idx+11],dest[idx+12],dest[idx+13],dest[idx+14],dest[idx+15]);
1026 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx+16],dest[idx+17],dest[idx+18],dest[idx+19],dest[idx+20],dest[idx+21],dest[idx+22],dest[idx+23]);
1027 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx+24],dest[idx+25],dest[idx+26],dest[idx+27],dest[idx+28],dest[idx+29],dest[idx+30],dest[idx+31]);
1028 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx+32],dest[idx+33],dest[idx+34],dest[idx+35],dest[idx+36],dest[idx+37],dest[idx+38],dest[idx+39]);
1029 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx+40],dest[idx+41],dest[idx+42],dest[idx+43],dest[idx+44],dest[idx+45],dest[idx+46],dest[idx+47]);
1030 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx+48],dest[idx+49],dest[idx+50],dest[idx+51],dest[idx+52],dest[idx+53],dest[idx+54],dest[idx+55]);
1031 Dbprintf("%d%d%d%d%d%d%d%d",dest[idx+56],dest[idx+57],dest[idx+58],dest[idx+59],dest[idx+60],dest[idx+61],dest[idx+62],dest[idx+63]);
1032
1033 short version='\x00';
1034 char unknown='\x00';
1035 uint16_t number=0;
1036 for(int j=14;j<18;j++){
1037 //Dbprintf("%d",dest[idx+j]);
1038 version <<=1;
1039 if (dest[idx+j]) version |= 1;
1040 }
1041 for(int j=19;j<27;j++){
1042 //Dbprintf("%d",dest[idx+j]);
1043 unknown <<=1;
1044 if (dest[idx+j]) unknown |= 1;
1045 }
1046 for(int j=36;j<45;j++){
1047 //Dbprintf("%d",dest[idx+j]);
1048 number <<=1;
1049 if (dest[idx+j]) number |= 1;
1050 }
1051 for(int j=46;j<53;j++){
1052 //Dbprintf("%d",dest[idx+j]);
1053 number <<=1;
1054 if (dest[idx+j]) number |= 1;
1055 }
1056 for(int j=0; j<32; j++){
1057 code <<=1;
1058 if(dest[idx+j]) code |= 1;
1059 }
1060 for(int j=32; j<64; j++){
1061 code2 <<=1;
1062 if(dest[idx+j]) code2 |= 1;
1063 }
1064
1065 Dbprintf("XSF(%02d)%02x:%d (%08x%08x)",version,unknown,number,code,code2);
1066 if (ledcontrol)
1067 LED_D_OFF();
1068 }
1069 // if we're only looking for one tag
1070 if (findone){
1071 //*high = hi;
1072 //*low = lo;
1073 LED_A_OFF();
1074 return;
1075 }
1076
1077 //hi=0;
1078 //lo=0;
1079 found=0;
1080 }
1081
1082 }
1083 }
1084 WDT_HIT();
1085 }
1086
1087 /*------------------------------
1088 * T5555/T5557/T5567 routines
1089 *------------------------------
1090 */
1091
1092 /* T55x7 configuration register definitions */
1093 #define T55x7_POR_DELAY 0x00000001
1094 #define T55x7_ST_TERMINATOR 0x00000008
1095 #define T55x7_PWD 0x00000010
1096 #define T55x7_MAXBLOCK_SHIFT 5
1097 #define T55x7_AOR 0x00000200
1098 #define T55x7_PSKCF_RF_2 0
1099 #define T55x7_PSKCF_RF_4 0x00000400
1100 #define T55x7_PSKCF_RF_8 0x00000800
1101 #define T55x7_MODULATION_DIRECT 0
1102 #define T55x7_MODULATION_PSK1 0x00001000
1103 #define T55x7_MODULATION_PSK2 0x00002000
1104 #define T55x7_MODULATION_PSK3 0x00003000
1105 #define T55x7_MODULATION_FSK1 0x00004000
1106 #define T55x7_MODULATION_FSK2 0x00005000
1107 #define T55x7_MODULATION_FSK1a 0x00006000
1108 #define T55x7_MODULATION_FSK2a 0x00007000
1109 #define T55x7_MODULATION_MANCHESTER 0x00008000
1110 #define T55x7_MODULATION_BIPHASE 0x00010000
1111 #define T55x7_BITRATE_RF_8 0
1112 #define T55x7_BITRATE_RF_16 0x00040000
1113 #define T55x7_BITRATE_RF_32 0x00080000
1114 #define T55x7_BITRATE_RF_40 0x000C0000
1115 #define T55x7_BITRATE_RF_50 0x00100000
1116 #define T55x7_BITRATE_RF_64 0x00140000
1117 #define T55x7_BITRATE_RF_100 0x00180000
1118 #define T55x7_BITRATE_RF_128 0x001C0000
1119
1120 /* T5555 (Q5) configuration register definitions */
1121 #define T5555_ST_TERMINATOR 0x00000001
1122 #define T5555_MAXBLOCK_SHIFT 0x00000001
1123 #define T5555_MODULATION_MANCHESTER 0
1124 #define T5555_MODULATION_PSK1 0x00000010
1125 #define T5555_MODULATION_PSK2 0x00000020
1126 #define T5555_MODULATION_PSK3 0x00000030
1127 #define T5555_MODULATION_FSK1 0x00000040
1128 #define T5555_MODULATION_FSK2 0x00000050
1129 #define T5555_MODULATION_BIPHASE 0x00000060
1130 #define T5555_MODULATION_DIRECT 0x00000070
1131 #define T5555_INVERT_OUTPUT 0x00000080
1132 #define T5555_PSK_RF_2 0
1133 #define T5555_PSK_RF_4 0x00000100
1134 #define T5555_PSK_RF_8 0x00000200
1135 #define T5555_USE_PWD 0x00000400
1136 #define T5555_USE_AOR 0x00000800
1137 #define T5555_BITRATE_SHIFT 12
1138 #define T5555_FAST_WRITE 0x00004000
1139 #define T5555_PAGE_SELECT 0x00008000
1140
1141 /*
1142 * Relevant times in microsecond
1143 * To compensate antenna falling times shorten the write times
1144 * and enlarge the gap ones.
1145 */
1146 #define START_GAP 250
1147 #define WRITE_GAP 160
1148 #define WRITE_0 144 // 192
1149 #define WRITE_1 400 // 432 for T55x7; 448 for E5550
1150
1151 // VALUES TAKEN FROM EM4x function: SendForward
1152 // START_GAP = 440; //(55*8)
1153 // WRITE_GAP = 128; //(16*8)
1154 // WRITE_1 = 256 32*8; //32 cycles at 125Khz (8us each) 1
1155 // //These timings work for 4469/4269/4305 (with the 55*8 above)
1156 // WRITE_0 = 23*8 , 9*8 SpinDelayUs(23*8); // (8us each) 0
1157
1158
1159
1160 // Write one bit to card
1161 void T55xxWriteBit(int bit)
1162 {
1163 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1164 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1165 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1166 if (bit == 0)
1167 SpinDelayUs(WRITE_0);
1168 else
1169 SpinDelayUs(WRITE_1);
1170 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1171 SpinDelayUs(WRITE_GAP);
1172 }
1173
1174 // Write one card block in page 0, no lock
1175 void T55xxWriteBlock(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
1176 {
1177 unsigned int i;
1178
1179 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1180 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1181 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1182
1183 // Give it a bit of time for the resonant antenna to settle.
1184 // And for the tag to fully power up
1185 SpinDelay(150);
1186
1187 // Now start writting
1188 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1189 SpinDelayUs(START_GAP);
1190
1191 // Opcode
1192 T55xxWriteBit(1);
1193 T55xxWriteBit(0); //Page 0
1194 if (PwdMode == 1){
1195 // Pwd
1196 for (i = 0x80000000; i != 0; i >>= 1)
1197 T55xxWriteBit(Pwd & i);
1198 }
1199 // Lock bit
1200 T55xxWriteBit(0);
1201
1202 // Data
1203 for (i = 0x80000000; i != 0; i >>= 1)
1204 T55xxWriteBit(Data & i);
1205
1206 // Block
1207 for (i = 0x04; i != 0; i >>= 1)
1208 T55xxWriteBit(Block & i);
1209
1210 // Now perform write (nominal is 5.6 ms for T55x7 and 18ms for E5550,
1211 // so wait a little more)
1212 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1213 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1214 SpinDelay(20);
1215 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1216 }
1217
1218 // Read one card block in page 0
1219 void T55xxReadBlock(uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
1220 {
1221 uint8_t *dest = mifare_get_bigbufptr();
1222 uint16_t bufferlength = 16000;
1223 uint32_t i = 0;
1224
1225 // Clear destination buffer before sending the command 0x80 = average.
1226 memset(dest, 0x80, bufferlength);
1227
1228 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1229
1230 // Connect the A/D to the peak-detected low-frequency path.
1231 SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1232 // Now set up the SSC to get the ADC samples that are now streaming at us.
1233 FpgaSetupSsc();
1234
1235 LED_D_ON();
1236 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1237 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1238
1239 // Give it a bit of time for the resonant antenna to settle.
1240 // And for the tag to fully power up
1241 SpinDelay(150);
1242
1243 // Now start writting
1244 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1245 SpinDelayUs(START_GAP);
1246
1247 // Opcode
1248 T55xxWriteBit(1);
1249 T55xxWriteBit(0); //Page 0
1250 if (PwdMode == 1){
1251 // Pwd
1252 for (i = 0x80000000; i != 0; i >>= 1)
1253 T55xxWriteBit(Pwd & i);
1254 }
1255 // Lock bit
1256 T55xxWriteBit(0);
1257 // Block
1258 for (i = 0x04; i != 0; i >>= 1)
1259 T55xxWriteBit(Block & i);
1260
1261 // Turn field on to read the response
1262 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1263 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1264
1265 // Now do the acquisition
1266 i = 0;
1267 for(;;) {
1268 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
1269 AT91C_BASE_SSC->SSC_THR = 0x43;
1270 LED_D_ON();
1271 }
1272 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
1273 dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
1274 LED_D_OFF();
1275 ++i;
1276 if (i > bufferlength) break;
1277 }
1278 }
1279
1280 cmd_send(CMD_ACK,0,0,0,0,0);
1281
1282 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1283 LED_D_OFF();
1284 }
1285
1286 // Read card traceability data (page 1)
1287 void T55xxReadTrace(void){
1288 uint8_t *dest = mifare_get_bigbufptr();
1289 uint16_t bufferlength = 16000;
1290 int i=0;
1291
1292 // Clear destination buffer before sending the command 0x80 = average
1293 memset(dest, 0x80, bufferlength);
1294
1295 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1296
1297 // Connect the A/D to the peak-detected low-frequency path.
1298 SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1299 // Now set up the SSC to get the ADC samples that are now streaming at us.
1300 FpgaSetupSsc();
1301
1302 LED_D_ON();
1303 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1304 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1305
1306 // Give it a bit of time for the resonant antenna to settle.
1307 // And for the tag to fully power up
1308 SpinDelay(150);
1309
1310 // Now start writting
1311 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1312 SpinDelayUs(START_GAP);
1313
1314 // Opcode
1315 T55xxWriteBit(1);
1316 T55xxWriteBit(1); //Page 1
1317
1318 // Turn field on to read the response
1319 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1320 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1321
1322 // Now do the acquisition
1323 i = 0;
1324 for(;;) {
1325 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
1326 AT91C_BASE_SSC->SSC_THR = 0x43;
1327 LED_D_ON();
1328 }
1329 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
1330 dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
1331 LED_D_OFF();
1332 ++i;
1333 if (i >= bufferlength) break;
1334 }
1335 }
1336
1337 cmd_send(CMD_ACK,0,0,0,0,0);
1338
1339 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1340 LED_D_OFF();
1341 }
1342
1343 /*-------------- Cloning routines -----------*/
1344 // Copy HID id to card and setup block 0 config
1345 void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT)
1346 {
1347 int data1=0, data2=0, data3=0, data4=0, data5=0, data6=0; //up to six blocks for long format
1348 int last_block = 0;
1349
1350 if (longFMT){
1351 // Ensure no more than 84 bits supplied
1352 if (hi2>0xFFFFF) {
1353 DbpString("Tags can only have 84 bits.");
1354 return;
1355 }
1356 // Build the 6 data blocks for supplied 84bit ID
1357 last_block = 6;
1358 data1 = 0x1D96A900; // load preamble (1D) & long format identifier (9E manchester encoded)
1359 for (int i=0;i<4;i++) {
1360 if (hi2 & (1<<(19-i)))
1361 data1 |= (1<<(((3-i)*2)+1)); // 1 -> 10
1362 else
1363 data1 |= (1<<((3-i)*2)); // 0 -> 01
1364 }
1365
1366 data2 = 0;
1367 for (int i=0;i<16;i++) {
1368 if (hi2 & (1<<(15-i)))
1369 data2 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1370 else
1371 data2 |= (1<<((15-i)*2)); // 0 -> 01
1372 }
1373
1374 data3 = 0;
1375 for (int i=0;i<16;i++) {
1376 if (hi & (1<<(31-i)))
1377 data3 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1378 else
1379 data3 |= (1<<((15-i)*2)); // 0 -> 01
1380 }
1381
1382 data4 = 0;
1383 for (int i=0;i<16;i++) {
1384 if (hi & (1<<(15-i)))
1385 data4 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1386 else
1387 data4 |= (1<<((15-i)*2)); // 0 -> 01
1388 }
1389
1390 data5 = 0;
1391 for (int i=0;i<16;i++) {
1392 if (lo & (1<<(31-i)))
1393 data5 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1394 else
1395 data5 |= (1<<((15-i)*2)); // 0 -> 01
1396 }
1397
1398 data6 = 0;
1399 for (int i=0;i<16;i++) {
1400 if (lo & (1<<(15-i)))
1401 data6 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1402 else
1403 data6 |= (1<<((15-i)*2)); // 0 -> 01
1404 }
1405 }
1406 else {
1407 // Ensure no more than 44 bits supplied
1408 if (hi>0xFFF) {
1409 DbpString("Tags can only have 44 bits.");
1410 return;
1411 }
1412
1413 // Build the 3 data blocks for supplied 44bit ID
1414 last_block = 3;
1415
1416 data1 = 0x1D000000; // load preamble
1417
1418 for (int i=0;i<12;i++) {
1419 if (hi & (1<<(11-i)))
1420 data1 |= (1<<(((11-i)*2)+1)); // 1 -> 10
1421 else
1422 data1 |= (1<<((11-i)*2)); // 0 -> 01
1423 }
1424
1425 data2 = 0;
1426 for (int i=0;i<16;i++) {
1427 if (lo & (1<<(31-i)))
1428 data2 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1429 else
1430 data2 |= (1<<((15-i)*2)); // 0 -> 01
1431 }
1432
1433 data3 = 0;
1434 for (int i=0;i<16;i++) {
1435 if (lo & (1<<(15-i)))
1436 data3 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1437 else
1438 data3 |= (1<<((15-i)*2)); // 0 -> 01
1439 }
1440 }
1441
1442 LED_D_ON();
1443 // Program the data blocks for supplied ID
1444 // and the block 0 for HID format
1445 T55xxWriteBlock(data1,1,0,0);
1446 T55xxWriteBlock(data2,2,0,0);
1447 T55xxWriteBlock(data3,3,0,0);
1448
1449 if (longFMT) { // if long format there are 6 blocks
1450 T55xxWriteBlock(data4,4,0,0);
1451 T55xxWriteBlock(data5,5,0,0);
1452 T55xxWriteBlock(data6,6,0,0);
1453 }
1454
1455 // Config for HID (RF/50, FSK2a, Maxblock=3 for short/6 for long)
1456 T55xxWriteBlock(T55x7_BITRATE_RF_50 |
1457 T55x7_MODULATION_FSK2a |
1458 last_block << T55x7_MAXBLOCK_SHIFT,
1459 0,0,0);
1460
1461 LED_D_OFF();
1462
1463 DbpString("DONE!");
1464 }
1465
1466 void CopyIOtoT55x7(uint32_t hi, uint32_t lo, uint8_t longFMT)
1467 {
1468 int data1=0, data2=0; //up to six blocks for long format
1469
1470 data1 = hi; // load preamble
1471 data2 = lo;
1472
1473 LED_D_ON();
1474 // Program the data blocks for supplied ID
1475 // and the block 0 for HID format
1476 T55xxWriteBlock(data1,1,0,0);
1477 T55xxWriteBlock(data2,2,0,0);
1478
1479 //Config Block
1480 T55xxWriteBlock(0x00147040,0,0,0);
1481 LED_D_OFF();
1482
1483 DbpString("DONE!");
1484 }
1485
1486 // Define 9bit header for EM410x tags
1487 #define EM410X_HEADER 0x1FF
1488 #define EM410X_ID_LENGTH 40
1489
1490 void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo)
1491 {
1492 int i, id_bit;
1493 uint64_t id = EM410X_HEADER;
1494 uint64_t rev_id = 0; // reversed ID
1495 int c_parity[4]; // column parity
1496 int r_parity = 0; // row parity
1497 uint32_t clock = 0;
1498
1499 // Reverse ID bits given as parameter (for simpler operations)
1500 for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1501 if (i < 32) {
1502 rev_id = (rev_id << 1) | (id_lo & 1);
1503 id_lo >>= 1;
1504 } else {
1505 rev_id = (rev_id << 1) | (id_hi & 1);
1506 id_hi >>= 1;
1507 }
1508 }
1509
1510 for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1511 id_bit = rev_id & 1;
1512
1513 if (i % 4 == 0) {
1514 // Don't write row parity bit at start of parsing
1515 if (i)
1516 id = (id << 1) | r_parity;
1517 // Start counting parity for new row
1518 r_parity = id_bit;
1519 } else {
1520 // Count row parity
1521 r_parity ^= id_bit;
1522 }
1523
1524 // First elements in column?
1525 if (i < 4)
1526 // Fill out first elements
1527 c_parity[i] = id_bit;
1528 else
1529 // Count column parity
1530 c_parity[i % 4] ^= id_bit;
1531
1532 // Insert ID bit
1533 id = (id << 1) | id_bit;
1534 rev_id >>= 1;
1535 }
1536
1537 // Insert parity bit of last row
1538 id = (id << 1) | r_parity;
1539
1540 // Fill out column parity at the end of tag
1541 for (i = 0; i < 4; ++i)
1542 id = (id << 1) | c_parity[i];
1543
1544 // Add stop bit
1545 id <<= 1;
1546
1547 Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555");
1548 LED_D_ON();
1549
1550 // Write EM410x ID
1551 T55xxWriteBlock((uint32_t)(id >> 32), 1, 0, 0);
1552 T55xxWriteBlock((uint32_t)id, 2, 0, 0);
1553
1554 // Config for EM410x (RF/64, Manchester, Maxblock=2)
1555 if (card) {
1556 // Clock rate is stored in bits 8-15 of the card value
1557 clock = (card & 0xFF00) >> 8;
1558 Dbprintf("Clock rate: %d", clock);
1559 switch (clock)
1560 {
1561 case 32:
1562 clock = T55x7_BITRATE_RF_32;
1563 break;
1564 case 16:
1565 clock = T55x7_BITRATE_RF_16;
1566 break;
1567 case 0:
1568 // A value of 0 is assumed to be 64 for backwards-compatibility
1569 // Fall through...
1570 case 64:
1571 clock = T55x7_BITRATE_RF_64;
1572 break;
1573 default:
1574 Dbprintf("Invalid clock rate: %d", clock);
1575 return;
1576 }
1577
1578 // Writing configuration for T55x7 tag
1579 T55xxWriteBlock(clock |
1580 T55x7_MODULATION_MANCHESTER |
1581 2 << T55x7_MAXBLOCK_SHIFT,
1582 0, 0, 0);
1583 }
1584 else
1585 // Writing configuration for T5555(Q5) tag
1586 T55xxWriteBlock(0x1F << T5555_BITRATE_SHIFT |
1587 T5555_MODULATION_MANCHESTER |
1588 2 << T5555_MAXBLOCK_SHIFT,
1589 0, 0, 0);
1590
1591 LED_D_OFF();
1592 Dbprintf("Tag %s written with 0x%08x%08x\n", card ? "T55x7":"T5555",
1593 (uint32_t)(id >> 32), (uint32_t)id);
1594 }
1595
1596 // Clone Indala 64-bit tag by UID to T55x7
1597 void CopyIndala64toT55x7(int hi, int lo)
1598 {
1599
1600 //Program the 2 data blocks for supplied 64bit UID
1601 // and the block 0 for Indala64 format
1602 T55xxWriteBlock(hi,1,0,0);
1603 T55xxWriteBlock(lo,2,0,0);
1604 //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=2)
1605 T55xxWriteBlock(T55x7_BITRATE_RF_32 |
1606 T55x7_MODULATION_PSK1 |
1607 2 << T55x7_MAXBLOCK_SHIFT,
1608 0, 0, 0);
1609 //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=2;Inverse data)
1610 // T5567WriteBlock(0x603E1042,0);
1611
1612 DbpString("DONE!");
1613
1614 }
1615
1616 void CopyIndala224toT55x7(int uid1, int uid2, int uid3, int uid4, int uid5, int uid6, int uid7)
1617 {
1618
1619 //Program the 7 data blocks for supplied 224bit UID
1620 // and the block 0 for Indala224 format
1621 T55xxWriteBlock(uid1,1,0,0);
1622 T55xxWriteBlock(uid2,2,0,0);
1623 T55xxWriteBlock(uid3,3,0,0);
1624 T55xxWriteBlock(uid4,4,0,0);
1625 T55xxWriteBlock(uid5,5,0,0);
1626 T55xxWriteBlock(uid6,6,0,0);
1627 T55xxWriteBlock(uid7,7,0,0);
1628 //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=7)
1629 T55xxWriteBlock(T55x7_BITRATE_RF_32 |
1630 T55x7_MODULATION_PSK1 |
1631 7 << T55x7_MAXBLOCK_SHIFT,
1632 0,0,0);
1633 //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=7;Inverse data)
1634 // T5567WriteBlock(0x603E10E2,0);
1635
1636 DbpString("DONE!");
1637
1638 }
1639
1640
1641 #define abs(x) ( ((x)<0) ? -(x) : (x) )
1642 #define max(x,y) ( x<y ? y:x)
1643
1644 int DemodPCF7931(uint8_t **outBlocks) {
1645 uint8_t BitStream[256];
1646 uint8_t Blocks[8][16];
1647 uint8_t *GraphBuffer = (uint8_t *)BigBuf;
1648 int GraphTraceLen = sizeof(BigBuf);
1649 int i, j, lastval, bitidx, half_switch;
1650 int clock = 64;
1651 int tolerance = clock / 8;
1652 int pmc, block_done;
1653 int lc, warnings = 0;
1654 int num_blocks = 0;
1655 int lmin=128, lmax=128;
1656 uint8_t dir;
1657
1658 AcquireRawAdcSamples125k(0);
1659
1660 lmin = 64;
1661 lmax = 192;
1662
1663 i = 2;
1664
1665 /* Find first local max/min */
1666 if(GraphBuffer[1] > GraphBuffer[0]) {
1667 while(i < GraphTraceLen) {
1668 if( !(GraphBuffer[i] > GraphBuffer[i-1]) && GraphBuffer[i] > lmax)
1669 break;
1670 i++;
1671 }
1672 dir = 0;
1673 }
1674 else {
1675 while(i < GraphTraceLen) {
1676 if( !(GraphBuffer[i] < GraphBuffer[i-1]) && GraphBuffer[i] < lmin)
1677 break;
1678 i++;
1679 }
1680 dir = 1;
1681 }
1682
1683 lastval = i++;
1684 half_switch = 0;
1685 pmc = 0;
1686 block_done = 0;
1687
1688 for (bitidx = 0; i < GraphTraceLen; i++)
1689 {
1690 if ( (GraphBuffer[i-1] > GraphBuffer[i] && dir == 1 && GraphBuffer[i] > lmax) || (GraphBuffer[i-1] < GraphBuffer[i] && dir == 0 && GraphBuffer[i] < lmin))
1691 {
1692 lc = i - lastval;
1693 lastval = i;
1694
1695 // Switch depending on lc length:
1696 // Tolerance is 1/8 of clock rate (arbitrary)
1697 if (abs(lc-clock/4) < tolerance) {
1698 // 16T0
1699 if((i - pmc) == lc) { /* 16T0 was previous one */
1700 /* It's a PMC ! */
1701 i += (128+127+16+32+33+16)-1;
1702 lastval = i;
1703 pmc = 0;
1704 block_done = 1;
1705 }
1706 else {
1707 pmc = i;
1708 }
1709 } else if (abs(lc-clock/2) < tolerance) {
1710 // 32TO
1711 if((i - pmc) == lc) { /* 16T0 was previous one */
1712 /* It's a PMC ! */
1713 i += (128+127+16+32+33)-1;
1714 lastval = i;
1715 pmc = 0;
1716 block_done = 1;
1717 }
1718 else if(half_switch == 1) {
1719 BitStream[bitidx++] = 0;
1720 half_switch = 0;
1721 }
1722 else
1723 half_switch++;
1724 } else if (abs(lc-clock) < tolerance) {
1725 // 64TO
1726 BitStream[bitidx++] = 1;
1727 } else {
1728 // Error
1729 warnings++;
1730 if (warnings > 10)
1731 {
1732 Dbprintf("Error: too many detection errors, aborting.");
1733 return 0;
1734 }
1735 }
1736
1737 if(block_done == 1) {
1738 if(bitidx == 128) {
1739 for(j=0; j<16; j++) {
1740 Blocks[num_blocks][j] = 128*BitStream[j*8+7]+
1741 64*BitStream[j*8+6]+
1742 32*BitStream[j*8+5]+
1743 16*BitStream[j*8+4]+
1744 8*BitStream[j*8+3]+
1745 4*BitStream[j*8+2]+
1746 2*BitStream[j*8+1]+
1747 BitStream[j*8];
1748 }
1749 num_blocks++;
1750 }
1751 bitidx = 0;
1752 block_done = 0;
1753 half_switch = 0;
1754 }
1755 if (GraphBuffer[i-1] > GraphBuffer[i]) dir=0;
1756 else dir = 1;
1757 }
1758 if(bitidx==255)
1759 bitidx=0;
1760 warnings = 0;
1761 if(num_blocks == 4) break;
1762 }
1763 memcpy(outBlocks, Blocks, 16*num_blocks);
1764 return num_blocks;
1765 }
1766
1767 int IsBlock0PCF7931(uint8_t *Block) {
1768 // Assume RFU means 0 :)
1769 if((memcmp(Block, "\x00\x00\x00\x00\x00\x00\x00\x01", 8) == 0) && memcmp(Block+9, "\x00\x00\x00\x00\x00\x00\x00", 7) == 0) // PAC enabled
1770 return 1;
1771 if((memcmp(Block+9, "\x00\x00\x00\x00\x00\x00\x00", 7) == 0) && Block[7] == 0) // PAC disabled, can it *really* happen ?
1772 return 1;
1773 return 0;
1774 }
1775
1776 int IsBlock1PCF7931(uint8_t *Block) {
1777 // Assume RFU means 0 :)
1778 if(Block[10] == 0 && Block[11] == 0 && Block[12] == 0 && Block[13] == 0)
1779 if((Block[14] & 0x7f) <= 9 && Block[15] <= 9)
1780 return 1;
1781
1782 return 0;
1783 }
1784 #define ALLOC 16
1785
1786 void ReadPCF7931() {
1787 uint8_t Blocks[8][17];
1788 uint8_t tmpBlocks[4][16];
1789 int i, j, ind, ind2, n;
1790 int num_blocks = 0;
1791 int max_blocks = 8;
1792 int ident = 0;
1793 int error = 0;
1794 int tries = 0;
1795
1796 memset(Blocks, 0, 8*17*sizeof(uint8_t));
1797
1798 do {
1799 memset(tmpBlocks, 0, 4*16*sizeof(uint8_t));
1800 n = DemodPCF7931((uint8_t**)tmpBlocks);
1801 if(!n)
1802 error++;
1803 if(error==10 && num_blocks == 0) {
1804 Dbprintf("Error, no tag or bad tag");
1805 return;
1806 }
1807 else if (tries==20 || error==10) {
1808 Dbprintf("Error reading the tag");
1809 Dbprintf("Here is the partial content");
1810 goto end;
1811 }
1812
1813 for(i=0; i<n; i++)
1814 Dbprintf("(dbg) %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
1815 tmpBlocks[i][0], tmpBlocks[i][1], tmpBlocks[i][2], tmpBlocks[i][3], tmpBlocks[i][4], tmpBlocks[i][5], tmpBlocks[i][6], tmpBlocks[i][7],
1816 tmpBlocks[i][8], tmpBlocks[i][9], tmpBlocks[i][10], tmpBlocks[i][11], tmpBlocks[i][12], tmpBlocks[i][13], tmpBlocks[i][14], tmpBlocks[i][15]);
1817 if(!ident) {
1818 for(i=0; i<n; i++) {
1819 if(IsBlock0PCF7931(tmpBlocks[i])) {
1820 // Found block 0 ?
1821 if(i < n-1 && IsBlock1PCF7931(tmpBlocks[i+1])) {
1822 // Found block 1!
1823 // \o/
1824 ident = 1;
1825 memcpy(Blocks[0], tmpBlocks[i], 16);
1826 Blocks[0][ALLOC] = 1;
1827 memcpy(Blocks[1], tmpBlocks[i+1], 16);
1828 Blocks[1][ALLOC] = 1;
1829 max_blocks = max((Blocks[1][14] & 0x7f), Blocks[1][15]) + 1;
1830 // Debug print
1831 Dbprintf("(dbg) Max blocks: %d", max_blocks);
1832 num_blocks = 2;
1833 // Handle following blocks
1834 for(j=i+2, ind2=2; j!=i; j++, ind2++, num_blocks++) {
1835 if(j==n) j=0;
1836 if(j==i) break;
1837 memcpy(Blocks[ind2], tmpBlocks[j], 16);
1838 Blocks[ind2][ALLOC] = 1;
1839 }
1840 break;
1841 }
1842 }
1843 }
1844 }
1845 else {
1846 for(i=0; i<n; i++) { // Look for identical block in known blocks
1847 if(memcmp(tmpBlocks[i], "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 16)) { // Block is not full of 00
1848 for(j=0; j<max_blocks; j++) {
1849 if(Blocks[j][ALLOC] == 1 && !memcmp(tmpBlocks[i], Blocks[j], 16)) {
1850 // Found an identical block
1851 for(ind=i-1,ind2=j-1; ind >= 0; ind--,ind2--) {
1852 if(ind2 < 0)
1853 ind2 = max_blocks;
1854 if(!Blocks[ind2][ALLOC]) { // Block ind2 not already found
1855 // Dbprintf("Tmp %d -> Block %d", ind, ind2);
1856 memcpy(Blocks[ind2], tmpBlocks[ind], 16);
1857 Blocks[ind2][ALLOC] = 1;
1858 num_blocks++;
1859 if(num_blocks == max_blocks) goto end;
1860 }
1861 }
1862 for(ind=i+1,ind2=j+1; ind < n; ind++,ind2++) {
1863 if(ind2 > max_blocks)
1864 ind2 = 0;
1865 if(!Blocks[ind2][ALLOC]) { // Block ind2 not already found
1866 // Dbprintf("Tmp %d -> Block %d", ind, ind2);
1867 memcpy(Blocks[ind2], tmpBlocks[ind], 16);
1868 Blocks[ind2][ALLOC] = 1;
1869 num_blocks++;
1870 if(num_blocks == max_blocks) goto end;
1871 }
1872 }
1873 }
1874 }
1875 }
1876 }
1877 }
1878 tries++;
1879 if (BUTTON_PRESS()) return;
1880 } while (num_blocks != max_blocks);
1881 end:
1882 Dbprintf("-----------------------------------------");
1883 Dbprintf("Memory content:");
1884 Dbprintf("-----------------------------------------");
1885 for(i=0; i<max_blocks; i++) {
1886 if(Blocks[i][ALLOC]==1)
1887 Dbprintf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
1888 Blocks[i][0], Blocks[i][1], Blocks[i][2], Blocks[i][3], Blocks[i][4], Blocks[i][5], Blocks[i][6], Blocks[i][7],
1889 Blocks[i][8], Blocks[i][9], Blocks[i][10], Blocks[i][11], Blocks[i][12], Blocks[i][13], Blocks[i][14], Blocks[i][15]);
1890 else
1891 Dbprintf("<missing block %d>", i);
1892 }
1893 Dbprintf("-----------------------------------------");
1894
1895 return ;
1896 }
1897
1898
1899 //-----------------------------------
1900 // EM4469 / EM4305 routines
1901 //-----------------------------------
1902 #define FWD_CMD_LOGIN 0xC //including the even parity, binary mirrored
1903 #define FWD_CMD_WRITE 0xA
1904 #define FWD_CMD_READ 0x9
1905 #define FWD_CMD_DISABLE 0x5
1906
1907
1908 uint8_t forwardLink_data[64]; //array of forwarded bits
1909 uint8_t * forward_ptr; //ptr for forward message preparation
1910 uint8_t fwd_bit_sz; //forwardlink bit counter
1911 uint8_t * fwd_write_ptr; //forwardlink bit pointer
1912
1913 //====================================================================
1914 // prepares command bits
1915 // see EM4469 spec
1916 //====================================================================
1917 //--------------------------------------------------------------------
1918 uint8_t Prepare_Cmd( uint8_t cmd ) {
1919 //--------------------------------------------------------------------
1920
1921 *forward_ptr++ = 0; //start bit
1922 *forward_ptr++ = 0; //second pause for 4050 code
1923
1924 *forward_ptr++ = cmd;
1925 cmd >>= 1;
1926 *forward_ptr++ = cmd;
1927 cmd >>= 1;
1928 *forward_ptr++ = cmd;
1929 cmd >>= 1;
1930 *forward_ptr++ = cmd;
1931
1932 return 6; //return number of emited bits
1933 }
1934
1935 //====================================================================
1936 // prepares address bits
1937 // see EM4469 spec
1938 //====================================================================
1939
1940 //--------------------------------------------------------------------
1941 uint8_t Prepare_Addr( uint8_t addr ) {
1942 //--------------------------------------------------------------------
1943
1944 register uint8_t line_parity;
1945
1946 uint8_t i;
1947 line_parity = 0;
1948 for(i=0;i<6;i++) {
1949 *forward_ptr++ = addr;
1950 line_parity ^= addr;
1951 addr >>= 1;
1952 }
1953
1954 *forward_ptr++ = (line_parity & 1);
1955
1956 return 7; //return number of emited bits
1957 }
1958
1959 //====================================================================
1960 // prepares data bits intreleaved with parity bits
1961 // see EM4469 spec
1962 //====================================================================
1963
1964 //--------------------------------------------------------------------
1965 uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) {
1966 //--------------------------------------------------------------------
1967
1968 register uint8_t line_parity;
1969 register uint8_t column_parity;
1970 register uint8_t i, j;
1971 register uint16_t data;
1972
1973 data = data_low;
1974 column_parity = 0;
1975
1976 for(i=0; i<4; i++) {
1977 line_parity = 0;
1978 for(j=0; j<8; j++) {
1979 line_parity ^= data;
1980 column_parity ^= (data & 1) << j;
1981 *forward_ptr++ = data;
1982 data >>= 1;
1983 }
1984 *forward_ptr++ = line_parity;
1985 if(i == 1)
1986 data = data_hi;
1987 }
1988
1989 for(j=0; j<8; j++) {
1990 *forward_ptr++ = column_parity;
1991 column_parity >>= 1;
1992 }
1993 *forward_ptr = 0;
1994
1995 return 45; //return number of emited bits
1996 }
1997
1998 //====================================================================
1999 // Forward Link send function
2000 // Requires: forwarLink_data filled with valid bits (1 bit per byte)
2001 // fwd_bit_count set with number of bits to be sent
2002 //====================================================================
2003 void SendForward(uint8_t fwd_bit_count) {
2004
2005 fwd_write_ptr = forwardLink_data;
2006 fwd_bit_sz = fwd_bit_count;
2007
2008 LED_D_ON();
2009
2010 //Field on
2011 FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
2012 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
2013 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
2014
2015 // Give it a bit of time for the resonant antenna to settle.
2016 // And for the tag to fully power up
2017 SpinDelay(150);
2018
2019 // force 1st mod pulse (start gap must be longer for 4305)
2020 fwd_bit_sz--; //prepare next bit modulation
2021 fwd_write_ptr++;
2022 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2023 SpinDelayUs(55*8); //55 cycles off (8us each)for 4305
2024 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
2025 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
2026 SpinDelayUs(16*8); //16 cycles on (8us each)
2027
2028 // now start writting
2029 while(fwd_bit_sz-- > 0) { //prepare next bit modulation
2030 if(((*fwd_write_ptr++) & 1) == 1)
2031 SpinDelayUs(32*8); //32 cycles at 125Khz (8us each)
2032 else {
2033 //These timings work for 4469/4269/4305 (with the 55*8 above)
2034 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2035 SpinDelayUs(23*8); //16-4 cycles off (8us each)
2036 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
2037 FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
2038 SpinDelayUs(9*8); //16 cycles on (8us each)
2039 }
2040 }
2041 }
2042
2043
2044 void EM4xLogin(uint32_t Password) {
2045
2046 uint8_t fwd_bit_count;
2047
2048 forward_ptr = forwardLink_data;
2049 fwd_bit_count = Prepare_Cmd( FWD_CMD_LOGIN );
2050 fwd_bit_count += Prepare_Data( Password&0xFFFF, Password>>16 );
2051
2052 SendForward(fwd_bit_count);
2053
2054 //Wait for command to complete
2055 SpinDelay(20);
2056
2057 }
2058
2059 void EM4xReadWord(uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
2060
2061 uint8_t *dest = mifare_get_bigbufptr();
2062 uint16_t bufferlength = 16000;
2063 uint32_t i = 0;
2064
2065 // Clear destination buffer before sending the command 0x80 = average.
2066 memset(dest, 0x80, bufferlength);
2067
2068 uint8_t fwd_bit_count;
2069
2070 //If password mode do login
2071 if (PwdMode == 1) EM4xLogin(Pwd);
2072
2073 forward_ptr = forwardLink_data;
2074 fwd_bit_count = Prepare_Cmd( FWD_CMD_READ );
2075 fwd_bit_count += Prepare_Addr( Address );
2076
2077 // Connect the A/D to the peak-detected low-frequency path.
2078 SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
2079 // Now set up the SSC to get the ADC samples that are now streaming at us.
2080 FpgaSetupSsc();
2081
2082 SendForward(fwd_bit_count);
2083
2084 // Now do the acquisition
2085 i = 0;
2086 for(;;) {
2087 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
2088 AT91C_BASE_SSC->SSC_THR = 0x43;
2089 }
2090 if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
2091 dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
2092 ++i;
2093 if (i >= bufferlength) break;
2094 }
2095 }
2096
2097 cmd_send(CMD_ACK,0,0,0,0,0);
2098 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2099 LED_D_OFF();
2100 }
2101
2102 void EM4xWriteWord(uint32_t Data, uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
2103
2104 uint8_t fwd_bit_count;
2105
2106 //If password mode do login
2107 if (PwdMode == 1) EM4xLogin(Pwd);
2108
2109 forward_ptr = forwardLink_data;
2110 fwd_bit_count = Prepare_Cmd( FWD_CMD_WRITE );
2111 fwd_bit_count += Prepare_Addr( Address );
2112 fwd_bit_count += Prepare_Data( Data&0xFFFF, Data>>16 );
2113
2114 SendForward(fwd_bit_count);
2115
2116 //Wait for write to complete
2117 SpinDelay(20);
2118 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2119 LED_D_OFF();
2120 }
Proxmark3 GIT tracking
Impressum, Datenschutz