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