diff --git a/armsrc/pcf7931.c b/armsrc/pcf7931.c index d5f630cc..bfc22bf8 100644 --- a/armsrc/pcf7931.c +++ b/armsrc/pcf7931.c @@ -9,13 +9,12 @@ #define ALLOC 16 size_t DemodPCF7931(uint8_t **outBlocks) { - - uint8_t bits[256] = {0x00}; + uint8_t bits[256] = {0x00}; uint8_t blocks[8][16]; - uint8_t *dest = BigBuf_get_addr(); + uint8_t *dest = BigBuf_get_addr(); int GraphTraceLen = BigBuf_max_traceLen(); - if ( GraphTraceLen > 18000 ) + if (GraphTraceLen > 18000) GraphTraceLen = 18000; int i, j, lastval, bitidx, half_switch; @@ -38,17 +37,16 @@ size_t DemodPCF7931(uint8_t **outBlocks) { i = 2; /* Find first local max/min */ - if(dest[1] > dest[0]) { + if(dest[1] > dest[0]) { while(i < GraphTraceLen) { - if( !(dest[i] > dest[i-1]) && dest[i] > lmax) + if( !(dest[i] > dest[i-1]) && dest[i] > lmax) break; i++; } dir = 0; - } - else { + } else { while(i < GraphTraceLen) { - if( !(dest[i] < dest[i-1]) && dest[i] < lmin) + if( !(dest[i] < dest[i-1]) && dest[i] < lmin) break; i++; } @@ -60,10 +58,8 @@ size_t DemodPCF7931(uint8_t **outBlocks) { pmc = 0; block_done = 0; - for (bitidx = 0; i < GraphTraceLen; i++) - { - if ( (dest[i-1] > dest[i] && dir == 1 && dest[i] > lmax) || (dest[i-1] < dest[i] && dir == 0 && dest[i] < lmin)) - { + for (bitidx = 0; i < GraphTraceLen; i++) { + 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; @@ -77,8 +73,7 @@ size_t DemodPCF7931(uint8_t **outBlocks) { lastval = i; pmc = 0; block_done = 1; - } - else { + } else { pmc = i; } } else if (ABS(lc-clock/2) < tolerance) { @@ -89,21 +84,18 @@ size_t DemodPCF7931(uint8_t **outBlocks) { lastval = i; pmc = 0; block_done = 1; - } - else if(half_switch == 1) { - bits[bitidx++] = 0; + } else if(half_switch == 1) { + bits[bitidx++] = 0; half_switch = 0; } else half_switch++; } else if (ABS(lc-clock) < tolerance) { // 64TO - bits[bitidx++] = 1; + bits[bitidx++] = 1; } else { // Error - warnings++; - if (warnings > 10) - { + if (++warnings > 10) { Dbprintf("Error: too many detection errors, aborting."); return 0; } @@ -111,16 +103,17 @@ size_t DemodPCF7931(uint8_t **outBlocks) { if(block_done == 1) { if(bitidx == 128) { - for(j=0; j<16; j++) { - 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]; - + for(j = 0; j < 16; ++j) { + 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++; } @@ -129,52 +122,43 @@ size_t DemodPCF7931(uint8_t **outBlocks) { half_switch = 0; } if(i < GraphTraceLen) - dir =(dest[i-1] > dest[i]) ? 0 : 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; } -uint8_t IsBlock0PCF7931(uint8_t *block) -{ +bool IsBlock0PCF7931(uint8_t *block) { // assuming all RFU bits are set to 0 // if PAC is enabled password is set to 0 if (block[7] == 0x01) { if (!memcmp(block, "\x00\x00\x00\x00\x00\x00\x00", 7) && !memcmp(block+9, "\x00\x00\x00\x00\x00\x00\x00", 7)) - return 1; + return true; } else if (block[7] == 0x00) { if (!memcmp(block+9, "\x00\x00\x00\x00\x00\x00\x00", 7)) - return 1; + return true; } - return 0; + return false; } -uint8_t IsBlock1PCF7931(uint8_t *block) { +bool IsBlock1PCF7931(uint8_t *block) { // assuming all RFU bits are set to 0 if (block[10] == 0 && block[11] == 0 && block[12] == 0 && block[13] == 0) if((block[14] & 0x7f) <= 9 && block[15] <= 9) - return 1; + return true; - return 0; + return false; } -static inline void print_block(uint8_t *data) -{ - Dbprintf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", - data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], - data[8], data[9], data[10], data[11], data[12], data[13], data[14], data[15]); -} - -void ReadPCF7931() -{ +void ReadPCF7931() { int found_blocks = 0; // successfully read blocks int max_blocks = 8; // readable blocks uint8_t memory_blocks[8][17]; // PCF content @@ -194,61 +178,51 @@ void ReadPCF7931() int i = 0, j = 0; - do - { + do { i = 0; memset(tmp_blocks, 0, 4*16*sizeof(uint8_t)); n = DemodPCF7931((uint8_t**)tmp_blocks); if(!n) - errors++; + ++errors; // exit if no block is received - if (errors == 10 && found_blocks == 0) - { + if (errors >= 10 && found_blocks == 0) { Dbprintf("Error, no tag or bad tag"); return; } - // exit if too many errors during reading - else if (tries > 50 && (float)(errors/tries) > .5f) - { + // exit if too many errors during reading + if (tries > 50 && (2*errors > tries)) { Dbprintf("Error reading the tag"); Dbprintf("Here is the partial content"); goto end; } // our logic breaks if we don't get at least two blocks - if (n < 2) - { - if (single_blocks_cnt < max_blocks) - { - for (i = 0; i < single_blocks_cnt; ++i) - { - if (!memcmp(single_blocks[i], tmp_blocks[0], 16)) - { + if (n < 2) { + if (single_blocks_cnt < max_blocks) { + for (i = 0; i < single_blocks_cnt; ++i) { + if (!memcmp(single_blocks[i], tmp_blocks[0], 16)) { j = 1; break; } } - if (j != 1) - { + if (j != 1) { memcpy(single_blocks[single_blocks_cnt], tmp_blocks[0], 16); single_blocks_cnt++; } j = 0; } + ++tries; continue; } - Dbprintf("(dbg) got %d blocks (%d/%d found)", n, found_blocks, (max_blocks == 0 ? found_blocks : max_blocks)); - + Dbprintf("(dbg) got %d blocks (%d/%d found) (%d tries, %d errors)", n, found_blocks, (max_blocks == 0 ? found_blocks : max_blocks), tries, errors); + i = 0; - if(!found_0_1) - { - while (i < n - 1) - { - if (IsBlock0PCF7931(tmp_blocks[i]) && IsBlock1PCF7931(tmp_blocks[i+1])) - { + if(!found_0_1) { + while (i < n - 1) { + if (IsBlock0PCF7931(tmp_blocks[i]) && IsBlock1PCF7931(tmp_blocks[i+1])) { found_0_1 = 1; memcpy(memory_blocks[0], tmp_blocks[i], 16); memcpy(memory_blocks[1], tmp_blocks[i+1], 16); @@ -260,8 +234,7 @@ void ReadPCF7931() Dbprintf("Found blocks 0 and 1. PCF is transmitting %d blocks.", max_blocks); // handle the following blocks - for (j = i + 2; j < n; ++j) - { + for (j = i + 2; j < n; ++j) { memcpy(memory_blocks[found_blocks], tmp_blocks[j], 16); memory_blocks[found_blocks][ALLOC] = 1; ++found_blocks; @@ -270,39 +243,27 @@ void ReadPCF7931() } ++i; } - } - else - { + } else { // Trying to re-order blocks // Look for identical block in memory blocks - while (i < n-1) - { + while (i < n-1) { // skip all zeroes blocks - if (memcmp(tmp_blocks[i], "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 16)) - { - for (j = 1; j < max_blocks - 1; ++j) - { - if (!memcmp(tmp_blocks[i], memory_blocks[j], 16) && !memory_blocks[j+1][ALLOC]) - { + if (memcmp(tmp_blocks[i], "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 16)) { + for (j = 1; j < max_blocks - 1; ++j) { + if (!memcmp(tmp_blocks[i], memory_blocks[j], 16) && !memory_blocks[j+1][ALLOC]) { memcpy(memory_blocks[j+1], tmp_blocks[i+1], 16); memory_blocks[j+1][ALLOC] = 1; if (++found_blocks >= max_blocks) goto end; } } } - if (memcmp(tmp_blocks[i+1], "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 16)) - { - for (j = 0; j < max_blocks; ++j) - { - if (!memcmp(tmp_blocks[i+1], memory_blocks[j], 16) && !memory_blocks[(j == 0 ? max_blocks : j) -1][ALLOC]) - { - if (j == 0) - { + if (memcmp(tmp_blocks[i+1], "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 16)) { + for (j = 0; j < max_blocks; ++j) { + if (!memcmp(tmp_blocks[i+1], memory_blocks[j], 16) && !memory_blocks[(j == 0 ? max_blocks : j) -1][ALLOC]) { + if (j == 0) { memcpy(memory_blocks[max_blocks - 1], tmp_blocks[i], 16); memory_blocks[max_blocks - 1][ALLOC] = 1; - } - else - { + } else { memcpy(memory_blocks[j-1], tmp_blocks[i], 16); memory_blocks[j-1][ALLOC] = 1; } @@ -314,8 +275,7 @@ void ReadPCF7931() } } ++tries; - if (BUTTON_PRESS()) - { + if (BUTTON_PRESS()) { Dbprintf("Button pressed, stopping."); goto end; } @@ -328,27 +288,25 @@ void ReadPCF7931() Dbprintf("-----------------------------------------"); for (i = 0; i < max_blocks; ++i) { if (memory_blocks[i][ALLOC]) - print_block(memory_blocks[i]); + print_result("Block", memory_blocks[i], 16); else Dbprintf("", i); } Dbprintf("-----------------------------------------"); - if (found_blocks < max_blocks) - { + if (found_blocks < max_blocks) { Dbprintf("-----------------------------------------"); Dbprintf("Blocks with unknown position:"); Dbprintf("-----------------------------------------"); for (i = 0; i < single_blocks_cnt; ++i) - print_block(single_blocks[i]); + print_result("Block", single_blocks[i], 16); Dbprintf("-----------------------------------------"); } cmd_send(CMD_ACK,0,0,0,0,0); } -static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int32_t p, uint8_t address, uint8_t byte, uint8_t data) -{ +static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int32_t p, uint8_t address, uint8_t byte, uint8_t data) { uint32_t tab[1024]={0}; // data times frame uint32_t u = 0; uint8_t parity = 0; @@ -372,8 +330,7 @@ static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int3 AddBitPCF7931(0, tab, l, p); //block adress on 6 bits - for (u = 0; u < 6; ++u) - { + for (u = 0; u < 6; ++u) { if (address & (1 << u)) { // bit 1 ++parity; AddBitPCF7931(1, tab, l, p); @@ -385,8 +342,7 @@ static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int3 //byte address on 4 bits for (u = 0; u < 4; ++u) { - if (byte & (1 << u)) - { // bit 1 + if (byte & (1 << u)) { // bit 1 parity++; AddBitPCF7931(1, tab, l, p); } @@ -419,12 +375,10 @@ static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int3 tab[u] = (tab[u] * 3) / 2; //compennsation of the counter reload - while (!comp) - { + while (!comp) { comp = 1; for (u = 0; tab[u] != 0; ++u) - if(tab[u] > 0xFFFF) - { + if(tab[u] > 0xFFFF) { tab[u] -= 0xFFFF; comp = 0; } @@ -433,15 +387,12 @@ static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int3 SendCmdPCF7931(tab); } -void BruteForcePCF7931(uint64_t password, uint8_t tries, uint16_t init_delay, int32_t l, int32_t p) -{ +void BruteForcePCF7931(uint64_t password, uint8_t tries, uint16_t init_delay, int32_t l, int32_t p) { uint8_t i = 0; uint8_t pass_array[7]; - while (password < 0x00FFFFFFFFFFFFFF) - { - if (BUTTON_PRESS()) - { + while (password < 0x00FFFFFFFFFFFFFF) { + if (BUTTON_PRESS()) { Dbprintf("Button pressed, stopping bruteforce ..."); return; } @@ -484,8 +435,7 @@ void BruteForcePCF7931(uint64_t password, uint8_t tries, uint16_t init_delay, in @param byte : address of the byte to write @param data : data to write */ -void WritePCF7931(uint8_t pass1, uint8_t pass2, uint8_t pass3, uint8_t pass4, uint8_t pass5, uint8_t pass6, uint8_t pass7, uint16_t init_delay, int32_t l, int32_t p, uint8_t address, uint8_t byte, uint8_t data) -{ +void WritePCF7931(uint8_t pass1, uint8_t pass2, uint8_t pass3, uint8_t pass4, uint8_t pass5, uint8_t pass6, uint8_t pass7, uint16_t init_delay, int32_t l, int32_t p, uint8_t address, uint8_t byte, uint8_t data) { Dbprintf("Initialization delay : %d us", init_delay); Dbprintf("Offsets : %d us on the low pulses width, %d us on the low pulses positions", l, p); Dbprintf("Password (LSB first on each byte): %02x %02x %02x %02x %02x %02x %02x", pass1, pass2, pass3, pass4, pass5, pass6, pass7); @@ -504,7 +454,7 @@ void WritePCF7931(uint8_t pass1, uint8_t pass2, uint8_t pass3, uint8_t pass4, ui * @param tab : array of the data frame */ -void SendCmdPCF7931(uint32_t * tab){ +void SendCmdPCF7931(uint32_t * tab) { uint16_t u=0; uint16_t tempo=0; @@ -529,8 +479,7 @@ void SendCmdPCF7931(uint32_t * tab){ AT91C_BASE_TCB->TCB_BCR = 1; tempo = AT91C_BASE_TC0->TC_CV; - for (u = 0; tab[u] != 0; u += 3) - { + for (u = 0; tab[u] != 0; u += 3) { // modulate antenna HIGH(GPIO_SSC_DOUT); while(tempo != tab[u]) @@ -563,15 +512,12 @@ void SendCmdPCF7931(uint32_t * tab){ * @param l : offset on low pulse width * @param p : offset on low pulse positioning */ - -bool AddBytePCF7931(uint8_t byte, uint32_t * tab, int32_t l, int32_t p){ - +bool AddBytePCF7931(uint8_t byte, uint32_t * tab, int32_t l, int32_t p) { uint32_t u; - for (u=0; u<8; u++) - { - if (byte&(1<