From: Martin Holst Swende <martin@swende.se>
Date: Fri, 24 Oct 2014 18:46:02 +0000 (+0200)
Subject: First try att merging with head
X-Git-Tag: v2.0.0-rc1~87^2~1^2~3
X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/commitdiff_plain/f97d4e2378e04397b3c884d10acb79725dee4e06?hp=-c

First try att merging with head
---

f97d4e2378e04397b3c884d10acb79725dee4e06
diff --combined armsrc/lfops.c
index 397ea847,6b131c26..d29ec375
--- a/armsrc/lfops.c
+++ b/armsrc/lfops.c
@@@ -15,41 -15,9 +15,46 @@@
  #include "crc16.h"
  #include "string.h"
  
 +// split into two routines so we can avoid timing issues after sending commands //
- void DoAcquisition125k_internal(bool silent)
++void DoAcquisition125k_internal(int trigger_threshold,bool silent)
 +{
 +	uint8_t *dest = (uint8_t *)BigBuf;
 +	int n = sizeof(BigBuf);
 +	int i;
 +
 +	memset(dest, 0, n);
 +	i = 0;
 +	for(;;) {
 +		if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
 +			AT91C_BASE_SSC->SSC_THR = 0x43;
 +			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 >= n) break;
++			if (trigger_threshold != -1 && dest[i] < trigger_threshold)
++				continue;
++			else
++				trigger_threshold = -1;
++			if (++i >= n) break;
 +		}
 +	}
- 	if( ! silent)
++	if(!silent)
 +	{
 +		Dbprintf("buffer samples: %02x %02x %02x %02x %02x %02x %02x %02x ...",
 +				dest[0], dest[1], dest[2], dest[3], dest[4], dest[5], dest[6], dest[7]);
++		
 +	}
 +}
- 
- void DoAcquisition125k(void)
++void DoAcquisition125k(int trigger_threshold)
 +{
- 	DoAcquisition125k_internal(false);
++	DoAcquisition125k_internal(trigger_threshold, false);
 +}
 +
- void SetupToAcquireRawAdcSamples(int divisor)
++//void SetupToAcquireRawAdcSamples(int divisor)
+ void LFSetupFPGAForADC(int divisor, bool lf_field)
  {
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	if ( (divisor == 1) || (divisor < 0) || (divisor > 255) )
  		FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
  	else if (divisor == 0)
@@@ -57,23 -25,54 +62,29 @@@
  	else
  		FpgaSendCommand(FPGA_CMD_SET_DIVISOR, divisor);
  
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | (lf_field ? FPGA_LF_ADC_READER_FIELD : 0));
  
  	// Connect the A/D to the peak-detected low-frequency path.
  	SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
- 
  	// Give it a bit of time for the resonant antenna to settle.
  	SpinDelay(50);
- 
  	// Now set up the SSC to get the ADC samples that are now streaming at us.
  	FpgaSetupSsc();
  }
  
  void AcquireRawAdcSamples125k(int divisor)
  {
- 	SetupToAcquireRawAdcSamples(divisor);
+ 	LFSetupFPGAForADC(divisor, true);
 -	DoAcquisition125k(-1);
 +	// Now call the acquisition routine
- 	DoAcquisition125k_internal(false);
++	DoAcquisition125k_internal(-1,false);
+ }
 -
+ void SnoopLFRawAdcSamples(int divisor, int trigger_threshold)
+ {
+ 	LFSetupFPGAForADC(divisor, false);
 -	DoAcquisition125k(trigger_threshold);
++	DoAcquisition125k(trigger_threshold, false);
+ }
+ 
 -// split into two routines so we can avoid timing issues after sending commands //
 -void DoAcquisition125k(int trigger_threshold)
 -{
 -	uint8_t *dest = (uint8_t *)BigBuf;
 -	int n = sizeof(BigBuf);
 -	int i;
+ 
 -	memset(dest, 0, n);
 -	i = 0;
 -	for(;;) {
 -		if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
 -			AT91C_BASE_SSC->SSC_THR = 0x43;
 -			LED_D_ON();
 -		}
 -		if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
 -			dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
 -			LED_D_OFF();
 -			if (trigger_threshold != -1 && dest[i] < trigger_threshold)
 -				continue;
 -			else
 -				trigger_threshold = -1;
 -			if (++i >= n) break;
 -		}
 -	}
 -	Dbprintf("buffer samples: %02x %02x %02x %02x %02x %02x %02x %02x ...",
 -			dest[0], dest[1], dest[2], dest[3], dest[4], dest[5], dest[6], dest[7]);
  }
  
  void ModThenAcquireRawAdcSamples125k(int delay_off, int period_0, int period_1, uint8_t *command)
@@@ -81,6 -80,7 +92,7 @@@
  	int at134khz;
  
  	/* Make sure the tag is reset */
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
  	SpinDelay(2500);
  
@@@ -95,7 -95,7 +107,7 @@@
  	else
  		FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
  
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
  
  	// Give it a bit of time for the resonant antenna to settle.
  	SpinDelay(50);
@@@ -115,7 -115,7 +127,7 @@@
  		else
  			FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
  
- 		FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 		FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
  		LED_D_ON();
  		if(*(command++) == '0')
  			SpinDelayUs(period_0);
@@@ -130,10 -130,10 +142,10 @@@
  	else
  		FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
  
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
  
  	// now do the read
- 	DoAcquisition125k();
+ 	DoAcquisition125k(-1);
  }
  
  /* blank r/w tag data stream
@@@ -170,6 -170,7 +182,7 @@@ void ReadTItag(void
  	uint32_t threshold = (sampleslo - sampleshi + 1)>>1;
  
  	// TI tags charge at 134.2Khz
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
  
  	// Place FPGA in passthrough mode, in this mode the CROSS_LO line
@@@ -377,6 -378,7 +390,7 @@@ void AcquireTiType(void
  // if not provided a valid crc will be computed from the data and written.
  void WriteTItag(uint32_t idhi, uint32_t idlo, uint16_t crc)
  {
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);	
  	if(crc == 0) {
  	 	crc = update_crc16(crc, (idlo)&0xff);
  		crc = update_crc16(crc, (idlo>>8)&0xff);
@@@ -448,6 -450,7 +462,7 @@@ void SimulateTagLowFrequency(int period
  	int i;
  	uint8_t *tab = (uint8_t *)BigBuf;
      
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT);
      
  	AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT | GPIO_SSC_CLK;
@@@ -606,213 -609,416 +621,212 @@@ void CmdHIDsimTAG(int hi, int lo, int l
  		LED_A_OFF();
  }
  
 -
 -// loop to capture raw HID waveform then FSK demodulate the TAG ID from it
 -void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
 +size_t fsk_demod(uint8_t * dest, size_t size)
  {
 -	uint8_t *dest = (uint8_t *)BigBuf;
 -	int m=0, n=0, i=0, idx=0, found=0, lastval=0;
 -  uint32_t hi2=0, hi=0, lo=0;
 +	uint32_t last_transition = 0;
 +	uint32_t idx = 1;
  
 -	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 -	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 -	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 +	// we don't care about actual value, only if it's more or less than a
 +	// threshold essentially we capture zero crossings for later analysis
 +	uint8_t threshold_value = 127;
  
- 
 -	// Connect the A/D to the peak-detected low-frequency path.
 -	SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
 +	// sync to first lo-hi transition, and threshold
  
 -	// Give it a bit of time for the resonant antenna to settle.
 -	SpinDelay(50);
 +	//Need to threshold first sample
 +	if(dest[0] < threshold_value) dest[0] = 0;
 +	else dest[0] = 1;
  
 -	// Now set up the SSC to get the ADC samples that are now streaming at us.
 -	FpgaSetupSsc();
 +	size_t numBits = 0;
 +	// count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
 +	// or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
 +	// between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
 +	for(idx = 1; idx < size; idx++) {
 +		// threshold current value
 +		if (dest[idx] < threshold_value) dest[idx] = 0;
 +		else dest[idx] = 1;
  
 -	for(;;) {
 -		WDT_HIT();
 -		if (ledcontrol)
 -			LED_A_ON();
 -		if(BUTTON_PRESS()) {
 -			DbpString("Stopped");
 -			if (ledcontrol)
 -				LED_A_OFF();
 -			return;
 -		}
 +		// Check for 0->1 transition
 +		if (dest[idx-1] < dest[idx]) { // 0 -> 1 transition
  
 -		i = 0;
 -		m = sizeof(BigBuf);
 -		memset(dest,128,m);
 -		for(;;) {
 -			if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
 -				AT91C_BASE_SSC->SSC_THR = 0x43;
 -				if (ledcontrol)
 -					LED_D_ON();
 -			}
 -			if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
 -				dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
 -				// we don't care about actual value, only if it's more or less than a
 -				// threshold essentially we capture zero crossings for later analysis
 -				if(dest[i] < 127) dest[i] = 0; else dest[i] = 1;
 -				i++;
 -				if (ledcontrol)
 -					LED_D_OFF();
 -				if(i >= m) {
 -					break;
 -				}
 +			if (idx-last_transition <  9) {
 +					dest[numBits]=1;
 +			} else {
 +					dest[numBits]=0;
  			}
 +			last_transition = idx;
 +			numBits++;
  		}
 +	}
 +	return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0
 +}
  
 -		// FSK demodulator
  
 -		// sync to first lo-hi transition
 -		for( idx=1; idx<m; idx++) {
 -			if (dest[idx-1]<dest[idx])
 -				lastval=idx;
 -				break;
 +size_t aggregate_bits(uint8_t *dest,size_t size, uint8_t h2l_crossing_value,uint8_t l2h_crossing_value, uint8_t maxConsequtiveBits )
 +{
 +	uint8_t lastval=dest[0];
 +	uint32_t idx=0;
 +	size_t numBits=0;
 +	uint32_t n=1;
 +
 +	for( idx=1; idx < size; idx++) {
 +
 +		if (dest[idx]==lastval) {
 +			n++;
 +			continue;
 +		}
 +		//if lastval was 1, we have a 1->0 crossing
 +		if ( dest[idx-1] ) {
 +			n=(n+1) / h2l_crossing_value;
 +		} else {// 0->1 crossing
 +			n=(n+1) / l2h_crossing_value;
 +		}
 +		if (n == 0) n = 1;
 +
 +		if(n < maxConsequtiveBits)
 +		{
 +			memset(dest+numBits, dest[idx-1] , n);
 +			numBits += n;
  		}
 +		n=0;
 +		lastval=dest[idx];
 +	}//end for
 +
 +	return numBits;
 +
 +}
 +// loop to capture raw HID waveform then FSK demodulate the TAG ID from it
 +void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
 +{
 +	uint8_t *dest = (uint8_t *)BigBuf;
 +
 +	size_t size=0,idx=0; //, found=0;
 +	uint32_t hi2=0, hi=0, lo=0;
 +
 +
 +	while(!BUTTON_PRESS()) {
 +
 +		// Configure to go in 125Khz listen mode
- 		SetupToAcquireRawAdcSamples(0);
++		LFSetupFPGAForADC(0, true)
 +
  		WDT_HIT();
 +		if (ledcontrol) LED_A_ON();
  
 -		// count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
 -		// or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
 -		// between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
 -		for( i=0; idx<m; idx++) {
 -			if (dest[idx-1]<dest[idx]) {
 -				dest[i]=idx-lastval;
 -				if (dest[i] <= 8) {
 -						dest[i]=1;
 -				} else {
 -						dest[i]=0;
 -				}
 +		DoAcquisition125k_internal(true);
 +		size  = sizeof(BigBuf);
  
 -				lastval=idx;
 -				i++;
 -			}
 -		}
 -		m=i;
 +		// FSK demodulator
 +		size = fsk_demod(dest, size);
  		WDT_HIT();
  
  		// we now have a set of cycle counts, loop over previous results and aggregate data into bit patterns
 -		lastval=dest[0];
 -		idx=0;
 -		i=0;
 -		n=0;
 -		for( idx=0; idx<m; idx++) {
 -			if (dest[idx]==lastval) {
 -				n++;
 -			} else {
 -				// a bit time is five fc/10 or six fc/8 cycles so figure out how many bits a pattern width represents,
 -				// an extra fc/8 pattern preceeds every 4 bits (about 200 cycles) just to complicate things but it gets
 -				// swallowed up by rounding
 -				// expected results are 1 or 2 bits, any more and it's an invalid manchester encoding
 -				// special start of frame markers use invalid manchester states (no transitions) by using sequences
 -				// like 111000
 -				if (dest[idx-1]) {
 -					n=(n+1)/6;			// fc/8 in sets of 6
 -				} else {
 -					n=(n+1)/5;			// fc/10 in sets of 5
 -				}
 -				switch (n) {			// stuff appropriate bits in buffer
 -					case 0:
 -					case 1:	// one bit
 -						dest[i++]=dest[idx-1];
 -						break;
 -					case 2: // two bits
 -						dest[i++]=dest[idx-1];
 -						dest[i++]=dest[idx-1];
 -						break;
 -					case 3: // 3 bit start of frame markers
 -						dest[i++]=dest[idx-1];
 -						dest[i++]=dest[idx-1];
 -						dest[i++]=dest[idx-1];
 -						break;
 -					// When a logic 0 is immediately followed by the start of the next transmisson
 -					// (special pattern) a pattern of 4 bit duration lengths is created.
 -					case 4:
 -						dest[i++]=dest[idx-1];
 -						dest[i++]=dest[idx-1];
 -						dest[i++]=dest[idx-1];
 -						dest[i++]=dest[idx-1];
 -						break;
 -					default:	// this shouldn't happen, don't stuff any bits
 -						break;
 -				}
 -				n=0;
 -				lastval=dest[idx];
 -			}
 -		}
 -		m=i;
 +		// 1->0 : fc/8 in sets of 6
 +		// 0->1 : fc/10 in sets of 5
 +		size = aggregate_bits(dest,size, 6,5,5);
 +
  		WDT_HIT();
  
  		// final loop, go over previously decoded manchester data and decode into usable tag ID
  		// 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
 -		for( idx=0; idx<m-6; idx++) {
 +		uint8_t frame_marker_mask[] = {1,1,1,0,0,0};
 +		int numshifts = 0;
 +		idx = 0;
 +		while( idx + sizeof(frame_marker_mask) < size) {
  			// search for a start of frame marker
 -			if ( dest[idx] && dest[idx+1] && dest[idx+2] && (!dest[idx+3]) && (!dest[idx+4]) && (!dest[idx+5]) )
 -			{
 -				found=1;
 -				idx+=6;
 -        if (found && (hi2|hi|lo)) {
 -          if (hi2 != 0){
 -            Dbprintf("TAG ID: %x%08x%08x (%d)",
 -                     (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 -          }
 -          else {
 -            Dbprintf("TAG ID: %x%08x (%d)",
 -                     (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 -          }
 -					/* if we're only looking for one tag */
 -					if (findone)
 -					{
 -						*high = hi;
 -						*low = lo;
 -						return;
 -					}
 -          hi2=0;
 -					hi=0;
 -					lo=0;
 -					found=0;
 -				}
 -			}
 -			if (found) {
 -				if (dest[idx] && (!dest[idx+1]) ) {
 -          hi2=(hi2<<1)|(hi>>31);
 -					hi=(hi<<1)|(lo>>31);
 -					lo=(lo<<1)|0;
 -				} else if ( (!dest[idx]) && dest[idx+1]) {
 -          hi2=(hi2<<1)|(hi>>31);
 -					hi=(hi<<1)|(lo>>31);
 -					lo=(lo<<1)|1;
 -				} else {
 -					found=0;
 -          hi2=0;
 -					hi=0;
 -					lo=0;
 +			if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
 +			{ // frame marker found
 +				idx+=sizeof(frame_marker_mask);
 +
 +				while(dest[idx] != dest[idx+1] && idx < size-2)
 +				{	// Keep going until next frame marker (or error)
 +					// Shift in a bit. Start by shifting high registers
 +					hi2 = (hi2<<1)|(hi>>31);
 +					hi = (hi<<1)|(lo>>31);
 +					//Then, shift in a 0 or one into low
 +					if (dest[idx] && !dest[idx+1])	// 1 0
 +						lo=(lo<<1)|0;
 +					else // 0 1
 +						lo=(lo<<1)|
 +								1;
 +					numshifts ++;
 +					idx += 2;
  				}
 -				idx++;
 -			}
 -			if ( dest[idx] && dest[idx+1] && dest[idx+2] && (!dest[idx+3]) && (!dest[idx+4]) && (!dest[idx+5]) )
 -			{
 -				found=1;
 -				idx+=6;
 -				if (found && (hi|lo)) {
 -          if (hi2 != 0){
 -            Dbprintf("TAG ID: %x%08x%08x (%d)",
 -                     (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 -          }
 -          else {
 -            Dbprintf("TAG ID: %x%08x (%d)",
 -                     (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 -          }
 -					/* if we're only looking for one tag */
 -					if (findone)
 -					{
 -						*high = hi;
 -						*low = lo;
 -						return;
 +				//Dbprintf("Num shifts: %d ", numshifts);
 +				// Hopefully, we read a tag and	 hit upon the next frame marker
 +				if ( memcmp(dest+idx, frame_marker_mask, sizeof(frame_marker_mask)) == 0)
 +				{
 +					if (hi2 != 0){
 +						Dbprintf("TAG ID: %x%08x%08x (%d)",
 +							 (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 +					}
 +					else {
 +						Dbprintf("TAG ID: %x%08x (%d)",
 +						 (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
  					}
 -          hi2=0;
 -					hi=0;
 -					lo=0;
 -					found=0;
  				}
 +
 +				// reset
 +				hi2 = hi = lo = 0;
 +				numshifts = 0;
 +			}else
 +			{
 +				idx++;
  			}
  		}
  		WDT_HIT();
 +
  	}
 +	DbpString("Stopped");
 +	if (ledcontrol) LED_A_OFF();
  }
  
 +uint32_t bytebits_to_byte(uint8_t* src, int numbits)
 +{
 +	uint32_t num = 0;
 +	for(int i = 0 ; i < numbits ; i++)
 +	{
 +		num = (num << 1) | (*src);
 +		src++;
 +	}
 +	return num;
 +}
 +
 +
  void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
  {
  	uint8_t *dest = (uint8_t *)BigBuf;
 -	int m=0, n=0, i=0, idx=0, lastval=0;
 -	int found=0;
 -	uint32_t code=0, code2=0;
 -	//uint32_t hi2=0, hi=0, lo=0;
  
 -	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 -	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 -	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 +	size_t size=0, idx=0;
 +	uint32_t code=0, code2=0;
  
 -	// Connect the A/D to the peak-detected low-frequency path.
 -	SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
  
 -	// Give it a bit of time for the resonant antenna to settle.
 -	SpinDelay(50);
 +	while(!BUTTON_PRESS()) {
  
 -	// Now set up the SSC to get the ADC samples that are now streaming at us.
 -	FpgaSetupSsc();
 +		// Configure to go in 125Khz listen mode
- 		SetupToAcquireRawAdcSamples(0);
++		LFSetupFPGAForADC(0, true);
  
 -	for(;;) {
  		WDT_HIT();
 -		if (ledcontrol)
 -			LED_A_ON();
 -		if(BUTTON_PRESS()) {
 -			DbpString("Stopped");
 -			if (ledcontrol)
 -				LED_A_OFF();
 -			return;
 -		}
 +		if (ledcontrol) LED_A_ON();
  
 -		i = 0;
 -		m = sizeof(BigBuf);
 -		memset(dest,128,m);
 -		for(;;) {
 -			if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
 -				AT91C_BASE_SSC->SSC_THR = 0x43;
 -				if (ledcontrol)
 -					LED_D_ON();
 -			}
 -			if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
 -				dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
 -				// we don't care about actual value, only if it's more or less than a
 -				// threshold essentially we capture zero crossings for later analysis
 -				if(dest[i] < 127) dest[i] = 0; else dest[i] = 1;
 -				i++;
 -				if (ledcontrol)
 -					LED_D_OFF();
 -				if(i >= m) {
 -					break;
 -				}
 -			}
 -		}
 +		DoAcquisition125k_internal(true);
 +		size  = sizeof(BigBuf);
  
  		// FSK demodulator
 -
 -		// sync to first lo-hi transition
 -		for( idx=1; idx<m; idx++) {
 -			if (dest[idx-1]<dest[idx])
 -				lastval=idx;
 -				break;
 -		}
 -		WDT_HIT();
 -
 -		// count cycles between consecutive lo-hi transitions, there should be either 8 (fc/8)
 -		// or 10 (fc/10) cycles but in practice due to noise etc we may end up with with anywhere
 -		// between 7 to 11 cycles so fuzz it by treat anything <9 as 8 and anything else as 10
 -		for( i=0; idx<m; idx++) {
 -			if (dest[idx-1]<dest[idx]) {
 -				dest[i]=idx-lastval;
 -				if (dest[i] <= 8) {
 -						dest[i]=1;
 -				} else {
 -						dest[i]=0;
 -				}
 -
 -				lastval=idx;
 -				i++;
 -			}
 -		}
 -		m=i;
 +		size = fsk_demod(dest, size);
  		WDT_HIT();
  
  		// we now have a set of cycle counts, loop over previous results and aggregate data into bit patterns
 -		lastval=dest[0];
 -		idx=0;
 -		i=0;
 -		n=0;
 -		for( idx=0; idx<m; idx++) {
 -			if (dest[idx]==lastval) {
 -				n++;
 -			} else {
 -				// a bit time is five fc/10 or six fc/8 cycles so figure out how many bits a pattern width represents,
 -				// an extra fc/8 pattern preceeds every 4 bits (about 200 cycles) just to complicate things but it gets
 -				// swallowed up by rounding
 -				// expected results are 1 or 2 bits, any more and it's an invalid manchester encoding
 -				// special start of frame markers use invalid manchester states (no transitions) by using sequences
 -				// like 111000
 -				if (dest[idx-1]) {
 -					n=(n+1)/7;			// fc/8 in sets of 7
 -				} else {
 -					n=(n+1)/6;			// fc/10 in sets of 6
 -				}
 -				switch (n) {			// stuff appropriate bits in buffer
 -					case 0:
 -					case 1:	// one bit
 -						dest[i++]=dest[idx-1]^1;
 -						//Dbprintf("%d",dest[idx-1]);
 -						break;
 -					case 2: // two bits
 -						dest[i++]=dest[idx-1]^1;
 -						dest[i++]=dest[idx-1]^1;
 -						//Dbprintf("%d",dest[idx-1]);
 -						//Dbprintf("%d",dest[idx-1]);
 -						break;
 -					case 3: // 3 bit start of frame markers
 -						for(int j=0; j<3; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 4:
 -						for(int j=0; j<4; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 5:
 -						for(int j=0; j<5; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 6:
 -						for(int j=0; j<6; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 7:
 -						for(int j=0; j<7; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 8:
 -						for(int j=0; j<8; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 9:
 -						for(int j=0; j<9; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 10:
 -						for(int j=0; j<10; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 11:
 -						for(int j=0; j<11; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						//  Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					case 12:
 -						for(int j=0; j<12; j++){
 -						  dest[i++]=dest[idx-1]^1;
 -						 // Dbprintf("%d",dest[idx-1]);
 -						}
 -						break;
 -					default:	// this shouldn't happen, don't stuff any bits
 -						//Dbprintf("%d",dest[idx-1]);
 -						break;
 -				}
 -				n=0;
 -				lastval=dest[idx];
 -			}
 -		}//end for
 -		/*for(int j=0; j<64;j+=8){
 -		  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]);
 -		}
 -		Dbprintf("\n");*/
 -		m=i;
 +		// 1->0 : fc/8 in sets of 7
 +		// 0->1 : fc/10 in sets of 6
 +		size = aggregate_bits(dest, size, 7,6,13);
 +
  		WDT_HIT();
  		
 -        for( idx=0; idx<m-9; idx++) {
 -	  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])){
 -		found=1;
 -		//idx+=9;
 -		if (found) {
 +		//Handle the data
 + 	    uint8_t mask[] = {0,0,0,0,0,0,0,0,0,1};
 +		for( idx=0; idx < size - 64; idx++) {
 +
 + 	    	if ( memcmp(dest + idx, mask, sizeof(mask)) ) continue;
 +
  		    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]);
  		    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]);			  
  		    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]);
@@@ -821,27 -1027,59 +835,27 @@@
  		    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]);
  		    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]);
  		    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]);
 -		
 -		    short version='\x00';
 -		    char unknown='\x00';
 -		    uint16_t number=0;
 -		    for(int j=14;j<18;j++){
 -		       //Dbprintf("%d",dest[idx+j]);
 -		       version <<=1;
 -		       if (dest[idx+j]) version |= 1;
 -		    }
 -		    for(int j=19;j<27;j++){
 -		       //Dbprintf("%d",dest[idx+j]);
 -		       unknown <<=1;
 -		       if (dest[idx+j]) unknown |= 1;
 -		    }
 -		    for(int j=36;j<45;j++){
 -		       //Dbprintf("%d",dest[idx+j]);
 -		       number <<=1;
 -		       if (dest[idx+j]) number |= 1;
 -		    }
 -		    for(int j=46;j<53;j++){
 -		       //Dbprintf("%d",dest[idx+j]);
 -		       number <<=1;
 -		       if (dest[idx+j]) number |= 1;
 -		    }
 -		    for(int j=0; j<32; j++){
 -			code <<=1;
 -			if(dest[idx+j]) code |= 1;
 -		    }
 -		    for(int j=32; j<64; j++){
 -			code2 <<=1;
 -			if(dest[idx+j]) code2 |= 1;
 -		    }
 +			
 +		    code = bytebits_to_byte(dest+idx,32);
 +		    code2 = bytebits_to_byte(dest+idx+32,32); 
 +
 +		    short version = bytebits_to_byte(dest+idx+14,4); 
 +		    char unknown = bytebits_to_byte(dest+idx+19,8) ;
 +		    uint16_t number = bytebits_to_byte(dest+idx+36,9); 
  		    
  		    Dbprintf("XSF(%02d)%02x:%d (%08x%08x)",version,unknown,number,code,code2);
 -		    if (ledcontrol)
 -			LED_D_OFF();
 -		}
 -		// if we're only looking for one tag 
 -		if (findone){
 -			//*high = hi;
 -			//*low = lo;
 -			LED_A_OFF();
 -			return;
 -		}
 -      
 -		//hi=0;
 -		//lo=0;
 -		found=0;
 -	  }
 +		    if (ledcontrol)	LED_D_OFF();
  		
 +			// if we're only looking for one tag 
 +			if (findone){
 +				LED_A_OFF();
 +				return;
 +			}		
 +		}
 +		WDT_HIT();
  	}
 -	}
 -	WDT_HIT();
 +	DbpString("Stopped");
 +	if (ledcontrol) LED_A_OFF();
  }
  
  /*------------------------------
@@@ -911,8 -1149,9 +925,9 @@@
  // Write one bit to card
  void T55xxWriteBit(int bit)
  {
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
  	if (bit == 0)
  		SpinDelayUs(WRITE_0);
  	else
@@@ -926,8 -1165,9 +941,9 @@@ void T55xxWriteBlock(uint32_t Data, uin
  {
  	unsigned int i;
  
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
  
  	// Give it a bit of time for the resonant antenna to settle.
  	// And for the tag to fully power up
@@@ -959,7 -1199,7 +975,7 @@@
  	// Now perform write (nominal is 5.6 ms for T55x7 and 18ms for E5550,
  	// so wait a little more)
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
  	SpinDelay(20);
  	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
  }
@@@ -970,6 -1210,7 +986,7 @@@ void T55xxReadBlock(uint32_t Block, uin
  	uint8_t *dest = (uint8_t *)BigBuf;
  	int m=0, i=0;
    
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	m = sizeof(BigBuf);
    // Clear destination buffer before sending the command
  	memset(dest, 128, m);
@@@ -980,7 -1221,7 +997,7 @@@
    
  	LED_D_ON();
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
    
  	// Give it a bit of time for the resonant antenna to settle.
  	// And for the tag to fully power up
@@@ -1006,7 -1247,7 +1023,7 @@@
    
    // Turn field on to read the response
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
    
  	// Now do the acquisition
  	i = 0;
@@@ -1034,6 -1275,7 +1051,7 @@@ void T55xxReadTrace(void)
  	uint8_t *dest = (uint8_t *)BigBuf;
  	int m=0, i=0;
    
+ 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  	m = sizeof(BigBuf);
    // Clear destination buffer before sending the command
  	memset(dest, 128, m);
@@@ -1044,7 -1286,7 +1062,7 @@@
    
  	LED_D_ON();
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
    
  	// Give it a bit of time for the resonant antenna to settle.
  	// And for the tag to fully power up
@@@ -1060,7 -1302,7 +1078,7 @@@
    
    // Turn field on to read the response
  	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
- 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+ 	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
    
  	// Now do the acquisition
  	i = 0;
@@@ -1749,8 -1991,9 +1767,9 @@@ void SendForward(uint8_t fwd_bit_count
    LED_D_ON();
    
    //Field on
+   FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
    FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
-   FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);
+   FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
    
    // Give it a bit of time for the resonant antenna to settle.
    // And for the tag to fully power up
@@@ -1762,7 -2005,7 +1781,7 @@@
    FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
    SpinDelayUs(55*8); //55 cycles off (8us each)for 4305
    FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
-   FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);//field on
+   FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
    SpinDelayUs(16*8); //16 cycles on (8us each)
    
    // now start writting
@@@ -1774,7 -2017,7 +1793,7 @@@
        FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
        SpinDelayUs(23*8); //16-4 cycles off (8us each)
        FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
-       FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_READER);//field on
+       FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
        SpinDelayUs(9*8); //16 cycles on (8us each)
      }
    }