From 6426f6ba86c8a2d3e390fab1ced35223a92d8647 Mon Sep 17 00:00:00 2001 From: iceman1001 Date: Fri, 20 Nov 2015 16:56:43 +0100 Subject: [PATCH] @marshmellows last LF changes. - wipe a t55x7 tag - stable demods - --- armsrc/lfops.c | 13 +- armsrc/lfsampling.c | 24 ++- client/cmddata.c | 13 +- client/cmdlf.c | 17 +- client/cmdlft55xx.c | 224 +++++++++++++++++-------- client/cmdlft55xx.h | 5 +- client/cmdlfviking.c | 6 +- common/lfdemod.c | 389 ++++++++++++++++++++++--------------------- common/lfdemod.h | 2 +- 9 files changed, 410 insertions(+), 283 deletions(-) diff --git a/armsrc/lfops.c b/armsrc/lfops.c index 4a9e07b1..00b50852 100644 --- a/armsrc/lfops.c +++ b/armsrc/lfops.c @@ -1065,7 +1065,7 @@ void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol) #define WRITE_GAP 20*8 // was 160 // SPEC: 1*8 to 20*8 - typ 10*8 (or 10fc) #define WRITE_0 16*8 // was 144 // SPEC: 16*8 to 32*8 - typ 24*8 (or 24fc) #define WRITE_1 50*8 // was 400 // SPEC: 48*8 to 64*8 - typ 56*8 (or 56fc) 432 for T55x7; 448 for E5550 -#define READ_GAP 52*8 +#define READ_GAP 15*8 // VALUES TAKEN FROM EM4x function: SendForward // START_GAP = 440; (55*8) cycles at 125Khz (8us = 1cycle) @@ -1183,7 +1183,7 @@ void T55xxWriteBlock(uint32_t Data, uint8_t Block, uint32_t Pwd, uint8_t arg) { cmd_send(CMD_ACK,0,0,0,0,0); } -// Read one card block in page 0 +// Read one card block in page [page] void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) { LED_A_ON(); bool PwdMode = arg0 & 0x1; @@ -1463,6 +1463,15 @@ uint8_t * fwd_write_ptr; //forwardlink bit pointer // prepares command bits // see EM4469 spec //==================================================================== +//-------------------------------------------------------------------- +// VALUES TAKEN FROM EM4x function: SendForward +// START_GAP = 440; (55*8) cycles at 125Khz (8us = 1cycle) +// WRITE_GAP = 128; (16*8) +// WRITE_1 = 256 32*8; (32*8) + +// These timings work for 4469/4269/4305 (with the 55*8 above) +// WRITE_0 = 23*8 , 9*8 SpinDelayUs(23*8); + uint8_t Prepare_Cmd( uint8_t cmd ) { *forward_ptr++ = 0; //start bit diff --git a/armsrc/lfsampling.c b/armsrc/lfsampling.c index 6eecf407..8931f41d 100644 --- a/armsrc/lfsampling.c +++ b/armsrc/lfsampling.c @@ -254,7 +254,8 @@ uint32_t SnoopLF() { **/ void doT55x7Acquisition(size_t sample_size) { - #define T55xx_READ_UPPER_THRESHOLD 128+40 // 40 grph + #define T55xx_READ_UPPER_THRESHOLD 128+60 // 60 grph + #define T55xx_READ_LOWER_THRESHOLD 128-60 // -60 grph #define T55xx_READ_TOL 5 uint8_t *dest = BigBuf_get_addr(); @@ -266,8 +267,9 @@ void doT55x7Acquisition(size_t sample_size) { uint16_t i = 0; bool startFound = false; bool highFound = false; + bool lowFound = false; uint8_t curSample = 0; - uint8_t firstSample = 0; + uint8_t lastSample = 0; uint16_t skipCnt = 0; while(!BUTTON_PRESS() && !usb_poll_validate_length() && skipCnt<1000) { WDT_HIT(); @@ -281,19 +283,29 @@ void doT55x7Acquisition(size_t sample_size) { // skip until the first high sample above threshold if (!startFound && curSample > T55xx_READ_UPPER_THRESHOLD) { - if (curSample > firstSample) - firstSample = curSample; + //if (curSample > lastSample) + // lastSample = curSample; highFound = true; } else if (!highFound) { skipCnt++; continue; } + // skip until the first Low sample below threshold + if (!startFound && curSample < T55xx_READ_LOWER_THRESHOLD) { + //if (curSample > lastSample) + lastSample = curSample; + lowFound = true; + } else if (!lowFound) { + skipCnt++; + continue; + } + // skip until first high samples begin to change - if (startFound || curSample < firstSample-T55xx_READ_TOL){ + if (startFound || curSample > T55xx_READ_LOWER_THRESHOLD+T55xx_READ_TOL){ // if just found start - recover last sample if (!startFound) { - dest[i++] = firstSample; + dest[i++] = lastSample; startFound = true; } // collect samples diff --git a/client/cmddata.c b/client/cmddata.c index 7244c614..b141b6f8 100644 --- a/client/cmddata.c +++ b/client/cmddata.c @@ -1495,6 +1495,10 @@ int CmdFDXBdemodBI(const char *Cmd){ if (g_debugMode) PrintAndLog("Error FDXBDemod , no startmarker found :: %d",preambleIndex); return 0; } + if (size != 128) { + if (g_debugMode) PrintAndLog("Error incorrect data length found"); + return 0; + } setDemodBuf(BitStream, 128, preambleIndex); @@ -1564,6 +1568,9 @@ int PSKDemod(const char *Cmd, bool verbose) //invalid carrier return 0; } + if (g_debugMode){ + PrintAndLog("Carrier: rf/%d",carrier); + } int errCnt=0; errCnt = pskRawDemod(BitStream, &BitLen, &clk, &invert); if (errCnt > maxErr){ @@ -1605,7 +1612,7 @@ int CmdIndalaDecode(const char *Cmd) uint8_t invert=0; size_t size = DemodBufferLen; size_t startIdx = indala26decode(DemodBuffer, &size, &invert); - if (startIdx < 1) { + if (startIdx < 1 || size > 224) { if (g_debugMode==1) PrintAndLog("Error2: %d",ans); return -1; @@ -1622,7 +1629,7 @@ int CmdIndalaDecode(const char *Cmd) uid2=bytebits_to_byte(DemodBuffer+32,32); if (DemodBufferLen==64){ PrintAndLog("Indala UID=%s (%x%08x)", sprint_bin(DemodBuffer,DemodBufferLen), uid1, uid2); - } else { + } else { uid3=bytebits_to_byte(DemodBuffer+64,32); uid4=bytebits_to_byte(DemodBuffer+96,32); uid5=bytebits_to_byte(DemodBuffer+128,32); @@ -1699,7 +1706,7 @@ int NRZrawDemod(const char *Cmd, bool verbose) size_t BitLen = getFromGraphBuf(BitStream); if (BitLen==0) return 0; int errCnt=0; - errCnt = nrzRawDemod(BitStream, &BitLen, &clk, &invert, maxErr); + errCnt = nrzRawDemod(BitStream, &BitLen, &clk, &invert); if (errCnt > maxErr){ if (g_debugMode) PrintAndLog("Too many errors found, clk: %d, invert: %d, numbits: %d, errCnt: %d",clk,invert,BitLen,errCnt); return 0; diff --git a/client/cmdlf.c b/client/cmdlf.c index 4cf9c815..ebf8242b 100644 --- a/client/cmdlf.c +++ b/client/cmdlf.c @@ -294,9 +294,12 @@ int CmdIndalaDemod(const char *Cmd) int rawbit = 0; int worst = 0, worstPos = 0; // PrintAndLog("Expecting a bit less than %d raw bits", GraphTraceLen / 32); + + // loop through raw signal - since we know it is psk1 rf/32 fc/2 skip every other value (+=2) for (i = 0; i < GraphTraceLen-1; i += 2) { count += 1; if ((GraphBuffer[i] > GraphBuffer[i + 1]) && (state != 1)) { + // appears redundant - marshmellow if (state == 0) { for (j = 0; j < count - 8; j += 16) { rawbits[rawbit++] = 0; @@ -309,6 +312,7 @@ int CmdIndalaDemod(const char *Cmd) state = 1; count = 0; } else if ((GraphBuffer[i] < GraphBuffer[i + 1]) && (state != 0)) { + //appears redundant if (state == 1) { for (j = 0; j < count - 8; j += 16) { rawbits[rawbit++] = 1; @@ -1098,13 +1102,6 @@ int CmdLFfind(const char *Cmd) return 1; } - //add psk and indala - ans=CmdIndalaDecode(""); - if (ans>0) { - PrintAndLog("\nValid Indala ID Found!"); - return 1; - } - ans=CmdAskEM410xDemod(""); if (ans>0) { PrintAndLog("\nValid EM410x ID Found!"); @@ -1135,6 +1132,12 @@ int CmdLFfind(const char *Cmd) return 1; } + ans=CmdIndalaDecode(""); + if (ans>0) { + PrintAndLog("\nValid Indala ID Found!"); + return 1; + } + ans=CmdPSKNexWatch(""); if (ans>0) { PrintAndLog("\nValid NexWatch ID Found!"); diff --git a/client/cmdlft55xx.c b/client/cmdlft55xx.c index 9a981080..31958eea 100644 --- a/client/cmdlft55xx.c +++ b/client/cmdlft55xx.c @@ -33,7 +33,7 @@ #define REGULAR_READ_MODE_BLOCK 0xFF // Default configuration -t55xx_conf_block_t config = { .modulation = DEMOD_ASK, .inverted = FALSE, .offset = 0x00, .block0 = 0x00}; +t55xx_conf_block_t config = { .modulation = DEMOD_ASK, .inverted = FALSE, .offset = 0x00, .block0 = 0x00, .Q5 = FALSE }; t55xx_conf_block_t Get_t55xx_Config(){ return config; @@ -43,13 +43,14 @@ void Set_t55xx_Config(t55xx_conf_block_t conf){ } int usage_t55xx_config(){ - PrintAndLog("Usage: lf t55xx config [d ] [i 1] [o ]"); + PrintAndLog("Usage: lf t55xx config [d ] [i 1] [o ] [Q5]"); PrintAndLog("Options:"); PrintAndLog(" h This help"); PrintAndLog(" b <8|16|32|40|50|64|100|128> Set bitrate"); PrintAndLog(" d Set demodulation FSK / ASK / PSK / NRZ / Biphase / Biphase A"); PrintAndLog(" i [1] Invert data signal, defaults to normal"); PrintAndLog(" o [offset] Set offset, where data should start decode in bitstream"); + PrintAndLog(" Q5 Set as Q5(T5555) chip instead of T55x7"); PrintAndLog(""); PrintAndLog("Examples:"); PrintAndLog(" lf t55xx config d FSK - FSK demodulation"); @@ -149,16 +150,24 @@ int usage_t55xx_wakup(){ static int CmdHelp(const char *Cmd); +void printT5xxHeader(uint8_t page){ + PrintAndLog("Reading Page %d:", page); + PrintAndLog("blk | hex data | binary"); + PrintAndLog("----+----------+---------------------------------"); +} + int CmdT55xxSetConfig(const char *Cmd) { uint8_t offset = 0; - bool errors = FALSE; - uint8_t cmdp = 0; char modulation[5] = {0x00}; char tmp = 0x00; uint8_t bitRate = 0; uint8_t rates[9] = {8,16,32,40,50,64,100,128,0}; - while(param_getchar(Cmd, cmdp) != 0x00 && !errors) { + uint8_t cmdp = 0; + config.Q5 = FALSE; + bool errors = FALSE; + while(param_getchar(Cmd, cmdp) != 0x00 && !errors) + { tmp = param_getchar(Cmd, cmdp); switch(tmp) { @@ -228,6 +237,11 @@ int CmdT55xxSetConfig(const char *Cmd) { config.offset = offset; cmdp+=2; break; + case 'Q': + case 'q': + config.Q5 = TRUE; + cmdp++; + break; default: PrintAndLog("Unknown parameter '%c'", param_getchar(Cmd, cmdp)); errors = TRUE; @@ -270,7 +284,7 @@ int T55xxReadBlock(uint8_t block, bool page1, bool usepwd, bool override, uint32 if (!DecodeT55xxBlock()) return 0; char blk[10]={0}; - sprintf(blk,"%d", block); + sprintf(blk,"%02d", block); printT55xxBlock(blk); return 1; } @@ -320,8 +334,8 @@ int CmdT55xxReadBlock(const char *Cmd) { PrintAndLog("Block must be between 0 and 7"); return 0; } - PrintAndLog("Reading Page %d:", page1); - PrintAndLog("blk | hex data | binary"); + + printT5xxHeader(page1); return T55xxReadBlock(block, page1, usepwd, override, password); } @@ -349,16 +363,20 @@ bool DecodeT55xxBlock(){ ans = FSKrawDemod(cmdStr, FALSE); break; case DEMOD_ASK: - snprintf(cmdStr, sizeof(buf),"%d %d 0", bitRate[config.bitrate], config.inverted ); + snprintf(cmdStr, sizeof(buf),"%d %d 1", bitRate[config.bitrate], config.inverted ); ans = ASKDemod(cmdStr, FALSE, FALSE, 1); break; case DEMOD_PSK1: - snprintf(cmdStr, sizeof(buf),"%d %d 0", bitRate[config.bitrate], config.inverted ); + // skip first 160 samples to allow antenna to settle in (psk gets inverted occasionally otherwise) + CmdLtrim("160"); + snprintf(cmdStr, sizeof(buf),"%d %d 6", bitRate[config.bitrate], config.inverted ); ans = PSKDemod(cmdStr, FALSE); break; case DEMOD_PSK2: //inverted won't affect this case DEMOD_PSK3: //not fully implemented - snprintf(cmdStr, sizeof(buf),"%d 0 1", bitRate[config.bitrate] ); + // skip first 160 samples to allow antenna to settle in (psk gets inverted occasionally otherwise) + CmdLtrim("160"); + snprintf(cmdStr, sizeof(buf),"%d 0 6", bitRate[config.bitrate] ); ans = PSKDemod(cmdStr, FALSE); psk1TOpsk2(DemodBuffer, DemodBufferLen); break; @@ -368,7 +386,7 @@ bool DecodeT55xxBlock(){ break; case DEMOD_BI: case DEMOD_BIa: - snprintf(cmdStr, sizeof(buf),"0 %d %d 0", bitRate[config.bitrate], config.inverted ); + snprintf(cmdStr, sizeof(buf),"0 %d %d 1", bitRate[config.bitrate], config.inverted ); ans = ASKbiphaseDemod(cmdStr, FALSE); break; default: @@ -379,15 +397,15 @@ bool DecodeT55xxBlock(){ int CmdT55xxDetect(const char *Cmd){ - bool override = false; + //bool override = false; //bool pwdmode = false; uint32_t password = 0; //default to blank Block 7 bool usepwd = ( strlen(Cmd) > 0); if ( usepwd ){ password = param_get32ex(Cmd, 0, 0, 16); - if (param_getchar(Cmd, 1) =='o' ) - override = true; + // if (param_getchar(Cmd, 1) =='o' ) + // override = true; } char cmdp = param_getchar(Cmd, 0); @@ -395,7 +413,7 @@ int CmdT55xxDetect(const char *Cmd){ if (strlen(Cmd)==0) { password = param_get32ex(Cmd, 0, 0, 16); - if (param_getchar(Cmd, 1) =='o' ) override = true; + //if (param_getchar(Cmd, 1) =='o' ) override = true; } if ( !AquireData(T55x7_PAGE0, T55x7_CONFIGURATION_BLOCK, usepwd, password) ) @@ -414,9 +432,10 @@ bool tryDetectModulation(){ int bitRate=0; uint8_t fc1 = 0, fc2 = 0, clk=0; save_restoreGB(1); + if (GetFskClock("", FALSE, FALSE)){ fskClocks(&fc1, &fc2, &clk, FALSE); - if ( FSKrawDemod("0 0", FALSE) && test(DEMOD_FSK, &tests[hits].offset, &bitRate)){ + if ( FSKrawDemod("0 0", FALSE) && test(DEMOD_FSK, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)){ tests[hits].modulation = DEMOD_FSK; if (fc1==8 && fc2 == 5) tests[hits].modulation = DEMOD_FSK1a; @@ -427,7 +446,7 @@ bool tryDetectModulation(){ tests[hits].block0 = PackBits(tests[hits].offset, 32, DemodBuffer); ++hits; } - if ( FSKrawDemod("0 1", FALSE) && test(DEMOD_FSK, &tests[hits].offset, &bitRate)) { + if ( FSKrawDemod("0 1", FALSE) && test(DEMOD_FSK, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_FSK; if (fc1 == 8 && fc2 == 5) tests[hits].modulation = DEMOD_FSK1; @@ -442,28 +461,28 @@ bool tryDetectModulation(){ } else { clk = GetAskClock("", FALSE, FALSE); if (clk>0) { - if ( ASKDemod("0 0 0", FALSE, FALSE, 1) && test(DEMOD_ASK, &tests[hits].offset, &bitRate)) { + if ( ASKDemod("0 0 1", TRUE, FALSE, 1) && test(DEMOD_ASK, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_ASK; tests[hits].bitrate = bitRate; tests[hits].inverted = FALSE; tests[hits].block0 = PackBits(tests[hits].offset, 32, DemodBuffer); ++hits; } - if ( ASKDemod("0 1 0", FALSE, FALSE, 1) && test(DEMOD_ASK, &tests[hits].offset, &bitRate)) { + if ( ASKDemod("0 1 1", TRUE, FALSE, 1) && test(DEMOD_ASK, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_ASK; tests[hits].bitrate = bitRate; tests[hits].inverted = TRUE; tests[hits].block0 = PackBits(tests[hits].offset, 32, DemodBuffer); ++hits; } - if ( ASKbiphaseDemod("0 0 0 0", FALSE) && test(DEMOD_BI, &tests[hits].offset, &bitRate) ) { + if ( ASKbiphaseDemod("0 0 0 2", FALSE) && test(DEMOD_BI, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5) ) { tests[hits].modulation = DEMOD_BI; tests[hits].bitrate = bitRate; tests[hits].inverted = FALSE; tests[hits].block0 = PackBits(tests[hits].offset, 32, DemodBuffer); ++hits; } - if ( ASKbiphaseDemod("0 0 1 0", FALSE) && test(DEMOD_BIa, &tests[hits].offset, &bitRate) ) { + if ( ASKbiphaseDemod("0 0 1 2", FALSE) && test(DEMOD_BIa, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5) ) { tests[hits].modulation = DEMOD_BIa; tests[hits].bitrate = bitRate; tests[hits].inverted = TRUE; @@ -475,7 +494,7 @@ bool tryDetectModulation(){ save_restoreGB(0); clk = GetNrzClock("", FALSE, FALSE); if (clk>0) { - if ( NRZrawDemod("0 0 1", FALSE) && test(DEMOD_NRZ, &tests[hits].offset, &bitRate)) { + if ( NRZrawDemod("0 0 1", FALSE) && test(DEMOD_NRZ, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_NRZ; tests[hits].bitrate = bitRate; tests[hits].inverted = FALSE; @@ -483,7 +502,7 @@ bool tryDetectModulation(){ ++hits; } - if ( NRZrawDemod("0 1 1", FALSE) && test(DEMOD_NRZ, &tests[hits].offset, &bitRate)) { + if ( NRZrawDemod("0 1 1", FALSE) && test(DEMOD_NRZ, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_NRZ; tests[hits].bitrate = bitRate; tests[hits].inverted = TRUE; @@ -494,16 +513,18 @@ bool tryDetectModulation(){ //undo trim from nrz save_restoreGB(0); + // skip first 160 samples to allow antenna to settle in (psk gets inverted occasionally otherwise) + CmdLtrim("160"); clk = GetPskClock("", FALSE, FALSE); if (clk>0) { - if ( PSKDemod("0 0 1", FALSE) && test(DEMOD_PSK1, &tests[hits].offset, &bitRate)) { + if ( PSKDemod("0 0 6", FALSE) && test(DEMOD_PSK1, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_PSK1; tests[hits].bitrate = bitRate; tests[hits].inverted = FALSE; tests[hits].block0 = PackBits(tests[hits].offset, 32, DemodBuffer); ++hits; } - if ( PSKDemod("0 1 1", FALSE) && test(DEMOD_PSK1, &tests[hits].offset, &bitRate)) { + if ( PSKDemod("0 1 6", FALSE) && test(DEMOD_PSK1, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)) { tests[hits].modulation = DEMOD_PSK1; tests[hits].bitrate = bitRate; tests[hits].inverted = TRUE; @@ -511,9 +532,9 @@ bool tryDetectModulation(){ ++hits; } // PSK2 - needs a call to psk1TOpsk2. - if ( PSKDemod("0 0 1", FALSE)) { + if ( PSKDemod("0 0 6", FALSE)) { psk1TOpsk2(DemodBuffer, DemodBufferLen); - if (test(DEMOD_PSK2, &tests[hits].offset, &bitRate)){ + if (test(DEMOD_PSK2, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)){ tests[hits].modulation = DEMOD_PSK2; tests[hits].bitrate = bitRate; tests[hits].inverted = FALSE; @@ -522,9 +543,9 @@ bool tryDetectModulation(){ } } // inverse waves does not affect this demod // PSK3 - needs a call to psk1TOpsk2. - if ( PSKDemod("0 0 1", FALSE)) { + if ( PSKDemod("0 0 6", FALSE)) { psk1TOpsk2(DemodBuffer, DemodBufferLen); - if (test(DEMOD_PSK3, &tests[hits].offset, &bitRate)){ + if (test(DEMOD_PSK3, &tests[hits].offset, &bitRate, clk, &tests[hits].Q5)){ tests[hits].modulation = DEMOD_PSK3; tests[hits].bitrate = bitRate; tests[hits].inverted = FALSE; @@ -534,6 +555,7 @@ bool tryDetectModulation(){ } // inverse waves does not affect this demod } } + save_restoreGB(0); if ( hits == 1) { config.modulation = tests[0].modulation; config.bitrate = tests[0].bitrate; @@ -586,37 +608,28 @@ bool testModulation(uint8_t mode, uint8_t modread){ return FALSE; } -bool testBitRate(uint8_t readRate, uint8_t mod){ - uint8_t expected[8] = {8, 16, 32, 40, 50, 64, 100, 128}; - uint8_t detRate = 0; - switch( mod ){ +bool testQ5Modulation(uint8_t mode, uint8_t modread){ + switch( mode ){ case DEMOD_FSK: - case DEMOD_FSK1: - case DEMOD_FSK1a: - case DEMOD_FSK2: - case DEMOD_FSK2a: - detRate = GetFskClock("",FALSE, FALSE); - if (expected[readRate] == detRate) - return TRUE; + if (modread >= 4 && modread <= 5) return TRUE; break; case DEMOD_ASK: - case DEMOD_BI: - case DEMOD_BIa: - detRate = GetAskClock("",FALSE, FALSE); - if (expected[readRate] == detRate) - return TRUE; + if (modread == 0) return TRUE; break; case DEMOD_PSK1: + if (modread == 1) return TRUE; + break; case DEMOD_PSK2: + if (modread == 2) return TRUE; + break; case DEMOD_PSK3: - detRate = GetPskClock("",FALSE, FALSE); - if (expected[readRate] == detRate) - return TRUE; + if (modread == 3) return TRUE; break; case DEMOD_NRZ: - detRate = GetNrzClock("",FALSE, FALSE); - if (expected[readRate] == detRate) - return TRUE; + if (modread == 7) return TRUE; + break; + case DEMOD_BI: + if (modread == 6) return TRUE; break; default: return FALSE; @@ -624,13 +637,60 @@ bool testBitRate(uint8_t readRate, uint8_t mod){ return FALSE; } -bool test(uint8_t mode, uint8_t *offset, int *fndBitRate){ +bool testQ5(uint8_t mode, uint8_t *offset, int *fndBitRate, uint8_t clk){ + + if ( DemodBufferLen < 64 ) return FALSE; + uint8_t si = 0; + for (uint8_t idx = 28; idx < 64; idx++){ + si = idx; + if ( PackBits(si, 28, DemodBuffer) == 0x00 ) continue; + + uint8_t safer = PackBits(si, 4, DemodBuffer); si += 4; //master key + uint8_t resv = PackBits(si, 8, DemodBuffer); si += 8; + // 2nibble must be zeroed. + if (safer != 0x6) continue; + if ( resv > 0x00) continue; + //uint8_t pageSel = PackBits(si, 1, DemodBuffer); si += 1; + //uint8_t fastWrite = PackBits(si, 1, DemodBuffer); si += 1; + si += 1+1; + int bitRate = PackBits(si, 5, DemodBuffer)*2 + 2; si += 5; //bit rate + if (bitRate > 128 || bitRate < 8) continue; + + //uint8_t AOR = PackBits(si, 1, DemodBuffer); si += 1; + //uint8_t PWD = PackBits(si, 1, DemodBuffer); si += 1; + //uint8_t pskcr = PackBits(si, 2, DemodBuffer); si += 2; //could check psk cr + //uint8_t inverse = PackBits(si, 1, DemodBuffer); si += 1; + si += 1+1+2+1; + uint8_t modread = PackBits(si, 3, DemodBuffer); si += 3; + uint8_t maxBlk = PackBits(si, 3, DemodBuffer); si += 3; + //uint8_t ST = PackBits(si, 1, DemodBuffer); si += 1; + if (maxBlk == 0) continue; + //test modulation + if (!testQ5Modulation(mode, modread)) continue; + if (bitRate != clk) continue; + *fndBitRate = bitRate; + *offset = idx; + + return TRUE; + } + return FALSE; +} + +bool testBitRate(uint8_t readRate, uint8_t clk){ + uint8_t expected[] = {8, 16, 32, 40, 50, 64, 100, 128}; + if (expected[readRate] == clk) + return true; + + return false; +} + +bool test(uint8_t mode, uint8_t *offset, int *fndBitRate, uint8_t clk, bool *Q5){ if ( DemodBufferLen < 64 ) return FALSE; uint8_t si = 0; - for (uint8_t idx = 0; idx < 64; idx++){ + for (uint8_t idx = 28; idx < 64; idx++){ si = idx; - if ( PackBits(si, 32, DemodBuffer) == 0x00 ) continue; + if ( PackBits(si, 28, DemodBuffer) == 0x00 ) continue; uint8_t safer = PackBits(si, 4, DemodBuffer); si += 4; //master key uint8_t resv = PackBits(si, 4, DemodBuffer); si += 4; //was 7 & +=7+3 //should be only 4 bits if extended mode @@ -655,9 +715,14 @@ bool test(uint8_t mode, uint8_t *offset, int *fndBitRate){ } //test modulation if (!testModulation(mode, modread)) continue; - if (!testBitRate(bitRate, mode)) continue; + if (!testBitRate(bitRate, clk)) continue; *fndBitRate = bitRate; *offset = idx; + *Q5 = FALSE; + return TRUE; + } + if (testQ5(mode, offset, fndBitRate, clk)) { + *Q5 = TRUE; return TRUE; } return FALSE; @@ -681,7 +746,8 @@ void printT55xxBlock(const char *blockNum){ bits[i - config.offset] = DemodBuffer[i]; blockData = PackBits(0, 32, bits); - PrintAndLog("%s | %08X | %s", blockNum, blockData, sprint_bin(bits,32)); + + PrintAndLog(" %s | %08X | %s", blockNum, blockData, sprint_bin(bits,32)); } int special(const char *Cmd) { @@ -704,6 +770,7 @@ int special(const char *Cmd) { } int printConfiguration( t55xx_conf_block_t b){ + PrintAndLog("Chip Type : %s", (b.Q5) ? "T5555(Q5)" : "T55x7"); PrintAndLog("Modulation : %s", GetSelectedModulationStr(b.modulation) ); PrintAndLog("Bit Rate : %s", GetBitRateStr(b.bitrate) ); PrintAndLog("Inverted : %s", (b.inverted) ? "Yes" : "No" ); @@ -851,11 +918,11 @@ int CmdT55xxReadTrace(const char *Cmd) { else year += 2010; + if (config.Q5) PrintAndLog("*** Warning *** Info read off a Q5 will not work as expected"); if ( acl != 0xE0 ) { PrintAndLog("The modulation is most likely wrong since the ACL is not 0xE0. "); return 0; } - PrintAndLog(""); PrintAndLog("-- T55xx Trace Information ----------------------------------"); PrintAndLog("-------------------------------------------------------------"); @@ -931,7 +998,7 @@ int CmdT55xxInfo(const char *Cmd){ uint32_t fw = PackBits(si, 1, DemodBuffer); si += 1; uint32_t inv = PackBits(si, 1, DemodBuffer); si += 1; uint32_t por = PackBits(si, 1, DemodBuffer); si += 1; - + if (config.Q5) PrintAndLog("*** Warning *** Config Info read off a Q5 will not display as expected"); PrintAndLog(""); PrintAndLog("-- T55xx Configuration & Tag Information --------------------"); PrintAndLog("-------------------------------------------------------------"); @@ -971,16 +1038,14 @@ int CmdT55xxDump(const char *Cmd){ override = true; } - PrintAndLog("Reading Page 0:"); - PrintAndLog("blk | hex data | binary"); - for ( uint8_t i = 0; i < 8; ++i){ + printT5xxHeader(0); + for ( uint8_t i = 0; i < 8; ++i) T55xxReadBlock(i, 0, usepwd, override, password); - } - PrintAndLog("Reading Page 1:"); - PrintAndLog("blk | hex data | binary"); - for ( uint8_t i = 0; i < 4; i++){ + + printT5xxHeader(1); + for ( uint8_t i = 0; i < 4; i++) T55xxReadBlock(i, 1, usepwd, override, password); - } + return 1; } @@ -1203,6 +1268,26 @@ int CmdResetRead(const char *Cmd) { return 1; } +int CmdT55xxWipe(const char *Cmd) { + char writeData[20] = {0}; + char *ptrData = writeData; + uint8_t blk = 0; + PrintAndLog("\nBeginning Wipe of a T55xx tag (assuming the tag is not password protected)\n"); + //try with the default password to reset block 0 (with a pwd should work even if pwd bit not set) + snprintf(ptrData,sizeof(writeData),"b %d d 00088040 p 0", blk); + if (!CmdT55xxWriteBlock(ptrData)){ + PrintAndLog("Error writing blk %d", blk); + } + blk = 1; + for (; blk<8; blk++) { + snprintf(ptrData,sizeof(writeData),"b %d d 0", blk); + if (!CmdT55xxWriteBlock(ptrData)){ + PrintAndLog("Error writing blk %d", blk); + } + } + return 0; +} + static command_t CommandTable[] = { {"help", CmdHelp, 1, "This help"}, @@ -1211,11 +1296,12 @@ static command_t CommandTable[] = {"read", CmdT55xxReadBlock, 0, "b p [password] [o] [1] -- Read T55xx block data. Optional [p password], [override], [page1]"}, {"resetread",CmdResetRead, 0, "Send Reset Cmd then lf read the stream to attempt to identify the start of it (needs a demod and/or plot after)"}, {"write", CmdT55xxWriteBlock,0, "b d p [password] [1] -- Write T55xx block data. Optional [p password], [page1]"}, - {"trace", CmdT55xxReadTrace, 0, "[1] Show T55xx traceability data (page 1/ blk 0-1)"}, - {"info", CmdT55xxInfo, 0, "[1] Show T55xx configuration data (page 0/ blk 0)"}, + {"trace", CmdT55xxReadTrace, 0, "[1] Show T55x7 traceability data (page 1/ blk 0-1)"}, + {"info", CmdT55xxInfo, 0, "[1] Show T55x7 configuration data (page 0/ blk 0)"}, {"dump", CmdT55xxDump, 0, "[password] [o] Dump T55xx card block 0-7. Optional [password], [override]"}, {"special", special, 0, "Show block changes with 64 different offsets"}, {"wakeup", CmdT55xxWakeUp, 0, "Send AOR wakeup command"}, + {"wipe", CmdT55xxWipe, 0, "Wipe a T55xx tag and set defaults (will destroy any data on tag)"}, {NULL, NULL, 0, NULL} }; diff --git a/client/cmdlft55xx.h b/client/cmdlft55xx.h index d17adbf2..79e37cef 100644 --- a/client/cmdlft55xx.h +++ b/client/cmdlft55xx.h @@ -59,6 +59,7 @@ typedef struct { RF_100 = 0x06, RF_128 = 0x07, } bitrate; + bool Q5; } t55xx_conf_block_t; t55xx_conf_block_t Get_t55xx_Config(); void Set_t55xx_Config(t55xx_conf_block_t conf); @@ -72,6 +73,7 @@ int CmdT55xxReadTrace(const char *Cmd); int CmdT55xxInfo(const char *Cmd); int CmdT55xxDetect(const char *Cmd); int CmdResetRead(const char *Cmd); +int CmdT55xxWipe(const char *Cmd); char * GetBitRateStr(uint32_t id); char * GetSaferStr(uint32_t id); @@ -79,12 +81,13 @@ char * GetModulationStr( uint32_t id); char * GetModelStrFromCID(uint32_t cid); char * GetSelectedModulationStr( uint8_t id); uint32_t PackBits(uint8_t start, uint8_t len, uint8_t *bitstream); +void printT5xxHeader(uint8_t page); void printT55xxBlock(const char *demodStr); int printConfiguration( t55xx_conf_block_t b); bool DecodeT55xxBlock(); bool tryDetectModulation(); -bool test(uint8_t mode, uint8_t *offset, int *fndBitRate); +bool test(uint8_t mode, uint8_t *offset, int *fndBitRate, uint8_t clk, bool *Q5); int special(const char *Cmd); int AquireData( uint8_t page, uint8_t block, bool pwdmode, uint32_t password ); diff --git a/client/cmdlfviking.c b/client/cmdlfviking.c index e50bc878..ef3c9629 100644 --- a/client/cmdlfviking.c +++ b/client/cmdlfviking.c @@ -23,8 +23,8 @@ int usage_lf_viking_clone(void){ //see ASKDemod for what args are accepted int CmdVikingDemod(const char *Cmd) { - CmdLFRead("s"); - getSamples("30000",false); + //CmdLFRead("s"); + //getSamples("30000",false); if (!ASKDemod(Cmd, false, false, 1)) { if (g_debugMode) PrintAndLog("ASKDemod failed"); @@ -34,7 +34,7 @@ int CmdVikingDemod(const char *Cmd) int ans = VikingDemod_AM(DemodBuffer, &size); if (ans < 0) { - if (g_debugMode) PrintAndLog("Error Viking_Demod"); + if (g_debugMode) PrintAndLog("Error Viking_Demod %d", ans); return 0; } //got a good demod diff --git a/common/lfdemod.c b/common/lfdemod.c index 21b4cce5..537c7962 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -11,6 +11,20 @@ #include #include #include "lfdemod.h" +#include "common.h" + +/* //un_comment to allow debug print calls when used not on device +void dummy(char *fmt, ...){} + +#ifndef ON_DEVICE +#include "ui.h" +#define prnt PrintAndLog +#else + +#define prnt dummy +#endif +*/ + uint8_t justNoise(uint8_t *BitStream, size_t size) { static const uint8_t THRESHOLD = 123; @@ -385,15 +399,15 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow // sync to first lo-hi transition, and threshold // Need to threshold first sample - - if(dest[0] < threshold_value) dest[0] = 0; + // skip 160 samples to allow antenna/samples to settle + if(dest[160] < threshold_value) dest[0] = 0; else dest[0] = 1; 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++) { + for(idx = 161; idx < size-20; idx++) { // threshold current value if (dest[idx] < threshold_value) dest[idx] = 0; @@ -404,15 +418,15 @@ size_t fsk_wave_demod(uint8_t * dest, size_t size, uint8_t fchigh, uint8_t fclow preLastSample = LastSample; LastSample = currSample; currSample = idx-last_transition; - if (currSample < (fclow-2)){ //0-5 = garbage noise + if (currSample < (fclow-2)){ //0-5 = garbage noise (or 0-3) //do nothing with extra garbage - } else if (currSample < (fchigh-1)) { //6-8 = 8 sample waves - if (LastSample > (fchigh-2) && preLastSample < (fchigh-1)){ - dest[numBits-1]=1; //correct last 9 wave surrounded by 8 waves + } else if (currSample < (fchigh-1)) { //6-8 = 8 sample waves or 3-6 = 5 + if (LastSample > (fchigh-2) && (preLastSample < (fchigh-1) || preLastSample == 0 )){ + dest[numBits-1]=1; //correct previous 9 wave surrounded by 8 waves } dest[numBits++]=1; - } else if (currSample > (fchigh+1) && !numBits) { //12 + and first bit = garbage + } else if (currSample > (fchigh) && !numBits) { //12 + and first bit = garbage //do nothing with beginning garbage } else if (currSample == (fclow+1) && LastSample == (fclow-1)) { // had a 7 then a 9 should be two 8's dest[numBits++]=1; @@ -439,19 +453,8 @@ size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, //if lastval was 1, we have a 1->0 crossing if (dest[idx-1]==1) { - if (!numBits && n < rfLen/fclow) { - n=0; - lastval = dest[idx]; - continue; - } n = (n * fclow + rfLen/2) / rfLen; } else {// 0->1 crossing - //test first bitsample too small - if (!numBits && n < rfLen/fchigh) { - n=0; - lastval = dest[idx]; - continue; - } n = (n * fchigh + rfLen/2) / rfLen; } if (n == 0) n = 1; @@ -473,6 +476,7 @@ size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t rfLen, } return numBits; } + //by marshmellow (from holiman's base) // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod) int fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow) @@ -682,7 +686,7 @@ int AWIDdemodFSK(uint8_t *dest, size_t *size) } // by marshmellow -// FSK Demod then try to locate an Farpointe Data (pyramid) ID +// FSK Demod then try to locate a Farpointe Data (pyramid) ID int PyramiddemodFSK(uint8_t *dest, size_t *size) { //make sure buffer has data @@ -707,22 +711,21 @@ int PyramiddemodFSK(uint8_t *dest, size_t *size) // to detect a wave that has heavily clipped (clean) samples uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t low) { - uint16_t allPeaks=1; + bool allArePeaks = true; uint16_t cntPeaks=0; - size_t loopEnd = 512+60; + size_t loopEnd = 512+160; if (loopEnd > size) loopEnd = size; - for (size_t i=60; ilow && dest[i] 300) return 1; + if (!allArePeaks){ + if (cntPeaks > 300) return true; } - return allPeaks; + return allArePeaks; } - // by marshmellow // to help detect clocks on heavily clipped samples // based on count of low to low @@ -730,7 +733,7 @@ int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low) { uint8_t fndClk[] = {8,16,32,40,50,64,128}; size_t startwave; - size_t i = 0; + size_t i = 100; size_t minClk = 255; // get to first full low to prime loop and skip incomplete first pulse while ((dest[i] < high) && (i < size)) @@ -753,6 +756,7 @@ int DetectStrongAskClock(uint8_t dest[], size_t size, uint8_t high, uint8_t low) minClk = i - startwave; } // set clock + //prnt("minClk: %d",minClk); for (uint8_t clkCnt = 0; clkCnt<7; clkCnt++) { if (minClk >= fndClk[clkCnt]-(fndClk[clkCnt]/8) && minClk <= fndClk[clkCnt]+1) return fndClk[clkCnt]; @@ -770,8 +774,8 @@ int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr) uint8_t clk[] = {255,8,16,32,40,50,64,100,128,255}; uint8_t clkEnd = 9; uint8_t loopCnt = 255; //don't need to loop through entire array... - if (size <= loopCnt) return -1; //not enough samples - + if (size <= loopCnt+60) return -1; //not enough samples + size -= 60; //sometimes there is a strange end wave - filter out this.... //if we already have a valid clock uint8_t clockFnd=0; for (;i= dest[i+2]){ if (waveStart == 0) { waveStart = i+1; - //PrintAndLog("DEBUG: waveStart: %d",waveStart); + //prnt("DEBUG: waveStart: %d",waveStart); } else { waveEnd = i+1; - //PrintAndLog("DEBUG: waveEnd: %d",waveEnd); + //prnt("DEBUG: waveEnd: %d",waveEnd); waveLenCnt = waveEnd-waveStart; if (waveLenCnt > fc){ firstFullWave = waveStart; @@ -909,7 +913,7 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) } } } - //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen); + //prnt("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen); //test each valid clock from greatest to smallest to see which lines up for(clkCnt=7; clkCnt >= 1 ; clkCnt--){ @@ -917,7 +921,7 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) waveStart = 0; errCnt=0; peakcnt=0; - //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit); + //prnt("DEBUG: clk: %d, lastClkBit: %d",clk[clkCnt],lastClkBit); for (i = firstFullWave+fullWaveLen-1; i < loopCnt-2; i++){ //top edge of wave = start of new wave @@ -930,7 +934,7 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) waveLenCnt = waveEnd-waveStart; if (waveLenCnt > fc){ //if this wave is a phase shift - //PrintAndLog("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc); + //prnt("DEBUG: phase shift at: %d, len: %d, nextClk: %d, ii: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+clk[clkCnt]-tol,ii+1,fc); if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit peakcnt++; lastClkBit+=clk[clkCnt]; @@ -959,11 +963,40 @@ int DetectPSKClock(uint8_t dest[], size_t size, int clock) if (peaksdet[i] > peaksdet[best]) { best = i; } - //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]); + //prnt("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]); } return clk[best]; } +int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low){ + //find shortest transition from high to low + size_t i = 0; + size_t transition1 = 0; + int lowestTransition = 255; + bool lastWasHigh = false; + + //find first valid beginning of a high or low wave + while ((dest[i] >= peak || dest[i] <= low) && (i < size)) + ++i; + while ((dest[i] < peak && dest[i] > low) && (i < size)) + ++i; + lastWasHigh = (dest[i] >= peak); + + if (i==size) return 0; + transition1 = i; + + for (;i < size; i++) { + if ((dest[i] >= peak && !lastWasHigh) || (dest[i] <= low && lastWasHigh)) { + lastWasHigh = (dest[i] >= peak); + if (i-transition1 < lowestTransition) lowestTransition = i-transition1; + transition1 = i; + } + } + //prnt("DEBUG: LowestTrs: %d",lowestTransition); + if (lowestTransition == 255) lowestTransition = 0; + return lowestTransition; +} + //by marshmellow //detect nrz clock by reading #peaks vs no peaks(or errors) int DetectNRZClock(uint8_t dest[], size_t size, int clock) @@ -972,8 +1005,7 @@ int DetectNRZClock(uint8_t dest[], size_t size, int clock) uint8_t clk[]={8,16,32,40,50,64,100,128,255}; size_t loopCnt = 4096; //don't need to loop through entire array... if (size == 0) return 0; - if (size= peak || dest[i] <= low){ - peakcnt++; + if (!firstpeak) continue; + smplCnt++; } else { - if (peakcnt>0 && maxPeak < peakcnt){ - maxPeak = peakcnt; + firstpeak=true; + if (smplCnt > 6 ){ + if (maxPeak > smplCnt){ + maxPeak = smplCnt; + //prnt("maxPk: %d",maxPeak); + } + peakcnt++; + //prnt("maxPk: %d, smplCnt: %d, peakcnt: %d",maxPeak,smplCnt,peakcnt); + smplCnt=0; } - peakcnt=0; } } + bool errBitHigh = 0; + bool bitHigh = 0; + uint8_t ignoreCnt = 0; + uint8_t ignoreWindow = 4; + bool lastPeakHigh = 0; + int lastBit = 0; peakcnt=0; //test each valid clock from smallest to greatest to see which lines up for(clkCnt=0; clkCnt < 8; ++clkCnt){ - //ignore clocks smaller than largest peak - if (clk[clkCnt]= peak) || (dest[ii] <= low)){ peakcnt=0; - // now that we have the first one lined up test rest of wave array - for (i=0; i < ((int)((size-ii-tol)/clk[clkCnt])-1); ++i){ - if (dest[ii+(i*clk[clkCnt])]>=peak || dest[ii+(i*clk[clkCnt])]<=low){ + bitHigh = false; + ignoreCnt = 0; + lastBit = ii-clk[clkCnt]; + //loop through to see if this start location works + for (i = ii; i < size-20; ++i) { + //if we are at a clock bit + if ((i >= lastBit + clk[clkCnt] - tol) && (i <= lastBit + clk[clkCnt] + tol)) { + //test high/low + if (dest[i] >= peak || dest[i] <= low) { + //if same peak don't count it + if ((dest[i] >= peak && !lastPeakHigh) || (dest[i] <= low && lastPeakHigh)) { peakcnt++; + } + lastPeakHigh = (dest[i] >= peak); + bitHigh = true; + errBitHigh = false; + ignoreCnt = ignoreWindow; + lastBit += clk[clkCnt]; + } else if (i == lastBit + clk[clkCnt] + tol) { + lastBit += clk[clkCnt]; + } + //else if not a clock bit and no peaks + } else if (dest[i] < peak && dest[i] > low){ + if (ignoreCnt==0){ + bitHigh=false; + if (errBitHigh==true) peakcnt--; + errBitHigh=false; + } else { + ignoreCnt--; + } + // else if not a clock bit but we have a peak + } else if ((dest[i]>=peak || dest[i]<=low) && (!bitHigh)) { + //error bar found no clock... + errBitHigh=true; } } if(peakcnt>peaksdet[clkCnt]) { @@ -1025,11 +1101,16 @@ int DetectNRZClock(uint8_t dest[], size_t size, int clock) int iii=7; uint8_t best=0; for (iii=7; iii > 0; iii--){ - if (peaksdet[iii] > peaksdet[best]){ + if ((peaksdet[iii] >= (peaksdet[best]-1)) && (peaksdet[iii] <= peaksdet[best]+1) && lowestTransition) { + if (clk[iii] > (lowestTransition - (clk[iii]/8)) && clk[iii] < (lowestTransition + (clk[iii]/8))) { best = iii; } - //PrintAndLog("DEBUG: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[iii],peaksdet[iii],bestErr[iii],clk[best]); + } else if (peaksdet[iii] > peaksdet[best]){ + best = iii; + } + //prnt("DEBUG: Clk: %d, peaks: %d, maxPeak: %d, bestClk: %d, lowestTrs: %d",clk[iii],peaksdet[iii],maxPeak, clk[best], lowestTransition); } + return clk[best]; } @@ -1089,123 +1170,37 @@ int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert) return (int) startidx; } -// by marshmellow - demodulate NRZ wave (both similar enough) +// by marshmellow - demodulate NRZ wave // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak -// there probably is a much simpler way to do this.... -int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr) -{ +int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert){ if (justNoise(dest, *size)) return -1; *clk = DetectNRZClock(dest, *size, *clk); if (*clk==0) return -2; size_t i, gLen = 4096; - if (gLen>*size) gLen = *size; + if (gLen>*size) gLen = *size-20; int high, low; if (getHiLo(dest, gLen, &high, &low, 75, 75) < 1) return -3; //25% fuzz on high 25% fuzz on low - int lastBit = 0; //set first clock check - size_t iii = 0, bitnum = 0; //bitnum counter - uint16_t errCnt = 0, MaxBits = 1000; - size_t bestErrCnt = maxErr+1; - size_t bestPeakCnt = 0, bestPeakStart = 0; - uint8_t bestFirstPeakHigh=0, firstPeakHigh=0, curBit=0, bitHigh=0, errBitHigh=0; - uint8_t tol = 1; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave - uint16_t peakCnt=0; - uint8_t ignoreWindow=4; - uint8_t ignoreCnt=ignoreWindow; //in case of noise near peak - //loop to find first wave that works - align to clock - for (iii=0; iii < gLen; ++iii){ - if ((dest[iii]>=high) || (dest[iii]<=low)){ - if (dest[iii]>=high) firstPeakHigh=1; - else firstPeakHigh=0; - lastBit=iii-*clk; - peakCnt=0; - errCnt=0; - //loop through to see if this start location works - for (i = iii; i < *size; ++i) { - // if we are at a clock bit - if ((i >= lastBit + *clk - tol) && (i <= lastBit + *clk + tol)) { - //test high/low - if (dest[i] >= high || dest[i] <= low) { - bitHigh = 1; - peakCnt++; - errBitHigh = 0; - ignoreCnt = ignoreWindow; - lastBit += *clk; - } else if (i == lastBit + *clk + tol) { - lastBit += *clk; - } - //else if no bars found - } else if (dest[i] < high && dest[i] > low){ - if (ignoreCnt==0){ - bitHigh=0; - if (errBitHigh==1) errCnt++; - errBitHigh=0; - } else { - ignoreCnt--; - } - } else if ((dest[i]>=high || dest[i]<=low) && (bitHigh==0)) { - //error bar found no clock... - errBitHigh=1; - } - if (((i-iii) / *clk)>=MaxBits) break; - } - //we got more than 64 good bits and not all errors - if (((i-iii) / *clk) > 64 && (errCnt <= (maxErr))) { - //possible good read - if (!errCnt || peakCnt > bestPeakCnt){ - bestFirstPeakHigh=firstPeakHigh; - bestErrCnt = errCnt; - bestPeakCnt = peakCnt; - bestPeakStart = iii; - if (!errCnt) break; //great read - finish - } - } - } + + uint8_t bit=0; + //convert wave samples to 1's and 0's + for(i=20; i < *size-20; i++){ + if (dest[i] >= high) bit = 1; + if (dest[i] <= low) bit = 0; + dest[i] = bit; } - //PrintAndLog("DEBUG: bestErrCnt: %d, maxErr: %d, bestStart: %d, bestPeakCnt: %d, bestPeakStart: %d",bestErrCnt,maxErr,bestStart,bestPeakCnt,bestPeakStart); - if (bestErrCnt > maxErr) return bestErrCnt; - - //best run is good enough set to best run and set overwrite BinStream - lastBit = bestPeakStart - *clk; - memset(dest, bestFirstPeakHigh^1, bestPeakStart / *clk); - bitnum += (bestPeakStart / *clk); - for (i = bestPeakStart; i < *size; ++i) { - // if expecting a clock bit - if ((i >= lastBit + *clk - tol) && (i <= lastBit + *clk + tol)) { - // test high/low - if (dest[i] >= high || dest[i] <= low) { - peakCnt++; - bitHigh = 1; - errBitHigh = 0; - ignoreCnt = ignoreWindow; - curBit = *invert; - if (dest[i] >= high) curBit ^= 1; - dest[bitnum++] = curBit; - lastBit += *clk; - //else no bars found in clock area - } else if (i == lastBit + *clk + tol) { - dest[bitnum++] = curBit; - lastBit += *clk; - } - //else if no bars found - } else if (dest[i] < high && dest[i] > low){ - if (ignoreCnt == 0){ - bitHigh = 0; - if (errBitHigh == 1){ - dest[bitnum++] = 7; - errCnt++; - } - errBitHigh=0; - } else { - ignoreCnt--; - } - } else if ((dest[i] >= high || dest[i] <= low) && (bitHigh == 0)) { - //error bar found no clock... - errBitHigh=1; + //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit) + size_t lastBit = 0; + size_t numBits = 0; + for(i=21; i < *size-20; i++) { + //if transition detected or large number of same bits - store the passed bits + if (dest[i] != dest[i-1] || (i-lastBit) == (10 * *clk)) { + memset(dest+numBits, dest[i-1] ^ *invert, (i - lastBit + (*clk/4)) / *clk); + numBits += (i - lastBit + (*clk/4)) / *clk; + lastBit = i-1; } - if (bitnum >= MaxBits) break; } - *size = bitnum; - return bestErrCnt; + *size = numBits; + return 0; } //by marshmellow @@ -1223,18 +1218,18 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc size_t i; if (size == 0) return 0; - uint8_t fcTol = (uint8_t)(0.5+(float)(fcHigh-fcLow)/2); + uint8_t fcTol = ((fcHigh*100 - fcLow*100)/2 + 50)/100; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2); rfLensFnd=0; fcCounter=0; rfCounter=0; firstBitFnd=0; //PrintAndLog("DEBUG: fcTol: %d",fcTol); - // prime i to first up transition - for (i = 1; i < size-1; i++) + // prime i to first peak / up transition + for (i = 160; i < size-20; i++) if (BitStream[i] > BitStream[i-1] && BitStream[i]>=BitStream[i+1]) break; - for (; i < size-1; i++){ + for (; i < size-20; i++){ fcCounter++; rfCounter++; @@ -1252,7 +1247,7 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc //not the same size as the last wave - start of new bit sequence if (firstBitFnd > 1){ //skip first wave change - probably not a complete bit for (int ii=0; ii<15; ii++){ - if (rfLens[ii] == rfCounter){ + if (rfLens[ii] >= (rfCounter-4) && rfLens[ii] <= (rfCounter+4)){ rfCnts[ii]++; rfCounter = 0; break; @@ -1274,7 +1269,7 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc uint8_t rfHighest=15, rfHighest2=15, rfHighest3=15; for (i=0; i<15; i++){ - //PrintAndLog("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]); + //prnt("DEBUG: RF %d, cnts %d",rfLens[i], rfCnts[i]); //get highest 2 RF values (might need to get more values to compare or compare all?) if (rfCnts[i]>rfCnts[rfHighest]){ rfHighest3=rfHighest2; @@ -1291,12 +1286,13 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off uint8_t tol1 = fcHigh+1; - //PrintAndLog("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]); + //prnt("DEBUG: hightest: 1 %d, 2 %d, 3 %d",rfLens[rfHighest],rfLens[rfHighest2],rfLens[rfHighest3]); // loop to find the highest clock that has a remainder less than the tolerance // compare samples counted divided by + // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less) int ii=7; - for (; ii>=0; ii--){ + for (; ii>=2; ii--){ if (rfLens[rfHighest] % clk[ii] < tol1 || rfLens[rfHighest] % clk[ii] > clk[ii]-tol1){ if (rfLens[rfHighest2] % clk[ii] < tol1 || rfLens[rfHighest2] % clk[ii] > clk[ii]-tol1){ if (rfLens[rfHighest3] % clk[ii] < tol1 || rfLens[rfHighest3] % clk[ii] > clk[ii]-tol1){ @@ -1317,8 +1313,8 @@ uint8_t detectFSKClk(uint8_t *BitStream, size_t size, uint8_t fcHigh, uint8_t fc //mainly used for FSK field clock detection uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) { - uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0}; - uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0}; + uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; uint8_t fcLensFnd = 0; uint8_t lastFCcnt=0; uint8_t fcCounter = 0; @@ -1326,11 +1322,11 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) if (size == 0) return 0; // prime i to first up transition - for (i = 1; i < size-1; i++) + for (i = 160; i < size-20; i++) if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]) break; - for (; i < size-1; i++){ + for (; i < size-20; i++){ if (BitStream[i] > BitStream[i-1] && BitStream[i] >= BitStream[i+1]){ // new up transition fcCounter++; @@ -1343,14 +1339,14 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) lastFCcnt = fcCounter; } // find which fcLens to save it to: - for (int ii=0; ii<10; ii++){ + for (int ii=0; ii<15; ii++){ if (fcLens[ii]==fcCounter){ fcCnts[ii]++; fcCounter=0; break; } } - if (fcCounter>0 && fcLensFnd<10){ + if (fcCounter>0 && fcLensFnd<15){ //add new fc length fcCnts[fcLensFnd]++; fcLens[fcLensFnd++]=fcCounter; @@ -1362,11 +1358,11 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) } } - uint8_t best1=9, best2=9, best3=9; + uint8_t best1=14, best2=14, best3=14; uint16_t maxCnt1=0; // go through fclens and find which ones are bigest 2 - for (i=0; i<10; i++){ - // PrintAndLog("DEBUG: FC %d, Cnt %d, Errs %d",fcLens[i],fcCnts[i],errCnt); + for (i=0; i<15; i++){ + //prnt("DEBUG: FC %d, Cnt %d",fcLens[i],fcCnts[i]); // get the 3 best FC values if (fcCnts[i]>maxCnt1) { best3=best2; @@ -1380,6 +1376,7 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) best3=i; } } + if (fcLens[best1]==0) return 0; uint8_t fcH=0, fcL=0; if (fcLens[best1]>fcLens[best2]){ fcH=fcLens[best1]; @@ -1388,11 +1385,13 @@ uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj) fcH=fcLens[best2]; fcL=fcLens[best1]; } + //prnt("DEBUG: dd %d > %d",(size-180)/fcH/3,fcCnts[best1]+fcCnts[best2]); + if ((size-180)/fcH/3 > fcCnts[best1]+fcCnts[best2]) return 0; //lots of waves not psk or fsk // TODO: take top 3 answers and compare to known Field clocks to get top 2 uint16_t fcs = (((uint16_t)fcH)<<8) | fcL; - // PrintAndLog("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]); + //prnt("DEBUG: Best %d best2 %d best3 %d",fcLens[best1],fcLens[best2],fcLens[best3]); if (fskAdj) return fcs; return fcLens[best1]; } @@ -1405,6 +1404,7 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) uint16_t loopCnt = 4096; //don't need to loop through entire array... if (*size fc && waveStart > fc){ //not first peak and is a large wave + if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+2)){ //not first peak and is a large wave but not out of whack lastAvgWaveVal = avgWaveVal/(waveLenCnt); firstFullWave = waveStart; fullWaveLen=waveLenCnt; @@ -1434,14 +1434,21 @@ int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert) } avgWaveVal += dest[i+2]; } + if (firstFullWave == 0) { + // no phase shift detected - could be all 1's or 0's - doesn't matter where we start + // so skip a little to ensure we are past any Start Signal + firstFullWave = 160; + memset(dest, curPhase, firstFullWave / *clock); + } else { + memset(dest, curPhase^1, firstFullWave / *clock); + } + //advance bits + numBits += (firstFullWave / *clock); + //set start of wave as clock align + lastClkBit = firstFullWave; //PrintAndLog("DEBUG: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen); - lastClkBit = firstFullWave; //set start of wave as clock align //PrintAndLog("DEBUG: clk: %d, lastClkBit: %d", *clock, lastClkBit); waveStart = 0; - size_t numBits=0; - //set skipped bits - memset(dest, curPhase^1, firstFullWave / *clock); - numBits += (firstFullWave / *clock); dest[numBits++] = curPhase; //set first read bit for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){ //top edge of wave = start of new wave diff --git a/common/lfdemod.h b/common/lfdemod.h index cc4fa27a..0e1e99ad 100644 --- a/common/lfdemod.h +++ b/common/lfdemod.h @@ -32,7 +32,7 @@ int getHiLo(uint8_t *BitStream, size_t size, int *high, int *low, uint8_t f uint32_t manchesterEncode2Bytes(uint16_t datain); int ManchesterEncode(uint8_t *BitStream, size_t size); int manrawdecode(uint8_t *BitStream, size_t *size, uint8_t invert); -int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int maxErr); +int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert); uint8_t parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType); uint8_t preambleSearch(uint8_t *BitStream, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx); int pskRawDemod(uint8_t dest[], size_t *size, int *clock, int *invert); -- 2.39.2