X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/31b3689f0b54048de31957e001c60bbd8dfca5a3..ba765c9edcdbc624c9e77673acf5fa60daec9deb:/armsrc/lfops.c

diff --git a/armsrc/lfops.c b/armsrc/lfops.c
index f3daacc6..6eb89912 100644
--- a/armsrc/lfops.c
+++ b/armsrc/lfops.c
@@ -16,6 +16,7 @@
 #include "string.h"
 #include "lfdemod.h"
 #include "lfsampling.h"
+#include "usb_cdc.h"
 
 
 /**
@@ -72,8 +73,6 @@ void ModThenAcquireRawAdcSamples125k(int delay_off, int period_0, int period_1,
 	DoAcquisition_config(false);
 }
 
-
-
 /* blank r/w tag data stream
 ...0000000000000000 01111111
 1010101010101010101010101010101010101010101010101010101010101010
@@ -394,19 +393,19 @@ void SimulateTagLowFrequency(uint16_t period, uint32_t gap, uint8_t ledcontrol)
     AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
     AT91C_BASE_PIOA->PIO_ODR = GPIO_SSC_CLK;
 
-#define SHORT_COIL()	LOW(GPIO_SSC_DOUT)
-#define OPEN_COIL()		HIGH(GPIO_SSC_DOUT)
+ #define SHORT_COIL()	LOW(GPIO_SSC_DOUT)
+ #define OPEN_COIL()		HIGH(GPIO_SSC_DOUT)
 
     i = 0;
     for(;;) {
+        //wait until SSC_CLK goes HIGH
         while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)) {
-            if(BUTTON_PRESS()) {
+			if(BUTTON_PRESS() || usb_poll()) {
                 DbpString("Stopped");
                 return;
             }
             WDT_HIT();
         }
-
         if (ledcontrol)
             LED_D_ON();
 
@@ -417,17 +416,18 @@ void SimulateTagLowFrequency(uint16_t period, uint32_t gap, uint8_t ledcontrol)
 
         if (ledcontrol)
             LED_D_OFF();
-
+        //wait until SSC_CLK goes LOW
         while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK) {
             if(BUTTON_PRESS()) {
                 DbpString("Stopped");
                 return;
             }
             WDT_HIT();
-        }
-
+        }    
+               
         i++;
         if(i == period) {
+      
             i = 0;
             if (gap) {
                 SHORT_COIL();
@@ -442,8 +442,9 @@ void SimulateTagLowFrequencyBidir(int divisor, int t0)
 {
 }
 
-// compose fc/8 fc/10 waveform
-static void fc(int c, int *n) {
+// compose fc/8 fc/10 waveform (FSK2)
+static void fc(int c, int *n)
+{
     uint8_t *dest = BigBuf_get_addr();
     int idx;
 
@@ -451,20 +452,21 @@ static void fc(int c, int *n) {
     if(c==0) {
         dest[((*n)++)]=1;
         dest[((*n)++)]=1;
-        dest[((*n)++)]=0;
-        dest[((*n)++)]=0;
+        dest[((*n)++)]=1;
+        dest[((*n)++)]=1;
         dest[((*n)++)]=0;
         dest[((*n)++)]=0;
         dest[((*n)++)]=0;
         dest[((*n)++)]=0;
     }
-    //	an fc/8  encoded bit is a bit pattern of  11000000  x6 = 48 samples
+  
+    //	an fc/8  encoded bit is a bit pattern of  11110000  x6 = 48 samples
     if(c==8) {
         for (idx=0; idx<6; idx++) {
             dest[((*n)++)]=1;
             dest[((*n)++)]=1;
-            dest[((*n)++)]=0;
-            dest[((*n)++)]=0;
+            dest[((*n)++)]=1;
+            dest[((*n)++)]=1;
             dest[((*n)++)]=0;
             dest[((*n)++)]=0;
             dest[((*n)++)]=0;
@@ -472,14 +474,14 @@ static void fc(int c, int *n) {
         }
     }
 
-    //	an fc/10 encoded bit is a bit pattern of 1110000000 x5 = 50 samples
+    //	an fc/10 encoded bit is a bit pattern of 1111100000 x5 = 50 samples
     if(c==10) {
         for (idx=0; idx<5; idx++) {
             dest[((*n)++)]=1;
             dest[((*n)++)]=1;
             dest[((*n)++)]=1;
-            dest[((*n)++)]=0;
-            dest[((*n)++)]=0;
+            dest[((*n)++)]=1;
+            dest[((*n)++)]=1;
             dest[((*n)++)]=0;
             dest[((*n)++)]=0;
             dest[((*n)++)]=0;
@@ -488,6 +490,36 @@ static void fc(int c, int *n) {
         }
     }
 }
+// compose fc/X fc/Y waveform (FSKx)
+static void fcAll(uint8_t fc, int *n, uint8_t clock, uint16_t *modCnt) 
+{
+    uint8_t *dest = BigBuf_get_addr();
+    uint8_t halfFC = fc/2;
+    uint8_t wavesPerClock = clock/fc;
+    uint8_t mod = clock % fc;    //modifier
+    uint8_t modAdj = fc/mod;     //how often to apply modifier
+    bool modAdjOk = !(fc % mod); //if (fc % mod==0) modAdjOk=TRUE;
+    // loop through clock - step field clock
+    for (uint8_t idx=0; idx < wavesPerClock; idx++){
+        // put 1/2 FC length 1's and 1/2 0's per field clock wave (to create the wave)
+        memset(dest+(*n), 0, fc-halfFC);  //in case of odd number use extra here
+        memset(dest+(*n)+(fc-halfFC), 1, halfFC);
+        *n += fc;
+    }
+    if (mod>0) (*modCnt)++;
+    if ((mod>0) && modAdjOk){  //fsk2 
+        if ((*modCnt % modAdj) == 0){ //if 4th 8 length wave in a rf/50 add extra 8 length wave
+            memset(dest+(*n), 0, fc-halfFC);
+            memset(dest+(*n)+(fc-halfFC), 1, halfFC);
+            *n += fc;
+        }
+    }
+    if (mod>0 && !modAdjOk){  //fsk1
+        memset(dest+(*n), 0, mod-(mod/2));
+        memset(dest+(*n)+(mod-(mod/2)), 1, mod/2);
+        *n += mod;
+    }
+}
 
 // prepare a waveform pattern in the buffer based on the ID given then
 // simulate a HID tag until the button is pressed
@@ -505,7 +537,7 @@ void CmdHIDsimTAG(int hi, int lo, int ledcontrol)
     */
 
     if (hi>0xFFF) {
-        DbpString("Tags can only have 44 bits.");
+        DbpString("Tags can only have 44 bits. - USE lf simfsk for larger tags");
         return;
     }
     fc(0,&n);
@@ -545,6 +577,178 @@ void CmdHIDsimTAG(int hi, int lo, int ledcontrol)
         LED_A_OFF();
 }
 
+// prepare a waveform pattern in the buffer based on the ID given then
+// simulate a FSK tag until the button is pressed
+// arg1 contains fcHigh and fcLow, arg2 contains invert and clock
+void CmdFSKsimTAG(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
+{
+    int ledcontrol=1;
+    int n=0, i=0;
+    uint8_t fcHigh = arg1 >> 8;
+    uint8_t fcLow = arg1 & 0xFF;
+    uint16_t modCnt = 0;
+    uint8_t clk = arg2 & 0xFF;
+    uint8_t invert = (arg2 >> 8) & 1;
+
+    for (i=0; i<size; i++){
+        if (BitStream[i] == invert){
+            fcAll(fcLow, &n, clk, &modCnt);
+        } else {
+            fcAll(fcHigh, &n, clk, &modCnt);
+        }
+    }
+    Dbprintf("Simulating with fcHigh: %d, fcLow: %d, clk: %d, invert: %d, n: %d",fcHigh, fcLow, clk, invert, n);
+    /*Dbprintf("DEBUG: First 32:");
+    uint8_t *dest = BigBuf_get_addr();
+    i=0;
+    Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
+    i+=16;
+    Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
+    */     
+    if (ledcontrol)
+        LED_A_ON();
+
+    SimulateTagLowFrequency(n, 0, ledcontrol);
+
+    if (ledcontrol)
+        LED_A_OFF();
+}
+
+// compose ask waveform for one bit(ASK)
+static void askSimBit(uint8_t c, int *n, uint8_t clock, uint8_t manchester) 
+{
+    uint8_t *dest = BigBuf_get_addr();
+    uint8_t halfClk = clock/2;
+    // c = current bit 1 or 0
+    if (manchester==1){
+        memset(dest+(*n), c, halfClk);
+        memset(dest+(*n) + halfClk, c^1, halfClk);
+    } else {
+        memset(dest+(*n), c, clock);
+    }
+    *n += clock;        
+}
+
+static void biphaseSimBit(uint8_t c, int *n, uint8_t clock, uint8_t *phase)
+{
+    uint8_t *dest = BigBuf_get_addr();
+    uint8_t halfClk = clock/2;
+    if (c){
+        memset(dest+(*n), c ^ 1 ^ *phase, halfClk);
+        memset(dest+(*n) + halfClk, c ^ *phase, halfClk);        
+    } else {
+        memset(dest+(*n), c ^ *phase, clock);
+        *phase ^= 1;
+    }
+
+}
+
+// args clock, ask/man or askraw, invert, transmission separator
+void CmdASKsimTag(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
+{
+    int ledcontrol = 1;
+    int n=0, i=0;
+    uint8_t clk = (arg1 >> 8) & 0xFF;
+    uint8_t encoding = arg1 & 1;
+    uint8_t separator = arg2 & 1;
+    uint8_t invert = (arg2 >> 8) & 1;
+
+    if (encoding==2){  //biphase
+        uint8_t phase=0;
+    for (i=0; i<size; i++){
+            biphaseSimBit(BitStream[i]^invert, &n, clk, &phase);
+    }
+        if (BitStream[0]==BitStream[size-1]){ //run a second set inverted to keep phase in check
+        for (i=0; i<size; i++){
+                biphaseSimBit(BitStream[i]^invert, &n, clk, &phase);
+            }
+        }
+    } else {  // ask/manchester || ask/raw
+        for (i=0; i<size; i++){
+            askSimBit(BitStream[i]^invert, &n, clk, encoding);
+        }
+        if (encoding==0 && BitStream[0]==BitStream[size-1]){ //run a second set inverted (for biphase phase)
+            for (i=0; i<size; i++){
+                askSimBit(BitStream[i]^invert^1, &n, clk, encoding);
+        }    
+        }    
+    }
+    
+    if (separator==1) Dbprintf("sorry but separator option not yet available"); 
+
+    Dbprintf("Simulating with clk: %d, invert: %d, encoding: %d, separator: %d, n: %d",clk, invert, encoding, separator, n);
+    //DEBUG
+    //Dbprintf("First 32:");
+    //uint8_t *dest = BigBuf_get_addr();
+    //i=0;
+    //Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
+    //i+=16;
+    //Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
+
+    if (ledcontrol)
+        LED_A_ON();
+    
+    SimulateTagLowFrequency(n, 0, ledcontrol);
+
+    if (ledcontrol)
+        LED_A_OFF();
+}
+
+//carrier can be 2,4 or 8
+static void pskSimBit(uint8_t waveLen, int *n, uint8_t clk, uint8_t *curPhase, bool phaseChg)
+{
+    uint8_t *dest = BigBuf_get_addr();
+    uint8_t halfWave = waveLen/2;
+    //uint8_t idx;
+    int i = 0;
+    if (phaseChg){
+        // write phase change
+        memset(dest+(*n), *curPhase^1, halfWave);
+        memset(dest+(*n) + halfWave, *curPhase, halfWave);
+        *n += waveLen;
+        *curPhase ^= 1;
+        i += waveLen;
+    }
+    //write each normal clock wave for the clock duration
+    for (; i < clk; i+=waveLen){
+        memset(dest+(*n), *curPhase, halfWave);
+        memset(dest+(*n) + halfWave, *curPhase^1, halfWave);
+        *n += waveLen;
+    }
+}
+
+// args clock, carrier, invert,
+void CmdPSKsimTag(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
+{
+    int ledcontrol=1;
+    int n=0, i=0;
+    uint8_t clk = arg1 >> 8;
+    uint8_t carrier = arg1 & 0xFF;
+    uint8_t invert = arg2 & 0xFF;
+    uint8_t curPhase = 0;
+    for (i=0; i<size; i++){
+        if (BitStream[i] == curPhase){
+            pskSimBit(carrier, &n, clk, &curPhase, FALSE);
+        } else {
+            pskSimBit(carrier, &n, clk, &curPhase, TRUE);
+        }            
+    }
+    Dbprintf("Simulating with Carrier: %d, clk: %d, invert: %d, n: %d",carrier, clk, invert, n);
+    //Dbprintf("DEBUG: First 32:");
+    //uint8_t *dest = BigBuf_get_addr();
+    //i=0;
+    //Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
+    //i+=16;
+    //Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
+           
+    if (ledcontrol)
+        LED_A_ON();
+    SimulateTagLowFrequency(n, 0, ledcontrol);
+
+    if (ledcontrol)
+        LED_A_OFF();
+}
+
 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
 void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
 {
@@ -641,6 +845,7 @@ void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol)
 
 	size_t size=0, idx=0;
     int clk=0, invert=0, errCnt=0, maxErr=20;
+    uint32_t hi=0;
     uint64_t lo=0;
     // Configure to go in 125Khz listen mode
     LFSetupFPGAForADC(95, true);
@@ -659,9 +864,18 @@ void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol)
         WDT_HIT();
 
         if (errCnt>=0){
-			lo = Em410xDecode(dest, &size, &idx);
+            errCnt = Em410xDecode(dest, &size, &idx, &hi, &lo);
             //Dbprintf("DEBUG: EM GOT");
-            if (lo>0){
+            if (errCnt){
+                if (size>64){
+                    Dbprintf("EM XL TAG ID: %06x%08x%08x - (%05d_%03d_%08d)",
+                        hi,
+                        (uint32_t)(lo>>32),
+                        (uint32_t)lo,
+                        (uint32_t)(lo&0xFFFF),
+                        (uint32_t)((lo>>16LL) & 0xFF),
+                        (uint32_t)(lo & 0xFFFFFF));
+                } else {
 				Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)",
 				    (uint32_t)(lo>>32),
 				    (uint32_t)lo,
@@ -669,6 +883,7 @@ void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol)
 				    (uint32_t)((lo>>16LL) & 0xFF),
 				    (uint32_t)(lo & 0xFFFFFF));
             }
+            }
             if (findone){
                 if (ledcontrol)	LED_A_OFF();
                 *high=lo>>32;
@@ -697,6 +912,8 @@ void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
     uint8_t version=0;
     uint8_t facilitycode=0;
     uint16_t number=0;
+	uint8_t crc = 0;
+	uint16_t calccrc = 0;
     // Configure to go in 125Khz listen mode
     LFSetupFPGAForADC(95, true);
 
@@ -715,8 +932,17 @@ void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
             //|           |           |           |           |           |           |
             //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
             //-----------------------------------------------------------------------------
-            //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
+            //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 checksum 11
             //
+			//Checksum:  
+			//00000000 0 11110000 1 11100000 1 00000001 1 00000011 1 10110110 1 01110101 11
+			//preamble      F0         E0         01         03         B6         75
+			// How to calc checksum,
+			// http://www.proxmark.org/forum/viewtopic.php?id=364&p=6
+			//   F0 + E0 + 01 + 03 + B6 = 28A
+			//   28A & FF = 8A
+			//   FF - 8A = 75
+			// Checksum: 0x75
             //XSF(version)facility:codeone+codetwo
             //Handle the data
             if(findone){ //only print binary if we are doing one
@@ -734,7 +960,15 @@ void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
             facilitycode = bytebits_to_byte(dest+idx+18,8) ;
             number = (bytebits_to_byte(dest+idx+36,8)<<8)|(bytebits_to_byte(dest+idx+45,8)); //36,9
 
-            Dbprintf("XSF(%02d)%02x:%05d (%08x%08x)",version,facilitycode,number,code,code2);
+			crc = bytebits_to_byte(dest+idx+54,8);
+			for (uint8_t i=1; i<6; ++i)
+				calccrc += bytebits_to_byte(dest+idx+9*i,8);
+			calccrc &= 0xff;
+			calccrc = 0xff - calccrc;
+			
+			char *crcStr = (crc == calccrc) ? "ok":"!crc";
+
+            Dbprintf("IO Prox XSF(%02d)%02x:%05d (%08x%08x)  [%02x %s]",version,facilitycode,number,code,code2, crc, crcStr);
             // if we're only looking for one tag
             if (findone){
                 if (ledcontrol)	LED_A_OFF();
@@ -813,7 +1047,7 @@ void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
  * To compensate antenna falling times shorten the write times
  * and enlarge the gap ones.
  */
-#define START_GAP 30*8 // 10 - 50fc 250
+#define START_GAP 50*8 // 10 - 50fc 250
 #define WRITE_GAP 20*8 //  8 - 30fc
 #define WRITE_0   24*8 // 16 - 31fc 24fc 192
 #define WRITE_1   54*8 // 48 - 63fc 54fc 432 for T55x7; 448 for E5550
@@ -826,6 +1060,12 @@ void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
 //  These timings work for 4469/4269/4305 (with the 55*8 above)
 //  WRITE_0 = 23*8 , 9*8  SpinDelayUs(23*8); 
 
+// Sam7s has several timers, we will use the source TIMER_CLOCK1 (aka AT91C_TC_CLKS_TIMER_DIV1_CLOCK)
+// TIMER_CLOCK1 = MCK/2, MCK is running at 48 MHz, Timer is running at 48/2 = 24 MHz
+// Hitag units (T0) have duration of 8 microseconds (us), which is 1/125000 per second (carrier)
+// T0 = TIMER_CLOCK1 / 125000 = 192
+// 1 Cycle = 8 microseconds(us)
+
 #define T55xx_SAMPLES_SIZE		12000 // 32 x 32 x 10  (32 bit times numofblock (7), times clock skip..)
 
 // Write one bit to card
@@ -845,11 +1085,11 @@ void T55xxWriteBit(int bit)
 // Write one card block in page 0, no lock
 void T55xxWriteBlock(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
 {
-	uint32_t i = 0;
+    uint32_t i = 0;
 
-	// Set up FPGA, 125kHz
-	// Wait for config.. (192+8190xPOW)x8 == 67ms
-	LFSetupFPGAForADC(0, true);
+    // Set up FPGA, 125kHz
+    // Wait for config.. (192+8190xPOW)x8 == 67ms
+    LFSetupFPGAForADC(0, true);
 
     // Now start writting
     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
@@ -882,19 +1122,28 @@ void T55xxWriteBlock(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t PwdMod
     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 }
 
+void TurnReadLFOn(){
+    FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
+    // Give it a bit of time for the resonant antenna to settle.
+    SpinDelayUs(8*150);
+}
+
+
 // Read one card block in page 0
 void T55xxReadBlock(uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
 {
-    uint8_t *dest = BigBuf_get_addr();
-    //uint16_t bufferlength = BigBuf_max_traceLen();
-	uint16_t bufferlength = T55xx_SAMPLES_SIZE;
     uint32_t i = 0;
-	// Clear destination buffer before sending the command  0x80 = average.
-	memset(dest, 0x80, bufferlength);	   
-	
-	// Set up FPGA, 125kHz
-	// Wait for config.. (192+8190xPOW)x8 == 67ms
-	LFSetupFPGAForADC(0, true);
+    uint8_t *dest = BigBuf_get_addr();
+    uint16_t bufferlength = BigBuf_max_traceLen();
+    if ( bufferlength > T55xx_SAMPLES_SIZE )
+        bufferlength = T55xx_SAMPLES_SIZE;
+
+    // Clear destination buffer before sending the command
+    memset(dest, 0x80, bufferlength);
+
+    // Set up FPGA, 125kHz
+    // Wait for config.. (192+8190xPOW)x8 == 67ms
+    LFSetupFPGAForADC(0, true);
     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
     SpinDelayUs(START_GAP);
 
@@ -913,40 +1162,40 @@ void T55xxReadBlock(uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
         T55xxWriteBit(Block & i);
 
     // Turn field on to read the response
-	TurnReadLFOn();
-
+    TurnReadLFOn();
     // Now do the acquisition
     i = 0;
     for(;;) {
         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
             AT91C_BASE_SSC->SSC_THR = 0x43;
-			//AT91C_BASE_SSC->SSC_THR = 0xff;
-			LED_D_ON();
+            LED_D_ON();
         }
         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
-			++i;
-			LED_D_OFF();
-			if (i >= bufferlength) break;
+            i++;
+            LED_D_OFF();
+            if (i >= bufferlength) break;
         }
     }
 
-	cmd_send(CMD_ACK,0,0,0,0,0);
+    cmd_send(CMD_ACK,0,0,0,0,0);    
     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
     LED_D_OFF();
 }
 
 // Read card traceability data (page 1)
 void T55xxReadTrace(void){
+    
+    uint32_t i = 0;
     uint8_t *dest = BigBuf_get_addr();
-    //uint16_t bufferlength = BigBuf_max_traceLen();
-	uint16_t bufferlength = T55xx_SAMPLES_SIZE;
-	uint32_t i = 0;
+    uint16_t bufferlength = BigBuf_max_traceLen();
+    if ( bufferlength > T55xx_SAMPLES_SIZE )
+        bufferlength= T55xx_SAMPLES_SIZE;
 
-	// Clear destination buffer before sending the command 0x80 = average
-	memset(dest, 0x80, bufferlength);  
-  
-	LFSetupFPGAForADC(0, true);
+    // Clear destination buffer before sending the command
+    memset(dest, 0x80, bufferlength);
+
+    LFSetupFPGAForADC(0, true);
     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
     SpinDelayUs(START_GAP);
 
@@ -955,34 +1204,35 @@ void T55xxReadTrace(void){
     T55xxWriteBit(1); //Page 1
 
     // Turn field on to read the response
-	TurnReadLFOn();
+    TurnReadLFOn();
 
     // Now do the acquisition
     for(;;) {
         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
             AT91C_BASE_SSC->SSC_THR = 0x43;
-			LED_D_ON();
+            LED_D_ON();
         }
         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
-			++i;
-			LED_D_OFF();
-		
-			if (i >= bufferlength) break;
+            i++;
+            LED_D_OFF();
+
+            if (i >= bufferlength) break;
 		}
 	}
   
 	cmd_send(CMD_ACK,0,0,0,0,0);
+    cmd_send(CMD_ACK,0,0,0,0,0);
     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
     LED_D_OFF();
 }
 
 void TurnReadLFOn(){
-	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
+	//FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 	// Give it a bit of time for the resonant antenna to settle.
 	//SpinDelay(30);
-	SpinDelayUs(9*150);
+	SpinDelayUs(8*150);
 }
 
 /*-------------- Cloning routines -----------*/
@@ -1287,10 +1537,16 @@ void CopyIndala224toT55x7(int uid1, int uid2, int uid3, int uid4, int uid5, int
 #define max(x,y) ( x<y ? y:x)
 
 int DemodPCF7931(uint8_t **outBlocks) {
-    uint8_t BitStream[256];
-    uint8_t Blocks[8][16];
-    uint8_t *GraphBuffer = BigBuf_get_addr();
-    int GraphTraceLen = BigBuf_max_traceLen();
+
+    uint8_t bits[256] = {0x00};
+	uint8_t blocks[8][16];
+    uint8_t *dest = BigBuf_get_addr();
+    
+	int GraphTraceLen = BigBuf_max_traceLen();
+	if (  GraphTraceLen > 18000 )
+		GraphTraceLen = 18000;
+	
+	
     int i, j, lastval, bitidx, half_switch;
     int clock = 64;
     int tolerance = clock / 8;
@@ -1301,8 +1557,7 @@ int DemodPCF7931(uint8_t **outBlocks) {
     uint8_t dir;
 
 	LFSetupFPGAForADC(95, true);
-	DoAcquisition_default(0, 0);
-
+	DoAcquisition_default(0, true);
 
     lmin = 64;
     lmax = 192;
@@ -1310,9 +1565,9 @@ int DemodPCF7931(uint8_t **outBlocks) {
     i = 2;
 
     /* Find first local max/min */
-    if(GraphBuffer[1] > GraphBuffer[0]) {
+    if(dest[1] > dest[0]) {
         while(i < GraphTraceLen) {
-            if( !(GraphBuffer[i] > GraphBuffer[i-1]) && GraphBuffer[i] > lmax)
+            if( !(dest[i] > dest[i-1]) && dest[i] > lmax)
                 break;
             i++;
         }
@@ -1320,7 +1575,7 @@ int DemodPCF7931(uint8_t **outBlocks) {
     }
     else {
         while(i < GraphTraceLen) {
-            if( !(GraphBuffer[i] < GraphBuffer[i-1]) && GraphBuffer[i] < lmin)
+            if( !(dest[i] < dest[i-1]) && dest[i] < lmin)
                 break;
             i++;
         }
@@ -1334,7 +1589,7 @@ int DemodPCF7931(uint8_t **outBlocks) {
 
     for (bitidx = 0; i < GraphTraceLen; i++)
     {
-        if ( (GraphBuffer[i-1] > GraphBuffer[i] && dir == 1 && GraphBuffer[i] > lmax) || (GraphBuffer[i-1] < GraphBuffer[i] && dir == 0 && GraphBuffer[i] < lmin))
+        if ( (dest[i-1] > dest[i] && dir == 1 && dest[i] > lmax) || (dest[i-1] < dest[i] && dir == 0 && dest[i] < lmin))
         {
             lc = i - lastval;
             lastval = i;
@@ -1363,14 +1618,14 @@ int DemodPCF7931(uint8_t **outBlocks) {
                     block_done = 1;
                 }
                 else if(half_switch == 1) {
-                    BitStream[bitidx++] = 0;
+                    bits[bitidx++] = 0;
                     half_switch = 0;
                 }
                 else
                     half_switch++;
             } else if (abs(lc-clock) < tolerance) {
                 // 64TO
-                BitStream[bitidx++] = 1;
+                bits[bitidx++] = 1;
             } else {
                 // Error
                 warnings++;
@@ -1384,14 +1639,15 @@ int DemodPCF7931(uint8_t **outBlocks) {
             if(block_done == 1) {
                 if(bitidx == 128) {
                     for(j=0; j<16; j++) {
-                        Blocks[num_blocks][j] = 128*BitStream[j*8+7]+
-                                64*BitStream[j*8+6]+
-                                32*BitStream[j*8+5]+
-                                16*BitStream[j*8+4]+
-                                8*BitStream[j*8+3]+
-                                4*BitStream[j*8+2]+
-                                2*BitStream[j*8+1]+
-                                BitStream[j*8];
+                        blocks[num_blocks][j] = 128*bits[j*8+7]+
+                                64*bits[j*8+6]+
+                                32*bits[j*8+5]+
+                                16*bits[j*8+4]+
+                                8*bits[j*8+3]+
+                                4*bits[j*8+2]+
+                                2*bits[j*8+1]+
+                                bits[j*8];
+						
                     }
                     num_blocks++;
                 }
@@ -1400,17 +1656,14 @@ int DemodPCF7931(uint8_t **outBlocks) {
                 half_switch = 0;
             }
             if(i < GraphTraceLen)
-            {
-                if (GraphBuffer[i-1] > GraphBuffer[i]) dir=0;
-                else dir = 1;
-            }
+                dir =(dest[i-1] > dest[i]) ? 0 : 1;
         }
         if(bitidx==255)
             bitidx=0;
         warnings = 0;
         if(num_blocks == 4) break;
     }
-    memcpy(outBlocks, Blocks, 16*num_blocks);
+    memcpy(outBlocks, blocks, 16*num_blocks);
     return num_blocks;
 }
 
@@ -1529,7 +1782,7 @@ void ReadPCF7931() {
         tries++;
         if (BUTTON_PRESS()) return;
     } while (num_blocks != max_blocks);
-end:
+ end:
     Dbprintf("-----------------------------------------");
     Dbprintf("Memory content:");
     Dbprintf("-----------------------------------------");