diff --git a/Makefile b/Makefile index bf298e652..fa3bcf3bc 100644 --- a/Makefile +++ b/Makefile @@ -90,6 +90,11 @@ print-%: ; @echo $* = $($*) style: find . \( -name "*.[ch]" -or -name "*.cpp" -or -name "*.lua" \) -exec perl -pi -e 's/[ \t\r]+$$//' {} \; + find . \( -name "*.[ch]" -or -name "*.cpp" \) -exec astyle --formatted --mode=c --suffix=none \ + --indent=spaces=4 --indent-switches --indent-preprocessor \ + --keep-one-line-blocks --max-instatement-indent=60 \ + --style=linux --pad-oper --unpad-paren --pad-header \ + --align-pointer=name {} \; # Dummy target to test for GNU make availability _test: diff --git a/armsrc/BigBuf.c b/armsrc/BigBuf.c index 26421732c..fcbf898ab 100644 --- a/armsrc/BigBuf.c +++ b/armsrc/BigBuf.c @@ -13,7 +13,7 @@ // BigBuf is the large multi-purpose buffer, typically used to hold A/D samples or traces. // Also used to hold various smaller buffers and the Mifare Emulator Memory. // declare it as uint32_t to achieve alignment to 4 Byte boundary -static uint32_t BigBuf[BIGBUF_SIZE/sizeof(uint32_t)]; +static uint32_t BigBuf[BIGBUF_SIZE / sizeof(uint32_t)]; /* BigBuf memory layout: Pointer to highest available memory: BigBuf_hi @@ -34,12 +34,14 @@ static uint32_t traceLen = 0; static bool tracing = true; //todo static? // get the address of BigBuf -uint8_t *BigBuf_get_addr(void) { +uint8_t *BigBuf_get_addr(void) +{ return (uint8_t *)BigBuf; } // get the address of the emulator memory. Allocate part of Bigbuf for it, if not yet done -uint8_t *BigBuf_get_EM_addr(void) { +uint8_t *BigBuf_get_EM_addr(void) +{ // not yet allocated if (emulator_memory == NULL) emulator_memory = BigBuf_malloc(CARD_MEMORY_SIZE); @@ -48,28 +50,33 @@ uint8_t *BigBuf_get_EM_addr(void) { } // clear ALL of BigBuf -void BigBuf_Clear(void) { +void BigBuf_Clear(void) +{ BigBuf_Clear_ext(true); } // clear ALL of BigBuf -void BigBuf_Clear_ext(bool verbose) { +void BigBuf_Clear_ext(bool verbose) +{ memset(BigBuf, 0, BIGBUF_SIZE); if (verbose) Dbprintf("Buffer cleared (%i bytes)", BIGBUF_SIZE); } -void BigBuf_Clear_EM(void) { +void BigBuf_Clear_EM(void) +{ memset(BigBuf_get_EM_addr(), 0, CARD_MEMORY_SIZE); } -void BigBuf_Clear_keep_EM(void) { +void BigBuf_Clear_keep_EM(void) +{ memset(BigBuf, 0, BigBuf_hi); } // allocate a chunk of memory from BigBuf. We allocate high memory first. The unallocated memory // at the beginning of BigBuf is always for traces/samples -uint8_t *BigBuf_malloc(uint16_t chunksize) { +uint8_t *BigBuf_malloc(uint16_t chunksize) +{ if (BigBuf_hi - chunksize < 0) return NULL; // no memory left @@ -79,14 +86,16 @@ uint8_t *BigBuf_malloc(uint16_t chunksize) { } // free ALL allocated chunks. The whole BigBuf is available for traces or samples again. -void BigBuf_free(void){ +void BigBuf_free(void) +{ BigBuf_hi = BIGBUF_SIZE; emulator_memory = NULL; // shouldn't this empty BigBuf also? } // free allocated chunks EXCEPT the emulator memory -void BigBuf_free_keep_EM(void) { +void BigBuf_free_keep_EM(void) +{ if (emulator_memory != NULL) BigBuf_hi = emulator_memory - (uint8_t *)BigBuf; else @@ -95,7 +104,8 @@ void BigBuf_free_keep_EM(void) { // shouldn't this empty BigBuf also? } -void BigBuf_print_status(void) { +void BigBuf_print_status(void) +{ Dbprintf("Memory"); Dbprintf(" BIGBUF_SIZE.............%d", BIGBUF_SIZE); Dbprintf(" Available memory........%d", BigBuf_hi); @@ -105,21 +115,26 @@ void BigBuf_print_status(void) { } // return the maximum trace length (i.e. the unallocated size of BigBuf) -uint16_t BigBuf_max_traceLen(void) { +uint16_t BigBuf_max_traceLen(void) +{ return BigBuf_hi; } -void clear_trace(void) { +void clear_trace(void) +{ traceLen = 0; } -void set_tracelen(uint32_t value) { +void set_tracelen(uint32_t value) +{ traceLen = value; } -void set_tracing(bool enable) { +void set_tracing(bool enable) +{ tracing = enable; } -bool get_tracing(void) { +bool get_tracing(void) +{ return tracing; } @@ -127,7 +142,8 @@ bool get_tracing(void) { * Get the number of bytes traced * @return */ -uint32_t BigBuf_get_traceLen(void) { +uint32_t BigBuf_get_traceLen(void) +{ return traceLen; } @@ -137,12 +153,13 @@ uint32_t BigBuf_get_traceLen(void) { by 'hf list raw', alternatively 'hf list ' for protocol-specific annotation of commands/responses. **/ -bool RAMFUNC LogTrace(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_start, uint32_t timestamp_end, uint8_t *parity, bool readerToTag) { +bool RAMFUNC LogTrace(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_start, uint32_t timestamp_end, uint8_t *parity, bool readerToTag) +{ if (!tracing) return false; uint8_t *trace = BigBuf_get_addr(); - uint32_t num_paritybytes = (iLen-1)/8 + 1; // number of valid paritybytes in *parity + uint32_t num_paritybytes = (iLen - 1) / 8 + 1; // number of valid paritybytes in *parity uint32_t duration = timestamp_end - timestamp_start; // Return when trace is full @@ -195,7 +212,8 @@ bool RAMFUNC LogTrace(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_ return true; } -int LogTraceHitag(const uint8_t * btBytes, int iBits, int iSamples, uint32_t dwParity, int readerToTag) { +int LogTraceHitag(const uint8_t *btBytes, int iBits, int iSamples, uint32_t dwParity, int readerToTag) +{ /** Todo, rewrite the logger to use the generic functionality instead. It should be noted, however, that this logger takes number of bits as argument, not number of bytes. @@ -237,10 +255,11 @@ int LogTraceHitag(const uint8_t * btBytes, int iBits, int iSamples, uint32_t dwP } // Emulator memory -uint8_t emlSet(uint8_t *data, uint32_t offset, uint32_t length){ - uint8_t* mem = BigBuf_get_EM_addr(); +uint8_t emlSet(uint8_t *data, uint32_t offset, uint32_t length) +{ + uint8_t *mem = BigBuf_get_EM_addr(); if (offset + length < CARD_MEMORY_SIZE) { - memcpy(mem+offset, data, length); + memcpy(mem + offset, data, length); return 0; } Dbprintf("Error, trying to set memory outside of bounds! %d > %d", (offset + length), CARD_MEMORY_SIZE); diff --git a/armsrc/BigBuf.h b/armsrc/BigBuf.h index e9a7b8098..995451b9b 100644 --- a/armsrc/BigBuf.h +++ b/armsrc/BigBuf.h @@ -42,6 +42,6 @@ extern void set_tracing(bool enable); extern void set_tracelen(uint32_t value); extern bool get_tracing(void); extern bool RAMFUNC LogTrace(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_start, uint32_t timestamp_end, uint8_t *parity, bool readerToTag); -extern int LogTraceHitag(const uint8_t * btBytes, int iBits, int iSamples, uint32_t dwParity, int bReader); +extern int LogTraceHitag(const uint8_t *btBytes, int iBits, int iSamples, uint32_t dwParity, int bReader); extern uint8_t emlSet(uint8_t *data, uint32_t offset, uint32_t length); #endif /* __BIGBUF_H */ diff --git a/armsrc/LCD.c b/armsrc/LCD.c index 7987da188..05c09964b 100644 --- a/armsrc/LCD.c +++ b/armsrc/LCD.c @@ -13,7 +13,7 @@ void LCDSend(unsigned int data) while ((AT91C_BASE_SPI->SPI_SR & AT91C_SPI_TXEMPTY) == 0); // wait for the transfer to complete // For clarity's sake we pass data with 9th bit clear and commands with 9th // bit set since they're implemented as defines, se we need to invert bit - AT91C_BASE_SPI->SPI_TDR = data^0x100; // Send the data/command + AT91C_BASE_SPI->SPI_TDR = data ^ 0x100; // Send the data/command } void LCDSetXY(unsigned char x, unsigned char y) @@ -29,29 +29,28 @@ void LCDSetXY(unsigned char x, unsigned char y) void LCDSetPixel(unsigned char x, unsigned char y, unsigned char color) { - LCDSetXY(x,y); // Set position + LCDSetXY(x, y); // Set position LCDSend(PRAMWR); // Now write the pixel to the display LCDSend(color); // Write the data in the specified Color } -void LCDFill (unsigned char xs,unsigned char ys,unsigned char width,unsigned char height, unsigned char color) +void LCDFill(unsigned char xs, unsigned char ys, unsigned char width, unsigned char height, unsigned char color) { - unsigned char i,j; + unsigned char i, j; - for (i=0;i < height;i++) // Number of horizontal lines - { - LCDSetXY(xs,ys+i); // Goto start of fill area (Top Left) + for (i = 0; i < height; i++) { // Number of horizontal lines + LCDSetXY(xs, ys + i); // Goto start of fill area (Top Left) LCDSend(PRAMWR); // Write to display - for (j=0;j < width;j++) // pixels per line + for (j = 0; j < width; j++) // pixels per line LCDSend(color); } } -void LCDString (char *lcd_string, const char *font_style,unsigned char x, unsigned char y, unsigned char fcolor, unsigned char bcolor) +void LCDString(char *lcd_string, const char *font_style, unsigned char x, unsigned char y, unsigned char fcolor, unsigned char bcolor) { unsigned int i; - unsigned char mask=0, px, py, xme, yme, offset; + unsigned char mask = 0, px, py, xme, yme, offset; const char *data; data = font_style; // point to the start of the font table @@ -62,29 +61,28 @@ void LCDString (char *lcd_string, const char *font_style,unsigned char x, unsign data++; offset = *data; // get data bytes per font - do - { + do { // point to data in table to be loaded - data = (font_style + offset) + (offset * (int)(*lcd_string - 32)); + data = (font_style + offset) + (offset * (int)(*lcd_string - 32)); - for (i=0;i < yme;i++) { - mask |=0x80; + for (i = 0; i < yme; i++) { + mask |= 0x80; - for (px=x; px < (x + xme); px++) { - py= y + i; + for (px = x; px < (x + xme); px++) { + py = y + i; - if (*data & mask) LCDSetPixel (px,py,fcolor); - else LCDSetPixel (px,py,bcolor); + if (*data & mask) LCDSetPixel(px, py, fcolor); + else LCDSetPixel(px, py, bcolor); - mask>>=1; + mask >>= 1; } data++; } - x+=xme; + x += xme; lcd_string++; // next character in string - } while(*lcd_string !='\0'); // keep spitting chars out until end of string + } while (*lcd_string != '\0'); // keep spitting chars out until end of string } void LCDReset(void) @@ -121,29 +119,29 @@ void LCDInit(void) LCDSend(0xDC); // clear display - LCDSetXY(0,0); + LCDSetXY(0, 0); LCDSend(PRAMWR); // Write to display - i=LCD_XRES*LCD_YRES; - while(i--) LCDSend(WHITE); + i = LCD_XRES * LCD_YRES; + while (i--) LCDSend(WHITE); - // test text on different colored backgrounds - LCDString(" The quick brown fox ", (char *)&FONT6x8,1,1+8*0,WHITE ,BLACK ); - LCDString(" jumped over the ", (char *)&FONT6x8,1,1+8*1,BLACK ,WHITE ); - LCDString(" lazy dog. ", (char *)&FONT6x8,1,1+8*2,YELLOW ,RED ); - LCDString(" AaBbCcDdEeFfGgHhIiJj ", (char *)&FONT6x8,1,1+8*3,RED ,GREEN ); - LCDString(" KkLlMmNnOoPpQqRrSsTt ", (char *)&FONT6x8,1,1+8*4,MAGENTA,BLUE ); - LCDString("UuVvWwXxYyZz0123456789", (char *)&FONT6x8,1,1+8*5,BLUE ,YELLOW); - LCDString("`-=[]_;',./~!@#$%^&*()", (char *)&FONT6x8,1,1+8*6,BLACK ,CYAN ); - LCDString(" _+{}|:\\\"<>? ", (char *)&FONT6x8,1,1+8*7,BLUE ,MAGENTA); + // test text on different colored backgrounds + LCDString(" The quick brown fox ", (char *)&FONT6x8, 1, 1 + 8 * 0, WHITE, BLACK); + LCDString(" jumped over the ", (char *)&FONT6x8, 1, 1 + 8 * 1, BLACK, WHITE); + LCDString(" lazy dog. ", (char *)&FONT6x8, 1, 1 + 8 * 2, YELLOW, RED); + LCDString(" AaBbCcDdEeFfGgHhIiJj ", (char *)&FONT6x8, 1, 1 + 8 * 3, RED, GREEN); + LCDString(" KkLlMmNnOoPpQqRrSsTt ", (char *)&FONT6x8, 1, 1 + 8 * 4, MAGENTA, BLUE); + LCDString("UuVvWwXxYyZz0123456789", (char *)&FONT6x8, 1, 1 + 8 * 5, BLUE, YELLOW); + LCDString("`-=[]_;',./~!@#$%^&*()", (char *)&FONT6x8, 1, 1 + 8 * 6, BLACK, CYAN); + LCDString(" _+{}|:\\\"<>? ", (char *)&FONT6x8, 1, 1 + 8 * 7, BLUE, MAGENTA); // color bands - LCDFill(0, 1+8* 8, 132, 8, BLACK); - LCDFill(0, 1+8* 9, 132, 8, WHITE); - LCDFill(0, 1+8*10, 132, 8, RED); - LCDFill(0, 1+8*11, 132, 8, GREEN); - LCDFill(0, 1+8*12, 132, 8, BLUE); - LCDFill(0, 1+8*13, 132, 8, YELLOW); - LCDFill(0, 1+8*14, 132, 8, CYAN); - LCDFill(0, 1+8*15, 132, 8, MAGENTA); + LCDFill(0, 1 + 8 * 8, 132, 8, BLACK); + LCDFill(0, 1 + 8 * 9, 132, 8, WHITE); + LCDFill(0, 1 + 8 * 10, 132, 8, RED); + LCDFill(0, 1 + 8 * 11, 132, 8, GREEN); + LCDFill(0, 1 + 8 * 12, 132, 8, BLUE); + LCDFill(0, 1 + 8 * 13, 132, 8, YELLOW); + LCDFill(0, 1 + 8 * 14, 132, 8, CYAN); + LCDFill(0, 1 + 8 * 15, 132, 8, MAGENTA); } diff --git a/armsrc/LCD.h b/armsrc/LCD.h index a21c3eeff..968c90246 100644 --- a/armsrc/LCD.h +++ b/armsrc/LCD.h @@ -124,7 +124,7 @@ void LCDInit(void); void LCDReset(void); void LCDSetXY(unsigned char x, unsigned char y); void LCDSetPixel(unsigned char x, unsigned char y, unsigned char color); -void LCDString (char *lcd_string, const char *font_style,unsigned char x, unsigned char y, unsigned char fcolor, unsigned char bcolor); -void LCDFill (unsigned char xs,unsigned char ys,unsigned char width,unsigned char height, unsigned char color); +void LCDString(char *lcd_string, const char *font_style, unsigned char x, unsigned char y, unsigned char fcolor, unsigned char bcolor); +void LCDFill(unsigned char xs, unsigned char ys, unsigned char width, unsigned char height, unsigned char color); #endif diff --git a/armsrc/Standalone/hf_bog.c b/armsrc/Standalone/hf_bog.c index 826fab0ee..a1197bdbe 100644 --- a/armsrc/Standalone/hf_bog.c +++ b/armsrc/Standalone/hf_bog.c @@ -27,13 +27,13 @@ from the client to view the stored quadlets. // Maximum number of auth attempts per standalone session #define MAX_PWDS_PER_SESSION 64 -uint8_t FindOffsetInFlash() { +uint8_t FindOffsetInFlash() +{ uint8_t mem[4] = { 0x00, 0x00, 0x00, 0x00 }; uint8_t eom[4] = { 0xFF, 0xFF, 0xFF, 0xFF }; uint8_t memcnt = 0; - while (memcnt < 0xFF) - { + while (memcnt < 0xFF) { Flash_ReadData(memcnt, mem, 4); if (memcmp(mem, eom, 4) == 0) { return memcnt; @@ -44,14 +44,15 @@ uint8_t FindOffsetInFlash() { return 0; // wrap-around } -void EraseMemory() { - if (!FlashInit()){ +void EraseMemory() +{ + if (!FlashInit()) { return; } Flash_CheckBusy(BUSY_TIMEOUT); Flash_WriteEnable(); - Flash_Erase4k(0,0); + Flash_Erase4k(0, 0); if (MF_DBGLEVEL > 1) Dbprintf("[!] Erased flash!"); FlashStop(); @@ -59,13 +60,15 @@ void EraseMemory() { } // This is actually copied from SniffIso14443a -void RAMFUNC SniffAndStore(uint8_t param) { +void RAMFUNC SniffAndStore(uint8_t param) +{ iso14443a_setup(FPGA_HF_ISO14443A_SNIFFER); // Allocate memory from BigBuf for some buffers // free all previous allocations first - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); @@ -96,13 +99,13 @@ void RAMFUNC SniffAndStore(uint8_t param) { UartInit(receivedCmd, receivedCmdPar); // Setup and start DMA. - if ( !FpgaSetupSscDma((uint8_t*) dmaBuf, DMA_BUFFER_SIZE) ){ + if (!FpgaSetupSscDma((uint8_t *) dmaBuf, DMA_BUFFER_SIZE)) { if (MF_DBGLEVEL > 1) Dbprintf("FpgaSetupSscDma failed. Exiting"); return; } - tUart* uart = GetUart(); - tDemod* demod = GetDemod(); + tUart *uart = GetUart(); + tDemod *demod = GetDemod(); // We won't start recording the frames that we acquire until we trigger; // a good trigger condition to get started is probably when we see a @@ -155,7 +158,7 @@ void RAMFUNC SniffAndStore(uint8_t param) { if (!TagIsActive) { // no need to try decoding reader data if the tag is sending uint8_t readerdata = (previous_data & 0xF0) | (*data >> 4); - if (MillerDecoding(readerdata, (rsamples-1)*4)) { + if (MillerDecoding(readerdata, (rsamples - 1) * 4)) { LED_C_ON(); // check - if there is a short 7bit request from reader @@ -166,14 +169,14 @@ void RAMFUNC SniffAndStore(uint8_t param) { if (MF_DBGLEVEL > 1) Dbprintf("PWD-AUTH KEY: 0x%02x%02x%02x%02x", receivedCmd[1], receivedCmd[2], receivedCmd[3], receivedCmd[4]); // temporarily save the captured pwd in our array - memcpy(&capturedPwds[4 * auth_attempts], receivedCmd+1, 4); + memcpy(&capturedPwds[4 * auth_attempts], receivedCmd + 1, 4); auth_attempts++; } if (!LogTrace(receivedCmd, uart->len, - uart->startTime*16 - DELAY_READER_AIR2ARM_AS_SNIFFER, - uart->endTime*16 - DELAY_READER_AIR2ARM_AS_SNIFFER, + uart->startTime * 16 - DELAY_READER_AIR2ARM_AS_SNIFFER, + uart->endTime * 16 - DELAY_READER_AIR2ARM_AS_SNIFFER, uart->parity, true)) break; } @@ -190,13 +193,13 @@ void RAMFUNC SniffAndStore(uint8_t param) { // no need to try decoding tag data if the reader is sending - and we cannot afford the time if (!ReaderIsActive) { uint8_t tagdata = (previous_data << 4) | (*data & 0x0F); - if (ManchesterDecoding(tagdata, 0, (rsamples-1)*4)) { + if (ManchesterDecoding(tagdata, 0, (rsamples - 1) * 4)) { LED_B_ON(); if (!LogTrace(receivedResp, demod->len, - demod->startTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, - demod->endTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, + demod->startTime * 16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, + demod->endTime * 16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, demod->parity, false)) break; @@ -239,8 +242,7 @@ void RAMFUNC SniffAndStore(uint8_t param) { uint8_t memoffset = FindOffsetInFlash(); if (MF_DBGLEVEL > 1) Dbprintf("[!] Memory offset = %u", memoffset); - if ((memoffset + 4 * auth_attempts) > 0xFF) - { + if ((memoffset + 4 * auth_attempts) > 0xFF) { // We opt to keep the new data only memoffset = 0; if (MF_DBGLEVEL > 1) Dbprintf("[!] Size of total data > 256 bytes. Discarding the old data."); @@ -248,8 +250,7 @@ void RAMFUNC SniffAndStore(uint8_t param) { // Get previous data from flash mem uint8_t *previousdata = BigBuf_malloc(memoffset); - if (memoffset > 0) - { + if (memoffset > 0) { uint16_t readlen = Flash_ReadData(0, previousdata, memoffset); if (MF_DBGLEVEL > 1) Dbprintf("[!] Read %u bytes from flash mem", readlen); } @@ -271,7 +272,7 @@ void RAMFUNC SniffAndStore(uint8_t param) { uint16_t writelen = Flash_WriteData(0, total_data, memoffset + 4 * auth_attempts); if (MF_DBGLEVEL > 1) Dbprintf("[!] Wrote %u bytes into flash mem", writelen); - // If pwd saved successfully, blink led A three times + // If pwd saved successfully, blink led A three times if (writelen > 0) { SpinErr(0, 200, 5); // blink led A } @@ -283,7 +284,8 @@ void RAMFUNC SniffAndStore(uint8_t param) { } } -void RunMod() { +void RunMod() +{ StandAloneMode(); diff --git a/armsrc/Standalone/hf_colin.c b/armsrc/Standalone/hf_colin.c index 229236376..335269c71 100644 --- a/armsrc/Standalone/hf_colin.c +++ b/armsrc/Standalone/hf_colin.c @@ -112,16 +112,12 @@ void ReadLastTagFromFlash() uint32_t end_time; uint32_t start_time = end_time = GetTickCount(); - for (size_t i = 0; i < len; i += size) - { + for (size_t i = 0; i < len; i += size) { len = MIN((len - i), size); isok = Flash_ReadDataCont(startidx + i, mem, len); - if (isok == len) - { + if (isok == len) { emlSetMem(mem, 0, 64); - } - else - { + } else { DbprintfEx(FLAG_NOLOG, "FlashMem reading failed | %d | %d", len, isok); cjSetCursLeft(); FlashStop(); @@ -153,24 +149,23 @@ void WriteTagToFlash(uint8_t index, size_t size) uint32_t bytes_sent = 0; uint32_t bytes_remaining = len; - uint8_t data[(size * (16 * 64))/1024]; + uint8_t data[(size * (16 * 64)) / 1024]; uint8_t buff[PAGESIZE]; - emlGetMem(data, 0, (size * 64)/1024); + emlGetMem(data, 0, (size * 64) / 1024); - if (!FlashInit()){ + if (!FlashInit()) { return; } Flash_CheckBusy(BUSY_TIMEOUT); Flash_WriteEnable(); - Flash_Erase4k(0,0); + Flash_Erase4k(0, 0); uint32_t end_time; uint32_t start_time = end_time = GetTickCount(); - while (bytes_remaining > 0) - { + while (bytes_remaining > 0) { Flash_CheckBusy(BUSY_TIMEOUT); Flash_WriteEnable(); @@ -184,8 +179,7 @@ void WriteTagToFlash(uint8_t index, size_t size) isok = (res == bytes_in_packet) ? 1 : 0; - if (!isok) - { + if (!isok) { DbprintfEx(FLAG_NOLOG, "FlashMem write FAILEd [offset %u]", bytes_sent); cjSetCursLeft(); SpinOff(100); @@ -223,40 +217,40 @@ void RunMod() uint8_t *keyBlock = NULL; // Where the keys will be held in memory. /* VIGIK EXPIRED DUMP FOR STUDY -Sector 0 -121C7F730208040001FA33F5CB2D021D -44001049164916491649000000000000 -00000000000000000000000000000000 -A0A1A2A3A4A579678800010203040506 -Sector 1 -0F000000000000000000000000000000 -AA0700002102080000740C110600AF13 -000000000000000001740C1108220000 -314B4947495679678800010203040506 -Sector 2 -24E572B923A3D243B402D60CAB576956 -216D6501FC8618B6C426762511AC2DEE -25BF4CEC3618D0BAB3A6E9210D887746 -314B4947495679678800010203040506 -Sector 3 -0FBC41A5D95398E76A1B2029E8EA9735 -088BA2CE732653D0C1147596AFCF94D7 -77B4D91F0442182273A29DEAF7A2D095 -314B4947495679678800010203040506 -Sector 4 -4CEE715866E508CDBC95C640EC9D1E58 -E800457CF8B079414E1B45DD3E6C9317 -77B4D91F0442182273A29DEAF7A2D095 -314B4947495679678800010203040506 -010203040506 0 -Sector 5-0F -00000000000000000000000000000000 -00000000000000000000000000000000 -00000000000000000000000000000000 -FFFFFFFFFFFFFF078069FFFFFFFFFFFF -KEY A : 1KGIV ; -ACCBITS : 796788[00]+VALUE -*/ + Sector 0 + 121C7F730208040001FA33F5CB2D021D + 44001049164916491649000000000000 + 00000000000000000000000000000000 + A0A1A2A3A4A579678800010203040506 + Sector 1 + 0F000000000000000000000000000000 + AA0700002102080000740C110600AF13 + 000000000000000001740C1108220000 + 314B4947495679678800010203040506 + Sector 2 + 24E572B923A3D243B402D60CAB576956 + 216D6501FC8618B6C426762511AC2DEE + 25BF4CEC3618D0BAB3A6E9210D887746 + 314B4947495679678800010203040506 + Sector 3 + 0FBC41A5D95398E76A1B2029E8EA9735 + 088BA2CE732653D0C1147596AFCF94D7 + 77B4D91F0442182273A29DEAF7A2D095 + 314B4947495679678800010203040506 + Sector 4 + 4CEE715866E508CDBC95C640EC9D1E58 + E800457CF8B079414E1B45DD3E6C9317 + 77B4D91F0442182273A29DEAF7A2D095 + 314B4947495679678800010203040506 + 010203040506 0 + Sector 5-0F + 00000000000000000000000000000000 + 00000000000000000000000000000000 + 00000000000000000000000000000000 + FFFFFFFFFFFFFF078069FFFFFFFFFFFF + KEY A : 1KGIV ; + ACCBITS : 796788[00]+VALUE + */ //---------------------------- // Set of keys to be used. @@ -310,8 +304,7 @@ ACCBITS : 796788[00]+VALUE keyBlock = BigBuf_malloc(STKEYS * 6); int mfKeysCnt = sizeof(mfKeys) / sizeof(uint64_t); - for (int mfKeyCounter = 0; mfKeyCounter < mfKeysCnt; mfKeyCounter++) - { + for (int mfKeyCounter = 0; mfKeyCounter < mfKeysCnt; mfKeyCounter++) { num_to_bytes(mfKeys[mfKeyCounter], 6, (uint8_t *)(keyBlock + mfKeyCounter * 6)); } @@ -319,10 +312,8 @@ ACCBITS : 796788[00]+VALUE // and why not a simple memset abuse to 0xffize the whole space in one go ? // uint8_t foundKey[2][40][6]; //= [ {0xff} ]; /* C99 abusal 6.7.8.21 uint8_t foundKey[2][40][6]; - for (uint16_t t = 0; t < 2; t++) - { - for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) - { + for (uint16_t t = 0; t < 2; t++) { + for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) { foundKey[t][sectorNo][0] = 0xFF; foundKey[t][sectorNo][1] = 0xFF; foundKey[t][sectorNo][2] = 0xFF; @@ -363,18 +354,15 @@ failtag: LED_A_ON(); uint8_t ticker = 0; //while (!BUTTON_PRESS() && !iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) - while (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) - { + while (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) { WDT_HIT(); ticker++; - if (ticker % 64 == 0) - { + if (ticker % 64 == 0) { LED_A_INV(); } - if (BUTTON_HELD(10) > 0) - { + if (BUTTON_HELD(10) > 0) { WDT_HIT(); DbprintfEx(FLAG_NOLOG, "\t\t\t[ READING FLASH ]"); ReadLastTagFromFlash(); @@ -393,8 +381,7 @@ failtag: DbprintfEx(FLAG_NOLOG, "\t%sGOT TAG :%s %08x%s", _RED_, _CYAN_, cjcuid, _WHITE_); - if (cjcuid == 0) - { + if (cjcuid == 0) { cjSetCursLeft(); DbprintfEx(FLAG_NOLOG, "%s>>%s BUG: 0000_CJCUID! Retrying...", _RED_, _WHITE_); SpinErr(0, 100, 8); @@ -441,29 +428,22 @@ failtag: // also we could avoid first UID check for every block // then let’s expose this “optimal case” of “well known vigik schemes” : - for (uint8_t type = 0; type < 2 && !err && !trapped; type++) - { - for (int sec = 0; sec < sectorsCnt && !err && !trapped; ++sec) - { + for (uint8_t type = 0; type < 2 && !err && !trapped; type++) { + for (int sec = 0; sec < sectorsCnt && !err && !trapped; ++sec) { key = cjat91_saMifareChkKeys(sec * 4, type, NULL, size, &keyBlock[0], &key64); - if (key == -1) - { + if (key == -1) { err = 1; allKeysFound = false; // used in “portable” imlementation on microcontroller: it reports back the fail and open the standalone lock // cmd_send(CMD_CJB_FSMSTATE_MENU, 0, 0, 0, 0, 0); break; - } - else if (key == -2) - { + } else if (key == -2) { err = 1; // Can't select card. allKeysFound = false; // cmd_send(CMD_CJB_FSMSTATE_MENU, 0, 0, 0, 0, 0); break; - } - else - { + } else { /* BRACE YOURSELF : AS LONG AS WE TRAP A KNOWN KEY, WE STOP CHECKING AND ENFORCE KNOWN SCHEMES */ // uint8_t tosendkey[13]; char tosendkey[13]; @@ -471,243 +451,236 @@ failtag: cjSetCursRight(); DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %012" PRIx64 " ; TYP: %i", sec, key64, type); /*cmd_send(CMD_CJB_INFORM_CLIENT_KEY, 12, sec, type, tosendkey, 12);*/ - switch (key64) - { - ///////////////////////////////////////////////////////// - // COMMON SCHEME 1 : INFINITRON/HEXACT - case 0x484558414354: - cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*STOP*!<<<<<<<<<<<<<<%s", _RED_, _WHITE_); - cjSetCursLeft(); + switch (key64) { + ///////////////////////////////////////////////////////// + // COMMON SCHEME 1 : INFINITRON/HEXACT + case 0x484558414354: + cjSetCursLeft(); + DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*STOP*!<<<<<<<<<<<<<<%s", _RED_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, " .TAG SEEMS %sDETERMINISTIC%s. ", _GREEN_, _WHITE_); - cjSetCursLeft(); + DbprintfEx(FLAG_NOLOG, " .TAG SEEMS %sDETERMINISTIC%s. ", _GREEN_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "%sDetected: %s INFI_HEXACT_VIGIK_TAG%s", _ORANGE_, _CYAN_, _WHITE_); - cjSetCursLeft(); + DbprintfEx(FLAG_NOLOG, "%sDetected: %s INFI_HEXACT_VIGIK_TAG%s", _ORANGE_, _CYAN_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "...%s[%sKey_derivation_schemeTest%s]%s...", _YELLOW_, _GREEN_, _YELLOW_, _GREEN_); - cjSetCursLeft(); + DbprintfEx(FLAG_NOLOG, "...%s[%sKey_derivation_schemeTest%s]%s...", _YELLOW_, _GREEN_, _YELLOW_, _GREEN_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*DONE*!<<<<<<<<<<<<<<%s", _GREEN_, _WHITE_); - ; - // Type 0 / A first - uint16_t t = 0; - for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) - { + DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*DONE*!<<<<<<<<<<<<<<%s", _GREEN_, _WHITE_); + ; + // Type 0 / A first + uint16_t t = 0; + for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) { + num_to_bytes(0x484558414354, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + } + t = 1; + uint16_t sectorNo = 0; + num_to_bytes(0xa22ae129c013, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 1; + num_to_bytes(0x49fae4e3849f, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 2; + num_to_bytes(0x38fcf33072e0, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 3; + num_to_bytes(0x8ad5517b4b18, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 4; + num_to_bytes(0x509359f131b1, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 5; + num_to_bytes(0x6c78928e1317, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 6; + num_to_bytes(0xaa0720018738, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 7; + num_to_bytes(0xa6cac2886412, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 8; + num_to_bytes(0x62d0c424ed8e, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 9; + num_to_bytes(0xe64a986a5d94, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 10; + num_to_bytes(0x8fa1d601d0a2, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 11; + num_to_bytes(0x89347350bd36, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 12; + num_to_bytes(0x66d2b7dc39ef, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 13; + num_to_bytes(0x6bc1e1ae547d, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 14; + num_to_bytes(0x22729a9bd40f, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); + + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + sectorNo = 15; num_to_bytes(0x484558414354, 6, foundKey[t][sectorNo]); sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - } - t = 1; - uint16_t sectorNo = 0; - num_to_bytes(0xa22ae129c013, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + trapped = 1; + break; + ////////////////END OF SCHEME 1////////////////////////////// - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 1; - num_to_bytes(0x49fae4e3849f, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + /////////////////////////////////////// + // COMMON SCHEME 2 : URMET CAPTIVE / COGELEC!/? + case 0x8829da9daf76: + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 2; - num_to_bytes(0x38fcf33072e0, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*STOP*!<<<<<<<<<<<<<<%s", _RED_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 3; - num_to_bytes(0x8ad5517b4b18, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, " .TAG SEEMS %sDETERMINISTIC%s. ", _GREEN_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 4; - num_to_bytes(0x509359f131b1, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "%sDetected :%sURMET_CAPTIVE_VIGIK_TAG%s", _ORANGE_, _CYAN_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 5; - num_to_bytes(0x6c78928e1317, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "...%s[%sKey_derivation_schemeTest%s]%s...", _YELLOW_, _GREEN_, _YELLOW_, _GREEN_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 6; - num_to_bytes(0xaa0720018738, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*DONE*!<<<<<<<<<<<<<<%s", _GREEN_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 7; - num_to_bytes(0xa6cac2886412, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + // emlClearMem(); + // A very weak one... + for (uint16_t t = 0; t < 2; t++) { + for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) { + num_to_bytes(key64, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 8; - num_to_bytes(0x62d0c424ed8e, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + } + } + trapped = 1; + break; + ////////////////END OF SCHEME 2////////////////////////////// - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 9; - num_to_bytes(0xe64a986a5d94, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + /////////////////////////////////////// + // COMMON SCHEME 3 : NORALSY "A-LARON & B-LARON . . . NORAL-B & NORAL-A" + case 0x414c41524f4e: // Thumbs up to the guy who had the idea of such a "mnemotechnical" key pair + case 0x424c41524f4e: + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 10; - num_to_bytes(0x8fa1d601d0a2, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*STOP*!<<<<<<<<<<<<<<%s", _RED_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 11; - num_to_bytes(0x89347350bd36, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, " .TAG SEEMS %sDETERMINISTIC%s. ", _GREEN_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 12; - num_to_bytes(0x66d2b7dc39ef, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "%s Detected :%sNORALSY_VIGIK_TAG %s", _ORANGE_, _CYAN_, _WHITE_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 13; - num_to_bytes(0x6bc1e1ae547d, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "...%s[%sKey_derivation_schemeTest%s]%s...", _YELLOW_, _GREEN_, _YELLOW_, _GREEN_); + cjSetCursLeft(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 14; - num_to_bytes(0x22729a9bd40f, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); + DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*DONE*!<<<<<<<<<<<<<<%s", _GREEN_, _WHITE_); + ; + t = 0; + for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) { + num_to_bytes(0x414c41524f4e, 6, foundKey[t][sectorNo]); + sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], + foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); + cjSetCursRight(); - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - sectorNo = 15; - num_to_bytes(0x484558414354, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); - - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - trapped = 1; - break; - ////////////////END OF SCHEME 1////////////////////////////// - - /////////////////////////////////////// - // COMMON SCHEME 2 : URMET CAPTIVE / COGELEC!/? - case 0x8829da9daf76: - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*STOP*!<<<<<<<<<<<<<<%s", _RED_, _WHITE_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, " .TAG SEEMS %sDETERMINISTIC%s. ", _GREEN_, _WHITE_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "%sDetected :%sURMET_CAPTIVE_VIGIK_TAG%s", _ORANGE_, _CYAN_, _WHITE_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "...%s[%sKey_derivation_schemeTest%s]%s...", _YELLOW_, _GREEN_, _YELLOW_, _GREEN_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*DONE*!<<<<<<<<<<<<<<%s", _GREEN_, _WHITE_); - cjSetCursLeft(); - - // emlClearMem(); - // A very weak one... - for (uint16_t t = 0; t < 2; t++) - { - for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) - { - num_to_bytes(key64, 6, foundKey[t][sectorNo]); + DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); + ; + } + t = 1; + for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) { + num_to_bytes(0x424c41524f4e, 6, foundKey[t][sectorNo]); sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); cjSetCursRight(); DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); } - } - trapped = 1; - break; - ////////////////END OF SCHEME 2////////////////////////////// - - /////////////////////////////////////// - // COMMON SCHEME 3 : NORALSY "A-LARON & B-LARON . . . NORAL-B & NORAL-A" - case 0x414c41524f4e: // Thumbs up to the guy who had the idea of such a "mnemotechnical" key pair - case 0x424c41524f4e: - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*STOP*!<<<<<<<<<<<<<<%s", _RED_, _WHITE_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, " .TAG SEEMS %sDETERMINISTIC%s. ", _GREEN_, _WHITE_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "%s Detected :%sNORALSY_VIGIK_TAG %s", _ORANGE_, _CYAN_, _WHITE_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "...%s[%sKey_derivation_schemeTest%s]%s...", _YELLOW_, _GREEN_, _YELLOW_, _GREEN_); - cjSetCursLeft(); - - DbprintfEx(FLAG_NOLOG, "%s>>>>>>>>>>>>!*DONE*!<<<<<<<<<<<<<<%s", _GREEN_, _WHITE_); - ; - t = 0; - for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) - { - num_to_bytes(0x414c41524f4e, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); - - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - ; - } - t = 1; - for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) - { - num_to_bytes(0x424c41524f4e, 6, foundKey[t][sectorNo]); - sprintf(tosendkey, "%02x%02x%02x%02x%02x%02x", foundKey[t][sectorNo][0], foundKey[t][sectorNo][1], foundKey[t][sectorNo][2], - foundKey[t][sectorNo][3], foundKey[t][sectorNo][4], foundKey[t][sectorNo][5]); - cjSetCursRight(); - - DbprintfEx(FLAG_NOLOG, "SEC: %02x ; KEY : %s ; TYP: %d", sectorNo, tosendkey, t); - } - trapped = 1; - break; - ////////////////END OF SCHEME 3////////////////////////////// + trapped = 1; + break; + ////////////////END OF SCHEME 3////////////////////////////// } /* etc etc for testing schemes quick schemes */ } } } - if (!allKeysFound) - { + if (!allKeysFound) { cjSetCursLeft(); cjTabulize(); DbprintfEx(FLAG_NOLOG, "%s[ FAIL ]%s\r\n->did not found all the keys :'(", _RED_, _WHITE_); @@ -720,11 +693,9 @@ failtag: /* Settings keys to emulator */ emlClearMem(); uint8_t mblock[16]; - for (uint8_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) - { + for (uint8_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) { emlGetMem(mblock, FirstBlockOfSector(sectorNo) + NumBlocksPerSector(sectorNo) - 1, 1); - for (uint8_t t = 0; t < 2; t++) - { + for (uint8_t t = 0; t < 2; t++) { memcpy(mblock + t * 10, foundKey[t][sectorNo], 6); } emlSetMem(mblock, FirstBlockOfSector(sectorNo) + NumBlocksPerSector(sectorNo) - 1, 1); @@ -739,16 +710,14 @@ failtag: DbprintfEx(FLAG_NOLOG, "%s>>%s Filling Emulator <- from A keys...", _YELLOW_, _WHITE_); e_MifareECardLoad(sectorsCnt, 0, 0, &filled); - if (filled != 1) - { + if (filled != 1) { cjSetCursLeft(); DbprintfEx(FLAG_NOLOG, "%s>>%s W_FAILURE ! %sTrying fallback B keys....", _RED_, _ORANGE_, _WHITE_); /* no trace, no dbg */ e_MifareECardLoad(sectorsCnt, 1, 0, &filled); - if (filled != 1) - { + if (filled != 1) { cjSetCursLeft(); DbprintfEx(FLAG_NOLOG, "FATAL:EML_FALLBACKFILL_B"); @@ -856,30 +825,23 @@ void e_MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *dat bool isOK = true; - if (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) - { + if (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) { isOK = false; if (MF_DBGLEVEL >= 1) DbprintfEx(FLAG_RAWPRINT, "Can't select card"); } - for (uint8_t sectorNo = 0; isOK && sectorNo < numSectors; sectorNo++) - { + for (uint8_t sectorNo = 0; isOK && sectorNo < numSectors; sectorNo++) { ui64Key = emlGetKey(sectorNo, keyType); - if (sectorNo == 0) - { - if (isOK && mifare_classic_auth(pcs, cjcuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) - { + if (sectorNo == 0) { + if (isOK && mifare_classic_auth(pcs, cjcuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) { isOK = false; if (MF_DBGLEVEL >= 1) DbprintfEx(FLAG_NOLOG, "Sector[%2d]. Auth error", sectorNo); break; } - } - else - { - if (isOK && mifare_classic_auth(pcs, cjcuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_NESTED)) - { + } else { + if (isOK && mifare_classic_auth(pcs, cjcuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_NESTED)) { isOK = false; if (MF_DBGLEVEL >= 1) DbprintfEx(FLAG_NOLOG, "Sector[%2d]. Auth nested error", sectorNo); @@ -887,38 +849,30 @@ void e_MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *dat } } - for (uint8_t blockNo = 0; isOK && blockNo < NumBlocksPerSector(sectorNo); blockNo++) - { - if (isOK && mifare_classic_readblock(pcs, cjcuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf)) - { + for (uint8_t blockNo = 0; isOK && blockNo < NumBlocksPerSector(sectorNo); blockNo++) { + if (isOK && mifare_classic_readblock(pcs, cjcuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf)) { isOK = false; if (MF_DBGLEVEL >= 1) DbprintfEx(FLAG_NOLOG, "Error reading sector %2d block %2d", sectorNo, blockNo); break; }; - if (isOK) - { + if (isOK) { *datain = 1; - if (blockNo < NumBlocksPerSector(sectorNo) - 1) - { + if (blockNo < NumBlocksPerSector(sectorNo) - 1) { emlSetMem(dataoutbuf, FirstBlockOfSector(sectorNo) + blockNo, 1); - } - else - { // sector trailer, keep the keys, set only the AC + } else { + // sector trailer, keep the keys, set only the AC emlGetMem(dataoutbuf2, FirstBlockOfSector(sectorNo) + blockNo, 1); memcpy(&dataoutbuf2[6], &dataoutbuf[6], 4); emlSetMem(dataoutbuf2, FirstBlockOfSector(sectorNo) + blockNo, 1); } - } - else - { + } else { *datain = 0; } } } - if (mifare_classic_halt(pcs, cjcuid)) - { + if (mifare_classic_halt(pcs, cjcuid)) { if (MF_DBGLEVEL >= 1) DbprintfEx(FLAG_NOLOG, "Halt error"); }; @@ -944,21 +898,18 @@ int cjat91_saMifareChkKeys(uint8_t blockNo, uint8_t keyType, bool clearTrace, ui struct Crypto1State *pcs; pcs = &mpcs; - for (int i = 0; i < keyCount; ++i) - { + for (int i = 0; i < keyCount; ++i) { /* no need for anticollision. just verify tag is still here */ // if (!iso14443a_fast_select_card(cjuid, 0)) { - if (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) - { + if (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) { cjSetCursLeft(); DbprintfEx(FLAG_NOLOG, "%sFATAL%s : E_MF_LOSTTAG", _RED_, _WHITE_); return -1; } uint64_t ui64Key = bytes_to_num(datain + i * 6, 6); - if (mifare_classic_auth(pcs, cjcuid, blockNo, keyType, ui64Key, AUTH_FIRST)) - { + if (mifare_classic_auth(pcs, cjcuid, blockNo, keyType, ui64Key, AUTH_FIRST)) { uint8_t dummy_answer = 0; ReaderTransmit(&dummy_answer, 1, NULL); // wait for the card to become ready again @@ -990,8 +941,7 @@ void saMifareMakeTag(void) DbprintfEx(FLAG_NOLOG, ">> Write to Special:"); int flags = 0; - for (int blockNum = 0; blockNum < 16 * 4; blockNum++) - { + for (int blockNum = 0; blockNum < 16 * 4; blockNum++) { uint8_t mblock[16]; // cnt = 0; emlGetMem(mblock, blockNum, 1); @@ -1007,12 +957,11 @@ void saMifareMakeTag(void) if (blockNum == 16 * 4 - 1) flags = 0x04 + 0x10; - if (saMifareCSetBlock(0, flags & 0xFE, blockNum, mblock)) - { //&& cnt <= retry) { + if (saMifareCSetBlock(0, flags & 0xFE, blockNum, mblock)) { + //&& cnt <= retry) { // cnt++; cjSetCursFRight(); - if (currfline > 53) - { + if (currfline > 53) { currfline = 54; } DbprintfEx(FLAG_NOLOG, "Block :%02x %sOK%s", blockNum, _GREEN_, _WHITE_); @@ -1020,9 +969,7 @@ void saMifareMakeTag(void) // cfail=1; // return; continue; - } - else - { + } else { cjSetCursLeft(); cjSetCursLeft(); @@ -1041,8 +988,7 @@ void saMifareMakeTag(void) break; } */ } - if (cfail == 0) - { + if (cfail == 0) { SpinUp(50); SpinUp(50); SpinUp(50); @@ -1078,52 +1024,43 @@ int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *data uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE]; // reset FPGA and LED - if (workFlags & 0x08) - { + if (workFlags & 0x08) { iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN); // clear_trace(); set_tracing(FALSE); } - while (true) - { + while (true) { cjSetCursLeft(); // get UID from chip - if (workFlags & 0x01) - { - if (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) - { + if (workFlags & 0x01) { + if (!iso14443a_select_card(cjuid, NULL, &cjcuid, true, 0, true)) { DbprintfEx(FLAG_NOLOG, "Can't select card"); break; }; - if (mifare_classic_halt(NULL, cjcuid)) - { + if (mifare_classic_halt(NULL, cjcuid)) { DbprintfEx(FLAG_NOLOG, "Halt error"); break; }; }; // reset chip - if (needWipe) - { + if (needWipe) { ReaderTransmitBitsPar(wupC1, 7, 0, NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wupC1 error"); break; }; ReaderTransmit(wipeC, sizeof(wipeC), NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wipeC error"); break; }; - if (mifare_classic_halt(NULL, cjcuid)) - { + if (mifare_classic_halt(NULL, cjcuid)) { DbprintfEx(FLAG_NOLOG, "Halt error"); break; }; @@ -1131,25 +1068,21 @@ int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *data // chaud // write block - if (workFlags & 0x02) - { + if (workFlags & 0x02) { ReaderTransmitBitsPar(wupC1, 7, 0, NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wupC1 error"); break; }; ReaderTransmit(wupC2, sizeof(wupC2), NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wupC2 errorv"); break; }; } - if ((mifare_sendcmd_short(NULL, 0, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL) != 1) || (receivedAnswer[0] != 0x0a)) - { + if ((mifare_sendcmd_short(NULL, 0, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL) != 1) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "write block send command error"); break; }; @@ -1157,16 +1090,13 @@ int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *data memcpy(d_block, datain, 16); AddCrc14A(d_block, 16); ReaderTransmit(d_block, sizeof(d_block), NULL); - if ((ReaderReceive(receivedAnswer, receivedAnswerPar) != 1) || (receivedAnswer[0] != 0x0a)) - { + if ((ReaderReceive(receivedAnswer, receivedAnswerPar) != 1) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "write block send data error"); break; }; - if (workFlags & 0x04) - { - if (mifare_classic_halt(NULL, cjcuid)) - { + if (workFlags & 0x04) { + if (mifare_classic_halt(NULL, cjcuid)) { cjSetCursFRight(); DbprintfEx(FLAG_NOLOG, "Halt error"); @@ -1178,8 +1108,7 @@ int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *data break; } - if ((workFlags & 0x10) || (!isOK)) - { + if ((workFlags & 0x10) || (!isOK)) { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); } diff --git a/armsrc/Standalone/hf_mattyrun.c b/armsrc/Standalone/hf_mattyrun.c index 1061d9bfe..a6caecef5 100644 --- a/armsrc/Standalone/hf_mattyrun.c +++ b/armsrc/Standalone/hf_mattyrun.c @@ -67,49 +67,40 @@ static int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_ uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE]; // reset FPGA and LED - if (workFlags & 0x08) - { + if (workFlags & 0x08) { iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN); set_tracing(false); } - while (true) - { + while (true) { // get UID from chip - if (workFlags & 0x01) - { - if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) - { + if (workFlags & 0x01) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { DbprintfEx(FLAG_NOLOG, "Can't select card"); break; }; - if (mifare_classic_halt(NULL, cuid)) - { + if (mifare_classic_halt(NULL, cuid)) { DbprintfEx(FLAG_NOLOG, "Halt error"); break; }; }; // reset chip - if (needWipe) - { + if (needWipe) { ReaderTransmitBitsPar(wupC1, 7, 0, NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wupC1 error"); break; }; ReaderTransmit(wipeC, sizeof(wipeC), NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wipeC error"); break; }; - if (mifare_classic_halt(NULL, cuid)) - { + if (mifare_classic_halt(NULL, cuid)) { DbprintfEx(FLAG_NOLOG, "Halt error"); break; }; @@ -117,25 +108,21 @@ static int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_ // chaud // write block - if (workFlags & 0x02) - { + if (workFlags & 0x02) { ReaderTransmitBitsPar(wupC1, 7, 0, NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wupC1 error"); break; }; ReaderTransmit(wupC2, sizeof(wupC2), NULL); - if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) - { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "wupC2 errorv"); break; }; } - if ((mifare_sendcmd_short(NULL, 0, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL) != 1) || (receivedAnswer[0] != 0x0a)) - { + if ((mifare_sendcmd_short(NULL, 0, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL) != 1) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "write block send command error"); break; }; @@ -143,16 +130,13 @@ static int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_ memcpy(d_block, datain, 16); AddCrc14A(d_block, 16); ReaderTransmit(d_block, sizeof(d_block), NULL); - if ((ReaderReceive(receivedAnswer, receivedAnswerPar) != 1) || (receivedAnswer[0] != 0x0a)) - { + if ((ReaderReceive(receivedAnswer, receivedAnswerPar) != 1) || (receivedAnswer[0] != 0x0a)) { DbprintfEx(FLAG_NOLOG, "write block send data error"); break; }; - if (workFlags & 0x04) - { - if (mifare_classic_halt(NULL, cuid)) - { + if (workFlags & 0x04) { + if (mifare_classic_halt(NULL, cuid)) { DbprintfEx(FLAG_NOLOG, "Halt error"); break; }; @@ -162,8 +146,7 @@ static int saMifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_ break; } - if ((workFlags & 0x10) || (!isOK)) - { + if ((workFlags & 0x10) || (!isOK)) { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); } @@ -182,20 +165,17 @@ static int saMifareChkKeys(uint8_t blockNo, uint8_t keyType, bool clearTrace, ui struct Crypto1State *pcs; pcs = &mpcs; - for (int i = 0; i < keyCount; ++i) - { + for (int i = 0; i < keyCount; ++i) { /* no need for anticollision. just verify tag is still here */ // if (!iso14443a_fast_select_card(cjuid, 0)) { - if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) - { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { DbprintfEx(FLAG_NOLOG, "FATAL : E_MF_LOSTTAG"); return -1; } uint64_t ui64Key = bytes_to_num(datain + i * 6, 6); - if (mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) - { + if (mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) { uint8_t dummy_answer = 0; ReaderTransmit(&dummy_answer, 1, NULL); // wait for the card to become ready again @@ -214,7 +194,8 @@ static int saMifareChkKeys(uint8_t blockNo, uint8_t keyType, bool clearTrace, ui } -void RunMod() { +void RunMod() +{ StandAloneMode(); Dbprintf(">> Matty mifare chk/dump/sim a.k.a MattyRun Started <<"); FpgaDownloadAndGo(FPGA_BITSTREAM_HF); @@ -249,7 +230,7 @@ void RunMod() { uint16_t mifare_size = 1024; // Mifare 1k (only 1k supported for now) uint8_t sectorSize = 64; // 1k's sector size is 64 bytes. uint8_t blockNo = 3; // Security block is number 3 for each sector. - uint8_t sectorsCnt = (mifare_size/sectorSize); + uint8_t sectorsCnt = (mifare_size / sectorSize); uint8_t keyType = 2; // Keytype buffer uint64_t key64; // Defines current key uint8_t *keyBlock = NULL; // Where the keys will be held in memory. @@ -284,7 +265,7 @@ void RunMod() { int mfKeysCnt = sizeof(mfKeys) / sizeof(uint64_t); for (int mfKeyCounter = 0; mfKeyCounter < mfKeysCnt; mfKeyCounter++) { - num_to_bytes(mfKeys[mfKeyCounter], 6, (uint8_t*)(keyBlock + mfKeyCounter * 6)); + num_to_bytes(mfKeys[mfKeyCounter], 6, (uint8_t *)(keyBlock + mfKeyCounter * 6)); } /* @@ -294,8 +275,8 @@ void RunMod() { Dbprintf("[+] Printing mf keys"); for (uint8_t keycnt = 0; keycnt < mfKeysCnt; keycnt++) Dbprintf("[-] chk mf key[%2d] %02x%02x%02x%02x%02x%02x", keycnt, - (keyBlock + 6*keycnt)[0], (keyBlock + 6*keycnt)[1], (keyBlock + 6*keycnt)[2], - (keyBlock + 6*keycnt)[3], (keyBlock + 6*keycnt)[4], (keyBlock + 6*keycnt)[5], 6); + (keyBlock + 6 * keycnt)[0], (keyBlock + 6 * keycnt)[1], (keyBlock + 6 * keycnt)[2], + (keyBlock + 6 * keycnt)[3], (keyBlock + 6 * keycnt)[4], (keyBlock + 6 * keycnt)[5], 6); DbpString("--------------------------------------------------------"); } @@ -327,7 +308,7 @@ void RunMod() { for (int type = !keyType; type < 2 && !err; keyType == 2 ? (type++) : (type = 2)) { block = blockNo; for (int sec = 0; sec < sectorsCnt && !err; ++sec) { - Dbprintf("\tCurrent sector:%3d, block:%3d, key type: %c, key count: %i ", sec, block, type ? 'B':'A', mfKeysCnt); + Dbprintf("\tCurrent sector:%3d, block:%3d, key type: %c, key count: %i ", sec, block, type ? 'B' : 'A', mfKeysCnt); key = saMifareChkKeys(block, type, true, size, &keyBlock[0], &key64); if (key == -1) { LED(LED_RED, 50); //red @@ -342,9 +323,9 @@ void RunMod() { validKey[type][sec] = true; keyFound = true; Dbprintf("\t✓ Found valid key: [%02x%02x%02x%02x%02x%02x]\n", - (keyBlock + 6*key)[0], (keyBlock + 6*key)[1], (keyBlock + 6*key)[2], - (keyBlock + 6*key)[3], (keyBlock + 6*key)[4], (keyBlock + 6*key)[5] - ); + (keyBlock + 6 * key)[0], (keyBlock + 6 * key)[1], (keyBlock + 6 * key)[2], + (keyBlock + 6 * key)[3], (keyBlock + 6 * key)[4], (keyBlock + 6 * key)[5] + ); } block < 127 ? (block += 4) : (block += 16); @@ -378,14 +359,14 @@ void RunMod() { for (uint16_t sectorNo = 0; sectorNo < sectorsCnt; sectorNo++) { if (validKey[0][sectorNo] || validKey[1][sectorNo]) { emlGetMem(mblock, FirstBlockOfSector(sectorNo) + NumBlocksPerSector(sectorNo) - 1, 1); // data, block num, blocks count (max 4) - for (uint16_t t = 0; t < 2; t++) { - if (validKey[t][sectorNo]) { - memcpy(mblock + t*10, foundKey[t][sectorNo], 6); - } + for (uint16_t t = 0; t < 2; t++) { + if (validKey[t][sectorNo]) { + memcpy(mblock + t * 10, foundKey[t][sectorNo], 6); } - emlSetMem(mblock, FirstBlockOfSector(sectorNo) + NumBlocksPerSector(sectorNo) - 1, 1); } + emlSetMem(mblock, FirstBlockOfSector(sectorNo) + NumBlocksPerSector(sectorNo) - 1, 1); } + } Dbprintf("\t✓ Found keys have been transferred to the emulator memory."); if (ecfill) { @@ -409,7 +390,7 @@ void RunMod() { LED_B_ON(); // green // assuming arg0==0, use hardcoded uid 0xdeadbeaf - Mifare1ksim( FLAG_4B_UID_IN_DATA | FLAG_UID_IN_EMUL, 0, 0, uid); + Mifare1ksim(FLAG_4B_UID_IN_DATA | FLAG_UID_IN_EMUL, 0, 0, uid); LED_B_OFF(); /* diff --git a/armsrc/Standalone/hf_young.c b/armsrc/Standalone/hf_young.c index 76c7b8c30..d5dfd69bd 100644 --- a/armsrc/Standalone/hf_young.c +++ b/armsrc/Standalone/hf_young.c @@ -18,7 +18,8 @@ typedef struct { } __attribute__((__packed__)) card_clone_t; -void RunMod() { +void RunMod() +{ StandAloneMode(); Dbprintf(">> Craig Young Mifare sniff UID/clone uid 2 magic/sim a.k.a YoungRun Started <<"); FpgaDownloadAndGo(FPGA_BITSTREAM_HF); @@ -59,9 +60,9 @@ void RunMod() { if (cardRead[selected]) { Dbprintf("Button press detected -- replaying card in bank[%d]", selected); break; - } else if (cardRead[(selected+1) % OPTS]) { - Dbprintf("Button press detected but no card in bank[%d] so playing from bank[%d]", selected, (selected+1)%OPTS); - selected = (selected+1) % OPTS; + } else if (cardRead[(selected + 1) % OPTS]) { + Dbprintf("Button press detected but no card in bank[%d] so playing from bank[%d]", selected, (selected + 1) % OPTS); + selected = (selected + 1) % OPTS; break; // playing = 1; } else { Dbprintf("Button press detected but no stored tag to play. (Ignoring button)"); @@ -75,12 +76,12 @@ void RunMod() { Dbprintf("Read UID:"); Dbhexdump(card[selected].uidlen, card[selected].uid, 0); - if (memcmp(uids[(selected+1)%OPTS].uid, card[selected].uid, card[selected].uidlen ) == 0 ) { + if (memcmp(uids[(selected + 1) % OPTS].uid, card[selected].uid, card[selected].uidlen) == 0) { Dbprintf("Card selected has same UID as what is stored in the other bank. Skipping."); } else { uids[selected].sak = card[selected].sak; uids[selected].uidlen = card[selected].uidlen; - memcpy(uids[selected].uid , card[selected].uid, uids[selected].uidlen); + memcpy(uids[selected].uid, card[selected].uid, uids[selected].uidlen); memcpy(uids[selected].atqa, card[selected].atqa, 2); if (uids[selected].uidlen > 4) @@ -110,8 +111,8 @@ void RunMod() { } /* MF Classic UID clone */ - else if (iGotoClone==1) { - iGotoClone=0; + else if (iGotoClone == 1) { + iGotoClone = 0; LEDsoff(); LED(selected + 1, 0); LED(LED_ORANGE, 250); @@ -176,7 +177,7 @@ void RunMod() { MifareCSetBlock(params, 0, newBlock0); MifareCGetBlock(params, 0, testBlock0); - if (memcmp(testBlock0, newBlock0, 16)==0) { + if (memcmp(testBlock0, newBlock0, 16) == 0) { DbpString("Cloned successfull!"); cardRead[selected] = 0; // Only if the card was cloned successfully should we clear it playing = 0; @@ -193,19 +194,19 @@ void RunMod() { // Change where to record (or begin playing) // button_pressed == BUTTON_SINGLE_CLICK && cardRead[selected]) - else if (playing==1) { + else if (playing == 1) { LEDsoff(); LED(selected + 1, 0); // Begin transmitting LED(LED_GREEN, 0); DbpString("Playing"); - for ( ; ; ) { + for (; ;) { // exit from Standalone Mode, send a usbcommand. if (usb_poll_validate_length()) return; int button_action = BUTTON_HELD(1000); - if ( button_action == 0) { // No button action, proceed with sim + if (button_action == 0) { // No button action, proceed with sim uint8_t flags = FLAG_4B_UID_IN_DATA; uint8_t data[USB_CMD_DATA_SIZE] = {0}; // in case there is a read command received we shouldn't break @@ -214,7 +215,7 @@ void RunMod() { uint64_t tmpuid = bytes_to_num(uids[selected].uid, uids[selected].uidlen); - if ( uids[selected].uidlen == 7 ) { + if (uids[selected].uidlen == 7) { flags = FLAG_7B_UID_IN_DATA; Dbprintf("Simulating ISO14443a tag with uid: %014" PRIx64 " [Bank: %d]", tmpuid, selected); } else { diff --git a/armsrc/Standalone/lf_hidbrute.c b/armsrc/Standalone/lf_hidbrute.c index b730246fa..ce5b7d337 100644 --- a/armsrc/Standalone/lf_hidbrute.c +++ b/armsrc/Standalone/lf_hidbrute.c @@ -27,7 +27,8 @@ #include "lf_hidbrute.h" // samy's sniff and repeat routine for LF -void RunMod() { +void RunMod() +{ StandAloneMode(); Dbprintf(">> LF HID corporate bruteforce a.k.a CorporateBrute Started <<"); FpgaDownloadAndGo(FPGA_BITSTREAM_LF); @@ -60,7 +61,7 @@ void RunMod() { DbpString("[=] starting recording"); // wait for button to be released - while(BUTTON_PRESS()) + while (BUTTON_PRESS()) WDT_HIT(); /* need this delay to prevent catching some weird data */ @@ -76,8 +77,7 @@ void RunMod() { // so next button push begins playing what we recorded playing = 0; cardRead = 1; - } - else if (button_pressed > 0 && cardRead == 1) { + } else if (button_pressed > 0 && cardRead == 1) { LEDsoff(); LED(selected + 1, 0); LED(LED_ORANGE, 0); @@ -86,7 +86,7 @@ void RunMod() { Dbprintf("[=] cloning %x %x %08x", selected, high[selected], low[selected]); // wait for button to be released - while(BUTTON_PRESS()) + while (BUTTON_PRESS()) WDT_HIT(); /* need this delay to prevent catching some weird data */ @@ -141,15 +141,13 @@ void RunMod() { playing = !playing; LEDsoff(); LED(selected + 1, 0); - } - else if (playing && selected == 2) - { + } else if (playing && selected == 2) { // Now it work only with HID Corporate 1000 (35bit), but is easily extensible to others RFID. // It is necessary only to calculate the correct parity. // Brute force code // Check if the badge is an HID Corporate 1000 - if( (high[selected] & 0xFFFFFFF8) != 0x28 ) { + if ((high[selected] & 0xFFFFFFF8) != 0x28) { DbpString("[-] Card is not a HID Corporate 1000. Skipping bruteforce."); continue; } @@ -162,7 +160,7 @@ void RunMod() { // Calculate Facility Code and Card Number from high and low uint32_t cardnum = (low[selected] >> 1) & 0xFFFFF; - uint32_t fc = ((high[selected] & 1 ) << 11 ) | (low[selected] >> 21); + uint32_t fc = ((high[selected] & 1) << 11) | (low[selected] >> 21); uint32_t original_cardnum = cardnum; Dbprintf("[=] Proxbrute - starting decrementing card number"); @@ -235,7 +233,7 @@ void RunMod() { LED(selected + 1, 0); } else { - while(BUTTON_PRESS()) + while (BUTTON_PRESS()) WDT_HIT(); } } @@ -247,7 +245,8 @@ out: } // Function that calculate next value for the brutforce of HID corporate 1000 -void hid_corporate_1000_calculate_checksum_and_set( uint32_t *high, uint32_t *low, uint32_t cardnum, uint32_t fc) { +void hid_corporate_1000_calculate_checksum_and_set(uint32_t *high, uint32_t *low, uint32_t cardnum, uint32_t fc) +{ uint32_t new_high = 0; uint32_t new_low = 0; @@ -264,8 +263,8 @@ void hid_corporate_1000_calculate_checksum_and_set( uint32_t *high, uint32_t *lo uint32_t parity_bit_34_low = new_low & 0xB6DB6DB6; n_ones = 0; // Calculate number of ones in low number - for ( i = 1; i != 0; i <<= 1) { - if( parity_bit_34_low & i ) + for (i = 1; i != 0; i <<= 1) { + if (parity_bit_34_low & i) n_ones++; } // Calculate number of ones in high number @@ -282,15 +281,15 @@ void hid_corporate_1000_calculate_checksum_and_set( uint32_t *high, uint32_t *lo n_ones = 0; // Calculate number of ones in low number - for ( i=1; i != 0; i <<= 1) { - if( parity_bit_1_low & i ) + for (i = 1; i != 0; i <<= 1) { + if (parity_bit_1_low & i) n_ones++; } // Calculate number of ones in high number - if ( new_high & 0x1) + if (new_high & 0x1) n_ones++; - if ( new_high & 0x2) + if (new_high & 0x2) n_ones++; // Set parity bit (Odd parity) @@ -301,14 +300,14 @@ void hid_corporate_1000_calculate_checksum_and_set( uint32_t *high, uint32_t *lo n_ones = 0; // Calculate number of ones in low number (all bit of low, bitmask unnecessary) for (i = 1; i != 0; i <<= 1) { - if ( new_low & i ) + if (new_low & i) n_ones++; } // Calculate number of ones in high number - if ( new_high & 0x1) + if (new_high & 0x1) n_ones++; - if ( new_high & 0x2) + if (new_high & 0x2) n_ones++; // Set parity bit (Odd parity) diff --git a/armsrc/Standalone/lf_hidbrute.h b/armsrc/Standalone/lf_hidbrute.h index 4cd2d5631..bc1cf2e31 100644 --- a/armsrc/Standalone/lf_hidbrute.h +++ b/armsrc/Standalone/lf_hidbrute.h @@ -19,6 +19,6 @@ #define OPTS 3 -void hid_corporate_1000_calculate_checksum_and_set( uint32_t *high, uint32_t *low, uint32_t cardnum, uint32_t fc); +void hid_corporate_1000_calculate_checksum_and_set(uint32_t *high, uint32_t *low, uint32_t cardnum, uint32_t fc); #endif /* __LF_HIDBRUTE_H */ \ No newline at end of file diff --git a/armsrc/Standalone/lf_proxbrute.c b/armsrc/Standalone/lf_proxbrute.c index 7d1bdd35d..baa7fe72a 100644 --- a/armsrc/Standalone/lf_proxbrute.c +++ b/armsrc/Standalone/lf_proxbrute.c @@ -12,7 +12,8 @@ #include "lf_proxbrute.h" // samy's sniff and repeat routine for LF -void RunMod() { +void RunMod() +{ StandAloneMode(); Dbprintf(">> LF HID proxII bruteforce a.k.a ProxBrute Started (Brad Antoniewicz) <<"); FpgaDownloadAndGo(FPGA_BITSTREAM_LF); @@ -61,8 +62,7 @@ void RunMod() { // so next button push begins playing what we recorded playing = 0; cardRead = 1; - } - else if (button_pressed > 0 && cardRead == 1) { + } else if (button_pressed > 0 && cardRead == 1) { LEDsoff(); LED(selected + 1, 0); LED(LED_ORANGE, 0); @@ -119,12 +119,12 @@ void RunMod() { worked or not, so its a crap shoot. One option is to time how long it takes to get a valid ID then start from scratch every time. */ - if ( selected == 1 ) { + if (selected == 1) { DbpString("[=] entering ProxBrute Mode"); Dbprintf("[=] current Tag: Selected = %x Facility = %08x ID = %08x", selected, high[selected], low[selected]); LED(LED_ORANGE, 0); LED(LED_RED, 0); - for (uint16_t i = low[selected]-1; i > 0; i--) { + for (uint16_t i = low[selected] - 1; i > 0; i--) { if (BUTTON_PRESS()) { DbpString("[-] told to stop"); break; @@ -156,8 +156,7 @@ void RunMod() { playing = !playing; LEDsoff(); LED(selected + 1, 0); - } - else { + } else { while (BUTTON_PRESS()) WDT_HIT(); } diff --git a/armsrc/Standalone/lf_samyrun.c b/armsrc/Standalone/lf_samyrun.c index 8536260ff..4f4545374 100644 --- a/armsrc/Standalone/lf_samyrun.c +++ b/armsrc/Standalone/lf_samyrun.c @@ -11,7 +11,8 @@ #include "lf_samyrun.h" // samy's sniff and repeat routine for LF -void RunMod() { +void RunMod() +{ StandAloneMode(); Dbprintf(">> LF HID Read/Clone/Sim a.k.a SamyRun Started <<"); FpgaDownloadAndGo(FPGA_BITSTREAM_LF); @@ -64,8 +65,7 @@ void RunMod() { cardRead = 1; gotCard = true; - } - else if (button_pressed > 0 && cardRead == 1) { + } else if (button_pressed > 0 && cardRead == 1) { LEDsoff(); LED(selected + 1, 0); LED(LED_ORANGE, 0); @@ -129,8 +129,7 @@ void RunMod() { playing = !playing; LEDsoff(); LED(selected + 1, 0); - } - else { + } else { while (BUTTON_PRESS()) WDT_HIT(); } diff --git a/armsrc/aes.c b/armsrc/aes.c index dc89a1cf0..fd6543745 100644 --- a/armsrc/aes.c +++ b/armsrc/aes.c @@ -672,9 +672,9 @@ static const unsigned int rcon[] = { ((unsigned int)(pt)[3])) #define PUTU32(ct, st) { (ct)[0] = (unsigned char)((st) >> 24); \ - (ct)[1] = (unsigned char)((st) >> 16); \ - (ct)[2] = (unsigned char)((st) >> 8); \ - (ct)[3] = (unsigned char)(st); } + (ct)[1] = (unsigned char)((st) >> 16); \ + (ct)[2] = (unsigned char)((st) >> 8); \ + (ct)[3] = (unsigned char)(st); } /* * Expand the cipher key into the encryption key schedule and return the @@ -685,7 +685,7 @@ int aes_setkey_enc(unsigned int rk[], const unsigned char cipherKey[], int keyBy int i = 0; unsigned int temp; - rk[0] = GETU32(cipherKey ); + rk[0] = GETU32(cipherKey); rk[1] = GETU32(cipherKey + 4); rk[2] = GETU32(cipherKey + 8); rk[3] = GETU32(cipherKey + 12); @@ -693,11 +693,11 @@ int aes_setkey_enc(unsigned int rk[], const unsigned char cipherKey[], int keyBy for (;;) { temp = rk[3]; rk[4] = rk[0] ^ - (Te4[(temp >> 16) & 0xff] & 0xff000000) ^ - (Te4[(temp >> 8) & 0xff] & 0x00ff0000) ^ - (Te4[(temp ) & 0xff] & 0x0000ff00) ^ - (Te4[(temp >> 24) ] & 0x000000ff) ^ - rcon[i]; + (Te4[(temp >> 16) & 0xff] & 0xff000000) ^ + (Te4[(temp >> 8) & 0xff] & 0x00ff0000) ^ + (Te4[(temp) & 0xff] & 0x0000ff00) ^ + (Te4[(temp >> 24) ] & 0x000000ff) ^ + rcon[i]; rk[5] = rk[1] ^ rk[4]; rk[6] = rk[2] ^ rk[5]; rk[7] = rk[3] ^ rk[6]; @@ -713,11 +713,11 @@ int aes_setkey_enc(unsigned int rk[], const unsigned char cipherKey[], int keyBy for (;;) { temp = rk[ 5]; rk[ 6] = rk[ 0] ^ - (Te4[(temp >> 16) & 0xff] & 0xff000000) ^ - (Te4[(temp >> 8) & 0xff] & 0x00ff0000) ^ - (Te4[(temp ) & 0xff] & 0x0000ff00) ^ - (Te4[(temp >> 24) ] & 0x000000ff) ^ - rcon[i]; + (Te4[(temp >> 16) & 0xff] & 0xff000000) ^ + (Te4[(temp >> 8) & 0xff] & 0x00ff0000) ^ + (Te4[(temp) & 0xff] & 0x0000ff00) ^ + (Te4[(temp >> 24) ] & 0x000000ff) ^ + rcon[i]; rk[ 7] = rk[ 1] ^ rk[ 6]; rk[ 8] = rk[ 2] ^ rk[ 7]; rk[ 9] = rk[ 3] ^ rk[ 8]; @@ -735,11 +735,11 @@ int aes_setkey_enc(unsigned int rk[], const unsigned char cipherKey[], int keyBy for (;;) { temp = rk[ 7]; rk[ 8] = rk[ 0] ^ - (Te4[(temp >> 16) & 0xff] & 0xff000000) ^ - (Te4[(temp >> 8) & 0xff] & 0x00ff0000) ^ - (Te4[(temp ) & 0xff] & 0x0000ff00) ^ - (Te4[(temp >> 24) ] & 0x000000ff) ^ - rcon[i]; + (Te4[(temp >> 16) & 0xff] & 0xff000000) ^ + (Te4[(temp >> 8) & 0xff] & 0x00ff0000) ^ + (Te4[(temp) & 0xff] & 0x0000ff00) ^ + (Te4[(temp >> 24) ] & 0x000000ff) ^ + rcon[i]; rk[ 9] = rk[ 1] ^ rk[ 8]; rk[10] = rk[ 2] ^ rk[ 9]; rk[11] = rk[ 3] ^ rk[10]; @@ -748,10 +748,10 @@ int aes_setkey_enc(unsigned int rk[], const unsigned char cipherKey[], int keyBy } temp = rk[11]; rk[12] = rk[ 4] ^ - (Te4[(temp >> 24) ] & 0xff000000) ^ - (Te4[(temp >> 16) & 0xff] & 0x00ff0000) ^ - (Te4[(temp >> 8) & 0xff] & 0x0000ff00) ^ - (Te4[(temp ) & 0xff] & 0x000000ff); + (Te4[(temp >> 24) ] & 0xff000000) ^ + (Te4[(temp >> 16) & 0xff] & 0x00ff0000) ^ + (Te4[(temp >> 8) & 0xff] & 0x0000ff00) ^ + (Te4[(temp) & 0xff] & 0x000000ff); rk[13] = rk[ 5] ^ rk[12]; rk[14] = rk[ 6] ^ rk[13]; rk[15] = rk[ 7] ^ rk[14]; @@ -779,10 +779,10 @@ int AesGenKeySched(unsigned int rk[], unsigned int rrk[], const unsigned char ci rrk[2] = rk[2]; rrk[3] = rk[3]; - /* - * apply the inverse MixColumn transform to all round keys but the first - * and the last - */ + /* + * apply the inverse MixColumn transform to all round keys but the first + * and the last + */ for (i = 1; i < Nr; i++) { rrk -= 4; rk += 4; @@ -790,22 +790,22 @@ int AesGenKeySched(unsigned int rk[], unsigned int rrk[], const unsigned char ci Td0[Te4[(rk[0] >> 24) ] & 0xff] ^ Td1[Te4[(rk[0] >> 16) & 0xff] & 0xff] ^ Td2[Te4[(rk[0] >> 8) & 0xff] & 0xff] ^ - Td3[Te4[(rk[0] ) & 0xff] & 0xff]; + Td3[Te4[(rk[0]) & 0xff] & 0xff]; rrk[1] = Td0[Te4[(rk[1] >> 24) ] & 0xff] ^ Td1[Te4[(rk[1] >> 16) & 0xff] & 0xff] ^ Td2[Te4[(rk[1] >> 8) & 0xff] & 0xff] ^ - Td3[Te4[(rk[1] ) & 0xff] & 0xff]; + Td3[Te4[(rk[1]) & 0xff] & 0xff]; rrk[2] = Td0[Te4[(rk[2] >> 24) ] & 0xff] ^ Td1[Te4[(rk[2] >> 16) & 0xff] & 0xff] ^ Td2[Te4[(rk[2] >> 8) & 0xff] & 0xff] ^ - Td3[Te4[(rk[2] ) & 0xff] & 0xff]; + Td3[Te4[(rk[2]) & 0xff] & 0xff]; rrk[3] = Td0[Te4[(rk[3] >> 24) ] & 0xff] ^ Td1[Te4[(rk[3] >> 16) & 0xff] & 0xff] ^ Td2[Te4[(rk[3] >> 8) & 0xff] & 0xff] ^ - Td3[Te4[(rk[3] ) & 0xff] & 0xff]; + Td3[Te4[(rk[3]) & 0xff] & 0xff]; } // invert the order of the last round keys rrk -= 4; @@ -833,7 +833,7 @@ void AesEncBlk(AesCtx *pCtx, const unsigned char pt[], unsigned char ct[]) * map byte array block to cipher state * and add initial round key: */ - s0 = GETU32(pt ) ^ rk[0]; + s0 = GETU32(pt) ^ rk[0]; s1 = GETU32(pt + 4) ^ rk[1]; s2 = GETU32(pt + 8) ^ rk[2]; s3 = GETU32(pt + 12) ^ rk[3]; @@ -852,25 +852,25 @@ void AesEncBlk(AesCtx *pCtx, const unsigned char pt[], unsigned char ct[]) Te0[(s0 >> 24) ] ^ Te1[(s1 >> 16) & 0xff] ^ Te2[(s2 >> 8) & 0xff] ^ - Te3[(s3 ) & 0xff] ^ + Te3[(s3) & 0xff] ^ rk[4]; t1 = Te0[(s1 >> 24) ] ^ Te1[(s2 >> 16) & 0xff] ^ Te2[(s3 >> 8) & 0xff] ^ - Te3[(s0 ) & 0xff] ^ + Te3[(s0) & 0xff] ^ rk[5]; t2 = Te0[(s2 >> 24) ] ^ Te1[(s3 >> 16) & 0xff] ^ Te2[(s0 >> 8) & 0xff] ^ - Te3[(s1 ) & 0xff] ^ + Te3[(s1) & 0xff] ^ rk[6]; t3 = Te0[(s3 >> 24) ] ^ Te1[(s0 >> 16) & 0xff] ^ Te2[(s1 >> 8) & 0xff] ^ - Te3[(s2 ) & 0xff] ^ + Te3[(s2) & 0xff] ^ rk[7]; rk += 8; @@ -882,25 +882,25 @@ void AesEncBlk(AesCtx *pCtx, const unsigned char pt[], unsigned char ct[]) Te0[(t0 >> 24) ] ^ Te1[(t1 >> 16) & 0xff] ^ Te2[(t2 >> 8) & 0xff] ^ - Te3[(t3 ) & 0xff] ^ + Te3[(t3) & 0xff] ^ rk[0]; s1 = Te0[(t1 >> 24) ] ^ Te1[(t2 >> 16) & 0xff] ^ Te2[(t3 >> 8) & 0xff] ^ - Te3[(t0 ) & 0xff] ^ + Te3[(t0) & 0xff] ^ rk[1]; s2 = Te0[(t2 >> 24) ] ^ Te1[(t3 >> 16) & 0xff] ^ Te2[(t0 >> 8) & 0xff] ^ - Te3[(t1 ) & 0xff] ^ + Te3[(t1) & 0xff] ^ rk[2]; s3 = Te0[(t3 >> 24) ] ^ Te1[(t0 >> 16) & 0xff] ^ Te2[(t1 >> 8) & 0xff] ^ - Te3[(t2 ) & 0xff] ^ + Te3[(t2) & 0xff] ^ rk[3]; } /* @@ -911,28 +911,28 @@ void AesEncBlk(AesCtx *pCtx, const unsigned char pt[], unsigned char ct[]) (Te4[(t0 >> 24) ] & 0xff000000) ^ (Te4[(t1 >> 16) & 0xff] & 0x00ff0000) ^ (Te4[(t2 >> 8) & 0xff] & 0x0000ff00) ^ - (Te4[(t3 ) & 0xff] & 0x000000ff) ^ + (Te4[(t3) & 0xff] & 0x000000ff) ^ rk[0]; - PUTU32(ct , s0); + PUTU32(ct, s0); s1 = (Te4[(t1 >> 24) ] & 0xff000000) ^ (Te4[(t2 >> 16) & 0xff] & 0x00ff0000) ^ (Te4[(t3 >> 8) & 0xff] & 0x0000ff00) ^ - (Te4[(t0 ) & 0xff] & 0x000000ff) ^ + (Te4[(t0) & 0xff] & 0x000000ff) ^ rk[1]; PUTU32(ct + 4, s1); s2 = (Te4[(t2 >> 24) ] & 0xff000000) ^ (Te4[(t3 >> 16) & 0xff] & 0x00ff0000) ^ (Te4[(t0 >> 8) & 0xff] & 0x0000ff00) ^ - (Te4[(t1 ) & 0xff] & 0x000000ff) ^ + (Te4[(t1) & 0xff] & 0x000000ff) ^ rk[2]; PUTU32(ct + 8, s2); s3 = (Te4[(t3 >> 24) ] & 0xff000000) ^ (Te4[(t0 >> 16) & 0xff] & 0x00ff0000) ^ (Te4[(t1 >> 8) & 0xff] & 0x0000ff00) ^ - (Te4[(t2 ) & 0xff] & 0x000000ff) ^ + (Te4[(t2) & 0xff] & 0x000000ff) ^ rk[3]; PUTU32(ct + 12, s3); @@ -959,10 +959,14 @@ void AesDecBlk(AesCtx *pCtx, const unsigned char ct[], unsigned char pt[]) * map byte array block to cipher state * and add initial round key: */ - v0 = GETU32(ct ); s0 = v0 ^ rk[0]; - v1 = GETU32(ct + 4); s1 = v1 ^ rk[1]; - v2 = GETU32(ct + 8); s2 = v2 ^ rk[2]; - v3 = GETU32(ct + 12); s3 = v3 ^ rk[3]; + v0 = GETU32(ct); + s0 = v0 ^ rk[0]; + v1 = GETU32(ct + 4); + s1 = v1 ^ rk[1]; + v2 = GETU32(ct + 8); + s2 = v2 ^ rk[2]; + v3 = GETU32(ct + 12); + s3 = v3 ^ rk[3]; /* * Nr - 1 full rounds: */ @@ -972,25 +976,25 @@ void AesDecBlk(AesCtx *pCtx, const unsigned char ct[], unsigned char pt[]) Td0[(s0 >> 24) ] ^ Td1[(s3 >> 16) & 0xff] ^ Td2[(s2 >> 8) & 0xff] ^ - Td3[(s1 ) & 0xff] ^ + Td3[(s1) & 0xff] ^ rk[4]; t1 = Td0[(s1 >> 24) ] ^ Td1[(s0 >> 16) & 0xff] ^ Td2[(s3 >> 8) & 0xff] ^ - Td3[(s2 ) & 0xff] ^ + Td3[(s2) & 0xff] ^ rk[5]; t2 = Td0[(s2 >> 24) ] ^ Td1[(s1 >> 16) & 0xff] ^ Td2[(s0 >> 8) & 0xff] ^ - Td3[(s3 ) & 0xff] ^ + Td3[(s3) & 0xff] ^ rk[6]; t3 = Td0[(s3 >> 24) ] ^ Td1[(s2 >> 16) & 0xff] ^ Td2[(s1 >> 8) & 0xff] ^ - Td3[(s0 ) & 0xff] ^ + Td3[(s0) & 0xff] ^ rk[7]; rk += 8; @@ -1002,25 +1006,25 @@ void AesDecBlk(AesCtx *pCtx, const unsigned char ct[], unsigned char pt[]) Td0[(t0 >> 24) ] ^ Td1[(t3 >> 16) & 0xff] ^ Td2[(t2 >> 8) & 0xff] ^ - Td3[(t1 ) & 0xff] ^ + Td3[(t1) & 0xff] ^ rk[0]; s1 = Td0[(t1 >> 24) ] ^ Td1[(t0 >> 16) & 0xff] ^ Td2[(t3 >> 8) & 0xff] ^ - Td3[(t2 ) & 0xff] ^ + Td3[(t2) & 0xff] ^ rk[1]; s2 = Td0[(t2 >> 24) ] ^ Td1[(t1 >> 16) & 0xff] ^ Td2[(t0 >> 8) & 0xff] ^ - Td3[(t3 ) & 0xff] ^ + Td3[(t3) & 0xff] ^ rk[2]; s3 = Td0[(t3 >> 24) ] ^ Td1[(t2 >> 16) & 0xff] ^ Td2[(t1 >> 8) & 0xff] ^ - Td3[(t0 ) & 0xff] ^ + Td3[(t0) & 0xff] ^ rk[3]; } /* @@ -1031,35 +1035,39 @@ void AesDecBlk(AesCtx *pCtx, const unsigned char ct[], unsigned char pt[]) (Td4[(t0 >> 24) ] & 0xff000000) ^ (Td4[(t3 >> 16) & 0xff] & 0x00ff0000) ^ (Td4[(t2 >> 8) & 0xff] & 0x0000ff00) ^ - (Td4[(t1 ) & 0xff] & 0x000000ff) ^ + (Td4[(t1) & 0xff] & 0x000000ff) ^ rk[0]; s1 = (Td4[(t1 >> 24) ] & 0xff000000) ^ (Td4[(t0 >> 16) & 0xff] & 0x00ff0000) ^ (Td4[(t3 >> 8) & 0xff] & 0x0000ff00) ^ - (Td4[(t2 ) & 0xff] & 0x000000ff) ^ + (Td4[(t2) & 0xff] & 0x000000ff) ^ rk[1]; s2 = (Td4[(t2 >> 24) ] & 0xff000000) ^ (Td4[(t1 >> 16) & 0xff] & 0x00ff0000) ^ (Td4[(t0 >> 8) & 0xff] & 0x0000ff00) ^ - (Td4[(t3 ) & 0xff] & 0x000000ff) ^ + (Td4[(t3) & 0xff] & 0x000000ff) ^ rk[2]; s3 = (Td4[(t3 >> 24) ] & 0xff000000) ^ (Td4[(t2 >> 16) & 0xff] & 0x00ff0000) ^ (Td4[(t1 >> 8) & 0xff] & 0x0000ff00) ^ - (Td4[(t0 ) & 0xff] & 0x000000ff) ^ + (Td4[(t0) & 0xff] & 0x000000ff) ^ rk[3]; if (pCtx->Mode) { - s0 = s0 ^ iv[0]; iv[0] = v0; - s1 = s1 ^ iv[1]; iv[1] = v1; - s2 = s2 ^ iv[2]; iv[2] = v2; - s3 = s3 ^ iv[3]; iv[3] = v3; + s0 = s0 ^ iv[0]; + iv[0] = v0; + s1 = s1 ^ iv[1]; + iv[1] = v1; + s2 = s2 ^ iv[2]; + iv[2] = v2; + s3 = s3 ^ iv[3]; + iv[3] = v3; } - PUTU32(pt , s0); + PUTU32(pt, s0); PUTU32(pt + 4, s1); PUTU32(pt + 8, s2); PUTU32(pt + 12, s3); @@ -1082,9 +1090,9 @@ int AesCtxIni(AesCtx *pCtx, unsigned char *pIV, unsigned char *pKey, unsigned in // initialize IV if (pIV != 0) { - pCtx->Iv[0] = GETU32(pIV ); - pCtx->Iv[1] = GETU32(pIV + 4 ); - pCtx->Iv[2] = GETU32(pIV + 8 ); + pCtx->Iv[0] = GETU32(pIV); + pCtx->Iv[1] = GETU32(pIV + 4); + pCtx->Iv[2] = GETU32(pIV + 8); pCtx->Iv[3] = GETU32(pIV + 12); } @@ -1149,18 +1157,18 @@ int main() // initialize context and encrypt data at one end - if( AesCtxIni(&ctx, iv, key, KEY128, CBC) < 0) + if (AesCtxIni(&ctx, iv, key, KEY128, CBC) < 0) printf("init error\n"); - if (AesEncrypt(&ctx, databuf, databuf, sizeof(databuf) ) < 0) + if (AesEncrypt(&ctx, databuf, databuf, sizeof(databuf)) < 0) printf("error in encryption\n"); // initialize context and decrypt cipher at other end - if( AesCtxIni(&ctx, iv, key, KEY128, CBC) < 0) + if (AesCtxIni(&ctx, iv, key, KEY128, CBC) < 0) printf("init error\n"); - if (AesDecrypt(&ctx, databuf, databuf, sizeof(databuf) ) < 0) + if (AesDecrypt(&ctx, databuf, databuf, sizeof(databuf)) < 0) printf("error in decryption\n"); printf("%s\n", databuf); diff --git a/armsrc/aes.h b/armsrc/aes.h index 6934d690c..afe7e5e4e 100644 --- a/armsrc/aes.h +++ b/armsrc/aes.h @@ -8,11 +8,11 @@ // AES context structure typedef struct { - unsigned int Ek[60]; - unsigned int Dk[60]; - unsigned int Iv[4]; - unsigned char Nr; - unsigned char Mode; + unsigned int Ek[60]; + unsigned int Dk[60]; + unsigned int Iv[4]; + unsigned char Nr; + unsigned char Mode; } AesCtx; // key length in bytes diff --git a/armsrc/appmain.c b/armsrc/appmain.c index 2d8730cd7..1982c57a7 100644 --- a/armsrc/appmain.c +++ b/armsrc/appmain.c @@ -27,7 +27,7 @@ #define DEBUG 1 #ifdef WITH_LCD - #include "LCD.h" +#include "LCD.h" #endif #ifdef WITH_SMARTCARD @@ -54,55 +54,59 @@ int ToSendMax = -1; static int ToSendBit; struct common_area common_area __attribute__((section(".commonarea"))); -void ToSendReset(void) { +void ToSendReset(void) +{ ToSendMax = -1; ToSendBit = 8; } -void ToSendStuffBit(int b) { - if(ToSendBit >= 8) { +void ToSendStuffBit(int b) +{ + if (ToSendBit >= 8) { ToSendMax++; ToSend[ToSendMax] = 0; ToSendBit = 0; } - if(b) + if (b) ToSend[ToSendMax] |= (1 << (7 - ToSendBit)); ToSendBit++; - if(ToSendMax >= sizeof(ToSend)) { + if (ToSendMax >= sizeof(ToSend)) { ToSendBit = 0; DbpString("ToSendStuffBit overflowed!"); } } -void PrintToSendBuffer(void) { +void PrintToSendBuffer(void) +{ DbpString("Printing ToSendBuffer:"); Dbhexdump(ToSendMax, ToSend, 0); } -void print_result(char *name, uint8_t *buf, size_t len) { +void print_result(char *name, uint8_t *buf, size_t len) +{ uint8_t *p = buf; uint16_t tmp = len & 0xFFF0; - for(; p-buf < tmp; p += 16) { + for (; p - buf < tmp; p += 16) { Dbprintf("[%s: %02d/%02d] %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", - name, - p-buf, - len, - p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7],p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15] - ); + name, + p - buf, + len, + p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15] + ); } if (len % 16 != 0) { char s[46] = {0}; char *sp = s; - for (; p-buf < len; p++ ) { - sprintf(sp, "%02x ", p[0] ); + for (; p - buf < len; p++) { + sprintf(sp, "%02x ", p[0]); sp += 3; } - Dbprintf("[%s: %02d/%02d] %s", name, p-buf, len, s); + Dbprintf("[%s: %02d/%02d] %s", name, p - buf, len, s); } } @@ -110,25 +114,29 @@ void print_result(char *name, uint8_t *buf, size_t len) { // Debug print functions, to go out over USB, to the usual PC-side client. //============================================================================= -void DbpStringEx(char *str, uint32_t cmd) { +void DbpStringEx(char *str, uint32_t cmd) +{ #if DEBUG uint8_t len = strlen(str); - cmd_send(CMD_DEBUG_PRINT_STRING, len, cmd, 0, (uint8_t*)str, len); + cmd_send(CMD_DEBUG_PRINT_STRING, len, cmd, 0, (uint8_t *)str, len); #endif } -void DbpString(char *str) { +void DbpString(char *str) +{ #if DEBUG DbpStringEx(str, 0); #endif } #if 0 -void DbpIntegers(int x1, int x2, int x3) { - cmd_send(CMD_DEBUG_PRINT_INTEGERS,x1,x2,x3,0,0); +void DbpIntegers(int x1, int x2, int x3) +{ + cmd_send(CMD_DEBUG_PRINT_INTEGERS, x1, x2, x3, 0, 0); } #endif -void DbprintfEx(uint32_t cmd, const char *fmt, ...) { +void DbprintfEx(uint32_t cmd, const char *fmt, ...) +{ #if DEBUG // should probably limit size here; oh well, let's just use a big buffer char output_string[128] = {0x00}; @@ -141,7 +149,8 @@ void DbprintfEx(uint32_t cmd, const char *fmt, ...) { #endif } -void Dbprintf(const char *fmt, ...) { +void Dbprintf(const char *fmt, ...) +{ #if DEBUG // should probably limit size here; oh well, let's just use a big buffer char output_string[128] = {0x00}; @@ -156,9 +165,10 @@ void Dbprintf(const char *fmt, ...) { } // prints HEX & ASCII -void Dbhexdump(int len, uint8_t *d, bool bAsci) { +void Dbhexdump(int len, uint8_t *d, bool bAsci) +{ #if DEBUG - int l=0, i; + int l = 0, i; char ascii[9]; while (len > 0) { @@ -169,7 +179,7 @@ void Dbhexdump(int len, uint8_t *d, bool bAsci) { ascii[l] = 0; // filter safe ascii - for (i=0; i 126) { ascii[i] = '.'; } @@ -191,7 +201,8 @@ void Dbhexdump(int len, uint8_t *d, bool bAsci) { // in ADC units (0 to 1023). Also a routine to average 32 samples and // return that. //----------------------------------------------------------------------------- -static uint16_t ReadAdc(int ch) { +static uint16_t ReadAdc(int ch) +{ // Note: ADC_MODE_PRESCALE and ADC_MODE_SAMPLE_HOLD_TIME are set to the maximum allowed value. // AMPL_HI is are high impedance (10MOhm || 1MOhm) output, the input capacitance of the ADC is 12pF (typical). This results in a time constant @@ -204,9 +215,9 @@ static uint16_t ReadAdc(int ch) { AT91C_BASE_ADC->ADC_CR = AT91C_ADC_SWRST; AT91C_BASE_ADC->ADC_MR = - ADC_MODE_PRESCALE(63) // ADC_CLK = MCK / ((63+1) * 2) = 48MHz / 128 = 375kHz - | ADC_MODE_STARTUP_TIME(1) // Startup Time = (1+1) * 8 / ADC_CLK = 16 / 375kHz = 42,7us Note: must be > 20us - | ADC_MODE_SAMPLE_HOLD_TIME(15); // Sample & Hold Time SHTIM = 15 / ADC_CLK = 15 / 375kHz = 40us + ADC_MODE_PRESCALE(63) // ADC_CLK = MCK / ((63+1) * 2) = 48MHz / 128 = 375kHz + | ADC_MODE_STARTUP_TIME(1) // Startup Time = (1+1) * 8 / ADC_CLK = 16 / 375kHz = 42,7us Note: must be > 20us + | ADC_MODE_SAMPLE_HOLD_TIME(15); // Sample & Hold Time SHTIM = 15 / ADC_CLK = 15 / 375kHz = 40us AT91C_BASE_ADC->ADC_CHER = ADC_CHANNEL(ch); AT91C_BASE_ADC->ADC_CR = AT91C_ADC_START; @@ -217,16 +228,18 @@ static uint16_t ReadAdc(int ch) { } // was static - merlok -uint16_t AvgAdc(int ch) { +uint16_t AvgAdc(int ch) +{ uint16_t a = 0; - for(uint8_t i = 0; i < 32; i++) + for (uint8_t i = 0; i < 32; i++) a += ReadAdc(ch); //division by 32 return (a + 15) >> 5; } -void MeasureAntennaTuning(void) { +void MeasureAntennaTuning(void) +{ uint8_t LF_Results[256]; uint32_t i, adcval = 0, peak = 0, peakv = 0, peakf = 0; @@ -235,20 +248,20 @@ void MeasureAntennaTuning(void) { memset(LF_Results, 0, sizeof(LF_Results)); LED_B_ON(); -/* - * Sweeps the useful LF range of the proxmark from - * 46.8kHz (divisor=255) to 600kHz (divisor=19) and - * read the voltage in the antenna, the result left - * in the buffer is a graph which should clearly show - * the resonating frequency of your LF antenna - * ( hopefully around 95 if it is tuned to 125kHz!) - */ + /* + * Sweeps the useful LF range of the proxmark from + * 46.8kHz (divisor=255) to 600kHz (divisor=19) and + * read the voltage in the antenna, the result left + * in the buffer is a graph which should clearly show + * the resonating frequency of your LF antenna + * ( hopefully around 95 if it is tuned to 125kHz!) + */ FpgaDownloadAndGo(FPGA_BITSTREAM_LF); FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD); SpinDelay(50); - for (i = 255; i >= 19; i--) { + for (i = 255; i >= 19; i--) { WDT_HIT(); FpgaSendCommand(FPGA_CMD_SET_DIVISOR, i); SpinDelay(20); @@ -259,7 +272,7 @@ void MeasureAntennaTuning(void) { v_lf134 = adcval; // voltage at 134Khz LF_Results[i] = adcval >> 9; // scale int to fit in byte for graphing purposes - if(LF_Results[i] > peak) { + if (LF_Results[i] > peak) { peakv = adcval; peakf = i; peak = LF_Results[i]; @@ -274,7 +287,7 @@ void MeasureAntennaTuning(void) { v_hf = (MAX_ADC_HF_VOLTAGE * AvgAdc(ADC_CHAN_HF)) >> 10; // RDV40 will hit the roof, try other ADC channel used in that hardware revision. - if ( v_hf > MAX_ADC_HF_VOLTAGE-300 ) { + if (v_hf > MAX_ADC_HF_VOLTAGE - 300) { v_hf = (MAX_ADC_HF_VOLTAGE_RDV40 * AvgAdc(ADC_CHAN_HF_RDV40)) >> 10; } @@ -291,29 +304,31 @@ void MeasureAntennaTuning(void) { LEDsoff(); } -void MeasureAntennaTuningHf(void) { +void MeasureAntennaTuningHf(void) +{ uint16_t volt = 0; // in mV // Let the FPGA drive the high-frequency antenna around 13.56 MHz. FpgaDownloadAndGo(FPGA_BITSTREAM_HF); FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR); SpinDelay(50); volt = (MAX_ADC_HF_VOLTAGE * AvgAdc(ADC_CHAN_HF)) >> 10; - bool use_high = ( volt > MAX_ADC_HF_VOLTAGE-300 ); + bool use_high = (volt > MAX_ADC_HF_VOLTAGE - 300); - while( !BUTTON_PRESS() ){ + while (!BUTTON_PRESS()) { SpinDelay(20); - if ( !use_high ) { + if (!use_high) { volt = (MAX_ADC_HF_VOLTAGE * AvgAdc(ADC_CHAN_HF)) >> 10; } else { volt = (MAX_ADC_HF_VOLTAGE_RDV40 * AvgAdc(ADC_CHAN_HF_RDV40)) >> 10; } - DbprintfEx(FLAG_NONEWLINE, "%u mV / %5u V", volt, (uint16_t)(volt/1000)); + DbprintfEx(FLAG_NONEWLINE, "%u mV / %5u V", volt, (uint16_t)(volt / 1000)); } FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); DbprintfEx(FLAG_NOOPT, "\n[+] cancelled", 1); } -void ReadMem(int addr) { +void ReadMem(int addr) +{ const uint8_t *data = ((uint8_t *)addr); Dbprintf("%x: %02x %02x %02x %02x %02x %02x %02x %02x", addr, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]); @@ -323,7 +338,8 @@ void ReadMem(int addr) { extern struct version_information version_information; /* bootrom version information is pointed to from _bootphase1_version_pointer */ extern char *_bootphase1_version_pointer, _flash_start, _flash_end, _bootrom_start, _bootrom_end, __data_src_start__; -void SendVersion(void) { +void SendVersion(void) +{ char temp[USB_CMD_DATA_SIZE]; /* Limited data payload in USB packets */ char VersionString[USB_CMD_DATA_SIZE] = { '\0' }; @@ -331,11 +347,11 @@ void SendVersion(void) { * symbol _bootphase1_version_pointer, perform slight sanity checks on the * pointer, then use it. */ - char *bootrom_version = *(char**)&_bootphase1_version_pointer; + char *bootrom_version = *(char **)&_bootphase1_version_pointer; strncat(VersionString, " [ ARM ]\n", sizeof(VersionString) - strlen(VersionString) - 1); - if( bootrom_version < &_flash_start || bootrom_version >= &_flash_end ) { + if (bootrom_version < &_flash_start || bootrom_version >= &_flash_end) { strcat(VersionString, "bootrom version information appears invalid\n"); } else { FormatVersionInformation(temp, sizeof(temp), " bootrom: ", bootrom_version); @@ -361,11 +377,12 @@ void SendVersion(void) { // measure the USB Speed by sending SpeedTestBufferSize bytes to client and measuring the elapsed time. // Note: this mimics GetFromBigbuf(), i.e. we have the overhead of the UsbCommand structure included. -void printUSBSpeed(void) { +void printUSBSpeed(void) +{ Dbprintf("USB Speed"); Dbprintf(" Sending USB packets to client..."); - #define USB_SPEED_TEST_MIN_TIME 1500 // in milliseconds +#define USB_SPEED_TEST_MIN_TIME 1500 // in milliseconds uint8_t *test_data = BigBuf_get_addr(); uint32_t end_time; @@ -388,7 +405,8 @@ void printUSBSpeed(void) { /** * Prints runtime information about the PM3. **/ -void SendStatus(void) { +void SendStatus(void) +{ BigBuf_print_status(); Fpga_print_status(); #ifdef WITH_FLASH @@ -412,7 +430,8 @@ void SendStatus(void) { } // Show some leds in a pattern to identify StandAlone mod is running -void StandAloneMode(void) { +void StandAloneMode(void) +{ DbpString("Stand-alone mode! No PC necessary."); @@ -425,7 +444,8 @@ void StandAloneMode(void) { } // detection of which Standalone Modes is installed // (iceman) -void printStandAloneModes(void) { +void printStandAloneModes(void) +{ DbpString("Installed StandAlone Mode"); @@ -492,18 +512,19 @@ at the same place! :-) LIGHT SCHEME USED: */ static const char LIGHT_SCHEME[] = { - 0x0, /* ---- | No field detected */ - 0x1, /* X--- | 14% of maximum current detected */ - 0x2, /* -X-- | 29% of maximum current detected */ - 0x4, /* --X- | 43% of maximum current detected */ - 0x8, /* ---X | 57% of maximum current detected */ - 0xC, /* --XX | 71% of maximum current detected */ - 0xE, /* -XXX | 86% of maximum current detected */ - 0xF, /* XXXX | 100% of maximum current detected */ + 0x0, /* ---- | No field detected */ + 0x1, /* X--- | 14% of maximum current detected */ + 0x2, /* -X-- | 29% of maximum current detected */ + 0x4, /* --X- | 43% of maximum current detected */ + 0x8, /* ---X | 57% of maximum current detected */ + 0xC, /* --XX | 71% of maximum current detected */ + 0xE, /* -XXX | 86% of maximum current detected */ + 0xF, /* XXXX | 100% of maximum current detected */ }; -static const int LIGHT_LEN = sizeof(LIGHT_SCHEME)/sizeof(LIGHT_SCHEME[0]); +static const int LIGHT_LEN = sizeof(LIGHT_SCHEME) / sizeof(LIGHT_SCHEME[0]); -void ListenReaderField(int limit) { +void ListenReaderField(int limit) +{ #define LF_ONLY 1 #define HF_ONLY 2 #define REPORT_CHANGE 10 // report new values only if they have changed at least by REPORT_CHANGE @@ -530,8 +551,8 @@ void ListenReaderField(int limit) { // iceman, useless, since we are measuring readerfield, not our field. My tests shows a max of 20v from a reader. // RDV40 will hit the roof, try other ADC channel used in that hardware revision. - bool use_high = ( ((MAX_ADC_HF_VOLTAGE * hf_max) >> 10) > MAX_ADC_HF_VOLTAGE-300 ); - if ( use_high ) { + bool use_high = (((MAX_ADC_HF_VOLTAGE * hf_max) >> 10) > MAX_ADC_HF_VOLTAGE - 300); + if (use_high) { hf_av = hf_max = AvgAdc(ADC_CHAN_HF_RDV40); } @@ -540,7 +561,7 @@ void ListenReaderField(int limit) { hf_baseline = hf_av; } - for(;;) { + for (;;) { // Switch modes with button if (BUTTON_PRESS()) { SpinDelay(500); @@ -560,7 +581,7 @@ void ListenReaderField(int limit) { WDT_HIT(); if (limit != HF_ONLY) { - if(mode == 1) { + if (mode == 1) { if (ABS(lf_av - lf_baseline) > REPORT_CHANGE) LED_D_ON(); else @@ -578,7 +599,7 @@ void ListenReaderField(int limit) { } if (limit != LF_ONLY) { - if (mode == 1){ + if (mode == 1) { if (ABS(hf_av - hf_baseline) > REPORT_CHANGE) LED_B_ON(); else @@ -588,7 +609,7 @@ void ListenReaderField(int limit) { hf_av_new = (use_high) ? AvgAdc(ADC_CHAN_HF_RDV40) : AvgAdc(ADC_CHAN_HF); // see if there's a significant change - if(ABS(hf_av - hf_av_new) > REPORT_CHANGE) { + if (ABS(hf_av - hf_av_new) > REPORT_CHANGE) { Dbprintf("HF 13.56MHz Field Change: %5dmV", (MAX_ADC_HF_VOLTAGE * hf_av_new) >> 10); hf_av = hf_av_new; if (hf_av > hf_max) @@ -604,7 +625,7 @@ void ListenReaderField(int limit) { display_val = hf_av; display_max = hf_max; } else { /* Pick one at random */ - if( (hf_max - hf_baseline) > (lf_max - lf_baseline) ) { + if ((hf_max - hf_baseline) > (lf_max - lf_baseline)) { display_val = hf_av; display_max = hf_max; } else { @@ -612,12 +633,16 @@ void ListenReaderField(int limit) { display_max = lf_max; } } - for (i=0; i= ((display_max/LIGHT_LEN)*i) && display_val <= ((display_max/LIGHT_LEN)*(i+1))) { - if (LIGHT_SCHEME[i] & 0x1) LED_C_ON(); else LED_C_OFF(); - if (LIGHT_SCHEME[i] & 0x2) LED_A_ON(); else LED_A_OFF(); - if (LIGHT_SCHEME[i] & 0x4) LED_B_ON(); else LED_B_OFF(); - if (LIGHT_SCHEME[i] & 0x8) LED_D_ON(); else LED_D_OFF(); + for (i = 0; i < LIGHT_LEN; i++) { + if (display_val >= ((display_max / LIGHT_LEN)*i) && display_val <= ((display_max / LIGHT_LEN) * (i + 1))) { + if (LIGHT_SCHEME[i] & 0x1) LED_C_ON(); + else LED_C_OFF(); + if (LIGHT_SCHEME[i] & 0x2) LED_A_ON(); + else LED_A_OFF(); + if (LIGHT_SCHEME[i] & 0x4) LED_B_ON(); + else LED_B_OFF(); + if (LIGHT_SCHEME[i] & 0x8) LED_D_ON(); + else LED_D_OFF(); break; } } @@ -625,15 +650,16 @@ void ListenReaderField(int limit) { } } -void UsbPacketReceived(uint8_t *packet, int len) { +void UsbPacketReceived(uint8_t *packet, int len) +{ UsbCommand *c = (UsbCommand *)packet; //Dbprintf("received %d bytes, with command: 0x%04x and args: %d %d %d", len, c->cmd, c->arg[0], c->arg[1], c->arg[2]); - switch(c->cmd) { + switch (c->cmd) { #ifdef WITH_LF case CMD_SET_LF_T55XX_CONFIG: - setT55xxConfig( c->arg[0], (t55xx_config *) c->d.asBytes); + setT55xxConfig(c->arg[0], (t55xx_config *) c->d.asBytes); break; case CMD_SET_LF_SAMPLING_CONFIG: setSamplingConfig((sample_config *) c->d.asBytes); @@ -692,7 +718,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { ReadTItag(); break; case CMD_WRITE_TI_TYPE: - WriteTItag(c->arg[0],c->arg[1],c->arg[2]); + WriteTItag(c->arg[0], c->arg[1], c->arg[2]); break; case CMD_SIMULATE_TAG_125K: LED_A_ON(); @@ -709,7 +735,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { CopyIndala224toT55x7( c->d.asDwords[0], c->d.asDwords[1], c->d.asDwords[2], c->d.asDwords[3], c->d.asDwords[4], c->d.asDwords[5], c->d.asDwords[6] - ); + ); break; case CMD_T55XX_READ_BLOCK: { T55xxReadBlock(c->arg[0], c->arg[1], c->arg[2]); @@ -738,7 +764,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { c->arg[0], c->arg[1], c->arg[2] - ); + ); break; case CMD_EM4X_READ_WORD: EM4xReadWord(c->arg[0], c->arg[1], c->arg[2]); @@ -765,25 +791,25 @@ void UsbPacketReceived(uint8_t *packet, int len) { SnoopHitag(c->arg[0]); break; case CMD_SIMULATE_HITAG: // Simulate Hitag tag, args = memory content - SimulateHitagTag((bool)c->arg[0],(byte_t*)c->d.asBytes); + SimulateHitagTag((bool)c->arg[0], (byte_t *)c->d.asBytes); break; case CMD_READER_HITAG: // Reader for Hitag tags, args = type and function - ReaderHitag((hitag_function)c->arg[0],(hitag_data*)c->d.asBytes); + ReaderHitag((hitag_function)c->arg[0], (hitag_data *)c->d.asBytes); break; case CMD_SIMULATE_HITAG_S:// Simulate Hitag s tag, args = memory content - SimulateHitagSTag((bool)c->arg[0],(byte_t*)c->d.asBytes); + SimulateHitagSTag((bool)c->arg[0], (byte_t *)c->d.asBytes); break; case CMD_TEST_HITAGS_TRACES:// Tests every challenge within the given file - check_challenges((bool)c->arg[0],(byte_t*)c->d.asBytes); + check_challenges((bool)c->arg[0], (byte_t *)c->d.asBytes); break; case CMD_READ_HITAG_S: //Reader for only Hitag S tags, args = key or challenge - ReadHitagS((hitag_function)c->arg[0],(hitag_data*)c->d.asBytes); + ReadHitagS((hitag_function)c->arg[0], (hitag_data *)c->d.asBytes); break; case CMD_WR_HITAG_S: //writer for Hitag tags args=data to write,page and key or challenge if ((hitag_function)c->arg[0] < 10) { - WritePageHitagS((hitag_function)c->arg[0],(hitag_data*)c->d.asBytes,c->arg[2]); + WritePageHitagS((hitag_function)c->arg[0], (hitag_data *)c->d.asBytes, c->arg[2]); } else if ((hitag_function)c->arg[0] >= 10) { - WriterHitag((hitag_function)c->arg[0],(hitag_data*)c->d.asBytes, c->arg[2]); + WriterHitag((hitag_function)c->arg[0], (hitag_data *)c->d.asBytes, c->arg[2]); } break; #endif @@ -892,10 +918,10 @@ void UsbPacketReceived(uint8_t *packet, int len) { MifareReadBlock(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes); break; case CMD_MIFAREU_READBL: - MifareUReadBlock(c->arg[0],c->arg[1], c->d.asBytes); + MifareUReadBlock(c->arg[0], c->arg[1], c->d.asBytes); break; case CMD_MIFAREUC_AUTH: - MifareUC_Auth(c->arg[0],c->d.asBytes); + MifareUC_Auth(c->arg[0], c->d.asBytes); break; case CMD_MIFAREU_READCARD: MifareUReadCard(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes); @@ -910,8 +936,8 @@ void UsbPacketReceived(uint8_t *packet, int len) { MifareWriteBlock(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes); break; //case CMD_MIFAREU_WRITEBL_COMPAT: - //MifareUWriteBlockCompat(c->arg[0], c->d.asBytes); - //break; + //MifareUWriteBlockCompat(c->arg[0], c->d.asBytes); + //break; case CMD_MIFAREU_WRITEBL: MifareUWriteBlock(c->arg[0], c->arg[1], c->d.asBytes); break; @@ -1050,11 +1076,11 @@ void UsbPacketReceived(uint8_t *packet, int len) { SmartCardAtr(); break; } - case CMD_SMART_SETBAUD:{ + case CMD_SMART_SETBAUD: { SmartCardSetBaud(c->arg[0]); break; } - case CMD_SMART_SETCLOCK:{ + case CMD_SMART_SETCLOCK: { SmartCardSetClock(c->arg[0]); break; } @@ -1065,8 +1091,8 @@ void UsbPacketReceived(uint8_t *packet, int len) { case CMD_SMART_UPLOAD: { // upload file from client uint8_t *mem = BigBuf_get_addr(); - memcpy( mem + c->arg[0], c->d.asBytes, USB_CMD_DATA_SIZE); - cmd_send(CMD_ACK,1,0,0,0,0); + memcpy(mem + c->arg[0], c->d.asBytes, USB_CMD_DATA_SIZE); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); break; } case CMD_SMART_UPGRADE: { @@ -1101,14 +1127,14 @@ void UsbPacketReceived(uint8_t *packet, int len) { char dest[USB_CMD_DATA_SIZE] = { '\0' }; - static const char* welcome = "Proxmark3 Serial interface via FPC ready\n"; + static const char *welcome = "Proxmark3 Serial interface via FPC ready\n"; strncat(dest, welcome, sizeof(dest) - strlen(dest) - 1); sprintf(dest + strlen(dest) - 1, "| bytes 0x%02x 0x%02x 0x%02x 0x%02x \n" - , c->d.asBytes[0] - , c->d.asBytes[1] - , c->d.asBytes[2] - , c->d.asBytes[3] - ); + , c->d.asBytes[0] + , c->d.asBytes[1] + , c->d.asBytes[2] + , c->d.asBytes[3] + ); UsbCommand txcmd = { CMD_DEBUG_PRINT_STRING, { strlen(dest), 0, 0 } }; memcpy(txcmd.d.asBytes, dest, sizeof(dest)); @@ -1116,7 +1142,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { LED_A_ON(); usart_init(); - usart_writebuffer((uint8_t*)&txcmd, sizeof(UsbCommand)); + usart_writebuffer((uint8_t *)&txcmd, sizeof(UsbCommand)); //usb cmd_send(CMD_DEBUG_PRINT_STRING, strlen(dest), 0, 0, dest, strlen(dest)); @@ -1140,7 +1166,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { //cmd_send(CMD_DEBUG_PRINT_STRING, strlen(dest), 0, 0, dest, strlen(dest)); - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); StopTicks(); break; } @@ -1180,11 +1206,11 @@ void UsbPacketReceived(uint8_t *packet, int len) { // arg2 = BigBuf tracelen //Dbprintf("transfer to client parameters: %" PRIu32 " | %" PRIu32 " | %" PRIu32, startidx, numofbytes, c->arg[2]); - for(size_t i = 0; i < numofbytes; i += USB_CMD_DATA_SIZE) { - len = MIN( (numofbytes - i), USB_CMD_DATA_SIZE); + for (size_t i = 0; i < numofbytes; i += USB_CMD_DATA_SIZE) { + len = MIN((numofbytes - i), USB_CMD_DATA_SIZE); isok = cmd_send(CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K, i, len, BigBuf_get_traceLen(), mem + startidx + i, len); if (isok != 0) - Dbprintf("transfer to client failed :: | bytes between %d - %d (%d)", i, i+len, len); + Dbprintf("transfer to client failed :: | bytes between %d - %d (%d)", i, i + len, len); } // Trigger a finish downloading signal with an ACK frame // iceman, when did sending samplingconfig array got attached here?!? @@ -1202,15 +1228,15 @@ void UsbPacketReceived(uint8_t *packet, int len) { // to be able to use this one for uploading data to device // arg1 = 0 upload for LF usage // 1 upload for HF usage - #define FPGA_LF 1 - if ( c->arg[1] == FPGA_LF ) +#define FPGA_LF 1 + if (c->arg[1] == FPGA_LF) FpgaDownloadAndGo(FPGA_BITSTREAM_LF); else FpgaDownloadAndGo(FPGA_BITSTREAM_HF); uint8_t *mem = BigBuf_get_addr(); - memcpy( mem + c->arg[0], c->d.asBytes, USB_CMD_DATA_SIZE); - cmd_send(CMD_ACK,1,0,0,0,0); + memcpy(mem + c->arg[0], c->d.asBytes, USB_CMD_DATA_SIZE); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); break; } case CMD_DOWNLOAD_EML_BIGBUF: { @@ -1229,7 +1255,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { len = MIN((numofbytes - i), USB_CMD_DATA_SIZE); isok = cmd_send(CMD_DOWNLOADED_EML_BIGBUF, i, len, 0, mem + startidx + i, len); if (isok != 0) - Dbprintf("transfer to client failed :: | bytes between %d - %d (%d)", i, i+len, len); + Dbprintf("transfer to client failed :: | bytes between %d - %d (%d)", i, i + len, len); } // Trigger a finish downloading signal with an ACK frame cmd_send(CMD_ACK, 1, 0, 0, 0, 0); @@ -1259,12 +1285,12 @@ void UsbPacketReceived(uint8_t *packet, int len) { uint8_t *mem = BigBuf_malloc(size); - for(size_t i = 0; i < len; i += size) { + for (size_t i = 0; i < len; i += size) { len = MIN((len - i), size); Dbprintf("FlashMem reading | %d | %d | %d |", startidx + i, i, len); isok = Flash_ReadDataCont(startidx + i, mem, len); - if ( isok == len ) { + if (isok == len) { print_result("Chunk: ", mem, len); } else { Dbprintf("FlashMem reading failed | %d | %d", len, isok); @@ -1282,7 +1308,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { uint16_t res = 0; uint32_t startidx = c->arg[0]; uint16_t len = c->arg[1]; - uint8_t* data = c->d.asBytes; + uint8_t *data = c->d.asBytes; uint32_t tmp = startidx + len; @@ -1293,9 +1319,9 @@ void UsbPacketReceived(uint8_t *packet, int len) { Flash_CheckBusy(BUSY_TIMEOUT); Flash_WriteEnable(); - if ( startidx == DEFAULT_T55XX_KEYS_OFFSET ) + if (startidx == DEFAULT_T55XX_KEYS_OFFSET) Flash_Erase4k(3, 0xC); - else if (startidx == DEFAULT_MF_KEYS_OFFSET ) + else if (startidx == DEFAULT_MF_KEYS_OFFSET) Flash_Erase4k(3, 0xB); else if (startidx == DEFAULT_ICLASS_KEYS_OFFSET) Flash_Erase4k(3, 0xA); @@ -1304,29 +1330,29 @@ void UsbPacketReceived(uint8_t *packet, int len) { Flash_WriteEnable(); // inside 256b page? - if ( (tmp & 0xFF) != 0) { + if ((tmp & 0xFF) != 0) { - // is offset+len larger than a page - tmp = (startidx & 0xFF ) + len; - if (tmp > 0xFF ) { + // is offset+len larger than a page + tmp = (startidx & 0xFF) + len; + if (tmp > 0xFF) { - // data spread over two pages. + // data spread over two pages. - // offset xxxx10, - uint8_t first_len = (~startidx & 0xFF)+1; + // offset xxxx10, + uint8_t first_len = (~startidx & 0xFF) + 1; - // first mem page - res = Flash_WriteDataCont(startidx, data, first_len); + // first mem page + res = Flash_WriteDataCont(startidx, data, first_len); - // second mem page - res = Flash_WriteDataCont(startidx + first_len, data + first_len, len - first_len); + // second mem page + res = Flash_WriteDataCont(startidx + first_len, data + first_len, len - first_len); - isok = (res == (len - first_len)) ? 1 : 0; + isok = (res == (len - first_len)) ? 1 : 0; - } else { - res = Flash_WriteDataCont(startidx, data, len); - isok = (res == len) ? 1 : 0; - } + } else { + res = Flash_WriteDataCont(startidx, data, len); + isok = (res == len) ? 1 : 0; + } } else { res = Flash_WriteDataCont(startidx, data, len); isok = (res == len) ? 1 : 0; @@ -1342,13 +1368,13 @@ void UsbPacketReceived(uint8_t *packet, int len) { uint8_t page = c->arg[0]; uint8_t initalwipe = c->arg[1]; bool isok = false; - if ( initalwipe ) { + if (initalwipe) { isok = Flash_WipeMemory(); cmd_send(CMD_ACK, isok, 0, 0, 0, 0); LED_B_OFF(); break; } - if ( page < 3) + if (page < 3) isok = Flash_WipeMemoryPage(page); cmd_send(CMD_ACK, isok, 0, 0, 0, 0); @@ -1375,14 +1401,14 @@ void UsbPacketReceived(uint8_t *packet, int len) { len = MIN((numofbytes - i), USB_CMD_DATA_SIZE); isok = Flash_ReadDataCont(startidx + i, mem, len); - if (!isok ) + if (!isok) Dbprintf("reading flash memory failed :: | bytes between %d - %d", i, len); isok = cmd_send(CMD_FLASHMEM_DOWNLOADED, i, len, 0, mem, len); if (isok != 0) Dbprintf("transfer to client failed :: | bytes between %d - %d", i, len); } - FlashStop(); + FlashStop(); cmd_send(CMD_ACK, 1, 0, 0, 0, 0); LED_B_OFF(); @@ -1391,12 +1417,12 @@ void UsbPacketReceived(uint8_t *packet, int len) { case CMD_FLASHMEM_INFO: { LED_B_ON(); - rdv40_validation_t *info = (rdv40_validation_t*)BigBuf_malloc( sizeof(rdv40_validation_t) ); + rdv40_validation_t *info = (rdv40_validation_t *)BigBuf_malloc(sizeof(rdv40_validation_t)); bool isok = Flash_ReadData(FLASH_MEM_SIGNATURE_OFFSET, info->signature, FLASH_MEM_SIGNATURE_LEN); if (FlashInit()) { - Flash_UniqueID( info->flashid); + Flash_UniqueID(info->flashid); FlashStop(); } cmd_send(CMD_ACK, isok, 0, 0, info, sizeof(rdv40_validation_t)); @@ -1412,14 +1438,22 @@ void UsbPacketReceived(uint8_t *packet, int len) { break; case CMD_SET_ADC_MUX: - switch(c->arg[0]) { - case 0: SetAdcMuxFor(GPIO_MUXSEL_LOPKD); break; - case 2: SetAdcMuxFor(GPIO_MUXSEL_HIPKD); break; + switch (c->arg[0]) { + case 0: + SetAdcMuxFor(GPIO_MUXSEL_LOPKD); + break; + case 2: + SetAdcMuxFor(GPIO_MUXSEL_HIPKD); + break; #ifndef WITH_FPC - case 1: SetAdcMuxFor(GPIO_MUXSEL_LORAW); break; - case 3: SetAdcMuxFor(GPIO_MUXSEL_HIRAW); break; + case 1: + SetAdcMuxFor(GPIO_MUXSEL_LORAW); + break; + case 3: + SetAdcMuxFor(GPIO_MUXSEL_HIRAW); + break; #endif - } + } break; case CMD_VERSION: @@ -1429,7 +1463,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { SendStatus(); break; case CMD_PING: - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); break; #ifdef WITH_LCD case CMD_LCD_RESET: @@ -1448,17 +1482,17 @@ void UsbPacketReceived(uint8_t *packet, int len) { SpinDelay(1000); AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST; // We're going to reset, and the bootrom will take control. - for(;;) {} + for (;;) {} break; case CMD_START_FLASH: - if(common_area.flags.bootrom_present) { + if (common_area.flags.bootrom_present) { common_area.command = COMMON_AREA_COMMAND_ENTER_FLASH_MODE; } usb_disable(); AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST; // We're going to flash, and the bootrom will take control. - for(;;) {} + for (;;) {} break; case CMD_DEVICE_INFO: { @@ -1466,21 +1500,22 @@ void UsbPacketReceived(uint8_t *packet, int len) { if (common_area.flags.bootrom_present) { dev_info |= DEVICE_INFO_FLAG_BOOTROM_PRESENT; } - cmd_send(CMD_DEVICE_INFO,dev_info,0,0,0,0); + cmd_send(CMD_DEVICE_INFO, dev_info, 0, 0, 0, 0); break; - } + } default: - Dbprintf("%s: 0x%04x","unknown command:", c->cmd); + Dbprintf("%s: 0x%04x", "unknown command:", c->cmd); break; } } -void __attribute__((noreturn)) AppMain(void) { +void __attribute__((noreturn)) AppMain(void) +{ SpinDelay(100); clear_trace(); - if(common_area.magic != COMMON_AREA_MAGIC || common_area.version != 1) { + if (common_area.magic != COMMON_AREA_MAGIC || common_area.version != 1) { /* Initialize common area */ memset(&common_area, 0, sizeof(common_area)); common_area.magic = COMMON_AREA_MAGIC; @@ -1537,12 +1572,12 @@ void __attribute__((noreturn)) AppMain(void) { uint8_t rx[sizeof(UsbCommand)]; - for(;;) { + for (;;) { WDT_HIT(); // Check if there is a usb packet available if (usb_poll_validate_length()) { - if (usb_read(rx, sizeof(rx)) ) + if (usb_read(rx, sizeof(rx))) UsbPacketReceived(rx, sizeof(rx)); } #ifdef WITH_FPC @@ -1558,11 +1593,11 @@ void __attribute__((noreturn)) AppMain(void) { // Press button for one second to enter a possible standalone mode if (BUTTON_HELD(1000) > 0) { -/* -* So this is the trigger to execute a standalone mod. Generic entrypoint by following the standalone/standalone.h headerfile -* All standalone mod "main loop" should be the RunMod() function. -* Since the standalone is either LF or HF, the somewhat bisarr defines below exists. -*/ + /* + * So this is the trigger to execute a standalone mod. Generic entrypoint by following the standalone/standalone.h headerfile + * All standalone mod "main loop" should be the RunMod() function. + * Since the standalone is either LF or HF, the somewhat bisarr defines below exists. + */ #if defined (WITH_LF) && ( defined (WITH_LF_SAMYRUN) || defined (WITH_LF_HIDBRUTE) || defined (WITH_LF_PROXBRUTE) ) RunMod(); #endif diff --git a/armsrc/apps.h b/armsrc/apps.h index 4a775702a..36c865900 100644 --- a/armsrc/apps.h +++ b/armsrc/apps.h @@ -110,7 +110,7 @@ void EM4xReadWord(uint8_t addr, uint32_t pwd, uint8_t usepwd); void EM4xWriteWord(uint32_t flag, uint32_t data, uint32_t pwd); void Cotag(uint32_t arg0); void setT55xxConfig(uint8_t arg0, t55xx_config *c); -t55xx_config * getT55xxConfig(void); +t55xx_config *getT55xxConfig(void); void printT55xxConfig(void); void loadT55xxConfig(void); @@ -133,11 +133,11 @@ void ReaderIso14443a(UsbCommand *c); void GetParity(const uint8_t *pbtCmd, uint16_t len, uint8_t *parity); void iso14a_set_trigger(bool enable); // also used in emv -bool prepare_allocated_tag_modulation(tag_response_info_t * response_info); +bool prepare_allocated_tag_modulation(tag_response_info_t *response_info); int GetIso14443aCommandFromReader(uint8_t *received, uint8_t *parity, int *len); // epa.h -void EPA_PACE_Collect_Nonce(UsbCommand * c); +void EPA_PACE_Collect_Nonce(UsbCommand *c); void EPA_PACE_Replay(UsbCommand *c); // mifarecmd.h @@ -169,35 +169,35 @@ void OnSuccessMagic(); void OnErrorMagic(uint8_t reason); int32_t dist_nt(uint32_t nt1, uint32_t nt2); -void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ); +void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype); //void RAMFUNC SniffMifare(uint8_t param); //desfire -void Mifare_DES_Auth1(uint8_t arg0,uint8_t *datain); +void Mifare_DES_Auth1(uint8_t arg0, uint8_t *datain); void Mifare_DES_Auth2(uint32_t arg0, uint8_t *datain); // mifaredesfire.h bool InitDesfireCard(); -void MifareSendCommand(uint8_t arg0,uint8_t arg1, uint8_t *datain); +void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain); void MifareDesfireGetInformation(); -void MifareDES_Auth1(uint8_t arg0,uint8_t arg1,uint8_t arg2, uint8_t *datain); -void ReaderMifareDES(uint32_t param, uint32_t param2, uint8_t * datain); +void MifareDES_Auth1(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain); +void ReaderMifareDES(uint32_t param, uint32_t param2, uint8_t *datain); int DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout); -size_t CreateAPDU( uint8_t *datain, size_t len, uint8_t *dataout); +size_t CreateAPDU(uint8_t *datain, size_t len, uint8_t *dataout); void OnSuccess(); void OnError(uint8_t reason); // desfire_crypto.h -void *mifare_cryto_preprocess_data (desfiretag_t tag, void *data, size_t *nbytes, size_t offset, int communication_settings); -void *mifare_cryto_postprocess_data (desfiretag_t tag, void *data, size_t *nbytes, int communication_settings); -void mifare_cypher_single_block (desfirekey_t key, uint8_t *data, uint8_t *ivect, MifareCryptoDirection direction, MifareCryptoOperation operation, size_t block_size); -void mifare_cypher_blocks_chained (desfiretag_t tag, desfirekey_t key, uint8_t *ivect, uint8_t *data, size_t data_size, MifareCryptoDirection direction, MifareCryptoOperation operation); -size_t key_block_size (const desfirekey_t key); -size_t padded_data_length (const size_t nbytes, const size_t block_size); -size_t maced_data_length (const desfirekey_t key, const size_t nbytes); -size_t enciphered_data_length (const desfiretag_t tag, const size_t nbytes, int communication_settings); -void cmac_generate_subkeys (desfirekey_t key); -void cmac (const desfirekey_t key, uint8_t *ivect, const uint8_t *data, size_t len, uint8_t *cmac); +void *mifare_cryto_preprocess_data(desfiretag_t tag, void *data, size_t *nbytes, size_t offset, int communication_settings); +void *mifare_cryto_postprocess_data(desfiretag_t tag, void *data, size_t *nbytes, int communication_settings); +void mifare_cypher_single_block(desfirekey_t key, uint8_t *data, uint8_t *ivect, MifareCryptoDirection direction, MifareCryptoOperation operation, size_t block_size); +void mifare_cypher_blocks_chained(desfiretag_t tag, desfirekey_t key, uint8_t *ivect, uint8_t *data, size_t data_size, MifareCryptoDirection direction, MifareCryptoOperation operation); +size_t key_block_size(const desfirekey_t key); +size_t padded_data_length(const size_t nbytes, const size_t block_size); +size_t maced_data_length(const desfirekey_t key, const size_t nbytes); +size_t enciphered_data_length(const desfiretag_t tag, const size_t nbytes, int communication_settings); +void cmac_generate_subkeys(desfirekey_t key); +void cmac(const desfirekey_t key, uint8_t *ivect, const uint8_t *data, size_t len, uint8_t *cmac); // iso15693.h void RecordRawAdcSamplesIso15693(void); @@ -205,14 +205,14 @@ void AcquireRawAdcSamplesIso15693(void); void ReaderIso15693(uint32_t parameter); // Simulate an ISO15693 reader - greg void SimTagIso15693(uint32_t parameter, uint8_t *uid); // simulate an ISO15693 tag - greg void BruteforceIso15693Afi(uint32_t speed); // find an AFI of a tag - atrox -void DirectTag15693Command(uint32_t datalen,uint32_t speed, uint32_t recv, uint8_t *data); // send arbitrary commands from CLI - atrox +void DirectTag15693Command(uint32_t datalen, uint32_t speed, uint32_t recv, uint8_t *data); // send arbitrary commands from CLI - atrox void Iso15693InitReader(void); // iclass.h void RAMFUNC SniffIClass(void); void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain); void ReaderIClass(uint8_t arg0); -void ReaderIClass_Replay(uint8_t arg0,uint8_t *MAC); +void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC); void iClass_Authentication(uint8_t *MAC); void iClass_Authentication_fast(uint64_t arg0, uint64_t arg1, uint8_t *datain); void iClass_WriteBlock(uint8_t blockNo, uint8_t *data); @@ -224,22 +224,22 @@ void iClass_ReadCheck(uint8_t blockNo, uint8_t keyType); // hitag2.h void SnoopHitag(uint32_t type); -void SimulateHitagTag(bool tag_mem_supplied, byte_t* data); -void ReaderHitag(hitag_function htf, hitag_data* htd); -void WriterHitag(hitag_function htf, hitag_data* htd, int page); +void SimulateHitagTag(bool tag_mem_supplied, byte_t *data); +void ReaderHitag(hitag_function htf, hitag_data *htd); +void WriterHitag(hitag_function htf, hitag_data *htd, int page); //hitagS.h -void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data); -void ReadHitagS(hitag_function htf, hitag_data* htd); -void WritePageHitagS(hitag_function htf, hitag_data* htd,int page); -void check_challenges(bool file_given, byte_t* data); +void SimulateHitagSTag(bool tag_mem_supplied, byte_t *data); +void ReadHitagS(hitag_function htf, hitag_data *htd); +void WritePageHitagS(hitag_function htf, hitag_data *htd, int page); +void check_challenges(bool file_given, byte_t *data); // cmd.h -uint8_t cmd_receive(UsbCommand* cmd); -uint8_t cmd_send(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, void* data, size_t len); +uint8_t cmd_receive(UsbCommand *cmd); +uint8_t cmd_send(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, void *data, size_t len); // util.h -void HfSnoop(int , int); +void HfSnoop(int, int); //felica.c extern void felica_sendraw(UsbCommand *c); diff --git a/armsrc/buzzer.c b/armsrc/buzzer.c index 987d8fade..a06b1adbb 100644 --- a/armsrc/buzzer.c +++ b/armsrc/buzzer.c @@ -1,41 +1,46 @@ #include "buzzer.h" -void Ring_BEE_ONCE(uint16_t music_note) { +void Ring_BEE_ONCE(uint16_t music_note) +{ BEE_ON(); SpinDelayUs(music_note); BEE_OFF(); SpinDelayUs(music_note); } -void ring_2_7khz(uint16_t count) { - Ring_BEE_TIME(n_2_7khz,count); +void ring_2_7khz(uint16_t count) +{ + Ring_BEE_TIME(n_2_7khz, count); } -void Ring_BEE_TIME(uint16_t music_note,uint16_t count) { - for(uint16_t i=0 ; i < count; i++) +void Ring_BEE_TIME(uint16_t music_note, uint16_t count) +{ + for (uint16_t i = 0 ; i < count; i++) Ring_BEE_ONCE(music_note); SpinDelay(9); } -void Ring_ALL(uint16_t count) { - Ring_BEE_TIME(note_1, count); - Ring_BEE_TIME(note_2, count); - Ring_BEE_TIME(note_3, count); - Ring_BEE_TIME(note_4, count); - Ring_BEE_TIME(note_5, count); - Ring_BEE_TIME(note_6, count); - Ring_BEE_TIME(note_7, count); - SpinDelay(10); +void Ring_ALL(uint16_t count) +{ + Ring_BEE_TIME(note_1, count); + Ring_BEE_TIME(note_2, count); + Ring_BEE_TIME(note_3, count); + Ring_BEE_TIME(note_4, count); + Ring_BEE_TIME(note_5, count); + Ring_BEE_TIME(note_6, count); + Ring_BEE_TIME(note_7, count); + SpinDelay(10); } -void Ring_Little_Star(uint16_t count) { - Ring_BEE_TIME(note_1,count); - Ring_BEE_TIME(note_1,count); - Ring_BEE_TIME(note_5,count); - Ring_BEE_TIME(note_5,count); - Ring_BEE_TIME(note_6,count); - Ring_BEE_TIME(note_6,count); - Ring_BEE_TIME(note_5,2*count); +void Ring_Little_Star(uint16_t count) +{ + Ring_BEE_TIME(note_1, count); + Ring_BEE_TIME(note_1, count); + Ring_BEE_TIME(note_5, count); + Ring_BEE_TIME(note_5, count); + Ring_BEE_TIME(note_6, count); + Ring_BEE_TIME(note_6, count); + Ring_BEE_TIME(note_5, 2 * count); LED_A_ON(); /* Ring_BEE_TIME(note_4,count); diff --git a/armsrc/buzzer.h b/armsrc/buzzer.h index e253977cf..801b35a21 100644 --- a/armsrc/buzzer.h +++ b/armsrc/buzzer.h @@ -21,7 +21,7 @@ #define note_8 0 extern void Ring_BEE_ONCE(uint16_t music_note); -extern void Ring_BEE_TIME(uint16_t music_note,uint16_t count); +extern void Ring_BEE_TIME(uint16_t music_note, uint16_t count); extern void ring_2_7khz(uint16_t count); extern void Ring_ALL(uint16_t count); extern void Ring_Little_Star(uint16_t count); diff --git a/armsrc/des.c b/armsrc/des.c index a8c416f06..2e9cfaef1 100644 --- a/armsrc/des.c +++ b/armsrc/des.c @@ -29,53 +29,53 @@ #include "des.h" const uint8_t sbox[256] = { - /* S-box 1 */ - 0xE4, 0xD1, 0x2F, 0xB8, 0x3A, 0x6C, 0x59, 0x07, - 0x0F, 0x74, 0xE2, 0xD1, 0xA6, 0xCB, 0x95, 0x38, - 0x41, 0xE8, 0xD6, 0x2B, 0xFC, 0x97, 0x3A, 0x50, - 0xFC, 0x82, 0x49, 0x17, 0x5B, 0x3E, 0xA0, 0x6D, - /* S-box 2 */ - 0xF1, 0x8E, 0x6B, 0x34, 0x97, 0x2D, 0xC0, 0x5A, - 0x3D, 0x47, 0xF2, 0x8E, 0xC0, 0x1A, 0x69, 0xB5, - 0x0E, 0x7B, 0xA4, 0xD1, 0x58, 0xC6, 0x93, 0x2F, - 0xD8, 0xA1, 0x3F, 0x42, 0xB6, 0x7C, 0x05, 0xE9, - /* S-box 3 */ - 0xA0, 0x9E, 0x63, 0xF5, 0x1D, 0xC7, 0xB4, 0x28, - 0xD7, 0x09, 0x34, 0x6A, 0x28, 0x5E, 0xCB, 0xF1, - 0xD6, 0x49, 0x8F, 0x30, 0xB1, 0x2C, 0x5A, 0xE7, - 0x1A, 0xD0, 0x69, 0x87, 0x4F, 0xE3, 0xB5, 0x2C, - /* S-box 4 */ - 0x7D, 0xE3, 0x06, 0x9A, 0x12, 0x85, 0xBC, 0x4F, - 0xD8, 0xB5, 0x6F, 0x03, 0x47, 0x2C, 0x1A, 0xE9, - 0xA6, 0x90, 0xCB, 0x7D, 0xF1, 0x3E, 0x52, 0x84, - 0x3F, 0x06, 0xA1, 0xD8, 0x94, 0x5B, 0xC7, 0x2E, - /* S-box 5 */ - 0x2C, 0x41, 0x7A, 0xB6, 0x85, 0x3F, 0xD0, 0xE9, - 0xEB, 0x2C, 0x47, 0xD1, 0x50, 0xFA, 0x39, 0x86, - 0x42, 0x1B, 0xAD, 0x78, 0xF9, 0xC5, 0x63, 0x0E, - 0xB8, 0xC7, 0x1E, 0x2D, 0x6F, 0x09, 0xA4, 0x53, - /* S-box 6 */ - 0xC1, 0xAF, 0x92, 0x68, 0x0D, 0x34, 0xE7, 0x5B, - 0xAF, 0x42, 0x7C, 0x95, 0x61, 0xDE, 0x0B, 0x38, - 0x9E, 0xF5, 0x28, 0xC3, 0x70, 0x4A, 0x1D, 0xB6, - 0x43, 0x2C, 0x95, 0xFA, 0xBE, 0x17, 0x60, 0x8D, - /* S-box 7 */ - 0x4B, 0x2E, 0xF0, 0x8D, 0x3C, 0x97, 0x5A, 0x61, - 0xD0, 0xB7, 0x49, 0x1A, 0xE3, 0x5C, 0x2F, 0x86, - 0x14, 0xBD, 0xC3, 0x7E, 0xAF, 0x68, 0x05, 0x92, - 0x6B, 0xD8, 0x14, 0xA7, 0x95, 0x0F, 0xE2, 0x3C, - /* S-box 8 */ - 0xD2, 0x84, 0x6F, 0xB1, 0xA9, 0x3E, 0x50, 0xC7, - 0x1F, 0xD8, 0xA3, 0x74, 0xC5, 0x6B, 0x0E, 0x92, - 0x7B, 0x41, 0x9C, 0xE2, 0x06, 0xAD, 0xF3, 0x58, - 0x21, 0xE7, 0x4A, 0x8D, 0xFC, 0x90, 0x35, 0x6B + /* S-box 1 */ + 0xE4, 0xD1, 0x2F, 0xB8, 0x3A, 0x6C, 0x59, 0x07, + 0x0F, 0x74, 0xE2, 0xD1, 0xA6, 0xCB, 0x95, 0x38, + 0x41, 0xE8, 0xD6, 0x2B, 0xFC, 0x97, 0x3A, 0x50, + 0xFC, 0x82, 0x49, 0x17, 0x5B, 0x3E, 0xA0, 0x6D, + /* S-box 2 */ + 0xF1, 0x8E, 0x6B, 0x34, 0x97, 0x2D, 0xC0, 0x5A, + 0x3D, 0x47, 0xF2, 0x8E, 0xC0, 0x1A, 0x69, 0xB5, + 0x0E, 0x7B, 0xA4, 0xD1, 0x58, 0xC6, 0x93, 0x2F, + 0xD8, 0xA1, 0x3F, 0x42, 0xB6, 0x7C, 0x05, 0xE9, + /* S-box 3 */ + 0xA0, 0x9E, 0x63, 0xF5, 0x1D, 0xC7, 0xB4, 0x28, + 0xD7, 0x09, 0x34, 0x6A, 0x28, 0x5E, 0xCB, 0xF1, + 0xD6, 0x49, 0x8F, 0x30, 0xB1, 0x2C, 0x5A, 0xE7, + 0x1A, 0xD0, 0x69, 0x87, 0x4F, 0xE3, 0xB5, 0x2C, + /* S-box 4 */ + 0x7D, 0xE3, 0x06, 0x9A, 0x12, 0x85, 0xBC, 0x4F, + 0xD8, 0xB5, 0x6F, 0x03, 0x47, 0x2C, 0x1A, 0xE9, + 0xA6, 0x90, 0xCB, 0x7D, 0xF1, 0x3E, 0x52, 0x84, + 0x3F, 0x06, 0xA1, 0xD8, 0x94, 0x5B, 0xC7, 0x2E, + /* S-box 5 */ + 0x2C, 0x41, 0x7A, 0xB6, 0x85, 0x3F, 0xD0, 0xE9, + 0xEB, 0x2C, 0x47, 0xD1, 0x50, 0xFA, 0x39, 0x86, + 0x42, 0x1B, 0xAD, 0x78, 0xF9, 0xC5, 0x63, 0x0E, + 0xB8, 0xC7, 0x1E, 0x2D, 0x6F, 0x09, 0xA4, 0x53, + /* S-box 6 */ + 0xC1, 0xAF, 0x92, 0x68, 0x0D, 0x34, 0xE7, 0x5B, + 0xAF, 0x42, 0x7C, 0x95, 0x61, 0xDE, 0x0B, 0x38, + 0x9E, 0xF5, 0x28, 0xC3, 0x70, 0x4A, 0x1D, 0xB6, + 0x43, 0x2C, 0x95, 0xFA, 0xBE, 0x17, 0x60, 0x8D, + /* S-box 7 */ + 0x4B, 0x2E, 0xF0, 0x8D, 0x3C, 0x97, 0x5A, 0x61, + 0xD0, 0xB7, 0x49, 0x1A, 0xE3, 0x5C, 0x2F, 0x86, + 0x14, 0xBD, 0xC3, 0x7E, 0xAF, 0x68, 0x05, 0x92, + 0x6B, 0xD8, 0x14, 0xA7, 0x95, 0x0F, 0xE2, 0x3C, + /* S-box 8 */ + 0xD2, 0x84, 0x6F, 0xB1, 0xA9, 0x3E, 0x50, 0xC7, + 0x1F, 0xD8, 0xA3, 0x74, 0xC5, 0x6B, 0x0E, 0x92, + 0x7B, 0x41, 0x9C, 0xE2, 0x06, 0xAD, 0xF3, 0x58, + 0x21, 0xE7, 0x4A, 0x8D, 0xFC, 0x90, 0x35, 0x6B }; -const uint8_t e_permtab[] ={ - 4, 6, /* 4 bytes in 6 bytes out*/ +const uint8_t e_permtab[] = { + 4, 6, /* 4 bytes in 6 bytes out*/ 32, 1, 2, 3, 4, 5, - 4, 5, 6, 7, 8, 9, - 8, 9, 10, 11, 12, 13, + 4, 5, 6, 7, 8, 9, + 8, 9, 10, 11, 12, 13, 12, 13, 14, 15, 16, 17, 16, 17, 18, 19, 20, 21, 20, 21, 22, 23, 24, 25, @@ -83,20 +83,20 @@ const uint8_t e_permtab[] ={ 28, 29, 30, 31, 32, 1 }; -const uint8_t p_permtab[] ={ - 4, 4, /* 32 bit -> 32 bit */ +const uint8_t p_permtab[] = { + 4, 4, /* 32 bit -> 32 bit */ 16, 7, 20, 21, 29, 12, 28, 17, - 1, 15, 23, 26, - 5, 18, 31, 10, - 2, 8, 24, 14, + 1, 15, 23, 26, + 5, 18, 31, 10, + 2, 8, 24, 14, 32, 27, 3, 9, 19, 13, 30, 6, 22, 11, 4, 25 }; -const uint8_t ip_permtab[] ={ - 8, 8, /* 64 bit -> 64 bit */ +const uint8_t ip_permtab[] = { + 8, 8, /* 64 bit -> 64 bit */ 58, 50, 42, 34, 26, 18, 10, 2, 60, 52, 44, 36, 28, 20, 12, 4, 62, 54, 46, 38, 30, 22, 14, 6, @@ -107,8 +107,8 @@ const uint8_t ip_permtab[] ={ 63, 55, 47, 39, 31, 23, 15, 7 }; -const uint8_t inv_ip_permtab[] ={ - 8, 8, /* 64 bit -> 64 bit */ +const uint8_t inv_ip_permtab[] = { + 8, 8, /* 64 bit -> 64 bit */ 40, 8, 48, 16, 56, 24, 64, 32, 39, 7, 47, 15, 55, 23, 63, 31, 38, 6, 46, 14, 54, 22, 62, 30, @@ -119,22 +119,22 @@ const uint8_t inv_ip_permtab[] ={ 33, 1, 41, 9, 49, 17, 57, 25 }; -const uint8_t pc1_permtab[] ={ - 8, 7, /* 64 bit -> 56 bit*/ +const uint8_t pc1_permtab[] = { + 8, 7, /* 64 bit -> 56 bit*/ 57, 49, 41, 33, 25, 17, 9, - 1, 58, 50, 42, 34, 26, 18, + 1, 58, 50, 42, 34, 26, 18, 10, 2, 59, 51, 43, 35, 27, 19, 11, 3, 60, 52, 44, 36, 63, 55, 47, 39, 31, 23, 15, - 7, 62, 54, 46, 38, 30, 22, + 7, 62, 54, 46, 38, 30, 22, 14, 6, 61, 53, 45, 37, 29, 21, 13, 5, 28, 20, 12, 4 }; -const uint8_t pc2_permtab[] ={ - 7, 6, /* 56 bit -> 48 bit */ +const uint8_t pc2_permtab[] = { + 7, 6, /* 56 bit -> 48 bit */ 14, 17, 11, 24, 1, 5, - 3, 28, 15, 6, 21, 10, + 3, 28, 15, 6, 21, 10, 23, 19, 12, 4, 26, 8, 16, 7, 27, 20, 13, 2, 41, 52, 31, 37, 47, 55, @@ -144,7 +144,7 @@ const uint8_t pc2_permtab[] ={ }; const uint8_t splitin6bitword_permtab[] = { - 8, 8, /* 64 bit -> 64 bit */ + 8, 8, /* 64 bit -> 64 bit */ 64, 64, 1, 6, 2, 3, 4, 5, 64, 64, 7, 12, 8, 9, 10, 11, 64, 64, 13, 18, 14, 15, 16, 17, @@ -156,8 +156,8 @@ const uint8_t splitin6bitword_permtab[] = { }; const uint8_t shiftkey_permtab[] = { - 7, 7, /* 56 bit -> 56 bit */ - 2, 3, 4, 5, 6, 7, 8, 9, + 7, 7, /* 56 bit -> 56 bit */ + 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 1, @@ -168,9 +168,9 @@ const uint8_t shiftkey_permtab[] = { }; const uint8_t shiftkeyinv_permtab[] = { - 7, 7, + 7, 7, 28, 1, 2, 3, 4, 5, 6, 7, - 8, 9, 10, 11, 12, 13, 14, 15, + 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 56, 29, 30, 31, 32, 33, 34, 35, @@ -202,27 +202,29 @@ const uint8_t shiftkeyinv_permtab[] = { #define ROTTABLE_INV 0x3F7E /******************************************************************************/ -void permute(const uint8_t *ptable, const uint8_t *in, uint8_t *out){ +void permute(const uint8_t *ptable, const uint8_t *in, uint8_t *out) +{ uint8_t ob; /* in-bytes and out-bytes */ uint8_t byte, bit; /* counter for bit and byte */ ob = ptable[1]; ptable = &(ptable[2]); - for(byte=0; byte>(x%8)) ){ - t|=0x01; + t <<= 1; + if ((in[x / 8]) & (0x80 >> (x % 8))) { + t |= 0x01; } } - out[byte]=t; + out[byte] = t; } } /******************************************************************************/ -void changeendian32(uint32_t * a){ +void changeendian32(uint32_t *a) +{ *a = (*a & 0x000000FF) << 24 | (*a & 0x0000FF00) << 8 | (*a & 0x00FF0000) >> 8 | @@ -231,65 +233,70 @@ void changeendian32(uint32_t * a){ /******************************************************************************/ static inline -void shiftkey(uint8_t *key){ +void shiftkey(uint8_t *key) +{ uint8_t k[7]; memcpy(k, key, 7); - permute((uint8_t*)shiftkey_permtab, k, key); + permute((uint8_t *)shiftkey_permtab, k, key); } /******************************************************************************/ static inline -void shiftkey_inv(uint8_t *key){ +void shiftkey_inv(uint8_t *key) +{ uint8_t k[7]; memcpy(k, key, 7); - permute((uint8_t*)shiftkeyinv_permtab, k, key); + permute((uint8_t *)shiftkeyinv_permtab, k, key); } /******************************************************************************/ static inline -uint64_t splitin6bitwords(uint64_t a){ - uint64_t ret=0; +uint64_t splitin6bitwords(uint64_t a) +{ + uint64_t ret = 0; a &= 0x0000ffffffffffffLL; - permute((uint8_t*)splitin6bitword_permtab, (uint8_t*)&a, (uint8_t*)&ret); + permute((uint8_t *)splitin6bitword_permtab, (uint8_t *)&a, (uint8_t *)&ret); return ret; } /******************************************************************************/ static inline -uint8_t substitute(uint8_t a, uint8_t * sbp){ +uint8_t substitute(uint8_t a, uint8_t *sbp) +{ uint8_t x; - x = sbp[a>>1]; - x = (a&1)?x&0x0F:x>>4; + x = sbp[a >> 1]; + x = (a & 1) ? x & 0x0F : x >> 4; return x; } /******************************************************************************/ -uint32_t des_f(uint32_t r, uint8_t* kr){ +uint32_t des_f(uint32_t r, uint8_t *kr) +{ uint8_t i; - uint32_t t=0,ret; + uint32_t t = 0, ret; uint64_t data; uint8_t *sbp; /* sboxpointer */ - permute((uint8_t*)e_permtab, (uint8_t*)&r, (uint8_t*)&data); - for(i=0; i<6; ++i) - ((uint8_t*)&data)[i] ^= kr[i]; + permute((uint8_t *)e_permtab, (uint8_t *)&r, (uint8_t *)&data); + for (i = 0; i < 6; ++i) + ((uint8_t *)&data)[i] ^= kr[i]; /* Sbox substitution */ data = splitin6bitwords(data); - sbp=(uint8_t*)sbox; - for(i=0; i<8; ++i){ + sbp = (uint8_t *)sbox; + for (i = 0; i < 8; ++i) { uint8_t x; - x = substitute(((uint8_t*)&data)[i], sbp); - t<<=4; + x = substitute(((uint8_t *)&data)[i], sbp); + t <<= 4; t |= x; sbp += 32; } changeendian32(&t); - permute((uint8_t*)p_permtab,(uint8_t*)&t, (uint8_t*)&ret); + permute((uint8_t *)p_permtab, (uint8_t *)&t, (uint8_t *)&ret); return ret; } @@ -305,26 +312,27 @@ typedef struct { #define R (data.d.v32[1]) #define L (data.d.v32[0]) -void des_enc(void* out, const void* in, const void* key){ +void des_enc(void *out, const void *in, const void *key) +{ uint8_t kr[6], k[7]; uint8_t i; data_t data; - permute((uint8_t*)ip_permtab, (uint8_t*)in, data.d.v8); - permute((uint8_t*)pc1_permtab, (const uint8_t*)key, k); + permute((uint8_t *)ip_permtab, (uint8_t *)in, data.d.v8); + permute((uint8_t *)pc1_permtab, (const uint8_t *)key, k); - for(i=0; i<8; ++i){ + for (i = 0; i < 8; ++i) { shiftkey(k); - if(ROTTABLE&((1<<((i<<1)+0))) ) + if (ROTTABLE & ((1 << ((i << 1) + 0)))) shiftkey(k); - permute((uint8_t*)pc2_permtab, k, kr); + permute((uint8_t *)pc2_permtab, k, kr); L ^= des_f(R, kr); shiftkey(k); - if(ROTTABLE&((1<<((i<<1)+1))) ) + if (ROTTABLE & ((1 << ((i << 1) + 1)))) shiftkey(k); - permute((uint8_t*)pc2_permtab, k, kr); + permute((uint8_t *)pc2_permtab, k, kr); R ^= des_f(L, kr); } @@ -333,32 +341,33 @@ void des_enc(void* out, const void* in, const void* key){ L ^= R; R ^= L; - permute((uint8_t*)inv_ip_permtab, data.d.v8, (uint8_t*)out); + permute((uint8_t *)inv_ip_permtab, data.d.v8, (uint8_t *)out); } /******************************************************************************/ -void des_dec(void* out, const void* in, const uint8_t* key){ +void des_dec(void *out, const void *in, const uint8_t *key) +{ - uint8_t kr[6],k[7]; + uint8_t kr[6], k[7]; int8_t i; data_t data; - permute((uint8_t*)ip_permtab, (uint8_t*)in, data.d.v8); - permute((uint8_t*)pc1_permtab, (const uint8_t*)key, k); - for(i=7; i>=0; --i){ + permute((uint8_t *)ip_permtab, (uint8_t *)in, data.d.v8); + permute((uint8_t *)pc1_permtab, (const uint8_t *)key, k); + for (i = 7; i >= 0; --i) { - permute((uint8_t*)pc2_permtab, k, kr); + permute((uint8_t *)pc2_permtab, k, kr); L ^= des_f(R, kr); shiftkey_inv(k); - if(ROTTABLE&((1<<((i<<1)+1))) ){ + if (ROTTABLE & ((1 << ((i << 1) + 1)))) { shiftkey_inv(k); } - permute((uint8_t*)pc2_permtab, k, kr); + permute((uint8_t *)pc2_permtab, k, kr); R ^= des_f(L, kr); shiftkey_inv(k); - if(ROTTABLE&((1<<((i<<1)+0))) ){ + if (ROTTABLE & ((1 << ((i << 1) + 0)))) { shiftkey_inv(k); } @@ -368,77 +377,79 @@ void des_dec(void* out, const void* in, const uint8_t* key){ L ^= R; R ^= L; - permute((uint8_t*)inv_ip_permtab, data.d.v8, (uint8_t*)out); + permute((uint8_t *)inv_ip_permtab, data.d.v8, (uint8_t *)out); } /******************************************************************************/ -void tdes_enc(void* out, void* in, const void* key){ - des_enc(out, in, (uint8_t*)key + 0); - des_dec(out, out, (uint8_t*)key + 8); - des_enc(out, out, (uint8_t*)key +16); +void tdes_enc(void *out, void *in, const void *key) +{ + des_enc(out, in, (uint8_t *)key + 0); + des_dec(out, out, (uint8_t *)key + 8); + des_enc(out, out, (uint8_t *)key + 16); } /******************************************************************************/ -void tdes_dec(void* out, void* in, const uint8_t* key){ - des_dec(out, in, (uint8_t*)key +16); - des_enc(out, out, (uint8_t*)key + 8); - des_dec(out, out, (uint8_t*)key + 0); +void tdes_dec(void *out, void *in, const uint8_t *key) +{ + des_dec(out, in, (uint8_t *)key + 16); + des_enc(out, out, (uint8_t *)key + 8); + des_dec(out, out, (uint8_t *)key + 0); } - void tdes_2key_enc(void* out, const void* in, size_t length, const void* key, unsigned char iv[8]){ +void tdes_2key_enc(void *out, const void *in, size_t length, const void *key, unsigned char iv[8]) +{ - if( length % 8 ) return; + if (length % 8) return; uint8_t i; - uint8_t* tin = (uint8_t*) in; - uint8_t* tout = (uint8_t*) out; + uint8_t *tin = (uint8_t *) in; + uint8_t *tout = (uint8_t *) out; - while( length > 0 ) - { - for( i = 0; i < 8; i++ ) - tout[i] = (unsigned char)( tin[i] ^ iv[i] ); + while (length > 0) { + for (i = 0; i < 8; i++) + tout[i] = (unsigned char)(tin[i] ^ iv[i]); - des_enc(tout, tin, (uint8_t*)key + 0); - des_dec(tout, tout, (uint8_t*)key + 8); - des_enc(tout, tout, (uint8_t*)key + 0); + des_enc(tout, tin, (uint8_t *)key + 0); + des_dec(tout, tout, (uint8_t *)key + 8); + des_enc(tout, tout, (uint8_t *)key + 0); - memcpy( iv, tout, 8 ); + memcpy(iv, tout, 8); tin += 8; tout += 8; length -= 8; } - } +} - void tdes_2key_dec(void* out, const void* in, size_t length, const void* key, unsigned char iv[8]){ +void tdes_2key_dec(void *out, const void *in, size_t length, const void *key, unsigned char iv[8]) +{ - if( length % 8 ) return; + if (length % 8) return; uint8_t i; unsigned char temp[8]; - uint8_t* tin = (uint8_t*) in; - uint8_t* tout = (uint8_t*) out; + uint8_t *tin = (uint8_t *) in; + uint8_t *tout = (uint8_t *) out; - while( length > 0 ) - { - memcpy( temp, tin, 8 ); + while (length > 0) { + memcpy(temp, tin, 8); - des_dec(tout, tin, (uint8_t*)key + 0); - des_enc(tout, tout, (uint8_t*)key + 8); - des_dec(tout, tout, (uint8_t*)key + 0); + des_dec(tout, tin, (uint8_t *)key + 0); + des_enc(tout, tout, (uint8_t *)key + 8); + des_dec(tout, tout, (uint8_t *)key + 0); - for( i = 0; i < 8; i++ ) - tout[i] = (unsigned char)( tout[i] ^ iv[i] ); + for (i = 0; i < 8; i++) + tout[i] = (unsigned char)(tout[i] ^ iv[i]); - memcpy( iv, temp, 8 ); + memcpy(iv, temp, 8); tin += 8; tout += 8; length -= 8; } - } +} /******************************************************************************/ diff --git a/armsrc/des.h b/armsrc/des.h index 2afe39b14..0d2cceefb 100644 --- a/armsrc/des.h +++ b/armsrc/des.h @@ -56,7 +56,7 @@ * \param in pointer to the block (64 bit = 8 byte) where the plaintext is read from * \param key pointer to the key (64 bit = 8 byte) */ -void des_enc(void* out, const void* in, const void* key); +void des_enc(void *out, const void *in, const void *key); /** \fn void des_dec(void* out, const void* in, const void* key) * \brief decrypt a block with DES @@ -71,7 +71,7 @@ void des_enc(void* out, const void* in, const void* key); * \param key pointer to the key (64 bit = 8 byte) */ //void des_dec(void* out, const void* in, const void* key); -void des_dec(void* out, const void* in, const uint8_t* key); +void des_dec(void *out, const void *in, const uint8_t *key); /** \fn void tdes_enc(void* out, const void* in, const void* key) * \brief encrypt a block with Tripple-DES @@ -86,7 +86,7 @@ void des_dec(void* out, const void* in, const uint8_t* key); * \param key pointer to the key (192 bit = 24 byte) */ //void tdes_enc(void* out, const void* in, const void* key); -void tdes_enc(void* out, void* in, const void* key); +void tdes_enc(void *out, void *in, const void *key); /** \fn void tdes_dec(void* out, const void* in, const void* key) * \brief decrypt a block with Tripple-DES @@ -100,11 +100,11 @@ void tdes_enc(void* out, void* in, const void* key); * \param in pointer to the block (64 bit = 8 byte) where the ciphertext is read from * \param key pointer to the key (192 bit = 24 byte) */ - //void tdes_dec(void* out, const void* in, const void* key); - void tdes_dec(void* out, void* in, const uint8_t* key); +//void tdes_dec(void* out, const void* in, const void* key); +void tdes_dec(void *out, void *in, const uint8_t *key); - void tdes_2key_enc(void* out, const void* in, size_t length, const void* key, unsigned char iv[8]); - void tdes_2key_dec(void* out, const void* in, size_t length, const void* key, unsigned char iv[8]); +void tdes_2key_enc(void *out, const void *in, size_t length, const void *key, unsigned char iv[8]); +void tdes_2key_dec(void *out, const void *in, size_t length, const void *key, unsigned char iv[8]); // Copied from des.h in desfire imp. typedef unsigned long DES_KS[16][2]; /* Single-key DES key schedule */ diff --git a/armsrc/desfire_crypto.c b/armsrc/desfire_crypto.c index dd958d3ea..55dde9c4c 100644 --- a/armsrc/desfire_crypto.c +++ b/armsrc/desfire_crypto.c @@ -27,68 +27,72 @@ */ #include "desfire_crypto.h" -static void xor (const uint8_t *ivect, uint8_t *data, const size_t len); -static size_t key_macing_length (desfirekey_t key); +static void xor(const uint8_t *ivect, uint8_t *data, const size_t len); +static size_t key_macing_length(desfirekey_t key); // iceman, see memxor inside string.c, dest/src swapped.. -static void xor (const uint8_t *ivect, uint8_t *data, const size_t len) { +static void xor(const uint8_t *ivect, uint8_t *data, const size_t len) +{ for (size_t i = 0; i < len; i++) { data[i] ^= ivect[i]; } } -void cmac_generate_subkeys ( desfirekey_t key) { - int kbs = key_block_size (key); +void cmac_generate_subkeys(desfirekey_t key) +{ + int kbs = key_block_size(key); const uint8_t R = (kbs == 8) ? 0x1B : 0x87; uint8_t l[kbs]; - memset (l, 0, kbs); + memset(l, 0, kbs); uint8_t ivect[kbs]; - memset (ivect, 0, kbs); + memset(ivect, 0, kbs); - mifare_cypher_blocks_chained (NULL, key, ivect, l, kbs, MCD_RECEIVE, MCO_ENCYPHER); + mifare_cypher_blocks_chained(NULL, key, ivect, l, kbs, MCD_RECEIVE, MCO_ENCYPHER); bool xor = false; // Used to compute CMAC on complete blocks - memcpy (key->cmac_sk1, l, kbs); + memcpy(key->cmac_sk1, l, kbs); xor = l[0] & 0x80; - lsl (key->cmac_sk1, kbs); + lsl(key->cmac_sk1, kbs); if (xor) - key->cmac_sk1[kbs-1] ^= R; + key->cmac_sk1[kbs - 1] ^= R; // Used to compute CMAC on the last block if non-complete - memcpy (key->cmac_sk2, key->cmac_sk1, kbs); + memcpy(key->cmac_sk2, key->cmac_sk1, kbs); xor = key->cmac_sk1[0] & 0x80; - lsl (key->cmac_sk2, kbs); + lsl(key->cmac_sk2, kbs); if (xor) - key->cmac_sk2[kbs-1] ^= R; + key->cmac_sk2[kbs - 1] ^= R; } -void cmac (const desfirekey_t key, uint8_t *ivect, const uint8_t *data, size_t len, uint8_t *cmac) { - int kbs = key_block_size (key); - uint8_t *buffer = malloc (padded_data_length (len, kbs)); +void cmac(const desfirekey_t key, uint8_t *ivect, const uint8_t *data, size_t len, uint8_t *cmac) +{ + int kbs = key_block_size(key); + uint8_t *buffer = malloc(padded_data_length(len, kbs)); - memcpy (buffer, data, len); + memcpy(buffer, data, len); if ((!len) || (len % kbs)) { buffer[len++] = 0x80; while (len % kbs) { buffer[len++] = 0x00; } - xor (key->cmac_sk2, buffer + len - kbs, kbs); + xor(key->cmac_sk2, buffer + len - kbs, kbs); } else { - xor (key->cmac_sk1, buffer + len - kbs, kbs); + xor(key->cmac_sk1, buffer + len - kbs, kbs); } - mifare_cypher_blocks_chained (NULL, key, ivect, buffer, len, MCD_SEND, MCO_ENCYPHER); + mifare_cypher_blocks_chained(NULL, key, ivect, buffer, len, MCD_SEND, MCO_ENCYPHER); - memcpy (cmac, ivect, kbs); + memcpy(cmac, ivect, kbs); free(buffer); } -size_t key_block_size (const desfirekey_t key) { +size_t key_block_size(const desfirekey_t key) +{ size_t block_size = 8; switch (key->type) { case T_DES: @@ -106,17 +110,18 @@ size_t key_block_size (const desfirekey_t key) { /* * Size of MACing produced with the key. */ -static size_t key_macing_length (const desfirekey_t key) { +static size_t key_macing_length(const desfirekey_t key) +{ size_t mac_length = MAC_LENGTH; switch (key->type) { - case T_DES: - case T_3DES: - mac_length = MAC_LENGTH; - break; - case T_3K3DES: - case T_AES: - mac_length = CMAC_LENGTH; - break; + case T_DES: + case T_3DES: + mac_length = MAC_LENGTH; + break; + case T_3K3DES: + case T_AES: + mac_length = CMAC_LENGTH; + break; } return mac_length; } @@ -124,7 +129,8 @@ static size_t key_macing_length (const desfirekey_t key) { /* * Size required to store nbytes of data in a buffer of size n*block_size. */ -size_t padded_data_length (const size_t nbytes, const size_t block_size) { +size_t padded_data_length(const size_t nbytes, const size_t block_size) +{ if ((!nbytes) || (nbytes % block_size)) return ((nbytes / block_size) + 1) * block_size; else @@ -134,31 +140,34 @@ size_t padded_data_length (const size_t nbytes, const size_t block_size) { /* * Buffer size required to MAC nbytes of data */ -size_t maced_data_length (const desfirekey_t key, const size_t nbytes) { - return nbytes + key_macing_length (key); +size_t maced_data_length(const desfirekey_t key, const size_t nbytes) +{ + return nbytes + key_macing_length(key); } /* * Buffer size required to encipher nbytes of data and a two bytes CRC. */ -size_t enciphered_data_length (const desfiretag_t tag, const size_t nbytes, int communication_settings) { +size_t enciphered_data_length(const desfiretag_t tag, const size_t nbytes, int communication_settings) +{ size_t crc_length = 0; if (!(communication_settings & NO_CRC)) { switch (DESFIRE(tag)->authentication_scheme) { - case AS_LEGACY: - crc_length = 2; - break; - case AS_NEW: - crc_length = 4; - break; + case AS_LEGACY: + crc_length = 2; + break; + case AS_NEW: + crc_length = 4; + break; } } - size_t block_size = DESFIRE(tag)->session_key ? key_block_size (DESFIRE(tag)->session_key) : 1; + size_t block_size = DESFIRE(tag)->session_key ? key_block_size(DESFIRE(tag)->session_key) : 1; - return padded_data_length (nbytes + crc_length, block_size); + return padded_data_length(nbytes + crc_length, block_size); } -void* mifare_cryto_preprocess_data (desfiretag_t tag, void *data, size_t *nbytes, size_t offset, int communication_settings) { +void *mifare_cryto_preprocess_data(desfiretag_t tag, void *data, size_t *nbytes, size_t offset, int communication_settings) +{ uint8_t *res = data; uint8_t mac[4]; size_t edl; @@ -169,124 +178,124 @@ void* mifare_cryto_preprocess_data (desfiretag_t tag, void *data, size_t *nbytes return data; switch (communication_settings & MDCM_MASK) { - case MDCM_PLAIN: - if (AS_LEGACY == DESFIRE(tag)->authentication_scheme) - break; + case MDCM_PLAIN: + if (AS_LEGACY == DESFIRE(tag)->authentication_scheme) + break; - /* - * When using new authentication methods, PLAIN data transmission from - * the PICC to the PCD are CMACed, so we have to maintain the - * cryptographic initialisation vector up-to-date to check data - * integrity later. - * - * The only difference with CMACed data transmission is that the CMAC - * is not apended to the data send by the PCD to the PICC. - */ + /* + * When using new authentication methods, PLAIN data transmission from + * the PICC to the PCD are CMACed, so we have to maintain the + * cryptographic initialisation vector up-to-date to check data + * integrity later. + * + * The only difference with CMACed data transmission is that the CMAC + * is not apended to the data send by the PCD to the PICC. + */ - append_mac = false; + append_mac = false; /* pass through */ - case MDCM_MACED: - switch (DESFIRE(tag)->authentication_scheme) { - case AS_LEGACY: - if (!(communication_settings & MAC_COMMAND)) - break; + case MDCM_MACED: + switch (DESFIRE(tag)->authentication_scheme) { + case AS_LEGACY: + if (!(communication_settings & MAC_COMMAND)) + break; - /* pass through */ - edl = padded_data_length (*nbytes - offset, key_block_size (DESFIRE(tag)->session_key)) + offset; + /* pass through */ + edl = padded_data_length(*nbytes - offset, key_block_size(DESFIRE(tag)->session_key)) + offset; - // Fill in the crypto buffer with data ... - memcpy (res, data, *nbytes); - // ... and 0 padding - memset (res + *nbytes, 0, edl - *nbytes); + // Fill in the crypto buffer with data ... + memcpy(res, data, *nbytes); + // ... and 0 padding + memset(res + *nbytes, 0, edl - *nbytes); - mifare_cypher_blocks_chained (tag, NULL, NULL, res + offset, edl - offset, MCD_SEND, MCO_ENCYPHER); + mifare_cypher_blocks_chained(tag, NULL, NULL, res + offset, edl - offset, MCD_SEND, MCO_ENCYPHER); - memcpy (mac, res + edl - 8, 4); + memcpy(mac, res + edl - 8, 4); - // Copy again provided data (was overwritten by mifare_cypher_blocks_chained) - memcpy (res, data, *nbytes); + // Copy again provided data (was overwritten by mifare_cypher_blocks_chained) + memcpy(res, data, *nbytes); - if (!(communication_settings & MAC_COMMAND)) - break; - // Append MAC - size_t bla = maced_data_length (DESFIRE(tag)->session_key, *nbytes - offset) + offset; - bla++; + if (!(communication_settings & MAC_COMMAND)) + break; + // Append MAC + size_t bla = maced_data_length(DESFIRE(tag)->session_key, *nbytes - offset) + offset; + bla++; - memcpy (res + *nbytes, mac, 4); + memcpy(res + *nbytes, mac, 4); - *nbytes += 4; - break; - case AS_NEW: - if (!(communication_settings & CMAC_COMMAND)) - break; - cmac (key, DESFIRE (tag)->ivect, res, *nbytes, DESFIRE (tag)->cmac); + *nbytes += 4; + break; + case AS_NEW: + if (!(communication_settings & CMAC_COMMAND)) + break; + cmac(key, DESFIRE(tag)->ivect, res, *nbytes, DESFIRE(tag)->cmac); - if (append_mac) { - size_t len = maced_data_length (key, *nbytes); - ++len; - memcpy (res, data, *nbytes); - memcpy (res + *nbytes, DESFIRE (tag)->cmac, CMAC_LENGTH); - *nbytes += CMAC_LENGTH; + if (append_mac) { + size_t len = maced_data_length(key, *nbytes); + ++len; + memcpy(res, data, *nbytes); + memcpy(res + *nbytes, DESFIRE(tag)->cmac, CMAC_LENGTH); + *nbytes += CMAC_LENGTH; + } + break; } - break; - } - break; - case MDCM_ENCIPHERED: - /* |<-------------- data -------------->| - * |<--- offset -->| | - * +---------------+--------------------+-----+---------+ - * | CMD + HEADERS | DATA TO BE SECURED | CRC | PADDING | - * +---------------+--------------------+-----+---------+ ---------------- - * | |<~~~~v~~~~~~~~~~~~~>| ^ | | (DES / 3DES) - * | | `---- crc16() ----' | | - * | | | ^ | | ----- *or* ----- - * |<~~~~~~~~~~~~~~~~~~~~v~~~~~~~~~~~~~>| ^ | | (3K3DES / AES) - * | `---- crc32() ----' | | - * | | ---- *then* ---- - * |<---------------------------------->| - * encypher()/decypher() - */ + break; + case MDCM_ENCIPHERED: + /* |<-------------- data -------------->| + * |<--- offset -->| | + * +---------------+--------------------+-----+---------+ + * | CMD + HEADERS | DATA TO BE SECURED | CRC | PADDING | + * +---------------+--------------------+-----+---------+ ---------------- + * | |<~~~~v~~~~~~~~~~~~~>| ^ | | (DES / 3DES) + * | | `---- crc16() ----' | | + * | | | ^ | | ----- *or* ----- + * |<~~~~~~~~~~~~~~~~~~~~v~~~~~~~~~~~~~>| ^ | | (3K3DES / AES) + * | `---- crc32() ----' | | + * | | ---- *then* ---- + * |<---------------------------------->| + * encypher()/decypher() + */ if (!(communication_settings & ENC_COMMAND)) break; - edl = enciphered_data_length (tag, *nbytes - offset, communication_settings) + offset; + edl = enciphered_data_length(tag, *nbytes - offset, communication_settings) + offset; // Fill in the crypto buffer with data ... - memcpy (res, data, *nbytes); + memcpy(res, data, *nbytes); if (!(communication_settings & NO_CRC)) { // ... CRC ... - switch (DESFIRE (tag)->authentication_scheme) { - case AS_LEGACY: - AddCrc14A(res + offset, *nbytes - offset); - *nbytes += 2; - break; - case AS_NEW: - crc32_append (res, *nbytes); - *nbytes += 4; - break; + switch (DESFIRE(tag)->authentication_scheme) { + case AS_LEGACY: + AddCrc14A(res + offset, *nbytes - offset); + *nbytes += 2; + break; + case AS_NEW: + crc32_append(res, *nbytes); + *nbytes += 4; + break; } } // ... and padding - memset (res + *nbytes, 0, edl - *nbytes); + memset(res + *nbytes, 0, edl - *nbytes); *nbytes = edl; - mifare_cypher_blocks_chained (tag, NULL, NULL, res + offset, *nbytes - offset, MCD_SEND, (AS_NEW == DESFIRE(tag)->authentication_scheme) ? MCO_ENCYPHER : MCO_DECYPHER); - break; - default: + mifare_cypher_blocks_chained(tag, NULL, NULL, res + offset, *nbytes - offset, MCD_SEND, (AS_NEW == DESFIRE(tag)->authentication_scheme) ? MCO_ENCYPHER : MCO_DECYPHER); + break; + default: - *nbytes = -1; - res = NULL; - break; + *nbytes = -1; + res = NULL; + break; } return res; } -void* mifare_cryto_postprocess_data (desfiretag_t tag, void *data, size_t *nbytes, int communication_settings) +void *mifare_cryto_postprocess_data(desfiretag_t tag, void *data, size_t *nbytes, int communication_settings) { void *res = data; size_t edl; @@ -303,300 +312,297 @@ void* mifare_cryto_postprocess_data (desfiretag_t tag, void *data, size_t *nbyte return res; switch (communication_settings & MDCM_MASK) { - case MDCM_PLAIN: + case MDCM_PLAIN: - if (AS_LEGACY == DESFIRE(tag)->authentication_scheme) - break; + if (AS_LEGACY == DESFIRE(tag)->authentication_scheme) + break; /* pass through */ - case MDCM_MACED: - switch (DESFIRE (tag)->authentication_scheme) { - case AS_LEGACY: - if (communication_settings & MAC_VERIFY) { - *nbytes -= key_macing_length (key); - if (*nbytes <= 0) { - *nbytes = -1; - res = NULL; + case MDCM_MACED: + switch (DESFIRE(tag)->authentication_scheme) { + case AS_LEGACY: + if (communication_settings & MAC_VERIFY) { + *nbytes -= key_macing_length(key); + if (*nbytes <= 0) { + *nbytes = -1; + res = NULL; #ifdef WITH_DEBUG - Dbprintf ("No room for MAC!"); + Dbprintf("No room for MAC!"); #endif + break; + } + + edl = enciphered_data_length(tag, *nbytes - 1, communication_settings); + edata = malloc(edl); + + memcpy(edata, data, *nbytes - 1); + memset((uint8_t *)edata + *nbytes - 1, 0, edl - *nbytes + 1); + + mifare_cypher_blocks_chained(tag, NULL, NULL, edata, edl, MCD_SEND, MCO_ENCYPHER); + + if (0 != memcmp((uint8_t *)data + *nbytes - 1, (uint8_t *)edata + edl - 8, 4)) { +#ifdef WITH_DEBUG + Dbprintf("MACing not verified"); + hexdump((uint8_t *)data + *nbytes - 1, key_macing_length(key), "Expect ", 0); + hexdump((uint8_t *)edata + edl - 8, key_macing_length(key), "Actual ", 0); +#endif + DESFIRE(tag)->last_pcd_error = CRYPTO_ERROR; + *nbytes = -1; + res = NULL; + } + } break; - } + case AS_NEW: + if (!(communication_settings & CMAC_COMMAND)) + break; + if (communication_settings & CMAC_VERIFY) { + if (*nbytes < 9) { + *nbytes = -1; + res = NULL; + break; + } + first_cmac_byte = ((uint8_t *)data)[*nbytes - 9]; + ((uint8_t *)data)[*nbytes - 9] = ((uint8_t *)data)[*nbytes - 1]; + } - edl = enciphered_data_length (tag, *nbytes - 1, communication_settings); - edata = malloc (edl); + int n = (communication_settings & CMAC_VERIFY) ? 8 : 0; + cmac(key, DESFIRE(tag)->ivect, ((uint8_t *)data), *nbytes - n, DESFIRE(tag)->cmac); - memcpy (edata, data, *nbytes - 1); - memset ((uint8_t *)edata + *nbytes - 1, 0, edl - *nbytes + 1); - - mifare_cypher_blocks_chained (tag, NULL, NULL, edata, edl, MCD_SEND, MCO_ENCYPHER); - - if (0 != memcmp ((uint8_t *)data + *nbytes - 1, (uint8_t *)edata + edl - 8, 4)) { + if (communication_settings & CMAC_VERIFY) { + ((uint8_t *)data)[*nbytes - 9] = first_cmac_byte; + if (0 != memcmp(DESFIRE(tag)->cmac, (uint8_t *)data + *nbytes - 9, 8)) { #ifdef WITH_DEBUG - Dbprintf ("MACing not verified"); - hexdump ((uint8_t *)data + *nbytes - 1, key_macing_length (key), "Expect ", 0); - hexdump ((uint8_t *)edata + edl - 8, key_macing_length (key), "Actual ", 0); + Dbprintf("CMAC NOT verified :-("); + hexdump((uint8_t *)data + *nbytes - 9, 8, "Expect ", 0); + hexdump(DESFIRE(tag)->cmac, 8, "Actual ", 0); #endif - DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR; - *nbytes = -1; - res = NULL; - } + DESFIRE(tag)->last_pcd_error = CRYPTO_ERROR; + *nbytes = -1; + res = NULL; + } else { + *nbytes -= 8; + } + } + break; } + + free(edata); + break; - case AS_NEW: - if (!(communication_settings & CMAC_COMMAND)) - break; - if (communication_settings & CMAC_VERIFY) { - if (*nbytes < 9) { - *nbytes = -1; - res = NULL; + case MDCM_ENCIPHERED: + (*nbytes)--; + bool verified = false; + int crc_pos = 0x00; + int end_crc_pos = 0x00; + uint8_t x; + + /* + * AS_LEGACY: + * ,-----------------+-------------------------------+--------+ + * \ BLOCK n-1 | BLOCK n | STATUS | + * / PAYLOAD | CRC0 | CRC1 | 0x80? | 0x000000000000 | 0x9100 | + * `-----------------+-------------------------------+--------+ + * + * <------------ DATA ------------> + * FRAME = PAYLOAD + CRC(PAYLOAD) + PADDING + * + * AS_NEW: + * ,-------------------------------+-----------------------------------------------+--------+ + * \ BLOCK n-1 | BLOCK n | STATUS | + * / PAYLOAD | CRC0 | CRC1 | CRC2 | CRC3 | 0x80? | 0x0000000000000000000000000000 | 0x9100 | + * `-------------------------------+-----------------------------------------------+--------+ + * <----------------------------------- DATA ------------------------------------->| + * + * <----------------- DATA ----------------> + * FRAME = PAYLOAD + CRC(PAYLOAD + STATUS) + PADDING + STATUS + * `------------------' + */ + + mifare_cypher_blocks_chained(tag, NULL, NULL, res, *nbytes, MCD_RECEIVE, MCO_DECYPHER); + + /* + * Look for the CRC and ensure it is followed by NULL padding. We + * can't start by the end because the CRC is supposed to be 0 when + * verified, and accumulating 0's in it should not change it. + */ + switch (DESFIRE(tag)->authentication_scheme) { + case AS_LEGACY: + crc_pos = *nbytes - 8 - 1; // The CRC can be over two blocks + if (crc_pos < 0) { + /* Single block */ + crc_pos = 0; + } + break; + case AS_NEW: + /* Move status between payload and CRC */ + res = DESFIRE(tag)->crypto_buffer; + memcpy(res, data, *nbytes); + + crc_pos = (*nbytes) - 16 - 3; + if (crc_pos < 0) { + /* Single block */ + crc_pos = 0; + } + memcpy((uint8_t *)res + crc_pos + 1, (uint8_t *)res + crc_pos, *nbytes - crc_pos); + ((uint8_t *)res)[crc_pos] = 0x00; + crc_pos++; + *nbytes += 1; break; - } - first_cmac_byte = ((uint8_t *)data)[*nbytes - 9]; - ((uint8_t *)data)[*nbytes - 9] = ((uint8_t *)data)[*nbytes-1]; } - int n = (communication_settings & CMAC_VERIFY) ? 8 : 0; - cmac (key, DESFIRE (tag)->ivect, ((uint8_t *)data), *nbytes - n, DESFIRE (tag)->cmac); + do { + uint16_t crc16 = 0x00; + uint32_t crc; + switch (DESFIRE(tag)->authentication_scheme) { + case AS_LEGACY: + AddCrc14A((uint8_t *)res, end_crc_pos); + end_crc_pos = crc_pos + 2; + // - if (communication_settings & CMAC_VERIFY) { - ((uint8_t *)data)[*nbytes - 9] = first_cmac_byte; - if (0 != memcmp (DESFIRE (tag)->cmac, (uint8_t *)data + *nbytes - 9, 8)) { -#ifdef WITH_DEBUG - Dbprintf ("CMAC NOT verified :-("); - hexdump ((uint8_t *)data + *nbytes - 9, 8, "Expect ", 0); - hexdump (DESFIRE (tag)->cmac, 8, "Actual ", 0); -#endif - DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR; - *nbytes = -1; - res = NULL; + + crc = crc16; + break; + case AS_NEW: + end_crc_pos = crc_pos + 4; + crc32_ex(res, end_crc_pos, (uint8_t *)&crc); + break; + } + if (!crc) { + verified = true; + for (int n = end_crc_pos; n < *nbytes - 1; n++) { + uint8_t byte = ((uint8_t *)res)[n]; + if (!((0x00 == byte) || ((0x80 == byte) && (n == end_crc_pos)))) + verified = false; + } + } + if (verified) { + *nbytes = crc_pos; + switch (DESFIRE(tag)->authentication_scheme) { + case AS_LEGACY: + ((uint8_t *)data)[(*nbytes)++] = 0x00; + break; + case AS_NEW: + /* The status byte was already before the CRC */ + break; + } } else { - *nbytes -= 8; + switch (DESFIRE(tag)->authentication_scheme) { + case AS_LEGACY: + break; + case AS_NEW: + x = ((uint8_t *)res)[crc_pos - 1]; + ((uint8_t *)res)[crc_pos - 1] = ((uint8_t *)res)[crc_pos]; + ((uint8_t *)res)[crc_pos] = x; + break; + } + crc_pos++; } - } - break; - } + } while (!verified && (end_crc_pos < *nbytes)); - free (edata); - - break; - case MDCM_ENCIPHERED: - (*nbytes)--; - bool verified = false; - int crc_pos = 0x00; - int end_crc_pos = 0x00; - uint8_t x; - - /* - * AS_LEGACY: - * ,-----------------+-------------------------------+--------+ - * \ BLOCK n-1 | BLOCK n | STATUS | - * / PAYLOAD | CRC0 | CRC1 | 0x80? | 0x000000000000 | 0x9100 | - * `-----------------+-------------------------------+--------+ - * - * <------------ DATA ------------> - * FRAME = PAYLOAD + CRC(PAYLOAD) + PADDING - * - * AS_NEW: - * ,-------------------------------+-----------------------------------------------+--------+ - * \ BLOCK n-1 | BLOCK n | STATUS | - * / PAYLOAD | CRC0 | CRC1 | CRC2 | CRC3 | 0x80? | 0x0000000000000000000000000000 | 0x9100 | - * `-------------------------------+-----------------------------------------------+--------+ - * <----------------------------------- DATA ------------------------------------->| - * - * <----------------- DATA ----------------> - * FRAME = PAYLOAD + CRC(PAYLOAD + STATUS) + PADDING + STATUS - * `------------------' - */ - - mifare_cypher_blocks_chained (tag, NULL, NULL, res, *nbytes, MCD_RECEIVE, MCO_DECYPHER); - - /* - * Look for the CRC and ensure it is followed by NULL padding. We - * can't start by the end because the CRC is supposed to be 0 when - * verified, and accumulating 0's in it should not change it. - */ - switch (DESFIRE (tag)->authentication_scheme) { - case AS_LEGACY: - crc_pos = *nbytes - 8 - 1; // The CRC can be over two blocks - if (crc_pos < 0) { - /* Single block */ - crc_pos = 0; - } - break; - case AS_NEW: - /* Move status between payload and CRC */ - res = DESFIRE (tag)->crypto_buffer; - memcpy (res, data, *nbytes); - - crc_pos = (*nbytes) - 16 - 3; - if (crc_pos < 0) { - /* Single block */ - crc_pos = 0; - } - memcpy ((uint8_t *)res + crc_pos + 1, (uint8_t *)res + crc_pos, *nbytes - crc_pos); - ((uint8_t *)res)[crc_pos] = 0x00; - crc_pos++; - *nbytes += 1; - break; - } - - do { - uint16_t crc16 =0x00; - uint32_t crc; - switch (DESFIRE (tag)->authentication_scheme) { - case AS_LEGACY: - AddCrc14A( (uint8_t*)res, end_crc_pos); - end_crc_pos = crc_pos + 2; - // - - - crc = crc16; - break; - case AS_NEW: - end_crc_pos = crc_pos + 4; - crc32_ex (res, end_crc_pos, (uint8_t *)&crc); - break; - } - if (!crc) { - verified = true; - for (int n = end_crc_pos; n < *nbytes - 1; n++) { - uint8_t byte = ((uint8_t *)res)[n]; - if (!( (0x00 == byte) || ((0x80 == byte) && (n == end_crc_pos)) )) - verified = false; - } - } - if (verified) { - *nbytes = crc_pos; - switch (DESFIRE (tag)->authentication_scheme) { - case AS_LEGACY: - ((uint8_t *)data)[(*nbytes)++] = 0x00; - break; - case AS_NEW: - /* The status byte was already before the CRC */ - break; - } - } else { - switch (DESFIRE (tag)->authentication_scheme) { - case AS_LEGACY: - break; - case AS_NEW: - x = ((uint8_t *)res)[crc_pos - 1]; - ((uint8_t *)res)[crc_pos - 1] = ((uint8_t *)res)[crc_pos]; - ((uint8_t *)res)[crc_pos] = x; - break; - } - crc_pos++; - } - } while (!verified && (end_crc_pos < *nbytes)); - - if (!verified) { + if (!verified) { #ifdef WITH_DEBUG - /* FIXME In some configurations, the file is transmitted PLAIN */ - Dbprintf("CRC not verified in decyphered stream"); + /* FIXME In some configurations, the file is transmitted PLAIN */ + Dbprintf("CRC not verified in decyphered stream"); #endif - DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR; + DESFIRE(tag)->last_pcd_error = CRYPTO_ERROR; + *nbytes = -1; + res = NULL; + } + + break; + default: + Dbprintf("Unknown communication settings"); *nbytes = -1; res = NULL; - } - - break; - default: - Dbprintf("Unknown communication settings"); - *nbytes = -1; - res = NULL; - break; + break; } return res; } -void mifare_cypher_single_block (desfirekey_t key, uint8_t *data, uint8_t *ivect, MifareCryptoDirection direction, MifareCryptoOperation operation, size_t block_size) +void mifare_cypher_single_block(desfirekey_t key, uint8_t *data, uint8_t *ivect, MifareCryptoDirection direction, MifareCryptoOperation operation, size_t block_size) { uint8_t ovect[MAX_CRYPTO_BLOCK_SIZE]; if (direction == MCD_SEND) { - xor (ivect, data, block_size); + xor(ivect, data, block_size); } else { - memcpy (ovect, data, block_size); + memcpy(ovect, data, block_size); } uint8_t edata[MAX_CRYPTO_BLOCK_SIZE]; switch (key->type) { - case T_DES: - switch (operation) { - case MCO_ENCYPHER: - //DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); - des_enc(edata, data, key->data); - break; - case MCO_DECYPHER: - //DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); - des_dec(edata, data, key->data); - break; - } - break; - case T_3DES: - switch (operation) { - case MCO_ENCYPHER: - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); - // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_DECRYPT); - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); - tdes_enc(edata,data, key->data); - break; - case MCO_DECYPHER: - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); - // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_ENCRYPT); - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); - tdes_dec(data, edata, key->data); - break; - } - break; - case T_3K3DES: - switch (operation) { - case MCO_ENCYPHER: - tdes_enc(edata,data, key->data); - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); - // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_DECRYPT); - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks3), DES_ENCRYPT); - break; - case MCO_DECYPHER: - tdes_dec(data, edata, key->data); - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks3), DES_DECRYPT); - // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_ENCRYPT); - // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); - break; - } - break; - case T_AES: - switch (operation) - { - case MCO_ENCYPHER: - { - AesCtx ctx; - AesCtxIni(&ctx, ivect, key->data, KEY128,CBC); - AesEncrypt(&ctx, data, edata, sizeof(edata) ); - break; + case T_DES: + switch (operation) { + case MCO_ENCYPHER: + //DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); + des_enc(edata, data, key->data); + break; + case MCO_DECYPHER: + //DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); + des_dec(edata, data, key->data); + break; } - case MCO_DECYPHER: - { - AesCtx ctx; - AesCtxIni(&ctx, ivect, key->data, KEY128,CBC); - AesDecrypt(&ctx, edata, data, sizeof(edata)); - break; + break; + case T_3DES: + switch (operation) { + case MCO_ENCYPHER: + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); + // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_DECRYPT); + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); + tdes_enc(edata, data, key->data); + break; + case MCO_DECYPHER: + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); + // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_ENCRYPT); + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); + tdes_dec(data, edata, key->data); + break; } - } - break; + break; + case T_3K3DES: + switch (operation) { + case MCO_ENCYPHER: + tdes_enc(edata, data, key->data); + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT); + // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_DECRYPT); + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks3), DES_ENCRYPT); + break; + case MCO_DECYPHER: + tdes_dec(data, edata, key->data); + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks3), DES_DECRYPT); + // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data, &(key->ks2), DES_ENCRYPT); + // DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT); + break; + } + break; + case T_AES: + switch (operation) { + case MCO_ENCYPHER: { + AesCtx ctx; + AesCtxIni(&ctx, ivect, key->data, KEY128, CBC); + AesEncrypt(&ctx, data, edata, sizeof(edata)); + break; + } + case MCO_DECYPHER: { + AesCtx ctx; + AesCtxIni(&ctx, ivect, key->data, KEY128, CBC); + AesDecrypt(&ctx, edata, data, sizeof(edata)); + break; + } + } + break; } - memcpy (data, edata, block_size); + memcpy(data, edata, block_size); if (direction == MCD_SEND) { - memcpy (ivect, data, block_size); + memcpy(ivect, data, block_size); } else { - xor (ivect, data, block_size); - memcpy (ivect, ovect, block_size); + xor(ivect, data, block_size); + memcpy(ivect, ovect, block_size); } } @@ -610,29 +616,30 @@ void mifare_cypher_single_block (desfirekey_t key, uint8_t *data, uint8_t *ivect * Because the tag may contain additional data, one may need to call this * function with tag, key and ivect defined. */ -void mifare_cypher_blocks_chained (desfiretag_t tag, desfirekey_t key, uint8_t *ivect, uint8_t *data, size_t data_size, MifareCryptoDirection direction, MifareCryptoOperation operation) { +void mifare_cypher_blocks_chained(desfiretag_t tag, desfirekey_t key, uint8_t *ivect, uint8_t *data, size_t data_size, MifareCryptoDirection direction, MifareCryptoOperation operation) +{ size_t block_size; if (tag) { if (!key) - key = DESFIRE (tag)->session_key; + key = DESFIRE(tag)->session_key; if (!ivect) - ivect = DESFIRE (tag)->ivect; + ivect = DESFIRE(tag)->ivect; - switch (DESFIRE (tag)->authentication_scheme) { + switch (DESFIRE(tag)->authentication_scheme) { case AS_LEGACY: - memset (ivect, 0, MAX_CRYPTO_BLOCK_SIZE); + memset(ivect, 0, MAX_CRYPTO_BLOCK_SIZE); break; case AS_NEW: break; } } - block_size = key_block_size (key); + block_size = key_block_size(key); size_t offset = 0; while (offset < data_size) { - mifare_cypher_single_block (key, data + offset, ivect, direction, operation, block_size); + mifare_cypher_single_block(key, data + offset, ivect, direction, operation, block_size); offset += block_size; } } \ No newline at end of file diff --git a/armsrc/desfire_key.c b/armsrc/desfire_key.c index f8ea8c794..1123225bb 100644 --- a/armsrc/desfire_key.c +++ b/armsrc/desfire_key.c @@ -19,82 +19,92 @@ #include "desfire_key.h" -static inline void update_key_schedules (desfirekey_t key); +static inline void update_key_schedules(desfirekey_t key); -static inline void update_key_schedules (desfirekey_t key) { +static inline void update_key_schedules(desfirekey_t key) +{ // DES_set_key ((DES_cblock *)key->data, &(key->ks1)); // DES_set_key ((DES_cblock *)(key->data + 8), &(key->ks2)); // if (T_3K3DES == key->type) { - // DES_set_key ((DES_cblock *)(key->data + 16), &(key->ks3)); + // DES_set_key ((DES_cblock *)(key->data + 16), &(key->ks3)); // } } -void Desfire_des_key_new (const uint8_t value[8], desfirekey_t key) { +void Desfire_des_key_new(const uint8_t value[8], desfirekey_t key) +{ uint8_t data[8]; - memcpy (data, value, 8); - for (int n=0; n < 8; n++) + memcpy(data, value, 8); + for (int n = 0; n < 8; n++) data[n] &= 0xfe; - Desfire_des_key_new_with_version (data, key); + Desfire_des_key_new_with_version(data, key); } -void Desfire_des_key_new_with_version (const uint8_t value[8], desfirekey_t key) { - if ( key != NULL) { +void Desfire_des_key_new_with_version(const uint8_t value[8], desfirekey_t key) +{ + if (key != NULL) { key->type = T_DES; - memcpy (key->data, value, 8); - memcpy (key->data+8, value, 8); - update_key_schedules (key); + memcpy(key->data, value, 8); + memcpy(key->data + 8, value, 8); + update_key_schedules(key); } } -void Desfire_3des_key_new (const uint8_t value[16], desfirekey_t key) { +void Desfire_3des_key_new(const uint8_t value[16], desfirekey_t key) +{ uint8_t data[16]; - memcpy (data, value, 16); - for (int n=0; n < 8; n++) + memcpy(data, value, 16); + for (int n = 0; n < 8; n++) data[n] &= 0xfe; - for (int n=8; n < 16; n++) + for (int n = 8; n < 16; n++) data[n] |= 0x01; - Desfire_3des_key_new_with_version (data, key); + Desfire_3des_key_new_with_version(data, key); } -void Desfire_3des_key_new_with_version (const uint8_t value[16], desfirekey_t key) { - if ( key != NULL ){ +void Desfire_3des_key_new_with_version(const uint8_t value[16], desfirekey_t key) +{ + if (key != NULL) { key->type = T_3DES; - memcpy (key->data, value, 16); - memcpy (key->data + 16, value, 8); - update_key_schedules (key); + memcpy(key->data, value, 16); + memcpy(key->data + 16, value, 8); + update_key_schedules(key); } } -void Desfire_3k3des_key_new (const uint8_t value[24], desfirekey_t key) { +void Desfire_3k3des_key_new(const uint8_t value[24], desfirekey_t key) +{ uint8_t data[24]; - memcpy (data, value, 24); - for (int n=0; n < 8; n++) + memcpy(data, value, 24); + for (int n = 0; n < 8; n++) data[n] &= 0xfe; - Desfire_3k3des_key_new_with_version (data, key); + Desfire_3k3des_key_new_with_version(data, key); } -void Desfire_3k3des_key_new_with_version (const uint8_t value[24], desfirekey_t key) { - if ( key != NULL){ +void Desfire_3k3des_key_new_with_version(const uint8_t value[24], desfirekey_t key) +{ + if (key != NULL) { key->type = T_3K3DES; - memcpy (key->data, value, 24); - update_key_schedules (key); + memcpy(key->data, value, 24); + update_key_schedules(key); } } - void Desfire_aes_key_new (const uint8_t value[16], desfirekey_t key) { - Desfire_aes_key_new_with_version (value, 0, key); +void Desfire_aes_key_new(const uint8_t value[16], desfirekey_t key) +{ + Desfire_aes_key_new_with_version(value, 0, key); } - void Desfire_aes_key_new_with_version (const uint8_t value[16], uint8_t version, desfirekey_t key) { +void Desfire_aes_key_new_with_version(const uint8_t value[16], uint8_t version, desfirekey_t key) +{ if (key != NULL) { - memcpy (key->data, value, 16); + memcpy(key->data, value, 16); key->type = T_AES; key->aes_version = version; } } -uint8_t Desfire_key_get_version (desfirekey_t key) { +uint8_t Desfire_key_get_version(desfirekey_t key) +{ uint8_t version = 0; for (int n = 0; n < 8; n++) { @@ -103,54 +113,55 @@ uint8_t Desfire_key_get_version (desfirekey_t key) { return version; } -void Desfire_key_set_version (desfirekey_t key, uint8_t version) +void Desfire_key_set_version(desfirekey_t key, uint8_t version) { for (int n = 0; n < 8; n++) { - uint8_t version_bit = ((version & (1 << (7-n))) >> (7-n)); + uint8_t version_bit = ((version & (1 << (7 - n))) >> (7 - n)); key->data[n] &= 0xfe; key->data[n] |= version_bit; if (key->type == T_DES) { - key->data[n+8] = key->data[n]; + key->data[n + 8] = key->data[n]; } else { // Write ~version to avoid turning a 3DES key into a DES key - key->data[n+8] &= 0xfe; - key->data[n+8] |= ~version_bit; + key->data[n + 8] &= 0xfe; + key->data[n + 8] |= ~version_bit; } } } -void Desfire_session_key_new (const uint8_t rnda[], const uint8_t rndb[], desfirekey_t authkey, desfirekey_t key) { +void Desfire_session_key_new(const uint8_t rnda[], const uint8_t rndb[], desfirekey_t authkey, desfirekey_t key) +{ uint8_t buffer[24]; switch (authkey->type) { - case T_DES: - memcpy (buffer, rnda, 4); - memcpy (buffer+4, rndb, 4); - Desfire_des_key_new_with_version (buffer, key); - break; - case T_3DES: - memcpy (buffer, rnda, 4); - memcpy (buffer+4, rndb, 4); - memcpy (buffer+8, rnda+4, 4); - memcpy (buffer+12, rndb+4, 4); - Desfire_3des_key_new_with_version (buffer, key); - break; - case T_3K3DES: - memcpy (buffer, rnda, 4); - memcpy (buffer+4, rndb, 4); - memcpy (buffer+8, rnda+6, 4); - memcpy (buffer+12, rndb+6, 4); - memcpy (buffer+16, rnda+12, 4); - memcpy (buffer+20, rndb+12, 4); - Desfire_3k3des_key_new (buffer, key); - break; - case T_AES: - memcpy (buffer, rnda, 4); - memcpy (buffer+4, rndb, 4); - memcpy (buffer+8, rnda+12, 4); - memcpy (buffer+12, rndb+12, 4); - Desfire_aes_key_new (buffer, key); - break; + case T_DES: + memcpy(buffer, rnda, 4); + memcpy(buffer + 4, rndb, 4); + Desfire_des_key_new_with_version(buffer, key); + break; + case T_3DES: + memcpy(buffer, rnda, 4); + memcpy(buffer + 4, rndb, 4); + memcpy(buffer + 8, rnda + 4, 4); + memcpy(buffer + 12, rndb + 4, 4); + Desfire_3des_key_new_with_version(buffer, key); + break; + case T_3K3DES: + memcpy(buffer, rnda, 4); + memcpy(buffer + 4, rndb, 4); + memcpy(buffer + 8, rnda + 6, 4); + memcpy(buffer + 12, rndb + 6, 4); + memcpy(buffer + 16, rnda + 12, 4); + memcpy(buffer + 20, rndb + 12, 4); + Desfire_3k3des_key_new(buffer, key); + break; + case T_AES: + memcpy(buffer, rnda, 4); + memcpy(buffer + 4, rndb, 4); + memcpy(buffer + 8, rnda + 12, 4); + memcpy(buffer + 12, rndb + 12, 4); + Desfire_aes_key_new(buffer, key); + break; } } \ No newline at end of file diff --git a/armsrc/desfire_key.h b/armsrc/desfire_key.h index cb0e8b250..adbda9c42 100644 --- a/armsrc/desfire_key.h +++ b/armsrc/desfire_key.h @@ -6,15 +6,15 @@ #include "iso14443a.h" #include "desfire.h" //#include "mifare.h" // iso14a_card_select_t struct -void Desfire_des_key_new (const uint8_t value[8], desfirekey_t key); -void Desfire_3des_key_new (const uint8_t value[16], desfirekey_t key); -void Desfire_des_key_new_with_version (const uint8_t value[8], desfirekey_t key); -void Desfire_3des_key_new_with_version (const uint8_t value[16], desfirekey_t key); -void Desfire_3k3des_key_new (const uint8_t value[24], desfirekey_t key); -void Desfire_3k3des_key_new_with_version (const uint8_t value[24], desfirekey_t key); -void Desfire_aes_key_new (const uint8_t value[16], desfirekey_t key); -void Desfire_aes_key_new_with_version (const uint8_t value[16], uint8_t version,desfirekey_t key); -uint8_t Desfire_key_get_version (desfirekey_t key); -void Desfire_key_set_version (desfirekey_t key, uint8_t version); -void Desfire_session_key_new (const uint8_t rnda[], const uint8_t rndb[], desfirekey_t authkey, desfirekey_t key); +void Desfire_des_key_new(const uint8_t value[8], desfirekey_t key); +void Desfire_3des_key_new(const uint8_t value[16], desfirekey_t key); +void Desfire_des_key_new_with_version(const uint8_t value[8], desfirekey_t key); +void Desfire_3des_key_new_with_version(const uint8_t value[16], desfirekey_t key); +void Desfire_3k3des_key_new(const uint8_t value[24], desfirekey_t key); +void Desfire_3k3des_key_new_with_version(const uint8_t value[24], desfirekey_t key); +void Desfire_aes_key_new(const uint8_t value[16], desfirekey_t key); +void Desfire_aes_key_new_with_version(const uint8_t value[16], uint8_t version, desfirekey_t key); +uint8_t Desfire_key_get_version(desfirekey_t key); +void Desfire_key_set_version(desfirekey_t key, uint8_t version); +void Desfire_session_key_new(const uint8_t rnda[], const uint8_t rndb[], desfirekey_t authkey, desfirekey_t key); #endif \ No newline at end of file diff --git a/armsrc/epa.c b/armsrc/epa.c index 63a57d46b..0f09980ee 100644 --- a/armsrc/epa.c +++ b/armsrc/epa.c @@ -105,8 +105,7 @@ static char iso_type = 0; //----------------------------------------------------------------------------- int EPA_APDU(uint8_t *apdu, size_t length, uint8_t *response) { - switch(iso_type) - { + switch (iso_type) { case 'a': return iso14_apdu(apdu, (uint16_t) length, false, response, NULL); break; @@ -156,7 +155,7 @@ size_t EPA_Parse_CardAccess(uint8_t *data, index += 2; // check for extended length if ((data[index - 1] & 0x80) != 0) { - index += (data[index-1] & 0x7F); + index += (data[index - 1] & 0x7F); } } // OID @@ -166,8 +165,7 @@ size_t EPA_Parse_CardAccess(uint8_t *data, && memcmp(data + index + 2, oid_pace_start, sizeof(oid_pace_start)) == 0 // content matches - && pace_info != NULL) - { + && pace_info != NULL) { // first, clear the pace_info struct memset(pace_info, 0, sizeof(pace_version_info_t)); memcpy(pace_info->oid, data + index + 2, sizeof(pace_info->oid)); @@ -176,8 +174,7 @@ size_t EPA_Parse_CardAccess(uint8_t *data, if (data[index] == 02 && data[index + 1] == 01) { pace_info->version = data[index + 2]; index += 3; - } - else { + } else { return index; } // after that there might(!) be the parameter ID @@ -185,8 +182,7 @@ size_t EPA_Parse_CardAccess(uint8_t *data, pace_info->parameter_id = data[index + 2]; index += 3; } - } - else { + } else { // skip this OID index += 2 + data[index + 1]; } @@ -195,8 +191,7 @@ size_t EPA_Parse_CardAccess(uint8_t *data, // TODO: This needs to be extended to support long tags else if (data[index + 1] == 0) { return index; - } - else { + } else { // skip this part // TODO: This needs to be extended to support long tags // TODO: This needs to be extended to support unknown elements with @@ -225,24 +220,22 @@ int EPA_Read_CardAccess(uint8_t *buffer, size_t max_length) // select the file EF.CardAccess rapdu_length = EPA_APDU((uint8_t *)apdu_select_binary_cardaccess, - sizeof(apdu_select_binary_cardaccess), - response_apdu); + sizeof(apdu_select_binary_cardaccess), + response_apdu); if (rapdu_length < 6 || response_apdu[rapdu_length - 4] != 0x90 - || response_apdu[rapdu_length - 3] != 0x00) - { + || response_apdu[rapdu_length - 3] != 0x00) { DbpString("Failed to select EF.CardAccess!"); return -1; } // read the file rapdu_length = EPA_APDU((uint8_t *)apdu_read_binary, - sizeof(apdu_read_binary), - response_apdu); + sizeof(apdu_read_binary), + response_apdu); if (rapdu_length <= 6 || response_apdu[rapdu_length - 4] != 0x90 - || response_apdu[rapdu_length - 3] != 0x00) - { + || response_apdu[rapdu_length - 3] != 0x00) { Dbprintf("Failed to read EF.CardAccess!"); return -1; } @@ -251,7 +244,7 @@ int EPA_Read_CardAccess(uint8_t *buffer, size_t max_length) // length of data available: apdu_length - 4 (ISO frame) - 2 (SW) size_t to_copy = rapdu_length - 6; to_copy = to_copy < max_length ? to_copy : max_length; - memcpy(buffer, response_apdu+2, to_copy); + memcpy(buffer, response_apdu + 2, to_copy); return to_copy; } @@ -265,7 +258,7 @@ static void EPA_PACE_Collect_Nonce_Abort(uint8_t step, int func_return) EPA_Finish(); // send the USB packet - cmd_send(CMD_ACK,step,func_return,0,0,0); + cmd_send(CMD_ACK, step, func_return, 0, 0, 0); } //----------------------------------------------------------------------------- @@ -324,8 +317,7 @@ void EPA_PACE_Collect_Nonce(UsbCommand *c) uint8_t requested_size = (uint8_t)c->arg[0]; func_return = EPA_PACE_Get_Nonce(requested_size, nonce); // check if the command succeeded - if (func_return < 0) - { + if (func_return < 0) { EPA_PACE_Collect_Nonce_Abort(4, func_return); return; } @@ -334,7 +326,7 @@ void EPA_PACE_Collect_Nonce(UsbCommand *c) EPA_Finish(); // save received information - cmd_send(CMD_ACK,0,func_return,0,nonce,func_return); + cmd_send(CMD_ACK, 0, func_return, 0, nonce, func_return); } //----------------------------------------------------------------------------- @@ -359,26 +351,23 @@ int EPA_PACE_Get_Nonce(uint8_t requested_length, uint8_t *nonce) // send it uint8_t response_apdu[262]; int send_return = EPA_APDU(apdu, - sizeof(apdu), - response_apdu); + sizeof(apdu), + response_apdu); // check if the command succeeded if (send_return < 6 || response_apdu[send_return - 4] != 0x90 - || response_apdu[send_return - 3] != 0x00) - { + || response_apdu[send_return - 3] != 0x00) { return -1; } // if there is no nonce in the RAPDU, return here - if (send_return < 10) - { + if (send_return < 10) { // no error return 0; } // get the actual length of the nonce uint8_t nonce_length = response_apdu[5]; - if (nonce_length > send_return - 10) - { + if (nonce_length > send_return - 10) { nonce_length = send_return - 10; } // copy the nonce @@ -430,13 +419,12 @@ int EPA_PACE_MSE_Set_AT(pace_version_info_t pace_version_info, uint8_t password) // send it uint8_t response_apdu[6]; int send_return = EPA_APDU(apdu, - apdu_length, - response_apdu); + apdu_length, + response_apdu); // check if the command succeeded if (send_return != 6 || response_apdu[send_return - 4] != 0x90 - || response_apdu[send_return - 3] != 0x00) - { + || response_apdu[send_return - 3] != 0x00) { return 1; } return 0; @@ -452,8 +440,7 @@ void EPA_PACE_Replay(UsbCommand *c) // if an APDU has been passed, save it if (c->arg[0] != 0) { // make sure it's not too big - if(c->arg[2] > apdus_replay[c->arg[0] - 1].len) - { + if (c->arg[2] > apdus_replay[c->arg[0] - 1].len) { cmd_send(CMD_ACK, 1, 0, 0, NULL, 0); } memcpy(apdus_replay[c->arg[0] - 1].data + c->arg[1], @@ -490,22 +477,21 @@ void EPA_PACE_Replay(UsbCommand *c) for (int i = 0; i < sizeof(apdu_lengths_replay); i++) { StartCountUS(); func_return = EPA_APDU(apdus_replay[i].data, - apdu_lengths_replay[i], - response_apdu); + apdu_lengths_replay[i], + response_apdu); timings[i] = GetCountUS(); // every step but the last one should succeed if (i < sizeof(apdu_lengths_replay) - 1 && (func_return < 6 || response_apdu[func_return - 4] != 0x90 - || response_apdu[func_return - 3] != 0x00)) - { + || response_apdu[func_return - 3] != 0x00)) { EPA_Finish(); cmd_send(CMD_ACK, 3 + i, func_return, 0, timings, 20); return; } } EPA_Finish(); - cmd_send(CMD_ACK,0,0,0,timings,20); + cmd_send(CMD_ACK, 0, 0, 0, timings, 20); return; } @@ -543,7 +529,7 @@ int EPA_Setup() // power up the field iso14443b_setup(); // select the card - return_code = iso14443b_select_card( &card_b_info ); + return_code = iso14443b_select_card(&card_b_info); if (return_code == 0) { Dbprintf("ISO 14443 Type B"); iso_type = 'b'; diff --git a/armsrc/felica.c b/armsrc/felica.c index 07aa763c9..1d3ef781e 100644 --- a/armsrc/felica.c +++ b/armsrc/felica.c @@ -31,23 +31,25 @@ static uint32_t felica_lasttime_prox2air_start; static void iso18092_setup(uint8_t fpga_minor_mode); static uint8_t felica_select_card(felica_card_select_t *card); -static void TransmitFor18092_AsReader(uint8_t * frame, int len, uint32_t *timing, uint8_t power, uint8_t highspeed); +static void TransmitFor18092_AsReader(uint8_t *frame, int len, uint32_t *timing, uint8_t power, uint8_t highspeed); bool WaitForFelicaReply(uint16_t maxbytes); -void iso18092_set_timeout(uint32_t timeout) { - felica_timeout = timeout + (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/(16*8) + 2; +void iso18092_set_timeout(uint32_t timeout) +{ + felica_timeout = timeout + (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER) / (16 * 8) + 2; } -uint32_t iso18092_get_timeout(void) { - return felica_timeout - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/(16*8) - 2; +uint32_t iso18092_get_timeout(void) +{ + return felica_timeout - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER) / (16 * 8) - 2; } #ifndef FELICA_MAX_FRAME_SIZE - #define FELICA_MAX_FRAME_SIZE 260 +#define FELICA_MAX_FRAME_SIZE 260 #endif //structure to hold outgoing NFC frame -static uint8_t frameSpace[FELICA_MAX_FRAME_SIZE+4]; +static uint8_t frameSpace[FELICA_MAX_FRAME_SIZE + 4]; //structure to hold incoming NFC frame, used for ISO/IEC 18092-compatible frames static struct { @@ -67,8 +69,8 @@ static struct { uint16_t len; uint8_t byte_offset; uint8_t *framebytes; - //should be enough. maxlen is 255, 254 for data, 2 for sync, 2 for crc - // 0,1 -> SYNC, 2 - len, 3-(len+1)->data, then crc +//should be enough. maxlen is 255, 254 for data, 2 for sync, 2 for crc +// 0,1 -> SYNC, 2 - len, 3-(len+1)->data, then crc } FelicaFrame; //b2 4d is SYNC, 45645 in 16-bit notation, 10110010 01001101 binary. Frame will not start filling until this is shifted in @@ -77,33 +79,37 @@ static struct { # define SYNC_16BIT 0xB24D #endif -static void FelicaFrameReset() { +static void FelicaFrameReset() +{ FelicaFrame.state = STATE_UNSYNCD; FelicaFrame.posCnt = 0; FelicaFrame.crc_ok = false; FelicaFrame.byte_offset = 0; } -static void FelicaFrameinit(uint8_t *data) { +static void FelicaFrameinit(uint8_t *data) +{ FelicaFrame.framebytes = data; FelicaFrameReset(); } //shift byte into frame, reversing it at the same time -static void shiftInByte(uint8_t bt) { +static void shiftInByte(uint8_t bt) +{ uint8_t j; - for(j=0; j < FelicaFrame.byte_offset; j++) { - FelicaFrame.framebytes[FelicaFrame.posCnt] = ( FelicaFrame.framebytes[FelicaFrame.posCnt]<<1 ) + (bt & 1); + for (j = 0; j < FelicaFrame.byte_offset; j++) { + FelicaFrame.framebytes[FelicaFrame.posCnt] = (FelicaFrame.framebytes[FelicaFrame.posCnt] << 1) + (bt & 1); bt >>= 1; } FelicaFrame.posCnt++; FelicaFrame.rem_len--; - for(j = FelicaFrame.byte_offset; j<8; j++) { - FelicaFrame.framebytes[FelicaFrame.posCnt] = (FelicaFrame.framebytes[FelicaFrame.posCnt]<<1 ) + (bt & 1); + for (j = FelicaFrame.byte_offset; j < 8; j++) { + FelicaFrame.framebytes[FelicaFrame.posCnt] = (FelicaFrame.framebytes[FelicaFrame.posCnt] << 1) + (bt & 1); bt >>= 1; } } -static void Process18092Byte(uint8_t bt) { +static void Process18092Byte(uint8_t bt) +{ switch (FelicaFrame.state) { case STATE_UNSYNCD: { //almost any nonzero byte can be start of SYNC. SYNC should be preceded by zeros, but that is not alsways the case @@ -119,7 +125,7 @@ static void Process18092Byte(uint8_t bt) { FelicaFrame.shiftReg = bt; FelicaFrame.state = STATE_UNSYNCD; } else { - for (uint8_t i=0; i<8; i++) { + for (uint8_t i = 0; i < 8; i++) { if (FelicaFrame.shiftReg == SYNC_16BIT) { //SYNC done! @@ -128,13 +134,13 @@ static void Process18092Byte(uint8_t bt) { FelicaFrame.framebytes[1] = 0x4d; FelicaFrame.byte_offset = i; //shift in remaining byte, slowly... - for(uint8_t j=i; j<8; j++) { + for (uint8_t j = i; j < 8; j++) { FelicaFrame.framebytes[2] = (FelicaFrame.framebytes[2] << 1) + (bt & 1); bt >>= 1; } FelicaFrame.posCnt = 2; - if (i==0) + if (i == 0) break; } FelicaFrame.shiftReg = (FelicaFrame.shiftReg << 1) + (bt & 1); @@ -171,12 +177,12 @@ static void Process18092Byte(uint8_t bt) { case STATE_GET_CRC: { shiftInByte(bt); - if ( FelicaFrame.rem_len <= 0 ) { + if (FelicaFrame.rem_len <= 0) { // skip sync 2bytes. IF ok, residue should be 0x0000 - FelicaFrame.crc_ok = check_crc(CRC_FELICA, FelicaFrame.framebytes+2, FelicaFrame.len-2); + FelicaFrame.crc_ok = check_crc(CRC_FELICA, FelicaFrame.framebytes + 2, FelicaFrame.len - 2); FelicaFrame.state = STATE_FULL; FelicaFrame.rem_len = 0; - if (MF_DBGLEVEL > 3) Dbprintf("[+] got 2 crc bytes [%s]", (FelicaFrame.crc_ok) ? "OK" : "No" ); + if (MF_DBGLEVEL > 3) Dbprintf("[+] got 2 crc bytes [%s]", (FelicaFrame.crc_ok) ? "OK" : "No"); } break; } @@ -190,7 +196,8 @@ static void Process18092Byte(uint8_t bt) { * Currently does NOT do any collision handling. * It expects 0-1 cards in the device's range. */ -static uint8_t felica_select_card(felica_card_select_t *card) { +static uint8_t felica_select_card(felica_card_select_t *card) +{ // POLL command // 0xB2 0x4B = sync code @@ -205,7 +212,7 @@ static uint8_t felica_select_card(felica_card_select_t *card) { // b0 = fc/64 (212kbps) // 0x00 = timeslot // 0x09 0x21 = crc - static uint8_t poll[10] = {0xb2,0x4d,0x06,FELICA_POLL_REQ,0xFF,0xFF,0x00,0x00,0x09,0x21}; + static uint8_t poll[10] = {0xb2, 0x4d, 0x06, FELICA_POLL_REQ, 0xFF, 0xFF, 0x00, 0x00, 0x09, 0x21}; int len = 20; @@ -225,7 +232,7 @@ static uint8_t felica_select_card(felica_card_select_t *card) { } while (--len); // timed-out - if ( len == 0 ) + if (len == 0) return 1; // wrong answer @@ -233,7 +240,7 @@ static uint8_t felica_select_card(felica_card_select_t *card) { return 2; // VALIDATE CRC residue is 0, hence if crc is a value it failed. - if (!check_crc(CRC_FELICA, FelicaFrame.framebytes+2, FelicaFrame.len-2)) + if (!check_crc(CRC_FELICA, FelicaFrame.framebytes + 2, FelicaFrame.len - 2)) return 3; // copy UID @@ -245,7 +252,7 @@ static uint8_t felica_select_card(felica_card_select_t *card) { memcpy(card->code, card->IDm, 2); memcpy(card->uid, card->IDm + 2, 6); memcpy(card->iccode, card->PMm, 2); - memcpy(card->mrt, card->PMm+2, 6); + memcpy(card->mrt, card->PMm + 2, 6); } // more status bytes? @@ -264,7 +271,8 @@ static uint8_t felica_select_card(felica_card_select_t *card) { // Felica standard has a different file system, AFAIK, // 8-byte IDm, number of blocks, blocks numbers // number of blocks limited to 4 for FelicaLite(S) -static void BuildFliteRdblk(uint8_t* idm, int blocknum, uint16_t *blocks ) { +static void BuildFliteRdblk(uint8_t *idm, int blocknum, uint16_t *blocks) +{ if (blocknum > 4 || blocknum <= 0) Dbprintf("Invalid number of blocks, %d != 4", blocknum); @@ -298,13 +306,13 @@ static void BuildFliteRdblk(uint8_t* idm, int blocknum, uint16_t *blocks ) { //number of blocks frameSpace[c++] = blocknum; - for (i=0; i < blocknum; i++) { + for (i = 0; i < blocknum; i++) { //3-byte block if (blocks[i] >= 256) { - frameSpace[c++] = 0x00; - frameSpace[c++] = (blocks[i] >> 8); //block number, little endian.... - frameSpace[c++] = (blocks[i] & 0xff); + frameSpace[c++] = 0x00; + frameSpace[c++] = (blocks[i] >> 8); //block number, little endian.... + frameSpace[c++] = (blocks[i] & 0xff); } else { frameSpace[c++] = 0x80; frameSpace[c++] = blocks[i]; @@ -312,15 +320,16 @@ static void BuildFliteRdblk(uint8_t* idm, int blocknum, uint16_t *blocks ) { } //set length - frameSpace[2] = c-2; - AddCrc(frameSpace, c-2); + frameSpace[2] = c - 2; + AddCrc(frameSpace, c - 2); } -static void TransmitFor18092_AsReader(uint8_t * frame, int len, uint32_t *timing, uint8_t power, uint8_t highspeed) { +static void TransmitFor18092_AsReader(uint8_t *frame, int len, uint32_t *timing, uint8_t power, uint8_t highspeed) +{ uint8_t flags = FPGA_MAJOR_MODE_ISO18092; - if ( power ) + if (power) flags |= FPGA_HF_ISO18092_FLAG_READER; if (highspeed) flags |= FPGA_HF_ISO18092_FLAG_424K; @@ -356,31 +365,32 @@ static void TransmitFor18092_AsReader(uint8_t * frame, int len, uint32_t *timing } } -/**/ + /**/ while (!(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY))) {}; AT91C_BASE_SSC->SSC_THR = 0x00; //minimum delay while (!(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY))) {}; AT91C_BASE_SSC->SSC_THR = 0x00; //spin -/**/ + /**/ // log LogTrace( frame, len, - (felica_lasttime_prox2air_start<<4) + DELAY_ARM2AIR_AS_READER, - ((felica_lasttime_prox2air_start + felica_lasttime_prox2air_start)<<4) + DELAY_ARM2AIR_AS_READER, + (felica_lasttime_prox2air_start << 4) + DELAY_ARM2AIR_AS_READER, + ((felica_lasttime_prox2air_start + felica_lasttime_prox2air_start) << 4) + DELAY_ARM2AIR_AS_READER, NULL, true ); - felica_nexttransfertime = MAX(felica_nexttransfertime ,felica_lasttime_prox2air_start + FELICA_REQUEST_GUARD_TIME); + felica_nexttransfertime = MAX(felica_nexttransfertime, felica_lasttime_prox2air_start + FELICA_REQUEST_GUARD_TIME); } // Wait for tag reply // stop when button is pressed // or return TRUE when command is captured -bool WaitForFelicaReply(uint16_t maxbytes) { +bool WaitForFelicaReply(uint16_t maxbytes) +{ uint32_t c = 0; @@ -393,7 +403,7 @@ bool WaitForFelicaReply(uint16_t maxbytes) { uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR; uint32_t timeout = iso18092_get_timeout(); - for(;;) { + for (;;) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { @@ -403,15 +413,15 @@ bool WaitForFelicaReply(uint16_t maxbytes) { felica_nexttransfertime = MAX( felica_nexttransfertime, - (GetCountSspClk() & 0xfffffff8) - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/16 + FELICA_FRAME_DELAY_TIME + (GetCountSspClk() & 0xfffffff8) - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER) / 16 + FELICA_FRAME_DELAY_TIME ) - ; + ; LogTrace( FelicaFrame.framebytes, FelicaFrame.len, - ((GetCountSspClk() & 0xfffffff8)<<4) - DELAY_AIR2ARM_AS_READER - timeout, - ((GetCountSspClk() & 0xfffffff8)<<4) - DELAY_AIR2ARM_AS_READER, + ((GetCountSspClk() & 0xfffffff8) << 4) - DELAY_AIR2ARM_AS_READER - timeout, + ((GetCountSspClk() & 0xfffffff8) << 4) - DELAY_AIR2ARM_AS_READER, NULL, false ); @@ -430,13 +440,15 @@ bool WaitForFelicaReply(uint16_t maxbytes) { // Set up FeliCa communication (similar to iso14443a_setup) // field is setup for "Sending as Reader" -static void iso18092_setup(uint8_t fpga_minor_mode) { +static void iso18092_setup(uint8_t fpga_minor_mode) +{ LEDsoff(); FpgaDownloadAndGo(FPGA_BITSTREAM_HF); // allocate command receive buffer - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); // Initialize Demod and Uart structs //DemodInit(BigBuf_malloc(MAX_FRAME_SIZE)); @@ -473,7 +485,8 @@ static void iso18092_setup(uint8_t fpga_minor_mode) { // arg0 FeliCa flags // arg1 len of commandbytes // d.asBytes command bytes to send -void felica_sendraw(UsbCommand *c) { +void felica_sendraw(UsbCommand *c) +{ if (MF_DBGLEVEL > 3) Dbprintf("FeliCa_sendraw Enter"); @@ -494,10 +507,10 @@ void felica_sendraw(UsbCommand *c) { // notify client selecting status. // if failed selecting, turn off antenna and quite. - if( !(param & FELICA_NO_SELECT) ) { + if (!(param & FELICA_NO_SELECT)) { arg0 = felica_select_card(&card); cmd_send(CMD_ACK, arg0, sizeof(card.uid), 0, &card, sizeof(felica_card_select_t)); - if ( arg0 > 0 ) + if (arg0 > 0) goto OUT; } } @@ -505,26 +518,26 @@ void felica_sendraw(UsbCommand *c) { if ((param & FELICA_RAW)) { // 2 sync, 1 len, 2crc == 5 - uint8_t *buf = BigBuf_malloc(len+5); + uint8_t *buf = BigBuf_malloc(len + 5); // add sync bits buf[0] = 0xb2; buf[1] = 0x4d; buf[2] = len; // copy command - memcpy(buf+2, cmd, len); + memcpy(buf + 2, cmd, len); if ((param & FELICA_APPEND_CRC)) { // Don't append crc on empty bytearray... - if ( len > 0 ) { + if (len > 0) { AddCrc(buf, len); len += 2; } } - TransmitFor18092_AsReader(buf, buf[2]+4, NULL, 1, 0); + TransmitFor18092_AsReader(buf, buf[2] + 4, NULL, 1, 0); arg0 = !WaitForFelicaReply(1024); - cmd_send(CMD_ACK, arg0, 0, 0, FelicaFrame.framebytes+2, FelicaFrame.len-2); + cmd_send(CMD_ACK, arg0, 0, 0, FelicaFrame.framebytes + 2, FelicaFrame.len - 2); } if ((param & FELICA_NO_DISCONNECT)) @@ -539,25 +552,26 @@ OUT: if (MF_DBGLEVEL > 3) Dbprintf("FeliCa_sendraw Exit"); } -void felica_sniff(uint32_t samplesToSkip, uint32_t triggersToSkip) { +void felica_sniff(uint32_t samplesToSkip, uint32_t triggersToSkip) +{ int remFrames = (samplesToSkip) ? samplesToSkip : 0; Dbprintf("Snoop FelicaLiteS: Getting first %d frames, Skipping %d triggers.\n", samplesToSkip, triggersToSkip); - iso18092_setup( FPGA_HF_ISO18092_FLAG_NOMOD); + iso18092_setup(FPGA_HF_ISO18092_FLAG_NOMOD); //the frame bits are slow enough. int n = BigBuf_max_traceLen() / sizeof(uint8_t); // take all memory int numbts = 0; uint8_t *dest = (uint8_t *)BigBuf_get_addr(); - uint8_t *destend = dest + n-2; + uint8_t *destend = dest + n - 2; uint32_t endframe = GetCountSspClk(); while (dest <= destend) { WDT_HIT(); - if( BUTTON_PRESS()) break; + if (BUTTON_PRESS()) break; if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { uint8_t dist = (uint8_t)(AT91C_BASE_SSC->SSC_RHR); @@ -566,7 +580,7 @@ void felica_sniff(uint32_t samplesToSkip, uint32_t triggersToSkip) { //to be sure we are in frame if (FelicaFrame.state == STATE_GET_LENGTH) { //length is after 48 (PRE)+16 (SYNC) - 64 ticks +maybe offset? not 100% - uint16_t distance = GetCountSspClk() - endframe - 64 + (FelicaFrame.byte_offset > 0 ? (8-FelicaFrame.byte_offset) : 0); + uint16_t distance = GetCountSspClk() - endframe - 64 + (FelicaFrame.byte_offset > 0 ? (8 - FelicaFrame.byte_offset) : 0); *dest = distance >> 8; dest++; *dest = (distance & 0xff); @@ -577,16 +591,16 @@ void felica_sniff(uint32_t samplesToSkip, uint32_t triggersToSkip) { endframe = GetCountSspClk(); //*dest = FelicaFrame.crc_ok; //kind of wasteful dest++; - for(int i=0; i < FelicaFrame.len; i++) { + for (int i = 0; i < FelicaFrame.len; i++) { *dest = FelicaFrame.framebytes[i]; dest++; - if (dest >= destend ) break; + if (dest >= destend) break; } remFrames--; if (remFrames <= 0) break; - if (dest >= destend ) break; + if (dest >= destend) break; numbts += FelicaFrame.len; @@ -602,7 +616,7 @@ void felica_sniff(uint32_t samplesToSkip, uint32_t triggersToSkip) { set_tracelen(numbts); Dbprintf("Felica sniffing done, tracelen: %i, use hf list felica for annotations", BigBuf_get_traceLen()); - cmd_send(CMD_ACK,1, numbts,0,0,0); + cmd_send(CMD_ACK, 1, numbts, 0, 0, 0); } #define R_POLL0_LEN 0x16 @@ -610,7 +624,8 @@ void felica_sniff(uint32_t samplesToSkip, uint32_t triggersToSkip) { #define R_READBLK_LEN 0x21 //simulate NFC Tag3 card - for now only poll response works // second half (4 bytes) of NDEF2 goes into nfcid2_0, first into nfcid2_1 -void felica_sim_lite(uint64_t nfcid) { +void felica_sim_lite(uint64_t nfcid) +{ int i, curlen = 0; uint8_t *curresp = 0; @@ -619,21 +634,21 @@ void felica_sim_lite(uint64_t nfcid) { num_to_bytes(nfcid, 8, ndef); //prepare our 3 responses... - uint8_t resp_poll0[R_POLL0_LEN] = { 0xb2,0x4d,0x12,FELICA_POLL_ACK,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xf1,0x00,0x00,0x00,0x01,0x43,0x00,0xb3,0x7f}; - uint8_t resp_poll1[R_POLL1_LEN] = { 0xb2,0x4d,0x14,FELICA_POLL_ACK,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xf1,0x00,0x00,0x00,0x01,0x43,0x00, 0x88,0xb4,0xb3,0x7f}; - uint8_t resp_readblk[R_READBLK_LEN] = { 0xb2,0x4d,0x1d,FELICA_RDBLK_ACK,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x10,0x04,0x01,0x00,0x0d,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x23,0xcb,0x6e}; + uint8_t resp_poll0[R_POLL0_LEN] = { 0xb2, 0x4d, 0x12, FELICA_POLL_ACK, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf1, 0x00, 0x00, 0x00, 0x01, 0x43, 0x00, 0xb3, 0x7f}; + uint8_t resp_poll1[R_POLL1_LEN] = { 0xb2, 0x4d, 0x14, FELICA_POLL_ACK, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf1, 0x00, 0x00, 0x00, 0x01, 0x43, 0x00, 0x88, 0xb4, 0xb3, 0x7f}; + uint8_t resp_readblk[R_READBLK_LEN] = { 0xb2, 0x4d, 0x1d, FELICA_RDBLK_ACK, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x10, 0x04, 0x01, 0x00, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x23, 0xcb, 0x6e}; //NFC tag 3/ ISo technically. Many overlapping standards DbpString("Felica Lite-S sim start"); Dbprintf("NDEF2 UID: %02x %02x %02x %02x %02x %02x %02x %02x", - ndef[0], ndef[1], ndef[2], ndef[3], ndef[4], ndef[5], ndef[6], ndef[7] - ); + ndef[0], ndef[1], ndef[2], ndef[3], ndef[4], ndef[5], ndef[6], ndef[7] + ); //fill in blanks - for( i=0; i<8; i++) { - resp_poll0[i+4] = ndef[i]; - resp_poll1[i+4] = ndef[i]; - resp_readblk[i+4] = ndef[i]; + for (i = 0; i < 8; i++) { + resp_poll0[i + 4] = ndef[i]; + resp_poll1[i + 4] = ndef[i]; + resp_readblk[i + 4] = ndef[i]; } //calculate and set CRC @@ -641,12 +656,12 @@ void felica_sim_lite(uint64_t nfcid) { AddCrc(resp_poll1, resp_poll1[2]); AddCrc(resp_readblk, resp_readblk[2]); - iso18092_setup( FPGA_HF_ISO18092_FLAG_NOMOD); + iso18092_setup(FPGA_HF_ISO18092_FLAG_NOMOD); bool listenmode = true; //uint32_t frtm = GetCountSspClk(); - for(;;) { - if( BUTTON_PRESS()) break; + for (;;) { + if (BUTTON_PRESS()) break; WDT_HIT(); if (listenmode) { @@ -716,11 +731,12 @@ void felica_sim_lite(uint64_t nfcid) { DbpString("Felica Lite-S sim end"); } -void felica_dump_lite_s() { +void felica_dump_lite_s() +{ uint8_t ndef[8]; - uint8_t poll[10] = { 0xb2,0x4d,0x06,FELICA_POLL_REQ,0xff,0xff,0x00,0x00,0x09,0x21}; - uint16_t liteblks[28] = {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x80,0x81,0x82,0x83,0x84,0x85,0x86,0x87,0x88,0x90,0x91,0x92,0xa0}; + uint8_t poll[10] = { 0xb2, 0x4d, 0x06, FELICA_POLL_REQ, 0xff, 0xff, 0x00, 0x00, 0x09, 0x21}; + uint16_t liteblks[28] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x90, 0x91, 0x92, 0xa0}; // setup device. iso18092_setup(FPGA_HF_ISO18092_FLAG_READER | FPGA_HF_ISO18092_FLAG_NOMOD); @@ -743,15 +759,15 @@ void felica_dump_lite_s() { // copy 8bytes to ndef. memcpy(ndef, FelicaFrame.framebytes + 4, 8); // for (c=0; c < 8; c++) - // ndef[c] = FelicaFrame.framebytes[c+4]; + // ndef[c] = FelicaFrame.framebytes[c+4]; - for (blknum=0; blknum < sizeof(liteblks); ) { + for (blknum = 0; blknum < sizeof(liteblks);) { // block to read. BuildFliteRdblk(ndef, 1, &liteblks[blknum]); //TransmitFor18092_AsReader(frameSpace, frameSpace[2]+4, GetCountSspClk()+512, 1, 0); - TransmitFor18092_AsReader(frameSpace, frameSpace[2]+4, NULL, 1, 0); + TransmitFor18092_AsReader(frameSpace, frameSpace[2] + 4, NULL, 1, 0); // read block if (WaitForFelicaReply(1024) && FelicaFrame.framebytes[3] == FELICA_RDBLK_ACK) { @@ -764,15 +780,15 @@ void felica_dump_lite_s() { //memcpy(dest+cnt, FelicaFrame.framebytes + 15, 16); //cnt += 16; - for(uint8_t j=0; j < 16; j++) - dest[cnt++] = fb[15+j]; + for (uint8_t j = 0; j < 16; j++) + dest[cnt++] = fb[15 + j]; blknum++; cntfails = 0; // // print raw log. // Dbprintf("LEN %u | Dump bytes count %u ", FelicaFrame.len, cnt); - Dbhexdump(FelicaFrame.len, FelicaFrame.framebytes+15, 0); + Dbhexdump(FelicaFrame.len, FelicaFrame.framebytes + 15, 0); } else { cntfails++; if (cntfails > 12) { diff --git a/armsrc/flashmem.c b/armsrc/flashmem.c index 1c9304c43..6773af444 100644 --- a/armsrc/flashmem.c +++ b/armsrc/flashmem.c @@ -13,13 +13,15 @@ uint32_t FLASHMEM_SPIBAUDRATE = FLASH_BAUD; -void FlashmemSetSpiBaudrate(uint32_t baudrate){ - FLASHMEM_SPIBAUDRATE = baudrate; - Dbprintf("Spi Baudrate : %dMhz", FLASHMEM_SPIBAUDRATE/1000000); +void FlashmemSetSpiBaudrate(uint32_t baudrate) +{ + FLASHMEM_SPIBAUDRATE = baudrate; + Dbprintf("Spi Baudrate : %dMhz", FLASHMEM_SPIBAUDRATE / 1000000); } // initialize -bool FlashInit() { +bool FlashInit() +{ FlashSetup(FLASHMEM_SPIBAUDRATE); StartTicks(); @@ -32,8 +34,9 @@ bool FlashInit() { return true; } -void FlashSetup(uint32_t baudrate){ - //WDT_DISABLE +void FlashSetup(uint32_t baudrate) +{ + //WDT_DISABLE AT91C_BASE_WDTC->WDTC_WDMR = AT91C_WDTC_WDDIS; // PA10 -> SPI_NCS2 chip select (FLASHMEM) @@ -68,13 +71,13 @@ void FlashSetup(uint32_t baudrate){ // NPCS2 Mode 0 AT91C_BASE_SPI->SPI_MR = (0 << 24) | // Delay between chip selects = DYLBCS/MCK BUT: - // If DLYBCS is less than or equal to six, six MCK periods - // will be inserted by default. + // If DLYBCS is less than or equal to six, six MCK periods + // will be inserted by default. SPI_PCS(SPI_CSR_NUM) | // Peripheral Chip Select (selects SPI_NCS2 or PA10) - ( 0 << 7) | // Disable LLB (1=MOSI2MISO test mode) - ( 1 << 4) | // Disable ModeFault Protection - ( 0 << 3) | // makes spi operate at MCK (1 is MCK/2) - ( 0 << 2) | // Chip selects connected directly to peripheral + (0 << 7) | // Disable LLB (1=MOSI2MISO test mode) + (1 << 4) | // Disable ModeFault Protection + (0 << 3) | // makes spi operate at MCK (1 is MCK/2) + (0 << 2) | // Chip selects connected directly to peripheral AT91C_SPI_PS_FIXED | // Fixed Peripheral Select AT91C_SPI_MSTR; // Master Mode @@ -87,17 +90,17 @@ void FlashSetup(uint32_t baudrate){ } AT91C_BASE_SPI->SPI_CSR[2] = - SPI_DLYBCT(dlybct,MCK)| // Delay between Consecutive Transfers (32 MCK periods) - SPI_DLYBS(0,MCK) | // Delay Beforce SPCK CLock - SPI_SCBR(baudrate,MCK)| // SPI Baudrate Selection + SPI_DLYBCT(dlybct, MCK) | // Delay between Consecutive Transfers (32 MCK periods) + SPI_DLYBS(0, MCK) | // Delay Beforce SPCK CLock + SPI_SCBR(baudrate, MCK) | // SPI Baudrate Selection AT91C_SPI_BITS_8 | // Bits per Transfer (8 bits) //AT91C_SPI_CSAAT | // Chip Select inactive after transfer - // 40.4.6.2 SPI: Bad tx_ready Behavior when CSAAT = 1 and SCBR = 1 - // If the SPI is programmed with CSAAT = 1, SCBR(baudrate) = 1 and two transfers are performed consecutively on - // the same slave with an IDLE state between them, the tx_ready signal does not rise after the second data has been - // transferred in the shifter. This can imply for example, that the second data is sent twice. - // COLIN :: For now we STILL use CSAAT=1 to avoid having to (de)assert NPCS manually via PIO lines and we deal with delay - ( csaat << 3) | + // 40.4.6.2 SPI: Bad tx_ready Behavior when CSAAT = 1 and SCBR = 1 + // If the SPI is programmed with CSAAT = 1, SCBR(baudrate) = 1 and two transfers are performed consecutively on + // the same slave with an IDLE state between them, the tx_ready signal does not rise after the second data has been + // transferred in the shifter. This can imply for example, that the second data is sent twice. + // COLIN :: For now we STILL use CSAAT=1 to avoid having to (de)assert NPCS manually via PIO lines and we deal with delay + (csaat << 3) | /* Spi modes: Mode CPOL CPHA NCPHA 0 0 0 1 clock normally low read on rising edge @@ -120,14 +123,15 @@ void FlashSetup(uint32_t baudrate){ 2 1 0 0 clock normally high read on falling edge 3 1 1 1 clock normally high read on rising edge */ - ( 0 << 1) | // Clock Phase data captured on leading edge, changes on following edge - ( 0 << 0); // Clock Polarity inactive state is logic 0 + (0 << 1) | // Clock Phase data captured on leading edge, changes on following edge + (0 << 0); // Clock Polarity inactive state is logic 0 // read first, empty buffer if (AT91C_BASE_SPI->SPI_RDR == 0) {}; } -void FlashStop(void) { +void FlashStop(void) +{ //Bof //* Reset all the Chip Select register AT91C_BASE_SPI->SPI_CSR[0] = 0; @@ -144,13 +148,14 @@ void FlashStop(void) { // SPI disable AT91C_BASE_SPI->SPI_CR = AT91C_SPI_SPIDIS; - if ( MF_DBGLEVEL > 3 ) Dbprintf("FlashStop"); + if (MF_DBGLEVEL > 3) Dbprintf("FlashStop"); StopTicks(); } // send one byte over SPI -uint16_t FlashSendByte(uint32_t data) { +uint16_t FlashSendByte(uint32_t data) +{ // wait until SPI is ready for transfer //if you are checking for incoming data returned then the TXEMPTY flag is redundant @@ -162,19 +167,21 @@ uint16_t FlashSendByte(uint32_t data) { //while ((AT91C_BASE_SPI->SPI_SR & AT91C_SPI_TDRE) == 0){}; // wait recive transfer is complete - while ((AT91C_BASE_SPI->SPI_SR & AT91C_SPI_RDRF) == 0){}; + while ((AT91C_BASE_SPI->SPI_SR & AT91C_SPI_RDRF) == 0) {}; // reading incoming data return ((AT91C_BASE_SPI->SPI_RDR) & 0xFFFF); } // send last byte over SPI -uint16_t FlashSendLastByte(uint32_t data) { +uint16_t FlashSendLastByte(uint32_t data) +{ return FlashSendByte(data | AT91C_SPI_LASTXFER); } // read state register 1 -uint8_t Flash_ReadStat1(void) { +uint8_t Flash_ReadStat1(void) +{ FlashSendByte(READSTAT1); return FlashSendLastByte(0xFF); } @@ -185,18 +192,15 @@ bool Flash_CheckBusy(uint32_t timeout) StartCountUS(); uint32_t _time = GetCountUS(); - if ( MF_DBGLEVEL > 3 ) Dbprintf("Checkbusy in..."); + if (MF_DBGLEVEL > 3) Dbprintf("Checkbusy in..."); - do - { - if (!(Flash_ReadStat1() & BUSY)) - { + do { + if (!(Flash_ReadStat1() & BUSY)) { return false; } } while ((GetCountUS() - _time) < timeout); - if (timeout <= (GetCountUS() - _time)) - { + if (timeout <= (GetCountUS() - _time)) { return true; } @@ -204,7 +208,8 @@ bool Flash_CheckBusy(uint32_t timeout) } // read ID out -uint8_t Flash_ReadID(void) { +uint8_t Flash_ReadID(void) +{ if (Flash_CheckBusy(BUSY_TIMEOUT)) return 0; @@ -217,16 +222,17 @@ uint8_t Flash_ReadID(void) { uint8_t man_id = FlashSendByte(0xFF); uint8_t dev_id = FlashSendLastByte(0xFF); - if ( MF_DBGLEVEL > 3 ) Dbprintf("Flash ReadID | Man ID %02x | Device ID %02x", man_id, dev_id); + if (MF_DBGLEVEL > 3) Dbprintf("Flash ReadID | Man ID %02x | Device ID %02x", man_id, dev_id); - if ( (man_id == WINBOND_MANID ) && (dev_id == WINBOND_DEVID) ) + if ((man_id == WINBOND_MANID) && (dev_id == WINBOND_DEVID)) return dev_id; return 0; } // read unique id for chip. -void Flash_UniqueID(uint8_t *uid) { +void Flash_UniqueID(uint8_t *uid) +{ if (Flash_CheckBusy(BUSY_TIMEOUT)) return; @@ -247,7 +253,8 @@ void Flash_UniqueID(uint8_t *uid) { uid[0] = FlashSendLastByte(0xFF); } -uint16_t Flash_ReadData(uint32_t address, uint8_t *out, uint16_t len) { +uint16_t Flash_ReadData(uint32_t address, uint8_t *out, uint16_t len) +{ if (!FlashInit()) return 0; @@ -259,7 +266,7 @@ uint16_t Flash_ReadData(uint32_t address, uint8_t *out, uint16_t len) { FlashSendByte(cmd); Flash_TransferAdresse(address); - if (FASTFLASH){ + if (FASTFLASH) { FlashSendByte(DUMMYBYTE); } @@ -272,14 +279,16 @@ uint16_t Flash_ReadData(uint32_t address, uint8_t *out, uint16_t len) { return len; } -void Flash_TransferAdresse(uint32_t address){ +void Flash_TransferAdresse(uint32_t address) +{ FlashSendByte((address >> 16) & 0xFF); FlashSendByte((address >> 8) & 0xFF); FlashSendByte((address >> 0) & 0xFF); } /* This ensure we can ReadData without having to cycle through initialization everytime */ -uint16_t Flash_ReadDataCont(uint32_t address, uint8_t *out, uint16_t len) { +uint16_t Flash_ReadDataCont(uint32_t address, uint8_t *out, uint16_t len) +{ // length should never be zero if (!len) return 0; @@ -289,7 +298,7 @@ uint16_t Flash_ReadDataCont(uint32_t address, uint8_t *out, uint16_t len) { FlashSendByte(cmd); Flash_TransferAdresse(address); - if (FASTFLASH){ + if (FASTFLASH) { FlashSendByte(DUMMYBYTE); } @@ -305,7 +314,8 @@ uint16_t Flash_ReadDataCont(uint32_t address, uint8_t *out, uint16_t len) { //////////////////////////////////////// // Write data can only program one page. A page has 256 bytes. // if len > 256, it might wrap around and overwrite pos 0. -uint16_t Flash_WriteData(uint32_t address, uint8_t *in, uint16_t len) { +uint16_t Flash_WriteData(uint32_t address, uint8_t *in, uint16_t len) +{ // length should never be zero if (!len) @@ -313,18 +323,18 @@ uint16_t Flash_WriteData(uint32_t address, uint8_t *in, uint16_t len) { // Max 256 bytes write if (((address & 0xFF) + len) > 256) { - Dbprintf("Flash_WriteData 256 fail [ 0x%02x ] [ %u ]", (address & 0xFF)+len, len ); + Dbprintf("Flash_WriteData 256 fail [ 0x%02x ] [ %u ]", (address & 0xFF) + len, len); return 0; } // out-of-range - if ( (( address >> 16 ) & 0xFF ) > MAX_BLOCKS) { + if (((address >> 16) & 0xFF) > MAX_BLOCKS) { Dbprintf("Flash_WriteData, block out-of-range"); return 0; } if (!FlashInit()) { - if ( MF_DBGLEVEL > 3 ) Dbprintf("Flash_WriteData init fail"); + if (MF_DBGLEVEL > 3) Dbprintf("Flash_WriteData init fail"); return 0; } @@ -351,17 +361,18 @@ uint16_t Flash_WriteData(uint32_t address, uint8_t *in, uint16_t len) { // length should never be zero // Max 256 bytes write // out-of-range -uint16_t Flash_WriteDataCont(uint32_t address, uint8_t *in, uint16_t len) { +uint16_t Flash_WriteDataCont(uint32_t address, uint8_t *in, uint16_t len) +{ if (!len) return 0; if (((address & 0xFF) + len) > 256) { - Dbprintf("Flash_WriteDataCont 256 fail [ 0x%02x ] [ %u ]", (address & 0xFF)+len, len ); + Dbprintf("Flash_WriteDataCont 256 fail [ 0x%02x ] [ %u ]", (address & 0xFF) + len, len); return 0; } - if ( (( address >> 16 ) & 0xFF ) > MAX_BLOCKS) { + if (((address >> 16) & 0xFF) > MAX_BLOCKS) { Dbprintf("Flash_WriteDataCont, block out-of-range"); return 0; } @@ -381,7 +392,8 @@ uint16_t Flash_WriteDataCont(uint32_t address, uint8_t *in, uint16_t len) { // assumes valid start 256 based 00 address // -uint16_t Flash_Write(uint32_t address, uint8_t *in, uint16_t len) { +uint16_t Flash_Write(uint32_t address, uint8_t *in, uint16_t len) +{ bool isok; uint16_t res, bytes_sent = 0, bytes_remaining = len; @@ -412,47 +424,61 @@ out: } -bool Flash_WipeMemoryPage(uint8_t page) { +bool Flash_WipeMemoryPage(uint8_t page) +{ if (!FlashInit()) { - if ( MF_DBGLEVEL > 3 ) Dbprintf("Flash_WriteData init fail"); + if (MF_DBGLEVEL > 3) Dbprintf("Flash_WriteData init fail"); return false; } Flash_ReadStat1(); // Each block is 64Kb. One block erase takes 1s ( 1000ms ) - Flash_WriteEnable(); Flash_Erase64k(page); Flash_CheckBusy(BUSY_TIMEOUT); + Flash_WriteEnable(); + Flash_Erase64k(page); + Flash_CheckBusy(BUSY_TIMEOUT); FlashStop(); return true; } // Wipes flash memory completely, fills with 0xFF -bool Flash_WipeMemory() { +bool Flash_WipeMemory() +{ if (!FlashInit()) { - if ( MF_DBGLEVEL > 3 ) Dbprintf("Flash_WriteData init fail"); + if (MF_DBGLEVEL > 3) Dbprintf("Flash_WriteData init fail"); return false; } Flash_ReadStat1(); // Each block is 64Kb. Four blocks // one block erase takes 1s ( 1000ms ) - Flash_WriteEnable(); Flash_Erase64k(0); Flash_CheckBusy(BUSY_TIMEOUT); - Flash_WriteEnable(); Flash_Erase64k(1); Flash_CheckBusy(BUSY_TIMEOUT); - Flash_WriteEnable(); Flash_Erase64k(2); Flash_CheckBusy(BUSY_TIMEOUT); - Flash_WriteEnable(); Flash_Erase64k(3); Flash_CheckBusy(BUSY_TIMEOUT); + Flash_WriteEnable(); + Flash_Erase64k(0); + Flash_CheckBusy(BUSY_TIMEOUT); + Flash_WriteEnable(); + Flash_Erase64k(1); + Flash_CheckBusy(BUSY_TIMEOUT); + Flash_WriteEnable(); + Flash_Erase64k(2); + Flash_CheckBusy(BUSY_TIMEOUT); + Flash_WriteEnable(); + Flash_Erase64k(3); + Flash_CheckBusy(BUSY_TIMEOUT); FlashStop(); return true; } // enable the flash write -void Flash_WriteEnable() { +void Flash_WriteEnable() +{ FlashSendLastByte(WRITEENABLE); - if ( MF_DBGLEVEL > 3 ) Dbprintf("Flash Write enabled"); + if (MF_DBGLEVEL > 3) Dbprintf("Flash Write enabled"); } // erase 4K at one time // execution time: 0.8ms / 800us -bool Flash_Erase4k(uint8_t block, uint8_t sector) { +bool Flash_Erase4k(uint8_t block, uint8_t sector) +{ if (block > MAX_BLOCKS || sector > MAX_SECTORS) return false; @@ -487,7 +513,8 @@ bool Flash_Erase32k(uint32_t address) { // 0x01 00 00 -- 0x 01 FF FF == block 1 // 0x02 00 00 -- 0x 02 FF FF == block 2 // 0x03 00 00 -- 0x 03 FF FF == block 3 -bool Flash_Erase64k(uint8_t block) { +bool Flash_Erase64k(uint8_t block) +{ if (block > MAX_BLOCKS) return false; @@ -499,13 +526,15 @@ bool Flash_Erase64k(uint8_t block) { } // Erase chip -void Flash_EraseChip(void) { +void Flash_EraseChip(void) +{ FlashSendLastByte(CHIPERASE); } -void Flashmem_print_status(void) { +void Flashmem_print_status(void) +{ DbpString("Flash memory"); - Dbprintf(" Baudrate................%dMHz",FLASHMEM_SPIBAUDRATE/1000000); + Dbprintf(" Baudrate................%dMHz", FLASHMEM_SPIBAUDRATE / 1000000); if (!FlashInit()) { DbpString(" Init....................FAIL"); @@ -529,12 +558,12 @@ void Flashmem_print_status(void) { break; } - uint8_t uid[8] = {0,0,0,0,0,0,0,0}; + uint8_t uid[8] = {0, 0, 0, 0, 0, 0, 0, 0}; Flash_UniqueID(uid); Dbprintf(" Unique ID...............0x%02x%02x%02x%02x%02x%02x%02x%02x", - uid[7], uid[6], uid[5], uid[4], - uid[3], uid[2], uid[1], uid[0] - ); + uid[7], uid[6], uid[5], uid[4], + uid[3], uid[2], uid[1], uid[0] + ); FlashStop(); } diff --git a/armsrc/fonts.c b/armsrc/fonts.c index 1e4b418c9..78b96e238 100644 --- a/armsrc/fonts.c +++ b/armsrc/fonts.c @@ -7,103 +7,103 @@ //----------------------------------------------------------------------------- const char FONT6x8[97][8] = { - {0x06,0x08,0x08,0x00,0x00,0x00,0x00,0x00}, // columns, rows, bytes per char - {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // space - {0x20,0x20,0x20,0x20,0x20,0x00,0x20,0x00}, // ! - {0x50,0x50,0x50,0x00,0x00,0x00,0x00,0x00}, // " - {0x50,0x50,0xF8,0x50,0xF8,0x50,0x50,0x00}, // # - {0x20,0x78,0xA0,0x70,0x28,0xF0,0x20,0x00}, // $ - {0xC0,0xC8,0x10,0x20,0x40,0x98,0x18,0x00}, // % - {0x40,0xA0,0xA0,0x40,0xA8,0x90,0x68,0x00}, // & - {0x30,0x30,0x20,0x40,0x00,0x00,0x00,0x00}, // ' - {0x10,0x20,0x40,0x40,0x40,0x20,0x10,0x00}, // ( - {0x40,0x20,0x10,0x10,0x10,0x20,0x40,0x00}, // ) - {0x00,0x20,0xA8,0x70,0x70,0xA8,0x20,0x00}, // * - {0x00,0x20,0x20,0xF8,0x20,0x20,0x00,0x00}, // + - {0x00,0x00,0x00,0x00,0x30,0x30,0x20,0x40}, // , - {0x00,0x00,0x00,0xF8,0x00,0x00,0x00,0x00}, // - - {0x00,0x00,0x00,0x00,0x00,0x30,0x30,0x00}, // . - {0x00,0x08,0x10,0x20,0x40,0x80,0x00,0x00}, // / - {0x70,0x88,0x88,0xA8,0x88,0x88,0x70,0x00}, // 0 - {0x20,0x60,0x20,0x20,0x20,0x20,0x70,0x00}, // 1 - {0x70,0x88,0x08,0x70,0x80,0x80,0xF8,0x00}, // 2 - {0xF8,0x08,0x10,0x30,0x08,0x88,0x70,0x00}, // 3 - {0x10,0x30,0x50,0x90,0xF8,0x10,0x10,0x00}, // 4 - {0xF8,0x80,0xF0,0x08,0x08,0x88,0x70,0x00}, // 5 - {0x38,0x40,0x80,0xF0,0x88,0x88,0x70,0x00}, // 6 - {0xF8,0x08,0x08,0x10,0x20,0x40,0x80,0x00}, // 7 - {0x70,0x88,0x88,0x70,0x88,0x88,0x70,0x00}, // 8 - {0x70,0x88,0x88,0x78,0x08,0x10,0xE0,0x00}, // 9 - {0x00,0x00,0x20,0x00,0x20,0x00,0x00,0x00}, // : - {0x00,0x00,0x20,0x00,0x20,0x20,0x40,0x00}, // ; - {0x08,0x10,0x20,0x40,0x20,0x10,0x08,0x00}, // < - {0x00,0x00,0xF8,0x00,0xF8,0x00,0x00,0x00}, // = - {0x40,0x20,0x10,0x08,0x10,0x20,0x40,0x00}, // > - {0x70,0x88,0x08,0x30,0x20,0x00,0x20,0x00}, // ? - {0x70,0x88,0xA8,0xB8,0xB0,0x80,0x78,0x00}, // @ - {0x20,0x50,0x88,0x88,0xF8,0x88,0x88,0x00}, // A - {0xF0,0x88,0x88,0xF0,0x88,0x88,0xF0,0x00}, // B - {0x70,0x88,0x80,0x80,0x80,0x88,0x70,0x00}, // C - {0xF0,0x88,0x88,0x88,0x88,0x88,0xF0,0x00}, // D - {0xF8,0x80,0x80,0xF0,0x80,0x80,0xF8,0x00}, // E - {0xF8,0x80,0x80,0xF0,0x80,0x80,0x80,0x00}, // F - {0x78,0x88,0x80,0x80,0x98,0x88,0x78,0x00}, // G - {0x88,0x88,0x88,0xF8,0x88,0x88,0x88,0x00}, // H - {0x70,0x20,0x20,0x20,0x20,0x20,0x70,0x00}, // I - {0x38,0x10,0x10,0x10,0x10,0x90,0x60,0x00}, // J - {0x88,0x90,0xA0,0xC0,0xA0,0x90,0x88,0x00}, // K - {0x80,0x80,0x80,0x80,0x80,0x80,0xF8,0x00}, // L - {0x88,0xD8,0xA8,0xA8,0xA8,0x88,0x88,0x00}, // M - {0x88,0x88,0xC8,0xA8,0x98,0x88,0x88,0x00}, // N - {0x70,0x88,0x88,0x88,0x88,0x88,0x70,0x00}, // O - {0xF0,0x88,0x88,0xF0,0x80,0x80,0x80,0x00}, // P - {0x70,0x88,0x88,0x88,0xA8,0x90,0x68,0x00}, // Q - {0xF0,0x88,0x88,0xF0,0xA0,0x90,0x88,0x00}, // R - {0x70,0x88,0x80,0x70,0x08,0x88,0x70,0x00}, // S - {0xF8,0xA8,0x20,0x20,0x20,0x20,0x20,0x00}, // T - {0x88,0x88,0x88,0x88,0x88,0x88,0x70,0x00}, // U - {0x88,0x88,0x88,0x88,0x88,0x50,0x20,0x00}, // V - {0x88,0x88,0x88,0xA8,0xA8,0xA8,0x50,0x00}, // W - {0x88,0x88,0x50,0x20,0x50,0x88,0x88,0x00}, // X - {0x88,0x88,0x50,0x20,0x20,0x20,0x20,0x00}, // Y - {0xF8,0x08,0x10,0x70,0x40,0x80,0xF8,0x00}, // Z - {0x78,0x40,0x40,0x40,0x40,0x40,0x78,0x00}, // [ - {0x00,0x80,0x40,0x20,0x10,0x08,0x00,0x00}, // backslash - {0x78,0x08,0x08,0x08,0x08,0x08,0x78,0x00}, // ] - {0x20,0x50,0x88,0x00,0x00,0x00,0x00,0x00}, // ^ - {0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x00}, // _ - {0x60,0x60,0x20,0x10,0x00,0x00,0x00,0x00}, // ` - {0x00,0x00,0x60,0x10,0x70,0x90,0x78,0x00}, // a - {0x80,0x80,0xB0,0xC8,0x88,0xC8,0xB0,0x00}, // b - {0x00,0x00,0x70,0x88,0x80,0x88,0x70,0x00}, // c - {0x08,0x08,0x68,0x98,0x88,0x98,0x68,0x00}, // d - {0x00,0x00,0x70,0x88,0xF8,0x80,0x70,0x00}, // e - {0x10,0x28,0x20,0x70,0x20,0x20,0x20,0x00}, // f - {0x00,0x00,0x70,0x98,0x98,0x68,0x08,0x70}, // g - {0x80,0x80,0xB0,0xC8,0x88,0x88,0x88,0x00}, // h - {0x20,0x00,0x60,0x20,0x20,0x20,0x70,0x00}, // i - {0x10,0x00,0x10,0x10,0x10,0x90,0x60,0x00}, // j - {0x80,0x80,0x90,0xA0,0xC0,0xA0,0x90,0x00}, // k - {0x60,0x20,0x20,0x20,0x20,0x20,0x70,0x00}, // l - {0x00,0x00,0xD0,0xA8,0xA8,0xA8,0xA8,0x00}, // m - {0x00,0x00,0xB0,0xC8,0x88,0x88,0x88,0x00}, // n - {0x00,0x00,0x70,0x88,0x88,0x88,0x70,0x00}, // o - {0x00,0x00,0xB0,0xC8,0xC8,0xB0,0x80,0x80}, // p - {0x00,0x00,0x68,0x98,0x98,0x68,0x08,0x08}, // q - {0x00,0x00,0xB0,0xC8,0x80,0x80,0x80,0x00}, // r - {0x00,0x00,0x78,0x80,0x70,0x08,0xF0,0x00}, // s - {0x20,0x20,0xF8,0x20,0x20,0x28,0x10,0x00}, // t - {0x00,0x00,0x88,0x88,0x88,0x98,0x68,0x00}, // u - {0x00,0x00,0x88,0x88,0x88,0x50,0x20,0x00}, // v - {0x00,0x00,0x88,0x88,0xA8,0xA8,0x50,0x00}, // w - {0x00,0x00,0x88,0x50,0x20,0x50,0x88,0x00}, // x - {0x00,0x00,0x88,0x88,0x78,0x08,0x88,0x70}, // y - {0x00,0x00,0xF8,0x10,0x20,0x40,0xF8,0x00}, // z - {0x10,0x20,0x20,0x40,0x20,0x20,0x10,0x00}, // { - {0x20,0x20,0x20,0x00,0x20,0x20,0x20,0x00}, // | - {0x40,0x20,0x20,0x10,0x20,0x20,0x40,0x00}, // } - {0x40,0xA8,0x10,0x00,0x00,0x00,0x00,0x00}, // ~ - {0x70,0xD8,0xD8,0x70,0x00,0x00,0x00,0x00} // DEL + {0x06, 0x08, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00}, // columns, rows, bytes per char + {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // space + {0x20, 0x20, 0x20, 0x20, 0x20, 0x00, 0x20, 0x00}, // ! + {0x50, 0x50, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00}, // " + {0x50, 0x50, 0xF8, 0x50, 0xF8, 0x50, 0x50, 0x00}, // # + {0x20, 0x78, 0xA0, 0x70, 0x28, 0xF0, 0x20, 0x00}, // $ + {0xC0, 0xC8, 0x10, 0x20, 0x40, 0x98, 0x18, 0x00}, // % + {0x40, 0xA0, 0xA0, 0x40, 0xA8, 0x90, 0x68, 0x00}, // & + {0x30, 0x30, 0x20, 0x40, 0x00, 0x00, 0x00, 0x00}, // ' + {0x10, 0x20, 0x40, 0x40, 0x40, 0x20, 0x10, 0x00}, // ( + {0x40, 0x20, 0x10, 0x10, 0x10, 0x20, 0x40, 0x00}, // ) + {0x00, 0x20, 0xA8, 0x70, 0x70, 0xA8, 0x20, 0x00}, // * + {0x00, 0x20, 0x20, 0xF8, 0x20, 0x20, 0x00, 0x00}, // + + {0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x20, 0x40}, // , + {0x00, 0x00, 0x00, 0xF8, 0x00, 0x00, 0x00, 0x00}, // - + {0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00}, // . + {0x00, 0x08, 0x10, 0x20, 0x40, 0x80, 0x00, 0x00}, // / + {0x70, 0x88, 0x88, 0xA8, 0x88, 0x88, 0x70, 0x00}, // 0 + {0x20, 0x60, 0x20, 0x20, 0x20, 0x20, 0x70, 0x00}, // 1 + {0x70, 0x88, 0x08, 0x70, 0x80, 0x80, 0xF8, 0x00}, // 2 + {0xF8, 0x08, 0x10, 0x30, 0x08, 0x88, 0x70, 0x00}, // 3 + {0x10, 0x30, 0x50, 0x90, 0xF8, 0x10, 0x10, 0x00}, // 4 + {0xF8, 0x80, 0xF0, 0x08, 0x08, 0x88, 0x70, 0x00}, // 5 + {0x38, 0x40, 0x80, 0xF0, 0x88, 0x88, 0x70, 0x00}, // 6 + {0xF8, 0x08, 0x08, 0x10, 0x20, 0x40, 0x80, 0x00}, // 7 + {0x70, 0x88, 0x88, 0x70, 0x88, 0x88, 0x70, 0x00}, // 8 + {0x70, 0x88, 0x88, 0x78, 0x08, 0x10, 0xE0, 0x00}, // 9 + {0x00, 0x00, 0x20, 0x00, 0x20, 0x00, 0x00, 0x00}, // : + {0x00, 0x00, 0x20, 0x00, 0x20, 0x20, 0x40, 0x00}, // ; + {0x08, 0x10, 0x20, 0x40, 0x20, 0x10, 0x08, 0x00}, // < + {0x00, 0x00, 0xF8, 0x00, 0xF8, 0x00, 0x00, 0x00}, // = + {0x40, 0x20, 0x10, 0x08, 0x10, 0x20, 0x40, 0x00}, // > + {0x70, 0x88, 0x08, 0x30, 0x20, 0x00, 0x20, 0x00}, // ? + {0x70, 0x88, 0xA8, 0xB8, 0xB0, 0x80, 0x78, 0x00}, // @ + {0x20, 0x50, 0x88, 0x88, 0xF8, 0x88, 0x88, 0x00}, // A + {0xF0, 0x88, 0x88, 0xF0, 0x88, 0x88, 0xF0, 0x00}, // B + {0x70, 0x88, 0x80, 0x80, 0x80, 0x88, 0x70, 0x00}, // C + {0xF0, 0x88, 0x88, 0x88, 0x88, 0x88, 0xF0, 0x00}, // D + {0xF8, 0x80, 0x80, 0xF0, 0x80, 0x80, 0xF8, 0x00}, // E + {0xF8, 0x80, 0x80, 0xF0, 0x80, 0x80, 0x80, 0x00}, // F + {0x78, 0x88, 0x80, 0x80, 0x98, 0x88, 0x78, 0x00}, // G + {0x88, 0x88, 0x88, 0xF8, 0x88, 0x88, 0x88, 0x00}, // H + {0x70, 0x20, 0x20, 0x20, 0x20, 0x20, 0x70, 0x00}, // I + {0x38, 0x10, 0x10, 0x10, 0x10, 0x90, 0x60, 0x00}, // J + {0x88, 0x90, 0xA0, 0xC0, 0xA0, 0x90, 0x88, 0x00}, // K + {0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0xF8, 0x00}, // L + {0x88, 0xD8, 0xA8, 0xA8, 0xA8, 0x88, 0x88, 0x00}, // M + {0x88, 0x88, 0xC8, 0xA8, 0x98, 0x88, 0x88, 0x00}, // N + {0x70, 0x88, 0x88, 0x88, 0x88, 0x88, 0x70, 0x00}, // O + {0xF0, 0x88, 0x88, 0xF0, 0x80, 0x80, 0x80, 0x00}, // P + {0x70, 0x88, 0x88, 0x88, 0xA8, 0x90, 0x68, 0x00}, // Q + {0xF0, 0x88, 0x88, 0xF0, 0xA0, 0x90, 0x88, 0x00}, // R + {0x70, 0x88, 0x80, 0x70, 0x08, 0x88, 0x70, 0x00}, // S + {0xF8, 0xA8, 0x20, 0x20, 0x20, 0x20, 0x20, 0x00}, // T + {0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x70, 0x00}, // U + {0x88, 0x88, 0x88, 0x88, 0x88, 0x50, 0x20, 0x00}, // V + {0x88, 0x88, 0x88, 0xA8, 0xA8, 0xA8, 0x50, 0x00}, // W + {0x88, 0x88, 0x50, 0x20, 0x50, 0x88, 0x88, 0x00}, // X + {0x88, 0x88, 0x50, 0x20, 0x20, 0x20, 0x20, 0x00}, // Y + {0xF8, 0x08, 0x10, 0x70, 0x40, 0x80, 0xF8, 0x00}, // Z + {0x78, 0x40, 0x40, 0x40, 0x40, 0x40, 0x78, 0x00}, // [ + {0x00, 0x80, 0x40, 0x20, 0x10, 0x08, 0x00, 0x00}, // backslash + {0x78, 0x08, 0x08, 0x08, 0x08, 0x08, 0x78, 0x00}, // ] + {0x20, 0x50, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00}, // ^ + {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF8, 0x00}, // _ + {0x60, 0x60, 0x20, 0x10, 0x00, 0x00, 0x00, 0x00}, // ` + {0x00, 0x00, 0x60, 0x10, 0x70, 0x90, 0x78, 0x00}, // a + {0x80, 0x80, 0xB0, 0xC8, 0x88, 0xC8, 0xB0, 0x00}, // b + {0x00, 0x00, 0x70, 0x88, 0x80, 0x88, 0x70, 0x00}, // c + {0x08, 0x08, 0x68, 0x98, 0x88, 0x98, 0x68, 0x00}, // d + {0x00, 0x00, 0x70, 0x88, 0xF8, 0x80, 0x70, 0x00}, // e + {0x10, 0x28, 0x20, 0x70, 0x20, 0x20, 0x20, 0x00}, // f + {0x00, 0x00, 0x70, 0x98, 0x98, 0x68, 0x08, 0x70}, // g + {0x80, 0x80, 0xB0, 0xC8, 0x88, 0x88, 0x88, 0x00}, // h + {0x20, 0x00, 0x60, 0x20, 0x20, 0x20, 0x70, 0x00}, // i + {0x10, 0x00, 0x10, 0x10, 0x10, 0x90, 0x60, 0x00}, // j + {0x80, 0x80, 0x90, 0xA0, 0xC0, 0xA0, 0x90, 0x00}, // k + {0x60, 0x20, 0x20, 0x20, 0x20, 0x20, 0x70, 0x00}, // l + {0x00, 0x00, 0xD0, 0xA8, 0xA8, 0xA8, 0xA8, 0x00}, // m + {0x00, 0x00, 0xB0, 0xC8, 0x88, 0x88, 0x88, 0x00}, // n + {0x00, 0x00, 0x70, 0x88, 0x88, 0x88, 0x70, 0x00}, // o + {0x00, 0x00, 0xB0, 0xC8, 0xC8, 0xB0, 0x80, 0x80}, // p + {0x00, 0x00, 0x68, 0x98, 0x98, 0x68, 0x08, 0x08}, // q + {0x00, 0x00, 0xB0, 0xC8, 0x80, 0x80, 0x80, 0x00}, // r + {0x00, 0x00, 0x78, 0x80, 0x70, 0x08, 0xF0, 0x00}, // s + {0x20, 0x20, 0xF8, 0x20, 0x20, 0x28, 0x10, 0x00}, // t + {0x00, 0x00, 0x88, 0x88, 0x88, 0x98, 0x68, 0x00}, // u + {0x00, 0x00, 0x88, 0x88, 0x88, 0x50, 0x20, 0x00}, // v + {0x00, 0x00, 0x88, 0x88, 0xA8, 0xA8, 0x50, 0x00}, // w + {0x00, 0x00, 0x88, 0x50, 0x20, 0x50, 0x88, 0x00}, // x + {0x00, 0x00, 0x88, 0x88, 0x78, 0x08, 0x88, 0x70}, // y + {0x00, 0x00, 0xF8, 0x10, 0x20, 0x40, 0xF8, 0x00}, // z + {0x10, 0x20, 0x20, 0x40, 0x20, 0x20, 0x10, 0x00}, // { + {0x20, 0x20, 0x20, 0x00, 0x20, 0x20, 0x20, 0x00}, // | + {0x40, 0x20, 0x20, 0x10, 0x20, 0x20, 0x40, 0x00}, // } + {0x40, 0xA8, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, // ~ + {0x70, 0xD8, 0xD8, 0x70, 0x00, 0x00, 0x00, 0x00} // DEL }; /* const char FONT8x8F[97][8] = { diff --git a/armsrc/fpgaloader.c b/armsrc/fpgaloader.c index 2af6fd0ec..76a33bd9e 100644 --- a/armsrc/fpgaloader.c +++ b/armsrc/fpgaloader.c @@ -28,7 +28,8 @@ static uint32_t uncompressed_bytes_cnt; // Used to write the FPGA config word // May also be used to write to other SPI attached devices like an LCD //----------------------------------------------------------------------------- -static void DisableSpi(void) { +static void DisableSpi(void) +{ //* Reset all the Chip Select register AT91C_BASE_SPI->SPI_CSR[0] = 0; AT91C_BASE_SPI->SPI_CSR[1] = 0; @@ -45,7 +46,8 @@ static void DisableSpi(void) { AT91C_BASE_SPI->SPI_CR = AT91C_SPI_SPIDIS; } -void SetupSpi(int mode) { +void SetupSpi(int mode) +{ // PA1 -> SPI_NCS3 chip select (MEM) // PA10 -> SPI_NCS2 chip select (LCD) // PA11 -> SPI_NCS0 chip select (FPGA) @@ -70,44 +72,44 @@ void SetupSpi(int mode) { switch (mode) { case SPI_FPGA_MODE: AT91C_BASE_SPI->SPI_MR = - ( 0 << 24) | // Delay between chip selects (take default: 6 MCK periods) + (0 << 24) | // Delay between chip selects (take default: 6 MCK periods) (0xE << 16) | // Peripheral Chip Select (selects FPGA SPI_NCS0 or PA11) - ( 0 << 7) | // Local Loopback Disabled + (0 << 7) | // Local Loopback Disabled AT91C_SPI_MODFDIS | // Mode Fault Detection disabled - ( 0 << 2) | // Chip selects connected directly to peripheral + (0 << 2) | // Chip selects connected directly to peripheral AT91C_SPI_PS_FIXED | // Fixed Peripheral Select AT91C_SPI_MSTR; // Master Mode AT91C_BASE_SPI->SPI_CSR[0] = - ( 1 << 24) | // Delay between Consecutive Transfers (32 MCK periods) - ( 1 << 16) | // Delay Before SPCK (1 MCK period) - ( 6 << 8) | // Serial Clock Baud Rate (baudrate = MCK/6 = 24Mhz/6 = 4M baud + (1 << 24) | // Delay between Consecutive Transfers (32 MCK periods) + (1 << 16) | // Delay Before SPCK (1 MCK period) + (6 << 8) | // Serial Clock Baud Rate (baudrate = MCK/6 = 24Mhz/6 = 4M baud AT91C_SPI_BITS_16 | // Bits per Transfer (16 bits) - ( 0 << 3) | // Chip Select inactive after transfer + (0 << 3) | // Chip Select inactive after transfer AT91C_SPI_NCPHA | // Clock Phase data captured on leading edge, changes on following edge - ( 0 << 0); // Clock Polarity inactive state is logic 0 + (0 << 0); // Clock Polarity inactive state is logic 0 break; -/* - case SPI_LCD_MODE: - AT91C_BASE_SPI->SPI_MR = - ( 0 << 24) | // Delay between chip selects (take default: 6 MCK periods) - (0xB << 16) | // Peripheral Chip Select (selects LCD SPI_NCS2 or PA10) - ( 0 << 7) | // Local Loopback Disabled - ( 1 << 4) | // Mode Fault Detection disabled - ( 0 << 2) | // Chip selects connected directly to peripheral - ( 0 << 1) | // Fixed Peripheral Select - ( 1 << 0); // Master Mode + /* + case SPI_LCD_MODE: + AT91C_BASE_SPI->SPI_MR = + ( 0 << 24) | // Delay between chip selects (take default: 6 MCK periods) + (0xB << 16) | // Peripheral Chip Select (selects LCD SPI_NCS2 or PA10) + ( 0 << 7) | // Local Loopback Disabled + ( 1 << 4) | // Mode Fault Detection disabled + ( 0 << 2) | // Chip selects connected directly to peripheral + ( 0 << 1) | // Fixed Peripheral Select + ( 1 << 0); // Master Mode - AT91C_BASE_SPI->SPI_CSR[2] = - ( 1 << 24) | // Delay between Consecutive Transfers (32 MCK periods) - ( 1 << 16) | // Delay Before SPCK (1 MCK period) - ( 6 << 8) | // Serial Clock Baud Rate (baudrate = MCK/6 = 24Mhz/6 = 4M baud - AT91C_SPI_BITS_9 | // Bits per Transfer (9 bits) - ( 0 << 3) | // Chip Select inactive after transfer - ( 1 << 1) | // Clock Phase data captured on leading edge, changes on following edge - ( 0 << 0); // Clock Polarity inactive state is logic 0 - break; -*/ + AT91C_BASE_SPI->SPI_CSR[2] = + ( 1 << 24) | // Delay between Consecutive Transfers (32 MCK periods) + ( 1 << 16) | // Delay Before SPCK (1 MCK period) + ( 6 << 8) | // Serial Clock Baud Rate (baudrate = MCK/6 = 24Mhz/6 = 4M baud + AT91C_SPI_BITS_9 | // Bits per Transfer (9 bits) + ( 0 << 3) | // Chip Select inactive after transfer + ( 1 << 1) | // Clock Phase data captured on leading edge, changes on following edge + ( 0 << 0); // Clock Polarity inactive state is logic 0 + break; + */ default: DisableSpi(); break; @@ -118,7 +120,8 @@ void SetupSpi(int mode) { // Set up the synchronous serial port, with the one set of options that we // always use when we are talking to the FPGA. Both RX and TX are enabled. //----------------------------------------------------------------------------- -void FpgaSetupSsc(void) { +void FpgaSetupSsc(void) +{ // First configure the GPIOs, and get ourselves a clock. AT91C_BASE_PIOA->PIO_ASR = GPIO_SSC_FRAME | @@ -127,7 +130,7 @@ void FpgaSetupSsc(void) { GPIO_SSC_CLK; AT91C_BASE_PIOA->PIO_PDR = GPIO_SSC_DOUT; - AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_SSC); + AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_SSC); // Now set up the SSC proper, starting from a known state. AT91C_BASE_SSC->SSC_CR = AT91C_SSC_SWRST; @@ -156,7 +159,8 @@ void FpgaSetupSsc(void) { // ourselves, not to another buffer). The stuff to manipulate those buffers // is in apps.h, because it should be inlined, for speed. //----------------------------------------------------------------------------- -bool FpgaSetupSscDma(uint8_t *buf, int len) { +bool FpgaSetupSscDma(uint8_t *buf, int len) +{ if (buf == NULL) return false; FpgaDisableSscDma(); @@ -172,7 +176,8 @@ bool FpgaSetupSscDma(uint8_t *buf, int len) { // Uncompress (inflate) the FPGA data. Returns one decompressed byte with // each call. //---------------------------------------------------------------------------- -static int get_from_fpga_combined_stream(z_streamp compressed_fpga_stream, uint8_t *output_buffer) { +static int get_from_fpga_combined_stream(z_streamp compressed_fpga_stream, uint8_t *output_buffer) +{ if (fpga_image_ptr == compressed_fpga_stream->next_out) { // need more data compressed_fpga_stream->next_out = output_buffer; compressed_fpga_stream->avail_out = OUTPUT_BUFFER_LEN; @@ -194,8 +199,9 @@ static int get_from_fpga_combined_stream(z_streamp compressed_fpga_stream, uint8 // are combined into one big file: // 288 bytes from FPGA file 1, followed by 288 bytes from FGPA file 2, etc. //---------------------------------------------------------------------------- -static int get_from_fpga_stream(int bitstream_version, z_streamp compressed_fpga_stream, uint8_t *output_buffer) { - while((uncompressed_bytes_cnt / FPGA_INTERLEAVE_SIZE) % fpga_bitstream_num != (bitstream_version - 1)) { +static int get_from_fpga_stream(int bitstream_version, z_streamp compressed_fpga_stream, uint8_t *output_buffer) +{ + while ((uncompressed_bytes_cnt / FPGA_INTERLEAVE_SIZE) % fpga_bitstream_num != (bitstream_version - 1)) { // skip undesired data belonging to other bitstream_versions get_from_fpga_combined_stream(compressed_fpga_stream, output_buffer); } @@ -203,19 +209,23 @@ static int get_from_fpga_stream(int bitstream_version, z_streamp compressed_fpga return get_from_fpga_combined_stream(compressed_fpga_stream, output_buffer); } -static voidpf fpga_inflate_malloc(voidpf opaque, uInt items, uInt size) { - return BigBuf_malloc(items*size); +static voidpf fpga_inflate_malloc(voidpf opaque, uInt items, uInt size) +{ + return BigBuf_malloc(items * size); } // free eventually allocated BigBuf memory -static void fpga_inflate_free(voidpf opaque, voidpf address) { - BigBuf_free(); BigBuf_Clear_ext(false); +static void fpga_inflate_free(voidpf opaque, voidpf address) +{ + BigBuf_free(); + BigBuf_Clear_ext(false); } //---------------------------------------------------------------------------- // Initialize decompression of the respective (HF or LF) FPGA stream //---------------------------------------------------------------------------- -static bool reset_fpga_stream(int bitstream_version, z_streamp compressed_fpga_stream, uint8_t *output_buffer) { +static bool reset_fpga_stream(int bitstream_version, z_streamp compressed_fpga_stream, uint8_t *output_buffer) +{ uint8_t header[FPGA_BITSTREAM_FIXED_HEADER_SIZE]; uncompressed_bytes_cnt = 0; @@ -242,7 +252,8 @@ static bool reset_fpga_stream(int bitstream_version, z_streamp compressed_fpga_s return false; } -static void DownloadFPGA_byte( uint8_t w) { +static void DownloadFPGA_byte(uint8_t w) +{ #define SEND_BIT(x) { if(w & (1<PIO_OER = GPIO_FPGA_ON; @@ -296,12 +308,12 @@ static void DownloadFPGA(int bitstream_version, int FpgaImageLen, z_streamp comp i = 100000; // wait for FPGA ready to accept data signal - while ((i) && ( !(AT91C_BASE_PIOA->PIO_PDSR & GPIO_FPGA_NINIT ) ) ) { + while ((i) && (!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_FPGA_NINIT))) { i--; } // crude error indicator, leave both red LEDs on and return - if (i==0){ + if (i == 0) { LED_C_ON(); LED_D_ON(); return; @@ -318,12 +330,12 @@ static void DownloadFPGA(int bitstream_version, int FpgaImageLen, z_streamp comp // continue to clock FPGA until ready signal goes high i = 100000; - while ( (i--) && ( !(AT91C_BASE_PIOA->PIO_PDSR & GPIO_FPGA_DONE ) ) ) { + while ((i--) && (!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_FPGA_DONE))) { HIGH(GPIO_FPGA_CCLK); LOW(GPIO_FPGA_CCLK); } // crude error indicator, leave both red LEDs on and return - if (i==0){ + if (i == 0) { LED_C_ON(); LED_D_ON(); return; @@ -337,11 +349,12 @@ static void DownloadFPGA(int bitstream_version, int FpgaImageLen, z_streamp comp * (big endian), bytes content. Except for section 'e' which has 4 bytes * length. */ -static int bitparse_find_section(int bitstream_version, char section_name, uint32_t *section_length, z_streamp compressed_fpga_stream, uint8_t *output_buffer) { +static int bitparse_find_section(int bitstream_version, char section_name, uint32_t *section_length, z_streamp compressed_fpga_stream, uint8_t *output_buffer) +{ int result = 0; - #define MAX_FPGA_BIT_STREAM_HEADER_SEARCH 100 // maximum number of bytes to search for the requested section +#define MAX_FPGA_BIT_STREAM_HEADER_SEARCH 100 // maximum number of bytes to search for the requested section uint16_t numbytes = 0; - while(numbytes < MAX_FPGA_BIT_STREAM_HEADER_SEARCH) { + while (numbytes < MAX_FPGA_BIT_STREAM_HEADER_SEARCH) { char current_name = get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer); numbytes++; uint32_t current_length = 0; @@ -351,15 +364,15 @@ static int bitparse_find_section(int bitstream_version, char section_name, uint3 } current_length = 0; switch (current_name) { - case 'e': - /* Four byte length field */ - current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 24; - current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 16; - numbytes += 2; - default: /* Fall through, two byte length field */ - current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 8; - current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 0; - numbytes += 2; + case 'e': + /* Four byte length field */ + current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 24; + current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 16; + numbytes += 2; + default: /* Fall through, two byte length field */ + current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 8; + current_length += get_from_fpga_stream(bitstream_version, compressed_fpga_stream, output_buffer) << 0; + numbytes += 2; } if (current_name != 'e' && current_length > 255) { @@ -386,7 +399,8 @@ static int bitparse_find_section(int bitstream_version, char section_name, uint3 // Check which FPGA image is currently loaded (if any). If necessary // decompress and load the correct (HF or LF) image to the FPGA //---------------------------------------------------------------------------- -void FpgaDownloadAndGo(int bitstream_version) { +void FpgaDownloadAndGo(int bitstream_version) +{ // check whether or not the bitstream is already loaded if (downloaded_bitstream == bitstream_version) @@ -398,7 +412,8 @@ void FpgaDownloadAndGo(int bitstream_version) { bool verbose = (MF_DBGLEVEL > 3); // make sure that we have enough memory to decompress - BigBuf_free(); BigBuf_Clear_ext(verbose); + BigBuf_free(); + BigBuf_Clear_ext(verbose); if (!reset_fpga_stream(bitstream_version, &compressed_fpga_stream, output_buffer)) return; @@ -415,7 +430,8 @@ void FpgaDownloadAndGo(int bitstream_version) { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // free eventually allocated BigBuf memory - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); } //----------------------------------------------------------------------------- @@ -423,7 +439,8 @@ void FpgaDownloadAndGo(int bitstream_version) { // The bit format is: C3 C2 C1 C0 D11 D10 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0 // where C is the 4 bit command and D is the 12 bit data //----------------------------------------------------------------------------- -void FpgaSendCommand(uint16_t cmd, uint16_t v) { +void FpgaSendCommand(uint16_t cmd, uint16_t v) +{ SetupSpi(SPI_FPGA_MODE); while ((AT91C_BASE_SPI->SPI_SR & AT91C_SPI_TXEMPTY) == 0); // wait for the transfer to complete AT91C_BASE_SPI->SPI_TDR = AT91C_SPI_LASTXFER | cmd | v; // send the data @@ -434,7 +451,8 @@ void FpgaSendCommand(uint16_t cmd, uint16_t v) { // vs. clone vs. etc.). This is now a special case of FpgaSendCommand() to // avoid changing this function's occurence everywhere in the source code. //----------------------------------------------------------------------------- -void FpgaWriteConfWord(uint8_t v) { +void FpgaWriteConfWord(uint8_t v) +{ FpgaSendCommand(FPGA_CMD_SET_CONFREG, v); } @@ -443,7 +461,8 @@ void FpgaWriteConfWord(uint8_t v) { // closable, but should only close one at a time. Not an FPGA thing, but // the samples from the ADC always flow through the FPGA. //----------------------------------------------------------------------------- -void SetAdcMuxFor(uint32_t whichGpio) { +void SetAdcMuxFor(uint32_t whichGpio) +{ AT91C_BASE_PIOA->PIO_OER = GPIO_MUXSEL_HIPKD | GPIO_MUXSEL_LOPKD | @@ -466,12 +485,14 @@ void SetAdcMuxFor(uint32_t whichGpio) { HIGH(whichGpio); } -void Fpga_print_status(void) { +void Fpga_print_status(void) +{ Dbprintf("Currently loaded FPGA image"); - Dbprintf(" mode....................%s", fpga_version_information[downloaded_bitstream-1]); + Dbprintf(" mode....................%s", fpga_version_information[downloaded_bitstream - 1]); } -int FpgaGetCurrent(void) { +int FpgaGetCurrent(void) +{ return downloaded_bitstream; } @@ -479,10 +500,11 @@ int FpgaGetCurrent(void) { // log message // if HF, Disable SSC DMA // turn off trace and leds off. -void switch_off(void) { +void switch_off(void) +{ if (MF_DBGLEVEL > 3) Dbprintf("switch_off"); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); - if (downloaded_bitstream == FPGA_BITSTREAM_HF ) + if (downloaded_bitstream == FPGA_BITSTREAM_HF) FpgaDisableSscDma(); set_tracing(false); LEDsoff(); diff --git a/armsrc/hfsnoop.c b/armsrc/hfsnoop.c index 117e064e8..9400b6b3d 100644 --- a/armsrc/hfsnoop.c +++ b/armsrc/hfsnoop.c @@ -11,22 +11,23 @@ static void RAMFUNC optimizedSnoop(void) int n = BigBuf_max_traceLen() / sizeof(uint16_t); // take all memory uint16_t *dest = (uint16_t *)BigBuf_get_addr(); - uint16_t *destend = dest + n-1; + uint16_t *destend = dest + n - 1; // Reading data loop - while(dest <= destend) { - if(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { + while (dest <= destend) { + if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { *dest = (uint16_t)(AT91C_BASE_SSC->SSC_RHR); dest++; } } //setting tracelen - important! it was set by buffer overflow before - set_tracelen( BigBuf_max_traceLen()); + set_tracelen(BigBuf_max_traceLen()); } void HfSnoop(int samplesToSkip, int triggersToSkip) { - BigBuf_free(); BigBuf_Clear(); + BigBuf_free(); + BigBuf_Clear(); Dbprintf("Skipping first %d sample pairs, Skipping %d triggers.\n", samplesToSkip, triggersToSkip); int trigger_cnt = 0; @@ -47,7 +48,7 @@ void HfSnoop(int samplesToSkip, int triggersToSkip) SpinDelay(100); uint16_t r = 0; - while (!BUTTON_PRESS() && !usb_poll_validate_length() ) { + while (!BUTTON_PRESS() && !usb_poll_validate_length()) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { @@ -55,16 +56,16 @@ void HfSnoop(int samplesToSkip, int triggersToSkip) r = MAX(r & 0xff, r >> 8); if (r >= 180) { // 0xB4 ?? if (++trigger_cnt > triggersToSkip) - break; + break; } } } if (!BUTTON_PRESS()) { int waitcount = samplesToSkip; // lets wait 40000 ticks of pck0 - while(waitcount != 0) { + while (waitcount != 0) { - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) waitcount--; } optimizedSnoop(); diff --git a/armsrc/hitag2.c b/armsrc/hitag2.c index 913ea92ab..4bbc8fc6b 100644 --- a/armsrc/hitag2.c +++ b/armsrc/hitag2.c @@ -33,7 +33,7 @@ struct hitag2_tag { uint32_t uid; enum { TAG_STATE_RESET = 0x01, // Just powered up, awaiting GetSnr - TAG_STATE_ACTIVATING = 0x02 , // In activation phase (password mode), sent UID, awaiting reader password + TAG_STATE_ACTIVATING = 0x02, // In activation phase (password mode), sent UID, awaiting reader password TAG_STATE_ACTIVATED = 0x03, // Activation complete, awaiting read/write commands TAG_STATE_WRITING = 0x04, // In write command, awaiting sector contents to be written } state; @@ -71,7 +71,7 @@ static enum { // ToDo: define a meaningful maximum size for auth_table. The bigger this is, the lower will be the available memory for traces. // Historically it used to be FREE_BUFFER_SIZE, which was 2744. #define AUTH_TABLE_LENGTH 2744 -static uint8_t* auth_table; +static uint8_t *auth_table; static size_t auth_table_pos = 0; static size_t auth_table_len = AUTH_TABLE_LENGTH; @@ -108,44 +108,43 @@ static const u32 ht2_f4a = 0x2C79; // 0010 1100 0111 1001 static const u32 ht2_f4b = 0x6671; // 0110 0110 0111 0001 static const u32 ht2_f5c = 0x7907287B; // 0111 1001 0000 0111 0010 1000 0111 1011 -static u32 _f20 (const u64 x) +static u32 _f20(const u64 x) { u32 i5; - i5 = ((ht2_f4a >> i4 (x, 1, 2, 4, 5)) & 1)* 1 - + ((ht2_f4b >> i4 (x, 7,11,13,14)) & 1)* 2 - + ((ht2_f4b >> i4 (x,16,20,22,25)) & 1)* 4 - + ((ht2_f4b >> i4 (x,27,28,30,32)) & 1)* 8 - + ((ht2_f4a >> i4 (x,33,42,43,45)) & 1)*16; + i5 = ((ht2_f4a >> i4(x, 1, 2, 4, 5)) & 1) * 1 + + ((ht2_f4b >> i4(x, 7, 11, 13, 14)) & 1) * 2 + + ((ht2_f4b >> i4(x, 16, 20, 22, 25)) & 1) * 4 + + ((ht2_f4b >> i4(x, 27, 28, 30, 32)) & 1) * 8 + + ((ht2_f4a >> i4(x, 33, 42, 43, 45)) & 1) * 16; return (ht2_f5c >> i5) & 1; } -static u64 _hitag2_init (const u64 key, const u32 serial, const u32 IV) +static u64 _hitag2_init(const u64 key, const u32 serial, const u32 IV) { u32 i; u64 x = ((key & 0xFFFF) << 32) + serial; - for (i = 0; i < 32; i++) - { + for (i = 0; i < 32; i++) { x >>= 1; - x += (u64) (_f20 (x) ^ (((IV >> i) ^ (key >> (i+16))) & 1)) << 47; + x += (u64)(_f20(x) ^ (((IV >> i) ^ (key >> (i + 16))) & 1)) << 47; } return x; } -static u64 _hitag2_round (u64 *state) +static u64 _hitag2_round(u64 *state) { u64 x = *state; x = (x >> 1) + - ((((x >> 0) ^ (x >> 2) ^ (x >> 3) ^ (x >> 6) - ^ (x >> 7) ^ (x >> 8) ^ (x >> 16) ^ (x >> 22) - ^ (x >> 23) ^ (x >> 26) ^ (x >> 30) ^ (x >> 41) - ^ (x >> 42) ^ (x >> 43) ^ (x >> 46) ^ (x >> 47)) & 1) << 47); + ((((x >> 0) ^ (x >> 2) ^ (x >> 3) ^ (x >> 6) + ^ (x >> 7) ^ (x >> 8) ^ (x >> 16) ^ (x >> 22) + ^ (x >> 23) ^ (x >> 26) ^ (x >> 30) ^ (x >> 41) + ^ (x >> 42) ^ (x >> 43) ^ (x >> 46) ^ (x >> 47)) & 1) << 47); *state = x; - return _f20 (x); + return _f20(x); } // "MIKRON" = O N M I K R @@ -158,45 +157,47 @@ static u64 _hitag2_round (u64 *state) // The inverse of the first 4 bytes is sent to the tag to authenticate. // The rest is encrypted by XORing it with the subsequent keystream. -static u32 _hitag2_byte (u64 * x) +static u32 _hitag2_byte(u64 *x) { u32 i, c; - for (i = 0, c = 0; i < 8; i++) c += (u32) _hitag2_round (x) << (i^7); + for (i = 0, c = 0; i < 8; i++) c += (u32) _hitag2_round(x) << (i ^ 7); return c; } -static int hitag2_reset(void) { +static int hitag2_reset(void) +{ tag.state = TAG_STATE_RESET; tag.crypto_active = 0; return 0; } -static int hitag2_init(void) { +static int hitag2_init(void) +{ hitag2_reset(); return 0; } static void hitag2_cipher_reset(struct hitag2_tag *tag, const uint8_t *iv) { - uint64_t key = ((uint64_t)tag->sectors[2][2]) | - ((uint64_t)tag->sectors[2][3] << 8) | - ((uint64_t)tag->sectors[1][0] << 16) | - ((uint64_t)tag->sectors[1][1] << 24) | - ((uint64_t)tag->sectors[1][2] << 32) | - ((uint64_t)tag->sectors[1][3] << 40); - uint32_t uid = ((uint32_t)tag->sectors[0][0]) | - ((uint32_t)tag->sectors[0][1] << 8) | - ((uint32_t)tag->sectors[0][2] << 16) | - ((uint32_t)tag->sectors[0][3] << 24); + uint64_t key = ((uint64_t)tag->sectors[2][2]) | + ((uint64_t)tag->sectors[2][3] << 8) | + ((uint64_t)tag->sectors[1][0] << 16) | + ((uint64_t)tag->sectors[1][1] << 24) | + ((uint64_t)tag->sectors[1][2] << 32) | + ((uint64_t)tag->sectors[1][3] << 40); + uint32_t uid = ((uint32_t)tag->sectors[0][0]) | + ((uint32_t)tag->sectors[0][1] << 8) | + ((uint32_t)tag->sectors[0][2] << 16) | + ((uint32_t)tag->sectors[0][3] << 24); uint32_t iv_ = (((uint32_t)(iv[0]))) | - (((uint32_t)(iv[1])) << 8) | - (((uint32_t)(iv[2])) << 16) | - (((uint32_t)(iv[3])) << 24); + (((uint32_t)(iv[1])) << 8) | + (((uint32_t)(iv[2])) << 16) | + (((uint32_t)(iv[3])) << 24); tag->cs = _hitag2_init(rev64(key), rev32(uid), rev32(iv_)); } -static int hitag2_cipher_authenticate(uint64_t* cs, const uint8_t *authenticator_is) +static int hitag2_cipher_authenticate(uint64_t *cs, const uint8_t *authenticator_is) { uint8_t authenticator_should[4]; authenticator_should[0] = ~_hitag2_byte(cs); @@ -206,11 +207,11 @@ static int hitag2_cipher_authenticate(uint64_t* cs, const uint8_t *authenticator return (memcmp(authenticator_should, authenticator_is, 4) == 0); } -static int hitag2_cipher_transcrypt(uint64_t* cs, uint8_t *data, unsigned int bytes, unsigned int bits) +static int hitag2_cipher_transcrypt(uint64_t *cs, uint8_t *data, unsigned int bytes, unsigned int bits) { int i; - for(i=0; iTC_CCR = AT91C_TC_SWTRG; // Fixed modulation, earlier proxmark version used inverted signal - if(bit == 0) { + if (bit == 0) { // Manchester: Unloaded, then loaded |__--| LOW(GPIO_SSC_DOUT); - while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_HALF_PERIOD); + while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD); HIGH(GPIO_SSC_DOUT); - while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_FULL_PERIOD); + while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD); } else { // Manchester: Loaded, then unloaded |--__| HIGH(GPIO_SSC_DOUT); - while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_HALF_PERIOD); + while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD); LOW(GPIO_SSC_DOUT); - while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_FULL_PERIOD); + while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD); } LED_A_OFF(); } -static void hitag_send_frame(const uint8_t* frame, size_t frame_len) +static void hitag_send_frame(const uint8_t *frame, size_t frame_len) { // Send start of frame - for(size_t i=0; i<5; i++) { + for (size_t i = 0; i < 5; i++) { hitag_send_bit(1); } // Send the content of the frame - for(size_t i=0; i> (7-(i%8)))&1); + for (size_t i = 0; i < frame_len; i++) { + hitag_send_bit((frame[i / 8] >> (7 - (i % 8))) & 1); } // Drop the modulation @@ -288,15 +290,15 @@ static void hitag_send_frame(const uint8_t* frame, size_t frame_len) } -static void hitag2_handle_reader_command(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* txlen) +static void hitag2_handle_reader_command(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) { uint8_t rx_air[HITAG_FRAME_LEN]; // Copy the (original) received frame how it is send over the air - memcpy(rx_air,rx,nbytes(rxlen)); + memcpy(rx_air, rx, nbytes(rxlen)); - if(tag.crypto_active) { - hitag2_cipher_transcrypt(&(tag.cs),rx,rxlen/8,rxlen%8); + if (tag.crypto_active) { + hitag2_cipher_transcrypt(&(tag.cs), rx, rxlen / 8, rxlen % 8); } // Reset the transmission frame length @@ -307,21 +309,21 @@ static void hitag2_handle_reader_command(uint8_t* rx, const size_t rxlen, uint8_ // Received 11000 from the reader, request for UID, send UID case 05: { // Always send over the air in the clear plaintext mode - if(rx_air[0] != 0xC0) { + if (rx_air[0] != 0xC0) { // Unknown frame ? return; } *txlen = 32; - memcpy(tx,tag.sectors[0],4); + memcpy(tx, tag.sectors[0], 4); tag.crypto_active = 0; } break; // Read/Write command: ..xx x..y yy with yyy == ~xxx, xxx is sector number case 10: { - unsigned int sector = (~( ((rx[0]<<2)&0x04) | ((rx[1]>>6)&0x03) ) & 0x07); + unsigned int sector = (~(((rx[0] << 2) & 0x04) | ((rx[1] >> 6) & 0x03)) & 0x07); // Verify complement of sector index - if(sector != ((rx[0]>>3)&0x07)) { + if (sector != ((rx[0] >> 3) & 0x07)) { //DbpString("Transmission error (read/write)"); return; } @@ -329,51 +331,51 @@ static void hitag2_handle_reader_command(uint8_t* rx, const size_t rxlen, uint8_ switch (rx[0] & 0xC6) { // Read command: 11xx x00y case 0xC0: - memcpy(tx,tag.sectors[sector],4); + memcpy(tx, tag.sectors[sector], 4); *txlen = 32; - break; + break; - // Inverted Read command: 01xx x10y + // Inverted Read command: 01xx x10y case 0x44: - for (size_t i=0; i<4; i++) { + for (size_t i = 0; i < 4; i++) { tx[i] = tag.sectors[sector][i] ^ 0xff; } *txlen = 32; - break; + break; // Write command: 10xx x01y case 0x82: // Prepare write, acknowledge by repeating command - memcpy(tx,rx,nbytes(rxlen)); + memcpy(tx, rx, nbytes(rxlen)); *txlen = rxlen; tag.active_sector = sector; - tag.state=TAG_STATE_WRITING; - break; + tag.state = TAG_STATE_WRITING; + break; // Unknown command default: - Dbprintf("Unknown command: %02x %02x",rx[0],rx[1]); + Dbprintf("Unknown command: %02x %02x", rx[0], rx[1]); return; - break; + break; } } break; // Writing data or Reader password case 32: { - if(tag.state == TAG_STATE_WRITING) { + if (tag.state == TAG_STATE_WRITING) { // These are the sector contents to be written. We don't have to do anything else. - memcpy(tag.sectors[tag.active_sector],rx,nbytes(rxlen)); - tag.state=TAG_STATE_RESET; + memcpy(tag.sectors[tag.active_sector], rx, nbytes(rxlen)); + tag.state = TAG_STATE_RESET; return; } else { // Received RWD password, respond with configuration and our password - if(memcmp(rx,tag.sectors[1],4) != 0) { + if (memcmp(rx, tag.sectors[1], 4) != 0) { DbpString("Reader password is wrong"); return; } *txlen = 32; - memcpy(tx,tag.sectors[3],4); + memcpy(tx, tag.sectors[3], 4); } } break; @@ -381,17 +383,17 @@ static void hitag2_handle_reader_command(uint8_t* rx, const size_t rxlen, uint8_ // Received RWD authentication challenge and respnse case 64: { // Store the authentication attempt - if (auth_table_len < (AUTH_TABLE_LENGTH-8)) { - memcpy(auth_table+auth_table_len,rx,8); + if (auth_table_len < (AUTH_TABLE_LENGTH - 8)) { + memcpy(auth_table + auth_table_len, rx, 8); auth_table_len += 8; } // Reset the cipher state - hitag2_cipher_reset(&tag,rx); + hitag2_cipher_reset(&tag, rx); // Check if the authentication was correct - if(!hitag2_cipher_authenticate(&(tag.cs),rx+4)) { + if (!hitag2_cipher_authenticate(&(tag.cs), rx + 4)) { // The reader failed to authenticate, do nothing - Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x Failed!",rx[0],rx[1],rx[2],rx[3],rx[4],rx[5],rx[6],rx[7]); + Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x Failed!", rx[0], rx[1], rx[2], rx[3], rx[4], rx[5], rx[6], rx[7]); return; } // Succesful, but commented out reporting back to the Host, this may delay to much. @@ -401,7 +403,7 @@ static void hitag2_handle_reader_command(uint8_t* rx, const size_t rxlen, uint8_ tag.crypto_active = 1; // Use the tag password as response - memcpy(tx,tag.sectors[3],4); + memcpy(tx, tag.sectors[3], 4); *txlen = 32; } break; @@ -410,12 +412,13 @@ static void hitag2_handle_reader_command(uint8_t* rx, const size_t rxlen, uint8_ // LogTraceHitag(rx,rxlen,0,0,false); // LogTraceHitag(tx,*txlen,0,0,true); - if(tag.crypto_active) { - hitag2_cipher_transcrypt(&(tag.cs), tx, *txlen/8, *txlen%8); + if (tag.crypto_active) { + hitag2_cipher_transcrypt(&(tag.cs), tx, *txlen / 8, *txlen % 8); } } -static void hitag_reader_send_bit(int bit) { +static void hitag_reader_send_bit(int bit) +{ LED_A_ON(); // Reset clock for the next bit AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG; @@ -427,43 +430,44 @@ static void hitag_reader_send_bit(int bit) { HIGH(GPIO_SSC_DOUT); // Wait for 4-10 times the carrier period - while(AT91C_BASE_TC0->TC_CV < T0*6); + while (AT91C_BASE_TC0->TC_CV < T0 * 6); // SpinDelayUs(8*8); // Disable modulation, just activates the field again LOW(GPIO_SSC_DOUT); - if(bit == 0) { + if (bit == 0) { // Zero bit: |_-| - while (AT91C_BASE_TC0->TC_CV < T0*22) {}; + while (AT91C_BASE_TC0->TC_CV < T0 * 22) {}; } else { // One bit: |_--| - while (AT91C_BASE_TC0->TC_CV < T0*28) {}; + while (AT91C_BASE_TC0->TC_CV < T0 * 28) {}; } LED_A_OFF(); } -static void hitag_reader_send_frame(const uint8_t* frame, size_t frame_len) +static void hitag_reader_send_frame(const uint8_t *frame, size_t frame_len) { // Send the content of the frame - for(size_t i=0; i> (7-(i%8)))&1); + for (size_t i = 0; i < frame_len; i++) { + hitag_reader_send_bit((frame[i / 8] >> (7 - (i % 8))) & 1); } // Send EOF AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG; // Enable modulation, which means, drop the field HIGH(GPIO_SSC_DOUT); // Wait for 4-10 times the carrier period - while(AT91C_BASE_TC0->TC_CV < T0*6); + while (AT91C_BASE_TC0->TC_CV < T0 * 6); // Disable modulation, just activates the field again LOW(GPIO_SSC_DOUT); } size_t blocknr; -static bool hitag2_password(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* txlen) { +static bool hitag2_password(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) +{ // Reset the transmission frame length *txlen = 0; @@ -477,24 +481,25 @@ static bool hitag2_password(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t return false; } *txlen = 5; - memcpy(tx,"\xc0",nbytes(*txlen)); - } break; + memcpy(tx, "\xc0", nbytes(*txlen)); + } + break; // Received UID, tag password case 32: { if (!bPwd) { *txlen = 32; - memcpy(tx,password,4); + memcpy(tx, password, 4); bPwd = true; - memcpy(tag.sectors[blocknr],rx,4); + memcpy(tag.sectors[blocknr], rx, 4); blocknr++; } else { - if(blocknr == 1){ + if (blocknr == 1) { //store password in block1, the TAG answers with Block3, but we need the password in memory - memcpy(tag.sectors[blocknr],tx,4); + memcpy(tag.sectors[blocknr], tx, 4); } else { - memcpy(tag.sectors[blocknr],rx,4); + memcpy(tag.sectors[blocknr], rx, 4); } blocknr++; @@ -504,68 +509,71 @@ static bool hitag2_password(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t return false; } *txlen = 10; - tx[0] = 0xc0 | (blocknr << 3) | ((blocknr^7) >> 2); - tx[1] = ((blocknr^7) << 6); + tx[0] = 0xc0 | (blocknr << 3) | ((blocknr ^ 7) >> 2); + tx[1] = ((blocknr ^ 7) << 6); } - } break; + } + break; // Unexpected response - default: { - Dbprintf("Uknown frame length: %d",rxlen); + default: { + Dbprintf("Uknown frame length: %d", rxlen); return false; - } break; + } + break; } return true; } -static bool hitag2_write_page(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* txlen) +static bool hitag2_write_page(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) { switch (writestate) { - case WRITE_STATE_START: - *txlen = 10; - tx[0] = 0x82 | (blocknr << 3) | ((blocknr^7) >> 2); - tx[1] = ((blocknr^7) << 6); - writestate = WRITE_STATE_PAGENUM_WRITTEN; - break; - case WRITE_STATE_PAGENUM_WRITTEN: - // Check if page number was received correctly - if ((rxlen == 10) && - (rx[0] == (0x82 | (blocknr << 3) | ((blocknr^7) >> 2))) && - (rx[1] == (((blocknr & 0x3) ^ 0x3) << 6))) { - *txlen = 32; - memset(tx, 0, HITAG_FRAME_LEN); - memcpy(tx, writedata, 4); - writestate = WRITE_STATE_PROG; - } else { - Dbprintf("hitag2_write_page: Page number was not received correctly: rxlen=%d rx=%02x%02x%02x%02x", - rxlen, rx[0], rx[1], rx[2], rx[3]); + case WRITE_STATE_START: + *txlen = 10; + tx[0] = 0x82 | (blocknr << 3) | ((blocknr ^ 7) >> 2); + tx[1] = ((blocknr ^ 7) << 6); + writestate = WRITE_STATE_PAGENUM_WRITTEN; + break; + case WRITE_STATE_PAGENUM_WRITTEN: + // Check if page number was received correctly + if ((rxlen == 10) && + (rx[0] == (0x82 | (blocknr << 3) | ((blocknr ^ 7) >> 2))) && + (rx[1] == (((blocknr & 0x3) ^ 0x3) << 6))) { + *txlen = 32; + memset(tx, 0, HITAG_FRAME_LEN); + memcpy(tx, writedata, 4); + writestate = WRITE_STATE_PROG; + } else { + Dbprintf("hitag2_write_page: Page number was not received correctly: rxlen=%d rx=%02x%02x%02x%02x", + rxlen, rx[0], rx[1], rx[2], rx[3]); + bSuccessful = false; + return false; + } + break; + case WRITE_STATE_PROG: + if (rxlen == 0) { + bSuccessful = true; + } else { + bSuccessful = false; + Dbprintf("hitag2_write_page: unexpected rx data (%d) after page write", rxlen); + } + return false; + default: + DbpString("hitag2_write_page: Unknown state %d"); bSuccessful = false; return false; - } - break; - case WRITE_STATE_PROG: - if (rxlen == 0) { - bSuccessful = true; - } else { - bSuccessful = false; - Dbprintf("hitag2_write_page: unexpected rx data (%d) after page write", rxlen); - } - return false; - default: - DbpString("hitag2_write_page: Unknown state %d"); - bSuccessful = false; - return false; } return true; } -static bool hitag2_crypto(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* txlen, bool write) { +static bool hitag2_crypto(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen, bool write) +{ // Reset the transmission frame length *txlen = 0; - if(bCrypto) { - hitag2_cipher_transcrypt(&cipher_state,rx,rxlen/8,rxlen%8); + if (bCrypto) { + hitag2_cipher_transcrypt(&cipher_state, rx, rxlen / 8, rxlen % 8); } if (bCrypto && !bAuthenticating && write) { @@ -574,92 +582,94 @@ static bool hitag2_crypto(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* } } else { - // Try to find out which command was send by selecting on length (in bits) - switch (rxlen) { - // No answer, try to resurrect - case 0: { - // Stop if there is no answer while we are in crypto mode (after sending NrAr) - if (bCrypto) { - // Failed during authentication - if (bAuthenticating) { - DbpString("Authentication failed!"); - return false; - } else { - // Failed reading a block, could be (read/write) locked, skip block and re-authenticate - if (blocknr == 1) { - // Write the low part of the key in memory - memcpy(tag.sectors[1],key+2,4); - } else if (blocknr == 2) { - // Write the high part of the key in memory - tag.sectors[2][0] = 0x00; - tag.sectors[2][1] = 0x00; - tag.sectors[2][2] = key[0]; - tag.sectors[2][3] = key[1]; - } else { - // Just put zero's in the memory (of the unreadable block) - memset(tag.sectors[blocknr],0x00,4); - } - blocknr++; - bCrypto = false; - } - } else { - *txlen = 5; - memcpy(tx,"\xc0",nbytes(*txlen)); - } - break; - } - // Received UID, crypto tag answer - case 32: { - if (!bCrypto) { - uint64_t ui64key = key[0] | ((uint64_t)key[1]) << 8 | ((uint64_t)key[2]) << 16 | ((uint64_t)key[3]) << 24 | ((uint64_t)key[4]) << 32 | ((uint64_t)key[5]) << 40; - uint32_t ui32uid = rx[0] | ((uint32_t)rx[1]) << 8 | ((uint32_t)rx[2]) << 16 | ((uint32_t)rx[3]) << 24; - Dbprintf("hitag2_crypto: key=0x%x%x uid=0x%x", (uint32_t) ((rev64(ui64key)) >> 32), (uint32_t) ((rev64(ui64key)) & 0xffffffff), rev32(ui32uid)); - cipher_state = _hitag2_init(rev64(ui64key), rev32(ui32uid), 0); - memset(tx,0x00,4); - memset(tx+4,0xff,4); - hitag2_cipher_transcrypt(&cipher_state,tx+4,4,0); - *txlen = 64; - bCrypto = true; - bAuthenticating = true; - } else { - // Check if we received answer tag (at) - if (bAuthenticating) { - bAuthenticating = false; - if (write) { - if (!hitag2_write_page(rx, rxlen, tx, txlen)) { + // Try to find out which command was send by selecting on length (in bits) + switch (rxlen) { + // No answer, try to resurrect + case 0: { + // Stop if there is no answer while we are in crypto mode (after sending NrAr) + if (bCrypto) { + // Failed during authentication + if (bAuthenticating) { + DbpString("Authentication failed!"); return false; + } else { + // Failed reading a block, could be (read/write) locked, skip block and re-authenticate + if (blocknr == 1) { + // Write the low part of the key in memory + memcpy(tag.sectors[1], key + 2, 4); + } else if (blocknr == 2) { + // Write the high part of the key in memory + tag.sectors[2][0] = 0x00; + tag.sectors[2][1] = 0x00; + tag.sectors[2][2] = key[0]; + tag.sectors[2][3] = key[1]; + } else { + // Just put zero's in the memory (of the unreadable block) + memset(tag.sectors[blocknr], 0x00, 4); + } + blocknr++; + bCrypto = false; } - break; + } else { + *txlen = 5; + memcpy(tx, "\xc0", nbytes(*txlen)); } - } else { - // Store the received block - memcpy(tag.sectors[blocknr],rx,4); - blocknr++; - } - if (blocknr > 7) { - DbpString("Read succesful!"); - bSuccessful = true; - return false; - } else { - *txlen = 10; - tx[0] = 0xc0 | (blocknr << 3) | ((blocknr^7) >> 2); - tx[1] = ((blocknr^7) << 6); + break; } + // Received UID, crypto tag answer + case 32: { + if (!bCrypto) { + uint64_t ui64key = key[0] | ((uint64_t)key[1]) << 8 | ((uint64_t)key[2]) << 16 | ((uint64_t)key[3]) << 24 | ((uint64_t)key[4]) << 32 | ((uint64_t)key[5]) << 40; + uint32_t ui32uid = rx[0] | ((uint32_t)rx[1]) << 8 | ((uint32_t)rx[2]) << 16 | ((uint32_t)rx[3]) << 24; + Dbprintf("hitag2_crypto: key=0x%x%x uid=0x%x", (uint32_t)((rev64(ui64key)) >> 32), (uint32_t)((rev64(ui64key)) & 0xffffffff), rev32(ui32uid)); + cipher_state = _hitag2_init(rev64(ui64key), rev32(ui32uid), 0); + memset(tx, 0x00, 4); + memset(tx + 4, 0xff, 4); + hitag2_cipher_transcrypt(&cipher_state, tx + 4, 4, 0); + *txlen = 64; + bCrypto = true; + bAuthenticating = true; + } else { + // Check if we received answer tag (at) + if (bAuthenticating) { + bAuthenticating = false; + if (write) { + if (!hitag2_write_page(rx, rxlen, tx, txlen)) { + return false; + } + break; + } + } else { + // Store the received block + memcpy(tag.sectors[blocknr], rx, 4); + blocknr++; + } + if (blocknr > 7) { + DbpString("Read succesful!"); + bSuccessful = true; + return false; + } else { + *txlen = 10; + tx[0] = 0xc0 | (blocknr << 3) | ((blocknr ^ 7) >> 2); + tx[1] = ((blocknr ^ 7) << 6); + } + } + } + break; + + // Unexpected response + default: { + Dbprintf("Uknown frame length: %d", rxlen); + return false; + } + break; } - } break; - - // Unexpected response - default: { - Dbprintf("Uknown frame length: %d",rxlen); - return false; - } break; - } } - if(bCrypto) { + if (bCrypto) { // We have to return now to avoid double encryption if (!bAuthenticating) { - hitag2_cipher_transcrypt(&cipher_state, tx, *txlen/8, *txlen%8); + hitag2_cipher_transcrypt(&cipher_state, tx, *txlen / 8, *txlen % 8); } } @@ -667,7 +677,8 @@ static bool hitag2_crypto(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* } -static bool hitag2_authenticate(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* txlen) { +static bool hitag2_authenticate(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) +{ // Reset the transmission frame length *txlen = 0; @@ -681,8 +692,9 @@ static bool hitag2_authenticate(uint8_t* rx, const size_t rxlen, uint8_t* tx, si return false; } *txlen = 5; - memcpy(tx,"\xc0", nbytes(*txlen)); - } break; + memcpy(tx, "\xc0", nbytes(*txlen)); + } + break; // Received UID, crypto tag answer case 32: { @@ -694,34 +706,37 @@ static bool hitag2_authenticate(uint8_t* rx, const size_t rxlen, uint8_t* tx, si DbpString("Authentication succesful!"); return true; } - } break; + } + break; // Unexpected response default: { Dbprintf("Uknown frame length: %d", rxlen); return false; - } break; + } + break; } return true; } -static bool hitag2_test_auth_attempts(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* txlen) { +static bool hitag2_test_auth_attempts(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) +{ // Reset the transmission frame length *txlen = 0; // Try to find out which command was send by selecting on length (in bits) switch (rxlen) { - // No answer, try to resurrect + // No answer, try to resurrect case 0: { // Stop if there is no answer while we are in crypto mode (after sending NrAr) if (bCrypto) { - Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x Failed, removed entry!",NrAr[0],NrAr[1],NrAr[2],NrAr[3],NrAr[4],NrAr[5],NrAr[6],NrAr[7]); + Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x Failed, removed entry!", NrAr[0], NrAr[1], NrAr[2], NrAr[3], NrAr[4], NrAr[5], NrAr[6], NrAr[7]); // Removing failed entry from authentiations table - memcpy(auth_table+auth_table_pos,auth_table+auth_table_pos+8,8); + memcpy(auth_table + auth_table_pos, auth_table + auth_table_pos + 8, 8); auth_table_len -= 8; // Return if we reached the end of the authentications table @@ -731,39 +746,43 @@ static bool hitag2_test_auth_attempts(uint8_t* rx, const size_t rxlen, uint8_t* } // Copy the next authentication attempt in row (at the same position, b/c we removed last failed entry) - memcpy(NrAr,auth_table+auth_table_pos,8); + memcpy(NrAr, auth_table + auth_table_pos, 8); } *txlen = 5; - memcpy(tx,"\xc0",nbytes(*txlen)); - } break; + memcpy(tx, "\xc0", nbytes(*txlen)); + } + break; - // Received UID, crypto tag answer, or read block response + // Received UID, crypto tag answer, or read block response case 32: { if (!bCrypto) { *txlen = 64; - memcpy(tx,NrAr,8); + memcpy(tx, NrAr, 8); bCrypto = true; } else { - Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x OK",NrAr[0],NrAr[1],NrAr[2],NrAr[3],NrAr[4],NrAr[5],NrAr[6],NrAr[7]); + Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x OK", NrAr[0], NrAr[1], NrAr[2], NrAr[3], NrAr[4], NrAr[5], NrAr[6], NrAr[7]); bCrypto = false; - if ((auth_table_pos+8) == auth_table_len) { + if ((auth_table_pos + 8) == auth_table_len) { return false; } auth_table_pos += 8; - memcpy(NrAr,auth_table+auth_table_pos,8); + memcpy(NrAr, auth_table + auth_table_pos, 8); } - } break; + } + break; default: { - Dbprintf("Uknown frame length: %d",rxlen); + Dbprintf("Uknown frame length: %d", rxlen); return false; - } break; + } + break; } return true; } -static bool hitag2_read_uid(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t* txlen) { +static bool hitag2_read_uid(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) +{ // Reset the transmission frame length *txlen = 0; @@ -773,8 +792,9 @@ static bool hitag2_read_uid(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t case 0: { // Just starting or if there is no answer *txlen = 5; - memcpy(tx, "\xC0", nbytes(*txlen) ); - } break; + memcpy(tx, "\xC0", nbytes(*txlen)); + } + break; // Received UID case 32: { // Check if we received answer tag (at) @@ -790,17 +810,20 @@ static bool hitag2_read_uid(uint8_t* rx, const size_t rxlen, uint8_t* tx, size_t bSuccessful = true; return false; } - } break; + } + break; // Unexpected response default: { Dbprintf("Uknown frame length: %d", rxlen); return false; - } break; + } + break; } return true; } -void SnoopHitag(uint32_t type) { +void SnoopHitag(uint32_t type) +{ int frame_count; int response; int overflow; @@ -810,12 +833,13 @@ void SnoopHitag(uint32_t type) { bool bSkip; int tag_sof; uint8_t rx[HITAG_FRAME_LEN]; - size_t rxlen=0; + size_t rxlen = 0; FpgaDownloadAndGo(FPGA_BITSTREAM_LF); // free eventually allocated BigBuf memory - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); // Clean up trace and prepare it for storing frames clear_trace(); @@ -869,16 +893,16 @@ void SnoopHitag(uint32_t type) { bSkip = true; tag_sof = 4; - while(!BUTTON_PRESS() && !usb_poll_validate_length()) { + while (!BUTTON_PRESS() && !usb_poll_validate_length()) { // Watchdog hit WDT_HIT(); // Receive frame, watch for at most T0*EOF periods - while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_EOF) { + while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_EOF) { // Check if rising edge in modulation is detected - if(AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) { + if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) { // Retrieve the new timing values - int ra = (AT91C_BASE_TC1->TC_RA/T0); + int ra = (AT91C_BASE_TC1->TC_RA / T0); // Find out if we are dealing with a rising or falling edge rising_edge = (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME) > 0; @@ -888,13 +912,13 @@ void SnoopHitag(uint32_t type) { // Switch from tag to reader capture LED_C_OFF(); reader_frame = true; - memset(rx,0x00,sizeof(rx)); + memset(rx, 0x00, sizeof(rx)); rxlen = 0; } // Only handle if reader frame and rising edge, or tag frame and falling edge if (reader_frame != rising_edge) { - overflow += ra; + overflow += ra; continue; } @@ -905,19 +929,19 @@ void SnoopHitag(uint32_t type) { if (reader_frame) { LED_B_ON(); // Capture reader frame - if(ra >= HITAG_T_STOP) { + if (ra >= HITAG_T_STOP) { if (rxlen != 0) { //DbpString("wierd0?"); } // Capture the T0 periods that have passed since last communication or field drop (reset) response = (ra - HITAG_T_LOW); - } else if(ra >= HITAG_T_1_MIN ) { + } else if (ra >= HITAG_T_1_MIN) { // '1' bit - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; - } else if(ra >= HITAG_T_0_MIN) { + } else if (ra >= HITAG_T_0_MIN) { // '0' bit - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; } else { // Ignore wierd value, is to small to mean anything @@ -925,38 +949,38 @@ void SnoopHitag(uint32_t type) { } else { LED_C_ON(); // Capture tag frame (manchester decoding using only falling edges) - if(ra >= HITAG_T_EOF) { + if (ra >= HITAG_T_EOF) { if (rxlen != 0) { //DbpString("wierd1?"); } // Capture the T0 periods that have passed since last communication or field drop (reset) // We always recieve a 'one' first, which has the falling edge after a half period |-_| - response = ra-HITAG_T_TAG_HALF_PERIOD; - } else if(ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) { + response = ra - HITAG_T_TAG_HALF_PERIOD; + } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) { // Manchester coding example |-_|_-|-_| (101) - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; - } else if(ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) { + } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) { // Manchester coding example |_-|...|_-|-_| (0...01) - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; // We have to skip this half period at start and add the 'one' the second time if (!bSkip) { - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; } lastbit = !lastbit; bSkip = !bSkip; - } else if(ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) { + } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) { // Manchester coding example |_-|_-| (00) or |-_|-_| (11) if (tag_sof) { // Ignore bits that are transmitted during SOF tag_sof--; } else { // bit is same as last bit - rx[rxlen / 8] |= lastbit << (7-(rxlen%8)); + rx[rxlen / 8] |= lastbit << (7 - (rxlen % 8)); rxlen++; } } else { @@ -967,9 +991,9 @@ void SnoopHitag(uint32_t type) { } // Check if frame was captured - if(rxlen > 0) { + if (rxlen > 0) { frame_count++; - if (!LogTraceHitag(rx,rxlen,response,0,reader_frame)) { + if (!LogTraceHitag(rx, rxlen, response, 0, reader_frame)) { DbpString("Trace full"); break; } @@ -977,14 +1001,14 @@ void SnoopHitag(uint32_t type) { // Check if we recognize a valid authentication attempt if (nbytes(rxlen) == 8) { // Store the authentication attempt - if (auth_table_len < (AUTH_TABLE_LENGTH-8)) { - memcpy(auth_table+auth_table_len,rx,8); + if (auth_table_len < (AUTH_TABLE_LENGTH - 8)) { + memcpy(auth_table + auth_table_len, rx, 8); auth_table_len += 8; } } // Reset the received frame and response timing info - memset(rx,0x00,sizeof(rx)); + memset(rx, 0x00, sizeof(rx)); response = 0; reader_frame = false; lastbit = 1; @@ -996,7 +1020,7 @@ void SnoopHitag(uint32_t type) { LED_C_OFF(); } else { // Save the timer overflow, will be 0 when frame was received - overflow += (AT91C_BASE_TC1->TC_CV/T0); + overflow += (AT91C_BASE_TC1->TC_CV / T0); } // Reset the frame length rxlen = 0; @@ -1017,21 +1041,23 @@ void SnoopHitag(uint32_t type) { // DbpString("All done"); } -void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { +void SimulateHitagTag(bool tag_mem_supplied, uint8_t *data) +{ int frame_count; int response; int overflow; uint8_t rx[HITAG_FRAME_LEN]; - size_t rxlen=0; + size_t rxlen = 0; uint8_t tx[HITAG_FRAME_LEN]; - size_t txlen=0; + size_t txlen = 0; bool bQuitTraceFull = false; bQuiet = false; FpgaDownloadAndGo(FPGA_BITSTREAM_LF); // free eventually allocated BigBuf memory - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); // Clean up trace and prepare it for storing frames clear_trace(); @@ -1039,7 +1065,7 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { auth_table_len = 0; auth_table_pos = 0; - uint8_t* auth_table; + uint8_t *auth_table; auth_table = (uint8_t *)BigBuf_malloc(AUTH_TABLE_LENGTH); memset(auth_table, 0x00, AUTH_TABLE_LENGTH); @@ -1050,16 +1076,16 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { if (tag_mem_supplied) { DbpString("Loading hitag2 memory..."); - memcpy((uint8_t*)tag.sectors,data,48); + memcpy((uint8_t *)tag.sectors, data, 48); } uint32_t block = 0; - for (size_t i=0; i<12; i++) { - for (size_t j=0; j<4; j++) { + for (size_t i = 0; i < 12; i++) { + for (size_t j = 0; j < 4; j++) { block <<= 8; block |= tag.sectors[i][j]; } - Dbprintf("| %d | %08x |",i,block); + Dbprintf("| %d | %08x |", i, block); } // Set up simulator mode, frequency divisor which will drive the FPGA @@ -1092,7 +1118,7 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING; // Reset the received frame, frame count and timing info - memset(rx,0x00,sizeof(rx)); + memset(rx, 0x00, sizeof(rx)); frame_count = 0; response = 0; overflow = 0; @@ -1100,16 +1126,16 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { // Enable and reset counter AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; - while(!BUTTON_PRESS() && !usb_poll_validate_length()) { + while (!BUTTON_PRESS() && !usb_poll_validate_length()) { // Watchdog hit WDT_HIT(); // Receive frame, watch for at most T0*EOF periods - while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_EOF) { + while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_EOF) { // Check if rising edge in modulation is detected - if(AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) { + if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) { // Retrieve the new timing values - int ra = (AT91C_BASE_TC1->TC_RA/T0) + overflow; + int ra = (AT91C_BASE_TC1->TC_RA / T0) + overflow; overflow = 0; // Reset timer every frame, we have to capture the last edge for timing @@ -1118,19 +1144,19 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { LED_B_ON(); // Capture reader frame - if(ra >= HITAG_T_STOP) { + if (ra >= HITAG_T_STOP) { if (rxlen != 0) { //DbpString("wierd0?"); } // Capture the T0 periods that have passed since last communication or field drop (reset) response = (ra - HITAG_T_LOW); - } else if(ra >= HITAG_T_1_MIN ) { + } else if (ra >= HITAG_T_1_MIN) { // '1' bit - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; - } else if(ra >= HITAG_T_0_MIN) { + } else if (ra >= HITAG_T_0_MIN) { // '0' bit - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; } else { // Ignore wierd value, is to small to mean anything @@ -1139,10 +1165,10 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { } // Check if frame was captured - if(rxlen > 4) { + if (rxlen > 4) { frame_count++; if (!bQuiet) { - if (!LogTraceHitag(rx,rxlen,response,0,true)) { + if (!LogTraceHitag(rx, rxlen, response, 0, true)) { DbpString("Trace full"); if (bQuitTraceFull) { break; @@ -1156,22 +1182,22 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // Process the incoming frame (rx) and prepare the outgoing frame (tx) - hitag2_handle_reader_command(rx,rxlen,tx,&txlen); + hitag2_handle_reader_command(rx, rxlen, tx, &txlen); // Wait for HITAG_T_WAIT_1 carrier periods after the last reader bit, // not that since the clock counts since the rising edge, but T_Wait1 is // with respect to the falling edge, we need to wait actually (T_Wait1 - T_Low) // periods. The gap time T_Low varies (4..10). All timer values are in // terms of T0 units - while(AT91C_BASE_TC0->TC_CV < T0*(HITAG_T_WAIT_1-HITAG_T_LOW)); + while (AT91C_BASE_TC0->TC_CV < T0 * (HITAG_T_WAIT_1 - HITAG_T_LOW)); // Send and store the tag answer (if there is any) if (txlen) { // Transmit the tag frame - hitag_send_frame(tx,txlen); + hitag_send_frame(tx, txlen); // Store the frame in the trace if (!bQuiet) { - if (!LogTraceHitag(tx,txlen,0,0,false)) { + if (!LogTraceHitag(tx, txlen, 0, 0, false)) { DbpString("Trace full"); if (bQuitTraceFull) { break; @@ -1183,7 +1209,7 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { } // Reset the received frame and response timing info - memset(rx,0x00,sizeof(rx)); + memset(rx, 0x00, sizeof(rx)); response = 0; // Enable and reset external trigger in timer for capturing future frames @@ -1193,7 +1219,7 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { // Reset the frame length rxlen = 0; // Save the timer overflow, will be 0 when frame was received - overflow += (AT91C_BASE_TC1->TC_CV/T0); + overflow += (AT91C_BASE_TC1->TC_CV / T0); // Reset the timer to restart while-loop that receives frames AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG; } @@ -1207,13 +1233,14 @@ void SimulateHitagTag(bool tag_mem_supplied, uint8_t* data) { set_tracing(false); } -void ReaderHitag(hitag_function htf, hitag_data* htd) { +void ReaderHitag(hitag_function htf, hitag_data *htd) +{ int frame_count = 0; int response = 0; uint8_t rx[HITAG_FRAME_LEN]; size_t rxlen = 0; uint8_t txbuf[HITAG_FRAME_LEN]; - uint8_t* tx = txbuf; + uint8_t *tx = txbuf; size_t txlen = 0; int lastbit = 1; bool bSkip; @@ -1237,54 +1264,60 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { switch (htf) { case RHT2F_PASSWORD: { Dbprintf("List identifier in password mode"); - memcpy(password,htd->pwd.password, 4); + memcpy(password, htd->pwd.password, 4); blocknr = 0; bQuitTraceFull = false; bQuiet = false; bPwd = false; - } break; + } + break; case RHT2F_AUTHENTICATE: { DbpString("Authenticating using nr,ar pair:"); - memcpy(NrAr,htd->auth.NrAr, 8); - Dbhexdump(8,NrAr,false); + memcpy(NrAr, htd->auth.NrAr, 8); + Dbhexdump(8, NrAr, false); bQuiet = false; bCrypto = false; bAuthenticating = false; bQuitTraceFull = true; - } break; + } + break; case RHT2F_CRYPTO: { DbpString("Authenticating using key:"); - memcpy(key,htd->crypto.key, 6); //HACK; 4 or 6?? I read both in the code. - Dbhexdump(6,key,false); + memcpy(key, htd->crypto.key, 6); //HACK; 4 or 6?? I read both in the code. + Dbhexdump(6, key, false); blocknr = 0; bQuiet = false; bCrypto = false; bAuthenticating = false; bQuitTraceFull = true; - } break; + } + break; case RHT2F_TEST_AUTH_ATTEMPTS: { - Dbprintf("Testing %d authentication attempts",(auth_table_len/8)); + Dbprintf("Testing %d authentication attempts", (auth_table_len / 8)); auth_table_pos = 0; memcpy(NrAr, auth_table, 8); bQuitTraceFull = false; bQuiet = false; bCrypto = false; - } break; + } + break; case RHT2F_UID_ONLY: { blocknr = 0; bQuiet = false; bCrypto = false; bAuthenticating = false; bQuitTraceFull = true; - } break; + } + break; default: { - Dbprintf("Error, unknown function: %d",htf); + Dbprintf("Error, unknown function: %d", htf); set_tracing(false); return; - } break; + } + break; } LED_D_ON(); @@ -1328,28 +1361,28 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; // Tag specific configuration settings (sof, timings, etc.) - if (htf < 10){ + if (htf < 10) { // hitagS settings reset_sof = 1; t_wait = 200; - // DbpString("Configured for hitagS reader"); + // DbpString("Configured for hitagS reader"); } else if (htf < 20) { // hitag1 settings reset_sof = 1; t_wait = 200; - // DbpString("Configured for hitag1 reader"); + // DbpString("Configured for hitag1 reader"); } else if (htf < 30) { // hitag2 settings reset_sof = 4; t_wait = HITAG_T_WAIT_2; - // DbpString("Configured for hitag2 reader"); + // DbpString("Configured for hitag2 reader"); } else { - Dbprintf("Error, unknown hitag reader type: %d",htf); + Dbprintf("Error, unknown hitag reader type: %d", htf); set_tracing(false); LED_D_OFF(); return; } - uint8_t attempt_count=0; + uint8_t attempt_count = 0; while (!bStop && !BUTTON_PRESS()) { // Watchdog hit WDT_HIT(); @@ -1358,7 +1391,7 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { if (rxlen > 0) { frame_count++; if (!bQuiet) { - if (!LogTraceHitag(rx,rxlen, response, 0, false)) { + if (!LogTraceHitag(rx, rxlen, response, 0, false)) { DbpString("Trace full"); if (bQuitTraceFull) break; @@ -1372,29 +1405,35 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { tx = txbuf; switch (htf) { case RHT2F_PASSWORD: { - bStop = !hitag2_password(rx,rxlen,tx,&txlen); - } break; + bStop = !hitag2_password(rx, rxlen, tx, &txlen); + } + break; case RHT2F_AUTHENTICATE: { - bStop = !hitag2_authenticate(rx,rxlen,tx,&txlen); - } break; + bStop = !hitag2_authenticate(rx, rxlen, tx, &txlen); + } + break; case RHT2F_CRYPTO: { - bStop = !hitag2_crypto(rx,rxlen,tx,&txlen, false); - } break; + bStop = !hitag2_crypto(rx, rxlen, tx, &txlen, false); + } + break; case RHT2F_TEST_AUTH_ATTEMPTS: { - bStop = !hitag2_test_auth_attempts(rx,rxlen,tx,&txlen); - } break; + bStop = !hitag2_test_auth_attempts(rx, rxlen, tx, &txlen); + } + break; case RHT2F_UID_ONLY: { bStop = !hitag2_read_uid(rx, rxlen, tx, &txlen); attempt_count++; //attempt 3 times to get uid then quit if (!bStop && attempt_count == 3) bStop = true; - } break; + } + break; default: { - Dbprintf("Error, unknown function: %d",htf); + Dbprintf("Error, unknown function: %d", htf); set_tracing(false); LED_D_OFF(); return; - } break; + } + break; } // Send and store the reader command @@ -1443,7 +1482,7 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { // Check if falling edge in tag modulation is detected if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) { // Retrieve the new timing values - int ra = (AT91C_BASE_TC1->TC_RA/T0); + int ra = (AT91C_BASE_TC1->TC_RA / T0); // Reset timer every frame, we have to capture the last edge for timing AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG; @@ -1454,20 +1493,20 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { if (ra >= HITAG_T_EOF) { // Capture the T0 periods that have passed since last communication or field drop (reset) // We always recieve a 'one' first, which has the falling edge after a half period |-_| - response = ra-HITAG_T_TAG_HALF_PERIOD; + response = ra - HITAG_T_TAG_HALF_PERIOD; } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) { // Manchester coding example |-_|_-|-_| (101) - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) { // Manchester coding example |_-|...|_-|-_| (0...01) - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; // We have to skip this half period at start and add the 'one' the second time if (!bSkip) { - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; } lastbit = !lastbit; @@ -1479,19 +1518,19 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { tag_sof--; } else { // bit is same as last bit - rx[rxlen / 8] |= lastbit << (7-(rxlen%8)); + rx[rxlen / 8] |= lastbit << (7 - (rxlen % 8)); rxlen++; } } else { - //Dbprintf("DEBUG: Wierd2"); - errorCount++; + //Dbprintf("DEBUG: Wierd2"); + errorCount++; // Ignore wierd value, is to small to mean anything } } //if we saw over 100 wierd values break it probably isn't hitag... if (errorCount > 100) break; // We can break this loop if we received the last bit from a frame - if (AT91C_BASE_TC1->TC_CV > T0*HITAG_T_EOF) { + if (AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF) { if (rxlen > 0) break; } } @@ -1503,20 +1542,21 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); set_tracing(false); - if ( bSuccessful ) - cmd_send(CMD_ACK, bSuccessful, 0, 0, (uint8_t*)tag.sectors, 48); + if (bSuccessful) + cmd_send(CMD_ACK, bSuccessful, 0, 0, (uint8_t *)tag.sectors, 48); else cmd_send(CMD_ACK, bSuccessful, 0, 0, 0, 0); } -void WriterHitag(hitag_function htf, hitag_data* htd, int page) { +void WriterHitag(hitag_function htf, hitag_data *htd, int page) +{ int frame_count; int response; uint8_t rx[HITAG_FRAME_LEN]; - size_t rxlen=0; + size_t rxlen = 0; uint8_t txbuf[HITAG_FRAME_LEN]; - uint8_t* tx = txbuf; - size_t txlen=0; + uint8_t *tx = txbuf; + size_t txlen = 0; int lastbit; bool bSkip; int reset_sof; @@ -1536,23 +1576,25 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { // DbpString("Starting Hitag reader family"); // Check configuration - switch(htf) { - case WHT2F_CRYPTO: { - DbpString("Authenticating using key:"); - memcpy(key,htd->crypto.key,6); //HACK; 4 or 6?? I read both in the code. - memcpy(writedata, htd->crypto.data, 4); - Dbhexdump(6,key,false); - blocknr = page; - bQuiet = false; - bCrypto = false; - bAuthenticating = false; - bQuitTraceFull = true; - writestate = WRITE_STATE_START; - } break; - default: { - Dbprintf("Error, unknown function: %d",htf); - return; - } break; + switch (htf) { + case WHT2F_CRYPTO: { + DbpString("Authenticating using key:"); + memcpy(key, htd->crypto.key, 6); //HACK; 4 or 6?? I read both in the code. + memcpy(writedata, htd->crypto.data, 4); + Dbhexdump(6, key, false); + blocknr = page; + bQuiet = false; + bCrypto = false; + bAuthenticating = false; + bQuitTraceFull = true; + writestate = WRITE_STATE_START; + } + break; + default: { + Dbprintf("Error, unknown function: %d", htf); + return; + } + break; } LED_D_ON(); @@ -1601,7 +1643,7 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { bStop = false; // Tag specific configuration settings (sof, timings, etc.) - if (htf < 10){ + if (htf < 10) { // hitagS settings reset_sof = 1; t_wait = 200; @@ -1617,18 +1659,18 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { t_wait = HITAG_T_WAIT_2; // DbpString("Configured for hitag2 reader"); } else { - Dbprintf("Error, unknown hitag reader type: %d",htf); + Dbprintf("Error, unknown hitag reader type: %d", htf); return; } - while(!bStop && !BUTTON_PRESS()) { + while (!bStop && !BUTTON_PRESS()) { // Watchdog hit WDT_HIT(); // Check if frame was captured and store it - if(rxlen > 0) { + if (rxlen > 0) { frame_count++; if (!bQuiet) { - if (!LogTraceHitag(rx,rxlen,response,0,false)) { + if (!LogTraceHitag(rx, rxlen, response, 0, false)) { DbpString("Trace full"); if (bQuitTraceFull) { break; @@ -1641,14 +1683,16 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { // By default reset the transmission buffer tx = txbuf; - switch(htf) { - case WHT2F_CRYPTO: { - bStop = !hitag2_crypto(rx,rxlen,tx,&txlen, true); - } break; - default: { - Dbprintf("Error, unknown function: %d",htf); - return; - } break; + switch (htf) { + case WHT2F_CRYPTO: { + bStop = !hitag2_crypto(rx, rxlen, tx, &txlen, true); + } + break; + default: { + Dbprintf("Error, unknown function: %d", htf); + return; + } + break; } // Send and store the reader command @@ -1660,22 +1704,22 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { // falling edge occured halfway the period. with respect to this falling edge, // we need to wait (T_Wait2 + half_tag_period) when the last was a 'one'. // All timer values are in terms of T0 units - while(AT91C_BASE_TC0->TC_CV < T0*(t_wait+(HITAG_T_TAG_HALF_PERIOD*lastbit))); + while (AT91C_BASE_TC0->TC_CV < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))); // Dbprintf("DEBUG: Sending reader frame"); // Transmit the reader frame - hitag_reader_send_frame(tx,txlen); + hitag_reader_send_frame(tx, txlen); // Enable and reset external trigger in timer for capturing future frames AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; // Add transmitted frame to total count - if(txlen > 0) { + if (txlen > 0) { frame_count++; if (!bQuiet) { // Store the frame in the trace - if (!LogTraceHitag(tx,txlen,HITAG_T_WAIT_2,0,true)) { + if (!LogTraceHitag(tx, txlen, HITAG_T_WAIT_2, 0, true)) { if (bQuitTraceFull) { break; } else { @@ -1686,7 +1730,7 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { } // Reset values for receiving frames - memset(rx,0x00,sizeof(rx)); + memset(rx, 0x00, sizeof(rx)); rxlen = 0; lastbit = 1; bSkip = true; @@ -1696,11 +1740,11 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { uint32_t errorCount = 0; // Receive frame, watch for at most T0*EOF periods - while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_WAIT_MAX) { + while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_WAIT_MAX) { // Check if falling edge in tag modulation is detected - if(AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) { + if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) { // Retrieve the new timing values - int ra = (AT91C_BASE_TC1->TC_RA/T0); + int ra = (AT91C_BASE_TC1->TC_RA / T0); // Reset timer every frame, we have to capture the last edge for timing AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG; @@ -1708,41 +1752,41 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { LED_B_ON(); // Capture tag frame (manchester decoding using only falling edges) - if(ra >= HITAG_T_EOF) { + if (ra >= HITAG_T_EOF) { if (rxlen != 0) { //Dbprintf("DEBUG: Wierd1"); } // Capture the T0 periods that have passed since last communication or field drop (reset) // We always recieve a 'one' first, which has the falling edge after a half period |-_| - response = ra-HITAG_T_TAG_HALF_PERIOD; - } else if(ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) { + response = ra - HITAG_T_TAG_HALF_PERIOD; + } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) { // Manchester coding example |-_|_-|-_| (101) // need to test to verify we don't exceed memory... // if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) { // break; // } - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; - } else if(ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) { + } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) { // Manchester coding example |_-|...|_-|-_| (0...01) // need to test to verify we don't exceed memory... // if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) { // break; // } - rx[rxlen / 8] |= 0 << (7-(rxlen%8)); + rx[rxlen / 8] |= 0 << (7 - (rxlen % 8)); rxlen++; // We have to skip this half period at start and add the 'one' the second time if (!bSkip) { - rx[rxlen / 8] |= 1 << (7-(rxlen%8)); + rx[rxlen / 8] |= 1 << (7 - (rxlen % 8)); rxlen++; } lastbit = !lastbit; bSkip = !bSkip; - } else if(ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) { + } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) { // Manchester coding example |_-|_-| (00) or |-_|-_| (11) // need to test to verify we don't exceed memory... @@ -1754,7 +1798,7 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { tag_sof--; } else { // bit is same as last bit - rx[rxlen / 8] |= lastbit << (7-(rxlen%8)); + rx[rxlen / 8] |= lastbit << (7 - (rxlen % 8)); rxlen++; } } else { @@ -1764,18 +1808,17 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { } } // if we saw over 100 wierd values break it probably isn't hitag... - if (errorCount >100) break; + if (errorCount > 100) break; // We can break this loop if we received the last bit from a frame - if (AT91C_BASE_TC1->TC_CV > T0*HITAG_T_EOF) { - if (rxlen>0) break; + if (AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF) { + if (rxlen > 0) break; } } // Wait some extra time for flash to be programmed - if ((rxlen == 0) && (writestate == WRITE_STATE_PROG)) - { + if ((rxlen == 0) && (writestate == WRITE_STATE_PROG)) { AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG; - while(AT91C_BASE_TC0->TC_CV < T0*(HITAG_T_PROG - HITAG_T_WAIT_MAX)); + while (AT91C_BASE_TC0->TC_CV < T0 * (HITAG_T_PROG - HITAG_T_WAIT_MAX)); } } // Dbprintf("DEBUG: Done waiting for frame"); @@ -1787,5 +1830,5 @@ void WriterHitag(hitag_function htf, hitag_data* htd, int page) { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // Dbprintf("frame received: %d",frame_count); // DbpString("All done"); - cmd_send(CMD_ACK,bSuccessful,0,0,(uint8_t*)tag.sectors,48); + cmd_send(CMD_ACK, bSuccessful, 0, 0, (uint8_t *)tag.sectors, 48); } diff --git a/armsrc/hitagS.c b/armsrc/hitagS.c index 544dbf086..33028b777 100644 --- a/armsrc/hitagS.c +++ b/armsrc/hitagS.c @@ -53,7 +53,7 @@ static byte_t pwdh0, pwdl0, pwdl1; //password bytes static uint32_t rnd = 0x74124485; //randomnumber static int test = 0; size_t blocknr; -bool end=false; +bool end = false; // Single bit Hitag2 functions: #define i4(x,a,b,c,d) ((u32)((((x)>>(a))&1)+(((x)>>(b))&1)*2+(((x)>>(c))&1)*4+(((x)>>(d))&1)*8)) @@ -65,39 +65,43 @@ static const u32 ht2_f5c = 0x7907287B; // 0111 1001 0000 0111 0010 1000 0 #define ht2bs_5c(a,b,c,d,e) (~((((((c^e)|d)&a)^b)&(c^b))^(((d^e)|a)&((d^b)|c)))) #define uf20bs u32 -static u32 f20(const u64 x) { +static u32 f20(const u64 x) +{ u32 i5; i5 = ((ht2_f4a >> i4(x, 1, 2, 4, 5)) & 1) * 1 - + ((ht2_f4b >> i4(x, 7, 11, 13, 14)) & 1) * 2 - + ((ht2_f4b >> i4(x, 16, 20, 22, 25)) & 1) * 4 - + ((ht2_f4b >> i4(x, 27, 28, 30, 32)) & 1) * 8 - + ((ht2_f4a >> i4(x, 33, 42, 43, 45)) & 1) * 16; + + ((ht2_f4b >> i4(x, 7, 11, 13, 14)) & 1) * 2 + + ((ht2_f4b >> i4(x, 16, 20, 22, 25)) & 1) * 4 + + ((ht2_f4b >> i4(x, 27, 28, 30, 32)) & 1) * 8 + + ((ht2_f4a >> i4(x, 33, 42, 43, 45)) & 1) * 16; return (ht2_f5c >> i5) & 1; } -static u64 hitag2_round(u64 *state) { +static u64 hitag2_round(u64 *state) +{ u64 x = *state; x = (x >> 1) - + ((((x >> 0) ^ (x >> 2) ^ (x >> 3) ^ (x >> 6) ^ (x >> 7) ^ (x >> 8) - ^ (x >> 16) ^ (x >> 22) ^ (x >> 23) ^ (x >> 26) ^ (x >> 30) - ^ (x >> 41) ^ (x >> 42) ^ (x >> 43) ^ (x >> 46) ^ (x >> 47)) - & 1) << 47); + + ((((x >> 0) ^ (x >> 2) ^ (x >> 3) ^ (x >> 6) ^ (x >> 7) ^ (x >> 8) + ^ (x >> 16) ^ (x >> 22) ^ (x >> 23) ^ (x >> 26) ^ (x >> 30) + ^ (x >> 41) ^ (x >> 42) ^ (x >> 43) ^ (x >> 46) ^ (x >> 47)) + & 1) << 47); *state = x; return f20(x); } -static u64 hitag2_init(const u64 key, const u32 serial, const u32 IV) { +static u64 hitag2_init(const u64 key, const u32 serial, const u32 IV) +{ u32 i; u64 x = ((key & 0xFFFF) << 32) + serial; for (i = 0; i < 32; i++) { x >>= 1; - x += (u64) (f20(x) ^ (((IV >> i) ^ (key >> (i + 16))) & 1)) << 47; + x += (u64)(f20(x) ^ (((IV >> i) ^ (key >> (i + 16))) & 1)) << 47; } return x; } -static u32 hitag2_byte(u64 *x) { +static u32 hitag2_byte(u64 *x) +{ u32 i, c; for (i = 0, c = 0; i < 8; i++) @@ -144,11 +148,11 @@ static u32 hitag2_byte(u64 *x) { * Implementation of the crc8 calculation from Hitag S * from http://www.proxmark.org/files/Documents/125%20kHz%20-%20Hitag/HitagS.V11.pdf */ -void calc_crc(unsigned char * crc, unsigned char data, unsigned char Bitcount) { +void calc_crc(unsigned char *crc, unsigned char data, unsigned char Bitcount) +{ *crc ^= data; // crc = crc (exor) data do { - if (*crc & 0x80) // if (MSB-CRC == 1) - { + if (*crc & 0x80) { // if (MSB-CRC == 1) *crc <<= 1; // CRC = CRC Bit-shift left *crc ^= CRC_POLYNOM; // CRC = CRC (exor) CRC_POLYNOM } else { @@ -157,109 +161,111 @@ void calc_crc(unsigned char * crc, unsigned char data, unsigned char Bitcount) { } while (--Bitcount); } -static void hitag_send_bit(int bit) { +static void hitag_send_bit(int bit) +{ LED_A_ON(); // Reset clock for the next bit AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG; switch (m) { - case AC2K: - if (bit == 0) { - // AC Coding --__ - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; + case AC2K: + if (bit == 0) { + // AC Coding --__ + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 64) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 64) {}; - } else { - // AC coding -_-_ - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; + } else { + // AC coding -_-_ + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 48) {}; + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 48) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 64) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 64) {}; - } - LED_A_OFF(); - break; - case AC4K: - if (bit == 0) { - // AC Coding --__ - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD) {}; + } + LED_A_OFF(); + break; + case AC4K: + if (bit == 0) { + // AC Coding --__ + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD) {}; - } else { - // AC coding -_-_ - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 8) {}; + } else { + // AC coding -_-_ + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 8) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 24) {}; + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 24) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; - } - LED_A_OFF(); - break; - case MC4K: - if (bit == 0) { - // Manchester: Unloaded, then loaded |__--| - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; + } + LED_A_OFF(); + break; + case MC4K: + if (bit == 0) { + // Manchester: Unloaded, then loaded |__--| + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; - } else { - // Manchester: Loaded, then unloaded |--__| - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; + } else { + // Manchester: Loaded, then unloaded |--__| + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 32) {}; - } - LED_A_OFF(); - break; - case MC8K: - if (bit == 0) { - // Manchester: Unloaded, then loaded |__--| - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 8) {}; + } + LED_A_OFF(); + break; + case MC8K: + if (bit == 0) { + // Manchester: Unloaded, then loaded |__--| + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 8) {}; - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; - } else { - // Manchester: Loaded, then unloaded |--__| - HIGH(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 8) {}; + } else { + // Manchester: Loaded, then unloaded |--__| + HIGH(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 8) {}; - LOW(GPIO_SSC_DOUT); - while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; + LOW(GPIO_SSC_DOUT); + while (AT91C_BASE_TC0->TC_CV < T0 * 16) {}; - } - LED_A_OFF(); - break; - default: - break; + } + LED_A_OFF(); + break; + default: + break; } } -static void hitag_send_frame(const byte_t* frame, size_t frame_len) { +static void hitag_send_frame(const byte_t *frame, size_t frame_len) +{ // Send start of frame for (size_t i = 0; i < sof_bits; i++) { @@ -274,7 +280,8 @@ static void hitag_send_frame(const byte_t* frame, size_t frame_len) { LOW(GPIO_SSC_DOUT); } -static void hitag_reader_send_bit(int bit) { +static void hitag_reader_send_bit(int bit) +{ //Dbprintf("BIT: %d",bit); LED_A_ON(); // Reset clock for the next bit @@ -330,7 +337,8 @@ static void hitag_reader_send_bit(int bit) { LED_A_OFF(); } -static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len) { +static void hitag_reader_send_frame(const byte_t *frame, size_t frame_len) +{ // Send the content of the frame for (size_t i = 0; i < frame_len; i++) { if (frame[0] == 0xf8) { @@ -352,7 +360,8 @@ static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len) { /* * to check if the right uid was selected */ -static int check_select(byte_t* rx, uint32_t uid) { +static int check_select(byte_t *rx, uint32_t uid) +{ unsigned char resp[48]; int i; uint32_t ans = 0x0; @@ -372,8 +381,9 @@ static int check_select(byte_t* rx, uint32_t uid) { /* * handles all commands from a reader */ -static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen, - byte_t* tx, size_t* txlen) { +static void hitagS_handle_reader_command(byte_t *rx, const size_t rxlen, + byte_t *tx, size_t *txlen) +{ byte_t rx_air[HITAG_FRAME_LEN]; byte_t page; int i; @@ -386,347 +396,344 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen, *txlen = 0; // Try to find out which command was send by selecting on length (in bits) switch (rxlen) { - case 5: { - //UID request with a selected response protocol mode - tag.pstate = HT_READY; - tag.tstate = HT_NO_OP; - if ((rx[0] & 0xf0) == 0x30) { - tag.mode = HT_STANDARD; - sof_bits = 1; - m = AC2K; - } - if ((rx[0] & 0xf0) == 0xc0) { - tag.mode = HT_ADVANCED; - sof_bits = 3; - m = AC2K; - } - - if ((rx[0] & 0xf0) == 0xd0) { - tag.mode = HT_FAST_ADVANCED; - sof_bits = 3; - m = AC4K; - } - //send uid as a response - *txlen = 32; - for (i = 0; i < 4; i++) - tx[i] = (tag.uid >> (24 - (i * 8))) & 0xff; - } - break; - case 45: { - //select command from reader received - if (check_select(rx, tag.uid) == 1) { - //if the right tag was selected - *txlen = 32; - switch (tag.mode) { - case HT_STANDARD: + case 5: { + //UID request with a selected response protocol mode + tag.pstate = HT_READY; + tag.tstate = HT_NO_OP; + if ((rx[0] & 0xf0) == 0x30) { + tag.mode = HT_STANDARD; sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; + m = AC2K; + } + if ((rx[0] & 0xf0) == 0xc0) { + tag.mode = HT_ADVANCED; + sof_bits = 3; + m = AC2K; } - //send configuration + if ((rx[0] & 0xf0) == 0xd0) { + tag.mode = HT_FAST_ADVANCED; + sof_bits = 3; + m = AC4K; + } + //send uid as a response + *txlen = 32; for (i = 0; i < 4; i++) - tx[i] = (tag.pages[0][1] >> (i * 8)) & 0xff; - tx[3] = 0xff; - if (tag.mode != HT_STANDARD) { - *txlen = 40; - crc = CRC_PRESET; + tx[i] = (tag.uid >> (24 - (i * 8))) & 0xff; + } + break; + case 45: { + //select command from reader received + if (check_select(rx, tag.uid) == 1) { + //if the right tag was selected + *txlen = 32; + switch (tag.mode) { + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; + } + + //send configuration for (i = 0; i < 4; i++) - calc_crc(&crc, tx[i], 8); - tx[4] = crc; + tx[i] = (tag.pages[0][1] >> (i * 8)) & 0xff; + tx[3] = 0xff; + if (tag.mode != HT_STANDARD) { + *txlen = 40; + crc = CRC_PRESET; + for (i = 0; i < 4; i++) + calc_crc(&crc, tx[i], 8); + tx[4] = crc; + } } } - } break; - case 64: { - //challenge message received - Dbprintf("Challenge for UID: %X", temp_uid); - temp2++; - *txlen = 32; - state = hitag2_init(rev64(tag.key), rev32(tag.pages[0][0]), - rev32(((rx[3] << 24) + (rx[2] << 16) + (rx[1] << 8) + rx[0]))); - Dbprintf( + case 64: { + //challenge message received + Dbprintf("Challenge for UID: %X", temp_uid); + temp2++; + *txlen = 32; + state = hitag2_init(rev64(tag.key), rev32(tag.pages[0][0]), + rev32(((rx[3] << 24) + (rx[2] << 16) + (rx[1] << 8) + rx[0]))); + Dbprintf( ",{0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X}", rx[0], rx[1], rx[2], rx[3], rx[4], rx[5], rx[6], rx[7]); - switch (tag.mode) { - case HT_STANDARD: - sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; - } - - for (i = 0; i < 4; i++) - hitag2_byte(&state); - //send con2,pwdh0,pwdl0,pwdl1 encrypted as a response - tx[0] = hitag2_byte(&state) ^ ((tag.pages[0][1] >> 16) & 0xff); - tx[1] = hitag2_byte(&state) ^ tag.pwdh0; - tx[2] = hitag2_byte(&state) ^ tag.pwdl0; - tx[3] = hitag2_byte(&state) ^ tag.pwdl1; - if (tag.mode != HT_STANDARD) { - //add crc8 - *txlen = 40; - crc = CRC_PRESET; - calc_crc(&crc, ((tag.pages[0][1] >> 16) & 0xff), 8); - calc_crc(&crc, tag.pwdh0, 8); - calc_crc(&crc, tag.pwdl0, 8); - calc_crc(&crc, tag.pwdl1, 8); - tx[4] = (crc ^ hitag2_byte(&state)); - } - /* - * some readers do not allow to authenticate multiple times in a row with the same tag. - * use this to change the uid between authentications. - */ - - /* - if (temp2 % 2 == 0) { - tag.uid = 0x11223344; - tag.pages[0][0] = 0x44332211; - } else { - tag.uid = 0x55667788; - tag.pages[0][0] = 0x88776655; - } - */ - } - case 40: - //data received to be written - if (tag.tstate == HT_WRITING_PAGE_DATA) { - tag.tstate = HT_NO_OP; - tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0] - << 0) + (rx[1] << 8) + (rx[2] << 16) + (rx[3] << 24); - //send ack - *txlen = 2; - tx[0] = 0x40; - page_to_be_written = 0; switch (tag.mode) { - case HT_STANDARD: - sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; - } - } else if (tag.tstate == HT_WRITING_BLOCK_DATA) { - tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0] - << 24) + (rx[1] << 16) + (rx[2] << 8) + rx[3]; - //send ack - *txlen = 2; - tx[0] = 0x40; - switch (tag.mode) { - case HT_STANDARD: - sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; - } - page_to_be_written++; - block_data_left--; - if (block_data_left == 0) { - tag.tstate = HT_NO_OP; - page_to_be_written = 0; - } - } - break; - case 20: { - //write page, write block, read page or read block command received - if ((rx[0] & 0xf0) == 0xc0) //read page - { - //send page data - page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16); - *txlen = 32; - tx[0] = (tag.pages[page / 4][page % 4]) & 0xff; - tx[1] = (tag.pages[page / 4][page % 4] >> 8) & 0xff; - tx[2] = (tag.pages[page / 4][page % 4] >> 16) & 0xff; - tx[3] = (tag.pages[page / 4][page % 4] >> 24) & 0xff; - if (tag.LKP && page == 1) - tx[3] = 0xff; - - switch (tag.mode) { - case HT_STANDARD: - sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; } + for (i = 0; i < 4; i++) + hitag2_byte(&state); + //send con2,pwdh0,pwdl0,pwdl1 encrypted as a response + tx[0] = hitag2_byte(&state) ^ ((tag.pages[0][1] >> 16) & 0xff); + tx[1] = hitag2_byte(&state) ^ tag.pwdh0; + tx[2] = hitag2_byte(&state) ^ tag.pwdl0; + tx[3] = hitag2_byte(&state) ^ tag.pwdl1; if (tag.mode != HT_STANDARD) { //add crc8 *txlen = 40; crc = CRC_PRESET; - for (i = 0; i < 4; i++) - calc_crc(&crc, tx[i], 8); - tx[4] = crc; + calc_crc(&crc, ((tag.pages[0][1] >> 16) & 0xff), 8); + calc_crc(&crc, tag.pwdh0, 8); + calc_crc(&crc, tag.pwdl0, 8); + calc_crc(&crc, tag.pwdl1, 8); + tx[4] = (crc ^ hitag2_byte(&state)); } + /* + * some readers do not allow to authenticate multiple times in a row with the same tag. + * use this to change the uid between authentications. + */ - if (tag.LKP && (page == 2 || page == 3)) { - //if reader asks for key or password and the LKP-mark is set do not respond - sof_bits = 0; - *txlen = 0; - } - } else if ((rx[0] & 0xf0) == 0xd0) //read block - { - page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16); - *txlen = 32 * 4; - //send page,...,page+3 data - for (i = 0; i < 4; i++) { - tx[0 + i * 4] = (tag.pages[page / 4][page % 4]) & 0xff; - tx[1 + i * 4] = (tag.pages[page / 4][page % 4] >> 8) & 0xff; - tx[2 + i * 4] = (tag.pages[page / 4][page % 4] >> 16) & 0xff; - tx[3 + i * 4] = (tag.pages[page / 4][page % 4] >> 24) & 0xff; - page++; + /* + if (temp2 % 2 == 0) { + tag.uid = 0x11223344; + tag.pages[0][0] = 0x44332211; + } else { + tag.uid = 0x55667788; + tag.pages[0][0] = 0x88776655; + } + */ + } + case 40: + //data received to be written + if (tag.tstate == HT_WRITING_PAGE_DATA) { + tag.tstate = HT_NO_OP; + tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0] + << 0) + (rx[1] << 8) + (rx[2] << 16) + (rx[3] << 24); + //send ack + *txlen = 2; + tx[0] = 0x40; + page_to_be_written = 0; + switch (tag.mode) { + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; + } + } else if (tag.tstate == HT_WRITING_BLOCK_DATA) { + tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0] + << 24) + (rx[1] << 16) + (rx[2] << 8) + rx[3]; + //send ack + *txlen = 2; + tx[0] = 0x40; + switch (tag.mode) { + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; + } + page_to_be_written++; + block_data_left--; + if (block_data_left == 0) { + tag.tstate = HT_NO_OP; + page_to_be_written = 0; + } } + break; + case 20: { + //write page, write block, read page or read block command received + if ((rx[0] & 0xf0) == 0xc0) { //read page + //send page data + page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16); + *txlen = 32; + tx[0] = (tag.pages[page / 4][page % 4]) & 0xff; + tx[1] = (tag.pages[page / 4][page % 4] >> 8) & 0xff; + tx[2] = (tag.pages[page / 4][page % 4] >> 16) & 0xff; + tx[3] = (tag.pages[page / 4][page % 4] >> 24) & 0xff; + if (tag.LKP && page == 1) + tx[3] = 0xff; - switch (tag.mode) { - case HT_STANDARD: - sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; - } + switch (tag.mode) { + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; + } - if (tag.mode != HT_STANDARD) { - //add crc8 - *txlen = 32 * 4 + 8; - crc = CRC_PRESET; - for (i = 0; i < 16; i++) - calc_crc(&crc, tx[i], 8); - tx[16] = crc; - } + if (tag.mode != HT_STANDARD) { + //add crc8 + *txlen = 40; + crc = CRC_PRESET; + for (i = 0; i < 4; i++) + calc_crc(&crc, tx[i], 8); + tx[4] = crc; + } - if ((page - 4) % 4 != 0 || (tag.LKP && (page - 4) == 0)) { - sof_bits = 0; - *txlen = 0; - } - } else if ((rx[0] & 0xf0) == 0x80) //write page - { - page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16); + if (tag.LKP && (page == 2 || page == 3)) { + //if reader asks for key or password and the LKP-mark is set do not respond + sof_bits = 0; + *txlen = 0; + } + } else if ((rx[0] & 0xf0) == 0xd0) { //read block + page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16); + *txlen = 32 * 4; + //send page,...,page+3 data + for (i = 0; i < 4; i++) { + tx[0 + i * 4] = (tag.pages[page / 4][page % 4]) & 0xff; + tx[1 + i * 4] = (tag.pages[page / 4][page % 4] >> 8) & 0xff; + tx[2 + i * 4] = (tag.pages[page / 4][page % 4] >> 16) & 0xff; + tx[3 + i * 4] = (tag.pages[page / 4][page % 4] >> 24) & 0xff; + page++; + } - switch (tag.mode) { - case HT_STANDARD: - sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; - } - if ((tag.LCON && page == 1) + switch (tag.mode) { + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; + } + + if (tag.mode != HT_STANDARD) { + //add crc8 + *txlen = 32 * 4 + 8; + crc = CRC_PRESET; + for (i = 0; i < 16; i++) + calc_crc(&crc, tx[i], 8); + tx[16] = crc; + } + + if ((page - 4) % 4 != 0 || (tag.LKP && (page - 4) == 0)) { + sof_bits = 0; + *txlen = 0; + } + } else if ((rx[0] & 0xf0) == 0x80) { //write page + page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16); + + switch (tag.mode) { + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; + } + if ((tag.LCON && page == 1) || (tag.LKP && (page == 2 || page == 3))) { - //deny - *txlen = 0; - } else { - //allow - *txlen = 2; - tx[0] = 0x40; - page_to_be_written = page; - tag.tstate = HT_WRITING_PAGE_DATA; - } + //deny + *txlen = 0; + } else { + //allow + *txlen = 2; + tx[0] = 0x40; + page_to_be_written = page; + tag.tstate = HT_WRITING_PAGE_DATA; + } - } else if ((rx[0] & 0xf0) == 0x90) //write block - { - page = ((rx[0] & 0x0f) * 6) + ((rx[1] & 0xf0) / 16); - switch (tag.mode) { - case HT_STANDARD: - sof_bits = 1; - m = MC4K; - break; - case HT_ADVANCED: - sof_bits = 6; - m = MC4K; - break; - case HT_FAST_ADVANCED: - sof_bits = 6; - m = MC8K; - break; - default: - break; - } - if (page % 4 != 0 || page == 0) { - //deny - *txlen = 0; - } else { - //allow - *txlen = 2; - tx[0] = 0x40; - page_to_be_written = page; - block_data_left = 4; - tag.tstate = HT_WRITING_BLOCK_DATA; + } else if ((rx[0] & 0xf0) == 0x90) { //write block + page = ((rx[0] & 0x0f) * 6) + ((rx[1] & 0xf0) / 16); + switch (tag.mode) { + case HT_STANDARD: + sof_bits = 1; + m = MC4K; + break; + case HT_ADVANCED: + sof_bits = 6; + m = MC4K; + break; + case HT_FAST_ADVANCED: + sof_bits = 6; + m = MC8K; + break; + default: + break; + } + if (page % 4 != 0 || page == 0) { + //deny + *txlen = 0; + } else { + //allow + *txlen = 2; + tx[0] = 0x40; + page_to_be_written = page; + block_data_left = 4; + tag.tstate = HT_WRITING_BLOCK_DATA; + } } } - } break; - default: + default: - break; + break; } } /* * to autenticate to a tag with the given key or challenge */ -static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr, byte_t* rx, const size_t rxlen, byte_t* tx, - size_t* txlen) { +static int hitagS_handle_tag_auth(hitag_function htf, uint64_t key, uint64_t NrAr, byte_t *rx, const size_t rxlen, byte_t *tx, + size_t *txlen) +{ byte_t rx_air[HITAG_FRAME_LEN]; int response_bit[200]; int i, j, z, k; @@ -742,7 +749,7 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr if (tag.pstate == HT_READY && rxlen >= 67) { //received uid - if(end==true) { + if (end == true) { Dbprintf("authentication failed!"); return -1; } @@ -763,13 +770,13 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr break; } uid1 = (uid[0] << 7) | (uid[1] << 6) | (uid[2] << 5) | (uid[3] << 4) - | (uid[4] << 3) | (uid[5] << 2) | (uid[6] << 1) | uid[7]; + | (uid[4] << 3) | (uid[5] << 2) | (uid[6] << 1) | uid[7]; uid2 = (uid[8] << 7) | (uid[9] << 6) | (uid[10] << 5) | (uid[11] << 4) - | (uid[12] << 3) | (uid[13] << 2) | (uid[14] << 1) | uid[15]; + | (uid[12] << 3) | (uid[13] << 2) | (uid[14] << 1) | uid[15]; uid3 = (uid[16] << 7) | (uid[17] << 6) | (uid[18] << 5) | (uid[19] << 4) - | (uid[20] << 3) | (uid[21] << 2) | (uid[22] << 1) | uid[23]; + | (uid[20] << 3) | (uid[21] << 2) | (uid[22] << 1) | uid[23]; uid4 = (uid[24] << 7) | (uid[25] << 6) | (uid[26] << 5) | (uid[27] << 4) - | (uid[28] << 3) | (uid[29] << 2) | (uid[30] << 1) | uid[31]; + | (uid[28] << 3) | (uid[29] << 2) | (uid[30] << 1) | uid[31]; if (DEBUG) Dbprintf("UID: %02X %02X %02X %02X", uid1, uid2, uid3, uid4); tag.uid = (uid4 << 24 | uid3 << 16 | uid2 << 8 | uid1); @@ -818,9 +825,9 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr } } conf_pages[0] = ((response_bit[4] << 7) | (response_bit[5] << 6) - | (response_bit[6] << 5) | (response_bit[7] << 4) - | (response_bit[8] << 3) | (response_bit[9] << 2) - | (response_bit[10] << 1) | response_bit[11]); + | (response_bit[6] << 5) | (response_bit[7] << 4) + | (response_bit[8] << 3) | (response_bit[9] << 2) + | (response_bit[10] << 1) | response_bit[11]); //check wich memorysize this tag has if (response_bit[10] == 0 && response_bit[11] == 0) tag.max_page = 32 / 32; @@ -829,9 +836,9 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr if (response_bit[10] == 1 && response_bit[11] == 0) tag.max_page = 2048 / 32; conf_pages[1] = ((response_bit[12] << 7) | (response_bit[13] << 6) - | (response_bit[14] << 5) | (response_bit[15] << 4) - | (response_bit[16] << 3) | (response_bit[17] << 2) - | (response_bit[18] << 1) | response_bit[19]); + | (response_bit[14] << 5) | (response_bit[15] << 4) + | (response_bit[16] << 3) | (response_bit[17] << 2) + | (response_bit[18] << 1) | response_bit[19]); tag.auth = response_bit[12]; tag.TTFC = response_bit[13]; //tag.TTFDR in response_bit[14] and response_bit[15] @@ -839,9 +846,9 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr tag.LCON = response_bit[18]; tag.LKP = response_bit[19]; conf_pages[2] = ((response_bit[20] << 7) | (response_bit[21] << 6) - | (response_bit[22] << 5) | (response_bit[23] << 4) - | (response_bit[24] << 3) | (response_bit[25] << 2) - | (response_bit[26] << 1) | response_bit[27]); + | (response_bit[22] << 5) | (response_bit[23] << 4) + | (response_bit[24] << 3) | (response_bit[25] << 2) + | (response_bit[26] << 1) | response_bit[27]); tag.LCK7 = response_bit[20]; tag.LCK6 = response_bit[21]; tag.LCK5 = response_bit[22]; @@ -853,14 +860,14 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr if (DEBUG) Dbprintf("conf0: %02X conf1: %02X conf2: %02X", conf_pages[0], - conf_pages[1], conf_pages[2]); + conf_pages[1], conf_pages[2]); if (tag.auth == 1) { //if the tag is in authentication mode try the key or challenge *txlen = 64; - if(end!=true){ - if(htf==02||htf==04){ //RHTS_KEY //WHTS_KEY + if (end != true) { + if (htf == 02 || htf == 04) { //RHTS_KEY //WHTS_KEY state = hitag2_init(rev64(key), rev32(tag.uid), - rev32(rnd)); + rev32(rnd)); for (i = 0; i < 4; i++) { auth_ks[i] = hitag2_byte(&state) ^ 0xff; @@ -877,12 +884,12 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr tx[7] = auth_ks[3]; if (DEBUG) Dbprintf("%02X %02X %02X %02X %02X %02X %02X %02X", tx[0], - tx[1], tx[2], tx[3], tx[4], tx[5], tx[6], tx[7]); - } else if(htf==01 || htf==03) { //RHTS_CHALLENGE //WHTS_CHALLENGE + tx[1], tx[2], tx[3], tx[4], tx[5], tx[6], tx[7]); + } else if (htf == 01 || htf == 03) { //RHTS_CHALLENGE //WHTS_CHALLENGE for (i = 0; i < 8; i++) - tx[i]=((NrAr>>(56-(i*8)))&0xff); + tx[i] = ((NrAr >> (56 - (i * 8))) & 0xff); } - end=true; + end = true; tag.pstate = HT_AUTHENTICATE; } else { Dbprintf("authentication failed!"); @@ -906,29 +913,29 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr } //decrypt password - pwdh0=0; - pwdl0=0; - pwdl1=0; - if(htf==02 || htf==04){ //RHTS_KEY //WHTS_KEY - { - state = hitag2_init(rev64(key), rev32(tag.uid), rev32(rnd)); - for (i = 0; i < 5; i++) - hitag2_byte(&state); - pwdh0 = ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16)) - ^ hitag2_byte(&state); - pwdl0 = ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16)) - ^ hitag2_byte(&state); - pwdl1 = ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16)) - ^ hitag2_byte(&state); + pwdh0 = 0; + pwdl0 = 0; + pwdl1 = 0; + if (htf == 02 || htf == 04) { //RHTS_KEY //WHTS_KEY + { + state = hitag2_init(rev64(key), rev32(tag.uid), rev32(rnd)); + for (i = 0; i < 5; i++) + hitag2_byte(&state); + pwdh0 = ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16)) + ^ hitag2_byte(&state); + pwdl0 = ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16)) + ^ hitag2_byte(&state); + pwdl1 = ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16)) + ^ hitag2_byte(&state); + } + + if (DEBUG) + Dbprintf("pwdh0 %02X pwdl0 %02X pwdl1 %02X", pwdh0, pwdl0, pwdl1); + + + //Dbprintf("%X %02X", rnd, ((rx[4] & 0x0f) * 16) + ((rx[5] & 0xf0) / 16)); + //rnd += 1; } - - if (DEBUG) - Dbprintf("pwdh0 %02X pwdl0 %02X pwdl1 %02X", pwdh0, pwdl0, pwdl1); - - - //Dbprintf("%X %02X", rnd, ((rx[4] & 0x0f) * 16) + ((rx[5] & 0xf0) / 16)); - //rnd += 1; - } tag.pstate = HT_SELECTED; //tag is now ready for read/write commands } return 0; @@ -938,7 +945,8 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr /* * Emulates a Hitag S Tag with the given data from the .hts file */ -void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) { +void SimulateHitagSTag(bool tag_mem_supplied, byte_t *data) +{ int frame_count; int response; int overflow; @@ -948,10 +956,11 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) { //bool bQuitTraceFull = false; bQuiet = false; byte_t txbuf[HITAG_FRAME_LEN]; - byte_t* tx = txbuf; + byte_t *tx = txbuf; size_t txlen = 0; // free eventually allocated BigBuf memory - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); // Clean up trace and prepare it for storing frames set_tracing(true); @@ -968,58 +977,58 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) { //read tag data into memory if (tag_mem_supplied) { DbpString("Loading hitagS memory..."); - memcpy((byte_t*)tag.pages,data,4*64); + memcpy((byte_t *)tag.pages, data, 4 * 64); } - tag.uid=(uint32_t)tag.pages[0]; + tag.uid = (uint32_t)tag.pages[0]; Dbprintf("Hitag S simulation started"); - tag.key=(intptr_t)tag.pages[3]; - tag.key<<=16; - tag.key+=((tag.pages[2][0])<<8)+tag.pages[2][1]; - tag.pwdl0=tag.pages[2][3]; - tag.pwdl1=tag.pages[2][2]; - tag.pwdh0=tag.pages[1][0]; + tag.key = (intptr_t)tag.pages[3]; + tag.key <<= 16; + tag.key += ((tag.pages[2][0]) << 8) + tag.pages[2][1]; + tag.pwdl0 = tag.pages[2][3]; + tag.pwdl1 = tag.pages[2][2]; + tag.pwdh0 = tag.pages[1][0]; //con0 - tag.max_page=64; - if((tag.pages[1][3]&0x2)==0 && (tag.pages[1][3]&0x1)==1) - tag.max_page=8; - if((tag.pages[1][3]&0x2)==0 && (tag.pages[1][3]&0x1)==0) - tag.max_page=0; + tag.max_page = 64; + if ((tag.pages[1][3] & 0x2) == 0 && (tag.pages[1][3] & 0x1) == 1) + tag.max_page = 8; + if ((tag.pages[1][3] & 0x2) == 0 && (tag.pages[1][3] & 0x1) == 0) + tag.max_page = 0; //con1 - tag.auth=0; - if((tag.pages[1][2]&0x80) == 0x80) - tag.auth=1; - tag.LCON=0; - if((tag.pages[1][2]&0x2) == 0x02) - tag.LCON=1; - tag.LKP=0; - if((tag.pages[1][2]&0x1) == 0x01) - tag.LKP=1; + tag.auth = 0; + if ((tag.pages[1][2] & 0x80) == 0x80) + tag.auth = 1; + tag.LCON = 0; + if ((tag.pages[1][2] & 0x2) == 0x02) + tag.LCON = 1; + tag.LKP = 0; + if ((tag.pages[1][2] & 0x1) == 0x01) + tag.LKP = 1; //con2 //0=read write 1=read only - tag.LCK7=0; - if((tag.pages[1][1]&0x80) == 0x80) - tag.LCK7=1; - tag.LCK6=0; - if((tag.pages[1][1]&0x40) == 0x040) - tag.LCK6=1; - tag.LCK5=0; - if((tag.pages[1][1]&0x20) == 0x20) - tag.LCK5=1; - tag.LCK4=0; - if((tag.pages[1][1]&0x10) == 0x10) - tag.LCK4=1; - tag.LCK3=0; - if((tag.pages[1][1]&0x8) == 0x08) - tag.LCK3=1; - tag.LCK2=0; - if((tag.pages[1][1]&0x4) == 0x04) - tag.LCK2=1; - tag.LCK1=0; - if((tag.pages[1][1]&0x2) == 0x02) - tag.LCK1=1; - tag.LCK0=0; - if((tag.pages[1][1]&0x1) == 0x01) - tag.LCK0=1; + tag.LCK7 = 0; + if ((tag.pages[1][1] & 0x80) == 0x80) + tag.LCK7 = 1; + tag.LCK6 = 0; + if ((tag.pages[1][1] & 0x40) == 0x040) + tag.LCK6 = 1; + tag.LCK5 = 0; + if ((tag.pages[1][1] & 0x20) == 0x20) + tag.LCK5 = 1; + tag.LCK4 = 0; + if ((tag.pages[1][1] & 0x10) == 0x10) + tag.LCK4 = 1; + tag.LCK3 = 0; + if ((tag.pages[1][1] & 0x8) == 0x08) + tag.LCK3 = 1; + tag.LCK2 = 0; + if ((tag.pages[1][1] & 0x4) == 0x04) + tag.LCK2 = 1; + tag.LCK1 = 0; + if ((tag.pages[1][1] & 0x2) == 0x02) + tag.LCK1 = 1; + tag.LCK0 = 0; + if ((tag.pages[1][1] & 0x1) == 0x01) + tag.LCK0 = 1; // Set up simulator mode, frequency divisor which will drive the FPGA // and analog mux selection. @@ -1050,7 +1059,7 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) { // Capture mode, default timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger, // external trigger rising edge, load RA on rising edge of TIOA. AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK - | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING; + | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING; // Reset the received frame, frame count and timing info memset(rx, 0x00, sizeof(rx)); @@ -1163,7 +1172,8 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) { * If the key was given the password will be decrypted. * Reads every page of a hitag S transpoder. */ -void ReadHitagS(hitag_function htf, hitag_data* htd) { +void ReadHitagS(hitag_function htf, hitag_data *htd) +{ int i, j, z, k; int frame_count; int response_bit[200]; @@ -1171,7 +1181,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { byte_t rx[HITAG_FRAME_LEN]; size_t rxlen = 0; byte_t txbuf[HITAG_FRAME_LEN]; - byte_t* tx = txbuf; + byte_t *tx = txbuf; size_t txlen = 0; int lastbit; bool bSkip; @@ -1197,18 +1207,21 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { memcpy(NrAr_, htd->auth.NrAr, 8); Dbhexdump(8, NrAr_, false); NrAr = NrAr_[7] | ((uint64_t)NrAr_[6]) << 8 | ((uint64_t)NrAr_[5]) << 16 | ((uint64_t)NrAr_[4]) << 24 | ((uint64_t)NrAr_[3]) << 32 | - ((uint64_t)NrAr_[2]) << 40| ((uint64_t)NrAr_[1]) << 48 | ((uint64_t)NrAr_[0]) << 56; - } break; + ((uint64_t)NrAr_[2]) << 40 | ((uint64_t)NrAr_[1]) << 48 | ((uint64_t)NrAr_[0]) << 56; + } + break; case 02: { //RHTS_KEY DbpString("Authenticating using key:"); memcpy(key_, htd->crypto.key, 6); Dbhexdump(6, key_, false); key = key_[5] | ((uint64_t)key_[4]) << 8 | ((uint64_t)key_[3]) << 16 | ((uint64_t)key_[2]) << 24 | ((uint64_t)key_[1]) << 32 | ((uint64_t)key_[0]) << 40; - } break; + } + break; default: { - Dbprintf("Error , unknown function: %d",htf); + Dbprintf("Error , unknown function: %d", htf); return; - } break; + } + break; } FpgaDownloadAndGo(FPGA_BITSTREAM_LF); @@ -1230,7 +1243,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { // Set fpga in edge detect with reader field, we can modulate as reader now FpgaWriteConfWord( - FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD); + FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD); // Set Frequency divisor which will drive the FPGA and analog mux selection FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz @@ -1304,7 +1317,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { tag.pstate = HT_READY; tag.tstate = HT_NO_OP; } else if (tag.pstate != HT_SELECTED) { - if (hitagS_handle_tag_auth(htf, key,NrAr,rx, rxlen, tx, &txlen) == -1) + if (hitagS_handle_tag_auth(htf, key, NrAr, rx, rxlen, tx, &txlen) == -1) bStop = !false; } if (tag.pstate == HT_SELECTED && tag.tstate == HT_NO_OP && rxlen > 0) { @@ -1318,7 +1331,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { tx[1] = 0x00 + ((sendNum % 16) * 16) + (crc / 16); tx[2] = 0x00 + (crc % 16) * 16; } else if (tag.pstate == HT_SELECTED && tag.tstate == HT_READING_PAGE - && rxlen > 0) { + && rxlen > 0) { //save received data z = 0; for (i = 0; i < 5; i++) { @@ -1338,25 +1351,25 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { tag.pages[sendNum / 4][sendNum % 4] = 0x0; for (i = 0; i < 4; i++) { tag.pages[sendNum / 4][sendNum % 4] += ((pageData[i * 8] << 7) - | (pageData[1 + (i * 8)] << 6) - | (pageData[2 + (i * 8)] << 5) - | (pageData[3 + (i * 8)] << 4) - | (pageData[4 + (i * 8)] << 3) - | (pageData[5 + (i * 8)] << 2) - | (pageData[6 + (i * 8)] << 1) | pageData[7 + (i * 8)]) - << (i * 8); + | (pageData[1 + (i * 8)] << 6) + | (pageData[2 + (i * 8)] << 5) + | (pageData[3 + (i * 8)] << 4) + | (pageData[4 + (i * 8)] << 3) + | (pageData[5 + (i * 8)] << 2) + | (pageData[6 + (i * 8)] << 1) | pageData[7 + (i * 8)]) + << (i * 8); } if (tag.auth && tag.LKP && sendNum == 1) { Dbprintf("Page[%2d]: %02X %02X %02X %02X", sendNum, pwdh0, - (tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff, - (tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff, - tag.pages[sendNum / 4][sendNum % 4] & 0xff); + (tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff, + (tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff, + tag.pages[sendNum / 4][sendNum % 4] & 0xff); } else { Dbprintf("Page[%2d]: %02X %02X %02X %02X", sendNum, - (tag.pages[sendNum / 4][sendNum % 4] >> 24) & 0xff, - (tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff, - (tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff, - tag.pages[sendNum / 4][sendNum % 4] & 0xff); + (tag.pages[sendNum / 4][sendNum % 4] >> 24) & 0xff, + (tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff, + (tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff, + tag.pages[sendNum / 4][sendNum % 4] & 0xff); } sendNum++; @@ -1364,13 +1377,13 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { if (sendNum == 2 && tag.auth == 1 && tag.LKP) { if (htf == 02) { //RHTS_KEY Dbprintf("Page[ 2]: %02X %02X %02X %02X", - (byte_t)(key >> 8) & 0xff, - (byte_t) key & 0xff, pwdl1, pwdl0); + (byte_t)(key >> 8) & 0xff, + (byte_t) key & 0xff, pwdl1, pwdl0); Dbprintf("Page[ 3]: %02X %02X %02X %02X", - (byte_t)(key >> 40) & 0xff, - (byte_t)(key >> 32) & 0xff, - (byte_t)(key >> 24) & 0xff, - (byte_t)(key >> 16) & 0xff); + (byte_t)(key >> 40) & 0xff, + (byte_t)(key >> 32) & 0xff, + (byte_t)(key >> 24) & 0xff, + (byte_t)(key >> 16) & 0xff); } else { //if the authentication is done with a challenge the key and password are unknown Dbprintf("Page[ 2]: __ __ __ __"); @@ -1505,13 +1518,14 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) { * Authenticates to the Tag with the given Key or Challenge. * Writes the given 32Bit data into page_ */ -void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { +void WritePageHitagS(hitag_function htf, hitag_data *htd, int page_) +{ int frame_count; int response; byte_t rx[HITAG_FRAME_LEN]; size_t rxlen = 0; byte_t txbuf[HITAG_FRAME_LEN]; - byte_t* tx = txbuf; + byte_t *tx = txbuf; size_t txlen = 0; int lastbit; bool bSkip; @@ -1522,36 +1536,39 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { bool bQuitTraceFull = false; int page = page_; unsigned char crc; - byte_t data[4]= {0,0,0,0}; + byte_t data[4] = {0, 0, 0, 0}; //read given key/challenge, the page and the data byte_t NrAr_[8]; - uint64_t key=0; - uint64_t NrAr=0; + uint64_t key = 0; + uint64_t NrAr = 0; byte_t key_[6]; - switch(htf) { + switch (htf) { case 03: { //WHTS_CHALLENGE - memcpy(data,htd->auth.data,4); + memcpy(data, htd->auth.data, 4); DbpString("Authenticating using nr,ar pair:"); - memcpy(NrAr_,htd->auth.NrAr,8); - Dbhexdump(8,NrAr_,false); - NrAr=NrAr_[7] | ((uint64_t)NrAr_[6]) << 8 | ((uint64_t)NrAr_[5]) << 16 | ((uint64_t)NrAr_[4]) << 24 | ((uint64_t)NrAr_[3]) << 32 | - ((uint64_t)NrAr_[2]) << 40| ((uint64_t)NrAr_[1]) << 48 | ((uint64_t)NrAr_[0]) << 56; - } break; + memcpy(NrAr_, htd->auth.NrAr, 8); + Dbhexdump(8, NrAr_, false); + NrAr = NrAr_[7] | ((uint64_t)NrAr_[6]) << 8 | ((uint64_t)NrAr_[5]) << 16 | ((uint64_t)NrAr_[4]) << 24 | ((uint64_t)NrAr_[3]) << 32 | + ((uint64_t)NrAr_[2]) << 40 | ((uint64_t)NrAr_[1]) << 48 | ((uint64_t)NrAr_[0]) << 56; + } + break; case 04: { //WHTS_KEY - memcpy(data,htd->crypto.data,4); + memcpy(data, htd->crypto.data, 4); DbpString("Authenticating using key:"); - memcpy(key_,htd->crypto.key,6); - Dbhexdump(6,key_,false); - key=key_[5] | ((uint64_t)key_[4]) << 8 | ((uint64_t)key_[3]) << 16 | ((uint64_t)key_[2]) << 24 | ((uint64_t)key_[1]) << 32 | ((uint64_t)key_[0]) << 40; - } break; + memcpy(key_, htd->crypto.key, 6); + Dbhexdump(6, key_, false); + key = key_[5] | ((uint64_t)key_[4]) << 8 | ((uint64_t)key_[3]) << 16 | ((uint64_t)key_[2]) << 24 | ((uint64_t)key_[1]) << 32 | ((uint64_t)key_[0]) << 40; + } + break; default: { - Dbprintf("Error , unknown function: %d",htf); + Dbprintf("Error , unknown function: %d", htf); return; - } break; + } + break; } - Dbprintf("Page: %d",page_); + Dbprintf("Page: %d", page_); Dbprintf("DATA: %02X %02X %02X %02X", data[0], data[1], data[2], data[3]); FpgaDownloadAndGo(FPGA_BITSTREAM_LF); // Reset the return status @@ -1575,7 +1592,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { // Set fpga in edge detect with reader field, we can modulate as reader now FpgaWriteConfWord( - FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD); + FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD); // Set Frequency divisor which will drive the FPGA and analog mux selection FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz @@ -1601,8 +1618,8 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { // Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger, // external trigger rising edge, load RA on falling edge of TIOA. AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK - | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG - | AT91C_TC_LDRA_FALLING; + | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG + | AT91C_TC_LDRA_FALLING; // Enable and reset counters AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; @@ -1639,7 +1656,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { //check for valid input if (page == 0) { Dbprintf( - "usage: lf hitag writer [03 | 04] [CHALLENGE | KEY] [page] [byte0] [byte1] [byte2] [byte3]"); + "usage: lf hitag writer [03 | 04] [CHALLENGE | KEY] [page] [byte0] [byte1] [byte2] [byte3]"); bStop = !false; } @@ -1659,7 +1676,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { tag.tstate = HT_NO_OP; } else if (tag.pstate != HT_SELECTED) { //try to authenticate with the given key or challenge - if (hitagS_handle_tag_auth(htf,key,NrAr,rx, rxlen, tx, &txlen) == -1) + if (hitagS_handle_tag_auth(htf, key, NrAr, rx, rxlen, tx, &txlen) == -1) bStop = !false; } if (tag.pstate == HT_SELECTED && tag.tstate == HT_NO_OP && rxlen > 0) { @@ -1678,7 +1695,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { tx[1] = 0x00 + ((page % 16) * 16) + (crc / 16); tx[2] = 0x00 + (crc % 16) * 16; } else if (tag.pstate == HT_SELECTED && tag.tstate == HT_WRITING_PAGE_ACK - && rxlen == 6 && rx[0] == 0xf4) { + && rxlen == 6 && rx[0] == 0xf4) { //ACK recieved to write the page. send data tag.tstate = HT_WRITING_PAGE_DATA; txlen = 40; @@ -1693,7 +1710,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { tx[3] = data[0]; tx[4] = crc; } else if (tag.pstate == HT_SELECTED && tag.tstate == HT_WRITING_PAGE_DATA - && rxlen == 6 && rx[0] == 0xf4) { + && rxlen == 6 && rx[0] == 0xf4) { //received ACK Dbprintf("Successful!"); bStop = !false; @@ -1710,7 +1727,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { // All timer values are in terms of T0 units while (AT91C_BASE_TC0->TC_CV - < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) + < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) ; // Transmit the reader frame @@ -1802,7 +1819,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { } } } - end=false; + end = false; LED_B_OFF(); LED_D_OFF(); AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; @@ -1818,7 +1835,8 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) { * is not received correctly due to Antenna problems. This function * detects these challenges. */ -void check_challenges(bool file_given, byte_t* data) { +void check_challenges(bool file_given, byte_t *data) +{ int i, j, z, k; byte_t uid_byte[4]; int frame_count; @@ -1828,7 +1846,7 @@ void check_challenges(bool file_given, byte_t* data) { int u1 = 0; size_t rxlen = 0; byte_t txbuf[HITAG_FRAME_LEN]; - byte_t* tx = txbuf; + byte_t *tx = txbuf; size_t txlen = 0; int lastbit; bool bSkip; @@ -1889,7 +1907,7 @@ void check_challenges(bool file_given, byte_t* data) { // external trigger rising edge, load RA on falling edge of TIOA. AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK - | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING; + | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING; // Enable and reset counters AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; @@ -1906,7 +1924,7 @@ void check_challenges(bool file_given, byte_t* data) { if (file_given) { DbpString("Loading challenges..."); - memcpy((byte_t*)unlocker,data,60*8); + memcpy((byte_t *)unlocker, data, 60 * 8); } while (file_given && !bStop && !BUTTON_PRESS()) { @@ -1934,10 +1952,10 @@ void check_challenges(bool file_given, byte_t* data) { if (STATE == 2) // challenge failed Dbprintf("Challenge failed: %02X %02X %02X %02X %02X %02X %02X %02X", - unlocker[u1 - 1][0], unlocker[u1 - 1][1], - unlocker[u1 - 1][2], unlocker[u1 - 1][3], - unlocker[u1 - 1][4], unlocker[u1 - 1][5], - unlocker[u1 - 1][6], unlocker[u1 - 1][7]); + unlocker[u1 - 1][0], unlocker[u1 - 1][1], + unlocker[u1 - 1][2], unlocker[u1 - 1][3], + unlocker[u1 - 1][4], unlocker[u1 - 1][5], + unlocker[u1 - 1][6], unlocker[u1 - 1][7]); STATE = 0; txlen = 5; //start new authentication @@ -1961,17 +1979,17 @@ void check_challenges(bool file_given, byte_t* data) { break; } uid_byte[0] = (uid[0] << 7) | (uid[1] << 6) | (uid[2] << 5) - | (uid[3] << 4) | (uid[4] << 3) | (uid[5] << 2) - | (uid[6] << 1) | uid[7]; + | (uid[3] << 4) | (uid[4] << 3) | (uid[5] << 2) + | (uid[6] << 1) | uid[7]; uid_byte[1] = (uid[8] << 7) | (uid[9] << 6) | (uid[10] << 5) - | (uid[11] << 4) | (uid[12] << 3) | (uid[13] << 2) - | (uid[14] << 1) | uid[15]; + | (uid[11] << 4) | (uid[12] << 3) | (uid[13] << 2) + | (uid[14] << 1) | uid[15]; uid_byte[2] = (uid[16] << 7) | (uid[17] << 6) | (uid[18] << 5) - | (uid[19] << 4) | (uid[20] << 3) | (uid[21] << 2) - | (uid[22] << 1) | uid[23]; + | (uid[19] << 4) | (uid[20] << 3) | (uid[21] << 2) + | (uid[22] << 1) | uid[23]; uid_byte[3] = (uid[24] << 7) | (uid[25] << 6) | (uid[26] << 5) - | (uid[27] << 4) | (uid[28] << 3) | (uid[29] << 2) - | (uid[30] << 1) | uid[31]; + | (uid[27] << 4) | (uid[28] << 3) | (uid[29] << 2) + | (uid[30] << 1) | uid[31]; //Dbhexdump(10, rx, rxlen); STATE = 1; txlen = 45; @@ -2042,7 +2060,7 @@ void check_challenges(bool file_given, byte_t* data) { // All timer values are in terms of T0 units while (AT91C_BASE_TC0->TC_CV - < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) + < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) ; // Transmit the reader frame diff --git a/armsrc/iclass.c b/armsrc/iclass.c index 7b529a999..d5f908a12 100644 --- a/armsrc/iclass.c +++ b/armsrc/iclass.c @@ -61,7 +61,7 @@ int doIClassSimulation(int simulationMode, uint8_t *reader_mac_buf); // The length of a received command will in most cases be no more than 18 bytes. // 32 should be enough! #ifndef ICLASS_BUFFER_SIZE - #define ICLASS_BUFFER_SIZE 32 +#define ICLASS_BUFFER_SIZE 32 #endif #define AddCrc(data, len) compute_crc(CRC_ICLASS, (data), (len), (data)+(len), (data)+(len)+1) @@ -141,16 +141,19 @@ typedef struct { } tUart; static tUart Uart; -static void uart_reset(void){ +static void uart_reset(void) +{ Uart.frame_done = false; Uart.synced = false; Uart.frame = false; } -static void uart_init(uint8_t *data){ +static void uart_init(uint8_t *data) +{ Uart.buf = data; uart_reset(); } -static void uart_bit(uint8_t bit) { +static void uart_bit(uint8_t bit) +{ static uint8_t buf = 0xff; static uint8_t n_buf; static uint8_t msg_byte; @@ -173,13 +176,13 @@ static void uart_bit(uint8_t bit) { case 0xbf: // 0 - 1011 1111 break; case 0xef: // 1 - 1110 1111 - msg_byte |= (1<<6); + msg_byte |= (1 << 6); break; case 0xfb: // 2 - 1111 1011 - msg_byte |= (2<<6); + msg_byte |= (2 << 6); break; case 0xfe: // 3 - 1111 1110 - msg_byte |= (3<<6); + msg_byte |= (3 << 6); break; case 0xdf: // eof - 1101 1111 Uart.frame = false; @@ -205,7 +208,8 @@ static void uart_bit(uint8_t bit) { } } -static void uart_samples(uint8_t byte) { +static void uart_samples(uint8_t byte) +{ static uint32_t buf; static int window; static int drop_next = 0; @@ -426,23 +430,23 @@ static RAMFUNC int OutOfNDecoding(int bit) { } } */ - /*if (error) { - Uart.output[Uart.byteCnt] = 0xAA; - Uart.byteCnt++; - Uart.output[Uart.byteCnt] = error & 0xFF; - Uart.byteCnt++; - Uart.output[Uart.byteCnt] = 0xAA; - Uart.byteCnt++; - Uart.output[Uart.byteCnt] = (Uart.bitBuffer >> 8) & 0xFF; - Uart.byteCnt++; - Uart.output[Uart.byteCnt] = Uart.bitBuffer & 0xFF; - Uart.byteCnt++; - Uart.output[Uart.byteCnt] = (Uart.syncBit >> 3) & 0xFF; - Uart.byteCnt++; - Uart.output[Uart.byteCnt] = 0xAA; - Uart.byteCnt++; - return true; - }*/ +/*if (error) { + Uart.output[Uart.byteCnt] = 0xAA; + Uart.byteCnt++; + Uart.output[Uart.byteCnt] = error & 0xFF; + Uart.byteCnt++; + Uart.output[Uart.byteCnt] = 0xAA; + Uart.byteCnt++; + Uart.output[Uart.byteCnt] = (Uart.bitBuffer >> 8) & 0xFF; + Uart.byteCnt++; + Uart.output[Uart.byteCnt] = Uart.bitBuffer & 0xFF; + Uart.byteCnt++; + Uart.output[Uart.byteCnt] = (Uart.syncBit >> 3) & 0xFF; + Uart.byteCnt++; + Uart.output[Uart.byteCnt] = 0xAA; + Uart.byteCnt++; + return true; +}*/ /* } } else { @@ -500,7 +504,8 @@ static RAMFUNC int OutOfNDecoding(int bit) { // Manchester //============================================================================= static tDemod Demod; -static void DemodReset() { +static void DemodReset() +{ Demod.bitCount = 0; Demod.posCount = 0; Demod.syncBit = 0; @@ -514,7 +519,8 @@ static void DemodReset() { Demod.sub = SUB_NONE; Demod.state = DEMOD_UNSYNCD; } -static void DemodInit(uint8_t *data) { +static void DemodInit(uint8_t *data) +{ Demod.output = data; DemodReset(); } @@ -540,7 +546,8 @@ Recorded Activity (TraceLen = 162 bytes) 2720 | 2720 | Rdr |0c | | IDENTIFY 3232 | 3232 | Tag |bb! d4! bb! 02 02 08 04 bb! | ok | */ -static void uart_debug(int error, int bit) { +static void uart_debug(int error, int bit) +{ Demod.output[Demod.len] = 0xBB; Demod.len++; Demod.output[Demod.len] = error & 0xFF; @@ -576,7 +583,8 @@ static void uart_debug(int error, int bit) { * * So for current implementation in ISO15693, its 330 µs from end of reader, to start of card. */ -static RAMFUNC int ManchesterDecoding_iclass( uint32_t v) { +static RAMFUNC int ManchesterDecoding_iclass(uint32_t v) +{ int bit; int modulation; int error = 0; @@ -629,10 +637,18 @@ static RAMFUNC int ManchesterDecoding_iclass( uint32_t v) { if (Demod.posCount) { switch (Demod.syncBit) { - case 0x08: Demod.samples = 3; break; - case 0x04: Demod.samples = 2; break; - case 0x02: Demod.samples = 1; break; - case 0x01: Demod.samples = 0; break; + case 0x08: + Demod.samples = 3; + break; + case 0x04: + Demod.samples = 2; + break; + case 0x02: + Demod.samples = 1; + break; + case 0x01: + Demod.samples = 0; + break; } // SOF must be long burst... otherwise stay unsynced!!! if (!(Demod.buffer & Demod.syncBit) || !(Demod.buffer2 & Demod.syncBit)) @@ -790,7 +806,8 @@ static RAMFUNC int ManchesterDecoding_iclass( uint32_t v) { // Finally, a `sniffer' for iClass communication // Both sides of communication! //============================================================================= -static void iclass_setup_sniff(void){ +static void iclass_setup_sniff(void) +{ if (MF_DBGLEVEL > 3) Dbprintf("iclass_setup_sniff Enter"); LEDsoff(); @@ -805,7 +822,8 @@ static void iclass_setup_sniff(void){ // Set up the synchronous serial port FpgaSetupSsc(); - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); @@ -842,7 +860,8 @@ static void iclass_setup_sniff(void){ // near the reader. //----------------------------------------------------------------------------- // turn off afterwards -void RAMFUNC SniffIClass(void) { +void RAMFUNC SniffIClass(void) +{ //int datalen = 0; uint32_t previous_data = 0; @@ -860,7 +879,7 @@ void RAMFUNC SniffIClass(void) { uint8_t *data = dmaBuf; // Setup and start DMA. - if ( !FpgaSetupSscDma(dmaBuf, ICLASS_DMA_BUFFER_SIZE) ){ + if (!FpgaSetupSscDma(dmaBuf, ICLASS_DMA_BUFFER_SIZE)) { if (MF_DBGLEVEL > 1) DbpString("[-] FpgaSetupSscDma failed. Exiting"); return; } @@ -891,7 +910,7 @@ void RAMFUNC SniffIClass(void) { AT91C_BASE_PDC_SSC->PDC_RNCR = ICLASS_DMA_BUFFER_SIZE; } - if ( *data & 0xF) { + if (*data & 0xF) { //tag_byte <<= 1; tag_byte ^= (1 << 4); foo ^= (1 << (3 - div)); @@ -910,7 +929,7 @@ void RAMFUNC SniffIClass(void) { uart_samples(reader_byte); if (Uart.frame_done) { time_stop = GetCountSspClk() - time_0; - LogTrace( Uart.buf, Uart.len, time_start, time_stop, NULL, true); + LogTrace(Uart.buf, Uart.len, time_start, time_stop, NULL, true); DemodReset(); uart_reset(); } else { @@ -920,7 +939,7 @@ void RAMFUNC SniffIClass(void) { } } // every four sample - if ( (sniffCounter % 4) == 0) { + if ((sniffCounter % 4) == 0) { // need two samples to feed Manchester // no need to try decoding tag data if the reader is sending - and we cannot afford the time // CARD TO READER @@ -959,15 +978,16 @@ void RAMFUNC SniffIClass(void) { if (MF_DBGLEVEL >= 1) { DbpString("[+] Sniff statistics:"); Dbhexdump(ICLASS_DMA_BUFFER_SIZE, data, false); - } + } switch_off(); } -void rotateCSN(uint8_t* originalCSN, uint8_t* rotatedCSN) { +void rotateCSN(uint8_t *originalCSN, uint8_t *rotatedCSN) +{ int i; - for(i = 0; i < 8; i++) - rotatedCSN[i] = (originalCSN[i] >> 3) | (originalCSN[(i+1)%8] << 5); + for (i = 0; i < 8; i++) + rotatedCSN[i] = (originalCSN[i] >> 3) | (originalCSN[(i + 1) % 8] << 5); } //----------------------------------------------------------------------------- @@ -976,7 +996,8 @@ void rotateCSN(uint8_t* originalCSN, uint8_t* rotatedCSN) { // Stop when button is pressed // Or return TRUE when command is captured //----------------------------------------------------------------------------- -static bool GetIClassCommandFromReader(uint8_t *received, int *len, int maxLen) { +static bool GetIClassCommandFromReader(uint8_t *received, int *len, int maxLen) +{ // Set FPGA mode to "simulated ISO 14443 tag", no modulation (listen // only, since we are receiving, not transmitting). // Signal field is off with the appropriate LED @@ -990,7 +1011,7 @@ static bool GetIClassCommandFromReader(uint8_t *received, int *len, int maxLen) while (!BUTTON_PRESS()) { WDT_HIT(); - // keep tx buffer in a defined state anyway. + // keep tx buffer in a defined state anyway. if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) AT91C_BASE_SSC->SSC_THR = 0x00; @@ -1008,7 +1029,8 @@ static bool GetIClassCommandFromReader(uint8_t *received, int *len, int maxLen) return false; } -static uint8_t encode4Bits(const uint8_t b) { +static uint8_t encode4Bits(const uint8_t b) +{ // OTA, the least significant bits first // Manchester encoding added // The columns are @@ -1019,30 +1041,47 @@ static uint8_t encode4Bits(const uint8_t b) { uint8_t c = b & 0xF; switch (c) { - // 1 2 3 4 - case 15: return 0x55; // 1111 -> 1111 -> 01010101 -> 0x55 - case 14: return 0x95; // 1110 -> 0111 -> 10010101 -> 0x95 - case 13: return 0x65; // 1101 -> 1011 -> 01100101 -> 0x65 - case 12: return 0xa5; // 1100 -> 0011 -> 10100101 -> 0xa5 - case 11: return 0x59; // 1011 -> 1101 -> 01011001 -> 0x59 - case 10: return 0x99; // 1010 -> 0101 -> 10011001 -> 0x99 - case 9: return 0x69; // 1001 -> 1001 -> 01101001 -> 0x69 - case 8: return 0xa9; // 1000 -> 0001 -> 10101001 -> 0xa9 - case 7: return 0x56; // 0111 -> 1110 -> 01010110 -> 0x56 - case 6: return 0x96; // 0110 -> 0110 -> 10010110 -> 0x96 - case 5: return 0x66; // 0101 -> 1010 -> 01100110 -> 0x66 - case 4: return 0xa6; // 0100 -> 0010 -> 10100110 -> 0xa6 - case 3: return 0x5a; // 0011 -> 1100 -> 01011010 -> 0x5a - case 2: return 0x9a; // 0010 -> 0100 -> 10011010 -> 0x9a - case 1: return 0x6a; // 0001 -> 1000 -> 01101010 -> 0x6a - default: return 0xaa; // 0000 -> 0000 -> 10101010 -> 0xaa + // 1 2 3 4 + case 15: + return 0x55; // 1111 -> 1111 -> 01010101 -> 0x55 + case 14: + return 0x95; // 1110 -> 0111 -> 10010101 -> 0x95 + case 13: + return 0x65; // 1101 -> 1011 -> 01100101 -> 0x65 + case 12: + return 0xa5; // 1100 -> 0011 -> 10100101 -> 0xa5 + case 11: + return 0x59; // 1011 -> 1101 -> 01011001 -> 0x59 + case 10: + return 0x99; // 1010 -> 0101 -> 10011001 -> 0x99 + case 9: + return 0x69; // 1001 -> 1001 -> 01101001 -> 0x69 + case 8: + return 0xa9; // 1000 -> 0001 -> 10101001 -> 0xa9 + case 7: + return 0x56; // 0111 -> 1110 -> 01010110 -> 0x56 + case 6: + return 0x96; // 0110 -> 0110 -> 10010110 -> 0x96 + case 5: + return 0x66; // 0101 -> 1010 -> 01100110 -> 0x66 + case 4: + return 0xa6; // 0100 -> 0010 -> 10100110 -> 0xa6 + case 3: + return 0x5a; // 0011 -> 1100 -> 01011010 -> 0x5a + case 2: + return 0x9a; // 0010 -> 0100 -> 10011010 -> 0x9a + case 1: + return 0x6a; // 0001 -> 1000 -> 01101010 -> 0x6a + default: + return 0xaa; // 0000 -> 0000 -> 10101010 -> 0xaa } } //----------------------------------------------------------------------------- // Prepare tag messages //----------------------------------------------------------------------------- -static void CodeIClassTagAnswer(const uint8_t *cmd, int len) { +static void CodeIClassTagAnswer(const uint8_t *cmd, int len) +{ /* * SOF comprises 3 parts; * * An unmodulated time of 56.64 us @@ -1082,7 +1121,7 @@ static void CodeIClassTagAnswer(const uint8_t *cmd, int len) { ToSend[++ToSendMax] = 0x1D; int i; - for(i = 0; i < len; i++) { + for (i = 0; i < len; i++) { uint8_t b = cmd[i]; ToSend[++ToSendMax] = encode4Bits(b & 0xF); // least significant half ToSend[++ToSendMax] = encode4Bits((b >> 4) & 0xF); // most significant half @@ -1096,7 +1135,8 @@ static void CodeIClassTagAnswer(const uint8_t *cmd, int len) { } // Only SOF -static void CodeIClassTagSOF() { +static void CodeIClassTagSOF() +{ //So far a dummy implementation, not used //int lastProxToAirDuration =0; @@ -1122,7 +1162,8 @@ static void CodeIClassTagSOF() { * @param datain */ // turn off afterwards -void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) { +void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) +{ if (MF_DBGLEVEL > 3) Dbprintf("[+] iClass_simulate Enter"); @@ -1161,14 +1202,14 @@ void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain // In this mode, a number of csns are within datain. We'll simulate each one, one at a time // in order to collect MAC's from the reader. This can later be used in an offlne-attack // in order to obtain the keys, as in the "dismantling iclass"-paper. - #define EPURSE_MAC_SIZE 16 +#define EPURSE_MAC_SIZE 16 int i = 0; for (; i < numberOfCSNS && i * EPURSE_MAC_SIZE + 8 < USB_CMD_DATA_SIZE; i++) { // The usb data is 512 bytes, fitting 65 8-byte CSNs in there. - memcpy(emulator, datain + (i*8), 8); + memcpy(emulator, datain + (i * 8), 8); - if (doIClassSimulation(MODE_EXIT_AFTER_MAC, mac_responses+i * EPURSE_MAC_SIZE)) { + if (doIClassSimulation(MODE_EXIT_AFTER_MAC, mac_responses + i * EPURSE_MAC_SIZE)) { // Button pressed cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i, 0, mac_responses, i * EPURSE_MAC_SIZE); goto out; @@ -1176,11 +1217,11 @@ void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain } cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i, 0, mac_responses, i * EPURSE_MAC_SIZE); - } else if (simType == 3){ + } else if (simType == 3) { //This is 'full sim' mode, where we use the emulator storage for data. //ie: BigBuf_get_EM_addr should be previously filled with data from the "eload" command doIClassSimulation(MODE_FULLSIM, NULL); - } else if (simType == 4){ + } else if (simType == 4) { // This is the KEYROLL version of sim 2. // the collected data (mac_response) is doubled out since we are trying to collect both keys in the keyroll process. @@ -1197,24 +1238,24 @@ void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain // The usb data is 512 bytes, fitting 65 8-byte CSNs in there. iceman fork uses 9 CSNS for (; i < numberOfCSNS && i * EPURSE_MAC_SIZE + 8 < USB_CMD_DATA_SIZE; i++) { - memcpy(emulator, datain + (i*8), 8); + memcpy(emulator, datain + (i * 8), 8); // keyroll 1 - if (doIClassSimulation(MODE_EXIT_AFTER_MAC, mac_responses + i * EPURSE_MAC_SIZE )) { - cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i*2, 0, mac_responses, i * EPURSE_MAC_SIZE * 2); + if (doIClassSimulation(MODE_EXIT_AFTER_MAC, mac_responses + i * EPURSE_MAC_SIZE)) { + cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i * 2, 0, mac_responses, i * EPURSE_MAC_SIZE * 2); // Button pressed goto out; } // keyroll 2 - if (doIClassSimulation(MODE_EXIT_AFTER_MAC, mac_responses + (i + numberOfCSNS) * EPURSE_MAC_SIZE )) { - cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i*2, 0, mac_responses, i * EPURSE_MAC_SIZE* 2); + if (doIClassSimulation(MODE_EXIT_AFTER_MAC, mac_responses + (i + numberOfCSNS) * EPURSE_MAC_SIZE)) { + cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i * 2, 0, mac_responses, i * EPURSE_MAC_SIZE * 2); // Button pressed goto out; } } // double the amount of collected data. - cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i*2, 0, mac_responses, i * EPURSE_MAC_SIZE * 2 ); + cmd_send(CMD_ACK, CMD_SIMULATE_TAG_ICLASS, i * 2, 0, mac_responses, i * EPURSE_MAC_SIZE * 2); } else { // We may want a mode here where we hardcode the csns to use (from proxclone). @@ -1232,7 +1273,8 @@ out: * @param csn - csn to use * @param breakAfterMacReceived if true, returns after reader MAC has been received. */ -int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { +int doIClassSimulation(int simulationMode, uint8_t *reader_mac_buf) +{ // free eventually allocated BigBuf memory BigBuf_free_keep_EM(); @@ -1257,15 +1299,15 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { uint8_t diversified_key[8] = { 0 }; // e-Purse - uint8_t card_challenge_data[8] = { 0xfe,0xff,0xff,0xff,0xff,0xff,0xff,0xff }; + uint8_t card_challenge_data[8] = { 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; //uint8_t card_challenge_data[8] = { 0 }; if (simulationMode == MODE_FULLSIM) { //The diversified key should be stored on block 3 //Get the diversified key from emulator memory - memcpy(diversified_key, emulator+(8*3),8); + memcpy(diversified_key, emulator + (8 * 3), 8); //Card challenge, a.k.a e-purse is on block 2 - memcpy(card_challenge_data, emulator + (8 * 2) ,8); + memcpy(card_challenge_data, emulator + (8 * 2), 8); //Precalculate the cipher state, feeding it the CC cipher_state = opt_doTagMAC_1(card_challenge_data, diversified_key); } @@ -1284,7 +1326,7 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { uint8_t *modulated_response; int modulated_response_size = 0; - uint8_t* trace_data = NULL; + uint8_t *trace_data = NULL; int trace_data_size = 0; // Respond SOF -- takes 1 bytes @@ -1304,7 +1346,7 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { // configuration picopass 2ks uint8_t *resp_conf = BigBuf_malloc(28); int resp_conf_len; - uint8_t conf_data[10] = {0x12,0xFF,0xFF,0xFF,0x7F,0x1F,0xFF,0x3C,0x00,0x00}; + uint8_t conf_data[10] = {0x12, 0xFF, 0xFF, 0xFF, 0x7F, 0x1F, 0xFF, 0x3C, 0x00, 0x00}; AddCrc(conf_data, 8); // e-Purse @@ -1315,7 +1357,7 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { // Application Issuer Area uint8_t *resp_aia = BigBuf_malloc(28); int resp_aia_len; - uint8_t aia_data[10] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00}; + uint8_t aia_data[10] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00}; AddCrc(aia_data, 8); // receive command @@ -1327,27 +1369,33 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { // First card answer: SOF CodeIClassTagSOF(); - memcpy(resp_sof, ToSend, ToSendMax); resp_sof_Len = ToSendMax; + memcpy(resp_sof, ToSend, ToSendMax); + resp_sof_Len = ToSendMax; // Anticollision CSN CodeIClassTagAnswer(anticoll_data, sizeof(anticoll_data)); - memcpy(resp_anticoll, ToSend, ToSendMax); resp_anticoll_len = ToSendMax; + memcpy(resp_anticoll, ToSend, ToSendMax); + resp_anticoll_len = ToSendMax; // CSN CodeIClassTagAnswer(csn_data, sizeof(csn_data)); - memcpy(resp_csn, ToSend, ToSendMax); resp_csn_len = ToSendMax; + memcpy(resp_csn, ToSend, ToSendMax); + resp_csn_len = ToSendMax; // Configuration CodeIClassTagAnswer(conf_data, sizeof(conf_data)); - memcpy(resp_conf, ToSend, ToSendMax); resp_conf_len = ToSendMax; + memcpy(resp_conf, ToSend, ToSendMax); + resp_conf_len = ToSendMax; // e-Purse CodeIClassTagAnswer(card_challenge_data, sizeof(card_challenge_data)); - memcpy(resp_cc, ToSend, ToSendMax); resp_cc_len = ToSendMax; + memcpy(resp_cc, ToSend, ToSendMax); + resp_cc_len = ToSendMax; // Application Issuer Area CodeIClassTagAnswer(aia_data, sizeof(aia_data)); - memcpy(resp_aia, ToSend, ToSendMax); resp_aia_len = ToSendMax; + memcpy(resp_aia, ToSend, ToSendMax); + resp_aia_len = ToSendMax; //This is used for responding to READ-block commands or other data which is dynamically generated //First the 'trace'-data, not encoded for FPGA @@ -1355,7 +1403,7 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { //Then storage for the modulated data //Each bit is doubled when modulated for FPGA, and we also have SOF and EOF (2 bytes) - uint8_t *data_response = BigBuf_malloc( (8+2) * 2 + 2); + uint8_t *data_response = BigBuf_malloc((8 + 2) * 2 + 2); FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_LISTEN); SpinDelay(100); @@ -1373,7 +1421,8 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { WDT_HIT(); //Signal tracer, can be used to get a trigger for an oscilloscope.. - LED_B_OFF(); LED_C_OFF(); + LED_B_OFF(); + LED_C_OFF(); r2t_stime = (GetCountSspClk() - time_0) << 4; if (!GetIClassCommandFromReader(receivedCmd, &len, 0)) { @@ -1381,15 +1430,16 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { exitLoop = true; continue; } - r2t_etime = ((GetCountSspClk() - time_0) << 4 ) - r2t_stime; + r2t_etime = ((GetCountSspClk() - time_0) << 4) - r2t_stime; // 330us normal wait, adjusted for our execution LED_C_ON(); //Signal tracer - if (receivedCmd[0] == ICLASS_CMD_ACTALL ) { // 0x0A + if (receivedCmd[0] == ICLASS_CMD_ACTALL) { // 0x0A // Reader in anticollission phase - modulated_response = resp_sof; modulated_response_size = resp_sof_Len; //order = 1; + modulated_response = resp_sof; + modulated_response_size = resp_sof_Len; //order = 1; trace_data = sof_data; trace_data_size = sizeof(sof_data); // adjusted for 330 + (160*num of slot) @@ -1397,26 +1447,30 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { } else if (receivedCmd[0] == ICLASS_CMD_READ_OR_IDENTIFY) { // 0x0C if (len == 1) { // Reader asks for anticollission CSN - modulated_response = resp_anticoll; modulated_response_size = resp_anticoll_len; //order = 2; + modulated_response = resp_anticoll; + modulated_response_size = resp_anticoll_len; //order = 2; trace_data = anticoll_data; trace_data_size = sizeof(anticoll_data); goto send; } - if (len == 4){ + if (len == 4) { // block0,1,2,5 is always readable. - switch (receivedCmd[1]){ + switch (receivedCmd[1]) { case 0: // csn (0c 00) - modulated_response = resp_csn; modulated_response_size = resp_csn_len; + modulated_response = resp_csn; + modulated_response_size = resp_csn_len; trace_data = csn_data; trace_data_size = sizeof(csn_data); break; case 1: // configuration (0c 01) - modulated_response = resp_conf; modulated_response_size = resp_conf_len; + modulated_response = resp_conf; + modulated_response_size = resp_conf_len; trace_data = conf_data; trace_data_size = sizeof(conf_data); break; case 2: // e-purse (0c 02) - modulated_response = resp_cc; modulated_response_size = resp_cc_len; + modulated_response = resp_cc; + modulated_response_size = resp_cc_len; trace_data = card_challenge_data; trace_data_size = sizeof(card_challenge_data); // set epurse of sim2,4 attack @@ -1425,11 +1479,13 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { } break; case 5:// Application Issuer Area (0c 05) - modulated_response = resp_aia; modulated_response_size = resp_aia_len; + modulated_response = resp_aia; + modulated_response_size = resp_aia_len; trace_data = aia_data; trace_data_size = sizeof(aia_data); break; - default: break; + default: + break; } goto send; } @@ -1437,20 +1493,23 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { } else if (receivedCmd[0] == ICLASS_CMD_SELECT) { // 0x81 // Reader selects anticollission CSN. // Tag sends the corresponding real CSN - modulated_response = resp_csn; modulated_response_size = resp_csn_len; //order = 3; + modulated_response = resp_csn; + modulated_response_size = resp_csn_len; //order = 3; trace_data = csn_data; trace_data_size = sizeof(csn_data); goto send; } else if (receivedCmd[0] == ICLASS_CMD_READCHECK_KD) { // 0x88 // Read e-purse (88 02) - modulated_response = resp_cc; modulated_response_size = resp_cc_len; //order = 4; + modulated_response = resp_cc; + modulated_response_size = resp_cc_len; //order = 4; trace_data = card_challenge_data; trace_data_size = sizeof(card_challenge_data); LED_B_ON(); goto send; } else if (receivedCmd[0] == ICLASS_CMD_READCHECK_KC) { // 0x18 // Read e-purse (18 02) - modulated_response = resp_cc; modulated_response_size = resp_cc_len; //order = 4; + modulated_response = resp_cc; + modulated_response_size = resp_cc_len; //order = 4; trace_data = card_challenge_data; trace_data_size = sizeof(card_challenge_data); LED_B_ON(); @@ -1459,34 +1518,35 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { // Reader random and reader MAC!!! if (simulationMode == MODE_FULLSIM) { // NR, from reader, is in receivedCmd +1 - opt_doTagMAC_2(cipher_state, receivedCmd+1, data_generic_trace, diversified_key); + opt_doTagMAC_2(cipher_state, receivedCmd + 1, data_generic_trace, diversified_key); trace_data = data_generic_trace; trace_data_size = 4; - CodeIClassTagAnswer(trace_data , trace_data_size); + CodeIClassTagAnswer(trace_data, trace_data_size); memcpy(data_response, ToSend, ToSendMax); modulated_response = data_response; modulated_response_size = ToSendMax; } else { // Not fullsim, we don't respond // We do not know what to answer, so lets keep quiet - modulated_response = resp_sof; modulated_response_size = 0; + modulated_response = resp_sof; + modulated_response_size = 0; trace_data = NULL; trace_data_size = 0; if (simulationMode == MODE_EXIT_AFTER_MAC) { - if ( MF_DBGLEVEL == MF_DBG_EXTENDED) { + if (MF_DBGLEVEL == MF_DBG_EXTENDED) { Dbprintf("[+] CSN: %02x %02x %02x %02x %02x %02x %02x %02x", csn[0], csn[1], csn[2], csn[3], csn[4], csn[5], csn[6], csn[7]); Dbprintf("[+] RDR: (len=%02d): %02x %02x %02x %02x %02x %02x %02x %02x %02x", len, - receivedCmd[0], receivedCmd[1], receivedCmd[2], - receivedCmd[3], receivedCmd[4], receivedCmd[5], - receivedCmd[6], receivedCmd[7], receivedCmd[8]); + receivedCmd[0], receivedCmd[1], receivedCmd[2], + receivedCmd[3], receivedCmd[4], receivedCmd[5], + receivedCmd[6], receivedCmd[7], receivedCmd[8]); } else { Dbprintf("[+] CSN: %02x .... %02x OK", csn[0], csn[7]); } if (reader_mac_buf != NULL) { - memcpy(reader_mac_buf + 8, receivedCmd+1, 8); + memcpy(reader_mac_buf + 8, receivedCmd + 1, 8); } exitLoop = true; } @@ -1494,19 +1554,20 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { goto send; } else if (receivedCmd[0] == ICLASS_CMD_HALT && len == 1) { // Reader ends the session - modulated_response = resp_sof; modulated_response_size = 0; //order = 0; + modulated_response = resp_sof; + modulated_response_size = 0; //order = 0; trace_data = NULL; trace_data_size = 0; goto send; - } else if (simulationMode == MODE_FULLSIM && receivedCmd[0] == ICLASS_CMD_READ_OR_IDENTIFY && len == 4){ // 0x0C + } else if (simulationMode == MODE_FULLSIM && receivedCmd[0] == ICLASS_CMD_READ_OR_IDENTIFY && len == 4) { // 0x0C //Read block uint16_t blk = receivedCmd[1]; //Take the data... - memcpy(data_generic_trace, emulator+(blk << 3),8); + memcpy(data_generic_trace, emulator + (blk << 3), 8); AddCrc(data_generic_trace, 8); trace_data = data_generic_trace; trace_data_size = 10; - CodeIClassTagAnswer(trace_data , trace_data_size); + CodeIClassTagAnswer(trace_data, trace_data_size); memcpy(data_response, ToSend, ToSendMax); modulated_response = data_response; modulated_response_size = ToSendMax; @@ -1519,7 +1580,7 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { //receivedcmd is now UPDATE 1b | ADDRESS 1b| DATA 8b| Signature 4b or CRC 2b| //Take the data... - memcpy(data_generic_trace, receivedCmd+2, 8); + memcpy(data_generic_trace, receivedCmd + 2, 8); AddCrc(data_generic_trace, 8); trace_data = data_generic_trace; trace_data_size = 10; @@ -1540,7 +1601,7 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf) { } else { //#db# Unknown command received from reader (len=5): 26 1 0 f6 a 44 44 44 44 // Never seen this command before - if ( MF_DBGLEVEL == MF_DBG_EXTENDED) + if (MF_DBGLEVEL == MF_DBG_EXTENDED) print_result("[-] Unhandled command received ", receivedCmd, len); // Do not respond @@ -1557,7 +1618,7 @@ send: if (modulated_response_size > 0) { t2r_stime = (GetCountSspClk() - time_0) << 4; SendIClassAnswer(modulated_response, modulated_response_size, 0); - t2r_etime = ((GetCountSspClk() - time_0) << 4 ) - t2r_stime; + t2r_etime = ((GetCountSspClk() - time_0) << 4) - t2r_stime; } LogTrace(receivedCmd, len, r2t_stime, r2t_etime, NULL, true); @@ -1580,7 +1641,8 @@ send: * @param respLen * @param delay */ -static int SendIClassAnswer(uint8_t *resp, int respLen, uint16_t delay) { +static int SendIClassAnswer(uint8_t *resp, int respLen, uint16_t delay) +{ int i = 0; volatile uint8_t b = 0; @@ -1591,14 +1653,15 @@ static int SendIClassAnswer(uint8_t *resp, int respLen, uint16_t delay) { while (!BUTTON_PRESS()) { // Prevent rx holding register from overflowing - if ( (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)){ - b = AT91C_BASE_SSC->SSC_RHR; (void) b; + if ((AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)) { + b = AT91C_BASE_SSC->SSC_RHR; + (void) b; } // Put byte into tx holding register as soon as it is ready - if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)){ + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { b = 0x00; - if ( i < respLen){ + if (i < respLen) { b = resp[i]; //Hack //b = 0xAC; @@ -1617,7 +1680,8 @@ static int SendIClassAnswer(uint8_t *resp, int respLen, uint16_t delay) { //----------------------------------------------------------------------------- // Transmit the command (to the tag) that was placed in ToSend[]. //----------------------------------------------------------------------------- -static void TransmitIClassCommand(const uint8_t *cmd, int len, int *samples, int *wait) { +static void TransmitIClassCommand(const uint8_t *cmd, int len, int *samples, int *wait) +{ int c = 0; volatile uint32_t b; @@ -1627,8 +1691,8 @@ static void TransmitIClassCommand(const uint8_t *cmd, int len, int *samples, int FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD); AT91C_BASE_SSC->SSC_THR = 0x00; - // make sure we timeout previous comms. - if ( *wait ) + // make sure we timeout previous comms. + if (*wait) SpinDelayUs(*wait); for (;;) { @@ -1657,7 +1721,8 @@ static void TransmitIClassCommand(const uint8_t *cmd, int len, int *samples, int // Prevent rx holding register from overflowing if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { - b = AT91C_BASE_SSC->SSC_RHR; (void)b; + b = AT91C_BASE_SSC->SSC_RHR; + (void)b; } } @@ -1672,7 +1737,8 @@ static void TransmitIClassCommand(const uint8_t *cmd, int len, int *samples, int //----------------------------------------------------------------------------- // Prepare iClass reader command to send to FPGA //----------------------------------------------------------------------------- -void CodeIClassCommand(const uint8_t* cmd, int len) { +void CodeIClassCommand(const uint8_t *cmd, int len) +{ int i, j, k; uint8_t b; @@ -1709,7 +1775,8 @@ void CodeIClassCommand(const uint8_t* cmd, int len) { ToSendMax++; } -void ReaderTransmitIClass_ext(uint8_t* frame, int len, int wait) { +void ReaderTransmitIClass_ext(uint8_t *frame, int len, int wait) +{ int samples = 0; @@ -1725,7 +1792,8 @@ void ReaderTransmitIClass_ext(uint8_t* frame, int len, int wait) { LogTrace(frame, len, rsamples, rsamples, NULL, true); } -void ReaderTransmitIClass(uint8_t* frame, int len) { +void ReaderTransmitIClass(uint8_t *frame, int len) +{ ReaderTransmitIClass_ext(frame, len, 330); } @@ -1734,7 +1802,8 @@ void ReaderTransmitIClass(uint8_t* frame, int len) { // If a response is captured return TRUE // If it takes too long return FALSE //----------------------------------------------------------------------------- -static int GetIClassAnswer(uint8_t* receivedResponse, int maxLen, int *samples, int *elapsed) { +static int GetIClassAnswer(uint8_t *receivedResponse, int maxLen, int *samples, int *elapsed) +{ // buffer needs to be 512 bytes // maxLen is not used... @@ -1757,11 +1826,11 @@ static int GetIClassAnswer(uint8_t* receivedResponse, int maxLen, int *samples, while (!BUTTON_PRESS()) { WDT_HIT(); - // keep tx buffer in a defined state anyway. + // keep tx buffer in a defined state anyway. if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = 0x00; // To make use of exact timing of next command from reader!! - if (elapsed) (*elapsed)++; + if (elapsed)(*elapsed)++; } // Wait for byte be become available in rx holding register @@ -1785,7 +1854,8 @@ static int GetIClassAnswer(uint8_t* receivedResponse, int maxLen, int *samples, return false; } -int ReaderReceiveIClass(uint8_t* receivedAnswer) { +int ReaderReceiveIClass(uint8_t *receivedAnswer) +{ int samples = 0; if (!GetIClassAnswer(receivedAnswer, 0, &samples, NULL)) @@ -1801,7 +1871,8 @@ int ReaderReceiveIClass(uint8_t* receivedAnswer) { return Demod.len; } -void setupIclassReader() { +void setupIclassReader() +{ LEDsoff(); @@ -1828,7 +1899,8 @@ void setupIclassReader() { LED_A_ON(); } -bool sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t* resp, uint8_t expected_size, uint8_t retries) { +bool sendCmdGetResponseWithRetries(uint8_t *command, size_t cmdsize, uint8_t *resp, uint8_t expected_size, uint8_t retries) +{ uint8_t got_n = 0; while (retries-- > 0) { @@ -1839,7 +1911,7 @@ bool sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t* re got_n = ReaderReceiveIClass(resp); // 0xBB is the internal debug separator byte.. - if ( expected_size != got_n|| (resp[0] == 0xBB || resp[7] == 0xBB || resp[2] == 0xBB)) { + if (expected_size != got_n || (resp[0] == 0xBB || resp[7] == 0xBB || resp[2] == 0xBB)) { //try again continue; } @@ -1857,7 +1929,8 @@ bool sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t* re * 1 = Got CSN * 2 = Got CSN and CC */ -uint8_t handshakeIclassTag_ext(uint8_t *card_data, bool use_credit_key) { +uint8_t handshakeIclassTag_ext(uint8_t *card_data, bool use_credit_key) +{ // act_all... static uint8_t act_all[] = { ICLASS_CMD_ACTALL }; @@ -1872,7 +1945,7 @@ uint8_t handshakeIclassTag_ext(uint8_t *card_data, bool use_credit_key) { uint8_t read_status = 0; // Send act_all - ReaderTransmitIClass_ext(act_all, 1, 330+160); + ReaderTransmitIClass_ext(act_all, 1, 330 + 160); // Card present? if (!ReaderReceiveIClass(resp)) return read_status;//Fail @@ -1903,29 +1976,31 @@ uint8_t handshakeIclassTag_ext(uint8_t *card_data, bool use_credit_key) { // Card selected, now read e-purse (cc) (block2) (only 8 bytes no CRC) // ReaderTransmitIClass(readcheck_cc, sizeof(readcheck_cc)); // if (ReaderReceiveIClass(resp) == 8) { - // //Save CC (e-purse) in response data - // memcpy(card_data+8, resp, 8); - // read_status++; + // //Save CC (e-purse) in response data + // memcpy(card_data+8, resp, 8); + // read_status++; // } bool isOK = sendCmdGetResponseWithRetries(readcheck_cc, sizeof(readcheck_cc), resp, 8, 3); if (!isOK) return read_status; //Save CC (e-purse) in response data - memcpy(card_data+8, resp, 8); + memcpy(card_data + 8, resp, 8); read_status++; return read_status; } -uint8_t handshakeIclassTag(uint8_t *card_data){ +uint8_t handshakeIclassTag(uint8_t *card_data) +{ return handshakeIclassTag_ext(card_data, false); } // Reader iClass Anticollission // turn off afterwards -void ReaderIClass(uint8_t arg0) { +void ReaderIClass(uint8_t arg0) +{ uint8_t card_data[6 * 8] = {0}; - uint8_t last_csn[8] = {0,0,0,0,0,0,0,0}; + uint8_t last_csn[8] = {0, 0, 0, 0, 0, 0, 0, 0}; uint8_t resp[ICLASS_BUFFER_SIZE]; memset(card_data, 0xFF, sizeof(card_data)); @@ -1973,13 +2048,13 @@ void ReaderIClass(uint8_t arg0) { // handshakeIclass returns CSN|CC, but the actual block // layout is CSN|CONFIG|CC, so here we reorder the data, // moving CC forward 8 bytes - memcpy(card_data+16, card_data+8, 8); + memcpy(card_data + 16, card_data + 8, 8); //Read block 1, config if (flagReadConfig) { if (sendCmdGetResponseWithRetries(readConf, sizeof(readConf), resp, 10, 5)) { result_status |= FLAG_ICLASS_READER_CONF; - memcpy(card_data+8, resp, 8); + memcpy(card_data + 8, resp, 8); } else { if (MF_DBGLEVEL > 1) DbpString("Failed to dump config block"); } @@ -1989,7 +2064,7 @@ void ReaderIClass(uint8_t arg0) { if (flagReadAIA) { if (sendCmdGetResponseWithRetries(readAA, sizeof(readAA), resp, 10, 5)) { result_status |= FLAG_ICLASS_READER_AIA; - memcpy(card_data+(8*5), resp, 8); + memcpy(card_data + (8 * 5), resp, 8); } else { if (MF_DBGLEVEL > 1) DbpString("Failed to dump AA block"); } @@ -2012,34 +2087,34 @@ void ReaderIClass(uint8_t arg0) { // If caller requires that we get Conf, CC, AA, continue until we got it if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("STATUS %02X | CSN %c | CONF %c | CC %c | AIA %c | ONCE %c | 1TRY %c", - result_status, - (result_status & FLAG_ICLASS_READER_CSN) ? 'Y':'N', - (result_status & FLAG_ICLASS_READER_CONF)? 'Y':'N', - (result_status & FLAG_ICLASS_READER_CC) ? 'Y':'N', - (result_status & FLAG_ICLASS_READER_AIA) ? 'Y':'N' - ); + result_status, + (result_status & FLAG_ICLASS_READER_CSN) ? 'Y' : 'N', + (result_status & FLAG_ICLASS_READER_CONF) ? 'Y' : 'N', + (result_status & FLAG_ICLASS_READER_CC) ? 'Y' : 'N', + (result_status & FLAG_ICLASS_READER_AIA) ? 'Y' : 'N' + ); Dbprintf(" aar %c | to %c, | uc %c | frc %c | fra %c | cc %c", - abort_after_read ? 'Y':'N', - try_once ? 'Y':'N', - use_credit_key ? 'Y':'N', - flagReadConfig ? 'Y':'N', - flagReadAIA ? 'Y':'N', - flagReadCC ? 'Y':'N' - ); + abort_after_read ? 'Y' : 'N', + try_once ? 'Y' : 'N', + use_credit_key ? 'Y' : 'N', + flagReadConfig ? 'Y' : 'N', + flagReadAIA ? 'Y' : 'N', + flagReadCC ? 'Y' : 'N' + ); } - bool send = (result_status & FLAG_ICLASS_READER_CSN ); + bool send = (result_status & FLAG_ICLASS_READER_CSN); if (flagReadCC) - send |= (result_status & FLAG_ICLASS_READER_CC ); + send |= (result_status & FLAG_ICLASS_READER_CC); if (flagReadAIA) - send |= (result_status & FLAG_ICLASS_READER_AIA ); + send |= (result_status & FLAG_ICLASS_READER_AIA); if (flagReadConfig) - send |= (result_status & FLAG_ICLASS_READER_CONF ); + send |= (result_status & FLAG_ICLASS_READER_CONF); - if (MF_DBGLEVEL >= MF_DBG_EXTENDED) Dbprintf("SEND %c", send?'y':'n'); + if (MF_DBGLEVEL >= MF_DBG_EXTENDED) Dbprintf("SEND %c", send ? 'y' : 'n'); - if ( send ) { - cmd_send(CMD_ACK, result_status, 0, 0, card_data, sizeof(card_data) ); + if (send) { + cmd_send(CMD_ACK, result_status, 0, 0, card_data, sizeof(card_data)); if (abort_after_read) { LED_B_OFF(); return; @@ -2061,7 +2136,8 @@ void ReaderIClass(uint8_t arg0) { } // turn off afterwards -void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { +void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) +{ uint8_t cardsize = 0; uint8_t mem = 0; @@ -2070,12 +2146,12 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { uint8_t card_data[USB_CMD_DATA_SIZE] = {0}; uint8_t resp[ICLASS_BUFFER_SIZE] = {0}; - static struct memory_t{ - int k16; - int book; - int k2; - int lockauth; - int keyaccess; + static struct memory_t { + int k16; + int book; + int k2; + int lockauth; + int keyaccess; } memory; setupIclassReader(); @@ -2088,7 +2164,7 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { if (read_status < 2) continue; //for now replay captured auth (as cc not updated) - memcpy(check+5, MAC, 4); + memcpy(check + 5, MAC, 4); if (!sendCmdGetResponseWithRetries(check, sizeof(check), resp, 4, 5)) { DbpString("Error: Authentication Fail!"); @@ -2097,7 +2173,7 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { //first get configuration block (block 1) read[1] = 1; - AddCrc( read+1, 1 ); + AddCrc(read + 1, 1); if (!sendCmdGetResponseWithRetries(read, sizeof(read), resp, 10, 5)) { DbpString("Dump config (block 1) failed"); @@ -2120,17 +2196,17 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { uint32_t stored_data_length = 0; //then loop around remaining blocks - for ( uint16_t block=0; block < cardsize; block++) { + for (uint16_t block = 0; block < cardsize; block++) { read[1] = block; - AddCrc( read+1, 1 ); + AddCrc(read + 1, 1); if (sendCmdGetResponseWithRetries(read, sizeof(read), resp, 10, 5)) { Dbprintf(" %02x: %02x %02x %02x %02x %02x %02x %02x %02x", - block, resp[0], resp[1], resp[2], - resp[3], resp[4], resp[5], - resp[6], resp[7] - ); + block, resp[0], resp[1], resp[2], + resp[3], resp[4], resp[5], + resp[6], resp[7] + ); //Fill up the buffer memcpy(card_data + stored_data_length, resp, 8); @@ -2143,7 +2219,7 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { 0,//Not used ATM card_data, stored_data_length - ); + ); //reset stored_data_length = 0; failedRead = 0; @@ -2163,7 +2239,7 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { 0,//Not used ATM card_data, stored_data_length - ); + ); } //If we got here, let's break break; @@ -2175,24 +2251,26 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) { 0,//Not used ATM card_data, 0 - ); + ); switch_off(); } // not used. ?!? ( CMD_ICLASS_READCHECK) // turn off afterwards -void iClass_ReadCheck(uint8_t blockNo, uint8_t keyType) { +void iClass_ReadCheck(uint8_t blockNo, uint8_t keyType) +{ uint8_t readcheck[] = { keyType, blockNo }; - uint8_t resp[] = {0,0,0,0,0,0,0,0}; + uint8_t resp[] = {0, 0, 0, 0, 0, 0, 0, 0}; size_t isOK = 0; isOK = sendCmdGetResponseWithRetries(readcheck, sizeof(readcheck), resp, sizeof(resp), 6); - cmd_send(CMD_ACK,isOK,0,0,0,0); + cmd_send(CMD_ACK, isOK, 0, 0, 0, 0); switch_off(); } // used with function select_and_auth (cmdhficlass.c) // which needs to authenticate before doing more things like read/write -void iClass_Authentication(uint8_t *mac) { +void iClass_Authentication(uint8_t *mac) +{ uint8_t check[] = { ICLASS_CMD_CHECK, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; uint8_t resp[ICLASS_BUFFER_SIZE]; @@ -2205,7 +2283,7 @@ void iClass_Authentication(uint8_t *mac) { // 6 retries bool isOK = sendCmdGetResponseWithRetries(check, sizeof(check), resp, 4, 6); - cmd_send(CMD_ACK,isOK,0,0,0,0); + cmd_send(CMD_ACK, isOK, 0, 0, 0, 0); } typedef struct iclass_premac { @@ -2219,16 +2297,17 @@ typedef struct iclass_premac { * - key loop only test one type of authtication key. Ie two calls needed * to cover debit and credit key. (AA1/AA2) */ -void iClass_Authentication_fast(uint64_t arg0, uint64_t arg1, uint8_t *datain) { +void iClass_Authentication_fast(uint64_t arg0, uint64_t arg1, uint8_t *datain) +{ uint8_t i = 0, isOK = 0; uint8_t lastChunk = ((arg0 >> 8) & 0xFF); - bool use_credit_key =((arg0 >> 16) & 0xFF); + bool use_credit_key = ((arg0 >> 16) & 0xFF); uint8_t keyCount = arg1 & 0xFF; uint8_t check[] = { ICLASS_CMD_CHECK, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; uint8_t resp[ICLASS_BUFFER_SIZE]; uint8_t readcheck_cc[] = { ICLASS_CMD_READCHECK_KD, 0x02 }; - if (use_credit_key) + if (use_credit_key) readcheck_cc[0] = ICLASS_CMD_READCHECK_KC; // select card / e-purse @@ -2245,12 +2324,12 @@ void iClass_Authentication_fast(uint64_t arg0, uint64_t arg1, uint8_t *datain) { int read_status = 0; uint8_t startup_limit = 10; - while ( read_status != 2) { + while (read_status != 2) { if (BUTTON_PRESS() && !usb_poll_validate_length()) goto out; read_status = handshakeIclassTag_ext(card_data, use_credit_key); - if ( startup_limit-- == 0 ) { + if (startup_limit-- == 0) { Dbprintf("[-] Handshake status | %d (fail 10)", read_status); isOK = 99; goto out; @@ -2275,7 +2354,7 @@ void iClass_Authentication_fast(uint64_t arg0, uint64_t arg1, uint8_t *datain) { // expect 4bytes, 3 retries times.. isOK = sendCmdGetResponseWithRetries(check, sizeof(check), resp, 4, 3); - if ( isOK ) + if (isOK) goto out; SpinDelayUs(400); //iClass (iso15693-2) should timeout after 330us. @@ -2291,7 +2370,7 @@ out: // send keyindex. cmd_send(CMD_ACK, isOK, i, 0, 0, 0); - if ( isOK >= 1 || lastChunk ) { + if (isOK >= 1 || lastChunk) { switch_off(); LED_A_OFF(); } @@ -2302,10 +2381,11 @@ out: // Tries to read block. // retries 10times. -bool iClass_ReadBlock(uint8_t blockNo, uint8_t *data, uint8_t len) { +bool iClass_ReadBlock(uint8_t blockNo, uint8_t *data, uint8_t len) +{ uint8_t resp[10]; uint8_t cmd[] = {ICLASS_CMD_READ_OR_IDENTIFY, blockNo, 0x00, 0x00}; - AddCrc( cmd+1, 1 ); + AddCrc(cmd + 1, 1); // expect size 10, retry 5times bool isOK = sendCmdGetResponseWithRetries(cmd, sizeof(cmd), resp, 10, 5); memcpy(data, resp, len); @@ -2314,30 +2394,32 @@ bool iClass_ReadBlock(uint8_t blockNo, uint8_t *data, uint8_t len) { // turn off afterwards // readblock 8 + 2. only want 8. -void iClass_ReadBlk(uint8_t blockno) { - uint8_t data[] = {0,0,0,0,0,0,0,0,0,0}; +void iClass_ReadBlk(uint8_t blockno) +{ + uint8_t data[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; bool isOK = iClass_ReadBlock(blockno, data, sizeof(data)); cmd_send(CMD_ACK, isOK, 0, 0, data, sizeof(data)); switch_off(); } // turn off afterwards -void iClass_Dump(uint8_t blockno, uint8_t numblks) { - uint8_t blockdata[] = {0,0,0,0,0,0,0,0,0,0}; +void iClass_Dump(uint8_t blockno, uint8_t numblks) +{ + uint8_t blockdata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; bool isOK = false; uint8_t blkCnt = 0; BigBuf_free(); - uint8_t *dataout = BigBuf_malloc(255*8); - if (dataout == NULL){ + uint8_t *dataout = BigBuf_malloc(255 * 8); + if (dataout == NULL) { DbpString("[!] out of memory"); OnError(1); return; } // fill mem with 0xFF - memset(dataout, 0xFF, 255*8); + memset(dataout, 0xFF, 255 * 8); - for (;blkCnt < numblks; blkCnt++) { + for (; blkCnt < numblks; blkCnt++) { isOK = iClass_ReadBlock(blockno + blkCnt, blockdata, sizeof(blockdata)); // 0xBB is the internal debug separator byte.. @@ -2356,12 +2438,13 @@ void iClass_Dump(uint8_t blockno, uint8_t numblks) { BigBuf_free(); } -bool iClass_WriteBlock_ext(uint8_t blockNo, uint8_t *data) { +bool iClass_WriteBlock_ext(uint8_t blockNo, uint8_t *data) +{ - uint8_t resp[] = {0,0,0,0,0,0,0,0,0,0}; + uint8_t resp[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t write[] = { ICLASS_CMD_UPDATE, blockNo, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; - memcpy(write+2, data, 12); // data + mac - AddCrc(write+1, 13); + memcpy(write + 2, data, 12); // data + mac + AddCrc(write + 1, 13); bool isOK = sendCmdGetResponseWithRetries(write, sizeof(write), resp, sizeof(resp), 5); if (isOK) { //if reader responded correctly @@ -2379,23 +2462,25 @@ bool iClass_WriteBlock_ext(uint8_t blockNo, uint8_t *data) { } // turn off afterwards -void iClass_WriteBlock(uint8_t blockNo, uint8_t *data) { +void iClass_WriteBlock(uint8_t blockNo, uint8_t *data) +{ bool isOK = iClass_WriteBlock_ext(blockNo, data); - cmd_send(CMD_ACK,isOK,0,0,0,0); + cmd_send(CMD_ACK, isOK, 0, 0, 0, 0); switch_off(); } // turn off afterwards -void iClass_Clone(uint8_t startblock, uint8_t endblock, uint8_t *data) { +void iClass_Clone(uint8_t startblock, uint8_t endblock, uint8_t *data) +{ int i, written = 0; int total_block = (endblock - startblock) + 1; - for (i = 0; i < total_block; i++){ + for (i = 0; i < total_block; i++) { // block number - if (iClass_WriteBlock_ext(i + startblock, data + ( i*12 ) )){ + if (iClass_WriteBlock_ext(i + startblock, data + (i * 12))) { Dbprintf("Write block [%02x] successful", i + startblock); written++; } else { - if (iClass_WriteBlock_ext(i + startblock, data + ( i*12 ) )){ + if (iClass_WriteBlock_ext(i + startblock, data + (i * 12))) { Dbprintf("Write block [%02x] successful", i + startblock); written++; } else { @@ -2408,6 +2493,6 @@ void iClass_Clone(uint8_t startblock, uint8_t endblock, uint8_t *data) { else DbpString("Clone incomplete"); - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); switch_off(); } \ No newline at end of file diff --git a/armsrc/iso14443a.c b/armsrc/iso14443a.c index e44af5fe3..4469ca402 100644 --- a/armsrc/iso14443a.c +++ b/armsrc/iso14443a.c @@ -22,7 +22,7 @@ int rsamples = 0; uint8_t trigger = 0; // the block number for the ISO14443-4 PCB static uint8_t iso14_pcb_blocknum = 0; -static uint8_t* free_buffer_pointer; +static uint8_t *free_buffer_pointer; // // ISO14443 timing: @@ -120,29 +120,33 @@ static uint32_t LastProxToAirDuration; #define SEC_Y 0x00 #define SEC_Z 0xc0 -void iso14a_set_trigger(bool enable) { +void iso14a_set_trigger(bool enable) +{ trigger = enable; } -void iso14a_set_timeout(uint32_t timeout) { - iso14a_timeout = timeout + (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/(16*8) + 2; +void iso14a_set_timeout(uint32_t timeout) +{ + iso14a_timeout = timeout + (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER) / (16 * 8) + 2; } -uint32_t iso14a_get_timeout(void) { - return iso14a_timeout - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/(16*8) - 2; +uint32_t iso14a_get_timeout(void) +{ + return iso14a_timeout - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER) / (16 * 8) - 2; } //----------------------------------------------------------------------------- // Generate the parity value for a byte sequence //----------------------------------------------------------------------------- -void GetParity(const uint8_t *pbtCmd, uint16_t iLen, uint8_t *par) { +void GetParity(const uint8_t *pbtCmd, uint16_t iLen, uint8_t *par) +{ uint16_t paritybit_cnt = 0; uint16_t paritybyte_cnt = 0; uint8_t parityBits = 0; for (uint16_t i = 0; i < iLen; i++) { // Generate the parity bits - parityBits |= ((oddparity8(pbtCmd[i])) << (7-paritybit_cnt)); + parityBits |= ((oddparity8(pbtCmd[i])) << (7 - paritybit_cnt)); if (paritybit_cnt == 7) { par[paritybyte_cnt] = parityBits; // save 8 Bits parity parityBits = 0; // and advance to next Parity Byte @@ -189,11 +193,13 @@ const bool Mod_Miller_LUT[] = { #define IsMillerModulationNibble1(b) (Mod_Miller_LUT[(b & 0x000000F0) >> 4]) #define IsMillerModulationNibble2(b) (Mod_Miller_LUT[(b & 0x0000000F)]) -tUart* GetUart() { +tUart *GetUart() +{ return &Uart; } -void UartReset(void) { +void UartReset(void) +{ Uart.state = STATE_UNSYNCD; Uart.bitCount = 0; Uart.len = 0; // number of decoded data bytes @@ -207,18 +213,20 @@ void UartReset(void) { Uart.syncBit = 9999; } -void UartInit(uint8_t *data, uint8_t *parity) { +void UartInit(uint8_t *data, uint8_t *parity) +{ Uart.output = data; Uart.parity = parity; UartReset(); } // use parameter non_real_time to provide a timestamp. Set to 0 if the decoder should measure real time -RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time) { +RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time) +{ Uart.fourBits = (Uart.fourBits << 8) | bit; if (Uart.state == STATE_UNSYNCD) { // not yet synced - Uart.syncBit = 9999; // not set + Uart.syncBit = 9999; // not set // 00x11111 2|3 ticks pause followed by 6|5 ticks unmodulated Sequence Z (a "0" or "start of communication") // 11111111 8 ticks unmodulation Sequence Y (a "0" or "end of communication" or "no information") @@ -228,9 +236,9 @@ RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time) { // Sequence X followed by Sequence Y followed by Sequence Z (111100x1 11111111 00x11111) // we therefore look for a ...xx1111 11111111 00x11111xxxxxx... pattern // (12 '1's followed by 2 '0's, eventually followed by another '0', followed by 5 '1's) - #define ISO14443A_STARTBIT_MASK 0x07FFEF80 // mask is 00000111 11111111 11101111 10000000 - #define ISO14443A_STARTBIT_PATTERN 0x07FF8F80 // pattern is 00000111 11111111 10001111 10000000 - if ((Uart.fourBits & (ISO14443A_STARTBIT_MASK >> 0)) == ISO14443A_STARTBIT_PATTERN >> 0) Uart.syncBit = 7; +#define ISO14443A_STARTBIT_MASK 0x07FFEF80 // mask is 00000111 11111111 11101111 10000000 +#define ISO14443A_STARTBIT_PATTERN 0x07FF8F80 // pattern is 00000111 11111111 10001111 10000000 + if ((Uart.fourBits & (ISO14443A_STARTBIT_MASK >> 0)) == ISO14443A_STARTBIT_PATTERN >> 0) Uart.syncBit = 7; else if ((Uart.fourBits & (ISO14443A_STARTBIT_MASK >> 1)) == ISO14443A_STARTBIT_PATTERN >> 1) Uart.syncBit = 6; else if ((Uart.fourBits & (ISO14443A_STARTBIT_MASK >> 2)) == ISO14443A_STARTBIT_PATTERN >> 2) Uart.syncBit = 5; else if ((Uart.fourBits & (ISO14443A_STARTBIT_MASK >> 3)) == ISO14443A_STARTBIT_PATTERN >> 3) Uart.syncBit = 4; @@ -297,11 +305,11 @@ RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time) { Uart.shiftReg >>= (9 - Uart.bitCount); // right align them Uart.output[Uart.len++] = (Uart.shiftReg & 0xff); // add last byte to the output Uart.parityBits <<= 1; // add a (void) parity bit - Uart.parityBits <<= (8 - (Uart.len&0x0007)); // left align parity bits + Uart.parityBits <<= (8 - (Uart.len & 0x0007)); // left align parity bits Uart.parity[Uart.parityLen++] = Uart.parityBits; // and store it return true; } else if (Uart.len & 0x0007) { // there are some parity bits to store - Uart.parityBits <<= (8 - (Uart.len&0x0007)); // left align remaining parity bits + Uart.parityBits <<= (8 - (Uart.len & 0x0007)); // left align remaining parity bits Uart.parity[Uart.parityLen++] = Uart.parityBits; // and store them } if (Uart.len) { @@ -361,10 +369,12 @@ const bool Mod_Manchester_LUT[] = { #define IsManchesterModulationNibble1(b) (Mod_Manchester_LUT[(b & 0x00F0) >> 4]) #define IsManchesterModulationNibble2(b) (Mod_Manchester_LUT[(b & 0x000F)]) -tDemod* GetDemod() { +tDemod *GetDemod() +{ return &Demod; } -void DemodReset(void) { +void DemodReset(void) +{ Demod.state = DEMOD_UNSYNCD; Demod.len = 0; // number of decoded data bytes Demod.parityLen = 0; @@ -380,14 +390,16 @@ void DemodReset(void) { Demod.samples = 0; } -void DemodInit(uint8_t *data, uint8_t *parity) { +void DemodInit(uint8_t *data, uint8_t *parity) +{ Demod.output = data; Demod.parity = parity; DemodReset(); } // use parameter non_real_time to provide a timestamp. Set to 0 if the decoder should measure real time -RAMFUNC int ManchesterDecoding(uint8_t bit, uint16_t offset, uint32_t non_real_time) { +RAMFUNC int ManchesterDecoding(uint8_t bit, uint16_t offset, uint32_t non_real_time) +{ Demod.twoBits = (Demod.twoBits << 8) | bit; if (Demod.state == DEMOD_UNSYNCD) { @@ -400,7 +412,7 @@ RAMFUNC int ManchesterDecoding(uint8_t bit, uint16_t offset, uint32_t non_real_t } } else { Demod.syncBit = 0xFFFF; // not set - if ((Demod.twoBits & 0x7700) == 0x7000) Demod.syncBit = 7; + if ((Demod.twoBits & 0x7700) == 0x7000) Demod.syncBit = 7; else if ((Demod.twoBits & 0x3B80) == 0x3800) Demod.syncBit = 6; else if ((Demod.twoBits & 0x1DC0) == 0x1C00) Demod.syncBit = 5; else if ((Demod.twoBits & 0x0EE0) == 0x0E00) Demod.syncBit = 4; @@ -454,15 +466,15 @@ RAMFUNC int ManchesterDecoding(uint8_t bit, uint16_t offset, uint32_t non_real_t } Demod.endTime = Demod.startTime + 8 * (9 * Demod.len + Demod.bitCount + 1); } else { // no modulation in both halves - End of communication - if(Demod.bitCount > 0) { // there are some remaining data bits + if (Demod.bitCount > 0) { // there are some remaining data bits Demod.shiftReg >>= (9 - Demod.bitCount); // right align the decoded bits Demod.output[Demod.len++] = Demod.shiftReg & 0xff; // and add them to the output Demod.parityBits <<= 1; // add a (void) parity bit - Demod.parityBits <<= (8 - (Demod.len&0x0007)); // left align remaining parity bits + Demod.parityBits <<= (8 - (Demod.len & 0x0007)); // left align remaining parity bits Demod.parity[Demod.parityLen++] = Demod.parityBits; // and store them return true; } else if (Demod.len & 0x0007) { // there are some parity bits to store - Demod.parityBits <<= (8 - (Demod.len&0x0007)); // left align remaining parity bits + Demod.parityBits <<= (8 - (Demod.len & 0x0007)); // left align remaining parity bits Demod.parity[Demod.parityLen++] = Demod.parityBits; // and store them } if (Demod.len) { @@ -487,7 +499,8 @@ RAMFUNC int ManchesterDecoding(uint8_t bit, uint16_t offset, uint32_t non_real_t // near the reader. // "hf 14a sniff" //----------------------------------------------------------------------------- -void RAMFUNC SniffIso14443a(uint8_t param) { +void RAMFUNC SniffIso14443a(uint8_t param) +{ LEDsoff(); // param: // bit 0 - trigger from first card answer @@ -496,7 +509,8 @@ void RAMFUNC SniffIso14443a(uint8_t param) { // Allocate memory from BigBuf for some buffers // free all previous allocations first - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); @@ -525,7 +539,7 @@ void RAMFUNC SniffIso14443a(uint8_t param) { UartInit(receivedCmd, receivedCmdPar); // Setup and start DMA. - if ( !FpgaSetupSscDma((uint8_t*) dmaBuf, DMA_BUFFER_SIZE) ){ + if (!FpgaSetupSscDma((uint8_t *) dmaBuf, DMA_BUFFER_SIZE)) { if (MF_DBGLEVEL > 1) Dbprintf("FpgaSetupSscDma failed. Exiting"); return; } @@ -581,7 +595,7 @@ void RAMFUNC SniffIso14443a(uint8_t param) { if (!TagIsActive) { // no need to try decoding reader data if the tag is sending uint8_t readerdata = (previous_data & 0xF0) | (*data >> 4); - if (MillerDecoding(readerdata, (rsamples-1)*4)) { + if (MillerDecoding(readerdata, (rsamples - 1) * 4)) { LED_C_ON(); // check - if there is a short 7bit request from reader @@ -589,11 +603,11 @@ void RAMFUNC SniffIso14443a(uint8_t param) { if (triggered) { if (!LogTrace(receivedCmd, - Uart.len, - Uart.startTime*16 - DELAY_READER_AIR2ARM_AS_SNIFFER, - Uart.endTime*16 - DELAY_READER_AIR2ARM_AS_SNIFFER, - Uart.parity, - true)) break; + Uart.len, + Uart.startTime * 16 - DELAY_READER_AIR2ARM_AS_SNIFFER, + Uart.endTime * 16 - DELAY_READER_AIR2ARM_AS_SNIFFER, + Uart.parity, + true)) break; } /* ready to receive another command. */ UartReset(); @@ -608,15 +622,15 @@ void RAMFUNC SniffIso14443a(uint8_t param) { // no need to try decoding tag data if the reader is sending - and we cannot afford the time if (!ReaderIsActive) { uint8_t tagdata = (previous_data << 4) | (*data & 0x0F); - if (ManchesterDecoding(tagdata, 0, (rsamples-1)*4)) { + if (ManchesterDecoding(tagdata, 0, (rsamples - 1) * 4)) { LED_B_ON(); if (!LogTrace(receivedResp, - Demod.len, - Demod.startTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, - Demod.endTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, - Demod.parity, - false)) break; + Demod.len, + Demod.startTime * 16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, + Demod.endTime * 16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, + Demod.parity, + false)) break; if ((!triggered) && (param & 0x01)) triggered = true; @@ -649,7 +663,8 @@ void RAMFUNC SniffIso14443a(uint8_t param) { //----------------------------------------------------------------------------- // Prepare tag messages //----------------------------------------------------------------------------- -static void CodeIso14443aAsTagPar(const uint8_t *cmd, uint16_t len, uint8_t *parity, bool collision) { +static void CodeIso14443aAsTagPar(const uint8_t *cmd, uint16_t len, uint8_t *parity, bool collision) +{ //uint8_t localCol = 0; ToSendReset(); @@ -668,11 +683,11 @@ static void CodeIso14443aAsTagPar(const uint8_t *cmd, uint16_t len, uint8_t *par ToSend[++ToSendMax] = SEC_D; LastProxToAirDuration = 8 * ToSendMax - 4; - for(uint16_t i = 0; i < len; i++) { + for (uint16_t i = 0; i < len; i++) { uint8_t b = cmd[i]; // Data bits - for(uint16_t j = 0; j < 8; j++) { + for (uint16_t j = 0; j < 8; j++) { //if (collision && (localCol >= colpos)){ if (collision) { ToSend[++ToSendMax] = SEC_COLL; @@ -692,7 +707,7 @@ static void CodeIso14443aAsTagPar(const uint8_t *cmd, uint16_t len, uint8_t *par LastProxToAirDuration = 8 * ToSendMax; } else { // Get the parity bit - if (parity[i>>3] & (0x80>>(i&0x0007))) { + if (parity[i >> 3] & (0x80 >> (i & 0x0007))) { ToSend[++ToSendMax] = SEC_D; LastProxToAirDuration = 8 * ToSendMax - 4; } else { @@ -709,16 +724,19 @@ static void CodeIso14443aAsTagPar(const uint8_t *cmd, uint16_t len, uint8_t *par ToSendMax++; } -static void CodeIso14443aAsTagEx(const uint8_t *cmd, uint16_t len, bool collision) { +static void CodeIso14443aAsTagEx(const uint8_t *cmd, uint16_t len, bool collision) +{ uint8_t par[MAX_PARITY_SIZE] = {0}; GetParity(cmd, len, par); CodeIso14443aAsTagPar(cmd, len, par, collision); } -static void CodeIso14443aAsTag(const uint8_t *cmd, uint16_t len) { +static void CodeIso14443aAsTag(const uint8_t *cmd, uint16_t len) +{ CodeIso14443aAsTagEx(cmd, len, false); } -static void Code4bitAnswerAsTag(uint8_t cmd) { +static void Code4bitAnswerAsTag(uint8_t cmd) +{ uint8_t b = cmd; ToSendReset(); @@ -736,8 +754,8 @@ static void Code4bitAnswerAsTag(uint8_t cmd) { // Send startbit ToSend[++ToSendMax] = SEC_D; - for(uint8_t i = 0; i < 4; i++) { - if(b & 1) { + for (uint8_t i = 0; i < 4; i++) { + if (b & 1) { ToSend[++ToSendMax] = SEC_D; LastProxToAirDuration = 8 * ToSendMax - 4; } else { @@ -759,7 +777,8 @@ static void Code4bitAnswerAsTag(uint8_t cmd) { // stop when button is pressed // or return TRUE when command is captured //----------------------------------------------------------------------------- -int GetIso14443aCommandFromReader(uint8_t *received, uint8_t *parity, int *len) { +int GetIso14443aCommandFromReader(uint8_t *received, uint8_t *parity, int *len) +{ // Set FPGA mode to "simulated ISO 14443 tag", no modulation (listen // only, since we are receiving, not transmitting). // Signal field is off with the appropriate LED @@ -781,12 +800,13 @@ int GetIso14443aCommandFromReader(uint8_t *received, uint8_t *parity, int *len) *len = Uart.len; return true; } - } + } } return false; } -bool prepare_tag_modulation(tag_response_info_t* response_info, size_t max_buffer_size) { +bool prepare_tag_modulation(tag_response_info_t *response_info, size_t max_buffer_size) +{ // Example response, answer to MIFARE Classic read block will be 16 bytes + 2 CRC = 18 bytes // This will need the following byte array for a modulation sequence // 144 data bits (18 * 8) @@ -798,7 +818,7 @@ bool prepare_tag_modulation(tag_response_info_t* response_info, size_t max_buffe // 166 bytes, since every bit that needs to be send costs us a byte // // Prepare the tag modulation bits from the message - CodeIso14443aAsTag(response_info->response,response_info->response_n); + CodeIso14443aAsTag(response_info->response, response_info->response_n); // Make sure we do not exceed the free buffer space if (ToSendMax > max_buffer_size) { @@ -824,7 +844,8 @@ bool prepare_tag_modulation(tag_response_info_t* response_info, size_t max_buffe // 47 * 8 data bits, 47 * 1 parity bits, 10 start bits, 10 stop bits, 10 correction bits #define ALLOCATED_TAG_MODULATION_BUFFER_SIZE 453 -bool prepare_allocated_tag_modulation(tag_response_info_t* response_info) { +bool prepare_allocated_tag_modulation(tag_response_info_t *response_info) +{ // Retrieve and store the current buffer index response_info->modulation = free_buffer_pointer; @@ -846,21 +867,22 @@ bool prepare_allocated_tag_modulation(tag_response_info_t* response_info) { // response to send, and send it. // 'hf 14a sim' //----------------------------------------------------------------------------- -void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { +void SimulateIso14443aTag(int tagType, int flags, uint8_t *data) +{ - #define ATTACK_KEY_COUNT 8 // keep same as define in cmdhfmf.c -> readerAttack() +#define ATTACK_KEY_COUNT 8 // keep same as define in cmdhfmf.c -> readerAttack() uint8_t sak = 0; uint32_t cuid = 0; uint32_t nonce = 0; // PACK response to PWD AUTH for EV1/NTAG - uint8_t response8[4] = {0,0,0,0}; + uint8_t response8[4] = {0, 0, 0, 0}; // Counter for EV1/NTAG - uint32_t counters[] = {0,0,0}; + uint32_t counters[] = {0, 0, 0}; // The first response contains the ATQA (note: bytes are transmitted in reverse order). - uint8_t response1[] = {0,0}; + uint8_t response1[] = {0, 0}; // Here, we collect CUID, block1, keytype1, NT1, NR1, AR1, CUID, block2, keytyp2, NT2, NR2, AR2 // it should also collect block, keytype. @@ -876,29 +898,35 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { case 1: { // MIFARE Classic 1k response1[0] = 0x04; sak = 0x08; - } break; + } + break; case 2: { // MIFARE Ultralight response1[0] = 0x44; sak = 0x00; - } break; + } + break; case 3: { // MIFARE DESFire response1[0] = 0x04; response1[1] = 0x03; sak = 0x20; - } break; + } + break; case 4: { // ISO/IEC 14443-4 - javacard (JCOP) response1[0] = 0x04; sak = 0x28; - } break; + } + break; case 5: { // MIFARE TNP3XXX response1[0] = 0x01; response1[1] = 0x0f; sak = 0x01; - } break; + } + break; case 6: { // MIFARE Mini 320b response1[0] = 0x44; sak = 0x09; - } break; + } + break; case 7: { // NTAG response1[0] = 0x44; sak = 0x00; @@ -907,28 +935,31 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { response8[1] = 0x80; compute_crc(CRC_14443_A, response8, 2, &response8[2], &response8[3]); // uid not supplied then get from emulator memory - if (data[0]==0) { - uint16_t start = 4 * (0+12); + if (data[0] == 0) { + uint16_t start = 4 * (0 + 12); uint8_t emdata[8]; - emlGetMemBt( emdata, start, sizeof(emdata)); + emlGetMemBt(emdata, start, sizeof(emdata)); memcpy(data, emdata, 3); // uid bytes 0-2 - memcpy(data+3, emdata+4, 4); // uid bytes 3-7 + memcpy(data + 3, emdata + 4, 4); // uid bytes 3-7 flags |= FLAG_7B_UID_IN_DATA; } - } break; + } + break; case 8: { // MIFARE Classic 4k response1[0] = 0x02; sak = 0x18; - } break; + } + break; case 9 : { // FM11RF005SH (Shanghai Metro) response1[0] = 0x03; response1[1] = 0x00; sak = 0x0A; } default: { - Dbprintf("Error: unkown tagtype (%d)",tagType); + Dbprintf("Error: unkown tagtype (%d)", tagType); return; - } break; + } + break; } // The second response contains the (mandatory) first 24 bits of the UID @@ -937,7 +968,7 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { // For UID size 7, uint8_t response2a[5] = {0x00}; - if ( (flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA ) { + if ((flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA) { response2[0] = 0x88; // Cascade Tag marker response2[1] = data[0]; response2[2] = data[1]; @@ -953,7 +984,7 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { response1[0] |= 0x40; sak |= 0x04; - cuid = bytes_to_num(data+3, 4); + cuid = bytes_to_num(data + 3, 4); } else { memcpy(response2, data, 4); // Configure the ATQA and SAK accordingly @@ -991,7 +1022,7 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { // Prepare CHK_TEARING // uint8_t response9[] = {0xBD,0x90,0x3f}; - #define TAG_RESPONSE_COUNT 10 +#define TAG_RESPONSE_COUNT 10 tag_response_info_t responses[TAG_RESPONSE_COUNT] = { { .response = response1, .response_n = sizeof(response1) }, // Answer to request - respond with card type { .response = response2, .response_n = sizeof(response2) }, // Anticollision cascade1 - respond with uid @@ -1003,14 +1034,14 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { { .response = response8, .response_n = sizeof(response8) } // EV1/NTAG PACK response }; - // { .response = response7_NTAG, .response_n = sizeof(response7_NTAG)}, // EV1/NTAG GET_VERSION response - // { .response = response9, .response_n = sizeof(response9) } // EV1/NTAG CHK_TEAR response + // { .response = response7_NTAG, .response_n = sizeof(response7_NTAG)}, // EV1/NTAG GET_VERSION response + // { .response = response9, .response_n = sizeof(response9) } // EV1/NTAG CHK_TEAR response // Allocate 512 bytes for the dynamic modulation, created when the reader queries for it // Such a response is less time critical, so we can prepare them on the fly - #define DYNAMIC_RESPONSE_BUFFER_SIZE 64 - #define DYNAMIC_MODULATION_BUFFER_SIZE 512 +#define DYNAMIC_RESPONSE_BUFFER_SIZE 64 +#define DYNAMIC_MODULATION_BUFFER_SIZE 512 uint8_t dynamic_response_buffer[DYNAMIC_RESPONSE_BUFFER_SIZE]; uint8_t dynamic_modulation_buffer[DYNAMIC_MODULATION_BUFFER_SIZE]; tag_response_info_t dynamic_response_info = { @@ -1034,7 +1065,7 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { // Prepare the responses of the anticollision phase // there will be not enough time to do this at the moment the reader sends it REQA - for (size_t i=0; i 2) { // send NACK 0x0 == invalid argument uint8_t nack[] = {0x00}; - EmSendCmd(nack,sizeof(nack)); + EmSendCmd(nack, sizeof(nack)); } else { - uint8_t cmd[] = {0x00,0x00,0x00,0x14,0xa5}; + uint8_t cmd[] = {0x00, 0x00, 0x00, 0x14, 0xa5}; num_to_bytes(counters[index], 3, cmd); - AddCrc14A(cmd, sizeof(cmd)-2); - EmSendCmd(cmd,sizeof(cmd)); + AddCrc14A(cmd, sizeof(cmd) - 2); + EmSendCmd(cmd, sizeof(cmd)); } p_response = NULL; } else if (receivedCmd[0] == MIFARE_ULEV1_INCR_CNT && tagType == 7) { // Received a INC COUNTER -- uint8_t index = receivedCmd[1]; - if ( index > 2) { - // send NACK 0x0 == invalid argument + if (index > 2) { + // send NACK 0x0 == invalid argument uint8_t nack[] = {0x00}; - EmSendCmd(nack,sizeof(nack)); + EmSendCmd(nack, sizeof(nack)); } else { - uint32_t val = bytes_to_num(receivedCmd+2,4); + uint32_t val = bytes_to_num(receivedCmd + 2, 4); // if new value + old value is bigger 24bits, fail - if ( val + counters[index] > 0xFFFFFF ) { + if (val + counters[index] > 0xFFFFFF) { // send NACK 0x4 == counter overflow uint8_t nack[] = {0x04}; - EmSendCmd(nack,sizeof(nack)); + EmSendCmd(nack, sizeof(nack)); } else { counters[index] = val; - // send ACK - uint8_t ack[] = {0x0a}; - EmSendCmd(ack,sizeof(ack)); + // send ACK + uint8_t ack[] = {0x0a}; + EmSendCmd(ack, sizeof(ack)); } } p_response = NULL; @@ -1158,24 +1195,24 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { // first 12 blocks of emu are [getversion answer - check tearing - pack - 0x00 - signature] uint8_t emdata[3]; uint8_t index = receivedCmd[1]; - if ( index > 2) { - // send NACK 0x0 == invalid argument + if (index > 2) { + // send NACK 0x0 == invalid argument uint8_t nack[] = {0x00}; - EmSendCmd(nack,sizeof(nack)); + EmSendCmd(nack, sizeof(nack)); } else { - emlGetMemBt( emdata, 10+index, 1); - AddCrc14A(emdata, sizeof(emdata)-2); - EmSendCmd(emdata, sizeof(emdata)); + emlGetMemBt(emdata, 10 + index, 1); + AddCrc14A(emdata, sizeof(emdata) - 2); + EmSendCmd(emdata, sizeof(emdata)); } p_response = NULL; } else if (receivedCmd[0] == ISO14443A_CMD_HALT) { // Received a HALT - LogTrace(receivedCmd, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(receivedCmd, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); p_response = NULL; } else if (receivedCmd[0] == MIFARE_AUTH_KEYA || receivedCmd[0] == MIFARE_AUTH_KEYB) { // Received an authentication request - if ( tagType == 7 ) { // IF NTAG /EV1 0x60 == GET_VERSION, not a authentication request. + if (tagType == 7) { // IF NTAG /EV1 0x60 == GET_VERSION, not a authentication request. uint8_t emdata[10]; - emlGetMemBt( emdata, 0, 8 ); - AddCrc14A(emdata, sizeof(emdata)-2); + emlGetMemBt(emdata, 0, 8); + AddCrc14A(emdata, sizeof(emdata) - 2); EmSendCmd(emdata, sizeof(emdata)); p_response = NULL; } else { @@ -1184,7 +1221,7 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { cardAUTHSC = receivedCmd[1] / 4; // received block num // incease nonce at AUTH requests. this is time consuming. - nonce = prng_successor( GetTickCount(), 32 ); + nonce = prng_successor(GetTickCount(), 32); //num_to_bytes(nonce, 4, response5); num_to_bytes(nonce, 4, dynamic_response_info.response); dynamic_response_info.response_n = 4; @@ -1200,30 +1237,31 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { EmSend4bit(CARD_NACK_NA); p_response = NULL; } else { - p_response = &responses[6]; order = 70; + p_response = &responses[6]; + order = 70; } } else if (order == 7 && len == 8) { // Received {nr] and {ar} (part of authentication) - LogTrace(receivedCmd, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); - uint32_t nr = bytes_to_num(receivedCmd,4); - uint32_t ar = bytes_to_num(receivedCmd+4,4); + LogTrace(receivedCmd, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + uint32_t nr = bytes_to_num(receivedCmd, 4); + uint32_t ar = bytes_to_num(receivedCmd + 4, 4); // Collect AR/NR per keytype & sector - if ( (flags & FLAG_NR_AR_ATTACK) == FLAG_NR_AR_ATTACK ) { + if ((flags & FLAG_NR_AR_ATTACK) == FLAG_NR_AR_ATTACK) { int8_t index = -1; int8_t empty = -1; for (uint8_t i = 0; i < ATTACK_KEY_COUNT; i++) { // find which index to use - if ( (cardAUTHSC == ar_nr_nonces[i].sector) && (cardAUTHKEY == ar_nr_nonces[i].keytype)) + if ((cardAUTHSC == ar_nr_nonces[i].sector) && (cardAUTHKEY == ar_nr_nonces[i].keytype)) index = i; // keep track of empty slots. - if ( ar_nr_nonces[i].state == EMPTY) + if (ar_nr_nonces[i].state == EMPTY) empty = i; } // if no empty slots. Choose first and overwrite. - if ( index == -1 ) { - if ( empty == -1 ) { + if (index == -1) { + if (empty == -1) { index = 0; ar_nr_nonces[index].state = EMPTY; } else { @@ -1231,7 +1269,7 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { } } - switch(ar_nr_nonces[index].state) { + switch (ar_nr_nonces[index].state) { case EMPTY: { // first nonce collect ar_nr_nonces[index].cuid = cuid; @@ -1260,23 +1298,24 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { moebius_count++; break; } - default: break; + default: + break; } } p_response = NULL; - } else if (receivedCmd[0] == MIFARE_ULC_AUTH_1 ) { // ULC authentication, or Desfire Authentication + } else if (receivedCmd[0] == MIFARE_ULC_AUTH_1) { // ULC authentication, or Desfire Authentication } else if (receivedCmd[0] == MIFARE_ULEV1_AUTH) { // NTAG / EV-1 authentication - if ( tagType == 7 ) { + if (tagType == 7) { uint16_t start = 13; // first 4 blocks of emu are [getversion answer - check tearing - pack - 0x00] uint8_t emdata[4]; - emlGetMemBt( emdata, start, 2); + emlGetMemBt(emdata, start, 2); AddCrc14A(emdata, 2); EmSendCmd(emdata, sizeof(emdata)); p_response = NULL; - uint32_t pwd = bytes_to_num(receivedCmd+1,4); + uint32_t pwd = bytes_to_num(receivedCmd + 1, 4); - if ( MF_DBGLEVEL >= 3) Dbprintf("Auth attempt: %08x", pwd); + if (MF_DBGLEVEL >= 3) Dbprintf("Auth attempt: %08x", pwd); } } else { // Check for ISO 14443A-4 compliant commands, look at left nibble @@ -1287,49 +1326,56 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { dynamic_response_info.response[1] = 0x90; dynamic_response_info.response[2] = 0x00; dynamic_response_info.response_n = 3; - } break; + } + break; case 0x0B: case 0x0A: { // IBlock (command CID) - dynamic_response_info.response[0] = receivedCmd[0]; - dynamic_response_info.response[1] = 0x00; - dynamic_response_info.response[2] = 0x90; - dynamic_response_info.response[3] = 0x00; - dynamic_response_info.response_n = 4; - } break; + dynamic_response_info.response[0] = receivedCmd[0]; + dynamic_response_info.response[1] = 0x00; + dynamic_response_info.response[2] = 0x90; + dynamic_response_info.response[3] = 0x00; + dynamic_response_info.response_n = 4; + } + break; case 0x1A: case 0x1B: { // Chaining command - dynamic_response_info.response[0] = 0xaa | ((receivedCmd[0]) & 1); - dynamic_response_info.response_n = 2; - } break; + dynamic_response_info.response[0] = 0xaa | ((receivedCmd[0]) & 1); + dynamic_response_info.response_n = 2; + } + break; case 0xAA: case 0xBB: { - dynamic_response_info.response[0] = receivedCmd[0] ^ 0x11; - dynamic_response_info.response_n = 2; - } break; + dynamic_response_info.response[0] = receivedCmd[0] ^ 0x11; + dynamic_response_info.response_n = 2; + } + break; case 0xBA: { // ping / pong dynamic_response_info.response[0] = 0xAB; dynamic_response_info.response[1] = 0x00; dynamic_response_info.response_n = 2; - } break; + } + break; case 0xCA: case 0xC2: { // Readers sends deselect command dynamic_response_info.response[0] = 0xCA; dynamic_response_info.response[1] = 0x00; dynamic_response_info.response_n = 2; - } break; + } + break; default: { // Never seen this command before - LogTrace(receivedCmd, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); - Dbprintf("Received unknown command (len=%d):",len); - Dbhexdump(len,receivedCmd,false); + LogTrace(receivedCmd, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + Dbprintf("Received unknown command (len=%d):", len); + Dbhexdump(len, receivedCmd, false); // Do not respond dynamic_response_info.response_n = 0; - } break; + } + break; } if (dynamic_response_info.response_n > 0) { @@ -1340,9 +1386,9 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { AddCrc14A(dynamic_response_info.response, dynamic_response_info.response_n); dynamic_response_info.response_n += 2; - if (prepare_tag_modulation(&dynamic_response_info,DYNAMIC_MODULATION_BUFFER_SIZE) == false) { + if (prepare_tag_modulation(&dynamic_response_info, DYNAMIC_MODULATION_BUFFER_SIZE) == false) { DbpString("Error preparing tag response"); - LogTrace(receivedCmd, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(receivedCmd, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } p_response = &dynamic_response_info; @@ -1364,24 +1410,24 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { GetParity(p_response->response, p_response->response_n, par); EmLogTrace(Uart.output, - Uart.len, - Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, - Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, - Uart.parity, - p_response->response, - p_response->response_n, - LastTimeProxToAirStart*16 + DELAY_ARM2AIR_AS_TAG, - (LastTimeProxToAirStart + p_response->ProxToAirDuration)*16 + DELAY_ARM2AIR_AS_TAG, - par); + Uart.len, + Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, + Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, + Uart.parity, + p_response->response, + p_response->response_n, + LastTimeProxToAirStart * 16 + DELAY_ARM2AIR_AS_TAG, + (LastTimeProxToAirStart + p_response->ProxToAirDuration) * 16 + DELAY_ARM2AIR_AS_TAG, + par); } } - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); switch_off(); BigBuf_free_keep_EM(); - if (MF_DBGLEVEL >= 4){ + if (MF_DBGLEVEL >= 4) { Dbprintf("-[ Wake ups after halt [%d]", happened); Dbprintf("-[ Messages after halt [%d]", happened2); Dbprintf("-[ Num of received cmd [%d]", cmdsRecvd); @@ -1391,7 +1437,8 @@ void SimulateIso14443aTag(int tagType, int flags, uint8_t* data) { // prepare a delayed transfer. This simply shifts ToSend[] by a number // of bits specified in the delay parameter. -void PrepareDelayedTransfer(uint16_t delay) { +void PrepareDelayedTransfer(uint16_t delay) +{ delay &= 0x07; if (!delay) return; @@ -1406,12 +1453,12 @@ void PrepareDelayedTransfer(uint16_t delay) { ToSend[ToSendMax++] = 0x00; for (i = 0; i < ToSendMax; i++) { - bits_to_shift = ToSend[i] & bitmask; - ToSend[i] = ToSend[i] >> delay; - ToSend[i] = ToSend[i] | (bits_shifted << (8 - delay)); - bits_shifted = bits_to_shift; - } + bits_to_shift = ToSend[i] & bitmask; + ToSend[i] = ToSend[i] >> delay; + ToSend[i] = ToSend[i] | (bits_shifted << (8 - delay)); + bits_shifted = bits_to_shift; } +} //------------------------------------------------------------------------------------- @@ -1422,7 +1469,8 @@ void PrepareDelayedTransfer(uint16_t delay) { // if == 0: transfer immediately and return time of transfer // if != 0: delay transfer until time specified //------------------------------------------------------------------------------------- -static void TransmitFor14443a(const uint8_t *cmd, uint16_t len, uint32_t *timing) { +static void TransmitFor14443a(const uint8_t *cmd, uint16_t len, uint32_t *timing) +{ FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD); @@ -1432,7 +1480,7 @@ static void TransmitFor14443a(const uint8_t *cmd, uint16_t len, uint32_t *timing else PrepareDelayedTransfer(*timing & 0x00000007); // Delay transfer (fine tuning - up to 7 MF clock ticks) - if(MF_DBGLEVEL >= 4 && GetCountSspClk() >= (*timing & 0xfffffff8)) + if (MF_DBGLEVEL >= 4 && GetCountSspClk() >= (*timing & 0xfffffff8)) Dbprintf("TransmitFor14443a: Missed timing"); while (GetCountSspClk() < (*timing & 0xfffffff8)) {}; // Delay transfer (multiple of 8 MF clock ticks) LastTimeProxToAirStart = *timing; @@ -1452,12 +1500,13 @@ static void TransmitFor14443a(const uint8_t *cmd, uint16_t len, uint32_t *timing volatile uint8_t b; uint16_t c = 0; while (c < len) { - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = cmd[c++]; } //iceman test if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { - b = (uint16_t)(AT91C_BASE_SSC->SSC_RHR); (void)b; + b = (uint16_t)(AT91C_BASE_SSC->SSC_RHR); + (void)b; } } @@ -1467,7 +1516,8 @@ static void TransmitFor14443a(const uint8_t *cmd, uint16_t len, uint32_t *timing //----------------------------------------------------------------------------- // Prepare reader command (in bits, support short frames) to send to FPGA //----------------------------------------------------------------------------- -void CodeIso14443aBitsAsReaderPar(const uint8_t *cmd, uint16_t bits, const uint8_t *parity) { +void CodeIso14443aBitsAsReaderPar(const uint8_t *cmd, uint16_t bits, const uint8_t *parity) +{ int i, j; int last = 0; uint8_t b; @@ -1476,26 +1526,26 @@ void CodeIso14443aBitsAsReaderPar(const uint8_t *cmd, uint16_t bits, const uint8 // Start of Communication (Seq. Z) ToSend[++ToSendMax] = SEC_Z; - LastProxToAirDuration = 8 * (ToSendMax+1) - 6; + LastProxToAirDuration = 8 * (ToSendMax + 1) - 6; size_t bytecount = nbytes(bits); // Generate send structure for the data bits for (i = 0; i < bytecount; i++) { // Get the current byte to send b = cmd[i]; - size_t bitsleft = MIN((bits-(i*8)),8); + size_t bitsleft = MIN((bits - (i * 8)), 8); for (j = 0; j < bitsleft; j++) { if (b & 1) { // Sequence X ToSend[++ToSendMax] = SEC_X; - LastProxToAirDuration = 8 * (ToSendMax+1) - 2; + LastProxToAirDuration = 8 * (ToSendMax + 1) - 2; last = 1; } else { if (last == 0) { - // Sequence Z - ToSend[++ToSendMax] = SEC_Z; - LastProxToAirDuration = 8 * (ToSendMax+1) - 6; + // Sequence Z + ToSend[++ToSendMax] = SEC_Z; + LastProxToAirDuration = 8 * (ToSendMax + 1) - 6; } else { // Sequence Y ToSend[++ToSendMax] = SEC_Y; @@ -1508,16 +1558,16 @@ void CodeIso14443aBitsAsReaderPar(const uint8_t *cmd, uint16_t bits, const uint8 // Only transmit parity bit if we transmitted a complete byte if (j == 8 && parity != NULL) { // Get the parity bit - if (parity[i>>3] & (0x80 >> (i&0x0007))) { + if (parity[i >> 3] & (0x80 >> (i & 0x0007))) { // Sequence X ToSend[++ToSendMax] = SEC_X; - LastProxToAirDuration = 8 * (ToSendMax+1) - 2; + LastProxToAirDuration = 8 * (ToSendMax + 1) - 2; last = 1; } else { if (last == 0) { // Sequence Z ToSend[++ToSendMax] = SEC_Z; - LastProxToAirDuration = 8 * (ToSendMax+1) - 6; + LastProxToAirDuration = 8 * (ToSendMax + 1) - 6; } else { // Sequence Y ToSend[++ToSendMax] = SEC_Y; @@ -1531,7 +1581,7 @@ void CodeIso14443aBitsAsReaderPar(const uint8_t *cmd, uint16_t bits, const uint8 if (last == 0) { // Sequence Z ToSend[++ToSendMax] = SEC_Z; - LastProxToAirDuration = 8 * (ToSendMax+1) - 6; + LastProxToAirDuration = 8 * (ToSendMax + 1) - 6; } else { // Sequence Y ToSend[++ToSendMax] = SEC_Y; @@ -1546,8 +1596,9 @@ void CodeIso14443aBitsAsReaderPar(const uint8_t *cmd, uint16_t bits, const uint8 //----------------------------------------------------------------------------- // Prepare reader command to send to FPGA //----------------------------------------------------------------------------- -void CodeIso14443aAsReaderPar(const uint8_t *cmd, uint16_t len, const uint8_t *parity) { - CodeIso14443aBitsAsReaderPar(cmd, len*8, parity); +void CodeIso14443aAsReaderPar(const uint8_t *cmd, uint16_t len, const uint8_t *parity) +{ + CodeIso14443aBitsAsReaderPar(cmd, len * 8, parity); } //----------------------------------------------------------------------------- @@ -1555,7 +1606,8 @@ void CodeIso14443aAsReaderPar(const uint8_t *cmd, uint16_t len, const uint8_t *p // Stop when button is pressed (return 1) or field was gone (return 2) // Or return 0 when command is captured //----------------------------------------------------------------------------- -int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity) { +int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity) +{ *len = 0; uint32_t timer = 0, vtime = 0; @@ -1571,9 +1623,9 @@ int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity) { // Set ADC to read field strength AT91C_BASE_ADC->ADC_CR = AT91C_ADC_SWRST; AT91C_BASE_ADC->ADC_MR = - ADC_MODE_PRESCALE(63) | - ADC_MODE_STARTUP_TIME(1) | - ADC_MODE_SAMPLE_HOLD_TIME(15); + ADC_MODE_PRESCALE(63) | + ADC_MODE_STARTUP_TIME(1) | + ADC_MODE_SAMPLE_HOLD_TIME(15); AT91C_BASE_ADC->ADC_CHER = ADC_CHANNEL(ADC_CHAN_HF); // start ADC AT91C_BASE_ADC->ADC_CR = AT91C_ADC_START; @@ -1584,7 +1636,7 @@ int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity) { // Clear RXRDY: uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR; - for(;;) { + for (;;) { WDT_HIT(); if (BUTTON_PRESS()) return 1; @@ -1600,17 +1652,16 @@ int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity) { if (!timer) timer = vtime; // 50ms no field --> card to idle state if (vtime - timer > 50) return 2; - } else - if (timer) timer = 0; + } else if (timer) timer = 0; analogCnt = 0; analogAVG = 0; } } // receive and test the miller decoding - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { b = (uint8_t)AT91C_BASE_SSC->SSC_RHR; - if(MillerDecoding(b, 0)) { + if (MillerDecoding(b, 0)) { *len = Uart.len; return 0; } @@ -1618,7 +1669,8 @@ int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity) { } } -int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen) { +int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen) +{ volatile uint8_t b; uint16_t i = 0; uint32_t ThisTransferTime; @@ -1628,28 +1680,27 @@ int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen) { FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_MOD); // Include correction bit if necessary - if (Uart.bitCount == 7) - { + if (Uart.bitCount == 7) { // Short tags (7 bits) don't have parity, determine the correct value from MSB correctionNeeded = Uart.output[0] & 0x40; - } - else - { + } else { // The parity bits are left-aligned - correctionNeeded = Uart.parity[(Uart.len-1)/8] & (0x80 >> ((Uart.len-1) & 7)); + correctionNeeded = Uart.parity[(Uart.len - 1) / 8] & (0x80 >> ((Uart.len - 1) & 7)); } // 1236, so correction bit needed i = (correctionNeeded) ? 0 : 1; - // clear receiving shift register and holding register - while(!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)); - b = AT91C_BASE_SSC->SSC_RHR; (void) b; - while(!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)); - b = AT91C_BASE_SSC->SSC_RHR; (void) b; + // clear receiving shift register and holding register + while (!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)); + b = AT91C_BASE_SSC->SSC_RHR; + (void) b; + while (!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)); + b = AT91C_BASE_SSC->SSC_RHR; + (void) b; // wait for the FPGA to signal fdt_indicator == 1 (the FPGA is ready to queue new data in its delay line) for (uint8_t j = 0; j < 5; j++) { // allow timeout - better late than never - while(!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)); + while (!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)); if (AT91C_BASE_SSC->SSC_RHR) break; } @@ -1659,22 +1710,23 @@ int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen) { AT91C_BASE_SSC->SSC_THR = SEC_F; // send cycle - for(; i < respLen; ) { - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + for (; i < respLen;) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = resp[i++]; FpgaSendQueueDelay = (uint8_t)AT91C_BASE_SSC->SSC_RHR; } if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { - b = (uint16_t)(AT91C_BASE_SSC->SSC_RHR); (void)b; + b = (uint16_t)(AT91C_BASE_SSC->SSC_RHR); + (void)b; } - if(BUTTON_PRESS()) break; + if (BUTTON_PRESS()) break; } // Ensure that the FPGA Delay Queue is empty before we switch to TAGSIM_LISTEN again: uint8_t fpga_queued_bits = FpgaSendQueueDelay >> 3; - for (i = 0; i <= fpga_queued_bits/8 + 1; ) { - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + for (i = 0; i <= fpga_queued_bits / 8 + 1;) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = SEC_F; FpgaSendQueueDelay = (uint8_t)AT91C_BASE_SSC->SSC_RHR; i++; @@ -1684,68 +1736,73 @@ int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen) { return 0; } -int EmSend4bit(uint8_t resp){ +int EmSend4bit(uint8_t resp) +{ Code4bitAnswerAsTag(resp); int res = EmSendCmd14443aRaw(ToSend, ToSendMax); // do the tracing for the previous reader request and this tag answer: uint8_t par[1] = {0x00}; GetParity(&resp, 1, par); EmLogTrace(Uart.output, - Uart.len, - Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, - Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, - Uart.parity, - &resp, - 1, - LastTimeProxToAirStart*16 + DELAY_ARM2AIR_AS_TAG, - (LastTimeProxToAirStart + LastProxToAirDuration)*16 + DELAY_ARM2AIR_AS_TAG, - par); + Uart.len, + Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, + Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, + Uart.parity, + &resp, + 1, + LastTimeProxToAirStart * 16 + DELAY_ARM2AIR_AS_TAG, + (LastTimeProxToAirStart + LastProxToAirDuration) * 16 + DELAY_ARM2AIR_AS_TAG, + par); return res; } -int EmSendCmdPar(uint8_t *resp, uint16_t respLen, uint8_t *par) { +int EmSendCmdPar(uint8_t *resp, uint16_t respLen, uint8_t *par) +{ return EmSendCmdParEx(resp, respLen, par, false); } -int EmSendCmdParEx(uint8_t *resp, uint16_t respLen, uint8_t *par, bool collision){ +int EmSendCmdParEx(uint8_t *resp, uint16_t respLen, uint8_t *par, bool collision) +{ CodeIso14443aAsTagPar(resp, respLen, par, collision); int res = EmSendCmd14443aRaw(ToSend, ToSendMax); // do the tracing for the previous reader request and this tag answer: EmLogTrace(Uart.output, - Uart.len, - Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, - Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, - Uart.parity, - resp, - respLen, - LastTimeProxToAirStart*16 + DELAY_ARM2AIR_AS_TAG, - (LastTimeProxToAirStart + LastProxToAirDuration)*16 + DELAY_ARM2AIR_AS_TAG, - par); + Uart.len, + Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, + Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, + Uart.parity, + resp, + respLen, + LastTimeProxToAirStart * 16 + DELAY_ARM2AIR_AS_TAG, + (LastTimeProxToAirStart + LastProxToAirDuration) * 16 + DELAY_ARM2AIR_AS_TAG, + par); return res; } -int EmSendCmd(uint8_t *resp, uint16_t respLen){ +int EmSendCmd(uint8_t *resp, uint16_t respLen) +{ return EmSendCmdEx(resp, respLen, false); } -int EmSendCmdEx(uint8_t *resp, uint16_t respLen, bool collision){ +int EmSendCmdEx(uint8_t *resp, uint16_t respLen, bool collision) +{ uint8_t par[MAX_PARITY_SIZE] = {0x00}; GetParity(resp, respLen, par); return EmSendCmdParEx(resp, respLen, par, collision); } bool EmLogTrace(uint8_t *reader_data, uint16_t reader_len, uint32_t reader_StartTime, uint32_t reader_EndTime, uint8_t *reader_Parity, - uint8_t *tag_data, uint16_t tag_len, uint32_t tag_StartTime, uint32_t tag_EndTime, uint8_t *tag_Parity) + uint8_t *tag_data, uint16_t tag_len, uint32_t tag_StartTime, uint32_t tag_EndTime, uint8_t *tag_Parity) { // we cannot exactly measure the end and start of a received command from reader. However we know that the delay from // end of the received command to start of the tag's (simulated by us) answer is n*128+20 or n*128+84 resp. // with n >= 9. The start of the tags answer can be measured and therefore the end of the received command be calculated: uint16_t reader_modlen = reader_EndTime - reader_StartTime; uint16_t approx_fdt = tag_StartTime - reader_EndTime; - uint16_t exact_fdt = (approx_fdt - 20 + 32)/64 * 64 + 20; + uint16_t exact_fdt = (approx_fdt - 20 + 32) / 64 * 64 + 20; reader_EndTime = tag_StartTime - exact_fdt; reader_StartTime = reader_EndTime - reader_modlen; if (!LogTrace(reader_data, reader_len, reader_StartTime, reader_EndTime, reader_Parity, true)) return false; else - return(!LogTrace(tag_data, tag_len, tag_StartTime, tag_EndTime, tag_Parity, false)); + return (!LogTrace(tag_data, tag_len, tag_StartTime, tag_EndTime, tag_Parity, false)); } @@ -1754,7 +1811,8 @@ bool EmLogTrace(uint8_t *reader_data, uint16_t reader_len, uint32_t reader_Start // If a response is captured return TRUE // If it takes too long return FALSE //----------------------------------------------------------------------------- -static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, uint8_t *receivedResponsePar, uint16_t offset) { +static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, uint8_t *receivedResponsePar, uint16_t offset) +{ uint32_t c = 0; // Set FPGA mode to "reader listen mode", no modulation (listen @@ -1770,13 +1828,13 @@ static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, uint8_t *receive uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR; uint32_t timeout = iso14a_get_timeout(); - for(;;) { + for (;;) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { b = (uint8_t)AT91C_BASE_SSC->SSC_RHR; if (ManchesterDecoding(b, offset, 0)) { - NextTransferTime = MAX(NextTransferTime, Demod.endTime - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/16 + FRAME_DELAY_TIME_PICC_TO_PCD); + NextTransferTime = MAX(NextTransferTime, Demod.endTime - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER) / 16 + FRAME_DELAY_TIME_PICC_TO_PCD); return true; } else if (c++ > timeout && Demod.state == DEMOD_UNSYNCD) { return false; @@ -1785,52 +1843,59 @@ static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, uint8_t *receive } } -void ReaderTransmitBitsPar(uint8_t* frame, uint16_t bits, uint8_t *par, uint32_t *timing) { +void ReaderTransmitBitsPar(uint8_t *frame, uint16_t bits, uint8_t *par, uint32_t *timing) +{ CodeIso14443aBitsAsReaderPar(frame, bits, par); // Send command to tag TransmitFor14443a(ToSend, ToSendMax, timing); - if(trigger) LED_A_ON(); + if (trigger) LED_A_ON(); - LogTrace(frame, nbytes(bits), (LastTimeProxToAirStart<<4) + DELAY_ARM2AIR_AS_READER, ((LastTimeProxToAirStart + LastProxToAirDuration)<<4) + DELAY_ARM2AIR_AS_READER, par, true); + LogTrace(frame, nbytes(bits), (LastTimeProxToAirStart << 4) + DELAY_ARM2AIR_AS_READER, ((LastTimeProxToAirStart + LastProxToAirDuration) << 4) + DELAY_ARM2AIR_AS_READER, par, true); } -void ReaderTransmitPar(uint8_t* frame, uint16_t len, uint8_t *par, uint32_t *timing) { - ReaderTransmitBitsPar(frame, len*8, par, timing); +void ReaderTransmitPar(uint8_t *frame, uint16_t len, uint8_t *par, uint32_t *timing) +{ + ReaderTransmitBitsPar(frame, len * 8, par, timing); } -void ReaderTransmitBits(uint8_t* frame, uint16_t len, uint32_t *timing) { +void ReaderTransmitBits(uint8_t *frame, uint16_t len, uint32_t *timing) +{ // Generate parity and redirect uint8_t par[MAX_PARITY_SIZE] = {0x00}; - GetParity(frame, len/8, par); + GetParity(frame, len / 8, par); ReaderTransmitBitsPar(frame, len, par, timing); } -void ReaderTransmit(uint8_t* frame, uint16_t len, uint32_t *timing) { +void ReaderTransmit(uint8_t *frame, uint16_t len, uint32_t *timing) +{ // Generate parity and redirect uint8_t par[MAX_PARITY_SIZE] = {0x00}; GetParity(frame, len, par); - ReaderTransmitBitsPar(frame, len*8, par, timing); + ReaderTransmitBitsPar(frame, len * 8, par, timing); } -int ReaderReceiveOffset(uint8_t* receivedAnswer, uint16_t offset, uint8_t *parity) { +int ReaderReceiveOffset(uint8_t *receivedAnswer, uint16_t offset, uint8_t *parity) +{ if (!GetIso14443aAnswerFromTag(receivedAnswer, parity, offset)) return false; - LogTrace(receivedAnswer, Demod.len, Demod.startTime*16 - DELAY_AIR2ARM_AS_READER, Demod.endTime*16 - DELAY_AIR2ARM_AS_READER, parity, false); + LogTrace(receivedAnswer, Demod.len, Demod.startTime * 16 - DELAY_AIR2ARM_AS_READER, Demod.endTime * 16 - DELAY_AIR2ARM_AS_READER, parity, false); return Demod.len; } -int ReaderReceive(uint8_t *receivedAnswer, uint8_t *parity) { +int ReaderReceive(uint8_t *receivedAnswer, uint8_t *parity) +{ if (!GetIso14443aAnswerFromTag(receivedAnswer, parity, 0)) return false; - LogTrace(receivedAnswer, Demod.len, Demod.startTime*16 - DELAY_AIR2ARM_AS_READER, Demod.endTime*16 - DELAY_AIR2ARM_AS_READER, parity, false); + LogTrace(receivedAnswer, Demod.len, Demod.startTime * 16 - DELAY_AIR2ARM_AS_READER, Demod.endTime * 16 - DELAY_AIR2ARM_AS_READER, parity, false); return Demod.len; } // This function misstreats the ISO 14443a anticollision procedure. // by fooling the reader there is a collision and forceing the reader to // increase the uid bytes. The might be an overflow, DoS will occure. -void iso14443a_antifuzz(uint32_t flags){ +void iso14443a_antifuzz(uint32_t flags) +{ // We need to listen to the high-frequency, peak-detected path. iso14443a_setup(FPGA_HF_ISO14443A_TAGSIM_LISTEN); @@ -1846,7 +1911,7 @@ void iso14443a_antifuzz(uint32_t flags){ uint8_t *receivedPar = BigBuf_malloc(MAX_PARITY_SIZE); uint8_t *resp = BigBuf_malloc(20); - memset(resp, 0xFF , 20); + memset(resp, 0xFF, 20); LED_A_ON(); for (;;) { @@ -1857,16 +1922,16 @@ void iso14443a_antifuzz(uint32_t flags){ Dbprintf("Anti-fuzz stopped. Trace length: %d ", BigBuf_get_traceLen()); break; } - if ( received[0] == ISO14443A_CMD_WUPA || received[0] == ISO14443A_CMD_REQA) { - resp[0] = 0x04; - resp[1] = 0x00; + if (received[0] == ISO14443A_CMD_WUPA || received[0] == ISO14443A_CMD_REQA) { + resp[0] = 0x04; + resp[1] = 0x00; - if ( (flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA ) { - resp[0] = 0x44; - } + if ((flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA) { + resp[0] = 0x44; + } - EmSendCmd(resp, 2); - continue; + EmSendCmd(resp, 2); + continue; } // Received request for UID (cascade 1) @@ -1879,7 +1944,7 @@ void iso14443a_antifuzz(uint32_t flags){ resp[4] = resp[0] ^ resp[1] ^ resp[2] ^ resp[3]; colpos = 0; - if ( (flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA ) { + if ((flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA) { resp[0] = 0x88; colpos = 8; } @@ -1898,12 +1963,13 @@ void iso14443a_antifuzz(uint32_t flags){ } } - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); switch_off(); BigBuf_free_keep_EM(); } -static void iso14a_set_ATS_times(uint8_t *ats) { +static void iso14a_set_ATS_times(uint8_t *ats) +{ uint8_t tb1; uint8_t fwi, sfgi; @@ -1919,24 +1985,25 @@ static void iso14a_set_ATS_times(uint8_t *ats) { fwi = (tb1 & 0xf0) >> 4; // frame waiting time integer (FWI) if (fwi != 15) { fwt = 256 * 16 * (1 << fwi); // frame waiting time (FWT) in 1/fc - iso14a_set_timeout(fwt/(8*16)); + iso14a_set_timeout(fwt / (8 * 16)); } sfgi = tb1 & 0x0f; // startup frame guard time integer (SFGI) if (sfgi != 0 && sfgi != 15) { sfgt = 256 * 16 * (1 << sfgi); // startup frame guard time (SFGT) in 1/fc - NextTransferTime = MAX(NextTransferTime, Demod.endTime + (sfgt - DELAY_AIR2ARM_AS_READER - DELAY_ARM2AIR_AS_READER)/16); + NextTransferTime = MAX(NextTransferTime, Demod.endTime + (sfgt - DELAY_AIR2ARM_AS_READER - DELAY_ARM2AIR_AS_READER) / 16); } } } } -static int GetATQA(uint8_t *resp, uint8_t *resp_par) { +static int GetATQA(uint8_t *resp, uint8_t *resp_par) +{ #define WUPA_RETRY_TIMEOUT 10 // 10ms uint8_t wupa[] = { ISO14443A_CMD_WUPA }; // 0x26 - REQA 0x52 - WAKE-UP uint32_t save_iso14a_timeout = iso14a_get_timeout(); - iso14a_set_timeout(1236/(16*8)+1); // response to WUPA is expected at exactly 1236/fc. No need to wait longer. + iso14a_set_timeout(1236 / (16 * 8) + 1); // response to WUPA is expected at exactly 1236/fc. No need to wait longer. uint32_t start_time = GetTickCount(); int len; @@ -1959,11 +2026,12 @@ static int GetATQA(uint8_t *resp, uint8_t *resp_par) { // if anticollision is false, then the UID must be provided in uid_ptr[] // and num_cascades must be set (1: 4 Byte UID, 2: 7 Byte UID, 3: 10 Byte UID) // requests ATS unless no_rats is true -int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_t *cuid_ptr, bool anticollision, uint8_t num_cascades, bool no_rats) { +int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_t *cuid_ptr, bool anticollision, uint8_t num_cascades, bool no_rats) +{ - uint8_t sel_all[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT,0x20 }; - uint8_t sel_uid[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; - uint8_t rats[] = { ISO14443A_CMD_RATS,0x80,0x00,0x00 }; // FSD=256, FSDI=8, CID=0 + uint8_t sel_all[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT, 0x20 }; + uint8_t sel_uid[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t rats[] = { ISO14443A_CMD_RATS, 0x80, 0x00, 0x00 }; // FSD=256, FSDI=8, CID=0 uint8_t resp[MAX_FRAME_SIZE] = {0}; // theoretically. A usual RATS will be much smaller uint8_t resp_par[MAX_PARITY_SIZE] = {0}; uint8_t uid_resp[4] = {0}; @@ -2000,12 +2068,12 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_ // OK we will select at least at cascade 1, lets see if first byte of UID was 0x88 in // which case we need to make a cascade 2 request and select - this is a long UID // While the UID is not complete, the 3nd bit (from the right) is set in the SAK. - for(; sak & 0x04; cascade_level++) { + for (; sak & 0x04; cascade_level++) { // SELECT_* (L1: 0x93, L2: 0x95, L3: 0x97) sel_uid[0] = sel_all[0] = 0x93 + cascade_level * 2; if (anticollision) { - // SELECT_ALL + // SELECT_ALL ReaderTransmit(sel_all, sizeof(sel_all), NULL); if (!ReaderReceive(resp, resp_par)) return 0; @@ -2017,24 +2085,24 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_ while (Demod.collisionPos) { Dbprintf("Multiple tags detected. Collision after Bit %d", Demod.collisionPos); for (uint16_t i = collision_answer_offset; i < Demod.collisionPos; i++, uid_resp_bits++) { // add valid UID bits before collision point - uint16_t UIDbit = (resp[i/8] >> (i % 8)) & 0x01; + uint16_t UIDbit = (resp[i / 8] >> (i % 8)) & 0x01; uid_resp[uid_resp_bits / 8] |= UIDbit << (uid_resp_bits % 8); } - uid_resp[uid_resp_bits/8] |= 1 << (uid_resp_bits % 8); // next time select the card(s) with a 1 in the collision position + uid_resp[uid_resp_bits / 8] |= 1 << (uid_resp_bits % 8); // next time select the card(s) with a 1 in the collision position uid_resp_bits++; // construct anticollosion command: - sel_uid[1] = ((2 + uid_resp_bits/8) << 4) | (uid_resp_bits & 0x07); // length of data in bytes and bits - for (uint16_t i = 0; i <= uid_resp_bits/8; i++) { - sel_uid[2+i] = uid_resp[i]; + sel_uid[1] = ((2 + uid_resp_bits / 8) << 4) | (uid_resp_bits & 0x07); // length of data in bytes and bits + for (uint16_t i = 0; i <= uid_resp_bits / 8; i++) { + sel_uid[2 + i] = uid_resp[i]; } - collision_answer_offset = uid_resp_bits%8; + collision_answer_offset = uid_resp_bits % 8; ReaderTransmitBits(sel_uid, 16 + uid_resp_bits, NULL); if (!ReaderReceiveOffset(resp, collision_answer_offset, resp_par)) return 0; } // finally, add the last bits and BCC of the UID - for (uint16_t i = collision_answer_offset; i < (Demod.len-1)*8; i++, uid_resp_bits++) { - uint16_t UIDbit = (resp[i/8] >> (i%8)) & 0x01; - uid_resp[uid_resp_bits/8] |= UIDbit << (uid_resp_bits % 8); + for (uint16_t i = collision_answer_offset; i < (Demod.len - 1) * 8; i++, uid_resp_bits++) { + uint16_t UIDbit = (resp[i / 8] >> (i % 8)) & 0x01; + uid_resp[uid_resp_bits / 8] |= UIDbit << (uid_resp_bits % 8); } } else { // no collision, use the response to SELECT_ALL as current uid @@ -2044,20 +2112,20 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_ } else { if (cascade_level < num_cascades - 1) { uid_resp[0] = 0x88; - memcpy(uid_resp+1, uid_ptr+cascade_level*3, 3); + memcpy(uid_resp + 1, uid_ptr + cascade_level * 3, 3); } else { - memcpy(uid_resp, uid_ptr+cascade_level*3, 4); + memcpy(uid_resp, uid_ptr + cascade_level * 3, 4); } } uid_resp_len = 4; // calculate crypto UID. Always use last 4 Bytes. - if(cuid_ptr) + if (cuid_ptr) *cuid_ptr = bytes_to_num(uid_resp, 4); // Construct SELECT UID command sel_uid[1] = 0x70; // transmitting a full UID (1 Byte cmd, 1 Byte NVB, 4 Byte UID, 1 Byte BCC, 2 Bytes CRC) - memcpy(sel_uid+2, uid_resp, 4); // the UID received during anticollision, or the provided UID + memcpy(sel_uid + 2, uid_resp, 4); // the UID received during anticollision, or the provided UID sel_uid[6] = sel_uid[2] ^ sel_uid[3] ^ sel_uid[4] ^ sel_uid[5]; // calculate and add BCC AddCrc14A(sel_uid, 7); // calculate and add CRC ReaderTransmit(sel_uid, sizeof(sel_uid), NULL); @@ -2077,11 +2145,11 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_ uid_resp_len = 3; } - if(uid_ptr && anticollision) - memcpy(uid_ptr + (cascade_level*3), uid_resp, uid_resp_len); + if (uid_ptr && anticollision) + memcpy(uid_ptr + (cascade_level * 3), uid_resp, uid_resp_len); - if(p_card) { - memcpy(p_card->uid + (cascade_level*3), uid_resp, uid_resp_len); + if (p_card) { + memcpy(p_card->uid + (cascade_level * 3), uid_resp, uid_resp_len); p_card->uidlen += uid_resp_len; } } @@ -2091,10 +2159,10 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_ } // PICC compilant with iso14443a-4 ---> (SAK & 0x20 != 0) - if( (sak & 0x20) == 0) return 2; + if ((sak & 0x20) == 0) return 2; // RATS, Request for answer to select - if ( !no_rats ) { + if (!no_rats) { AddCrc14A(rats, 2); ReaderTransmit(rats, sizeof(rats), NULL); @@ -2116,9 +2184,10 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_card, uint32_ return 1; } -int iso14443a_fast_select_card(uint8_t *uid_ptr, uint8_t num_cascades) { - uint8_t sel_all[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT,0x20 }; - uint8_t sel_uid[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; +int iso14443a_fast_select_card(uint8_t *uid_ptr, uint8_t num_cascades) +{ + uint8_t sel_all[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT, 0x20 }; + uint8_t sel_uid[] = { ISO14443A_CMD_ANTICOLL_OR_SELECT, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; uint8_t resp[5] = {0}; // theoretically. A usual RATS will be much smaller uint8_t resp_par[1] = {0}; uint8_t uid_resp[4] = {0}; @@ -2133,20 +2202,20 @@ int iso14443a_fast_select_card(uint8_t *uid_ptr, uint8_t num_cascades) { // OK we will select at least at cascade 1, lets see if first byte of UID was 0x88 in // which case we need to make a cascade 2 request and select - this is a long UID // While the UID is not complete, the 3nd bit (from the right) is set in the SAK. - for(; sak & 0x04; cascade_level++) { + for (; sak & 0x04; cascade_level++) { // SELECT_* (L1: 0x93, L2: 0x95, L3: 0x97) sel_uid[0] = sel_all[0] = 0x93 + cascade_level * 2; if (cascade_level < num_cascades - 1) { uid_resp[0] = 0x88; - memcpy(uid_resp+1, uid_ptr+cascade_level*3, 3); + memcpy(uid_resp + 1, uid_ptr + cascade_level * 3, 3); } else { - memcpy(uid_resp, uid_ptr+cascade_level*3, 4); + memcpy(uid_resp, uid_ptr + cascade_level * 3, 4); } // Construct SELECT UID command //sel_uid[1] = 0x70; // transmitting a full UID (1 Byte cmd, 1 Byte NVB, 4 Byte UID, 1 Byte BCC, 2 Bytes CRC) - memcpy(sel_uid+2, uid_resp, 4); // the UID received during anticollision, or the provided UID + memcpy(sel_uid + 2, uid_resp, 4); // the UID received during anticollision, or the provided UID sel_uid[6] = sel_uid[2] ^ sel_uid[3] ^ sel_uid[4] ^ sel_uid[5]; // calculate and add BCC AddCrc14A(sel_uid, 7); // calculate and add CRC ReaderTransmit(sel_uid, sizeof(sel_uid), NULL); @@ -2168,7 +2237,8 @@ int iso14443a_fast_select_card(uint8_t *uid_ptr, uint8_t num_cascades) { return 1; } -void iso14443a_setup(uint8_t fpga_minor_mode) { +void iso14443a_setup(uint8_t fpga_minor_mode) +{ FpgaDownloadAndGo(FPGA_BITSTREAM_HF); // Set up the synchronous serial port @@ -2219,7 +2289,8 @@ b8 b7 b6 b5 b4 b3 b2 b1 b5,b6 = 00 - DESELECT 11 - WTX */ -int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, bool send_chaining, void *data, uint8_t *res) { +int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, bool send_chaining, void *data, uint8_t *res) +{ uint8_t parity[MAX_PARITY_SIZE] = {0x00}; uint8_t real_cmd[cmd_len + 4]; @@ -2246,12 +2317,12 @@ int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, bool send_chaining, void *data, u if (!len) { return 0; //DATA LINK ERROR - } else{ + } else { // S-Block WTX - while(len && ((data_bytes[0] & 0xF2) == 0xF2)) { + while (len && ((data_bytes[0] & 0xF2) == 0xF2)) { uint32_t save_iso14a_timeout = iso14a_get_timeout(); // temporarily increase timeout - iso14a_set_timeout( MAX((data_bytes[1] & 0x3f) * save_iso14a_timeout, MAX_ISO14A_TIMEOUT) ); + iso14a_set_timeout(MAX((data_bytes[1] & 0x3f) * save_iso14a_timeout, MAX_ISO14A_TIMEOUT)); // Transmit WTX back // byte1 - WTXM [1..59]. command FWT=FWT*WTXM data_bytes[1] = data_bytes[1] & 0x3f; // 2 high bits mandatory set to 0b @@ -2269,10 +2340,9 @@ int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, bool send_chaining, void *data, u // if we received an I- or R(ACK)-Block with a block number equal to the // current block number, toggle the current block number if (len >= 3 // PCB+CRC = 3 bytes - && ((data_bytes[0] & 0xC0) == 0 // I-Block - || (data_bytes[0] & 0xD0) == 0x80) // R-Block with ACK bit set to 0 - && (data_bytes[0] & 0x01) == iso14_pcb_blocknum) // equal block numbers - { + && ((data_bytes[0] & 0xC0) == 0 // I-Block + || (data_bytes[0] & 0xD0) == 0x80) // R-Block with ACK bit set to 0 + && (data_bytes[0] & 0x01) == iso14_pcb_blocknum) { // equal block numbers iso14_pcb_blocknum ^= 1; } @@ -2306,7 +2376,8 @@ int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, bool send_chaining, void *data, u // low :: len of commandbytes // arg2 timeout // d.asBytes command bytes to send -void ReaderIso14443a(UsbCommand *c) { +void ReaderIso14443a(UsbCommand *c) +{ iso14a_command_t param = c->arg[0]; size_t len = c->arg[1] & 0xffff; size_t lenbits = c->arg[1] >> 16; @@ -2329,11 +2400,11 @@ void ReaderIso14443a(UsbCommand *c) { // notify client selecting status. // if failed selecting, turn off antenna and quite. - if( !(param & ISO14A_NO_SELECT) ) { - iso14a_card_select_t *card = (iso14a_card_select_t*)buf; - arg0 = iso14443a_select_card(NULL, card, NULL, true, 0, param & ISO14A_NO_RATS ); + if (!(param & ISO14A_NO_SELECT)) { + iso14a_card_select_t *card = (iso14a_card_select_t *)buf; + arg0 = iso14443a_select_card(NULL, card, NULL, true, 0, param & ISO14A_NO_RATS); cmd_send(CMD_ACK, arg0, card->uidlen, 0, buf, sizeof(iso14a_card_select_t)); - if ( arg0 == 0 ) + if (arg0 == 0) goto OUT; } } @@ -2351,7 +2422,7 @@ void ReaderIso14443a(UsbCommand *c) { if ((param & ISO14A_APPEND_CRC)) { // Don't append crc on empty bytearray... - if ( len > 0 ) { + if (len > 0) { if ((param & ISO14A_TOPAZMODE)) AddCrc14B(cmd, len); else @@ -2373,7 +2444,7 @@ void ReaderIso14443a(UsbCommand *c) { bits_to_send -= 8; } } else { - GetParity(cmd, lenbits/8, par); + GetParity(cmd, lenbits / 8, par); ReaderTransmitBitsPar(cmd, lenbits, par, NULL); // bytes are 8 bit with odd parity } } else { // want to send complete bytes only @@ -2406,7 +2477,8 @@ OUT: // Determine the distance between two nonces. // Assume that the difference is small, but we don't know which is first. // Therefore try in alternating directions. -int32_t dist_nt(uint32_t nt1, uint32_t nt2) { +int32_t dist_nt(uint32_t nt1, uint32_t nt2) +{ if (nt1 == nt2) return 0; @@ -2421,7 +2493,7 @@ int32_t dist_nt(uint32_t nt1, uint32_t nt2) { if (nttmp2 == nt1) return -i; } - return(-99999); // either nt1 or nt2 are invalid nonces + return (-99999); // either nt1 or nt2 are invalid nonces } @@ -2435,19 +2507,21 @@ int32_t dist_nt(uint32_t nt1, uint32_t nt2) { // Cloning MiFare Classic Rail and Building Passes, Anywhere, Anytime" // (article by Nicolas T. Courtois, 2009) //----------------------------------------------------------------------------- -void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ) { +void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype) +{ iso14443a_setup(FPGA_HF_ISO14443A_READER_MOD); - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); uint8_t mf_auth[] = { keytype, block, 0x00, 0x00 }; - uint8_t mf_nr_ar[] = {0,0,0,0,0,0,0,0}; - uint8_t uid[10] = {0,0,0,0,0,0,0,0,0,0}; - uint8_t par_list[8] = {0,0,0,0,0,0,0,0}; - uint8_t ks_list[8] = {0,0,0,0,0,0,0,0}; + uint8_t mf_nr_ar[] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t uid[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t par_list[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t ks_list[8] = {0, 0, 0, 0, 0, 0, 0, 0}; uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00}; uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00}; uint8_t par[1] = {0}; // maximum 8 Bytes to be sent here, 1 byte parity is therefore enough @@ -2512,10 +2586,17 @@ void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ) { continue; } switch (card_info.uidlen) { - case 4 : cascade_levels = 1; break; - case 7 : cascade_levels = 2; break; - case 10: cascade_levels = 3; break; - default: break; + case 4 : + cascade_levels = 1; + break; + case 7 : + cascade_levels = 2; + break; + case 10: + cascade_levels = 3; + break; + default: + break; } have_uid = true; } else { // no need for anticollision. We can directly select the card @@ -2528,15 +2609,15 @@ void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ) { elapsed_prng_sequences = 1; // Sending timeslot of ISO14443a frame - sync_time = (sync_time & 0xfffffff8 ) + sync_cycles + catch_up_cycles; + sync_time = (sync_time & 0xfffffff8) + sync_cycles + catch_up_cycles; catch_up_cycles = 0; - #define SYNC_TIME_BUFFER 16 // if there is only SYNC_TIME_BUFFER left before next planned sync, wait for next PRNG cycle +#define SYNC_TIME_BUFFER 16 // if there is only SYNC_TIME_BUFFER left before next planned sync, wait for next PRNG cycle // if we missed the sync time already or are about to miss it, advance to the next nonce repeat - while ( sync_time < GetCountSspClk() + SYNC_TIME_BUFFER) { + while (sync_time < GetCountSspClk() + SYNC_TIME_BUFFER) { ++elapsed_prng_sequences; - sync_time = (sync_time & 0xfffffff8 ) + sync_cycles; + sync_time = (sync_time & 0xfffffff8) + sync_cycles; } // Transmit MIFARE_CLASSIC_AUTH at synctime. Should result in returning the same tag nonce (== nt_attacked) @@ -2581,13 +2662,13 @@ void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ) { break; } - sync_cycles = (sync_cycles - nt_distance)/elapsed_prng_sequences; + sync_cycles = (sync_cycles - nt_distance) / elapsed_prng_sequences; // no negative sync_cycles if (sync_cycles <= 0) sync_cycles += PRNG_SEQUENCE_LENGTH; // reset sync_cycles - if (sync_cycles > PRNG_SEQUENCE_LENGTH * 2 ) { + if (sync_cycles > PRNG_SEQUENCE_LENGTH * 2) { sync_cycles = PRNG_SEQUENCE_LENGTH; sync_time = GetCountSspClk() & 0xfffffff8; } @@ -2601,7 +2682,7 @@ void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ) { } LED_B_OFF(); - if ( (nt != nt_attacked) && nt_attacked) { // we somehow lost sync. Try to catch up again... + if ((nt != nt_attacked) && nt_attacked) { // we somehow lost sync. Try to catch up again... catch_up_cycles = -dist_nt(nt_attacked, nt); if (catch_up_cycles == 99999) { // invalid nonce received. Don't resync on that one. @@ -2685,7 +2766,7 @@ void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ) { memcpy(buf + 16, ks_list, 8); memcpy(buf + 24, mf_nr_ar, 8); - cmd_send(CMD_ACK, isOK, 0, 0, buf, sizeof(buf) ); + cmd_send(CMD_ACK, isOK, 0, 0, buf, sizeof(buf)); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); @@ -2696,10 +2777,11 @@ void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ) { * Mifare Classic NACK-bug detection * Thanks to @doegox for the feedback and new approaches. */ -void DetectNACKbug() { +void DetectNACKbug() +{ uint8_t mf_auth[] = {0x60, 0x00, 0xF5, 0x7B}; - uint8_t mf_nr_ar[] = {0,0,0,0,0,0,0,0}; - uint8_t uid[10] = {0,0,0,0,0,0,0,0,0,0}; + uint8_t mf_nr_ar[] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t uid[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00}; uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00}; uint8_t par[1] = {0}; // maximum 8 Bytes to be sent here, 1 byte parity is therefore enough @@ -2718,7 +2800,8 @@ void DetectNACKbug() { // Mifare Classic's random generator repeats every 2^16 cycles (and so do the nonces). uint32_t sync_cycles = PRNG_SEQUENCE_LENGTH; - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); iso14443a_setup(FPGA_HF_ISO14443A_READER_MOD); @@ -2732,7 +2815,7 @@ void DetectNACKbug() { received_nack = false; // Cards always leaks a NACK, no matter the parity - if ((i==10) && (num_nacks == i-1)) { + if ((i == 10) && (num_nacks == i - 1)) { isOK = 2; break; } @@ -2753,10 +2836,17 @@ void DetectNACKbug() { continue; } switch (card_info.uidlen) { - case 4 : cascade_levels = 1; break; - case 7 : cascade_levels = 2; break; - case 10: cascade_levels = 3; break; - default: break; + case 4 : + cascade_levels = 1; + break; + case 7 : + cascade_levels = 2; + break; + case 10: + cascade_levels = 3; + break; + default: + break; } have_uid = true; } else { // no need for anticollision. We can directly select the card @@ -2769,13 +2859,13 @@ void DetectNACKbug() { elapsed_prng_sequences = 1; // Sending timeslot of ISO14443a frame - sync_time = (sync_time & 0xfffffff8 ) + sync_cycles + catch_up_cycles; + sync_time = (sync_time & 0xfffffff8) + sync_cycles + catch_up_cycles; catch_up_cycles = 0; // if we missed the sync time already, advance to the next nonce repeat - while ( GetCountSspClk() > sync_time) { + while (GetCountSspClk() > sync_time) { ++elapsed_prng_sequences; - sync_time = (sync_time & 0xfffffff8 ) + sync_cycles; + sync_time = (sync_time & 0xfffffff8) + sync_cycles; } // Transmit MIFARE_CLASSIC_AUTH at synctime. Should result in returning the same tag nonce (== nt_attacked) @@ -2795,7 +2885,7 @@ void DetectNACKbug() { received_nack = true; num_nacks++; // ALWAYS leak Detection. - if ( i == num_nacks ) { + if (i == num_nacks) { continue; } } @@ -2812,7 +2902,7 @@ void DetectNACKbug() { } else { if (nt_distance == -99999) { // invalid nonce received unexpected_random++; - if (unexpected_random > MAX_UNEXPECTED_RANDOM ) { + if (unexpected_random > MAX_UNEXPECTED_RANDOM) { // Card has an unpredictable PRNG. Give up isOK = 98; break; @@ -2829,12 +2919,12 @@ void DetectNACKbug() { break; } - sync_cycles = (sync_cycles - nt_distance)/elapsed_prng_sequences; + sync_cycles = (sync_cycles - nt_distance) / elapsed_prng_sequences; if (sync_cycles <= 0) sync_cycles += PRNG_SEQUENCE_LENGTH; - if (sync_cycles > PRNG_SEQUENCE_LENGTH * 2 ) { + if (sync_cycles > PRNG_SEQUENCE_LENGTH * 2) { isOK = 96; // Card's PRNG runs at an unexpected frequency or resets unexpectedly break; } @@ -2846,7 +2936,7 @@ void DetectNACKbug() { } } - if ( (nt != nt_attacked) && nt_attacked) { + if ((nt != nt_attacked) && nt_attacked) { // we somehow lost sync. Try to catch up again... catch_up_cycles = -dist_nt(nt_attacked, nt); @@ -2874,7 +2964,7 @@ void DetectNACKbug() { if (MF_DBGLEVEL >= 4) { Dbprintf("Lost sync in cycle %d for the fourth time consecutively (nt_distance = %d). Adjusting sync_cycles to %d.\n", i, catch_up_cycles, sync_cycles); - Dbprintf("nt [%08x] attacted [%08x]", nt, nt_attacked ); + Dbprintf("nt [%08x] attacted [%08x]", nt, nt_attacked); } last_catch_up = 0; catch_up_cycles = 0; @@ -2892,7 +2982,7 @@ void DetectNACKbug() { // tried all 256 possible parities without success. if (par[0] == 0) { - if ( num_nacks == 1 ) + if (num_nacks == 1) isOK = 1; break; } @@ -2903,7 +2993,7 @@ void DetectNACKbug() { // num_nacks = number of nacks recieved. should be only 1. if not its a clone card which always sends NACK (parity == 0) ? // i = number of authentications sent. Not always 256, since we are trying to sync but close to it. - cmd_send(CMD_ACK, isOK, num_nacks, i, 0, 0 ); + cmd_send(CMD_ACK, isOK, num_nacks, i, 0, 0); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); @@ -2923,7 +3013,8 @@ void DetectNACKbug() { *@param exitAfterNReads, exit simulation after n blocks have been read, 0 is inifite * (unless reader attack mode enabled then it runs util it gets enough nonces to recover all keys attmpted) */ -void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t *datain) { +void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t *datain) +{ int cardSTATE = MFEMUL_NOFIELD; int _UID_LEN = 0; // 4, 7, 10 @@ -2960,7 +3051,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * // TAG Nonce - Authenticate response uint8_t rAUTH_NT[4]; - uint32_t nonce = prng_successor( GetTickCount(), 32 ); + uint32_t nonce = prng_successor(GetTickCount(), 32); num_to_bytes(nonce, 4, rAUTH_NT); // uint8_t rAUTH_NT[] = {0x55, 0x41, 0x49, 0x92};// nonce from nested? why this? @@ -2974,20 +3065,20 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * // -- Determine the UID // Can be set from emulator memory or incoming data // Length: 4,7,or 10 bytes - if ( (flags & FLAG_UID_IN_EMUL) == FLAG_UID_IN_EMUL) + if ((flags & FLAG_UID_IN_EMUL) == FLAG_UID_IN_EMUL) emlGetMemBt(datain, 0, 10); // load 10bytes from EMUL to the datain pointer. to be used below. - if ( (flags & FLAG_4B_UID_IN_DATA) == FLAG_4B_UID_IN_DATA) { + if ((flags & FLAG_4B_UID_IN_DATA) == FLAG_4B_UID_IN_DATA) { memcpy(rUIDBCC1, datain, 4); _UID_LEN = 4; - } else if ( (flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA) { + } else if ((flags & FLAG_7B_UID_IN_DATA) == FLAG_7B_UID_IN_DATA) { memcpy(&rUIDBCC1[1], datain, 3); - memcpy( rUIDBCC2, datain+3, 4); + memcpy(rUIDBCC2, datain + 3, 4); _UID_LEN = 7; - } else if ( (flags & FLAG_10B_UID_IN_DATA) == FLAG_10B_UID_IN_DATA) { + } else if ((flags & FLAG_10B_UID_IN_DATA) == FLAG_10B_UID_IN_DATA) { memcpy(&rUIDBCC1[1], datain, 3); - memcpy(&rUIDBCC2[1], datain+3, 3); - memcpy( rUIDBCC3, datain+6, 4); + memcpy(&rUIDBCC2[1], datain + 3, 3); + memcpy(rUIDBCC3, datain + 6, 4); _UID_LEN = 10; } @@ -3000,11 +3091,11 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * rUIDBCC1[4] = rUIDBCC1[0] ^ rUIDBCC1[1] ^ rUIDBCC1[2] ^ rUIDBCC1[3]; if (MF_DBGLEVEL >= 2) { Dbprintf("4B UID: %02x%02x%02x%02x", - rUIDBCC1[0], - rUIDBCC1[1], - rUIDBCC1[2], - rUIDBCC1[3] - ); + rUIDBCC1[0], + rUIDBCC1[1], + rUIDBCC1[2], + rUIDBCC1[3] + ); } break; case 7: @@ -3012,21 +3103,21 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * sak_7[0] &= 0xFB; // save CUID cuid = bytes_to_num(rUIDBCC2, 4); - // CascadeTag, CT + // CascadeTag, CT rUIDBCC1[0] = 0x88; // BCC rUIDBCC1[4] = rUIDBCC1[0] ^ rUIDBCC1[1] ^ rUIDBCC1[2] ^ rUIDBCC1[3]; rUIDBCC2[4] = rUIDBCC2[0] ^ rUIDBCC2[1] ^ rUIDBCC2[2] ^ rUIDBCC2[3]; if (MF_DBGLEVEL >= 2) { Dbprintf("7B UID: %02x %02x %02x %02x %02x %02x %02x", - rUIDBCC1[1], - rUIDBCC1[2], - rUIDBCC1[3], - rUIDBCC2[0], - rUIDBCC2[1], - rUIDBCC2[2], - rUIDBCC2[3] - ); + rUIDBCC1[1], + rUIDBCC1[2], + rUIDBCC1[3], + rUIDBCC2[0], + rUIDBCC2[1], + rUIDBCC2[2], + rUIDBCC2[3] + ); } break; case 10: @@ -3034,7 +3125,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * sak_10[0] &= 0xFB; // save CUID cuid = bytes_to_num(rUIDBCC3, 4); - // CascadeTag, CT + // CascadeTag, CT rUIDBCC1[0] = 0x88; rUIDBCC2[0] = 0x88; // BCC @@ -3044,17 +3135,17 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * if (MF_DBGLEVEL >= 2) { Dbprintf("10B UID: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", - rUIDBCC1[1], - rUIDBCC1[2], - rUIDBCC1[3], - rUIDBCC2[1], - rUIDBCC2[2], - rUIDBCC2[3], - rUIDBCC3[0], - rUIDBCC3[1], - rUIDBCC3[2], - rUIDBCC3[3] - ); + rUIDBCC1[1], + rUIDBCC1[2], + rUIDBCC1[3], + rUIDBCC2[1], + rUIDBCC2[2], + rUIDBCC2[3], + rUIDBCC3[0], + rUIDBCC3[1], + rUIDBCC3[2], + rUIDBCC3[3] + ); } break; default: @@ -3114,11 +3205,11 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * switch (cardSTATE) { case MFEMUL_NOFIELD: case MFEMUL_HALTED: - case MFEMUL_IDLE:{ - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + case MFEMUL_IDLE: { + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } - case MFEMUL_SELECT1:{ + case MFEMUL_SELECT1: { if (len == 2 && (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT && receivedCmd[1] == 0x20)) { if (MF_DBGLEVEL >= 4) Dbprintf("SELECT ALL received"); EmSendCmd(rUIDBCC1, sizeof(rUIDBCC1)); @@ -3126,13 +3217,13 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * } // select card if (len == 9 && - ( receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT && - receivedCmd[1] == 0x70 && - memcmp(&receivedCmd[2], rUIDBCC1, 4) == 0)) { + (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT && + receivedCmd[1] == 0x70 && + memcmp(&receivedCmd[2], rUIDBCC1, 4) == 0)) { // SAK 4b EmSendCmd(sak_4, sizeof(sak_4)); - switch(_UID_LEN){ + switch (_UID_LEN) { case 4: cardSTATE = MFEMUL_WORK; LED_B_ON(); @@ -3142,16 +3233,17 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * case 10: cardSTATE = MFEMUL_SELECT2; continue; - default:break; + default: + break; } } else { cardSTATE_TO_IDLE(); } break; } - case MFEMUL_SELECT2:{ + case MFEMUL_SELECT2: { if (!len) { - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } if (len == 2 && (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT_2 && receivedCmd[1] == 0x20)) { @@ -3159,12 +3251,12 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * break; } if (len == 9 && - (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT_2 && - receivedCmd[1] == 0x70 && - memcmp(&receivedCmd[2], rUIDBCC2, 4) == 0) ) { + (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT_2 && + receivedCmd[1] == 0x70 && + memcmp(&receivedCmd[2], rUIDBCC2, 4) == 0)) { EmSendCmd(sak_7, sizeof(sak_7)); - switch(_UID_LEN){ + switch (_UID_LEN) { case 7: cardSTATE = MFEMUL_WORK; LED_B_ON(); @@ -3173,15 +3265,16 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * case 10: cardSTATE = MFEMUL_SELECT3; continue; - default:break; + default: + break; } } cardSTATE_TO_IDLE(); break; } - case MFEMUL_SELECT3:{ + case MFEMUL_SELECT3: { if (!len) { - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } if (len == 2 && (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT_3 && receivedCmd[1] == 0x20)) { @@ -3189,9 +3282,9 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * break; } if (len == 9 && - (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT_3 && - receivedCmd[1] == 0x70 && - memcmp(&receivedCmd[2], rUIDBCC3, 4) == 0) ) { + (receivedCmd[0] == ISO14443A_CMD_ANTICOLL_OR_SELECT_3 && + receivedCmd[1] == 0x70 && + memcmp(&receivedCmd[2], rUIDBCC3, 4) == 0)) { EmSendCmd(sak_10, sizeof(sak_10)); cardSTATE = MFEMUL_WORK; @@ -3202,10 +3295,10 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * cardSTATE_TO_IDLE(); break; } - case MFEMUL_AUTH1:{ - if( len != 8) { + case MFEMUL_AUTH1: { + if (len != 8) { cardSTATE_TO_IDLE(); - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } @@ -3213,22 +3306,22 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * uint32_t ar = bytes_to_num(&receivedCmd[4], 4); // Collect AR/NR per keytype & sector - if ( (flags & FLAG_NR_AR_ATTACK) == FLAG_NR_AR_ATTACK ) { + if ((flags & FLAG_NR_AR_ATTACK) == FLAG_NR_AR_ATTACK) { int8_t index = -1; int8_t empty = -1; for (uint8_t i = 0; i < ATTACK_KEY_COUNT; i++) { // find which index to use - if ( (cardAUTHSC == ar_nr_nonces[i].sector) && (cardAUTHKEY == ar_nr_nonces[i].keytype)) + if ((cardAUTHSC == ar_nr_nonces[i].sector) && (cardAUTHKEY == ar_nr_nonces[i].keytype)) index = i; // keep track of empty slots. - if ( ar_nr_nonces[i].state == EMPTY) + if (ar_nr_nonces[i].state == EMPTY) empty = i; } // if no empty slots. Choose first and overwrite. - if ( index == -1 ) { - if ( empty == -1 ) { + if (index == -1) { + if (empty == -1) { index = 0; ar_nr_nonces[index].state = EMPTY; } else { @@ -3236,7 +3329,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * } } - switch(ar_nr_nonces[index].state) { + switch (ar_nr_nonces[index].state) { case EMPTY: { // first nonce collect ar_nr_nonces[index].cuid = cuid; @@ -3263,32 +3356,33 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * ar_nr_nonces[index].keytype = 0; break; } - default: break; + default: + break; } } - crypto1_word(pcs, nr , 1); + crypto1_word(pcs, nr, 1); uint32_t cardRr = ar ^ crypto1_word(pcs, 0, 0); //test if auth OK - if (cardRr != prng_successor(nonce, 64)){ + if (cardRr != prng_successor(nonce, 64)) { if (MF_DBGLEVEL >= 3) { Dbprintf("AUTH FAILED for sector %d with key %c. [nr=%08x cardRr=%08x] [nt=%08x succ=%08x]" - , cardAUTHSC - , (cardAUTHKEY == 0) ? 'A' : 'B' - , nr - , cardRr - , nonce // nt - , prng_successor(nonce, 64) - ); + , cardAUTHSC + , (cardAUTHKEY == 0) ? 'A' : 'B' + , nr + , cardRr + , nonce // nt + , prng_successor(nonce, 64) + ); } // Shouldn't we respond anything here? // Right now, we don't nack or anything, which causes the // reader to do a WUPA after a while. /Martin // -- which is the correct response. /piwi cardSTATE_TO_IDLE(); - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } @@ -3299,26 +3393,26 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * if (MF_DBGLEVEL >= 3) { Dbprintf("AUTH COMPLETED for sector %d with key %c. time=%d", - cardAUTHSC, - cardAUTHKEY == 0 ? 'A' : 'B', - GetTickCount() - authTimer - ); + cardAUTHSC, + cardAUTHKEY == 0 ? 'A' : 'B', + GetTickCount() - authTimer + ); } cardSTATE = MFEMUL_WORK; break; } - case MFEMUL_WORK:{ + case MFEMUL_WORK: { if (len == 0) { - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } bool encrypted_data = (cardAUTHKEY != 0xFF) ; - if(encrypted_data) + if (encrypted_data) mf_crypto1_decrypt(pcs, receivedCmd, len); if (len == 4 && (receivedCmd[0] == MIFARE_AUTH_KEYA || - receivedCmd[0] == MIFARE_AUTH_KEYB) ) { + receivedCmd[0] == MIFARE_AUTH_KEYB)) { authTimer = GetTickCount(); cardAUTHSC = receivedCmd[1] / 4; // received block -> sector @@ -3359,27 +3453,27 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * break; } - if(len != 4) { - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + if (len != 4) { + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } - if ( receivedCmd[0] == ISO14443A_CMD_READBLOCK || - receivedCmd[0] == ISO14443A_CMD_WRITEBLOCK || - receivedCmd[0] == MIFARE_CMD_INC || - receivedCmd[0] == MIFARE_CMD_DEC || - receivedCmd[0] == MIFARE_CMD_RESTORE || - receivedCmd[0] == MIFARE_CMD_TRANSFER ) { + if (receivedCmd[0] == ISO14443A_CMD_READBLOCK || + receivedCmd[0] == ISO14443A_CMD_WRITEBLOCK || + receivedCmd[0] == MIFARE_CMD_INC || + receivedCmd[0] == MIFARE_CMD_DEC || + receivedCmd[0] == MIFARE_CMD_RESTORE || + receivedCmd[0] == MIFARE_CMD_TRANSFER) { if (receivedCmd[1] >= 16 * 4) { EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_NACK_NA)); - if (MF_DBGLEVEL >= 4) Dbprintf("Reader tried to operate (0x%02) on out of range block: %d (0x%02x), nacking",receivedCmd[0],receivedCmd[1],receivedCmd[1]); + if (MF_DBGLEVEL >= 4) Dbprintf("Reader tried to operate (0x%02) on out of range block: %d (0x%02x), nacking", receivedCmd[0], receivedCmd[1], receivedCmd[1]); break; } if (receivedCmd[1] / 4 != cardAUTHSC) { EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_NACK_NA)); - if (MF_DBGLEVEL >= 4) Dbprintf("Reader tried to operate (0x%02) on block (0x%02x) not authenticated for (0x%02x), nacking",receivedCmd[0],receivedCmd[1],cardAUTHSC); + if (MF_DBGLEVEL >= 4) Dbprintf("Reader tried to operate (0x%02) on block (0x%02x) not authenticated for (0x%02x), nacking", receivedCmd[0], receivedCmd[1], cardAUTHSC); break; } } @@ -3392,7 +3486,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * mf_crypto1_encrypt(pcs, response, 18, response_par); EmSendCmdPar(response, 18, response_par); numReads++; - if(exitAfterNReads > 0 && numReads >= exitAfterNReads) { + if (exitAfterNReads > 0 && numReads >= exitAfterNReads) { Dbprintf("%d reads done, exiting", numReads); finished = true; } @@ -3407,11 +3501,11 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * break; } // increment, decrement, restore - if ( receivedCmd[0] == MIFARE_CMD_INC || - receivedCmd[0] == MIFARE_CMD_DEC || - receivedCmd[0] == MIFARE_CMD_RESTORE) { + if (receivedCmd[0] == MIFARE_CMD_INC || + receivedCmd[0] == MIFARE_CMD_DEC || + receivedCmd[0] == MIFARE_CMD_RESTORE) { - if (MF_DBGLEVEL >= 4) Dbprintf("RECV 0x%02x inc(0xC1)/dec(0xC0)/restore(0xC2) block %d (%02x)",receivedCmd[0], receivedCmd[1], receivedCmd[1]); + if (MF_DBGLEVEL >= 4) Dbprintf("RECV 0x%02x inc(0xC1)/dec(0xC0)/restore(0xC2) block %d (%02x)", receivedCmd[0], receivedCmd[1], receivedCmd[1]); if (emlCheckValBl(receivedCmd[1])) { if (MF_DBGLEVEL >= 4) Dbprintf("Reader tried to operate on block, but emlCheckValBl failed, nacking"); @@ -3440,7 +3534,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * LED_C_OFF(); cardSTATE = MFEMUL_HALTED; if (MF_DBGLEVEL >= 4) Dbprintf("--> HALTED. Selected time: %d ms", GetTickCount() - selTimer); - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); break; } // RATS @@ -3453,7 +3547,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_NACK_NA)); break; } - case MFEMUL_WRITEBL2:{ + case MFEMUL_WRITEBL2: { if (len == 18) { mf_crypto1_decrypt(pcs, receivedCmd, len); emlSetMem(receivedCmd, cardWRBL, 1); @@ -3461,11 +3555,11 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * cardSTATE = MFEMUL_WORK; } else { cardSTATE_TO_IDLE(); - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); } break; } - case MFEMUL_INTREG_INC:{ + case MFEMUL_INTREG_INC: { mf_crypto1_decrypt(pcs, receivedCmd, len); memcpy(&ans, receivedCmd, 4); if (emlGetValBl(&cardINTREG, &cardINTBLOCK, cardWRBL)) { @@ -3473,12 +3567,12 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * cardSTATE_TO_IDLE(); break; } - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); cardINTREG = cardINTREG + ans; cardSTATE = MFEMUL_WORK; break; } - case MFEMUL_INTREG_DEC:{ + case MFEMUL_INTREG_DEC: { mf_crypto1_decrypt(pcs, receivedCmd, len); memcpy(&ans, receivedCmd, 4); if (emlGetValBl(&cardINTREG, &cardINTBLOCK, cardWRBL)) { @@ -3486,12 +3580,12 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * cardSTATE_TO_IDLE(); break; } - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); cardINTREG = cardINTREG - ans; cardSTATE = MFEMUL_WORK; break; } - case MFEMUL_INTREG_REST:{ + case MFEMUL_INTREG_REST: { mf_crypto1_decrypt(pcs, receivedCmd, len); memcpy(&ans, receivedCmd, 4); if (emlGetValBl(&cardINTREG, &cardINTBLOCK, cardWRBL)) { @@ -3499,7 +3593,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * cardSTATE_TO_IDLE(); break; } - LogTrace(Uart.output, Uart.len, Uart.startTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime*16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); + LogTrace(Uart.output, Uart.len, Uart.startTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.endTime * 16 - DELAY_AIR2ARM_AS_TAG, Uart.parity, true); cardSTATE = MFEMUL_WORK; break; } @@ -3509,7 +3603,7 @@ void Mifare1ksim(uint8_t flags, uint8_t exitAfterNReads, uint8_t arg2, uint8_t * if (MF_DBGLEVEL >= 1) Dbprintf("Emulator stopped. Trace length: %d ", BigBuf_get_traceLen()); - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); set_tracing(false); diff --git a/armsrc/iso14443a.h b/armsrc/iso14443a.h index 38d0b99e1..4d28a8dfb 100644 --- a/armsrc/iso14443a.h +++ b/armsrc/iso14443a.h @@ -70,7 +70,7 @@ typedef struct { STATE_MILLER_Z, // DROP_NONE, // DROP_FIRST_HALF, - } state; + } state; uint16_t shiftReg; int16_t bitCount; uint16_t len; @@ -95,10 +95,10 @@ typedef struct { extern void GetParity(const uint8_t *pbtCmd, uint16_t len, uint8_t *par); -extern tDemod* GetDemod(void); +extern tDemod *GetDemod(void); extern void DemodReset(void); extern void DemodInit(uint8_t *data, uint8_t *parity); -extern tUart* GetUart(void); +extern tUart *GetUart(void); extern void UartReset(void); extern void UartInit(uint8_t *data, uint8_t *parity); extern RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time); @@ -133,7 +133,7 @@ bool EmLogTrace(uint8_t *reader_data, uint16_t reader_len, uint32_t reader_Start //extern bool prepare_allocated_tag_modulation(tag_response_info_t *response_info, uint8_t **buffer, size_t *buffer_size); -void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype ); +void ReaderMifare(bool first_try, uint8_t block, uint8_t keytype); void DetectNACKbug(); #ifdef __cplusplus diff --git a/armsrc/iso14443b.c b/armsrc/iso14443b.c index 2e2aec7cd..5041d9b3f 100644 --- a/armsrc/iso14443b.c +++ b/armsrc/iso14443b.c @@ -38,7 +38,7 @@ // 4sample #define SEND4STUFFBIT(x) ToSendStuffBit(x);ToSendStuffBit(x);ToSendStuffBit(x);ToSendStuffBit(x); //#define SEND4STUFFBIT(x) ToSendStuffBit(x); - // iceman, this threshold value, what makes 8 a good amplitude for this IQ values? +// iceman, this threshold value, what makes 8 a good amplitude for this IQ values? #ifndef SUBCARRIER_DETECT_THRESHOLD # define SUBCARRIER_DETECT_THRESHOLD 8 #endif @@ -77,7 +77,8 @@ static struct { uint8_t *output; } Uart; -static void UartReset() { +static void UartReset() +{ Uart.state = STATE_UNSYNCD; Uart.shiftReg = 0; Uart.bitCnt = 0; @@ -86,7 +87,8 @@ static void UartReset() { Uart.posCnt = 0; } -static void UartInit(uint8_t *data) { +static void UartInit(uint8_t *data) +{ Uart.output = data; UartReset(); // memset(Uart.output, 0x00, MAX_FRAME_SIZE); @@ -107,10 +109,10 @@ static struct { uint16_t bitCount; int posCount; int thisBit; -/* this had been used to add RSSI (Received Signal Strength Indication) to traces. Currently not implemented. - int metric; - int metricN; -*/ + /* this had been used to add RSSI (Received Signal Strength Indication) to traces. Currently not implemented. + int metric; + int metricN; + */ uint16_t shiftReg; uint8_t *output; uint16_t len; @@ -120,7 +122,8 @@ static struct { } Demod; // Clear out the state of the "UART" that receives from the tag. -static void DemodReset() { +static void DemodReset() +{ Demod.state = DEMOD_UNSYNCD; Demod.bitCount = 0; Demod.posCount = 0; @@ -133,7 +136,8 @@ static void DemodReset() { Demod.endTime = 0; } -static void DemodInit(uint8_t *data) { +static void DemodInit(uint8_t *data) +{ Demod.output = data; DemodReset(); // memset(Demod.output, 0x00, MAX_FRAME_SIZE); @@ -153,20 +157,22 @@ static void DemodInit(uint8_t *data) { * 13560000000 / 384000 = 35312 FWT * @param timeout is in frame wait time, fwt, measured in ETUs */ -static void iso14b_set_timeout(uint32_t timeout) { - #define MAX_TIMEOUT 40542464 // 13560000Hz * 1000ms / (2^32-1) * (8*16) - if(timeout > MAX_TIMEOUT) +static void iso14b_set_timeout(uint32_t timeout) +{ +#define MAX_TIMEOUT 40542464 // 13560000Hz * 1000ms / (2^32-1) * (8*16) + if (timeout > MAX_TIMEOUT) timeout = MAX_TIMEOUT; iso14b_timeout = timeout; - if(MF_DBGLEVEL >= 3) Dbprintf("ISO14443B Timeout set to %ld fwt", iso14b_timeout); + if (MF_DBGLEVEL >= 3) Dbprintf("ISO14443B Timeout set to %ld fwt", iso14b_timeout); } -static void iso14b_set_maxframesize(uint16_t size) { +static void iso14b_set_maxframesize(uint16_t size) +{ if (size > 256) size = MAX_FRAME_SIZE; Uart.byteCntMax = size; - if(MF_DBGLEVEL >= 3) Dbprintf("ISO14443B Max frame size set to %d bytes", Uart.byteCntMax); + if (MF_DBGLEVEL >= 3) Dbprintf("ISO14443B Max frame size set to %d bytes", Uart.byteCntMax); } //----------------------------------------------------------------------------- @@ -174,7 +180,8 @@ static void iso14b_set_maxframesize(uint16_t size) { // that here) so that they can be transmitted to the reader. Doesn't transmit // them yet, just leaves them ready to send in ToSend[]. //----------------------------------------------------------------------------- -static void CodeIso14443bAsTag(const uint8_t *cmd, int len) { +static void CodeIso14443bAsTag(const uint8_t *cmd, int len) +{ /* ISO 14443 B * * Reader to card | ASK - Amplitude Shift Keying Modulation (PCD to PICC for Type B) (NRZ-L encodig) @@ -237,7 +244,7 @@ static void CodeIso14443bAsTag(const uint8_t *cmd, int len) { * */ - int i,j; + int i, j; uint8_t b; ToSendReset(); @@ -252,15 +259,15 @@ static void CodeIso14443bAsTag(const uint8_t *cmd, int len) { // Send SOF. // 10-11 ETU * 4times samples ZEROS - for(i = 0; i < 10; i++) { SEND4STUFFBIT(0); } + for (i = 0; i < 10; i++) { SEND4STUFFBIT(0); } //for(i = 0; i < 10; i++) { ToSendStuffBit(0); } // 2-3 ETU * 4times samples ONES - for(i = 0; i < 3; i++) { SEND4STUFFBIT(1); } + for (i = 0; i < 3; i++) { SEND4STUFFBIT(1); } //for(i = 0; i < 3; i++) { ToSendStuffBit(1); } // data - for(i = 0; i < len; ++i) { + for (i = 0; i < len; ++i) { // Start bit SEND4STUFFBIT(0); @@ -268,15 +275,15 @@ static void CodeIso14443bAsTag(const uint8_t *cmd, int len) { // Data bits b = cmd[i]; - for(j = 0; j < 8; ++j) { + for (j = 0; j < 8; ++j) { // if(b & 1) { - // SEND4STUFFBIT(1); - // //ToSendStuffBit(1); + // SEND4STUFFBIT(1); + // //ToSendStuffBit(1); // } else { - // SEND4STUFFBIT(0); - // //ToSendStuffBit(0); + // SEND4STUFFBIT(0); + // //ToSendStuffBit(0); // } - SEND4STUFFBIT( b & 1 ); + SEND4STUFFBIT(b & 1); b >>= 1; } @@ -292,11 +299,11 @@ static void CodeIso14443bAsTag(const uint8_t *cmd, int len) { // Send EOF. // 10-11 ETU * 4 sample rate = ZEROS - for(i = 0; i < 10; i++) { SEND4STUFFBIT(0); } + for (i = 0; i < 10; i++) { SEND4STUFFBIT(0); } //for(i = 0; i < 10; i++) { ToSendStuffBit(0); } // why this? - for(i = 0; i < 40; i++) { SEND4STUFFBIT(1); } + for (i = 0; i < 40; i++) { SEND4STUFFBIT(1); } //for(i = 0; i < 40; i++) { ToSendStuffBit(1); } // Convert from last byte pos to length @@ -316,7 +323,8 @@ static void CodeIso14443bAsTag(const uint8_t *cmd, int len) { * Returns: true if we received a EOF * false if we are still waiting for some more */ -static RAMFUNC int Handle14443bReaderUartBit(uint8_t bit) { +static RAMFUNC int Handle14443bReaderUartBit(uint8_t bit) +{ switch (Uart.state) { case STATE_UNSYNCD: if (!bit) { @@ -358,7 +366,7 @@ static RAMFUNC int Handle14443bReaderUartBit(uint8_t bit) { case STATE_AWAITING_START_BIT: Uart.posCnt++; if (bit) { - if (Uart.posCnt > 50/2) { // max 57us between characters = 49 1/fs, max 3 etus after low phase of SOF = 24 1/fs + if (Uart.posCnt > 50 / 2) { // max 57us between characters = 49 1/fs, max 3 etus after low phase of SOF = 24 1/fs // stayed high for too long between characters, error Uart.state = STATE_UNSYNCD; } @@ -385,8 +393,7 @@ static RAMFUNC int Handle14443bReaderUartBit(uint8_t bit) { Uart.posCnt = 0; } if (Uart.bitCnt == 10) { - if ((Uart.shiftReg & 0x200) && !(Uart.shiftReg & 0x001)) - { + if ((Uart.shiftReg & 0x200) && !(Uart.shiftReg & 0x001)) { // this is a data byte, with correct // start and stop bits Uart.output[Uart.byteCnt] = (Uart.shiftReg >> 1) & 0xff; @@ -433,7 +440,8 @@ static RAMFUNC int Handle14443bReaderUartBit(uint8_t bit) { // Assume that we're called with the SSC (to the FPGA) and ADC path set // correctly. //----------------------------------------------------------------------------- -static int GetIso14443bCommandFromReader(uint8_t *received, uint16_t *len) { +static int GetIso14443bCommandFromReader(uint8_t *received, uint16_t *len) +{ // Set FPGA mode to "simulated ISO 14443B tag", no modulation (listen // only, since we are receiving, not transmitting). // Signal field is off with the appropriate LED @@ -447,21 +455,21 @@ static int GetIso14443bCommandFromReader(uint8_t *received, uint16_t *len) { // clear receiving shift register and holding register // What does this loop do? Is it TR1? // loop is a wait/delay ? -/* - for(uint8_t c = 0; c < 10;) { + /* + for(uint8_t c = 0; c < 10;) { - // keep tx buffer in a defined state anyway. - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { - AT91C_BASE_SSC->SSC_THR = 0xFF; - ++c; + // keep tx buffer in a defined state anyway. + if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + AT91C_BASE_SSC->SSC_THR = 0xFF; + ++c; + } } - } - */ + */ // Now run a `software UART' on the stream of incoming samples. UartInit(received); uint8_t mask; - while( !BUTTON_PRESS() ) { + while (!BUTTON_PRESS()) { WDT_HIT(); // keep tx buffer in a defined state anyway. @@ -470,12 +478,12 @@ static int GetIso14443bCommandFromReader(uint8_t *received, uint16_t *len) { } // Wait for byte be become available in rx holding register - if ( AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY ) { + if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { b = (uint8_t) AT91C_BASE_SSC->SSC_RHR; - for ( mask = 0x80; mask != 0; mask >>= 1) { - if ( Handle14443bReaderUartBit(b & mask)) { + for (mask = 0x80; mask != 0; mask >>= 1) { + if (Handle14443bReaderUartBit(b & mask)) { *len = Uart.byteCnt; return true; } @@ -485,18 +493,21 @@ static int GetIso14443bCommandFromReader(uint8_t *received, uint16_t *len) { return false; } -void ClearFpgaShiftingRegisters(void){ +void ClearFpgaShiftingRegisters(void) +{ volatile uint8_t b; // clear receiving shift register and holding register - while(!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)) {}; + while (!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)) {}; - b = AT91C_BASE_SSC->SSC_RHR; (void) b; + b = AT91C_BASE_SSC->SSC_RHR; + (void) b; - while(!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)) {}; + while (!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)) {}; - b = AT91C_BASE_SSC->SSC_RHR; (void) b; + b = AT91C_BASE_SSC->SSC_RHR; + (void) b; // wait for the FPGA to signal fdt_indicator == 1 (the FPGA is ready to queue new data in its delay line) for (uint8_t j = 0; j < 5; j++) { // allow timeout - better late than never @@ -508,18 +519,20 @@ void ClearFpgaShiftingRegisters(void){ //AT91C_BASE_SSC->SSC_THR = 0xFF; } -void WaitForFpgaDelayQueueIsEmpty( uint16_t delay ){ +void WaitForFpgaDelayQueueIsEmpty(uint16_t delay) +{ // Ensure that the FPGA Delay Queue is empty before we switch to TAGSIM_LISTEN again: uint8_t fpga_queued_bits = delay >> 3; // twich /8 ?? >>3, - for (uint8_t i = 0; i <= fpga_queued_bits/8 + 1; ) { - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + for (uint8_t i = 0; i <= fpga_queued_bits / 8 + 1;) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = 0xFF; i++; } } } -static void TransmitFor14443b_AsTag( uint8_t *response, uint16_t len) { +static void TransmitFor14443b_AsTag(uint8_t *response, uint16_t len) +{ volatile uint32_t b; @@ -536,16 +549,17 @@ static void TransmitFor14443b_AsTag( uint8_t *response, uint16_t len) { FpgaSetupSsc(); // Transmit the response. - for(uint16_t i = 0; i < len;) { + for (uint16_t i = 0; i < len;) { - // Put byte into tx holding register as soon as it is ready - if(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) { + // Put byte into tx holding register as soon as it is ready + if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) { AT91C_BASE_SSC->SSC_THR = response[++i]; } // Prevent rx holding register from overflowing - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { - b = AT91C_BASE_SSC->SSC_RHR;(void)b; + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { + b = AT91C_BASE_SSC->SSC_RHR; + (void)b; } } @@ -556,7 +570,8 @@ static void TransmitFor14443b_AsTag( uint8_t *response, uint16_t len) { // Main loop of simulated tag: receive commands from reader, decide what // response to send, and send it. //----------------------------------------------------------------------------- -void SimulateIso14443bTag(uint32_t pupi) { +void SimulateIso14443bTag(uint32_t pupi) +{ // setup device. FpgaDownloadAndGo(FPGA_BITSTREAM_HF); @@ -566,7 +581,8 @@ void SimulateIso14443bTag(uint32_t pupi) { FpgaSetupSsc(); // allocate command receive buffer - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); //sim set_tracing(true); @@ -590,14 +606,15 @@ void SimulateIso14443bTag(uint32_t pupi) { // supports only 106kBit/s in both directions, max frame size = 32Bytes, // supports ISO14443-4, FWI=8 (77ms), NAD supported, CID not supported: uint8_t respATQB[] = { 0x50, 0x82, 0x0d, 0xe1, 0x74, 0x20, 0x38, 0x19, - 0x22, 0x00, 0x21, 0x85, 0x5e, 0xd7 }; + 0x22, 0x00, 0x21, 0x85, 0x5e, 0xd7 + }; // response to HLTB and ATTRIB static const uint8_t respOK[] = {0x00, 0x78, 0xF0}; // ...PUPI/UID supplied from user. Adjust ATQB response accordingly - if ( pupi > 0 ) { - num_to_bytes(pupi, 4, respATQB+1); + if (pupi > 0) { + num_to_bytes(pupi, 4, respATQB + 1); AddCrc14B(respATQB, 12); } @@ -621,7 +638,7 @@ void SimulateIso14443bTag(uint32_t pupi) { // find reader field if (cardSTATE == SIM_NOFIELD) { vHf = (MAX_ADC_HF_VOLTAGE * AvgAdc(ADC_CHAN_HF)) >> 10; - if ( vHf > MF_MINFIELDV ) { + if (vHf > MF_MINFIELDV) { cardSTATE = SIM_IDLE; LED_A_ON(); } @@ -637,9 +654,9 @@ void SimulateIso14443bTag(uint32_t pupi) { // ISO14443-B protocol states: // REQ or WUP request in ANY state // WUP in HALTED state - if (len == 5 ) { - if ( (receivedCmd[0] == ISO14443B_REQB && (receivedCmd[2] & 0x8)== 0x8 && cardSTATE == SIM_HALTED) || - receivedCmd[0] == ISO14443B_REQB ){ + if (len == 5) { + if ((receivedCmd[0] == ISO14443B_REQB && (receivedCmd[2] & 0x8) == 0x8 && cardSTATE == SIM_HALTED) || + receivedCmd[0] == ISO14443B_REQB) { LogTrace(receivedCmd, len, 0, 0, NULL, true); cardSTATE = SIM_SELECTING; } @@ -663,27 +680,27 @@ void SimulateIso14443bTag(uint32_t pupi) { break; } case SIM_SELECTING: { - TransmitFor14443b_AsTag( encodedATQB, encodedATQBLen ); + TransmitFor14443b_AsTag(encodedATQB, encodedATQBLen); LogTrace(respATQB, sizeof(respATQB), 0, 0, NULL, false); cardSTATE = SIM_WORK; break; } case SIM_HALTING: { - TransmitFor14443b_AsTag( encodedOK, encodedOKLen ); + TransmitFor14443b_AsTag(encodedOK, encodedOKLen); LogTrace(respOK, sizeof(respOK), 0, 0, NULL, false); cardSTATE = SIM_HALTED; break; } case SIM_ACKNOWLEDGE: { - TransmitFor14443b_AsTag( encodedOK, encodedOKLen ); + TransmitFor14443b_AsTag(encodedOK, encodedOKLen); LogTrace(respOK, sizeof(respOK), 0, 0, NULL, false); cardSTATE = SIM_IDLE; break; } case SIM_WORK: { - if ( len == 7 && receivedCmd[0] == ISO14443B_HALT ) { + if (len == 7 && receivedCmd[0] == ISO14443B_HALT) { cardSTATE = SIM_HALTED; - } else if ( len == 11 && receivedCmd[0] == ISO14443B_ATTRIB ) { + } else if (len == 11 && receivedCmd[0] == ISO14443B_ATTRIB) { cardSTATE = SIM_ACKNOWLEDGE; } else { // Todo: @@ -693,7 +710,7 @@ void SimulateIso14443bTag(uint32_t pupi) { Dbprintf("new cmd from reader: len=%d, cmdsRecvd=%d", len, cmdsReceived); // CRC Check - if (len >= 3){ // if crc exists + if (len >= 3) { // if crc exists if (!check_crc(CRC_14443_B, receivedCmd, len)) DbpString("+++CRC fail"); @@ -704,7 +721,8 @@ void SimulateIso14443bTag(uint32_t pupi) { } break; } - default: break; + default: + break; } ++cmdsReceived; @@ -735,7 +753,8 @@ void SimulateIso14443bTag(uint32_t pupi) { * false if we are still waiting for some more * */ -static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { +static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) +{ int v = 0, myI = ABS(ci), myQ = ABS(cq); // The soft decision on the bit uses an estimate of just the @@ -790,7 +809,7 @@ static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { //note: couldn't we just use MAX(ABS(ci),ABS(cq)) + (MIN(ABS(ci),ABS(cq))/2) from common.h - marshmellow #define CHECK_FOR_SUBCARRIER() { v = MAX(myI, myQ) + (MIN(myI, myQ) >> 1); } - switch(Demod.state) { + switch (Demod.state) { case DEMOD_UNSYNCD: CHECK_FOR_SUBCARRIER(); @@ -834,7 +853,7 @@ static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { Demod.posCount = 0; // start of SOF sequence } else { // maximum length of TR1 = 200 1/fs - if (Demod.posCount > 200/4) Demod.state = DEMOD_UNSYNCD; + if (Demod.posCount > 200 / 4) Demod.state = DEMOD_UNSYNCD; } Demod.posCount++; break; @@ -846,7 +865,7 @@ static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { if (v > 0) { // low phase of SOF too short (< 9 etu). Note: spec is >= 10, but FPGA tends to "smear" edges - if (Demod.posCount < 9*2) { + if (Demod.posCount < 9 * 2) { Demod.state = DEMOD_UNSYNCD; } else { LED_C_ON(); // Got SOF @@ -856,7 +875,7 @@ static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { } } else { // low phase of SOF too long (> 12 etu) - if (Demod.posCount > 14*2) { + if (Demod.posCount > 14 * 2) { Demod.state = DEMOD_UNSYNCD; LED_C_OFF(); } @@ -869,7 +888,7 @@ static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { MAKE_SOFT_DECISION(); if (v > 0) { - if (Demod.posCount > 6*2) { // max 19us between characters = 16 1/fs, max 3 etu after low phase of SOF = 24 1/fs + if (Demod.posCount > 6 * 2) { // max 19us between characters = 16 1/fs, max 3 etu after low phase of SOF = 24 1/fs Demod.state = DEMOD_UNSYNCD; LED_C_OFF(); } @@ -907,7 +926,7 @@ static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { uint16_t s = Demod.shiftReg; // stop bit == '1', start bit == '0' - if ((s & 0x200) && (s & 0x001) == 0 ) { + if ((s & 0x200) && (s & 0x001) == 0) { // left shift to drop the startbit uint8_t b = (s >> 1); Demod.output[Demod.len] = b; @@ -939,7 +958,8 @@ static RAMFUNC int Handle14443bTagSamplesDemod(int ci, int cq) { * Demodulate the samples we received from the tag, also log to tracebuffer * quiet: set to 'TRUE' to disable debug output */ -static void GetTagSamplesFor14443bDemod() { +static void GetTagSamplesFor14443bDemod() +{ bool gotFrame = false, finished = false; int lastRxCounter = ISO14443B_DMA_BUFFER_SIZE; int ci = 0, cq = 0; @@ -951,11 +971,11 @@ static void GetTagSamplesFor14443bDemod() { DemodInit(BigBuf_malloc(MAX_FRAME_SIZE)); // The DMA buffer, used to stream samples from the FPGA - int8_t *dmaBuf = (int8_t*) BigBuf_malloc(ISO14443B_DMA_BUFFER_SIZE); + int8_t *dmaBuf = (int8_t *) BigBuf_malloc(ISO14443B_DMA_BUFFER_SIZE); int8_t *upTo = dmaBuf; // Setup and start DMA. - if ( !FpgaSetupSscDma((uint8_t*) dmaBuf, ISO14443B_DMA_BUFFER_SIZE) ){ + if (!FpgaSetupSscDma((uint8_t *) dmaBuf, ISO14443B_DMA_BUFFER_SIZE)) { if (MF_DBGLEVEL > 1) Dbprintf("FpgaSetupSscDma failed. Exiting"); return; } @@ -967,7 +987,7 @@ static void GetTagSamplesFor14443bDemod() { time_0 = GetCountSspClk(); // rx counter - dma counter? (how much?) & (mod) mask > 2. (since 2bytes at the time is read) - while ( !finished ) { + while (!finished) { LED_A_INV(); WDT_HIT(); @@ -979,7 +999,7 @@ static void GetTagSamplesFor14443bDemod() { lastRxCounter -= 2; // restart DMA buffer to receive again. - if(upTo >= dmaBuf + ISO14443B_DMA_BUFFER_SIZE) { + if (upTo >= dmaBuf + ISO14443B_DMA_BUFFER_SIZE) { upTo = dmaBuf; lastRxCounter = ISO14443B_DMA_BUFFER_SIZE; AT91C_BASE_PDC_SSC->PDC_RNPR = (uint32_t) upTo; @@ -995,17 +1015,18 @@ static void GetTagSamplesFor14443bDemod() { FpgaDisableSscDma(); - if ( upTo ) + if (upTo) upTo = NULL; - if ( Demod.len > 0 ) + if (Demod.len > 0) LogTrace(Demod.output, Demod.len, time_0, time_stop, NULL, false); } //----------------------------------------------------------------------------- // Transmit the command (to the tag) that was placed in ToSend[]. //----------------------------------------------------------------------------- -static void TransmitFor14443b_AsReader(void) { +static void TransmitFor14443b_AsReader(void) +{ int c; FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_TX | FPGA_HF_READER_TX_SHALLOW_MOD); @@ -1016,16 +1037,16 @@ static void TransmitFor14443b_AsReader(void) { // loop 10 * 8 = 80 ETU of delay, with a non modulated signal. why? // 80*9 = 720us. - for(c = 0; c < 50;) { + for (c = 0; c < 50;) { - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = 0xFF; c++; } } // Send frame loop - for(c = 0; c < ToSendMax;) { + for (c = 0; c < ToSendMax;) { // Put byte into tx holding register as soon as it is ready if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { @@ -1039,7 +1060,8 @@ static void TransmitFor14443b_AsReader(void) { // Code a layer 2 command (string of octets, including CRC) into ToSend[], // so that it is ready to transmit to the tag using TransmitFor14443b(). //----------------------------------------------------------------------------- -static void CodeIso14443bAsReader(const uint8_t *cmd, int len) { +static void CodeIso14443bAsReader(const uint8_t *cmd, int len) +{ /* * Reader data transmission: * - no modulation ONES @@ -1061,7 +1083,7 @@ static void CodeIso14443bAsReader(const uint8_t *cmd, int len) { // Send SOF // 10-11 ETUs of ZERO - for(i = 0; i < 10; ++i) ToSendStuffBit(0); + for (i = 0; i < 10; ++i) ToSendStuffBit(0); // 2-3 ETUs of ONE ToSendStuffBit(1); @@ -1070,7 +1092,7 @@ static void CodeIso14443bAsReader(const uint8_t *cmd, int len) { // Sending cmd, LSB // from here we add BITS - for(i = 0; i < len; ++i) { + for (i = 0; i < len; ++i) { // Start bit ToSendStuffBit(0); // Data bits @@ -1084,14 +1106,14 @@ static void CodeIso14443bAsReader(const uint8_t *cmd, int len) { // if ( (b>>6) & 1) ToSendStuffBit(1); else ToSendStuffBit(0); // if ( (b>>7) & 1) ToSendStuffBit(1); else ToSendStuffBit(0); - ToSendStuffBit( b & 1); - ToSendStuffBit( (b>>1) & 1); - ToSendStuffBit( (b>>2) & 1); - ToSendStuffBit( (b>>3) & 1); - ToSendStuffBit( (b>>4) & 1); - ToSendStuffBit( (b>>5) & 1); - ToSendStuffBit( (b>>6) & 1); - ToSendStuffBit( (b>>7) & 1); + ToSendStuffBit(b & 1); + ToSendStuffBit((b >> 1) & 1); + ToSendStuffBit((b >> 2) & 1); + ToSendStuffBit((b >> 3) & 1); + ToSendStuffBit((b >> 4) & 1); + ToSendStuffBit((b >> 5) & 1); + ToSendStuffBit((b >> 6) & 1); + ToSendStuffBit((b >> 7) & 1); // Stop bit ToSendStuffBit(1); @@ -1104,13 +1126,13 @@ static void CodeIso14443bAsReader(const uint8_t *cmd, int len) { // Send EOF // 10-11 ETUs of ZERO - for(i = 0; i < 10; ++i) ToSendStuffBit(0); + for (i = 0; i < 10; ++i) ToSendStuffBit(0); // Transition time. TR0 - guard time // 8ETUS minum? // Per specification, Subcarrier must be stopped no later than 2 ETUs after EOF. // I'm guessing this is for the FPGA to be able to send all bits before we switch to listening mode - for(i = 0; i < 24 ; ++i) ToSendStuffBit(1); + for (i = 0; i < 24 ; ++i) ToSendStuffBit(1); // TR1 - Synchronization time // Convert from last character reference to length @@ -1120,7 +1142,8 @@ static void CodeIso14443bAsReader(const uint8_t *cmd, int len) { /* * Convenience function to encode, transmit and trace iso 14443b comms */ -static void CodeAndTransmit14443bAsReader(const uint8_t *cmd, int len) { +static void CodeAndTransmit14443bAsReader(const uint8_t *cmd, int len) +{ uint32_t time_start = GetCountSspClk(); @@ -1128,15 +1151,16 @@ static void CodeAndTransmit14443bAsReader(const uint8_t *cmd, int len) { TransmitFor14443b_AsReader(); - if(trigger) LED_A_ON(); + if (trigger) LED_A_ON(); - LogTrace(cmd, len, time_start, GetCountSspClk()-time_start, NULL, true); + LogTrace(cmd, len, time_start, GetCountSspClk() - time_start, NULL, true); } /* Sends an APDU to the tag * TODO: check CRC and preamble */ -uint8_t iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *response) { +uint8_t iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *response) +{ uint8_t message_frame[message_length + 4]; // PCB @@ -1152,16 +1176,16 @@ uint8_t iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *r CodeAndTransmit14443bAsReader(message_frame, message_length + 4); //no // get response GetTagSamplesFor14443bDemod(); //no - if(Demod.len < 3) + if (Demod.len < 3) return 0; // VALIDATE CRC - if (!check_crc(CRC_14443_B, Demod.output, Demod.len)){ + if (!check_crc(CRC_14443_B, Demod.output, Demod.len)) { if (MF_DBGLEVEL > 3) Dbprintf("crc fail ICE"); return 0; } // copy response contents - if(response != NULL) + if (response != NULL) memcpy(response, Demod.output, Demod.len); return Demod.len; @@ -1170,7 +1194,8 @@ uint8_t iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *r /** * SRx Initialise. */ -uint8_t iso14443b_select_srx_card(iso14b_card_select_t *card ) { +uint8_t iso14443b_select_srx_card(iso14b_card_select_t *card) +{ // INITIATE command: wake up the tag using the INITIATE static const uint8_t init_srx[] = { ISO14443B_INITIATE, 0x00, 0x97, 0x5b }; // SELECT command (with space for CRC) @@ -1230,7 +1255,8 @@ uint8_t iso14443b_select_srx_card(iso14b_card_select_t *card ) { * TODO: Support multiple cards (perform anticollision) * TODO: Verify CRC checksums */ -uint8_t iso14443b_select_card(iso14b_card_select_t *card ) { +uint8_t iso14443b_select_card(iso14b_card_select_t *card) +{ // WUPB command (including CRC) // Note: WUPB wakes up all tags, REQB doesn't wake up tags in HALT state static const uint8_t wupb[] = { ISO14443B_REQB, 0x00, 0x08, 0x39, 0x73 }; @@ -1251,8 +1277,8 @@ uint8_t iso14443b_select_card(iso14b_card_select_t *card ) { if (card) { card->uidlen = 4; - memcpy(card->uid, Demod.output+1, 4); - memcpy(card->atqb, Demod.output+5, 7); + memcpy(card->uid, Demod.output + 1, 4); + memcpy(card->atqb, Demod.output + 5, 7); } // copy the PUPI to ATTRIB ( PUPI == UID ) @@ -1266,11 +1292,11 @@ uint8_t iso14443b_select_card(iso14b_card_select_t *card ) { GetTagSamplesFor14443bDemod();//select_card // Answer to ATTRIB too short? - if(Demod.len < 3) + if (Demod.len < 3) return 2; // VALIDATE CRC - if (!check_crc(CRC_14443_B, Demod.output, Demod.len) ) + if (!check_crc(CRC_14443_B, Demod.output, Demod.len)) return 3; if (card) { @@ -1290,9 +1316,9 @@ uint8_t iso14443b_select_card(iso14b_card_select_t *card ) { // FWT uint8_t fwt = card->atqb[6] >> 4; - if ( fwt < 16 ){ + if (fwt < 16) { uint32_t fwt_time = (302 << fwt); - iso14b_set_timeout( fwt_time); + iso14b_set_timeout(fwt_time); } } // reset PCB block number @@ -1302,7 +1328,8 @@ uint8_t iso14443b_select_card(iso14b_card_select_t *card ) { // Set up ISO 14443 Type B communication (similar to iso14443a_setup) // field is setup for "Sending as Reader" -void iso14443b_setup() { +void iso14443b_setup() +{ LEDsoff(); FpgaDownloadAndGo(FPGA_BITSTREAM_HF); @@ -1335,7 +1362,8 @@ void iso14443b_setup() { // // I tried to be systematic and check every answer of the tag, every CRC, etc... //----------------------------------------------------------------------------- -static bool ReadSTBlock(uint8_t block) { +static bool ReadSTBlock(uint8_t block) +{ uint8_t cmd[] = {ISO14443B_READ_BLK, block, 0x00, 0x00}; AddCrc14B(cmd, 2); CodeAndTransmit14443bAsReader(cmd, sizeof(cmd)); @@ -1353,7 +1381,8 @@ static bool ReadSTBlock(uint8_t block) { } return true; } -void ReadSTMemoryIso14443b(uint8_t numofblocks) { +void ReadSTMemoryIso14443b(uint8_t numofblocks) +{ // Make sure that we start from off, since the tags are stateful; // confusing things will happen if we don't reset them between reads. //switch_off(); @@ -1363,11 +1392,11 @@ void ReadSTMemoryIso14443b(uint8_t numofblocks) { iso14443b_setup(); - iso14b_card_select_t *card = (iso14b_card_select_t*)buf; + iso14b_card_select_t *card = (iso14b_card_select_t *)buf; uint8_t res = iso14443b_select_srx_card(card); // 0: OK 2: attrib fail, 3:crc fail, - if ( res > 0 ) goto out; + if (res > 0) goto out; Dbprintf("[+] Tag memory dump, block 0 to %d", numofblocks); @@ -1390,8 +1419,8 @@ void ReadSTMemoryIso14443b(uint8_t numofblocks) { // Now print out the memory location: Dbprintf("Address=%02x, Contents=%08x, CRC=%04x", i, - (Demod.output[3]<<24) + (Demod.output[2]<<16) + (Demod.output[1]<<8) + Demod.output[0], - (Demod.output[4]<<8)+Demod.output[5]); + (Demod.output[3] << 24) + (Demod.output[2] << 16) + (Demod.output[1] << 8) + Demod.output[0], + (Demod.output[4] << 8) + Demod.output[5]); if (i == 0xff) break; ++i; @@ -1402,7 +1431,8 @@ out: SpinDelay(20); } -static void iso1444b_setup_sniff(void){ +static void iso1444b_setup_sniff(void) +{ LEDsoff(); FpgaDownloadAndGo(FPGA_BITSTREAM_HF); @@ -1455,7 +1485,8 @@ static void iso1444b_setup_sniff(void){ * DMA Buffer - ISO14443B_DMA_BUFFER_SIZE * Demodulated samples received - all the rest */ -void RAMFUNC SniffIso14443b(void) { +void RAMFUNC SniffIso14443b(void) +{ uint32_t time_0 = 0, time_start = 0, time_stop = 0; int ci = 0, cq = 0; @@ -1469,11 +1500,11 @@ void RAMFUNC SniffIso14443b(void) { iso1444b_setup_sniff(); // The DMA buffer, used to stream samples from the FPGA - int8_t *dmaBuf = (int8_t*) BigBuf_malloc(ISO14443B_DMA_BUFFER_SIZE); + int8_t *dmaBuf = (int8_t *) BigBuf_malloc(ISO14443B_DMA_BUFFER_SIZE); int8_t *data = dmaBuf; // Setup and start DMA. - if ( !FpgaSetupSscDma((uint8_t*) dmaBuf, ISO14443B_DMA_BUFFER_SIZE) ){ + if (!FpgaSetupSscDma((uint8_t *) dmaBuf, ISO14443B_DMA_BUFFER_SIZE)) { if (MF_DBGLEVEL > 1) Dbprintf("[!] FpgaSetupSscDma failed. Exiting"); BigBuf_free(); return; @@ -1547,7 +1578,8 @@ void RAMFUNC SniffIso14443b(void) { switch_off(); } -void iso14b_set_trigger(bool enable) { +void iso14b_set_trigger(bool enable) +{ trigger = enable; } @@ -1562,7 +1594,8 @@ void iso14b_set_trigger(bool enable) { * none * */ -void SendRawCommand14443B_Ex(UsbCommand *c) { +void SendRawCommand14443B_Ex(UsbCommand *c) +{ iso14b_command_t param = c->arg[0]; size_t len = c->arg[1] & 0xffff; uint8_t *cmd = c->d.asBytes; @@ -1570,7 +1603,7 @@ void SendRawCommand14443B_Ex(UsbCommand *c) { uint32_t sendlen = sizeof(iso14b_card_select_t); uint8_t buf[USB_CMD_DATA_SIZE] = {0x00}; - if (MF_DBGLEVEL > 3) Dbprintf("14b raw: param, %04x", param ); + if (MF_DBGLEVEL > 3) Dbprintf("14b raw: param, %04x", param); // turn on trigger (LED_A) if ((param & ISO14B_REQUEST_TRIGGER) == ISO14B_REQUEST_TRIGGER) @@ -1584,19 +1617,19 @@ void SendRawCommand14443B_Ex(UsbCommand *c) { set_tracing(true); if ((param & ISO14B_SELECT_STD) == ISO14B_SELECT_STD) { - iso14b_card_select_t *card = (iso14b_card_select_t*)buf; + iso14b_card_select_t *card = (iso14b_card_select_t *)buf; status = iso14443b_select_card(card); cmd_send(CMD_ACK, status, sendlen, 0, buf, sendlen); // 0: OK 2: attrib fail, 3:crc fail, - if ( status > 0 ) goto out; + if (status > 0) goto out; } if ((param & ISO14B_SELECT_SR) == ISO14B_SELECT_SR) { - iso14b_card_select_t *card = (iso14b_card_select_t*)buf; + iso14b_card_select_t *card = (iso14b_card_select_t *)buf; status = iso14443b_select_srx_card(card); cmd_send(CMD_ACK, status, sendlen, 0, buf, sendlen); // 0: OK 2: demod fail, 3:crc fail, - if ( status > 0 ) goto out; + if (status > 0) goto out; } if ((param & ISO14B_APDU) == ISO14B_APDU) { @@ -1605,7 +1638,7 @@ void SendRawCommand14443B_Ex(UsbCommand *c) { } if ((param & ISO14B_RAW) == ISO14B_RAW) { - if((param & ISO14B_APPEND_CRC) == ISO14B_APPEND_CRC) { + if ((param & ISO14B_APPEND_CRC) == ISO14B_APPEND_CRC) { AddCrc14B(cmd, len); len += 2; } @@ -1614,7 +1647,7 @@ void SendRawCommand14443B_Ex(UsbCommand *c) { GetTagSamplesFor14443bDemod(); // raw sendlen = MIN(Demod.len, USB_CMD_DATA_SIZE); - status = (Demod.len > 0) ? 0 : 1; + status = (Demod.len > 0) ? 0 : 1; cmd_send(CMD_ACK, status, sendlen, 0, Demod.output, sendlen); } diff --git a/armsrc/iso14443b.h b/armsrc/iso14443b.h index c7e98f182..e3c6c1dc8 100644 --- a/armsrc/iso14443b.h +++ b/armsrc/iso14443b.h @@ -37,11 +37,11 @@ extern "C" { extern void SendRawCommand14443B_Ex(UsbCommand *c); extern void iso14443b_setup(); extern uint8_t iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *response); -extern uint8_t iso14443b_select_card(iso14b_card_select_t* card); -extern uint8_t iso14443b_select_card_srx(iso14b_card_select_t* card); +extern uint8_t iso14443b_select_card(iso14b_card_select_t *card); +extern uint8_t iso14443b_select_card_srx(iso14b_card_select_t *card); // testfunctions -extern void WaitForFpgaDelayQueueIsEmpty( uint16_t delay ); +extern void WaitForFpgaDelayQueueIsEmpty(uint16_t delay); extern void ClearFpgaShiftingRegisters(void); // States for 14B SIM command diff --git a/armsrc/iso15693.c b/armsrc/iso15693.c index 1afaac95f..5702ddcac 100644 --- a/armsrc/iso15693.c +++ b/armsrc/iso15693.c @@ -99,13 +99,14 @@ static void BuildInventoryResponse(uint8_t *cmdout, uint8_t *uid); // resulting data rate is 26,48 kbit/s (fc/512) // cmd ... data // n ... length of data -static void CodeIso15693AsReader(uint8_t *cmd, int n) { +static void CodeIso15693AsReader(uint8_t *cmd, int n) +{ int i, j; ToSendReset(); // Give it a bit of slack at the beginning - for(i = 0; i < 24; i++) + for (i = 0; i < 24; i++) ToSendStuffBit(1); // SOF for 1of4 @@ -117,10 +118,10 @@ static void CodeIso15693AsReader(uint8_t *cmd, int n) { ToSendStuffBit(0); ToSendStuffBit(1); ToSendStuffBit(1); - for(i = 0; i < n; i++) { - for(j = 0; j < 8; j += 2) { + for (i = 0; i < n; i++) { + for (j = 0; j < 8; j += 2) { int these = (cmd[i] >> j) & 3; - switch(these) { + switch (these) { case 0: ToSendStuffBit(1); ToSendStuffBit(0); @@ -171,20 +172,21 @@ static void CodeIso15693AsReader(uint8_t *cmd, int n) { ToSendStuffBit(1); // And slack at the end, too. - for(i = 0; i < 24; i++) + for (i = 0; i < 24; i++) ToSendStuffBit(1); } // encode data using "1 out of 256" sheme // data rate is 1,66 kbit/s (fc/8192) // is designed for more robust communication over longer distances -static void CodeIso15693AsReader256(uint8_t *cmd, int n) { +static void CodeIso15693AsReader256(uint8_t *cmd, int n) +{ int i, j; ToSendReset(); // Give it a bit of slack at the beginning - for(i = 0; i < 24; i++) + for (i = 0; i < 24; i++) ToSendStuffBit(1); // SOF for 1of256 @@ -197,7 +199,7 @@ static void CodeIso15693AsReader256(uint8_t *cmd, int n) { ToSendStuffBit(1); ToSendStuffBit(0); - for(i = 0; i < n; i++) { + for (i = 0; i < n; i++) { for (j = 0; j <= 255; j++) { if (cmd[i] == j) { ToSendStuffBit(1); @@ -215,12 +217,13 @@ static void CodeIso15693AsReader256(uint8_t *cmd, int n) { ToSendStuffBit(1); // And slack at the end, too. - for(i = 0; i < 24; i++) + for (i = 0; i < 24; i++) ToSendStuffBit(1); } // Transmit the command (to the tag) that was placed in ToSend[]. -static void TransmitTo15693Tag(const uint8_t *cmd, int len, int *samples, int *wait) { +static void TransmitTo15693Tag(const uint8_t *cmd, int len, int *samples, int *wait) +{ int c; FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_TX); @@ -236,10 +239,10 @@ static void TransmitTo15693Tag(const uint8_t *cmd, int len, int *samples, int *w } c = 0; - for(;;) { + for (;;) { if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = cmd[c]; - if( ++c >= len) break; + if (++c >= len) break; } WDT_HIT(); } @@ -255,7 +258,8 @@ static void TransmitTo15693Tag(const uint8_t *cmd, int len, int *samples, int *w //----------------------------------------------------------------------------- // Transmit the command (to the reader) that was placed in ToSend[]. //----------------------------------------------------------------------------- -static void TransmitTo15693Reader(const uint8_t *cmd, int len, int *samples, int *wait) { +static void TransmitTo15693Reader(const uint8_t *cmd, int len, int *samples, int *wait) +{ int c = 0; FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_SIMULATOR | FPGA_HF_SIMULATOR_MODULATE_424K); @@ -270,10 +274,10 @@ static void TransmitTo15693Reader(const uint8_t *cmd, int len, int *samples, int } c = 0; - for(;;) { - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + for (;;) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = cmd[c]; - if( ++c >= len) break; + if (++c >= len) break; } WDT_HIT(); } @@ -288,7 +292,8 @@ static void TransmitTo15693Reader(const uint8_t *cmd, int len, int *samples, int //----------------------------------------------------------------------------- // DEMODULATE tag answer //----------------------------------------------------------------------------- -static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) { +static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) +{ int i, j; int max = 0, maxPos = 0, skip = 4; @@ -297,8 +302,8 @@ static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) { // First, correlate for SOF for (i = 0; i < samplecount; i++) { int corr = 0; - for ( j = 0; j < ARRAYLEN(FrameSOF); j += skip) { - corr += FrameSOF[j] * dest[i+(j/skip)]; + for (j = 0; j < ARRAYLEN(FrameSOF); j += skip) { + corr += FrameSOF[j] * dest[i + (j / skip)]; } if (corr > max) { max = corr; @@ -308,7 +313,7 @@ static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) { // DbpString("SOF at %d, correlation %d", maxPos,max/(ARRAYLEN(FrameSOF)/skip)); // greg - If correlation is less than 1 then there's little point in continuing - if ((max / (ARRAYLEN(FrameSOF)/skip) ) < 1) + if ((max / (ARRAYLEN(FrameSOF) / skip)) < 1) return k; i = maxPos + ARRAYLEN(FrameSOF) / skip; @@ -316,22 +321,22 @@ static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) { uint8_t outBuf[ISO15_MAX_FRAME]; memset(outBuf, 0, sizeof(outBuf)); uint8_t mask = 0x01; - for(;;) { + for (;;) { int corr0 = 0, corr1 = 0, corrEOF = 0; for (j = 0; j < ARRAYLEN(Logic0); j += skip) { - corr0 += Logic0[j] * dest[i+(j/skip)]; + corr0 += Logic0[j] * dest[i + (j / skip)]; } for (j = 0; j < ARRAYLEN(Logic1); j += skip) { - corr1 += Logic1[j] * dest[i+(j/skip)]; + corr1 += Logic1[j] * dest[i + (j / skip)]; } for (j = 0; j < ARRAYLEN(FrameEOF); j += skip) { - corrEOF += FrameEOF[j] * dest[i+(j/skip)]; + corrEOF += FrameEOF[j] * dest[i + (j / skip)]; } // Even things out by the length of the target waveform. corr0 *= 4; corr1 *= 4; // if (MF_DBGLEVEL >= MF_DBG_EXTENDED) - // Dbprintf("Corr1 %d, Corr0 %d, CorrEOF %d", corr1, corr0, corrEOF); + // Dbprintf("Corr1 %d, Corr0 %d, CorrEOF %d", corr1, corr0, corrEOF); if (corrEOF > corr1 && corrEOF > corr0) break; @@ -350,7 +355,7 @@ static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) { mask = 0x01; } - if ( ( i + (int)ARRAYLEN(FrameEOF)) >= samplecount-1) { + if ((i + (int)ARRAYLEN(FrameEOF)) >= samplecount - 1) { //Dbprintf("[!] ran off end! %d | %d",( i + (int)ARRAYLEN(FrameEOF)), samplecount-1); break; } @@ -366,7 +371,7 @@ static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) { // 0x08 is an assumption - but works quite often } - for(i = 0; i < k; i++) + for (i = 0; i < k; i++) received[i] = outBuf[i]; // return the number of bytes demodulated @@ -381,7 +386,8 @@ static int DemodAnswer(uint8_t *received, uint8_t *dest, uint16_t samplecount) { // returns: // number of decoded bytes // logging enabled -static int GetIso15693AnswerFromTag(uint8_t *received, int *elapsed) { +static int GetIso15693AnswerFromTag(uint8_t *received, int *elapsed) +{ #define SIGNAL_BUFF_SIZE 15000 // get current clock @@ -395,13 +401,13 @@ static int GetIso15693AnswerFromTag(uint8_t *received, int *elapsed) { FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR); - for(;;) { + for (;;) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = 0x00; //0x43; // To make use of exact timing of next command from reader!! - if (elapsed) (*elapsed)++; + if (elapsed)(*elapsed)++; } if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { @@ -415,7 +421,7 @@ static int GetIso15693AnswerFromTag(uint8_t *received, int *elapsed) { // iceman 2016, amplitude sqrt(abs(i) + abs(q)) if (getNext) { - buf[counter++] = (uint8_t)(MAX(ci,cq) + (MIN(ci, cq) >> 1)); + buf[counter++] = (uint8_t)(MAX(ci, cq) + (MIN(ci, cq) >> 1)); if (counter >= SIGNAL_BUFF_SIZE) break; @@ -435,7 +441,8 @@ static int GetIso15693AnswerFromTag(uint8_t *received, int *elapsed) { // Now the GetISO15693 message from sniffing command // logging enable, -static int GetIso15693AnswerFromSniff(uint8_t *received, int *samples, int *elapsed) { +static int GetIso15693AnswerFromSniff(uint8_t *received, int *samples, int *elapsed) +{ bool getNext = false; int counter = 0, ci = 0, cq = 0; @@ -447,7 +454,7 @@ static int GetIso15693AnswerFromSniff(uint8_t *received, int *samples, int *elap FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR); - for(;;) { + for (;;) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { @@ -461,12 +468,12 @@ static int GetIso15693AnswerFromSniff(uint8_t *received, int *samples, int *elap // close to what we want. if (getNext) { - buf[counter++] = (uint8_t)(MAX(ci,cq) + (MIN(ci, cq) >> 1)); + buf[counter++] = (uint8_t)(MAX(ci, cq) + (MIN(ci, cq) >> 1)); - if(counter >= 20000) + if (counter >= 20000) break; } else { - cq = ci; + cq = ci; } getNext = !getNext; } @@ -483,7 +490,8 @@ static int GetIso15693AnswerFromSniff(uint8_t *received, int *samples, int *elap // for the response. The response is not demodulated, just left in the buffer // so that it can be downloaded to a PC and processed there. //----------------------------------------------------------------------------- -void AcquireRawAdcSamplesIso15693(void) { +void AcquireRawAdcSamplesIso15693(void) +{ int c = 0, getNext = false; int ci = 0, cq = 0; @@ -503,13 +511,13 @@ void AcquireRawAdcSamplesIso15693(void) { // sending command c = 0; - for(;;) { + for (;;) { WDT_HIT(); - if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) { AT91C_BASE_SSC->SSC_THR = ToSend[c]; c++; - if(c == ToSendMax + 3) { + if (c == ToSendMax + 3) { break; } } @@ -521,7 +529,7 @@ void AcquireRawAdcSamplesIso15693(void) { FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR); c = 0; - for(;;) { + for (;;) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { @@ -536,7 +544,7 @@ void AcquireRawAdcSamplesIso15693(void) { // iceman 2016, amplitude sqrt(abs(i) + abs(q)) if (getNext) { - buf[c++] = (uint8_t)(MAX(ci,cq) + (MIN(ci, cq) >> 1)); + buf[c++] = (uint8_t)(MAX(ci, cq) + (MIN(ci, cq) >> 1)); if (c >= 7000) break; @@ -549,7 +557,8 @@ void AcquireRawAdcSamplesIso15693(void) { } // switch_off, initreader, no logging -void RecordRawAdcSamplesIso15693(void) { +void RecordRawAdcSamplesIso15693(void) +{ int c = 0, getNext = false; int ci = 0, cq = 0; @@ -558,7 +567,7 @@ void RecordRawAdcSamplesIso15693(void) { uint8_t *buf = BigBuf_get_addr(); - for(;;) { + for (;;) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { @@ -571,9 +580,9 @@ void RecordRawAdcSamplesIso15693(void) { // close to what we want. if (getNext) { - buf[c++] = (uint8_t)(MAX(ci,cq) + (MIN(ci, cq) >> 1)); + buf[c++] = (uint8_t)(MAX(ci, cq) + (MIN(ci, cq) >> 1)); - if(c >= 7000) + if (c >= 7000) break; } else { cq = ci; @@ -589,7 +598,8 @@ void RecordRawAdcSamplesIso15693(void) { // Initialize the proxmark as iso15k reader // (this might produces glitches that confuse some tags -void Iso15693InitReader(void) { +void Iso15693InitReader(void) +{ LEDsoff(); clear_trace(); set_tracing(true); @@ -621,7 +631,8 @@ void Iso15693InitReader(void) { // Encode (into the ToSend buffers) an identify request, which is the first // thing that you must send to a tag to get a response. -static void BuildIdentifyRequest(uint8_t *out) { +static void BuildIdentifyRequest(uint8_t *out) +{ uint8_t cmd[CMD_ID_RESP] = {0, ISO15_CMD_INVENTORY, 0, 0, 0}; // flags @@ -665,9 +676,10 @@ static void BuildReadBlockRequest(uint8_t **out, uint8_t *uid, uint8_t blockNumb */ // Now the VICC>VCD responses when we are simulating a tag -static void BuildInventoryResponse(uint8_t *out, uint8_t *uid) { +static void BuildInventoryResponse(uint8_t *out, uint8_t *uid) +{ - uint8_t cmd[CMD_INV_RESP] = {0,0,0,0,0,0,0,0,0,0,0,0}; + uint8_t cmd[CMD_INV_RESP] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // one sub-carrier, inventory, 1 slot, fast rate // AFI is at bit 5 (1<<4) when doing an INVENTORY @@ -696,7 +708,8 @@ static void BuildInventoryResponse(uint8_t *out, uint8_t *uid) { // If you do not need the answer use NULL for *recv[] // return: lenght of received data // logging enabled -int SendDataTag(uint8_t *send, int sendlen, bool init, int speed, uint8_t *outdata) { +int SendDataTag(uint8_t *send, int sendlen, bool init, int speed, uint8_t *outdata) +{ int t_samples = 0, wait = 0, elapsed = 0, answer_len = 0; @@ -734,11 +747,12 @@ int SendDataTag(uint8_t *send, int sendlen, bool init, int speed, uint8_t *outda // Decodes a message from a tag and displays its metadata and content #define DBD15STATLEN 48 -void DbdecodeIso15693Answer(int len, uint8_t *d) { - char status[DBD15STATLEN+1] = {0}; +void DbdecodeIso15693Answer(int len, uint8_t *d) +{ + char status[DBD15STATLEN + 1] = {0}; if (len > 3) { - if (d[0] & ( 1 << 3 )) + if (d[0] & (1 << 3)) strncat(status, "ProtExt ", DBD15STATLEN); if (d[0] & 1) { // error @@ -774,9 +788,9 @@ void DbdecodeIso15693Answer(int len, uint8_t *d) { default: strncat(status, "unknown error", DBD15STATLEN); } - strncat(status ," " ,DBD15STATLEN); + strncat(status, " ", DBD15STATLEN); } else { - strncat(status ,"No error ", DBD15STATLEN); + strncat(status, "No error ", DBD15STATLEN); } if (CheckCrc(d, len)) @@ -784,7 +798,7 @@ void DbdecodeIso15693Answer(int len, uint8_t *d) { else strncat(status, "[!] crc fail", DBD15STATLEN); - if ( MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("%s", status); + if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("%s", status); } } @@ -798,11 +812,12 @@ void DbdecodeIso15693Answer(int len, uint8_t *d) { //----------------------------------------------------------------------------- // ok // parameter is unused !?! -void ReaderIso15693(uint32_t parameter) { +void ReaderIso15693(uint32_t parameter) +{ int answerLen1 = 0; int tsamples = 0, wait = 0, elapsed = 0; - uint8_t uid[8] = {0,0,0,0,0,0,0,0}; + uint8_t uid[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // set up device/fpga Iso15693InitReader(); @@ -819,7 +834,7 @@ void ReaderIso15693(uint32_t parameter) { // THIS MEANS WE CAN PRE-BUILD REQUESTS TO SAVE CPU TIME uint32_t time_start = GetCountSspClk(); uint8_t cmd[CMD_ID_RESP] = {0}; - BuildIdentifyRequest( cmd ); + BuildIdentifyRequest(cmd); TransmitTo15693Tag(ToSend, ToSendMax, &tsamples, &wait); LogTrace(cmd, CMD_ID_RESP, time_start << 4, (GetCountSspClk() - time_start) << 4, NULL, true); @@ -837,11 +852,11 @@ void ReaderIso15693(uint32_t parameter) { uid[6] = answer1[3]; uid[7] = answer1[2]; - if ( MF_DBGLEVEL >= MF_DBG_EXTENDED) { + if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("[+] UID = %02X%02X%02X%02X%02X%02X%02X%02X", - uid[0], uid[1], uid[2], uid[3], - uid[4], uid[5], uid[5], uid[6] - ); + uid[0], uid[1], uid[2], uid[3], + uid[4], uid[5], uid[5], uid[6] + ); } // send UID back to client. // arg0 = 1 = OK @@ -851,7 +866,7 @@ void ReaderIso15693(uint32_t parameter) { cmd_send(CMD_ACK, 1, sizeof(uid), 0, uid, sizeof(uid)); } - if ( MF_DBGLEVEL >= MF_DBG_EXTENDED) { + if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("[+] %d octets read from IDENTIFY request:", answerLen1); DbdecodeIso15693Answer(answerLen1, answer1); Dbhexdump(answerLen1, answer1, true); @@ -862,7 +877,8 @@ void ReaderIso15693(uint32_t parameter) { // Simulate an ISO15693 TAG, perform anti-collision and then print any reader commands // all demodulation performed in arm rather than host. - greg -void SimTagIso15693(uint32_t parameter, uint8_t *uid) { +void SimTagIso15693(uint32_t parameter, uint8_t *uid) +{ LEDsoff(); FpgaDownloadAndGo(FPGA_BITSTREAM_HF); @@ -890,14 +906,14 @@ void SimTagIso15693(uint32_t parameter, uint8_t *uid) { uint8_t cmd[CMD_INV_RESP] = {0}; BuildInventoryResponse(cmd, uid); - while (!BUTTON_PRESS() && !usb_poll_validate_length() ) { + while (!BUTTON_PRESS() && !usb_poll_validate_length()) { WDT_HIT(); // Listen to reader ans = GetIso15693AnswerFromSniff(buf, &samples, &elapsed) ; // we should do a better check than this - if (ans >= 1 ) { + if (ans >= 1) { time_start = GetCountSspClk(); TransmitTo15693Reader(ToSend, ToSendMax, &tsamples, &wait); @@ -905,9 +921,9 @@ void SimTagIso15693(uint32_t parameter, uint8_t *uid) { if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("[+] %d octets read from reader command: %x %x %x %x %x %x %x %x", ans, - buf[0], buf[1], buf[2], buf[3], - buf[4], buf[5], buf[6], buf[7] - ); + buf[0], buf[1], buf[2], buf[3], + buf[4], buf[5], buf[6], buf[7] + ); } } } @@ -916,9 +932,10 @@ void SimTagIso15693(uint32_t parameter, uint8_t *uid) { // Since there is no standardized way of reading the AFI out of a tag, we will brute force it // (some manufactures offer a way to read the AFI, though) -void BruteforceIso15693Afi(uint32_t speed) { +void BruteforceIso15693Afi(uint32_t speed) +{ - uint8_t data[7] = {0,0,0,0,0,0,0}; + uint8_t data[7] = {0, 0, 0, 0, 0, 0, 0}; uint8_t buf[ISO15_MAX_FRAME]; memset(buf, 0x00, sizeof(buf)); int datalen = 0, recvlen = 0; @@ -939,7 +956,7 @@ void BruteforceIso15693Afi(uint32_t speed) { WDT_HIT(); if (recvlen >= 12) { - Dbprintf("NoAFI UID = %s", sprintUID(NULL, buf + 2) ); + Dbprintf("NoAFI UID = %s", sprintUID(NULL, buf + 2)); } // now with AFI @@ -955,7 +972,7 @@ void BruteforceIso15693Afi(uint32_t speed) { recvlen = SendDataTag(data, datalen, false, speed, buf); WDT_HIT(); if (recvlen >= 12) { - Dbprintf("AFI = %i UID = %s", i, sprintUID(NULL, buf + 2) ); + Dbprintf("AFI = %i UID = %s", i, sprintUID(NULL, buf + 2)); } if (BUTTON_PRESS()) { @@ -970,7 +987,8 @@ void BruteforceIso15693Afi(uint32_t speed) { // Allows to directly send commands to the tag via the client // Has to increase dialog between device and client. -void DirectTag15693Command(uint32_t datalen, uint32_t speed, uint32_t recv, uint8_t *data) { +void DirectTag15693Command(uint32_t datalen, uint32_t speed, uint32_t recv, uint8_t *data) +{ bool init = true; int buflen = 0; @@ -997,6 +1015,6 @@ void DirectTag15693Command(uint32_t datalen, uint32_t speed, uint32_t recv, uint Dbhexdump(buflen, buf, true); } } else { - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); } } \ No newline at end of file diff --git a/armsrc/legicrf.c b/armsrc/legicrf.c index 958d3421a..eef626ff7 100644 --- a/armsrc/legicrf.c +++ b/armsrc/legicrf.c @@ -16,7 +16,7 @@ #include "legic_prng.h" /* legic PRNG impl */ #include "legic.h" /* legic_card_select_t struct */ -static uint8_t* legic_mem; /* card memory, used for read, write */ +static uint8_t *legic_mem; /* card memory, used for read, write */ static legic_card_select_t card;/* metadata of currently selected card */ static crc_t legic_crc; @@ -50,21 +50,22 @@ static uint32_t last_frame_end; /* ts of last bit of previews rx or tx frame */ #define WRITE_LOWERLIMIT 4 /* UID and MCC are not writable */ #define INPUT_THRESHOLD 8 /* heuristically determined, lower values */ - /* lead to detecting false ack during write */ +/* lead to detecting false ack during write */ //----------------------------------------------------------------------------- // I/O interface abstraction (FPGA -> ARM) //----------------------------------------------------------------------------- -static inline uint8_t rx_byte_from_fpga() { - for(;;) { - WDT_HIT(); +static inline uint8_t rx_byte_from_fpga() +{ + for (;;) { + WDT_HIT(); - // wait for byte be become available in rx holding register - if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { - return AT91C_BASE_SSC->SSC_RHR; + // wait for byte be become available in rx holding register + if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) { + return AT91C_BASE_SSC->SSC_RHR; + } } - } } //----------------------------------------------------------------------------- @@ -83,11 +84,14 @@ static inline uint8_t rx_byte_from_fpga() { // // Note: The SSC receiver is never synchronized the calculation may be performed // on a i/q pair from two subsequent correlations, but does not matter. -static inline int32_t sample_power() { - int32_t q = (int8_t)rx_byte_from_fpga(); q = ABS(q); - int32_t i = (int8_t)rx_byte_from_fpga(); i = ABS(i); +static inline int32_t sample_power() +{ + int32_t q = (int8_t)rx_byte_from_fpga(); + q = ABS(q); + int32_t i = (int8_t)rx_byte_from_fpga(); + i = ABS(i); - return MAX(i, q) + (MIN(i, q) >> 1); + return MAX(i, q) + (MIN(i, q) >> 1); } // Returns a demedulated bit @@ -97,14 +101,15 @@ static inline int32_t sample_power() { // // Note: The demodulator would be drifting (18.9us * 5 != 100us), rx_frame // has a delay loop that aligns rx_bit calls to the TAG tx timeslots. -static inline bool rx_bit() { - int32_t power; +static inline bool rx_bit() +{ + int32_t power; - for (size_t i = 0; i<5; ++i) { - power = sample_power(); - } + for (size_t i = 0; i < 5; ++i) { + power = sample_power(); + } - return (power > INPUT_THRESHOLD); + return (power > INPUT_THRESHOLD); } //----------------------------------------------------------------------------- @@ -116,16 +121,17 @@ static inline bool rx_bit() { // be circumvented, but the adventage over bitbang would be little. //----------------------------------------------------------------------------- -static inline void tx_bit(bool bit) { - // insert pause - LOW(GPIO_SSC_DOUT); - last_frame_end += RWD_TIME_PAUSE; - while (GET_TICKS < last_frame_end) { }; - HIGH(GPIO_SSC_DOUT); +static inline void tx_bit(bool bit) +{ + // insert pause + LOW(GPIO_SSC_DOUT); + last_frame_end += RWD_TIME_PAUSE; + while (GET_TICKS < last_frame_end) { }; + HIGH(GPIO_SSC_DOUT); - // return to high, wait for bit periode to end - last_frame_end += (bit ? RWD_TIME_1 : RWD_TIME_0) - RWD_TIME_PAUSE; - while (GET_TICKS < last_frame_end) { }; + // return to high, wait for bit periode to end + last_frame_end += (bit ? RWD_TIME_1 : RWD_TIME_0) - RWD_TIME_PAUSE; + while (GET_TICKS < last_frame_end) { }; } //----------------------------------------------------------------------------- @@ -138,163 +144,168 @@ static inline void tx_bit(bool bit) { // present. //----------------------------------------------------------------------------- -static void tx_frame(uint32_t frame, uint8_t len) { - FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_TX); +static void tx_frame(uint32_t frame, uint8_t len) +{ + FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_TX); - // wait for next tx timeslot - last_frame_end += RWD_FRAME_WAIT; - while (GET_TICKS < last_frame_end) { }; - - // backup ts for trace log - uint32_t last_frame_start = last_frame_end; - - // transmit frame, MSB first - for (uint8_t i = 0; i < len; ++i) { - bool bit = (frame >> i) & 0x01; - tx_bit(bit ^ legic_prng_get_bit()); - legic_prng_forward(1); - }; - - // add pause to mark end of the frame - LOW(GPIO_SSC_DOUT); - last_frame_end += RWD_TIME_PAUSE; - while (GET_TICKS < last_frame_end) { }; - HIGH(GPIO_SSC_DOUT); - - // log - uint8_t cmdbytes[] = {len, BYTEx(frame, 0), BYTEx(frame, 1), BYTEx(frame, 2)}; - LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, true); -} - -static uint32_t rx_frame(uint8_t len) { - FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR - | FPGA_HF_READER_RX_XCORR_848_KHZ - | FPGA_HF_READER_RX_XCORR_QUARTER); - - // hold sampling until card is expected to respond - last_frame_end += TAG_FRAME_WAIT; - while (GET_TICKS < last_frame_end) { }; - - // backup ts for trace log - uint32_t last_frame_start = last_frame_end; - - uint32_t frame = 0; - for (uint8_t i = 0; i < len; ++i) { - frame |= (rx_bit() ^ legic_prng_get_bit()) << i; - legic_prng_forward(1); - - // rx_bit runs only 95us, resync to TAG_BIT_PERIOD - last_frame_end += TAG_BIT_PERIOD; - while (GET_TICKS < last_frame_end) { }; - } - - // log - uint8_t cmdbytes[] = {len, BYTEx(frame, 0), BYTEx(frame, 1)}; - LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); - - return frame; -} - -static bool rx_ack() { - // change fpga into rx mode - FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR - | FPGA_HF_READER_RX_XCORR_848_KHZ - | FPGA_HF_READER_RX_XCORR_QUARTER); - - // hold sampling until card is expected to respond - last_frame_end += TAG_FRAME_WAIT; - while (GET_TICKS < last_frame_end) { }; - - // backup ts for trace log - uint32_t last_frame_start = last_frame_end; - - uint32_t ack = 0; - for (uint8_t i = 0; i < TAG_WRITE_TIMEOUT; ++i) { - // sample bit - ack = rx_bit(); - legic_prng_forward(1); - - // rx_bit runs only 95us, resync to TAG_BIT_PERIOD - last_frame_end += TAG_BIT_PERIOD; + // wait for next tx timeslot + last_frame_end += RWD_FRAME_WAIT; while (GET_TICKS < last_frame_end) { }; - // check if it was an ACK - if (ack) { - break; + // backup ts for trace log + uint32_t last_frame_start = last_frame_end; + + // transmit frame, MSB first + for (uint8_t i = 0; i < len; ++i) { + bool bit = (frame >> i) & 0x01; + tx_bit(bit ^ legic_prng_get_bit()); + legic_prng_forward(1); + }; + + // add pause to mark end of the frame + LOW(GPIO_SSC_DOUT); + last_frame_end += RWD_TIME_PAUSE; + while (GET_TICKS < last_frame_end) { }; + HIGH(GPIO_SSC_DOUT); + + // log + uint8_t cmdbytes[] = {len, BYTEx(frame, 0), BYTEx(frame, 1), BYTEx(frame, 2)}; + LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, true); +} + +static uint32_t rx_frame(uint8_t len) +{ + FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR + | FPGA_HF_READER_RX_XCORR_848_KHZ + | FPGA_HF_READER_RX_XCORR_QUARTER); + + // hold sampling until card is expected to respond + last_frame_end += TAG_FRAME_WAIT; + while (GET_TICKS < last_frame_end) { }; + + // backup ts for trace log + uint32_t last_frame_start = last_frame_end; + + uint32_t frame = 0; + for (uint8_t i = 0; i < len; ++i) { + frame |= (rx_bit() ^ legic_prng_get_bit()) << i; + legic_prng_forward(1); + + // rx_bit runs only 95us, resync to TAG_BIT_PERIOD + last_frame_end += TAG_BIT_PERIOD; + while (GET_TICKS < last_frame_end) { }; } - } - // log - uint8_t cmdbytes[] = {1, BYTEx(ack, 0)}; - LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); + // log + uint8_t cmdbytes[] = {len, BYTEx(frame, 0), BYTEx(frame, 1)}; + LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); - return ack; + return frame; +} + +static bool rx_ack() +{ + // change fpga into rx mode + FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR + | FPGA_HF_READER_RX_XCORR_848_KHZ + | FPGA_HF_READER_RX_XCORR_QUARTER); + + // hold sampling until card is expected to respond + last_frame_end += TAG_FRAME_WAIT; + while (GET_TICKS < last_frame_end) { }; + + // backup ts for trace log + uint32_t last_frame_start = last_frame_end; + + uint32_t ack = 0; + for (uint8_t i = 0; i < TAG_WRITE_TIMEOUT; ++i) { + // sample bit + ack = rx_bit(); + legic_prng_forward(1); + + // rx_bit runs only 95us, resync to TAG_BIT_PERIOD + last_frame_end += TAG_BIT_PERIOD; + while (GET_TICKS < last_frame_end) { }; + + // check if it was an ACK + if (ack) { + break; + } + } + + // log + uint8_t cmdbytes[] = {1, BYTEx(ack, 0)}; + LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); + + return ack; } //----------------------------------------------------------------------------- // Legic Reader //----------------------------------------------------------------------------- -static int init_card(uint8_t cardtype, legic_card_select_t *p_card) { - p_card->tagtype = cardtype; +static int init_card(uint8_t cardtype, legic_card_select_t *p_card) +{ + p_card->tagtype = cardtype; - switch(p_card->tagtype) { - case 0x0d: - p_card->cmdsize = 6; - p_card->addrsize = 5; - p_card->cardsize = 22; - break; - case 0x1d: - p_card->cmdsize = 9; - p_card->addrsize = 8; - p_card->cardsize = 256; - break; - case 0x3d: - p_card->cmdsize = 11; - p_card->addrsize = 10; - p_card->cardsize = 1024; - break; - default: - p_card->cmdsize = 0; - p_card->addrsize = 0; - p_card->cardsize = 0; - return 2; - } - return 0; + switch (p_card->tagtype) { + case 0x0d: + p_card->cmdsize = 6; + p_card->addrsize = 5; + p_card->cardsize = 22; + break; + case 0x1d: + p_card->cmdsize = 9; + p_card->addrsize = 8; + p_card->cardsize = 256; + break; + case 0x3d: + p_card->cmdsize = 11; + p_card->addrsize = 10; + p_card->cardsize = 1024; + break; + default: + p_card->cmdsize = 0; + p_card->addrsize = 0; + p_card->cardsize = 0; + return 2; + } + return 0; } -static void init_reader(bool clear_mem) { - // configure FPGA - FpgaDownloadAndGo(FPGA_BITSTREAM_HF); - FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR - | FPGA_HF_READER_RX_XCORR_848_KHZ - | FPGA_HF_READER_RX_XCORR_QUARTER); - SetAdcMuxFor(GPIO_MUXSEL_HIPKD); - LED_A_ON(); +static void init_reader(bool clear_mem) +{ + // configure FPGA + FpgaDownloadAndGo(FPGA_BITSTREAM_HF); + FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_RX_XCORR + | FPGA_HF_READER_RX_XCORR_848_KHZ + | FPGA_HF_READER_RX_XCORR_QUARTER); + SetAdcMuxFor(GPIO_MUXSEL_HIPKD); + LED_A_ON(); - // configure SSC with defaults - FpgaSetupSsc(); + // configure SSC with defaults + FpgaSetupSsc(); - // re-claim GPIO_SSC_DOUT as GPIO and enable output - AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT; - AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT; - HIGH(GPIO_SSC_DOUT); + // re-claim GPIO_SSC_DOUT as GPIO and enable output + AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT; + AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT; + HIGH(GPIO_SSC_DOUT); - // reserve a cardmem, meaning we can use the tracelog function in bigbuff easier. - legic_mem = BigBuf_get_EM_addr(); - if (legic_mem) { - memset(legic_mem, 0x00, LEGIC_CARD_MEMSIZE); - } + // reserve a cardmem, meaning we can use the tracelog function in bigbuff easier. + legic_mem = BigBuf_get_EM_addr(); + if (legic_mem) { + memset(legic_mem, 0x00, LEGIC_CARD_MEMSIZE); + } - // start trace - clear_trace(); - set_tracing(true); + // start trace + clear_trace(); + set_tracing(true); - // init crc calculator - crc_init(&legic_crc, 4, 0x19 >> 1, 0x05, 0); + // init crc calculator + crc_init(&legic_crc, 4, 0x19 >> 1, 0x05, 0); - // start us timer - StartTicks(); + // start us timer + StartTicks(); } // Setup reader to card connection @@ -303,89 +314,93 @@ static void init_reader(bool clear_mem) { // - Transmit initialisation vector 7 bits // - Receive card type 6 bits // - Transmit Acknowledge 6 bits -static uint32_t setup_phase(uint8_t iv) { - // init coordination timestamp - last_frame_end = GET_TICKS; +static uint32_t setup_phase(uint8_t iv) +{ + // init coordination timestamp + last_frame_end = GET_TICKS; - // Switch on carrier and let the card charge for 5ms. - last_frame_end += 7500; - while (GET_TICKS < last_frame_end) { }; + // Switch on carrier and let the card charge for 5ms. + last_frame_end += 7500; + while (GET_TICKS < last_frame_end) { }; - legic_prng_init(0); - tx_frame(iv, 7); + legic_prng_init(0); + tx_frame(iv, 7); - // configure prng - legic_prng_init(iv); - legic_prng_forward(2); + // configure prng + legic_prng_init(iv); + legic_prng_forward(2); - // receive card type - int32_t card_type = rx_frame(6); - legic_prng_forward(3); + // receive card type + int32_t card_type = rx_frame(6); + legic_prng_forward(3); - // send obsfuscated acknowledgment frame - switch (card_type) { - case 0x0D: - tx_frame(0x19, 6); // MIM22 | READCMD = 0x18 | 0x01 - break; - case 0x1D: - case 0x3D: - tx_frame(0x39, 6); // MIM256 | READCMD = 0x38 | 0x01 - break; - } + // send obsfuscated acknowledgment frame + switch (card_type) { + case 0x0D: + tx_frame(0x19, 6); // MIM22 | READCMD = 0x18 | 0x01 + break; + case 0x1D: + case 0x3D: + tx_frame(0x39, 6); // MIM256 | READCMD = 0x38 | 0x01 + break; + } - return card_type; + return card_type; } -static uint8_t calc_crc4(uint16_t cmd, uint8_t cmd_sz, uint8_t value) { - crc_clear(&legic_crc); - crc_update(&legic_crc, (value << cmd_sz) | cmd, 8 + cmd_sz); - return crc_finish(&legic_crc); +static uint8_t calc_crc4(uint16_t cmd, uint8_t cmd_sz, uint8_t value) +{ + crc_clear(&legic_crc); + crc_update(&legic_crc, (value << cmd_sz) | cmd, 8 + cmd_sz); + return crc_finish(&legic_crc); } -static int16_t read_byte(uint16_t index, uint8_t cmd_sz) { - uint16_t cmd = (index << 1) | LEGIC_READ; +static int16_t read_byte(uint16_t index, uint8_t cmd_sz) +{ + uint16_t cmd = (index << 1) | LEGIC_READ; - // read one byte - LED_B_ON(); - legic_prng_forward(2); - tx_frame(cmd, cmd_sz); - legic_prng_forward(2); - uint32_t frame = rx_frame(12); - LED_B_OFF(); + // read one byte + LED_B_ON(); + legic_prng_forward(2); + tx_frame(cmd, cmd_sz); + legic_prng_forward(2); + uint32_t frame = rx_frame(12); + LED_B_OFF(); - // split frame into data and crc - uint8_t byte = BYTEx(frame, 0); - uint8_t crc = BYTEx(frame, 1); + // split frame into data and crc + uint8_t byte = BYTEx(frame, 0); + uint8_t crc = BYTEx(frame, 1); - // check received against calculated crc - uint8_t calc_crc = calc_crc4(cmd, cmd_sz, byte); - if (calc_crc != crc) { - Dbprintf("!!! crc mismatch: %x != %x !!!", calc_crc, crc); - return -1; - } + // check received against calculated crc + uint8_t calc_crc = calc_crc4(cmd, cmd_sz, byte); + if (calc_crc != crc) { + Dbprintf("!!! crc mismatch: %x != %x !!!", calc_crc, crc); + return -1; + } - legic_prng_forward(1); + legic_prng_forward(1); - return byte; + return byte; } // Transmit write command, wait until (3.6ms) the tag sends back an unencrypted // ACK ('1' bit) and forward the prng time based. -bool write_byte(uint16_t index, uint8_t byte, uint8_t addr_sz) { - uint32_t cmd = index << 1 | LEGIC_WRITE; // prepare command - uint8_t crc = calc_crc4(cmd, addr_sz + 1, byte); // calculate crc - cmd |= byte << (addr_sz + 1); // append value - cmd |= (crc & 0xF) << (addr_sz + 1 + 8); // and crc +bool write_byte(uint16_t index, uint8_t byte, uint8_t addr_sz) +{ + uint32_t cmd = index << 1 | LEGIC_WRITE; // prepare command + uint8_t crc = calc_crc4(cmd, addr_sz + 1, byte); // calculate crc + cmd |= byte << (addr_sz + 1); // append value + cmd |= (crc & 0xF) << (addr_sz + 1 + 8); // and crc - // send write command - LED_C_ON(); - legic_prng_forward(2); - tx_frame(cmd, addr_sz + 1 + 8 + 4); // cmd_sz = addr_sz + cmd + data + crc - legic_prng_forward(3); - LED_C_OFF(); + // send write command + LED_C_ON(); + legic_prng_forward(2); + tx_frame(cmd, addr_sz + 1 + 8 + 4); // cmd_sz = addr_sz + cmd + data + crc + legic_prng_forward(3); + LED_C_OFF(); - // wait for ack - return rx_ack(); + // wait for ack + return rx_ack(); } //----------------------------------------------------------------------------- @@ -393,111 +408,114 @@ bool write_byte(uint16_t index, uint8_t byte, uint8_t addr_sz) { // // Only this functions are public / called from appmain.c //----------------------------------------------------------------------------- -void LegicRfInfo(void) { - // configure ARM and FPGA - init_reader(false); +void LegicRfInfo(void) +{ + // configure ARM and FPGA + init_reader(false); - // establish shared secret and detect card type - uint8_t card_type = setup_phase(0x01); - if (init_card(card_type, &card) != 0) { - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; - } - - // read UID - for (uint8_t i = 0; i < sizeof(card.uid); ++i) { - int16_t byte = read_byte(i, card.cmdsize); - if (byte == -1) { - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; + // establish shared secret and detect card type + uint8_t card_type = setup_phase(0x01); + if (init_card(card_type, &card) != 0) { + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; } - card.uid[i] = byte & 0xFF; - } - // read MCC and check against UID - int16_t mcc = read_byte(4, card.cmdsize); - int16_t calc_mcc = CRC8Legic(card.uid, 4);; - if (mcc != calc_mcc) { - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; - } + // read UID + for (uint8_t i = 0; i < sizeof(card.uid); ++i) { + int16_t byte = read_byte(i, card.cmdsize); + if (byte == -1) { + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; + } + card.uid[i] = byte & 0xFF; + } - // OK - cmd_send(CMD_ACK, 1, 0, 0, (uint8_t*)&card, sizeof(legic_card_select_t)); + // read MCC and check against UID + int16_t mcc = read_byte(4, card.cmdsize); + int16_t calc_mcc = CRC8Legic(card.uid, 4);; + if (mcc != calc_mcc) { + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; + } + + // OK + cmd_send(CMD_ACK, 1, 0, 0, (uint8_t *)&card, sizeof(legic_card_select_t)); OUT: - switch_off(); - StopTicks(); + switch_off(); + StopTicks(); } -void LegicRfReader(uint16_t offset, uint16_t len, uint8_t iv) { - // configure ARM and FPGA - init_reader(false); +void LegicRfReader(uint16_t offset, uint16_t len, uint8_t iv) +{ + // configure ARM and FPGA + init_reader(false); - // establish shared secret and detect card type - uint8_t card_type = setup_phase(iv); - if (init_card(card_type, &card) != 0) { - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; - } - - // do not read beyond card memory - if (len + offset > card.cardsize) { - len = card.cardsize - offset; - } - - for (uint16_t i = 0; i < len; ++i) { - int16_t byte = read_byte(offset + i, card.cmdsize); - if (byte == -1) { - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; + // establish shared secret and detect card type + uint8_t card_type = setup_phase(iv); + if (init_card(card_type, &card) != 0) { + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; } - legic_mem[i] = byte; - } - // OK - cmd_send(CMD_ACK, 1, len, 0, legic_mem, len); + // do not read beyond card memory + if (len + offset > card.cardsize) { + len = card.cardsize - offset; + } + + for (uint16_t i = 0; i < len; ++i) { + int16_t byte = read_byte(offset + i, card.cmdsize); + if (byte == -1) { + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; + } + legic_mem[i] = byte; + } + + // OK + cmd_send(CMD_ACK, 1, len, 0, legic_mem, len); OUT: - switch_off(); - StopTicks(); + switch_off(); + StopTicks(); } -void LegicRfWriter(uint16_t offset, uint16_t len, uint8_t iv, uint8_t *data) { - // configure ARM and FPGA - init_reader(false); +void LegicRfWriter(uint16_t offset, uint16_t len, uint8_t iv, uint8_t *data) +{ + // configure ARM and FPGA + init_reader(false); - // uid is not writeable - if (offset <= WRITE_LOWERLIMIT) { - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; - } - - // establish shared secret and detect card type - uint8_t card_type = setup_phase(iv); - if (init_card(card_type, &card) != 0) { - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; - } - - // do not write beyond card memory - if (len + offset > card.cardsize) { - len = card.cardsize - offset; - } - - // write in reverse order, only then is DCF (decremental field) writable - while (len-- > 0 && !BUTTON_PRESS()) { - if (!write_byte(len + offset, data[len], card.addrsize)) { - Dbprintf("operation failed | %02X | %02X | %02X", len + offset, len, data[len]); - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); - goto OUT; + // uid is not writeable + if (offset <= WRITE_LOWERLIMIT) { + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; } - } - // OK - cmd_send(CMD_ACK, 1, len, 0, legic_mem, len); + // establish shared secret and detect card type + uint8_t card_type = setup_phase(iv); + if (init_card(card_type, &card) != 0) { + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; + } + + // do not write beyond card memory + if (len + offset > card.cardsize) { + len = card.cardsize - offset; + } + + // write in reverse order, only then is DCF (decremental field) writable + while (len-- > 0 && !BUTTON_PRESS()) { + if (!write_byte(len + offset, data[len], card.addrsize)) { + Dbprintf("operation failed | %02X | %02X | %02X", len + offset, len, data[len]); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + goto OUT; + } + } + + // OK + cmd_send(CMD_ACK, 1, len, 0, legic_mem, len); OUT: - switch_off(); - StopTicks(); + switch_off(); + StopTicks(); } diff --git a/armsrc/legicrfsim.c b/armsrc/legicrfsim.c index 86448275f..a855401a4 100644 --- a/armsrc/legicrfsim.c +++ b/armsrc/legicrfsim.c @@ -16,7 +16,7 @@ #include "legic_prng.h" /* legic PRNG impl */ #include "legic.h" /* legic_card_select_t struct */ -static uint8_t* legic_mem; /* card memory, used for sim */ +static uint8_t *legic_mem; /* card memory, used for sim */ static legic_card_select_t card;/* metadata of currently selected card */ static crc_t legic_crc; @@ -58,13 +58,14 @@ static uint32_t last_frame_end; /* ts of last bit of previews rx or tx frame */ //----------------------------------------------------------------------------- // Returns true if a pulse/pause is received within timeout -static inline bool wait_for(bool value, const uint32_t timeout) { - while ((bool)(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_DIN) != value) { - if (GetCountSspClk() > timeout) { - return false; +static inline bool wait_for(bool value, const uint32_t timeout) +{ + while ((bool)(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_DIN) != value) { + if (GetCountSspClk() > timeout) { + return false; + } } - } - return true; + return true; } // Returns a demedulated bit or -1 on code violation @@ -76,30 +77,31 @@ static inline bool wait_for(bool value, const uint32_t timeout) { // - A bit length >80.2us is a 1 // - A bit length <80.2us is a 0 // - A bit length >148.6us is a code violation -static inline int8_t rx_bit() { - // backup ts for threshold calculation - uint32_t bit_start = last_frame_end; +static inline int8_t rx_bit() +{ + // backup ts for threshold calculation + uint32_t bit_start = last_frame_end; - // wait for pause to end - if (!wait_for(RWD_PULSE, bit_start + RWD_TIME_1*3/2)) { - return -1; - } + // wait for pause to end + if (!wait_for(RWD_PULSE, bit_start + RWD_TIME_1 * 3 / 2)) { + return -1; + } - // wait for next pause - if (!wait_for(RWD_PAUSE, bit_start + RWD_TIME_1*3/2)) { - return -1; - } + // wait for next pause + if (!wait_for(RWD_PAUSE, bit_start + RWD_TIME_1 * 3 / 2)) { + return -1; + } - // update bit and frame end - last_frame_end = GetCountSspClk(); + // update bit and frame end + last_frame_end = GetCountSspClk(); - // check for code violation (bit to short) - if (last_frame_end - bit_start < RWD_TIME_PAUSE) { - return -1; - } + // check for code violation (bit to short) + if (last_frame_end - bit_start < RWD_TIME_PAUSE) { + return -1; + } - // apply threshold (average of RWD_TIME_0 and ) - return (last_frame_end - bit_start > (RWD_TIME_0 + RWD_TIME_1) / 2); + // apply threshold (average of RWD_TIME_0 and ) + return (last_frame_end - bit_start > (RWD_TIME_0 + RWD_TIME_1) / 2); } //----------------------------------------------------------------------------- @@ -119,21 +121,22 @@ static inline int8_t rx_bit() { // Note: The Subcarrier is not disabled during bits to prevent glitches. This is // not mandatory but results in a cleaner signal. tx_frame will disable // the subcarrier when the frame is done. -static inline void tx_bit(bool bit) { - LED_C_ON(); +static inline void tx_bit(bool bit) +{ + LED_C_ON(); - if (bit) { - // modulate subcarrier - HIGH(GPIO_SSC_DOUT); - } else { - // do not modulate subcarrier - LOW(GPIO_SSC_DOUT); - } + if (bit) { + // modulate subcarrier + HIGH(GPIO_SSC_DOUT); + } else { + // do not modulate subcarrier + LOW(GPIO_SSC_DOUT); + } - // wait for tx timeslot to end - last_frame_end += TAG_BIT_PERIOD; - while (GetCountSspClk() < last_frame_end) { }; - LED_C_OFF(); + // wait for tx timeslot to end + last_frame_end += TAG_BIT_PERIOD; + while (GetCountSspClk() < last_frame_end) { }; + LED_C_OFF(); } //----------------------------------------------------------------------------- @@ -146,49 +149,51 @@ static inline void tx_bit(bool bit) { // and depends only the command received (IV, ACK, READ or WRITE). //----------------------------------------------------------------------------- -static void tx_frame(uint32_t frame, uint8_t len) { - // wait for next tx timeslot - last_frame_end += TAG_FRAME_WAIT; - legic_prng_forward(TAG_FRAME_WAIT/TAG_BIT_PERIOD - 1); - while (GetCountSspClk() < last_frame_end) { }; +static void tx_frame(uint32_t frame, uint8_t len) +{ + // wait for next tx timeslot + last_frame_end += TAG_FRAME_WAIT; + legic_prng_forward(TAG_FRAME_WAIT / TAG_BIT_PERIOD - 1); + while (GetCountSspClk() < last_frame_end) { }; - // backup ts for trace log - uint32_t last_frame_start = last_frame_end; + // backup ts for trace log + uint32_t last_frame_start = last_frame_end; - // transmit frame, MSB first - for (uint8_t i = 0; i < len; ++i) { - bool bit = (frame >> i) & 0x01; - tx_bit(bit ^ legic_prng_get_bit()); - legic_prng_forward(1); - }; + // transmit frame, MSB first + for (uint8_t i = 0; i < len; ++i) { + bool bit = (frame >> i) & 0x01; + tx_bit(bit ^ legic_prng_get_bit()); + legic_prng_forward(1); + }; - // disable subcarrier - LOW(GPIO_SSC_DOUT); + // disable subcarrier + LOW(GPIO_SSC_DOUT); - // log - uint8_t cmdbytes[] = {len, BYTEx(frame, 0), BYTEx(frame, 1)}; - LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); + // log + uint8_t cmdbytes[] = {len, BYTEx(frame, 0), BYTEx(frame, 1)}; + LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); } -static void tx_ack() { - // wait for ack timeslot - last_frame_end += TAG_ACK_WAIT; - legic_prng_forward(TAG_ACK_WAIT/TAG_BIT_PERIOD - 1); - while (GetCountSspClk() < last_frame_end) { }; +static void tx_ack() +{ + // wait for ack timeslot + last_frame_end += TAG_ACK_WAIT; + legic_prng_forward(TAG_ACK_WAIT / TAG_BIT_PERIOD - 1); + while (GetCountSspClk() < last_frame_end) { }; - // backup ts for trace log - uint32_t last_frame_start = last_frame_end; + // backup ts for trace log + uint32_t last_frame_start = last_frame_end; - // transmit ack (ack is not encrypted) - tx_bit(true); - legic_prng_forward(1); + // transmit ack (ack is not encrypted) + tx_bit(true); + legic_prng_forward(1); - // disable subcarrier - LOW(GPIO_SSC_DOUT); + // disable subcarrier + LOW(GPIO_SSC_DOUT); - // log - uint8_t cmdbytes[] = {1, 1}; - LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); + // log + uint8_t cmdbytes[] = {1, 1}; + LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, false); } // Returns a demedulated frame or -1 on code violation @@ -198,126 +203,129 @@ static void tx_ack() { // - forward prng based on ts/TAG_BIT_PERIOD // - receive the frame // - detect end of frame (last pause) -static int32_t rx_frame(uint8_t *len) { - int32_t frame = 0; +static int32_t rx_frame(uint8_t *len) +{ + int32_t frame = 0; - // add 2 SSP clock cycles (1 for tx and 1 for rx pipeline delay) - // those will be substracted at the end of the rx phase - last_frame_end -= 2; + // add 2 SSP clock cycles (1 for tx and 1 for rx pipeline delay) + // those will be substracted at the end of the rx phase + last_frame_end -= 2; - // wait for first pause (start of frame) - for (uint8_t i = 0; true; ++i) { - // increment prng every TAG_BIT_PERIOD - last_frame_end += TAG_BIT_PERIOD; - legic_prng_forward(1); + // wait for first pause (start of frame) + for (uint8_t i = 0; true; ++i) { + // increment prng every TAG_BIT_PERIOD + last_frame_end += TAG_BIT_PERIOD; + legic_prng_forward(1); - // if start of frame was received exit delay loop - if (wait_for(RWD_PAUSE, last_frame_end)) { - last_frame_end = GetCountSspClk(); - break; + // if start of frame was received exit delay loop + if (wait_for(RWD_PAUSE, last_frame_end)) { + last_frame_end = GetCountSspClk(); + break; + } + + // check for code violation + if (i > RWD_CMD_TIMEOUT) { + return -1; + } } - // check for code violation - if (i > RWD_CMD_TIMEOUT) { - return -1; - } - } + // backup ts for trace log + uint32_t last_frame_start = last_frame_end; - // backup ts for trace log - uint32_t last_frame_start = last_frame_end; + // receive frame + for (*len = 0; true; ++(*len)) { + // receive next bit + LED_B_ON(); + int8_t bit = rx_bit(); + LED_B_OFF(); - // receive frame - for (*len = 0; true; ++(*len)) { - // receive next bit - LED_B_ON(); - int8_t bit = rx_bit(); - LED_B_OFF(); + // check for code violation and to short / long frame + if ((bit < 0) && ((*len < RWD_MIN_FRAME_LEN) || (*len > RWD_MAX_FRAME_LEN))) { + return -1; + } - // check for code violation and to short / long frame - if ((bit < 0) && ((*len < RWD_MIN_FRAME_LEN) || (*len > RWD_MAX_FRAME_LEN))) { - return -1; + // check for code violation caused by end of frame + if (bit < 0) { + break; + } + + // append bit + frame |= (bit ^ legic_prng_get_bit()) << (*len); + legic_prng_forward(1); } - // check for code violation caused by end of frame - if (bit < 0) { - break; - } + // rx_bit sets coordination timestamp to start of pause, append pause duration + // and substract 2 SSP clock cycles (1 for rx and 1 for tx pipeline delay) to + // obtain exact end of frame. + last_frame_end += RWD_TIME_PAUSE - 2; - // append bit - frame |= (bit ^ legic_prng_get_bit()) << (*len); - legic_prng_forward(1); - } - - // rx_bit sets coordination timestamp to start of pause, append pause duration - // and substract 2 SSP clock cycles (1 for rx and 1 for tx pipeline delay) to - // obtain exact end of frame. - last_frame_end += RWD_TIME_PAUSE - 2; - - // log - uint8_t cmdbytes[] = {*len, BYTEx(frame, 0), BYTEx(frame, 1), BYTEx(frame, 2)}; - LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, true); - return frame; + // log + uint8_t cmdbytes[] = {*len, BYTEx(frame, 0), BYTEx(frame, 1), BYTEx(frame, 2)}; + LogTrace(cmdbytes, sizeof(cmdbytes), last_frame_start, last_frame_end, NULL, true); + return frame; } //----------------------------------------------------------------------------- // Legic Simulator //----------------------------------------------------------------------------- -static int32_t init_card(uint8_t cardtype, legic_card_select_t *p_card) { - p_card->tagtype = cardtype; +static int32_t init_card(uint8_t cardtype, legic_card_select_t *p_card) +{ + p_card->tagtype = cardtype; - switch (p_card->tagtype) { - case 0: - p_card->cmdsize = 6; - p_card->addrsize = 5; - p_card->cardsize = 22; - break; - case 1: - p_card->cmdsize = 9; - p_card->addrsize = 8; - p_card->cardsize = 256; - break; - case 2: - p_card->cmdsize = 11; - p_card->addrsize = 10; - p_card->cardsize = 1024; - break; - default: - p_card->cmdsize = 0; - p_card->addrsize = 0; - p_card->cardsize = 0; - return 2; - } - return 0; + switch (p_card->tagtype) { + case 0: + p_card->cmdsize = 6; + p_card->addrsize = 5; + p_card->cardsize = 22; + break; + case 1: + p_card->cmdsize = 9; + p_card->addrsize = 8; + p_card->cardsize = 256; + break; + case 2: + p_card->cmdsize = 11; + p_card->addrsize = 10; + p_card->cardsize = 1024; + break; + default: + p_card->cmdsize = 0; + p_card->addrsize = 0; + p_card->cardsize = 0; + return 2; + } + return 0; } -static void init_tag() { - // configure FPGA - FpgaDownloadAndGo(FPGA_BITSTREAM_HF); - FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_SIMULATOR - | FPGA_HF_SIMULATOR_MODULATE_212K); - SetAdcMuxFor(GPIO_MUXSEL_HIPKD); +static void init_tag() +{ + // configure FPGA + FpgaDownloadAndGo(FPGA_BITSTREAM_HF); + FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_SIMULATOR + | FPGA_HF_SIMULATOR_MODULATE_212K); + SetAdcMuxFor(GPIO_MUXSEL_HIPKD); - // configure SSC with defaults - FpgaSetupSsc(); + // configure SSC with defaults + FpgaSetupSsc(); - // first pull output to low to prevent glitches then re-claim GPIO_SSC_DOUT - LOW(GPIO_SSC_DOUT); - AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT; - AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT; + // first pull output to low to prevent glitches then re-claim GPIO_SSC_DOUT + LOW(GPIO_SSC_DOUT); + AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT; + AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT; - // reserve a cardmem, meaning we can use the tracelog function in bigbuff easier. - legic_mem = BigBuf_get_EM_addr(); + // reserve a cardmem, meaning we can use the tracelog function in bigbuff easier. + legic_mem = BigBuf_get_EM_addr(); - // start trace - clear_trace(); - set_tracing(true); + // start trace + clear_trace(); + set_tracing(true); - // init crc calculator - crc_init(&legic_crc, 4, 0x19 >> 1, 0x05, 0); + // init crc calculator + crc_init(&legic_crc, 4, 0x19 >> 1, 0x05, 0); - // start 212kHz timer (running from SSP Clock) - StartCountSspClk(); + // start 212kHz timer (running from SSP Clock) + StartCountSspClk(); } // Setup reader to card connection @@ -326,119 +334,122 @@ static void init_tag() { // - Receive initialisation vector 7 bits // - Transmit card type 6 bits // - Receive Acknowledge 6 bits -static int32_t setup_phase(legic_card_select_t *p_card) { - uint8_t len = 0; +static int32_t setup_phase(legic_card_select_t *p_card) +{ + uint8_t len = 0; - // init coordination timestamp - last_frame_end = GetCountSspClk(); + // init coordination timestamp + last_frame_end = GetCountSspClk(); - // reset prng - legic_prng_init(0); + // reset prng + legic_prng_init(0); - // wait for iv - int32_t iv = rx_frame(&len); - if ((len != 7) || (iv < 0)) { - return -1; - } - - // configure prng - legic_prng_init(iv); - - // reply with card type - switch (p_card->tagtype) { - case 0: - tx_frame(0x0D, 6); - break; - case 1: - tx_frame(0x1D, 6); - break; - case 2: - tx_frame(0x3D, 6); - break; - } - - // wait for ack - int32_t ack = rx_frame(&len); - if ((len != 6) || (ack < 0)) { - return -1; - } - - // validate data - switch (p_card->tagtype) { - case 0: - if(ack != 0x19) return -1; - break; - case 1: - if(ack != 0x39) return -1; - break; - case 2: - if(ack != 0x39) return -1; - break; - } - - // During rx the prng is clocked using the variable reader period. - // Since rx_frame detects end of frame by detecting a code violation, - // the prng is off by one bit period after each rx phase. Hence, tx - // code advances the prng by (TAG_FRAME_WAIT/TAG_BIT_PERIOD - 1). - // This is not possible for back to back rx, so this quirk reduces - // the gap by one period. - last_frame_end += TAG_BIT_PERIOD; - - return 0; -} - -static uint8_t calc_crc4(uint16_t cmd, uint8_t cmd_sz, uint8_t value) { - crc_clear(&legic_crc); - crc_update(&legic_crc, (value << cmd_sz) | cmd, 8 + cmd_sz); - return crc_finish(&legic_crc); -} - -static int32_t connected_phase(legic_card_select_t *p_card) { - uint8_t len = 0; - - // wait for command - int32_t cmd = rx_frame(&len); - if (cmd < 0) { - return -1; - } - - // check if command is LEGIC_READ - if (len == p_card->cmdsize) { - // prepare data - uint8_t byte = legic_mem[cmd >> 1]; - uint8_t crc = calc_crc4(cmd, p_card->cmdsize, byte); - - // transmit data - tx_frame((crc << 8) | byte, 12); - - return 0; - } - - // check if command is LEGIC_WRITE - if (len == p_card->cmdsize + 8 + 4) { - // decode data - uint16_t mask = (1 << p_card->addrsize) - 1; - uint16_t addr = (cmd >> 1) & mask; - uint8_t byte = (cmd >> p_card->cmdsize) & 0xff; - uint8_t crc = (cmd >> (p_card->cmdsize + 8)) & 0xf; - - // check received against calculated crc - uint8_t calc_crc = calc_crc4(addr << 1, p_card->cmdsize, byte); - if (calc_crc != crc) { - Dbprintf("!!! crc mismatch: %x != %x !!!", calc_crc, crc); - return -1; + // wait for iv + int32_t iv = rx_frame(&len); + if ((len != 7) || (iv < 0)) { + return -1; } - // store data - legic_mem[addr] = byte; + // configure prng + legic_prng_init(iv); - // transmit ack - tx_ack(); + // reply with card type + switch (p_card->tagtype) { + case 0: + tx_frame(0x0D, 6); + break; + case 1: + tx_frame(0x1D, 6); + break; + case 2: + tx_frame(0x3D, 6); + break; + } + + // wait for ack + int32_t ack = rx_frame(&len); + if ((len != 6) || (ack < 0)) { + return -1; + } + + // validate data + switch (p_card->tagtype) { + case 0: + if (ack != 0x19) return -1; + break; + case 1: + if (ack != 0x39) return -1; + break; + case 2: + if (ack != 0x39) return -1; + break; + } + + // During rx the prng is clocked using the variable reader period. + // Since rx_frame detects end of frame by detecting a code violation, + // the prng is off by one bit period after each rx phase. Hence, tx + // code advances the prng by (TAG_FRAME_WAIT/TAG_BIT_PERIOD - 1). + // This is not possible for back to back rx, so this quirk reduces + // the gap by one period. + last_frame_end += TAG_BIT_PERIOD; return 0; - } +} - return -1; +static uint8_t calc_crc4(uint16_t cmd, uint8_t cmd_sz, uint8_t value) +{ + crc_clear(&legic_crc); + crc_update(&legic_crc, (value << cmd_sz) | cmd, 8 + cmd_sz); + return crc_finish(&legic_crc); +} + +static int32_t connected_phase(legic_card_select_t *p_card) +{ + uint8_t len = 0; + + // wait for command + int32_t cmd = rx_frame(&len); + if (cmd < 0) { + return -1; + } + + // check if command is LEGIC_READ + if (len == p_card->cmdsize) { + // prepare data + uint8_t byte = legic_mem[cmd >> 1]; + uint8_t crc = calc_crc4(cmd, p_card->cmdsize, byte); + + // transmit data + tx_frame((crc << 8) | byte, 12); + + return 0; + } + + // check if command is LEGIC_WRITE + if (len == p_card->cmdsize + 8 + 4) { + // decode data + uint16_t mask = (1 << p_card->addrsize) - 1; + uint16_t addr = (cmd >> 1) & mask; + uint8_t byte = (cmd >> p_card->cmdsize) & 0xff; + uint8_t crc = (cmd >> (p_card->cmdsize + 8)) & 0xf; + + // check received against calculated crc + uint8_t calc_crc = calc_crc4(addr << 1, p_card->cmdsize, byte); + if (calc_crc != crc) { + Dbprintf("!!! crc mismatch: %x != %x !!!", calc_crc, crc); + return -1; + } + + // store data + legic_mem[addr] = byte; + + // transmit ack + tx_ack(); + + return 0; + } + + return -1; } //----------------------------------------------------------------------------- @@ -447,39 +458,40 @@ static int32_t connected_phase(legic_card_select_t *p_card) { // Only this function is public / called from appmain.c //----------------------------------------------------------------------------- -void LegicRfSimulate(uint8_t cardtype) { - // configure ARM and FPGA - init_tag(); +void LegicRfSimulate(uint8_t cardtype) +{ + // configure ARM and FPGA + init_tag(); - // verify command line input - if (init_card(cardtype, &card) != 0) { - DbpString("Unknown tagtype."); - goto OUT; - } - - LED_A_ON(); - DbpString("Starting Legic emulator, press button to end"); - while (!BUTTON_PRESS() && !usb_poll_validate_length()) { - WDT_HIT(); - - // wait for carrier, restart after timeout - if (!wait_for(RWD_PULSE, GetCountSspClk() + TAG_BIT_PERIOD)) { - continue; + // verify command line input + if (init_card(cardtype, &card) != 0) { + DbpString("Unknown tagtype."); + goto OUT; } - // wait for connection, restart on error - if (setup_phase(&card)) { - continue; - } + LED_A_ON(); + DbpString("Starting Legic emulator, press button to end"); + while (!BUTTON_PRESS() && !usb_poll_validate_length()) { + WDT_HIT(); - // conection is established, process commands until one fails - while (!connected_phase(&card)) { - WDT_HIT(); + // wait for carrier, restart after timeout + if (!wait_for(RWD_PULSE, GetCountSspClk() + TAG_BIT_PERIOD)) { + continue; + } + + // wait for connection, restart on error + if (setup_phase(&card)) { + continue; + } + + // conection is established, process commands until one fails + while (!connected_phase(&card)) { + WDT_HIT(); + } } - } OUT: - DbpString("Stopped"); - switch_off(); - StopTicks(); + DbpString("Stopped"); + switch_off(); + StopTicks(); } diff --git a/armsrc/lfops.c b/armsrc/lfops.c index bd34e46df..87a0bcfb9 100644 --- a/armsrc/lfops.c +++ b/armsrc/lfops.c @@ -59,18 +59,20 @@ Default LF T55xx config is set to: write_1 = 47*8 read_gap = 15*8 */ -t55xx_config t_config = { 29*8, 17*8, 15*8, 47*8, 15*8 } ; +t55xx_config t_config = { 29 * 8, 17 * 8, 15 * 8, 47 * 8, 15 * 8 } ; -void printT55xxConfig(void) { +void printT55xxConfig(void) +{ Dbprintf("LF T55XX config"); - Dbprintf(" [a] startgap............%d*8 (%d)", t_config.start_gap/8, t_config.start_gap); - Dbprintf(" [b] writegap............%d*8 (%d)", t_config.write_gap/8, t_config.write_gap); - Dbprintf(" [c] write_0.............%d*8 (%d)", t_config.write_0/8, t_config.write_0); - Dbprintf(" [d] write_1.............%d*8 (%d)", t_config.write_1/8, t_config.write_1); - Dbprintf(" [e] readgap.............%d*8 (%d)", t_config.read_gap/8, t_config.read_gap); + Dbprintf(" [a] startgap............%d*8 (%d)", t_config.start_gap / 8, t_config.start_gap); + Dbprintf(" [b] writegap............%d*8 (%d)", t_config.write_gap / 8, t_config.write_gap); + Dbprintf(" [c] write_0.............%d*8 (%d)", t_config.write_0 / 8, t_config.write_0); + Dbprintf(" [d] write_1.............%d*8 (%d)", t_config.write_1 / 8, t_config.write_1); + Dbprintf(" [e] readgap.............%d*8 (%d)", t_config.read_gap / 8, t_config.read_gap); } -void setT55xxConfig(uint8_t arg0, t55xx_config *c) { +void setT55xxConfig(uint8_t arg0, t55xx_config *c) +{ if (c->start_gap != 0) t_config.start_gap = c->start_gap; if (c->write_gap != 0) t_config.write_gap = c->write_gap; @@ -93,7 +95,7 @@ void setT55xxConfig(uint8_t arg0, t55xx_config *c) { uint8_t *buf = BigBuf_malloc(T55XX_CONFIG_LEN); Flash_CheckBusy(BUSY_TIMEOUT); uint16_t res = Flash_ReadDataCont(T55XX_CONFIG_OFFSET, buf, T55XX_CONFIG_LEN); - if ( res == 0) { + if (res == 0) { FlashStop(); BigBuf_free(); return; @@ -106,7 +108,7 @@ void setT55xxConfig(uint8_t arg0, t55xx_config *c) { Flash_Erase4k(3, 0xD); res = Flash_Write(T55XX_CONFIG_OFFSET, buf, T55XX_CONFIG_LEN); - if ( res == T55XX_CONFIG_LEN && MF_DBGLEVEL > 1) { + if (res == T55XX_CONFIG_LEN && MF_DBGLEVEL > 1) { DbpString("T55XX Config save success"); } @@ -114,11 +116,13 @@ void setT55xxConfig(uint8_t arg0, t55xx_config *c) { #endif } -t55xx_config* getT55xxConfig(void) { +t55xx_config *getT55xxConfig(void) +{ return &t_config; } -void loadT55xxConfig(void) { +void loadT55xxConfig(void) +{ #ifdef WITH_FLASH if (!FlashInit()) { return; @@ -132,18 +136,18 @@ void loadT55xxConfig(void) { // verify read mem is actual data. uint8_t cntA = T55XX_CONFIG_LEN, cntB = T55XX_CONFIG_LEN; - for (int i=0; i< T55XX_CONFIG_LEN; i++) { - if ( buf[i] == 0xFF) cntA--; - if ( buf[i] == 0x00) cntB--; + for (int i = 0; i < T55XX_CONFIG_LEN; i++) { + if (buf[i] == 0xFF) cntA--; + if (buf[i] == 0x00) cntB--; } - if ( !cntA || !cntB ) { + if (!cntA || !cntB) { BigBuf_free(); return; } memcpy((uint8_t *)&t_config, buf, T55XX_CONFIG_LEN); - if ( isok == T55XX_CONFIG_LEN) { + if (isok == T55XX_CONFIG_LEN) { if (MF_DBGLEVEL > 1) DbpString("T55XX Config load success"); } #endif @@ -156,7 +160,8 @@ void loadT55xxConfig(void) { * @param period_1 * @param command (in binary char array) */ -void ModThenAcquireRawAdcSamples125k(uint32_t delay_off, uint32_t period_0, uint32_t period_1, uint8_t *command) { +void ModThenAcquireRawAdcSamples125k(uint32_t delay_off, uint32_t period_0, uint32_t period_1, uint8_t *command) +{ // start timer StartTicks(); @@ -197,7 +202,7 @@ void ModThenAcquireRawAdcSamples125k(uint32_t delay_off, uint32_t period_0, uint // prime cmd_len to save time comparing strings while modulating int cmd_len = 0; - while(command[cmd_len] != '\0' && command[cmd_len] != ' ') + while (command[cmd_len] != '\0' && command[cmd_len] != ' ') cmd_len++; int counter = 0; @@ -213,7 +218,7 @@ void ModThenAcquireRawAdcSamples125k(uint32_t delay_off, uint32_t period_0, uint } // note we appear to take about 7us to switch over (or run the if statements/loop...) WaitUS(period_0 - hack_cnt); - // else if cmd = 1 then turn field on + // else if cmd = 1 then turn field on } else { // if field already on leave alone (affects timing otherwise) if (off) { @@ -226,7 +231,7 @@ void ModThenAcquireRawAdcSamples125k(uint32_t delay_off, uint32_t period_0, uint } } } else { // old mode of cmd read using delay as off period - while(*command != '\0' && *command != ' ') { + while (*command != '\0' && *command != ' ') { LED_D_ON(); if (*(command++) == '0') TurnReadLFOn(period_0); @@ -249,7 +254,7 @@ void ModThenAcquireRawAdcSamples125k(uint32_t delay_off, uint32_t period_0, uint // Turn off antenna FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // tell client we are done - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); } /* blank r/w tag data stream @@ -268,20 +273,20 @@ void ReadTItag(void) // when we read a TI tag we sample the zerocross line at 2Mhz // TI tags modulate a 1 as 16 cycles of 123.2Khz // TI tags modulate a 0 as 16 cycles of 134.2Khz - #define FSAMPLE 2000000 - #define FREQLO 123200 - #define FREQHI 134200 +#define FSAMPLE 2000000 +#define FREQLO 123200 +#define FREQHI 134200 signed char *dest = (signed char *)BigBuf_get_addr(); uint16_t n = BigBuf_max_traceLen(); // 128 bit shift register [shift3:shift2:shift1:shift0] uint32_t shift3 = 0, shift2 = 0, shift1 = 0, shift0 = 0; - int i, cycles=0, samples=0; + int i, cycles = 0, samples = 0; // how many sample points fit in 16 cycles of each frequency - uint32_t sampleslo = (FSAMPLE<<4)/FREQLO, sampleshi = (FSAMPLE<<4)/FREQHI; + uint32_t sampleslo = (FSAMPLE << 4) / FREQLO, sampleshi = (FSAMPLE << 4) / FREQHI; // when to tell if we're close enough to one freq or another - uint32_t threshold = (sampleslo - sampleshi + 1)>>1; + uint32_t threshold = (sampleslo - sampleshi + 1) >> 1; // TI tags charge at 134.2Khz FpgaDownloadAndGo(FPGA_BITSTREAM_LF); @@ -298,28 +303,28 @@ void ReadTItag(void) FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); - for (i=0; i0) ) { + if ((dest[i] < 0) && (dest[i + 1] > 0)) { cycles++; // after 16 cycles, measure the frequency - if (cycles>15) { - cycles=0; - samples=i-samples; // number of samples in these 16 cycles + if (cycles > 15) { + cycles = 0; + samples = i - samples; // number of samples in these 16 cycles // TI bits are coming to us lsb first so shift them // right through our 128 bit right shift register - shift0 = (shift0>>1) | (shift1 << 31); - shift1 = (shift1>>1) | (shift2 << 31); - shift2 = (shift2>>1) | (shift3 << 31); + shift0 = (shift0 >> 1) | (shift1 << 31); + shift1 = (shift1 >> 1) | (shift2 << 31); + shift2 = (shift2 >> 1) | (shift3 << 31); shift3 >>= 1; // check if the cycles fall close to the number // expected for either the low or high frequency - if ( (samples>(sampleslo-threshold)) && (samples<(sampleslo+threshold)) ) { + if ((samples > (sampleslo - threshold)) && (samples < (sampleslo + threshold))) { // low frequency represents a 1 - shift3 |= (1<<31); - } else if ( (samples>(sampleshi-threshold)) && (samples<(sampleshi+threshold)) ) { + shift3 |= (1 << 31); + } else if ((samples > (sampleshi - threshold)) && (samples < (sampleshi + threshold))) { // high frequency represents a 0 } else { // probably detected a gay waveform or noise @@ -332,9 +337,9 @@ void ReadTItag(void) // if we see 17 zeroes followed by 6 ones, we might have a tag // remember the bits are backwards - if ( ((shift0 & 0x7fffff) == 0x7e0000) ) { + if (((shift0 & 0x7fffff) == 0x7e0000)) { // if start and end bytes match, we have a tag so break out of the loop - if ( ((shift0>>16)&0xff) == ((shift3>>8)&0xff) ) { + if (((shift0 >> 16) & 0xff) == ((shift3 >> 8) & 0xff)) { cycles = 0xF0B; //use this as a flag (ugly but whatever) break; } @@ -344,21 +349,21 @@ void ReadTItag(void) } // if flag is set we have a tag - if (cycles!=0xF0B) { + if (cycles != 0xF0B) { DbpString("Info: No valid tag detected."); } else { // put 64 bit data into shift1 and shift0 - shift0 = (shift0>>24) | (shift1 << 8); - shift1 = (shift1>>24) | (shift2 << 8); + shift0 = (shift0 >> 24) | (shift1 << 8); + shift1 = (shift1 >> 24) | (shift2 << 8); // align 16 bit crc into lower half of shift2 - shift2 = ((shift2>>24) | (shift3 << 8)) & 0x0ffff; + shift2 = ((shift2 >> 24) | (shift3 << 8)) & 0x0ffff; // if r/w tag, check ident match - if (shift3 & (1<<15) ) { + if (shift3 & (1 << 15)) { DbpString("Info: TI tag is rewriteable"); // only 15 bits compare, last bit of ident is not valid - if (((shift3 >> 16) ^ shift0) & 0x7fff ) { + if (((shift3 >> 16) ^ shift0) & 0x7fff) { DbpString("Error: Ident mismatch!"); } else { DbpString("Info: TI tag ident is valid"); @@ -371,19 +376,19 @@ void ReadTItag(void) // i'm 99% sure the crc algorithm is correct, but it may need to eat the // bytes in reverse or something // calculate CRC - uint32_t crc=0; + uint32_t crc = 0; - crc = update_crc16(crc, (shift0)&0xff); - crc = update_crc16(crc, (shift0>>8)&0xff); - crc = update_crc16(crc, (shift0>>16)&0xff); - crc = update_crc16(crc, (shift0>>24)&0xff); - crc = update_crc16(crc, (shift1)&0xff); - crc = update_crc16(crc, (shift1>>8)&0xff); - crc = update_crc16(crc, (shift1>>16)&0xff); - crc = update_crc16(crc, (shift1>>24)&0xff); + crc = update_crc16(crc, (shift0) & 0xff); + crc = update_crc16(crc, (shift0 >> 8) & 0xff); + crc = update_crc16(crc, (shift0 >> 16) & 0xff); + crc = update_crc16(crc, (shift0 >> 24) & 0xff); + crc = update_crc16(crc, (shift1) & 0xff); + crc = update_crc16(crc, (shift1 >> 8) & 0xff); + crc = update_crc16(crc, (shift1 >> 16) & 0xff); + crc = update_crc16(crc, (shift1 >> 24) & 0xff); Dbprintf("Info: Tag data: %x%08x, crc=%x", (unsigned int)shift1, (unsigned int)shift0, (unsigned int)shift2 & 0xFFFF); - if (crc != (shift2&0xffff)) { + if (crc != (shift2 & 0xffff)) { Dbprintf("Error: CRC mismatch, expected %x", (unsigned int)crc); } else { DbpString("Info: CRC is good"); @@ -397,9 +402,8 @@ void WriteTIbyte(uint8_t b) int i = 0; // modulate 8 bits out to the antenna - for (i=0; i<8; i++) - { - if ( b & ( 1 << i ) ) { + for (i = 0; i < 8; i++) { + if (b & (1 << i)) { // stop modulating antenna 1ms LOW(GPIO_SSC_DOUT); WaitUS(1000); @@ -422,7 +426,7 @@ void AcquireTiType(void) int i, j, n; // tag transmission is <20ms, sampling at 2M gives us 40K samples max // each sample is 1 bit stuffed into a uint32_t so we need 1250 uint32_t - #define TIBUFLEN 1250 +#define TIBUFLEN 1250 // clear buffer uint32_t *buf = (uint32_t *)BigBuf_get_addr(); @@ -483,9 +487,9 @@ void AcquireTiType(void) n = TIBUFLEN * 32; // unpack buffer - for (i = TIBUFLEN-1; i >= 0; i--) { + for (i = TIBUFLEN - 1; i >= 0; i--) { for (j = 0; j < 32; j++) { - if(buf[i] & (1 << j)) { + if (buf[i] & (1 << j)) { dest[--n] = 1; } else { dest[--n] = -1; @@ -503,15 +507,15 @@ void AcquireTiType(void) 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); - crc = update_crc16(crc, (idlo>>16)&0xff); - crc = update_crc16(crc, (idlo>>24)&0xff); - crc = update_crc16(crc, (idhi)&0xff); - crc = update_crc16(crc, (idhi>>8)&0xff); - crc = update_crc16(crc, (idhi>>16)&0xff); - crc = update_crc16(crc, (idhi>>24)&0xff); + if (crc == 0) { + crc = update_crc16(crc, (idlo) & 0xff); + crc = update_crc16(crc, (idlo >> 8) & 0xff); + crc = update_crc16(crc, (idlo >> 16) & 0xff); + crc = update_crc16(crc, (idlo >> 24) & 0xff); + crc = update_crc16(crc, (idhi) & 0xff); + crc = update_crc16(crc, (idhi >> 8) & 0xff); + crc = update_crc16(crc, (idhi >> 16) & 0xff); + crc = update_crc16(crc, (idhi >> 24) & 0xff); } Dbprintf("Writing to tag: %x%08x, crc=%x", idhi, idlo, crc); @@ -546,16 +550,16 @@ void WriteTItag(uint32_t idhi, uint32_t idlo, uint16_t crc) WriteTIbyte(0xbb); // keyword WriteTIbyte(0xeb); // password - WriteTIbyte( (idlo )&0xff ); - WriteTIbyte( (idlo>>8 )&0xff ); - WriteTIbyte( (idlo>>16)&0xff ); - WriteTIbyte( (idlo>>24)&0xff ); - WriteTIbyte( (idhi )&0xff ); - WriteTIbyte( (idhi>>8 )&0xff ); - WriteTIbyte( (idhi>>16)&0xff ); - WriteTIbyte( (idhi>>24)&0xff ); // data hi to lo - WriteTIbyte( (crc )&0xff ); // crc lo - WriteTIbyte( (crc>>8 )&0xff ); // crc hi + WriteTIbyte((idlo) & 0xff); + WriteTIbyte((idlo >> 8) & 0xff); + WriteTIbyte((idlo >> 16) & 0xff); + WriteTIbyte((idlo >> 24) & 0xff); + WriteTIbyte((idhi) & 0xff); + WriteTIbyte((idhi >> 8) & 0xff); + WriteTIbyte((idhi >> 16) & 0xff); + WriteTIbyte((idhi >> 24) & 0xff); // data hi to lo + WriteTIbyte((crc) & 0xff); // crc lo + WriteTIbyte((crc >> 8) & 0xff); // crc hi WriteTIbyte(0x00); // write frame lo WriteTIbyte(0x03); // write frame hi HIGH(GPIO_SSC_DOUT); @@ -573,7 +577,8 @@ void WriteTItag(uint32_t idhi, uint32_t idlo, uint16_t crc) // note: a call to FpgaDownloadAndGo(FPGA_BITSTREAM_LF) must be done before, but // this may destroy the bigbuf so be sure this is called before calling SimulateTagLowFrequencyEx -void SimulateTagLowFrequencyEx(int period, int gap, int ledcontrol, int numcycles) { +void SimulateTagLowFrequencyEx(int period, int gap, int ledcontrol, int numcycles) +{ // start us timer StartTicks(); @@ -588,7 +593,7 @@ void SimulateTagLowFrequencyEx(int period, int gap, int ledcontrol, int numcycle // set frequency, get values from 'lf config' command sample_config *sc = getSamplingConfig(); - if ( (sc->divisor == 1) || (sc->divisor < 0) || (sc->divisor > 255) ) + if ((sc->divisor == 1) || (sc->divisor < 0) || (sc->divisor > 255)) FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz else if (sc->divisor == 0) FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz @@ -601,10 +606,10 @@ void SimulateTagLowFrequencyEx(int period, int gap, int ledcontrol, int numcycle uint8_t check = 1; - for(;;) { + for (;;) { - if ( numcycles > -1 ) { - if ( x != numcycles ) { + if (numcycles > -1) { + if (x != numcycles) { ++x; } else { // exit without turning of field @@ -618,8 +623,8 @@ void SimulateTagLowFrequencyEx(int period, int gap, int ledcontrol, int numcycle // used as a simple detection of a reader field? while (!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)) { WDT_HIT(); - if ( !check ) { - if ( usb_poll_validate_length() || BUTTON_PRESS() ) + if (!check) { + if (usb_poll_validate_length() || BUTTON_PRESS()) goto OUT; } ++check; @@ -633,8 +638,8 @@ void SimulateTagLowFrequencyEx(int period, int gap, int ledcontrol, int numcycle //wait until SSC_CLK goes LOW while (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK) { WDT_HIT(); - if ( !check ) { - if ( usb_poll_validate_length() || BUTTON_PRESS() ) + if (!check) { + if (usb_poll_validate_length() || BUTTON_PRESS()) goto OUT; } ++check; @@ -657,7 +662,8 @@ OUT: LED_D_OFF(); } -void SimulateTagLowFrequency(int period, int gap, int ledcontrol) { +void SimulateTagLowFrequency(int period, int gap, int ledcontrol) +{ SimulateTagLowFrequencyEx(period, gap, ledcontrol, -1); } @@ -689,7 +695,7 @@ static void fc(int c, int *n) // an fc/8 encoded bit is a bit pattern of 11110000 x6 = 48 samples if (c == 8) { - for (idx=0; idx < 6; idx++) { + for (idx = 0; idx < 6; idx++) { dest[((*n)++)] = 1; dest[((*n)++)] = 1; dest[((*n)++)] = 1; @@ -720,11 +726,16 @@ static void fc(int c, int *n) // special start of frame marker containing invalid bit sequences // this one is focused on HID, with manchester encoding. -static void fcSTT(int *n) { - fc(8, n); fc(8, n); // invalid - fc(8, n); fc(10, n); // logical 0 - fc(10, n); fc(10, n); // invalid - fc(8, n); fc(10, n); // logical 0 +static void fcSTT(int *n) +{ + fc(8, n); + fc(8, n); // invalid + fc(8, n); + fc(10, n); // logical 0 + fc(10, n); + fc(10, n); // invalid + fc(8, n); + fc(10, n); // logical 0 } // compose fc/X fc/Y waveform (FSKx) @@ -732,28 +743,28 @@ static void fcAll(uint8_t fc, int *n, uint8_t clock, uint16_t *modCnt) { uint8_t *dest = BigBuf_get_addr(); uint8_t halfFC = fc >> 1; - uint8_t wavesPerClock = clock/fc; + uint8_t wavesPerClock = clock / fc; uint8_t mod = clock % fc; //modifier - uint8_t modAdj = fc/mod; //how often to apply modifier + uint8_t modAdj = fc / mod; //how often to apply modifier bool modAdjOk = !(fc % mod); //if (fc % mod==0) modAdjOk = true; // loop through clock - step field clock - for (uint8_t idx=0; idx < wavesPerClock; idx++){ + for (uint8_t idx = 0; idx < wavesPerClock; idx++) { // put 1/2 FC length 1's and 1/2 0's per field clock wave (to create the wave) memset(dest + (*n), 0, fc - halfFC); //in case of odd number use extra here memset(dest + (*n) + (fc - halfFC), 1, halfFC); *n += fc; } - if (mod > 0) (*modCnt)++; + if (mod > 0)(*modCnt)++; - if ((mod > 0) && modAdjOk){ //fsk2 - if ((*modCnt % modAdj) == 0){ //if 4th 8 length wave in a rf/50 add extra 8 length wave + if ((mod > 0) && modAdjOk) { //fsk2 + if ((*modCnt % modAdj) == 0) { //if 4th 8 length wave in a rf/50 add extra 8 length wave memset(dest + (*n), 0, fc - halfFC); - memset(dest + (*n) + ( fc - halfFC), 1, halfFC); + memset(dest + (*n) + (fc - halfFC), 1, halfFC); *n += fc; } } - if (mod > 0 && !modAdjOk){ //fsk1 + if (mod > 0 && !modAdjOk) { //fsk1 memset(dest + (*n), 0, mod - (mod >> 1)); memset(dest + (*n) + (mod - (mod >> 1)), 1, mod >> 1); *n += mod; @@ -762,7 +773,8 @@ static void fcAll(uint8_t fc, int *n, uint8_t clock, uint16_t *modCnt) // prepare a waveform pattern in the buffer based on the ID given then // simulate a HID tag until the button is pressed -void CmdHIDsimTAGEx( uint32_t hi, uint32_t lo, int ledcontrol, int numcycles) { +void CmdHIDsimTAGEx(uint32_t hi, uint32_t lo, int ledcontrol, int numcycles) +{ if (hi > 0xFFF) { DbpString("[!] tags can only have 44 bits. - USE lf simfsk for larger tags"); @@ -798,9 +810,11 @@ void CmdHIDsimTAGEx( uint32_t hi, uint32_t lo, int ledcontrol, int numcycles) { if ((i % 4) == 3) fc(0, &n); if ((hi >> i) & 1) { - fc(10, &n); fc(8, &n); // low-high transition + fc(10, &n); + fc(8, &n); // low-high transition } else { - fc(8, &n); fc(10, &n); // high-low transition + fc(8, &n); + fc(10, &n); // high-low transition } } @@ -809,10 +823,12 @@ void CmdHIDsimTAGEx( uint32_t hi, uint32_t lo, int ledcontrol, int numcycles) { if ((i % 4) == 3) fc(0, &n); - if (( lo >> i ) & 1) { - fc(10, &n); fc(8, &n); // low-high transition + if ((lo >> i) & 1) { + fc(10, &n); + fc(8, &n); // low-high transition } else { - fc(8, &n); fc(10, &n); // high-low transition + fc(8, &n); + fc(10, &n); // high-low transition } } @@ -821,19 +837,22 @@ void CmdHIDsimTAGEx( uint32_t hi, uint32_t lo, int ledcontrol, int numcycles) { if (ledcontrol) LED_A_OFF(); } -void CmdHIDsimTAG( uint32_t hi, uint32_t lo, int ledcontrol) { - CmdHIDsimTAGEx( hi, lo, ledcontrol, -1); +void CmdHIDsimTAG(uint32_t hi, uint32_t lo, int ledcontrol) +{ + CmdHIDsimTAGEx(hi, lo, ledcontrol, -1); DbpString("[!] simulation finished"); } // prepare a waveform pattern in the buffer based on the ID given then // simulate a FSK tag until the button is pressed // arg1 contains fcHigh and fcLow, arg2 contains STT marker and clock -void CmdFSKsimTAG(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *bits) { +void CmdFSKsimTAG(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *bits) +{ FpgaDownloadAndGo(FPGA_BITSTREAM_LF); // free eventually allocated BigBuf memory - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(false); @@ -844,12 +863,12 @@ void CmdFSKsimTAG(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *bits) { uint8_t clk = arg2 & 0xFF; uint8_t stt = (arg2 >> 8) & 1; - if ( stt ) { + if (stt) { //int fsktype = ( fcHigh == 8 && fcLow == 5) ? 1 : 2; //fcSTT(&n); } - for (i=0; i> 8) & 1; - if (encoding == 2){ //biphase + if (encoding == 2) { //biphase uint8_t phase = 0; - for (i=0; i 0 && lo > 0 && (size == 96 || size == 192)){ + if (idx > 0 && lo > 0 && (size == 96 || size == 192)) { // go over previously decoded manchester data and decode into usable tag ID - if (hi2 != 0){ //extra large HID tags 88/192 bits + if (hi2 != 0) { //extra large HID tags 88/192 bits Dbprintf("TAG ID: %x%08x%08x (%d)", - hi2, - hi, - lo, - (lo >> 1) & 0xFFFF - ); + hi2, + hi, + lo, + (lo >> 1) & 0xFFFF + ); } else { //standard HID tags 44/96 bits uint8_t bitlen = 0; uint32_t fc = 0; uint32_t cardnum = 0; - if (((hi >> 5) & 1) == 1){//if bit 38 is set then < 37 bit format is used + if (((hi >> 5) & 1) == 1) { //if bit 38 is set then < 37 bit format is used uint32_t lo2 = 0; - lo2=(((hi & 31) << 12) | (lo>>20)); //get bits 21-37 to check for format len bit + lo2 = (((hi & 31) << 12) | (lo >> 20)); //get bits 21-37 to check for format len bit uint8_t idx3 = 1; - while (lo2 > 1){ //find last bit set to 1 (format len bit) + while (lo2 > 1) { //find last bit set to 1 (format len bit) lo2 >>= 1; idx3++; } bitlen = idx3 + 19; fc = 0; cardnum = 0; - if (bitlen == 26){ + if (bitlen == 26) { cardnum = (lo >> 1) & 0xFFFF; fc = (lo >> 17) & 0xFF; } - if (bitlen == 37){ - cardnum = (lo >> 1 ) & 0x7FFFF; + if (bitlen == 37) { + cardnum = (lo >> 1) & 0x7FFFF; fc = ((hi & 0xF) << 12) | (lo >> 20); } - if (bitlen == 34){ + if (bitlen == 34) { cardnum = (lo >> 1) & 0xFFFF; fc = ((hi & 1) << 15) | (lo >> 17); } - if (bitlen == 35){ + if (bitlen == 35) { cardnum = (lo >> 1) & 0xFFFFF; - fc = ((hi & 1) << 11)|(lo >> 21); + fc = ((hi & 1) << 11) | (lo >> 21); } - } - else { //if bit 38 is not set then 37 bit format is used - bitlen= 37; + } else { //if bit 38 is not set then 37 bit format is used + bitlen = 37; fc = 0; cardnum = 0; - if (bitlen == 37){ + if (bitlen == 37) { cardnum = (lo >> 1) & 0x7FFFF; fc = ((hi & 0xF) << 12) | (lo >> 20); } @@ -1085,9 +1105,9 @@ void CmdHIDdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) bitlen, fc, cardnum - ); + ); } - if (findone){ + if (findone) { *high = hi; *low = lo; break; @@ -1102,7 +1122,8 @@ void CmdHIDdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) } // loop to get raw HID waveform then FSK demodulate the TAG ID from it -void CmdAWIDdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) { +void CmdAWIDdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) +{ uint8_t *dest = BigBuf_get_addr(); @@ -1140,11 +1161,11 @@ void CmdAWIDdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) // (26 bit format shown) //get raw ID before removing parities - uint32_t rawLo = bytebits_to_byte(dest+idx+64, 32); - uint32_t rawHi = bytebits_to_byte(dest+idx+32, 32); - uint32_t rawHi2 = bytebits_to_byte(dest+idx, 32); + uint32_t rawLo = bytebits_to_byte(dest + idx + 64, 32); + uint32_t rawHi = bytebits_to_byte(dest + idx + 32, 32); + uint32_t rawHi2 = bytebits_to_byte(dest + idx, 32); - size = removeParity(dest, idx+8, 4, 1, 88); + size = removeParity(dest, idx + 8, 4, 1, 88); if (size != 66) continue; // ok valid card found! @@ -1166,23 +1187,23 @@ void CmdAWIDdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) uint32_t code1 = 0; uint32_t code2 = 0; uint8_t fmtLen = bytebits_to_byte(dest, 8); - if (fmtLen == 26){ - fc = bytebits_to_byte(dest+9, 8); - cardnum = bytebits_to_byte(dest+17, 16); - code1 = bytebits_to_byte(dest+8, fmtLen); + if (fmtLen == 26) { + fc = bytebits_to_byte(dest + 9, 8); + cardnum = bytebits_to_byte(dest + 17, 16); + code1 = bytebits_to_byte(dest + 8, fmtLen); Dbprintf("AWID Found - BitLength: %d, FC: %d, Card: %d - Wiegand: %x, Raw: %08x%08x%08x", fmtLen, fc, cardnum, code1, rawHi2, rawHi, rawLo); } else { - cardnum = bytebits_to_byte(dest+8+(fmtLen-17), 16); - if (fmtLen > 32){ - code1 = bytebits_to_byte(dest+8, fmtLen-32); - code2 = bytebits_to_byte(dest+8+(fmtLen-32), 32); + cardnum = bytebits_to_byte(dest + 8 + (fmtLen - 17), 16); + if (fmtLen > 32) { + code1 = bytebits_to_byte(dest + 8, fmtLen - 32); + code2 = bytebits_to_byte(dest + 8 + (fmtLen - 32), 32); Dbprintf("AWID Found - BitLength: %d -unknown BitLength- (%d) - Wiegand: %x%08x, Raw: %08x%08x%08x", fmtLen, cardnum, code1, code2, rawHi2, rawHi, rawLo); - } else{ - code1 = bytebits_to_byte(dest+8, fmtLen); + } else { + code1 = bytebits_to_byte(dest + 8, fmtLen); Dbprintf("AWID Found - BitLength: %d -unknown BitLength- (%d) - Wiegand: %x, Raw: %08x%08x%08x", fmtLen, cardnum, code1, rawHi2, rawHi, rawLo); } } - if (findone){ + if (findone) { if (ledcontrol) LED_A_OFF(); *high = rawHi; *low = rawLo; @@ -1197,7 +1218,8 @@ void CmdAWIDdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) if (ledcontrol) LED_A_OFF(); } -void CmdEM410xdemod(int findone, uint32_t *high, uint64_t *low, int ledcontrol) { +void CmdEM410xdemod(int findone, uint32_t *high, uint64_t *low, int ledcontrol) +{ uint8_t *dest = BigBuf_get_addr(); size_t size = 0, idx = 0; @@ -1209,7 +1231,7 @@ void CmdEM410xdemod(int findone, uint32_t *high, uint64_t *low, int ledcontrol) LFSetupFPGAForADC(95, true); - while(!BUTTON_PRESS() && !usb_poll_validate_length()) { + while (!BUTTON_PRESS() && !usb_poll_validate_length()) { WDT_HIT(); if (ledcontrol) LED_A_ON(); @@ -1224,26 +1246,26 @@ void CmdEM410xdemod(int findone, uint32_t *high, uint64_t *low, int ledcontrol) if (errCnt < 0) continue; - errCnt = Em410xDecode(dest, &size, &idx, &hi, &lo); - if (errCnt == 1){ - if (size == 128){ - Dbprintf("EM XL TAG ID: %06x%08x%08x - (%05d_%03d_%08d)", - hi, - (uint32_t)(lo >> 32), - (uint32_t)lo, - (uint32_t)(lo & 0xFFFF), - (uint32_t)((lo >> 16LL) & 0xFF), - (uint32_t)(lo & 0xFFFFFF)); - } else { - Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)", - (uint32_t)(lo >> 32), - (uint32_t)lo, - (uint32_t)(lo & 0xFFFF), - (uint32_t)((lo >> 16LL) & 0xFF), - (uint32_t)(lo & 0xFFFFFF)); - } + errCnt = Em410xDecode(dest, &size, &idx, &hi, &lo); + if (errCnt == 1) { + if (size == 128) { + Dbprintf("EM XL TAG ID: %06x%08x%08x - (%05d_%03d_%08d)", + hi, + (uint32_t)(lo >> 32), + (uint32_t)lo, + (uint32_t)(lo & 0xFFFF), + (uint32_t)((lo >> 16LL) & 0xFF), + (uint32_t)(lo & 0xFFFFFF)); + } else { + Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)", + (uint32_t)(lo >> 32), + (uint32_t)lo, + (uint32_t)(lo & 0xFFFF), + (uint32_t)((lo >> 16LL) & 0xFF), + (uint32_t)(lo & 0xFFFFFF)); + } - if (findone){ + if (findone) { if (ledcontrol) LED_A_OFF(); *high = hi; *low = lo; @@ -1259,7 +1281,8 @@ void CmdEM410xdemod(int findone, uint32_t *high, uint64_t *low, int ledcontrol) if (ledcontrol) LED_A_OFF(); } -void CmdIOdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) { +void CmdIOdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) +{ uint8_t *dest = BigBuf_get_addr(); @@ -1285,61 +1308,61 @@ void CmdIOdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) { WDT_HIT(); idx = detectIOProx(dest, &size, &dummyIdx); if (idx < 0) continue; - //valid tag found + //valid tag found - //Index map - //0 10 20 30 40 50 60 - //| | | | | | | - //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23 - //----------------------------------------------------------------------------- - //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 checksum 11 - // - //Checksum: - //00000000 0 11110000 1 11100000 1 00000001 1 00000011 1 10110110 1 01110101 11 - //preamble F0 E0 01 03 B6 75 - // How to calc checksum, - // http://www.proxmark.org/forum/viewtopic.php?id=364&p=6 - // F0 + E0 + 01 + 03 + B6 = 28A - // 28A & FF = 8A - // FF - 8A = 75 - // Checksum: 0x75 - //XSF(version)facility:codeone+codetwo - //Handle the data - // if(findone){ //only print binary if we are doing one - // Dbprintf("%d%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],dest[idx+8]); - // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+9], dest[idx+10],dest[idx+11],dest[idx+12],dest[idx+13],dest[idx+14],dest[idx+15],dest[idx+16],dest[idx+17]); - // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+18],dest[idx+19],dest[idx+20],dest[idx+21],dest[idx+22],dest[idx+23],dest[idx+24],dest[idx+25],dest[idx+26]); - // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+27],dest[idx+28],dest[idx+29],dest[idx+30],dest[idx+31],dest[idx+32],dest[idx+33],dest[idx+34],dest[idx+35]); - // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+36],dest[idx+37],dest[idx+38],dest[idx+39],dest[idx+40],dest[idx+41],dest[idx+42],dest[idx+43],dest[idx+44]); - // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+45],dest[idx+46],dest[idx+47],dest[idx+48],dest[idx+49],dest[idx+50],dest[idx+51],dest[idx+52],dest[idx+53]); - // Dbprintf("%d%d%d%d%d%d%d%d %d%d",dest[idx+54],dest[idx+55],dest[idx+56],dest[idx+57],dest[idx+58],dest[idx+59],dest[idx+60],dest[idx+61],dest[idx+62],dest[idx+63]); - // } - code = bytebits_to_byte(dest+idx, 32); - code2 = bytebits_to_byte(dest+idx+32, 32); - version = bytebits_to_byte(dest+idx+27, 8); //14,4 - facilitycode = bytebits_to_byte(dest+idx+18, 8); - number = (bytebits_to_byte(dest+idx+36, 8) << 8) | (bytebits_to_byte(dest+idx+45, 8)); //36,9 + //Index map + //0 10 20 30 40 50 60 + //| | | | | | | + //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23 + //----------------------------------------------------------------------------- + //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 checksum 11 + // + //Checksum: + //00000000 0 11110000 1 11100000 1 00000001 1 00000011 1 10110110 1 01110101 11 + //preamble F0 E0 01 03 B6 75 + // How to calc checksum, + // http://www.proxmark.org/forum/viewtopic.php?id=364&p=6 + // F0 + E0 + 01 + 03 + B6 = 28A + // 28A & FF = 8A + // FF - 8A = 75 + // Checksum: 0x75 + //XSF(version)facility:codeone+codetwo + //Handle the data + // if(findone){ //only print binary if we are doing one + // Dbprintf("%d%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],dest[idx+8]); + // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+9], dest[idx+10],dest[idx+11],dest[idx+12],dest[idx+13],dest[idx+14],dest[idx+15],dest[idx+16],dest[idx+17]); + // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+18],dest[idx+19],dest[idx+20],dest[idx+21],dest[idx+22],dest[idx+23],dest[idx+24],dest[idx+25],dest[idx+26]); + // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+27],dest[idx+28],dest[idx+29],dest[idx+30],dest[idx+31],dest[idx+32],dest[idx+33],dest[idx+34],dest[idx+35]); + // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+36],dest[idx+37],dest[idx+38],dest[idx+39],dest[idx+40],dest[idx+41],dest[idx+42],dest[idx+43],dest[idx+44]); + // Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+45],dest[idx+46],dest[idx+47],dest[idx+48],dest[idx+49],dest[idx+50],dest[idx+51],dest[idx+52],dest[idx+53]); + // Dbprintf("%d%d%d%d%d%d%d%d %d%d",dest[idx+54],dest[idx+55],dest[idx+56],dest[idx+57],dest[idx+58],dest[idx+59],dest[idx+60],dest[idx+61],dest[idx+62],dest[idx+63]); + // } + code = bytebits_to_byte(dest + idx, 32); + code2 = bytebits_to_byte(dest + idx + 32, 32); + version = bytebits_to_byte(dest + idx + 27, 8); //14,4 + facilitycode = bytebits_to_byte(dest + idx + 18, 8); + number = (bytebits_to_byte(dest + idx + 36, 8) << 8) | (bytebits_to_byte(dest + idx + 45, 8)); //36,9 - crc = bytebits_to_byte(dest+idx+54, 8); - for (uint8_t i=1; i<6; ++i) - calccrc += bytebits_to_byte(dest+idx+9*i, 8); - calccrc &= 0xff; - calccrc = 0xff - calccrc; + crc = bytebits_to_byte(dest + idx + 54, 8); + for (uint8_t i = 1; i < 6; ++i) + calccrc += bytebits_to_byte(dest + idx + 9 * i, 8); + calccrc &= 0xff; + calccrc = 0xff - calccrc; - char *crcStr = (crc == calccrc) ? "ok" : "!crc"; + char *crcStr = (crc == calccrc) ? "ok" : "!crc"; - Dbprintf("IO Prox XSF(%02d)%02x:%05d (%08x%08x) [%02x %s]", version, facilitycode, number, code, code2, crc, crcStr); - // if we're only looking for one tag - if (findone){ - if (ledcontrol) LED_A_OFF(); - *high = code; - *low = code2; - break; - } - code = code2 = 0; - version = facilitycode = 0; - number = 0; - idx = 0; + Dbprintf("IO Prox XSF(%02d)%02x:%05d (%08x%08x) [%02x %s]", version, facilitycode, number, code, code2, crc, crcStr); + // if we're only looking for one tag + if (findone) { + if (ledcontrol) LED_A_OFF(); + *high = code; + *low = code2; + break; + } + code = code2 = 0; + version = facilitycode = 0; + number = 0; + idx = 0; WDT_HIT(); } @@ -1359,20 +1382,23 @@ void CmdIOdemodFSK(int findone, uint32_t *high, uint32_t *low, int ledcontrol) { * Q5 tags seems to have issues when these values changes. */ -void TurnReadLFOn(uint32_t delay) { +void TurnReadLFOn(uint32_t delay) +{ FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD); // measure antenna strength. //int adcval = ((MAX_ADC_LF_VOLTAGE * AvgAdc(ADC_CHAN_LF)) >> 10); WaitUS(delay); } -void TurnReadLF_off(uint32_t delay) { +void TurnReadLF_off(uint32_t delay) +{ FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); WaitUS(delay); } // Write one bit to card -void T55xxWriteBit(int bit) { +void T55xxWriteBit(int bit) +{ if (!bit) TurnReadLFOn(t_config.write_0); else @@ -1382,7 +1408,8 @@ void T55xxWriteBit(int bit) { } // Send T5577 reset command then read stream (see if we can identify the start of the stream) -void T55xxResetRead(void) { +void T55xxResetRead(void) +{ LED_A_ON(); //clear buffer now so it does not interfere with timing later BigBuf_Clear_keep_EM(); @@ -1409,15 +1436,16 @@ void T55xxResetRead(void) { // Turn the field off FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); LED_A_OFF(); } // Write one card block in page 0, no lock -void T55xxWriteBlockExt(uint32_t Data, uint8_t Block, uint32_t Pwd, uint8_t arg) { +void T55xxWriteBlockExt(uint32_t Data, uint8_t Block, uint32_t Pwd, uint8_t arg) +{ LED_A_ON(); bool PwdMode = arg & 0x1; - uint8_t Page = (arg & 0x2)>>1; + uint8_t Page = (arg & 0x2) >> 1; bool testMode = arg & 0x4; uint32_t i = 0; @@ -1437,7 +1465,7 @@ void T55xxWriteBlockExt(uint32_t Data, uint8_t Block, uint32_t Pwd, uint8_t arg) T55xxWriteBit(testMode ? 0 : 1); T55xxWriteBit(testMode ? 1 : Page); //Page 0 - if (PwdMode){ + if (PwdMode) { // Send Pwd for (i = 0x80000000; i != 0; i >>= 1) T55xxWriteBit(Pwd & i); @@ -1488,16 +1516,18 @@ void T55xxWriteBlockExt(uint32_t Data, uint8_t Block, uint32_t Pwd, uint8_t arg) } // Write one card block in page 0, no lock -void T55xxWriteBlock(uint32_t Data, uint8_t Block, uint32_t Pwd, uint8_t arg) { +void T55xxWriteBlock(uint32_t Data, uint8_t Block, uint32_t Pwd, uint8_t arg) +{ T55xxWriteBlockExt(Data, Block, Pwd, arg); - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); } // Read one card block in page [page] -void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) { +void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) +{ LED_A_ON(); bool PwdMode = arg0 & 0x1; - uint8_t Page = ( arg0 & 0x2 ) >> 1; + uint8_t Page = (arg0 & 0x2) >> 1; bool brute_mem = arg0 & 0x4; uint32_t i = 0; @@ -1507,7 +1537,7 @@ void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) { uint8_t start_wait = 4; size_t samples = 12000; - if ( brute_mem ) { + if (brute_mem) { start_wait = 0; samples = 1024; } @@ -1533,7 +1563,7 @@ void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) { T55xxWriteBit(1); T55xxWriteBit(Page); //Page 0 - if (PwdMode){ + if (PwdMode) { // Send Pwd for (i = 0x80000000; i != 0; i >>= 1) T55xxWriteBit(Pwd & i); @@ -1549,21 +1579,22 @@ void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) { // Turn field on to read the response // 137*8 seems to get to the start of data pretty well... // but we want to go past the start and let the repeating data settle in... - TurnReadLFOn(200*8); + TurnReadLFOn(200 * 8); // Acquisition // Now do the acquisition DoPartialAcquisition(0, true, samples, 0); // Turn the field off - if ( !brute_mem ) { + if (!brute_mem) { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); LED_A_OFF(); } } -void T55xx_ChkPwds() { +void T55xx_ChkPwds() +{ DbpString("[+] T55XX Check pwds using flashmemory starting"); @@ -1579,7 +1610,7 @@ void T55xx_ChkPwds() { while (x--) { b1 = 0; T55xxReadBlock(4, 1, 0); - for (uint16_t j=0; j < 1024; ++j) + for (uint16_t j = 0; j < 1024; ++j) b1 += buf[j]; b1 *= b1; @@ -1597,21 +1628,21 @@ void T55xx_ChkPwds() { #ifdef WITH_FLASH bool use_flashmem = true; - if ( use_flashmem ) { + if (use_flashmem) { BigBuf_Clear_EM(); uint16_t isok = 0; uint8_t counter[2] = {0x00, 0x00}; - isok = Flash_ReadData(DEFAULT_T55XX_KEYS_OFFSET, counter, sizeof(counter) ); - if ( isok != sizeof(counter) ) + isok = Flash_ReadData(DEFAULT_T55XX_KEYS_OFFSET, counter, sizeof(counter)); + if (isok != sizeof(counter)) goto OUT; pwdCount = counter[1] << 8 | counter[0]; - if ( pwdCount == 0 && pwdCount == 0xFFFF) + if (pwdCount == 0 && pwdCount == 0xFFFF) goto OUT; - isok = Flash_ReadData(DEFAULT_T55XX_KEYS_OFFSET+2, pwds, pwdCount * 4); - if ( isok != pwdCount * 4 ) + isok = Flash_ReadData(DEFAULT_T55XX_KEYS_OFFSET + 2, pwds, pwdCount * 4); + if (isok != pwdCount * 4) goto OUT; Dbprintf("[=] Password dictionary count %d ", pwdCount); @@ -1619,7 +1650,7 @@ void T55xx_ChkPwds() { #endif uint32_t pwd = 0, curr = 0, prev = 0; - for (uint16_t i =0; i< pwdCount; ++i) { + for (uint16_t i = 0; i < pwdCount; ++i) { if (BUTTON_PRESS() && !usb_poll_validate_length()) { goto OUT; @@ -1632,7 +1663,7 @@ void T55xx_ChkPwds() { // calc mean of BigBuf 1024 samples. uint32_t sum = 0; - for (uint16_t j=0; j<1024; ++j) { + for (uint16_t j = 0; j < 1024; ++j) { sum += buf[j]; } @@ -1642,27 +1673,28 @@ void T55xx_ChkPwds() { int32_t tmp = (sum - baseline); curr = ABS(tmp); - Dbprintf("[=] Pwd %08X | ABS %u", pwd, curr ); + Dbprintf("[=] Pwd %08X | ABS %u", pwd, curr); - if ( curr > prev ) { + if (curr > prev) { - Dbprintf("[=] --> ABS %u Candidate %08X <--", curr, pwd ); + Dbprintf("[=] --> ABS %u Candidate %08X <--", curr, pwd); candidate = pwd; prev = curr; } } - if ( candidate ) + if (candidate) ret = 1; OUT: FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); - cmd_send(CMD_ACK,ret,candidate,0,0,0); + cmd_send(CMD_ACK, ret, candidate, 0, 0, 0); LEDsoff(); } -void T55xxWakeUp(uint32_t Pwd){ +void T55xxWakeUp(uint32_t Pwd) +{ LED_B_ON(); uint32_t i = 0; @@ -1686,22 +1718,24 @@ void T55xxWakeUp(uint32_t Pwd){ T55xxWriteBit(Pwd & i); // Turn and leave field on to let the begin repeating transmission - TurnReadLFOn(20*1000); + TurnReadLFOn(20 * 1000); } /*-------------- Cloning routines -----------*/ -void WriteT55xx(uint32_t *blockdata, uint8_t startblock, uint8_t numblocks) { +void WriteT55xx(uint32_t *blockdata, uint8_t startblock, uint8_t numblocks) +{ // write last block first and config block last (if included) - for (uint8_t i = numblocks+startblock; i > startblock; i--) - T55xxWriteBlockExt(blockdata[i-1], i-1, 0, 0); + for (uint8_t i = numblocks + startblock; i > startblock; i--) + T55xxWriteBlockExt(blockdata[i - 1], i - 1, 0, 0); } // Copy HID id to card and setup block 0 config -void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT) { - uint32_t data[] = {0,0,0,0,0,0,0}; +void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT) +{ + uint32_t data[] = {0, 0, 0, 0, 0, 0, 0}; uint8_t last_block = 0; - if (longFMT){ + if (longFMT) { // Ensure no more than 84 bits supplied if (hi2 > 0xFFFFF) { DbpString("Tags can only have 84 bits."); @@ -1737,11 +1771,12 @@ void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT) { // data[0] = T5555_SET_BITRATE(50) | T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | last_block << T5555_MAXBLOCK_SHIFT; LED_D_ON(); - WriteT55xx(data, 0, last_block+1); + WriteT55xx(data, 0, last_block + 1); LED_D_OFF(); } -void CopyIOtoT55x7(uint32_t hi, uint32_t lo) { +void CopyIOtoT55x7(uint32_t hi, uint32_t lo) +{ uint32_t data[] = {T55x7_BITRATE_RF_64 | T55x7_MODULATION_FSK2a | (2 << T55x7_MAXBLOCK_SHIFT), hi, lo}; //TODO add selection of chip for Q5 or T55x7 // data[0] = T5555_SET_BITRATE(64) | T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | 2 << T5555_MAXBLOCK_SHIFT; @@ -1754,7 +1789,8 @@ void CopyIOtoT55x7(uint32_t hi, uint32_t lo) { } // Clone Indala 64-bit tag by UID to T55x7 -void CopyIndala64toT55x7(uint32_t hi, uint32_t lo) { +void CopyIndala64toT55x7(uint32_t hi, uint32_t lo) +{ //Program the 2 data blocks for supplied 64bit UID // and the Config for Indala 64 format (RF/32;PSK2 with RF/2;Maxblock=2) uint32_t data[] = { T55x7_BITRATE_RF_32 | T55x7_MODULATION_PSK2 | (2 << T55x7_MAXBLOCK_SHIFT), hi, lo}; @@ -1766,7 +1802,8 @@ void CopyIndala64toT55x7(uint32_t hi, uint32_t lo) { // T5567WriteBlock(0x603E1042,0); } // Clone Indala 224-bit tag by UID to T55x7 -void CopyIndala224toT55x7(uint32_t uid1, uint32_t uid2, uint32_t uid3, uint32_t uid4, uint32_t uid5, uint32_t uid6, uint32_t uid7) { +void CopyIndala224toT55x7(uint32_t uid1, uint32_t uid2, uint32_t uid3, uint32_t uid4, uint32_t uid5, uint32_t uid6, uint32_t uid7) +{ //Program the 7 data blocks for supplied 224bit UID uint32_t data[] = {0, uid1, uid2, uid3, uid4, uid5, uid6, uid7}; // and the block 0 for Indala224 format @@ -1779,20 +1816,22 @@ void CopyIndala224toT55x7(uint32_t uid1, uint32_t uid2, uint32_t uid3, uint32_t // T5567WriteBlock(0x603E10E2,0); } // clone viking tag to T55xx -void CopyVikingtoT55xx(uint32_t block1, uint32_t block2, uint8_t Q5) { +void CopyVikingtoT55xx(uint32_t block1, uint32_t block2, uint8_t Q5) +{ uint32_t data[] = {T55x7_BITRATE_RF_32 | T55x7_MODULATION_MANCHESTER | (2 << T55x7_MAXBLOCK_SHIFT), block1, block2}; if (Q5) data[0] = T5555_SET_BITRATE(32) | T5555_MODULATION_MANCHESTER | 2 << T5555_MAXBLOCK_SHIFT; // Program the data blocks for supplied ID and the block 0 config WriteT55xx(data, 0, 3); LED_D_OFF(); - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); } // Define 9bit header for EM410x tags #define EM410X_HEADER 0x1FF #define EM410X_ID_LENGTH 40 -void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo) { +void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo) +{ int i, id_bit; uint64_t id = EM410X_HEADER; uint64_t rev_id = 0; // reversed ID @@ -1848,11 +1887,11 @@ void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo) { // Add stop bit id <<= 1; - Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555"); + Dbprintf("Started writing %s tag ...", card ? "T55x7" : "T5555"); LED_D_ON(); // Write EM410x ID - uint32_t data[] = {0, (uint32_t)(id>>32), (uint32_t)(id & 0xFFFFFFFF)}; + uint32_t data[] = {0, (uint32_t)(id >> 32), (uint32_t)(id & 0xFFFFFFFF)}; clock = (card & 0xFF00) >> 8; clock = (clock == 0) ? 64 : clock; @@ -1872,9 +1911,9 @@ void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo) { LED_D_OFF(); Dbprintf("Tag %s written with 0x%08x%08x\n", - card ? "T55x7":"T5555", - (uint32_t)(id >> 32), - (uint32_t)id); + card ? "T55x7" : "T5555", + (uint32_t)(id >> 32), + (uint32_t)id); } //----------------------------------- @@ -1888,9 +1927,9 @@ void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo) { #define FWD_CMD_DISABLE 0x5 uint8_t forwardLink_data[64]; //array of forwarded bits -uint8_t * forward_ptr; //ptr for forward message preparation +uint8_t *forward_ptr; //ptr for forward message preparation uint8_t fwd_bit_sz; //forwardlink bit counter -uint8_t * fwd_write_ptr; //forwardlink bit pointer +uint8_t *fwd_write_ptr; //forwardlink bit pointer //==================================================================== // prepares command bits @@ -1905,7 +1944,8 @@ uint8_t * fwd_write_ptr; //forwardlink bit pointer // These timings work for 4469/4269/4305 (with the 55*8 above) // WRITE_0 = 23*8 , 9*8 -uint8_t Prepare_Cmd( uint8_t cmd ) { +uint8_t Prepare_Cmd(uint8_t cmd) +{ *forward_ptr++ = 0; //start bit *forward_ptr++ = 0; //second pause for 4050 code @@ -1925,13 +1965,14 @@ uint8_t Prepare_Cmd( uint8_t cmd ) { // prepares address bits // see EM4469 spec //==================================================================== -uint8_t Prepare_Addr( uint8_t addr ) { +uint8_t Prepare_Addr(uint8_t addr) +{ register uint8_t line_parity; uint8_t i; line_parity = 0; - for( i=0; i<6; i++ ) { + for (i = 0; i < 6; i++) { *forward_ptr++ = addr; line_parity ^= addr; addr >>= 1; @@ -1946,7 +1987,8 @@ uint8_t Prepare_Addr( uint8_t addr ) { // prepares data bits intreleaved with parity bits // see EM4469 spec //==================================================================== -uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) { +uint8_t Prepare_Data(uint16_t data_low, uint16_t data_hi) +{ register uint8_t line_parity; register uint8_t column_parity; @@ -1956,20 +1998,20 @@ uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) { data = data_low; column_parity = 0; - for(i=0; i<4; i++) { + for (i = 0; i < 4; i++) { line_parity = 0; - for(j=0; j<8; j++) { + for (j = 0; j < 8; j++) { line_parity ^= data; column_parity ^= (data & 1) << j; *forward_ptr++ = data; data >>= 1; } *forward_ptr++ = line_parity; - if(i == 1) + if (i == 1) data = data_hi; } - for(j=0; j<8; j++) { + for (j = 0; j < 8; j++) { *forward_ptr++ = column_parity; column_parity >>= 1; } @@ -1983,7 +2025,8 @@ uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) { // Requires: forwarLink_data filled with valid bits (1 bit per byte) // fwd_bit_count set with number of bits to be sent //==================================================================== -void SendForward(uint8_t fwd_bit_count) { +void SendForward(uint8_t fwd_bit_count) +{ // iceman, 21.3us increments for the USclock verification. // 55FC * 8us == 440us / 21.3 === 20.65 steps. could be too short. Go for 56FC instead @@ -2004,24 +2047,25 @@ void SendForward(uint8_t fwd_bit_count) { fwd_write_ptr++; TurnReadLF_off(EM_START_GAP); - TurnReadLFOn(18*8); + TurnReadLFOn(18 * 8); // now start writting with bitbanging the antenna. - while(fwd_bit_sz-- > 0) { //prepare next bit modulation - if(((*fwd_write_ptr++) & 1) == 1) { - WaitUS(32*8); + while (fwd_bit_sz-- > 0) { //prepare next bit modulation + if (((*fwd_write_ptr++) & 1) == 1) { + WaitUS(32 * 8); } else { - TurnReadLF_off(23*8); - TurnReadLFOn(18*8); + TurnReadLF_off(23 * 8); + TurnReadLFOn(18 * 8); } } } -void EM4xLogin(uint32_t pwd) { +void EM4xLogin(uint32_t pwd) +{ uint8_t len; forward_ptr = forwardLink_data; - len = Prepare_Cmd( FWD_CMD_LOGIN ); - len += Prepare_Data( pwd & 0xFFFF, pwd >> 16 ); + len = Prepare_Cmd(FWD_CMD_LOGIN); + len += Prepare_Data(pwd & 0xFFFF, pwd >> 16); SendForward(len); //WaitUS(20); // no wait for login command. // should receive @@ -2029,7 +2073,8 @@ void EM4xLogin(uint32_t pwd) { // 0000 0001 fail } -void EM4xReadWord(uint8_t addr, uint32_t pwd, uint8_t usepwd) { +void EM4xReadWord(uint8_t addr, uint32_t pwd, uint8_t usepwd) +{ LED_A_ON(); uint8_t len; @@ -2047,8 +2092,8 @@ void EM4xReadWord(uint8_t addr, uint32_t pwd, uint8_t usepwd) { if (usepwd) EM4xLogin(pwd); forward_ptr = forwardLink_data; - len = Prepare_Cmd( FWD_CMD_READ ); - len += Prepare_Addr( addr ); + len = Prepare_Cmd(FWD_CMD_READ); + len += Prepare_Addr(addr); SendForward(len); @@ -2057,11 +2102,12 @@ void EM4xReadWord(uint8_t addr, uint32_t pwd, uint8_t usepwd) { DoPartialAcquisition(20, true, 6000, 1000); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); LED_A_OFF(); } -void EM4xWriteWord(uint32_t flag, uint32_t data, uint32_t pwd) { +void EM4xWriteWord(uint32_t flag, uint32_t data, uint32_t pwd) +{ LED_A_ON(); @@ -2081,9 +2127,9 @@ void EM4xWriteWord(uint32_t flag, uint32_t data, uint32_t pwd) { if (usePwd) EM4xLogin(pwd); forward_ptr = forwardLink_data; - len = Prepare_Cmd( FWD_CMD_WRITE ); - len += Prepare_Addr( addr ); - len += Prepare_Data( data & 0xFFFF, data >> 16 ); + len = Prepare_Cmd(FWD_CMD_WRITE); + len += Prepare_Addr(addr); + len += Prepare_Data(data & 0xFFFF, data >> 16); SendForward(len); @@ -2093,7 +2139,7 @@ void EM4xWriteWord(uint32_t flag, uint32_t data, uint32_t pwd) { DoPartialAcquisition(20, true, 6000, 1000); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); LED_A_OFF(); } @@ -2111,7 +2157,8 @@ pulse 3.6 msecs This triggers a COTAG tag to response */ -void Cotag(uint32_t arg0) { +void Cotag(uint32_t arg0) +{ #ifndef OFF # define OFF(x) { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); WaitUS((x)); } #endif @@ -2133,15 +2180,21 @@ void Cotag(uint32_t arg0) { ON(740) OFF(2035) ON(1000) - switch(rawsignal) { - case 0: doCotagAcquisition(50000); break; - case 1: doCotagAcquisitionManchester(); break; - case 2: DoAcquisition_config(true, 0); break; + switch (rawsignal) { + case 0: + doCotagAcquisition(50000); + break; + case 1: + doCotagAcquisitionManchester(); + break; + case 2: + DoAcquisition_config(true, 0); + break; } // Turn the field off FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); LEDsoff(); } diff --git a/armsrc/lfsampling.c b/armsrc/lfsampling.c index 93ba535b6..086b60d2f 100644 --- a/armsrc/lfsampling.c +++ b/armsrc/lfsampling.c @@ -18,9 +18,10 @@ Default LF config is set to: */ sample_config config = { 1, 8, 1, 95, 0 } ; -void printConfig() { +void printConfig() +{ Dbprintf("LF Sampling config"); - Dbprintf(" [q] divisor.............%d (%d KHz)", config.divisor, 12000 / (config.divisor+1)); + Dbprintf(" [q] divisor.............%d (%d KHz)", config.divisor, 12000 / (config.divisor + 1)); Dbprintf(" [b] bps.................%d", config.bits_per_sample); Dbprintf(" [d] decimation..........%d", config.decimation); Dbprintf(" [a] averaging...........%s", (config.averaging) ? "Yes" : "No"); @@ -38,24 +39,26 @@ void printConfig() { * @brief setSamplingConfig * @param sc */ -void setSamplingConfig(sample_config *sc) { - if(sc->divisor != 0) config.divisor = sc->divisor; - if(sc->bits_per_sample != 0) config.bits_per_sample = sc->bits_per_sample; - if(sc->trigger_threshold != -1) config.trigger_threshold = sc->trigger_threshold; +void setSamplingConfig(sample_config *sc) +{ + if (sc->divisor != 0) config.divisor = sc->divisor; + if (sc->bits_per_sample != 0) config.bits_per_sample = sc->bits_per_sample; + if (sc->trigger_threshold != -1) config.trigger_threshold = sc->trigger_threshold; config.decimation = (sc->decimation != 0) ? sc->decimation : 1; config.averaging = sc->averaging; - if(config.bits_per_sample > 8) config.bits_per_sample = 8; + if (config.bits_per_sample > 8) config.bits_per_sample = 8; printConfig(); } -sample_config* getSamplingConfig() { +sample_config *getSamplingConfig() +{ return &config; } struct BitstreamOut { - uint8_t * buffer; + uint8_t *buffer; uint32_t numbits; uint32_t position; }; @@ -65,10 +68,11 @@ struct BitstreamOut { * @param stream * @param bit */ -void pushBit( BitstreamOut* stream, uint8_t bit) { +void pushBit(BitstreamOut *stream, uint8_t bit) +{ int bytepos = stream->position >> 3; // divide by 8 int bitpos = stream->position & 7; - *(stream->buffer+bytepos) |= (bit > 0) << (7 - bitpos); + *(stream->buffer + bytepos) |= (bit > 0) << (7 - bitpos); stream->position++; stream->numbits++; } @@ -80,9 +84,10 @@ void pushBit( BitstreamOut* stream, uint8_t bit) { * 0 or 95 ==> 125 KHz * **/ -void LFSetupFPGAForADC(int divisor, bool lf_field) { +void LFSetupFPGAForADC(int divisor, bool lf_field) +{ FpgaDownloadAndGo(FPGA_BITSTREAM_LF); - if ( (divisor == 1) || (divisor < 0) || (divisor > 255) ) + if ((divisor == 1) || (divisor < 0) || (divisor > 255)) FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz else if (divisor == 0) FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz @@ -116,7 +121,8 @@ void LFSetupFPGAForADC(int divisor, bool lf_field) { * @param silent - is true, now outputs are made. If false, dbprints the status * @return the number of bits occupied by the samples. */ -uint32_t DoAcquisition(uint8_t decimation, uint32_t bits_per_sample, bool averaging, int trigger_threshold, bool silent, int bufsize, uint32_t cancel_after) { +uint32_t DoAcquisition(uint8_t decimation, uint32_t bits_per_sample, bool averaging, int trigger_threshold, bool silent, int bufsize, uint32_t cancel_after) +{ uint8_t *dest = BigBuf_get_addr(); bufsize = (bufsize > 0 && bufsize < BigBuf_max_traceLen()) ? bufsize : BigBuf_max_traceLen(); @@ -127,17 +133,17 @@ uint32_t DoAcquisition(uint8_t decimation, uint32_t bits_per_sample, bool averag if (decimation < 1) decimation = 1; // use a bit stream to handle the output - BitstreamOut data = { dest , 0, 0}; + BitstreamOut data = { dest, 0, 0}; int sample_counter = 0; uint8_t sample = 0; // if we want to do averaging - uint32_t sample_sum =0 ; + uint32_t sample_sum = 0 ; uint32_t sample_total_numbers = 0; uint32_t sample_total_saved = 0; uint32_t cancel_counter = 0; - while (!BUTTON_PRESS() && !usb_poll_validate_length() ) { + while (!BUTTON_PRESS() && !usb_poll_validate_length()) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { @@ -172,13 +178,13 @@ uint32_t DoAcquisition(uint8_t decimation, uint32_t bits_per_sample, bool averag // averaging if (averaging && decimation > 1) { sample = sample_sum / decimation; - sample_sum =0; + sample_sum = 0; } // store the sample sample_total_saved ++; if (bits_per_sample == 8) { - dest[sample_total_saved-1] = sample; + dest[sample_total_saved - 1] = sample; // Get the return value correct data.numbits = sample_total_saved << 3; @@ -201,7 +207,7 @@ uint32_t DoAcquisition(uint8_t decimation, uint32_t bits_per_sample, bool averag if (!silent) { Dbprintf("Done, saved %d out of %d seen samples at %d bits/sample", sample_total_saved, sample_total_numbers, bits_per_sample); 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]); + dest[0], dest[1], dest[2], dest[3], dest[4], dest[5], dest[6], dest[7]); } // Ensure that DC offset removal and noise check is performed for any device-side processing @@ -218,24 +224,28 @@ uint32_t DoAcquisition(uint8_t decimation, uint32_t bits_per_sample, bool averag * @param silent * @return number of bits sampled */ -uint32_t DoAcquisition_default(int trigger_threshold, bool silent) { - return DoAcquisition(1, 8, 0,trigger_threshold, silent, 0, 0); +uint32_t DoAcquisition_default(int trigger_threshold, bool silent) +{ + return DoAcquisition(1, 8, 0, trigger_threshold, silent, 0, 0); } -uint32_t DoAcquisition_config( bool silent, int sample_size) { +uint32_t DoAcquisition_config(bool silent, int sample_size) +{ return DoAcquisition(config.decimation - ,config.bits_per_sample - ,config.averaging - ,config.trigger_threshold - ,silent - ,sample_size - ,0); + , config.bits_per_sample + , config.averaging + , config.trigger_threshold + , silent + , sample_size + , 0); } -uint32_t DoPartialAcquisition(int trigger_threshold, bool silent, int sample_size, uint32_t cancel_after) { +uint32_t DoPartialAcquisition(int trigger_threshold, bool silent, int sample_size, uint32_t cancel_after) +{ return DoAcquisition(1, 8, 0, trigger_threshold, silent, sample_size, cancel_after); } -uint32_t ReadLF(bool activeField, bool silent, int sample_size) { +uint32_t ReadLF(bool activeField, bool silent, int sample_size) +{ if (!silent) printConfig(); LFSetupFPGAForADC(config.divisor, activeField); @@ -246,7 +256,8 @@ uint32_t ReadLF(bool activeField, bool silent, int sample_size) { * Initializes the FPGA for reader-mode (field on), and acquires the samples. * @return number of bits sampled **/ -uint32_t SampleLF(bool printCfg, int sample_size) { +uint32_t SampleLF(bool printCfg, int sample_size) +{ BigBuf_Clear_ext(false); uint32_t ret = ReadLF(true, printCfg, sample_size); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); @@ -256,7 +267,8 @@ uint32_t SampleLF(bool printCfg, int sample_size) { * Initializes the FPGA for snoop-mode (field off), and acquires the samples. * @return number of bits sampled **/ -uint32_t SnoopLF() { +uint32_t SnoopLF() +{ BigBuf_Clear_ext(false); uint32_t ret = ReadLF(false, true, 0); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); @@ -267,16 +279,17 @@ uint32_t SnoopLF() { * acquisition of T55x7 LF signal. Similar to other LF, but adjusted with @marshmellows thresholds * the data is collected in BigBuf. **/ -void doT55x7Acquisition(size_t sample_size) { +void doT55x7Acquisition(size_t sample_size) +{ - #define T55xx_READ_UPPER_THRESHOLD 128+60 // 60 grph - #define T55xx_READ_LOWER_THRESHOLD 128-60 // -60 grph - #define T55xx_READ_TOL 5 +#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(); uint16_t bufsize = BigBuf_max_traceLen(); - if ( bufsize > sample_size ) + if (bufsize > sample_size) bufsize = sample_size; uint8_t curSample = 0, lastSample = 0; @@ -285,7 +298,7 @@ void doT55x7Acquisition(size_t sample_size) { bool highFound = false; bool lowFound = false; - while(!BUTTON_PRESS() && !usb_poll_validate_length() && skipCnt < 1000 && (i < bufsize) ) { + while (!BUTTON_PRESS() && !usb_poll_validate_length() && skipCnt < 1000 && (i < bufsize)) { WDT_HIT(); @@ -313,7 +326,7 @@ void doT55x7Acquisition(size_t sample_size) { } // skip until first high samples begin to change - if (startFound || curSample > T55xx_READ_LOWER_THRESHOLD + T55xx_READ_TOL){ + if (startFound || curSample > T55xx_READ_LOWER_THRESHOLD + T55xx_READ_TOL) { // if just found start - recover last sample if (!startFound) { dest[i++] = lastSample; @@ -337,12 +350,13 @@ void doT55x7Acquisition(size_t sample_size) { #ifndef COTAG_BITS #define COTAG_BITS 264 #endif -void doCotagAcquisition(size_t sample_size) { +void doCotagAcquisition(size_t sample_size) +{ uint8_t *dest = BigBuf_get_addr(); uint16_t bufsize = BigBuf_max_traceLen(); - if ( bufsize > sample_size ) + if (bufsize > sample_size) bufsize = sample_size; dest[0] = 0; @@ -350,7 +364,7 @@ void doCotagAcquisition(size_t sample_size) { uint16_t i = 0; uint16_t noise_counter = 0; - while (!BUTTON_PRESS() && !usb_poll_validate_length() && (i < bufsize) && (noise_counter < (COTAG_T1 << 1)) ) { + while (!BUTTON_PRESS() && !usb_poll_validate_length() && (i < bufsize) && (noise_counter < (COTAG_T1 << 1))) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { @@ -358,7 +372,7 @@ void doCotagAcquisition(size_t sample_size) { LED_D_OFF(); // find first peak - if ( !firsthigh ) { + if (!firsthigh) { if (sample < COTAG_ONE_THRESHOLD) { noise_counter++; continue; @@ -366,8 +380,8 @@ void doCotagAcquisition(size_t sample_size) { noise_counter = 0; firsthigh = 1; } - if ( !firstlow ){ - if (sample > COTAG_ZERO_THRESHOLD ) { + if (!firstlow) { + if (sample > COTAG_ZERO_THRESHOLD) { noise_counter++; continue; } @@ -377,22 +391,23 @@ void doCotagAcquisition(size_t sample_size) { ++i; - if ( sample > COTAG_ONE_THRESHOLD) + if (sample > COTAG_ONE_THRESHOLD) dest[i] = 255; - else if ( sample < COTAG_ZERO_THRESHOLD) + else if (sample < COTAG_ZERO_THRESHOLD) dest[i] = 0; else - dest[i] = dest[i-1]; + dest[i] = dest[i - 1]; } } } -uint32_t doCotagAcquisitionManchester() { +uint32_t doCotagAcquisitionManchester() +{ uint8_t *dest = BigBuf_get_addr(); uint16_t bufsize = BigBuf_max_traceLen(); - if ( bufsize > COTAG_BITS ) + if (bufsize > COTAG_BITS) bufsize = COTAG_BITS; dest[0] = 0; @@ -401,7 +416,7 @@ uint32_t doCotagAcquisitionManchester() { uint8_t curr = 0, prev = 0; uint16_t noise_counter = 0; - while (!BUTTON_PRESS() && !usb_poll_validate_length() && (sample_counter < bufsize) && (noise_counter < (COTAG_T1 << 1)) ) { + while (!BUTTON_PRESS() && !usb_poll_validate_length() && (sample_counter < bufsize) && (noise_counter < (COTAG_T1 << 1))) { WDT_HIT(); if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) { @@ -409,7 +424,7 @@ uint32_t doCotagAcquisitionManchester() { LED_D_OFF(); // find first peak - if ( !firsthigh ) { + if (!firsthigh) { if (sample < COTAG_ONE_THRESHOLD) { noise_counter++; continue; @@ -418,8 +433,8 @@ uint32_t doCotagAcquisitionManchester() { firsthigh = 1; } - if ( !firstlow ){ - if (sample > COTAG_ZERO_THRESHOLD ) { + if (!firstlow) { + if (sample > COTAG_ZERO_THRESHOLD) { noise_counter++; continue; } @@ -428,20 +443,18 @@ uint32_t doCotagAcquisitionManchester() { } // set sample 255, 0, or previous - if ( sample > COTAG_ONE_THRESHOLD){ + if (sample > COTAG_ONE_THRESHOLD) { prev = curr; curr = 1; - } - else if ( sample < COTAG_ZERO_THRESHOLD) { + } else if (sample < COTAG_ZERO_THRESHOLD) { prev = curr; curr = 0; - } - else { + } else { curr = prev; } // full T1 periods, - if ( period > 0 ) { + if (period > 0) { --period; continue; } diff --git a/armsrc/lfsampling.h b/armsrc/lfsampling.h index 15510198a..94c370ed9 100644 --- a/armsrc/lfsampling.h +++ b/armsrc/lfsampling.h @@ -78,7 +78,7 @@ void LFSetupFPGAForADC(int divisor, bool lf_field); */ void setSamplingConfig(sample_config *sc); -sample_config * getSamplingConfig(); +sample_config *getSamplingConfig(); void printConfig(); diff --git a/armsrc/mifarecmd.c b/armsrc/mifarecmd.c index 4889afce0..c4710d936 100644 --- a/armsrc/mifarecmd.c +++ b/armsrc/mifarecmd.c @@ -26,10 +26,10 @@ // send an incomplete dummy response in order to trigger the card's authentication failure timeout #ifndef CHK_TIMEOUT # define CHK_TIMEOUT() { \ - ReaderTransmit(&dummy_answer, 1, NULL); \ - uint32_t timeout = GetCountSspClk() + HARDNESTED_AUTHENTICATION_TIMEOUT; \ - while (GetCountSspClk() < timeout) {}; \ - } + ReaderTransmit(&dummy_answer, 1, NULL); \ + uint32_t timeout = GetCountSspClk() + HARDNESTED_AUTHENTICATION_TIMEOUT; \ + while (GetCountSspClk() < timeout) {}; \ + } #endif static uint8_t dummy_answer = 0; @@ -40,7 +40,7 @@ static uint8_t dummy_answer = 0; //----------------------------------------------------------------------------- void MifareReadBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) { - // params + // params uint8_t blockNo = arg0; uint8_t keyType = arg1; uint64_t ui64Key = 0; @@ -65,22 +65,22 @@ void MifareReadBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) LED_C_OFF(); while (true) { - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= 1) Dbprintf("Can't select card"); break; }; - if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) { + if (mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) { if (MF_DBGLEVEL >= 1) Dbprintf("Auth error"); break; }; - if(mifare_classic_readblock(pcs, cuid, blockNo, dataoutbuf)) { + if (mifare_classic_readblock(pcs, cuid, blockNo, dataoutbuf)) { if (MF_DBGLEVEL >= 1) Dbprintf("Read block error"); break; }; - if(mifare_classic_halt(pcs, cuid)) { + if (mifare_classic_halt(pcs, cuid)) { if (MF_DBGLEVEL >= 1) Dbprintf("Halt error"); break; }; @@ -94,31 +94,34 @@ void MifareReadBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) if (MF_DBGLEVEL >= 2) DbpString("READ BLOCK FINISHED"); LED_B_ON(); - cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,16); + cmd_send(CMD_ACK, isOK, 0, 0, dataoutbuf, 16); LED_B_OFF(); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); } -void MifareUC_Auth(uint8_t arg0, uint8_t *keybytes){ +void MifareUC_Auth(uint8_t arg0, uint8_t *keybytes) +{ bool turnOffField = (arg0 == 1); - LED_A_ON(); LED_B_OFF(); LED_C_OFF(); + LED_A_ON(); + LED_B_OFF(); + LED_C_OFF(); iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN); clear_trace(); set_tracing(true); - if(!iso14443a_select_card(NULL, NULL, NULL, true, 0, true)) { + if (!iso14443a_select_card(NULL, NULL, NULL, true, 0, true)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Can't select card"); OnError(0); return; }; - if(!mifare_ultra_auth(keybytes)){ + if (!mifare_ultra_auth(keybytes)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Authentication failed"); OnError(1); return; @@ -128,7 +131,7 @@ void MifareUC_Auth(uint8_t arg0, uint8_t *keybytes){ FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); } - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); } // Arg0 = BlockNo, @@ -149,47 +152,47 @@ void MifareUReadBlock(uint8_t arg0, uint8_t arg1, uint8_t *datain) set_tracing(true); int len = iso14443a_select_card(NULL, NULL, NULL, true, 0, true); - if(!len) { - if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Can't select card (RC:%02X)",len); + if (!len) { + if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Can't select card (RC:%02X)", len); OnError(1); return; } // UL-C authentication - if ( useKey ) { + if (useKey) { uint8_t key[16] = {0x00}; - memcpy(key, datain, sizeof(key) ); + memcpy(key, datain, sizeof(key)); - if ( !mifare_ultra_auth(key) ) { + if (!mifare_ultra_auth(key)) { OnError(1); return; } } // UL-EV1 / NTAG authentication - if ( usePwd ) { + if (usePwd) { uint8_t pwd[4] = {0x00}; memcpy(pwd, datain, 4); - uint8_t pack[4] = {0,0,0,0}; + uint8_t pack[4] = {0, 0, 0, 0}; if (!mifare_ul_ev1_auth(pwd, pack)) { OnError(1); return; } } - if( mifare_ultra_readblock(blockNo, dataout) ) { + if (mifare_ultra_readblock(blockNo, dataout)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Read block error"); OnError(2); return; } - if( mifare_ultra_halt() ) { + if (mifare_ultra_halt()) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Halt error"); OnError(3); return; } - cmd_send(CMD_ACK,1,0,0,dataout,16); + cmd_send(CMD_ACK, 1, 0, 0, dataout, 16); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); } @@ -200,7 +203,7 @@ void MifareUReadBlock(uint8_t arg0, uint8_t arg1, uint8_t *datain) //----------------------------------------------------------------------------- void MifareReadSector(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) { - // params + // params uint8_t sectorNo = arg0; uint8_t keyType = arg1; uint64_t ui64Key = 0; @@ -225,26 +228,26 @@ void MifareReadSector(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) LED_C_OFF(); isOK = 1; - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { isOK = 0; if (MF_DBGLEVEL >= 1) Dbprintf("Can't select card"); } - if(isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) { + if (isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) { isOK = 0; if (MF_DBGLEVEL >= 1) Dbprintf("Auth error"); } for (uint8_t blockNo = 0; isOK && blockNo < NumBlocksPerSector(sectorNo); blockNo++) { - if(mifare_classic_readblock(pcs, cuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf + 16 * blockNo)) { + if (mifare_classic_readblock(pcs, cuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf + 16 * blockNo)) { isOK = 0; if (MF_DBGLEVEL >= 1) Dbprintf("Read sector %2d block %2d error", sectorNo, blockNo); break; } } - if(mifare_classic_halt(pcs, cuid)) { + if (mifare_classic_halt(pcs, cuid)) { if (MF_DBGLEVEL >= 1) Dbprintf("Halt error"); } @@ -253,7 +256,7 @@ void MifareReadSector(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) crypto1_destroy(pcs); LED_B_ON(); - cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,16*NumBlocksPerSector(sectorNo)); + cmd_send(CMD_ACK, isOK, 0, 0, dataoutbuf, 16 * NumBlocksPerSector(sectorNo)); LED_B_OFF(); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); @@ -272,7 +275,8 @@ void MifareUReadCard(uint8_t arg0, uint16_t arg1, uint8_t arg2, uint8_t *datain) iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN); // free eventually allocated BigBuf memory - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); @@ -283,7 +287,7 @@ void MifareUReadCard(uint8_t arg0, uint16_t arg1, uint8_t arg2, uint8_t *datain) bool usePwd = (arg2 == 2); //UL_EV1/NTAG uint32_t countblocks = 0; uint8_t *dataout = BigBuf_malloc(CARD_MEMORY_SIZE); - if (dataout == NULL){ + if (dataout == NULL) { Dbprintf("out of memory"); OnError(1); return; @@ -291,17 +295,17 @@ void MifareUReadCard(uint8_t arg0, uint16_t arg1, uint8_t arg2, uint8_t *datain) int len = iso14443a_select_card(NULL, NULL, NULL, true, 0, true); if (!len) { - if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Can't select card (RC:%d)",len); + if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Can't select card (RC:%d)", len); OnError(1); return; } // UL-C authentication - if ( useKey ) { + if (useKey) { uint8_t key[16] = {0x00}; - memcpy(key, datain, sizeof(key) ); + memcpy(key, datain, sizeof(key)); - if ( !mifare_ultra_auth(key) ) { + if (!mifare_ultra_auth(key)) { OnError(1); return; } @@ -311,16 +315,16 @@ void MifareUReadCard(uint8_t arg0, uint16_t arg1, uint8_t arg2, uint8_t *datain) if (usePwd) { uint8_t pwd[4] = {0x00}; memcpy(pwd, datain, sizeof(pwd)); - uint8_t pack[4] = {0,0,0,0}; + uint8_t pack[4] = {0, 0, 0, 0}; - if (!mifare_ul_ev1_auth(pwd, pack)){ + if (!mifare_ul_ev1_auth(pwd, pack)) { OnError(1); return; } } - for (int i = 0; i < blocks; i++){ - if ((i*4) + 4 >= CARD_MEMORY_SIZE) { + for (int i = 0; i < blocks; i++) { + if ((i * 4) + 4 >= CARD_MEMORY_SIZE) { Dbprintf("Data exceeds buffer!!"); break; } @@ -328,7 +332,7 @@ void MifareUReadCard(uint8_t arg0, uint16_t arg1, uint8_t arg2, uint8_t *datain) len = mifare_ultra_readblock(blockNo + i, dataout + 4 * i); if (len) { - if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Read block %d error",i); + if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Read block %d error", i); // if no blocks read - error out if (i == 0) { OnError(2); @@ -393,22 +397,22 @@ void MifareWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) LED_C_OFF(); while (true) { - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= 1) Dbprintf("Can't select card"); break; }; - if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) { + if (mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) { if (MF_DBGLEVEL >= 1) Dbprintf("Auth error"); break; }; - if(mifare_classic_writeblock(pcs, cuid, blockNo, blockdata)) { + if (mifare_classic_writeblock(pcs, cuid, blockNo, blockdata)) { if (MF_DBGLEVEL >= 1) Dbprintf("Write block error"); break; }; - if(mifare_classic_halt(pcs, cuid)) { + if (mifare_classic_halt(pcs, cuid)) { if (MF_DBGLEVEL >= 1) Dbprintf("Halt error"); break; }; @@ -421,7 +425,7 @@ void MifareWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) if (MF_DBGLEVEL >= 2) DbpString("WRITE BLOCK FINISHED"); - cmd_send(CMD_ACK,isOK,0,0,0,0); + cmd_send(CMD_ACK, isOK, 0, 0, 0, 0); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); @@ -498,11 +502,11 @@ void MifareUWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t *datain) }; // UL-C authentication - if ( useKey ) { + if (useKey) { uint8_t key[16] = {0x00}; - memcpy(key, datain+4, sizeof(key) ); + memcpy(key, datain + 4, sizeof(key)); - if ( !mifare_ultra_auth(key) ) { + if (!mifare_ultra_auth(key)) { OnError(1); return; } @@ -511,8 +515,8 @@ void MifareUWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t *datain) // UL-EV1 / NTAG authentication if (usePwd) { uint8_t pwd[4] = {0x00}; - memcpy(pwd, datain+4, 4); - uint8_t pack[4] = {0,0,0,0}; + memcpy(pwd, datain + 4, 4); + uint8_t pack[4] = {0, 0, 0, 0}; if (!mifare_ul_ev1_auth(pwd, pack)) { OnError(1); return; @@ -533,20 +537,23 @@ void MifareUWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t *datain) if (MF_DBGLEVEL >= 2) DbpString("WRITE BLOCK FINISHED"); - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); set_tracing(false); } -void MifareUSetPwd(uint8_t arg0, uint8_t *datain){ +void MifareUSetPwd(uint8_t arg0, uint8_t *datain) +{ uint8_t pwd[16] = {0x00}; byte_t blockdata[4] = {0x00}; memcpy(pwd, datain, 16); - LED_A_ON(); LED_B_OFF(); LED_C_OFF(); + LED_A_ON(); + LED_B_OFF(); + LED_C_OFF(); iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN); clear_trace(); @@ -562,7 +569,7 @@ void MifareUSetPwd(uint8_t arg0, uint8_t *datain){ blockdata[1] = pwd[6]; blockdata[2] = pwd[5]; blockdata[3] = pwd[4]; - if (mifare_ultra_writeblock( 44, blockdata)) { + if (mifare_ultra_writeblock(44, blockdata)) { if (MF_DBGLEVEL >= 1) Dbprintf("Write block error"); OnError(44); return; @@ -572,7 +579,7 @@ void MifareUSetPwd(uint8_t arg0, uint8_t *datain){ blockdata[1] = pwd[2]; blockdata[2] = pwd[1]; blockdata[3] = pwd[0]; - if (mifare_ultra_writeblock( 45, blockdata)) { + if (mifare_ultra_writeblock(45, blockdata)) { if (MF_DBGLEVEL >= 1) Dbprintf("Write block error"); OnError(45); return; @@ -582,7 +589,7 @@ void MifareUSetPwd(uint8_t arg0, uint8_t *datain){ blockdata[1] = pwd[14]; blockdata[2] = pwd[13]; blockdata[3] = pwd[12]; - if (mifare_ultra_writeblock( 46, blockdata)) { + if (mifare_ultra_writeblock(46, blockdata)) { if (MF_DBGLEVEL >= 1) Dbprintf("Write block error"); OnError(46); return; @@ -592,7 +599,7 @@ void MifareUSetPwd(uint8_t arg0, uint8_t *datain){ blockdata[1] = pwd[10]; blockdata[2] = pwd[9]; blockdata[3] = pwd[8]; - if (mifare_ultra_writeblock( 47, blockdata)) { + if (mifare_ultra_writeblock(47, blockdata)) { if (MF_DBGLEVEL >= 1) Dbprintf("Write block error"); OnError(47); return; @@ -604,20 +611,22 @@ void MifareUSetPwd(uint8_t arg0, uint8_t *datain){ return; }; - cmd_send(CMD_ACK,1,0,0,0,0); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); set_tracing(false); } // Return 1 if the nonce is invalid else return 0 -int valid_nonce(uint32_t Nt, uint32_t NtEnc, uint32_t Ks1, uint8_t *parity) { - return ((oddparity8((Nt >> 24) & 0xFF) == ((parity[0]) ^ oddparity8((NtEnc >> 24) & 0xFF) ^ BIT(Ks1,16))) & \ - (oddparity8((Nt >> 16) & 0xFF) == ((parity[1]) ^ oddparity8((NtEnc >> 16) & 0xFF) ^ BIT(Ks1,8))) & \ - (oddparity8((Nt >> 8) & 0xFF) == ((parity[2]) ^ oddparity8((NtEnc >> 8) & 0xFF) ^ BIT(Ks1,0)))) ? 1 : 0; +int valid_nonce(uint32_t Nt, uint32_t NtEnc, uint32_t Ks1, uint8_t *parity) +{ + return ((oddparity8((Nt >> 24) & 0xFF) == ((parity[0]) ^ oddparity8((NtEnc >> 24) & 0xFF) ^ BIT(Ks1, 16))) & \ + (oddparity8((Nt >> 16) & 0xFF) == ((parity[1]) ^ oddparity8((NtEnc >> 16) & 0xFF) ^ BIT(Ks1, 8))) & \ + (oddparity8((Nt >> 8) & 0xFF) == ((parity[2]) ^ oddparity8((NtEnc >> 8) & 0xFF) ^ BIT(Ks1, 0)))) ? 1 : 0; } -void MifareAcquireNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t *datain) { +void MifareAcquireNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t *datain) +{ uint8_t uid[10] = {0x00}; uint8_t answer[MAX_MIFARE_FRAME_SIZE] = {0x00}; @@ -636,7 +645,8 @@ void MifareAcquireNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t * LED_A_ON(); LED_C_OFF(); - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); @@ -645,7 +655,7 @@ void MifareAcquireNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t * LED_C_ON(); - for (uint16_t i = 0; i <= USB_CMD_DATA_SIZE-4; i += 4 ) { + for (uint16_t i = 0; i <= USB_CMD_DATA_SIZE - 4; i += 4) { // Test if the action was cancelled if (BUTTON_PRESS()) { @@ -661,10 +671,17 @@ void MifareAcquireNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t * continue; } switch (card_info.uidlen) { - case 4 : cascade_levels = 1; break; - case 7 : cascade_levels = 2; break; - case 10: cascade_levels = 3; break; - default: break; + case 4 : + cascade_levels = 1; + break; + case 7 : + cascade_levels = 2; + break; + case 10: + cascade_levels = 3; + break; + default: + break; } have_uid = true; } else { // no need for anticollision. We can directly select the card @@ -692,14 +709,14 @@ void MifareAcquireNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t * // Save the tag nonce (nt) buf[i] = answer[0]; - buf[i+1] = answer[1]; - buf[i+2] = answer[2]; - buf[i+3] = answer[3]; + buf[i + 1] = answer[1]; + buf[i + 2] = answer[2]; + buf[i + 3] = answer[3]; } LED_C_OFF(); LED_B_ON(); - cmd_send(CMD_ACK, isOK, cuid, num_nonces-1, buf, sizeof(buf)); + cmd_send(CMD_ACK, isOK, cuid, num_nonces - 1, buf, sizeof(buf)); LED_B_OFF(); if (MF_DBGLEVEL >= 3) DbpString("AcquireNonces finished"); @@ -717,7 +734,8 @@ void MifareAcquireNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t * // Mifare Classic Cards" in Proceedings of the 22nd ACM SIGSAC Conference on // Computer and Communications Security, 2015 //----------------------------------------------------------------------------- -void MifareAcquireEncryptedNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t *datain) { +void MifareAcquireEncryptedNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, uint8_t *datain) +{ struct Crypto1State mpcs = {0, 0}; struct Crypto1State *pcs; @@ -746,7 +764,8 @@ void MifareAcquireEncryptedNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, LED_A_ON(); LED_C_OFF(); - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(false); @@ -755,10 +774,10 @@ void MifareAcquireEncryptedNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, LED_C_ON(); - for (uint16_t i = 0; i <= USB_CMD_DATA_SIZE - 9; ) { + for (uint16_t i = 0; i <= USB_CMD_DATA_SIZE - 9;) { // Test if the action was cancelled - if(BUTTON_PRESS()) { + if (BUTTON_PRESS()) { isOK = 2; field_off = true; break; @@ -766,15 +785,22 @@ void MifareAcquireEncryptedNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, if (!have_uid) { // need a full select cycle to get the uid first iso14a_card_select_t card_info; - if(!iso14443a_select_card(uid, &card_info, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, &card_info, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= 1) Dbprintf("AcquireNonces: Can't select card (ALL)"); continue; } switch (card_info.uidlen) { - case 4 : cascade_levels = 1; break; - case 7 : cascade_levels = 2; break; - case 10: cascade_levels = 3; break; - default: break; + case 4 : + cascade_levels = 1; + break; + case 7 : + cascade_levels = 2; + break; + case 10: + cascade_levels = 3; + break; + default: + break; } have_uid = true; } else { // no need for anticollision. We can directly select the card @@ -806,12 +832,12 @@ void MifareAcquireEncryptedNonces(uint32_t arg0, uint32_t arg1, uint32_t flags, num_nonces++; if (num_nonces % 2) { - memcpy(buf+i, receivedAnswer, 4); + memcpy(buf + i, receivedAnswer, 4); nt_par_enc = par_enc[0] & 0xf0; } else { nt_par_enc |= par_enc[0] >> 4; - memcpy(buf+i+4, receivedAnswer, 4); - memcpy(buf+i+8, &nt_par_enc, 1); + memcpy(buf + i + 4, receivedAnswer, 4); + memcpy(buf + i + 8, &nt_par_enc, 1); i += 9; } } @@ -871,14 +897,15 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN); // free eventually allocated BigBuf memory - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); if (calibrate) clear_trace(); set_tracing(true); // statistics on nonce distance int16_t isOK = 0; - #define NESTED_MAX_TRIES 12 +#define NESTED_MAX_TRIES 12 uint16_t unsuccessfull_tries = 0; if (calibrate) { // for first call only. Otherwise reuse previous calibration LED_B_ON(); @@ -891,33 +918,33 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat for (rtr = 0; rtr < 17; rtr++) { // Test if the action was cancelled - if(BUTTON_PRESS()) { + if (BUTTON_PRESS()) { isOK = -2; break; } // prepare next select. No need to power down the card. - if(mifare_classic_halt(pcs, cuid)) { + if (mifare_classic_halt(pcs, cuid)) { if (MF_DBGLEVEL >= 2) Dbprintf("Nested: Halt error"); rtr--; continue; } - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= 2) Dbprintf("Nested: Can't select card"); rtr--; continue; }; auth1_time = 0; - if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1, &auth1_time)) { + if (mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1, &auth1_time)) { if (MF_DBGLEVEL >= 2) Dbprintf("Nested: Auth1 error"); rtr--; continue; }; auth2_time = (delta_time) ? auth1_time + delta_time : 0; - if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_NESTED, &nt2, &auth2_time)) { + if (mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_NESTED, &nt2, &auth2_time)) { if (MF_DBGLEVEL >= 2) Dbprintf("Nested: Auth2 error"); rtr--; continue; @@ -934,8 +961,7 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat davg += i; dmin = MIN(dmin, i); dmax = MAX(dmax, i); - } - else { + } else { delta_time = auth2_time - auth1_time + 32; // allow some slack for proper timing } if (MF_DBGLEVEL >= 3) Dbprintf("Nested: calibrating... ntdist=%d", i); @@ -947,7 +973,7 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat } } - davg = (davg + (rtr - 1)/2) / (rtr - 1); + davg = (davg + (rtr - 1) / 2) / (rtr - 1); if (MF_DBGLEVEL >= 3) Dbprintf("rtr=%d isOK=%d min=%d max=%d avg=%d, delta_time=%d", rtr, isOK, dmin, dmax, davg, delta_time); @@ -961,24 +987,24 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat LED_C_ON(); // get crypted nonces for target sector - for(i=0; i < 2 && !isOK; i++) { // look for exactly two different nonces + for (i = 0; i < 2 && !isOK; i++) { // look for exactly two different nonces target_nt[i] = 0; - while(target_nt[i] == 0) { // continue until we have an unambiguous nonce + while (target_nt[i] == 0) { // continue until we have an unambiguous nonce // prepare next select. No need to power down the card. - if(mifare_classic_halt(pcs, cuid)) { + if (mifare_classic_halt(pcs, cuid)) { if (MF_DBGLEVEL >= 2) Dbprintf("Nested: Halt error"); continue; } - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= 2) Dbprintf("Nested: Can't select card"); continue; }; auth1_time = 0; - if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1, &auth1_time)) { + if (mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1, &auth1_time)) { if (MF_DBGLEVEL >= 2) Dbprintf("Nested: Auth1 error"); continue; }; @@ -993,11 +1019,11 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat }; nt2 = bytes_to_num(receivedAnswer, 4); - if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: Testing nt1=%08x nt2enc=%08x nt2par=%02x", i+1, nt1, nt2, par[0]); + if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: Testing nt1=%08x nt2enc=%08x nt2par=%02x", i + 1, nt1, nt2, par[0]); // Parity validity check for (j = 0; j < 4; j++) { - par_array[j] = (oddparity8(receivedAnswer[j]) != ((par[0] >> (7-j)) & 0x01)); + par_array[j] = (oddparity8(receivedAnswer[j]) != ((par[0] >> (7 - j)) & 0x01)); } ncount = 0; @@ -1006,9 +1032,9 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat nttest = prng_successor(nttest, 1); ks1 = nt2 ^ nttest; - if (valid_nonce(nttest, nt2, ks1, par_array)){ + if (valid_nonce(nttest, nt2, ks1, par_array)) { if (ncount > 0) { // we are only interested in disambiguous nonces, try again - if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: dismissed (ambigous), ntdist=%d", i+1, j); + if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: dismissed (ambigous), ntdist=%d", i + 1, j); target_nt[i] = 0; break; } @@ -1020,10 +1046,10 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#2: dismissed (= nonce#1), ntdist=%d", j); break; } - if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: valid, ntdist=%d", i+1, j); + if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: valid, ntdist=%d", i + 1, j); } } - if (target_nt[i] == 0 && j == dmax+1 && MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: dismissed (all invalid)", i+1); + if (target_nt[i] == 0 && j == dmax + 1 && MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: dismissed (all invalid)", i + 1); } } @@ -1033,10 +1059,10 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat uint8_t buf[4 + 4 * 4] = {0}; memcpy(buf, &cuid, 4); - memcpy(buf+4, &target_nt[0], 4); - memcpy(buf+8, &target_ks[0], 4); - memcpy(buf+12, &target_nt[1], 4); - memcpy(buf+16, &target_ks[1], 4); + memcpy(buf + 4, &target_nt[0], 4); + memcpy(buf + 8, &target_ks[0], 4); + memcpy(buf + 12, &target_nt[1], 4); + memcpy(buf + 16, &target_ks[1], 4); LED_B_ON(); cmd_send(CMD_ACK, isOK, 0, targetBlockNo + (targetKeyType * 0x100), buf, sizeof(buf)); @@ -1075,9 +1101,10 @@ typedef struct chk_t { // 2 = failed to select. // 1 = wrong key // 0 = correct key -uint8_t chkKey( struct chk_t *c ) { +uint8_t chkKey(struct chk_t *c) +{ uint8_t i = 0, res = 2; - while( i < 5 ) { + while (i < 5) { // this part is from Piwi's faster nonce collecting part in Hardnested. // assume: fast select if (!iso14443a_fast_select_card(c->uid, c->cl)) { @@ -1090,28 +1117,29 @@ uint8_t chkKey( struct chk_t *c ) { // if successfull auth, send HALT // if ( !res ) - // mifare_classic_halt_ex(c->pcs); + // mifare_classic_halt_ex(c->pcs); break; } return res; } -uint8_t chkKey_readb(struct chk_t *c, uint8_t *keyb) { +uint8_t chkKey_readb(struct chk_t *c, uint8_t *keyb) +{ if (!iso14443a_fast_select_card(c->uid, c->cl)) return 2; - if ( mifare_classic_authex(c->pcs, c->cuid, c->block, 0, c->key, AUTH_FIRST, NULL, NULL) ) + if (mifare_classic_authex(c->pcs, c->cuid, c->block, 0, c->key, AUTH_FIRST, NULL, NULL)) return 1; uint8_t data[16] = {0x00}; uint8_t res = mifare_classic_readblock(c->pcs, c->cuid, c->block, data); // successful read - if ( !res ) { + if (!res) { // data was something else than zeros. - if ( memcmp(data+10, "\x00\x00\x00\x00\x00\x00", 6) != 0) { - memcpy(keyb, data+10, 6); + if (memcmp(data + 10, "\x00\x00\x00\x00\x00\x00", 6) != 0) { + memcpy(keyb, data + 10, 6); res = 0; } else { res = 3; @@ -1121,17 +1149,18 @@ uint8_t chkKey_readb(struct chk_t *c, uint8_t *keyb) { return res; } -void chkKey_scanA(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, uint8_t *sectorcnt, uint8_t *foundkeys) { +void chkKey_scanA(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, uint8_t *sectorcnt, uint8_t *foundkeys) +{ for (uint8_t s = 0; s < *sectorcnt; s++) { // skip already found A keys - if ( found[(s*2)] ) + if (found[(s * 2)]) continue; - c->block = FirstBlockOfSector( s ); - if ( chkKey( c ) == 0 ) { + c->block = FirstBlockOfSector(s); + if (chkKey(c) == 0) { num_to_bytes(c->key, 6, k_sector[s].keyA); - found[(s*2)] = 1; + found[(s * 2)] = 1; ++*foundkeys; if (MF_DBGLEVEL >= 3) Dbprintf("ChkKeys_fast: Scan A found (%d)", c->block); @@ -1139,17 +1168,18 @@ void chkKey_scanA(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, ui } } -void chkKey_scanB(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, uint8_t *sectorcnt, uint8_t *foundkeys) { +void chkKey_scanB(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, uint8_t *sectorcnt, uint8_t *foundkeys) +{ for (uint8_t s = 0; s < *sectorcnt; s++) { // skip already found B keys - if ( found[(s*2)+1] ) + if (found[(s * 2) + 1]) continue; - c->block = FirstBlockOfSector( s ); - if ( chkKey( c ) == 0 ) { + c->block = FirstBlockOfSector(s); + if (chkKey(c) == 0) { num_to_bytes(c->key, 6, k_sector[s].keyB); - found[(s*2)+1] = 1; + found[(s * 2) + 1] = 1; ++*foundkeys; if (MF_DBGLEVEL >= 3) Dbprintf("ChkKeys_fast: Scan B found (%d)", c->block); @@ -1159,29 +1189,30 @@ void chkKey_scanB(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, ui // loop all A keys, // when A is found but not B, try to read B. -void chkKey_loopBonly(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, uint8_t *sectorcnt, uint8_t *foundkeys) { +void chkKey_loopBonly(struct chk_t *c, struct sector_t *k_sector, uint8_t *found, uint8_t *sectorcnt, uint8_t *foundkeys) +{ // read Block B, if A is found. for (uint8_t s = 0; s < *sectorcnt; ++s) { - if ( found[(s*2)] && found[(s*2)+1] ) + if (found[(s * 2)] && found[(s * 2) + 1]) continue; - c->block = (FirstBlockOfSector( s ) + NumBlocksPerSector( s ) - 1); + c->block = (FirstBlockOfSector(s) + NumBlocksPerSector(s) - 1); // A but not B - if ( found[(s*2)] && !found[(s*2)+1] ){ + if (found[(s * 2)] && !found[(s * 2) + 1]) { c->key = bytes_to_num(k_sector[s].keyA, 6); uint8_t status = chkKey_readb(c, k_sector[s].keyB); - if ( status == 0 ){ - found[(s*2)+1] = 1; + if (status == 0) { + found[(s * 2) + 1] = 1; ++*foundkeys; if (MF_DBGLEVEL >= 3) Dbprintf("ChkKeys_fast: Reading B found (%d)", c->block); // try quick find all B? // assume: keys comes in groups. Find one B, test against all B. - c->key = bytes_to_num( k_sector[s].keyB, 6); + c->key = bytes_to_num(k_sector[s].keyB, 6); c->keyType = 1; chkKey_scanB(c, k_sector, found, sectorcnt, foundkeys); } @@ -1197,7 +1228,8 @@ void chkKey_loopBonly(struct chk_t *c, struct sector_t *k_sector, uint8_t *found // arg1 = clear trace // arg2 = antal nycklar i keychunk // datain = keys as array -void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) { +void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) +{ // first call or uint8_t sectorcnt = arg0 & 0xFF; // 16; @@ -1223,25 +1255,25 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da static uint8_t *uid; #ifdef WITH_FLASH - if ( use_flashmem ) { + if (use_flashmem) { BigBuf_free(); uint16_t isok = 0; uint8_t size[2] = {0x00, 0x00}; isok = Flash_ReadData(DEFAULT_MF_KEYS_OFFSET, size, 2); - if ( isok != 2 ) + if (isok != 2) goto OUT; keyCount = size[1] << 8 | size[0]; - if ( keyCount == 0 && keyCount == 0xFFFF) + if (keyCount == 0 && keyCount == 0xFFFF) goto OUT; - datain = BigBuf_malloc( keyCount * 6); - if (datain == NULL ) + datain = BigBuf_malloc(keyCount * 6); + if (datain == NULL) goto OUT; - isok = Flash_ReadData(DEFAULT_MF_KEYS_OFFSET+2, datain, keyCount * 6); - if ( isok != keyCount * 6 ) + isok = Flash_ReadData(DEFAULT_MF_KEYS_OFFSET + 2, datain, keyCount * 6); + if (isok != keyCount * 6) goto OUT; } @@ -1249,7 +1281,7 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da if (uid == NULL || firstchunk) { uid = BigBuf_malloc(10); - if (uid == NULL ) + if (uid == NULL) goto OUT; } @@ -1258,25 +1290,32 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da LEDsoff(); LED_A_ON(); - if ( firstchunk ) { + if (firstchunk) { clear_trace(); set_tracing(false); - memset(k_sector, 0x00, 480+10); + memset(k_sector, 0x00, 480 + 10); memset(found, 0x00, sizeof(found)); foundkeys = 0; iso14a_card_select_t card_info; - if ( !iso14443a_select_card(uid, &card_info, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, &card_info, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= 1) Dbprintf("ChkKeys_fast: Can't select card (ALL)"); goto OUT; } switch (card_info.uidlen) { - case 4 : cascade_levels = 1; break; - case 7 : cascade_levels = 2; break; - case 10: cascade_levels = 3; break; - default: break; + case 4 : + cascade_levels = 1; + break; + case 7 : + cascade_levels = 2; + break; + case 10: + cascade_levels = 3; + break; + default: + break; } CHK_TIMEOUT(); @@ -1290,7 +1329,7 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da chk_data.block = 0; // keychunk loop - depth first one sector. - if ( strategy == 1 || use_flashmem) { + if (strategy == 1 || use_flashmem) { uint8_t newfound = foundkeys; @@ -1300,7 +1339,7 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da // keep track of how many sectors on card. for (uint8_t s = 0; s < sectorcnt; ++s) { - if ( found[(s*2)] && found[(s*2)+1] ) + if (found[(s * 2)] && found[(s * 2) + 1]) continue; for (uint16_t i = s_point; i < keyCount; ++i) { @@ -1313,40 +1352,40 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da } // found all keys? - if ( foundkeys == allkeys ) + if (foundkeys == allkeys) goto OUT; WDT_HIT(); // assume: block0,1,2 has more read rights in accessbits than the sectortrailer. authenticating against block0 in each sector - chk_data.block = FirstBlockOfSector( s ); + chk_data.block = FirstBlockOfSector(s); // new key chk_data.key = bytes_to_num(datain + i * 6, 6); // skip already found A keys - if( !found[(s*2)] ) { + if (!found[(s * 2)]) { chk_data.keyType = 0; - status = chkKey( &chk_data); - if ( status == 0 ) { + status = chkKey(&chk_data); + if (status == 0) { memcpy(k_sector[s].keyA, datain + i * 6, 6); - found[(s*2)] = 1; + found[(s * 2)] = 1; ++foundkeys; chkKey_scanA(&chk_data, k_sector, found, §orcnt, &foundkeys); // read Block B, if A is found. - chkKey_loopBonly( &chk_data, k_sector, found, §orcnt, &foundkeys); + chkKey_loopBonly(&chk_data, k_sector, found, §orcnt, &foundkeys); chk_data.keyType = 1; chkKey_scanB(&chk_data, k_sector, found, §orcnt, &foundkeys); chk_data.keyType = 0; - chk_data.block = FirstBlockOfSector( s ); + chk_data.block = FirstBlockOfSector(s); - if ( use_flashmem ) { - if ( lastpos != i && lastpos != 0) { - if ( i - lastpos < 0xF) { + if (use_flashmem) { + if (lastpos != i && lastpos != 0) { + if (i - lastpos < 0xF) { s_point = i & 0xFFF0; } } else { @@ -1357,20 +1396,20 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da } // skip already found B keys - if( !found[(s*2)+1] ) { + if (!found[(s * 2) + 1]) { chk_data.keyType = 1; - status = chkKey( &chk_data); - if ( status == 0 ) { + status = chkKey(&chk_data); + if (status == 0) { memcpy(k_sector[s].keyB, datain + i * 6, 6); - found[(s*2)+1] = 1; + found[(s * 2) + 1] = 1; ++foundkeys; chkKey_scanB(&chk_data, k_sector, found, §orcnt, &foundkeys); - if ( use_flashmem ) { - if ( lastpos != i && lastpos != 0) { + if (use_flashmem) { + if (lastpos != i && lastpos != 0) { - if ( i - lastpos < 0xF) + if (i - lastpos < 0xF) s_point = i & 0xFFF0; } else { lastpos = i; @@ -1379,22 +1418,22 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da } } - if ( found[(s*2)] && found[(s*2)+1] ) + if (found[(s * 2)] && found[(s * 2) + 1]) break; } // end keys test loop - depth first // assume1. if no keys found in first sector, get next keychunk from client - if ( !use_flashmem && (newfound-foundkeys == 0) ) + if (!use_flashmem && (newfound - foundkeys == 0)) goto OUT; } // end loop - sector } // end strategy 1 - if ( foundkeys == allkeys ) + if (foundkeys == allkeys) goto OUT; - if ( strategy == 2 || use_flashmem ) { + if (strategy == 2 || use_flashmem) { // Keychunk loop for (uint16_t i = 0; i < keyCount; i++) { @@ -1403,7 +1442,7 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da if (BUTTON_PRESS() && !usb_poll_validate_length()) break; // found all keys? - if ( foundkeys == allkeys ) + if (foundkeys == allkeys) goto OUT; WDT_HIT(); @@ -1415,40 +1454,40 @@ void MifareChkKeys_fast(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *da // keep track of how many sectors on card. for (uint8_t s = 0; s < sectorcnt; ++s) { - if ( found[(s*2)] && found[(s*2)+1] ) continue; + if (found[(s * 2)] && found[(s * 2) + 1]) continue; // found all keys? - if ( foundkeys == allkeys ) + if (foundkeys == allkeys) goto OUT; // assume: block0,1,2 has more read rights in accessbits than the sectortrailer. authenticating against block0 in each sector - chk_data.block = FirstBlockOfSector( s ); + chk_data.block = FirstBlockOfSector(s); // skip already found A keys - if( !found[(s*2)] ) { + if (!found[(s * 2)]) { chk_data.keyType = 0; - status = chkKey( &chk_data); - if ( status == 0 ) { + status = chkKey(&chk_data); + if (status == 0) { memcpy(k_sector[s].keyA, datain + i * 6, 6); - found[(s*2)] = 1; + found[(s * 2)] = 1; ++foundkeys; - chkKey_scanA( &chk_data, k_sector, found, §orcnt, &foundkeys); + chkKey_scanA(&chk_data, k_sector, found, §orcnt, &foundkeys); // read Block B, if A is found. - chkKey_loopBonly( &chk_data, k_sector, found, §orcnt, &foundkeys); + chkKey_loopBonly(&chk_data, k_sector, found, §orcnt, &foundkeys); - chk_data.block = FirstBlockOfSector( s ); + chk_data.block = FirstBlockOfSector(s); } } // skip already found B keys - if( !found[(s*2)+1] ) { + if (!found[(s * 2) + 1]) { chk_data.keyType = 1; - status = chkKey( &chk_data); - if ( status == 0 ) { + status = chkKey(&chk_data); + if (status == 0) { memcpy(k_sector[s].keyB, datain + i * 6, 6); - found[(s*2)+1] = 1; + found[(s * 2) + 1] = 1; ++foundkeys; chkKey_scanB(&chk_data, k_sector, found, §orcnt, &foundkeys); @@ -1463,7 +1502,7 @@ OUT: crypto1_destroy(pcs); // All keys found, send to client, or last keychunk from client - if (foundkeys == allkeys || lastchunk ) { + if (foundkeys == allkeys || lastchunk) { uint64_t foo = 0; for (uint8_t m = 0; m < 64; m++) { @@ -1472,28 +1511,30 @@ OUT: uint16_t bar = 0; uint8_t j = 0; - for (uint8_t m=64; m < sizeof(found); m++) { + for (uint8_t m = 64; m < sizeof(found); m++) { bar |= ((uint16_t)(found[m] & 1) << j++); } - uint8_t *tmp = BigBuf_malloc(480+10); - memcpy(tmp, k_sector, sectorcnt * sizeof(sector_t) ); - num_to_bytes(foo, 8, tmp+480); + uint8_t *tmp = BigBuf_malloc(480 + 10); + memcpy(tmp, k_sector, sectorcnt * sizeof(sector_t)); + num_to_bytes(foo, 8, tmp + 480); tmp[488] = bar & 0xFF; tmp[489] = bar >> 8 & 0xFF; - cmd_send(CMD_ACK, foundkeys, 0, 0, tmp, 480+10); + cmd_send(CMD_ACK, foundkeys, 0, 0, tmp, 480 + 10); set_tracing(false); FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); } else { // partial/none keys found cmd_send(CMD_ACK, foundkeys, 0, 0, 0, 0); } } -void MifareChkKeys(uint16_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) { +void MifareChkKeys(uint16_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) +{ FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); @@ -1535,10 +1576,17 @@ void MifareChkKeys(uint16_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) { continue; } switch (card_info.uidlen) { - case 4 : cascade_levels = 1; break; - case 7 : cascade_levels = 2; break; - case 10: cascade_levels = 3; break; - default: break; + case 4 : + cascade_levels = 1; + break; + case 7 : + cascade_levels = 2; + break; + case 10: + cascade_levels = 3; + break; + default: + break; } have_uid = true; } else { // no need for anticollision. We can directly select the card @@ -1575,7 +1623,8 @@ void MifareChkKeys(uint16_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain) { // MIFARE commands set debug level // //----------------------------------------------------------------------------- -void MifareSetDbgLvl(uint16_t arg0){ +void MifareSetDbgLvl(uint16_t arg0) +{ MF_DBGLEVEL = arg0; Dbprintf("Debug level: %d", MF_DBGLEVEL); } @@ -1588,24 +1637,27 @@ void MifareSetDbgLvl(uint16_t arg0){ // destroy the Emulator Memory. //----------------------------------------------------------------------------- -void MifareEMemClr(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){ +void MifareEMemClr(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) +{ FpgaDownloadAndGo(FPGA_BITSTREAM_HF); emlClearMem(); } -void MifareEMemSet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){ +void MifareEMemSet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) +{ FpgaDownloadAndGo(FPGA_BITSTREAM_HF); - if (arg2==0) arg2 = 16; // backwards compat... default bytewidth + if (arg2 == 0) arg2 = 16; // backwards compat... default bytewidth emlSetMem_xt(datain, arg0, arg1, arg2); // data, block num, blocks count, block byte width } -void MifareEMemGet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){ +void MifareEMemGet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) +{ FpgaDownloadAndGo(FPGA_BITSTREAM_HF); byte_t buf[USB_CMD_DATA_SIZE] = {0x00}; emlGetMem(buf, arg0, arg1); // data, block num, blocks count (max 4) LED_B_ON(); - cmd_send(CMD_ACK,arg0,arg1,0,buf,USB_CMD_DATA_SIZE); + cmd_send(CMD_ACK, arg0, arg1, 0, buf, USB_CMD_DATA_SIZE); LED_B_OFF(); } @@ -1613,7 +1665,8 @@ void MifareEMemGet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) // Load a card into the emulator memory // //----------------------------------------------------------------------------- -void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){ +void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain) +{ uint8_t numSectors = arg0; uint8_t keyType = arg1; uint64_t ui64Key = 0; @@ -1637,21 +1690,21 @@ void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datai bool isOK = true; - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { isOK = false; if (MF_DBGLEVEL >= 1) Dbprintf("Can't select card"); } for (uint8_t sectorNo = 0; isOK && sectorNo < numSectors; sectorNo++) { ui64Key = emlGetKey(sectorNo, keyType); - if (sectorNo == 0){ - if(isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) { + if (sectorNo == 0) { + if (isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) { isOK = false; if (MF_DBGLEVEL >= 1) Dbprintf("Sector[%2d]. Auth error", sectorNo); break; } } else { - if(isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_NESTED)) { + if (isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_NESTED)) { isOK = false; if (MF_DBGLEVEL >= 1) Dbprintf("Sector[%2d]. Auth nested error", sectorNo); break; @@ -1659,7 +1712,7 @@ void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datai } for (uint8_t blockNo = 0; isOK && blockNo < NumBlocksPerSector(sectorNo); blockNo++) { - if(isOK && mifare_classic_readblock(pcs, cuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf)) { + if (isOK && mifare_classic_readblock(pcs, cuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf)) { isOK = false; if (MF_DBGLEVEL >= 1) Dbprintf("Error reading sector %2d block %2d", sectorNo, blockNo); break; @@ -1677,7 +1730,7 @@ void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datai } - if(mifare_classic_halt(pcs, cuid)) + if (mifare_classic_halt(pcs, cuid)) if (MF_DBGLEVEL >= 1) Dbprintf("Halt error"); @@ -1710,7 +1763,8 @@ uint8_t wupC1[] = { MIFARE_MAGICWUPC1 }; uint8_t wupC2[] = { MIFARE_MAGICWUPC2 }; uint8_t wipeC[] = { MIFARE_MAGICWIPEC }; -void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ +void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain) +{ // params uint8_t workFlags = arg0; @@ -1741,7 +1795,7 @@ void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ while (true) { // read UID and return to client with write if (workFlags & MAGIC_UID) { - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Can't select card"); errormsg = MAGIC_UID; } @@ -1750,16 +1804,16 @@ void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ } // wipe tag, fill it with zeros - if (workFlags & MAGIC_WIPE){ + if (workFlags & MAGIC_WIPE) { ReaderTransmitBitsPar(wupC1, 7, NULL, NULL); - if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("wupC1 error"); errormsg = MAGIC_WIPE; break; } ReaderTransmit(wipeC, sizeof(wipeC), NULL); - if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("wipeC error"); errormsg = MAGIC_WIPE; break; @@ -1771,15 +1825,15 @@ void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ // write block if (workFlags & MAGIC_WUPC) { ReaderTransmitBitsPar(wupC1, 7, NULL, NULL); - if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("wupC1 error"); errormsg = MAGIC_WUPC; break; } - if ( !is1b ) { + if (!is1b) { ReaderTransmit(wupC2, sizeof(wupC2), NULL); - if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { if (MF_DBGLEVEL >= MF_DBG_ALL) Dbprintf("Assuming Magic Gen 1B tag. [wupC2 failed]"); is1b = true; continue; @@ -1811,8 +1865,8 @@ void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ } // end while - if (isOK ) - cmd_send(CMD_ACK,1,0,0,uid,sizeof(uid)); + if (isOK) + cmd_send(CMD_ACK, 1, 0, 0, uid, sizeof(uid)); else OnErrorMagic(errormsg); @@ -1820,7 +1874,8 @@ void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ OnSuccessMagic(); } -void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ +void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain) +{ uint8_t workFlags = arg0; uint8_t blockNo = arg1; @@ -1849,15 +1904,15 @@ void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ while (true) { if (workFlags & MAGIC_WUPC) { ReaderTransmitBitsPar(wupC1, 7, NULL, NULL); - if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("wupC1 error"); errormsg = MAGIC_WUPC; break; } - if ( !is1b ) { + if (!is1b) { ReaderTransmit(wupC2, sizeof(wupC2), NULL); - if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { + if (!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) { if (MF_DBGLEVEL >= MF_DBG_ALL) Dbprintf("Assuming Magic Gen 1B tag. [wupC2 failed]"); is1b = true; continue; @@ -1887,7 +1942,7 @@ void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ memcpy(datain, data, sizeof(data)); } else { if (isOK) - cmd_send(CMD_ACK,1,0,0,data,sizeof(data)); + cmd_send(CMD_ACK, 1, 0, 0, data, sizeof(data)); else OnErrorMagic(errormsg); } @@ -1896,10 +1951,11 @@ void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint8_t *datain){ OnSuccessMagic(); } -void MifareCIdent(){ - #define GEN_1A 1 - #define GEN_1B 2 - #define GEN_2 4 +void MifareCIdent() +{ +#define GEN_1A 1 +#define GEN_1B 2 +#define GEN_2 4 // variables uint8_t isGen = 0; uint8_t rec[1] = {0x00}; @@ -1909,38 +1965,40 @@ void MifareCIdent(){ // Generation 1 test ReaderTransmitBitsPar(wupC1, 7, NULL, NULL); - if(!ReaderReceive(rec, recpar) || (rec[0] != 0x0a)) { + if (!ReaderReceive(rec, recpar) || (rec[0] != 0x0a)) { goto TEST2; }; isGen = GEN_1B; ReaderTransmit(wupC2, sizeof(wupC2), NULL); - if(!ReaderReceive(rec, recpar) || (rec[0] != 0x0a)) { + if (!ReaderReceive(rec, recpar) || (rec[0] != 0x0a)) { goto OUT; }; isGen = GEN_1A; goto OUT; -TEST2:; -/* - // Generation 2 test +TEST2: + ; + /* + // Generation 2 test - // halt previous. - mifare_classic_halt(NULL, 0); + // halt previous. + mifare_classic_halt(NULL, 0); - //select - if (!iso14443a_select_card(NULL, NULL, NULL, true, 0, true)) { - goto OUT; - }; + //select + if (!iso14443a_select_card(NULL, NULL, NULL, true, 0, true)) { + goto OUT; + }; - // MIFARE_CLASSIC_WRITEBLOCK 0xA0 - // ACK 0x0a - uint16_t len = mifare_sendcmd_short(null, 1, 0xA0, 0, rec, recpar, NULL); - if ((len != 1) || (rec[0] != 0x0A)) { - isGen = GEN_2; - }; - */ -OUT:; + // MIFARE_CLASSIC_WRITEBLOCK 0xA0 + // ACK 0x0a + uint16_t len = mifare_sendcmd_short(null, 1, 0xA0, 0, rec, recpar, NULL); + if ((len != 1) || (rec[0] != 0x0A)) { + isGen = GEN_2; + }; + */ +OUT: + ; // removed the if, since some magic tags misbehavies and send an answer to it. mifare_classic_halt_ex(NULL); cmd_send(CMD_ACK, isGen, 0, 0, 0, 0); @@ -1948,18 +2006,21 @@ OUT:; OnSuccessMagic(); } -void OnSuccessMagic(){ +void OnSuccessMagic() +{ FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); LEDsoff(); set_tracing(false); } -void OnErrorMagic(uint8_t reason){ +void OnErrorMagic(uint8_t reason) +{ // ACK, ISOK, reason,0,0,0 - cmd_send(CMD_ACK,0,reason,0,0,0); + cmd_send(CMD_ACK, 0, reason, 0, 0, 0); OnSuccessMagic(); } -void MifareSetMod(uint8_t mod, uint8_t *key) { +void MifareSetMod(uint8_t mod, uint8_t *key) +{ uint64_t ui64Key = bytes_to_num(key, 6); // variables @@ -1982,12 +2043,12 @@ void MifareSetMod(uint8_t mod, uint8_t *key) { LED_C_OFF(); while (true) { - if(!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { + if (!iso14443a_select_card(uid, NULL, &cuid, true, 0, true)) { if (MF_DBGLEVEL >= 1) Dbprintf("Can't select card"); break; } - if(mifare_classic_auth(pcs, cuid, 0, 0, ui64Key, AUTH_FIRST)) { + if (mifare_classic_auth(pcs, cuid, 0, 0, ui64Key, AUTH_FIRST)) { if (MF_DBGLEVEL >= 1) Dbprintf("Auth error"); break; } @@ -1997,7 +2058,7 @@ void MifareSetMod(uint8_t mod, uint8_t *key) { break; } - if(mifare_classic_halt(pcs, cuid)) { + if (mifare_classic_halt(pcs, cuid)) { if (MF_DBGLEVEL >= 1) Dbprintf("Halt error"); break; } @@ -2019,7 +2080,8 @@ void MifareSetMod(uint8_t mod, uint8_t *key) { // // DESFIRE // -void Mifare_DES_Auth1(uint8_t arg0, uint8_t *datain){ +void Mifare_DES_Auth1(uint8_t arg0, uint8_t *datain) +{ byte_t dataout[12] = {0x00}; uint8_t uid[10] = {0x00}; uint32_t cuid = 0; @@ -2029,13 +2091,13 @@ void Mifare_DES_Auth1(uint8_t arg0, uint8_t *datain){ set_tracing(true); int len = iso14443a_select_card(uid, NULL, &cuid, true, 0, false); - if(!len) { + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Can't select card"); OnError(1); return; }; - if(mifare_desfire_des_auth1(cuid, dataout)){ + if (mifare_desfire_des_auth1(cuid, dataout)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Authentication part1: Fail."); OnError(4); return; @@ -2045,7 +2107,8 @@ void Mifare_DES_Auth1(uint8_t arg0, uint8_t *datain){ cmd_send(CMD_ACK, 1, cuid, 0, dataout, sizeof(dataout)); } -void Mifare_DES_Auth2(uint32_t arg0, uint8_t *datain){ +void Mifare_DES_Auth2(uint32_t arg0, uint8_t *datain) +{ uint32_t cuid = arg0; uint8_t key[16] = {0x00}; byte_t dataout[12] = {0x00}; @@ -2055,7 +2118,7 @@ void Mifare_DES_Auth2(uint32_t arg0, uint8_t *datain){ isOK = mifare_desfire_des_auth2(cuid, key, dataout); - if( isOK) { + if (isOK) { if (MF_DBGLEVEL >= MF_DBG_EXTENDED) Dbprintf("Authentication part2: Failed"); OnError(4); return; diff --git a/armsrc/mifaredesfire.c b/armsrc/mifaredesfire.c index e53ac0f59..55dbb4646 100644 --- a/armsrc/mifaredesfire.c +++ b/armsrc/mifaredesfire.c @@ -10,13 +10,14 @@ // the block number for the ISO14443-4 PCB uint8_t pcb_blocknum = 0; // Deselect card by sending a s-block. the crc is precalced for speed -static uint8_t deselect_cmd[] = {0xc2,0xe0,0xb4}; +static uint8_t deselect_cmd[] = {0xc2, 0xe0, 0xb4}; //static uint8_t __msg[MAX_FRAME_SIZE] = { 0x0A, 0x00, 0x00, /* ..., */ 0x00 }; /* PCB CID CMD PAYLOAD */ //static uint8_t __res[MAX_FRAME_SIZE]; -bool InitDesfireCard(){ +bool InitDesfireCard() +{ iso14a_card_select_t card; @@ -33,14 +34,15 @@ bool InitDesfireCard(){ // ARG0 flag enums enum { - NONE = 0x00, - INIT = 0x01, - DISCONNECT = 0x02, - CLEARTRACE = 0x04, - BAR = 0x08, + NONE = 0x00, + INIT = 0x01, + DISCONNECT = 0x02, + CLEARTRACE = 0x04, + BAR = 0x08, } CmdOptions ; -void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain){ +void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain) +{ /* ARG0 contains flags. 0x01 = init card. @@ -50,7 +52,7 @@ void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain){ uint8_t flags = arg0; size_t datalen = arg1; uint8_t resp[RECEIVE_SIZE]; - memset(resp,0,sizeof(resp)); + memset(resp, 0, sizeof(resp)); if (MF_DBGLEVEL >= 4) { Dbprintf(" flags : %02X", flags); @@ -58,11 +60,11 @@ void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain){ print_result(" RX : ", datain, datalen); } - if ( flags & CLEARTRACE ) + if (flags & CLEARTRACE) clear_trace(); - if ( flags & INIT ){ - if ( !InitDesfireCard() ) + if (flags & INIT) { + if (!InitDesfireCard()) return; } @@ -70,7 +72,7 @@ void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain){ if (MF_DBGLEVEL >= 4) print_result("ERR <--: ", resp, len); - if ( !len ) { + if (!len) { OnError(2); return; } @@ -78,13 +80,14 @@ void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain){ // reset the pcb_blocknum, pcb_blocknum = 0; - if ( flags & DISCONNECT ) + if (flags & DISCONNECT) OnSuccess(); - cmd_send(CMD_ACK,1,len,0,resp,len); + cmd_send(CMD_ACK, 1, len, 0, resp, len); } -void MifareDesfireGetInformation(){ +void MifareDesfireGetInformation() +{ int len = 0; iso14a_card_select_t card; @@ -105,13 +108,13 @@ void MifareDesfireGetInformation(){ iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN); // card select - information - if ( !iso14443a_select_card(NULL, &card, NULL, true, 0, false) ) { + if (!iso14443a_select_card(NULL, &card, NULL, true, 0, false)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) DbpString("Can't select card"); OnError(1); return; } - if ( card.uidlen != 7 ) { + if (card.uidlen != 7) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Wrong UID size. Expected 7byte got %d", card.uidlen); OnError(2); return; @@ -127,7 +130,7 @@ void MifareDesfireGetInformation(){ size_t cmd_len = sizeof(cmd); len = DesfireAPDU(cmd, cmd_len, resp); - if ( !len ) { + if (!len) { print_result("ERROR <--: ", resp, len); OnError(3); return; @@ -135,12 +138,12 @@ void MifareDesfireGetInformation(){ LED_A_OFF(); LED_B_ON(); - memcpy(dataout+7,resp+3,7); + memcpy(dataout + 7, resp + 3, 7); // ADDITION_FRAME 1 cmd[0] = ADDITIONAL_FRAME; len = DesfireAPDU(cmd, cmd_len, resp); - if ( !len ) { + if (!len) { print_result("ERROR <--: ", resp, len); OnError(3); return; @@ -148,30 +151,31 @@ void MifareDesfireGetInformation(){ LED_B_OFF(); LED_C_ON(); - memcpy(dataout+7+7,resp+3,7); + memcpy(dataout + 7 + 7, resp + 3, 7); // ADDITION_FRAME 2 len = DesfireAPDU(cmd, cmd_len, resp); - if ( !len ) { + if (!len) { print_result("ERROR <--: ", resp, len); OnError(3); return; } - memcpy(dataout+7+7+7,resp+3,14); + memcpy(dataout + 7 + 7 + 7, resp + 3, 14); - cmd_send(CMD_ACK,1,0,0,dataout,sizeof(dataout)); + cmd_send(CMD_ACK, 1, 0, 0, dataout, sizeof(dataout)); // reset the pcb_blocknum, pcb_blocknum = 0; OnSuccess(); } -void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain){ +void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain) +{ int len = 0; //uint8_t PICC_MASTER_KEY8[8] = { 0x40,0x41,0x42,0x43,0x44,0x45,0x46,0x47}; - uint8_t PICC_MASTER_KEY16[16] = { 0x40,0x41,0x42,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4a,0x4b,0x4c,0x4d,0x4e,0x4f }; + uint8_t PICC_MASTER_KEY16[16] = { 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f }; uint8_t null_key_data8[8] = {0x00}; //uint8_t null_key_data16[16] = {0x00}; //uint8_t new_key_data8[8] = { 0x00,0x11,0x22,0x33,0x44,0x55,0x66,0x77}; @@ -200,25 +204,25 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain // 3 olika kommunikations stt, PLAIN,MAC,CRYPTO // des, nyckel 0, - switch (mode){ - case 1:{ + switch (mode) { + case 1: { uint8_t keybytes[16]; uint8_t RndA[8] = {0x00}; uint8_t RndB[8] = {0x00}; if (algo == 2) { - if (datain[1] == 0xff){ - memcpy(keybytes,PICC_MASTER_KEY16,16); + if (datain[1] == 0xff) { + memcpy(keybytes, PICC_MASTER_KEY16, 16); } else { - memcpy(keybytes, datain+1, datalen); + memcpy(keybytes, datain + 1, datalen); } } else { if (algo == 1) { - if (datain[1] == 0xff){ - memcpy(keybytes,null_key_data8,8); - } else{ - memcpy(keybytes, datain+1, datalen); - } + if (datain[1] == 0xff) { + memcpy(keybytes, null_key_data8, 8); + } else { + memcpy(keybytes, datain + 1, datalen); + } } } @@ -227,13 +231,13 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain if (algo == 2) Desfire_3des_key_new_with_version(keybytes, key); - else if (algo ==1) - Desfire_des_key_new(keybytes, key); + else if (algo == 1) + Desfire_des_key_new(keybytes, key); cmd[0] = AUTHENTICATE; cmd[1] = keyno; //keynumber len = DesfireAPDU(cmd, 2, resp); - if ( !len ) { + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) { DbpString("Authentication failed. Card timeout."); } @@ -241,21 +245,21 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain return; } - if ( resp[2] == 0xaf ){ + if (resp[2] == 0xaf) { } else { DbpString("Authentication failed. Invalid key number."); OnError(3); return; } - memcpy( encRndB, resp+3, 8); + memcpy(encRndB, resp + 3, 8); if (algo == 2) tdes_dec(&decRndB, &encRndB, key->data); else if (algo == 1) - des_dec(&decRndB, &encRndB, key->data); + des_dec(&decRndB, &encRndB, key->data); memcpy(RndB, decRndB, 8); - rol(decRndB,8); + rol(decRndB, 8); // This should be random uint8_t decRndA[8] = {0x00}; @@ -265,7 +269,7 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain if (algo == 2) tdes_dec(&encRndA, &decRndA, key->data); else if (algo == 1) - des_dec(&encRndA, &decRndA, key->data); + des_dec(&encRndA, &decRndA, key->data); memcpy(both, encRndA, 8); @@ -277,15 +281,15 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain if (algo == 2) tdes_dec(&encRndB, &decRndB, key->data); else if (algo == 1) - des_dec(&encRndB, &decRndB, key->data); + des_dec(&encRndB, &decRndB, key->data); memcpy(both + 8, encRndB, 8); cmd[0] = ADDITIONAL_FRAME; - memcpy(cmd+1, both, 16 ); + memcpy(cmd + 1, both, 16); len = DesfireAPDU(cmd, 17, resp); - if ( !len ) { + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) { DbpString("Authentication failed. Card timeout."); } @@ -293,21 +297,21 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain return; } - if ( resp[2] == 0x00 ){ + if (resp[2] == 0x00) { struct desfire_key sessionKey = {0}; desfirekey_t skey = &sessionKey; - Desfire_session_key_new( RndA, RndB , key, skey ); + Desfire_session_key_new(RndA, RndB, key, skey); //print_result("SESSION : ", skey->data, 8); - memcpy(encRndA, resp+3, 8); + memcpy(encRndA, resp + 3, 8); if (algo == 2) tdes_dec(&encRndA, &encRndA, key->data); else if (algo == 1) - des_dec(&encRndA, &encRndA, key->data); + des_dec(&encRndA, &encRndA, key->data); - rol(decRndA,8); + rol(decRndA, 8); for (int x = 0; x < 8; x++) { if (decRndA[x] != encRndA[x]) { DbpString("Authentication failed. Cannot varify PICC."); @@ -407,36 +411,36 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain OnSuccess(); if (algo == 2) - cmd_send(CMD_ACK,1,0,0,skey->data,16); + cmd_send(CMD_ACK, 1, 0, 0, skey->data, 16); else if (algo == 1) - cmd_send(CMD_ACK,1,0,0,skey->data,8); + cmd_send(CMD_ACK, 1, 0, 0, skey->data, 8); } else { DbpString("Authentication failed."); OnError(6); return; } - } - break; + } + break; case 2: //SendDesfireCommand(AUTHENTICATE_ISO, &keyno, resp); break; - case 3:{ + case 3: { //defaultkey uint8_t keybytes[16] = {0x00}; - if (datain[1] == 0xff){ - memcpy(keybytes,PICC_MASTER_KEY16,16); - } else{ - memcpy(keybytes, datain+1, datalen); + if (datain[1] == 0xff) { + memcpy(keybytes, PICC_MASTER_KEY16, 16); + } else { + memcpy(keybytes, datain + 1, datalen); } struct desfire_key defaultkey = {0x00}; desfirekey_t key = &defaultkey; - Desfire_aes_key_new( keybytes, key); + Desfire_aes_key_new(keybytes, key); AesCtx ctx; - if ( AesCtxIni(&ctx, IV, key->data, KEY128, CBC) < 0 ){ - if( MF_DBGLEVEL >= 4) { + if (AesCtxIni(&ctx, IV, key->data, KEY128, CBC) < 0) { + if (MF_DBGLEVEL >= 4) { DbpString("AES context failed to init"); } OnError(7); @@ -446,7 +450,7 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain cmd[0] = AUTHENTICATE_AES; cmd[1] = 0x00; //keynumber len = DesfireAPDU(cmd, 2, resp); - if ( !len ) { + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) { DbpString("Authentication failed. Card timeout."); } @@ -454,20 +458,20 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain return; } - memcpy( encRndB, resp+3, 16); + memcpy(encRndB, resp + 3, 16); // dekryptera tagnonce. AesDecrypt(&ctx, encRndB, decRndB, 16); - rol(decRndB,16); - memcpy(both, nonce,16); - memcpy(both+16, decRndB ,16 ); - AesEncrypt(&ctx, both, encBoth, 32 ); + rol(decRndB, 16); + memcpy(both, nonce, 16); + memcpy(both + 16, decRndB, 16); + AesEncrypt(&ctx, both, encBoth, 32); cmd[0] = ADDITIONAL_FRAME; - memcpy(cmd+1, encBoth, 32 ); + memcpy(cmd + 1, encBoth, 32); len = DesfireAPDU(cmd, 33, resp); // 1 + 32 == 33 - if ( !len ) { + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) { DbpString("Authentication failed. Card timeout."); } @@ -475,11 +479,11 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain return; } - if ( resp[2] == 0x00 ){ + if (resp[2] == 0x00) { // Create AES Session key struct desfire_key sessionKey = {0}; desfirekey_t skey = &sessionKey; - Desfire_session_key_new( nonce, decRndB , key, skey ); + Desfire_session_key_new(nonce, decRndB, key, skey); print_result("SESSION : ", skey->data, 16); } else { DbpString("Authentication failed."); @@ -492,14 +496,15 @@ void MifareDES_Auth1(uint8_t mode, uint8_t algo, uint8_t keyno, uint8_t *datain } OnSuccess(); - cmd_send(CMD_ACK,1,len,0,resp,len); + cmd_send(CMD_ACK, 1, len, 0, resp, len); } // 3 olika ISO stt att skicka data till DESFIRE (direkt, inkapslat, inkapslat ISO) // cmd = cmd bytes to send // cmd_len = length of cmd // dataout = pointer to response data array -int DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout){ +int DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout) +{ size_t len = 0; size_t wrappedLen = 0; @@ -507,15 +512,15 @@ int DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout){ uint8_t resp[MAX_FRAME_SIZE]; uint8_t par[MAX_PARITY_SIZE]; - wrappedLen = CreateAPDU( cmd, cmd_len, wCmd); + wrappedLen = CreateAPDU(cmd, cmd_len, wCmd); if (MF_DBGLEVEL >= 4) print_result("WCMD <--: ", wCmd, wrappedLen); - ReaderTransmit( wCmd, wrappedLen, NULL); + ReaderTransmit(wCmd, wrappedLen, NULL); len = ReaderReceive(resp, par); - if ( !len ) { + if (!len) { if (MF_DBGLEVEL >= 4) Dbprintf("fukked"); return false; //DATA LINK ERROR } @@ -524,8 +529,7 @@ int DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout){ else if (len >= 4 // PCB+CID+CRC = 4 bytes && ((resp[0] & 0xC0) == 0 // I-Block || (resp[0] & 0xD0) == 0x80) // R-Block with ACK bit set to 0 - && (resp[0] & 0x01) == pcb_blocknum) // equal block numbers - { + && (resp[0] & 0x01) == pcb_blocknum) { // equal block numbers pcb_blocknum ^= 1; //toggle next block } @@ -534,9 +538,10 @@ int DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout){ } // CreateAPDU -size_t CreateAPDU( uint8_t *datain, size_t len, uint8_t *dataout){ +size_t CreateAPDU(uint8_t *datain, size_t len, uint8_t *dataout) +{ - size_t cmdlen = MIN(len+4, USB_CMD_DATA_SIZE-1); + size_t cmdlen = MIN(len + 4, USB_CMD_DATA_SIZE - 1); uint8_t cmd[cmdlen]; memset(cmd, 0, cmdlen); @@ -545,27 +550,29 @@ size_t CreateAPDU( uint8_t *datain, size_t len, uint8_t *dataout){ cmd[0] |= pcb_blocknum; // OR the block number into the PCB cmd[1] = 0x00; // CID: 0x00 //TODO: allow multiple selected cards - memcpy(cmd+2, datain, len); - AddCrc14A(cmd, len+2); + memcpy(cmd + 2, datain, len); + AddCrc14A(cmd, len + 2); memcpy(dataout, cmd, cmdlen); return cmdlen; } - // crc_update(&desfire_crc32, 0, 1); /* CMD_WRITE */ - // crc_update(&desfire_crc32, addr, addr_sz); - // crc_update(&desfire_crc32, byte, 8); - // uint32_t crc = crc_finish(&desfire_crc32); +// crc_update(&desfire_crc32, 0, 1); /* CMD_WRITE */ +// crc_update(&desfire_crc32, addr, addr_sz); +// crc_update(&desfire_crc32, byte, 8); +// uint32_t crc = crc_finish(&desfire_crc32); -void OnSuccess(){ +void OnSuccess() +{ pcb_blocknum = 0; - ReaderTransmit(deselect_cmd, 3 , NULL); + ReaderTransmit(deselect_cmd, 3, NULL); mifare_ultra_halt(); switch_off(); } -void OnError(uint8_t reason){ - cmd_send(CMD_ACK,0,reason,0,0,0); +void OnError(uint8_t reason) +{ + cmd_send(CMD_ACK, 0, reason, 0, 0, 0); OnSuccess(); } diff --git a/armsrc/mifaresniff.c b/armsrc/mifaresniff.c index cbdf3a99d..2a32e268e 100644 --- a/armsrc/mifaresniff.c +++ b/armsrc/mifaresniff.c @@ -12,8 +12,8 @@ //static int sniffState = SNF_INIT; static uint8_t sniffUIDType = 0; -static uint8_t sniffUID[10] = {0,0,0,0,0,0,0,0,0,0}; -static uint8_t sniffATQA[2] = {0,0}; +static uint8_t sniffUID[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; +static uint8_t sniffATQA[2] = {0, 0}; static uint8_t sniffSAK = 0; static uint8_t sniffBuf[17]; static uint32_t timerData = 0; @@ -24,7 +24,8 @@ static uint32_t timerData = 0; // if no activity for 2sec, it sends the collected data to the client. //----------------------------------------------------------------------------- // "hf mf sniff" -void RAMFUNC SniffMifare(uint8_t param) { +void RAMFUNC SniffMifare(uint8_t param) +{ // param: // bit 0 - trigger from first card answer // bit 1 - trigger from first reader 7-bit request @@ -35,7 +36,8 @@ void RAMFUNC SniffMifare(uint8_t param) { // Allocate memory from BigBuf for some buffers // free all previous allocations first - BigBuf_free(); BigBuf_Clear_ext(false); + BigBuf_free(); + BigBuf_Clear_ext(false); clear_trace(); set_tracing(true); @@ -71,13 +73,13 @@ void RAMFUNC SniffMifare(uint8_t param) { // Setup and start DMA. // set transfer address and number of bytes. Start transfer. - if ( !FpgaSetupSscDma(dmaBuf, DMA_BUFFER_SIZE) ){ + if (!FpgaSetupSscDma(dmaBuf, DMA_BUFFER_SIZE)) { if (MF_DBGLEVEL > 1) Dbprintf("[!] FpgaSetupSscDma failed. Exiting"); return; } - tUart* uart = GetUart(); - tDemod* demod = GetDemod(); + tUart *uart = GetUart(); + tDemod *demod = GetDemod(); MfSniffInit(); @@ -86,23 +88,23 @@ void RAMFUNC SniffMifare(uint8_t param) { while (!BUTTON_PRESS()) { WDT_HIT(); LED_A_ON(); -/* - if ((sniffCounter & 0x0000FFFF) == 0) { // from time to time - // check if a transaction is completed (timeout after 2000ms). - // if yes, stop the DMA transfer and send what we have so far to the client - if (BigBuf_get_traceLen()) { - MfSniffSend(); - // Reset everything - we missed some sniffed data anyway while the DMA was stopped - sniffCounter = 0; - dmaBuf = BigBuf_malloc(DMA_BUFFER_SIZE); - data = dmaBuf; - maxDataLen = 0; - ReaderIsActive = false; - TagIsActive = false; - FpgaSetupSscDma((uint8_t *)dmaBuf, DMA_BUFFER_SIZE); // set transfer address and number of bytes. Start transfer. - } - } - */ + /* + if ((sniffCounter & 0x0000FFFF) == 0) { // from time to time + // check if a transaction is completed (timeout after 2000ms). + // if yes, stop the DMA transfer and send what we have so far to the client + if (BigBuf_get_traceLen()) { + MfSniffSend(); + // Reset everything - we missed some sniffed data anyway while the DMA was stopped + sniffCounter = 0; + dmaBuf = BigBuf_malloc(DMA_BUFFER_SIZE); + data = dmaBuf; + maxDataLen = 0; + ReaderIsActive = false; + TagIsActive = false; + FpgaSetupSscDma((uint8_t *)dmaBuf, DMA_BUFFER_SIZE); // set transfer address and number of bytes. Start transfer. + } + } + */ // number of bytes we have processed so far int register readBufDataP = data - dmaBuf; @@ -143,7 +145,7 @@ void RAMFUNC SniffMifare(uint8_t param) { // no need to try decoding tag data if the reader is sending if (!TagIsActive) { uint8_t readerbyte = (previous_data & 0xF0) | (*data >> 4); - if (MillerDecoding(readerbyte, (sniffCounter-1)*4)) { + if (MillerDecoding(readerbyte, (sniffCounter - 1) * 4)) { LogTrace(receivedCmd, uart->len, 0, 0, NULL, true); DemodReset(); UartReset(); @@ -154,7 +156,7 @@ void RAMFUNC SniffMifare(uint8_t param) { // no need to try decoding tag data if the reader is sending if (!ReaderIsActive) { uint8_t tagbyte = (previous_data << 4) | (*data & 0x0F); - if (ManchesterDecoding(tagbyte, 0, (sniffCounter-1)*4)) { + if (ManchesterDecoding(tagbyte, 0, (sniffCounter - 1) * 4)) { LogTrace(receivedResp, demod->len, 0, 0, NULL, false); DemodReset(); UartReset(); @@ -175,7 +177,8 @@ void RAMFUNC SniffMifare(uint8_t param) { switch_off(); } -void MfSniffInit(void){ +void MfSniffInit(void) +{ memset(sniffUID, 0x00, sizeof(sniffUID)); memset(sniffATQA, 0x00, sizeof(sniffATQA)); memset(sniffBuf, 0x00, sizeof(sniffBuf)); @@ -184,9 +187,10 @@ void MfSniffInit(void){ timerData = 0; } -void MfSniffEnd(void){ +void MfSniffEnd(void) +{ LED_B_ON(); - cmd_send(CMD_ACK,0,0,0,0,0); + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); LED_B_OFF(); } @@ -304,7 +308,8 @@ bool RAMFUNC MfSniffLogic(const uint8_t *data, uint16_t len, uint8_t *parity, ui } */ -void RAMFUNC MfSniffSend() { +void RAMFUNC MfSniffSend() +{ uint16_t tracelen = BigBuf_get_traceLen(); uint16_t chunksize = 0; int packlen = tracelen; // total number of bytes to send diff --git a/armsrc/mifareutil.c b/armsrc/mifareutil.c index f15d13dec..42e69113d 100644 --- a/armsrc/mifareutil.c +++ b/armsrc/mifareutil.c @@ -13,7 +13,8 @@ int MF_DBGLEVEL = MF_DBG_ERROR; // crypto1 helpers -void mf_crypto1_decryptEx(struct Crypto1State *pcs, uint8_t *data_in, int len, uint8_t *data_out){ +void mf_crypto1_decryptEx(struct Crypto1State *pcs, uint8_t *data_in, int len, uint8_t *data_out) +{ uint8_t bt = 0; int i; @@ -30,11 +31,13 @@ void mf_crypto1_decryptEx(struct Crypto1State *pcs, uint8_t *data_in, int len, u return; } -void mf_crypto1_decrypt(struct Crypto1State *pcs, uint8_t *data, int len){ +void mf_crypto1_decrypt(struct Crypto1State *pcs, uint8_t *data, int len) +{ mf_crypto1_decryptEx(pcs, data, len, data); } -void mf_crypto1_encrypt(struct Crypto1State *pcs, uint8_t *data, uint16_t len, uint8_t *par) { +void mf_crypto1_encrypt(struct Crypto1State *pcs, uint8_t *data, uint16_t len, uint8_t *par) +{ uint8_t bt = 0; int i; par[0] = 0; @@ -42,13 +45,14 @@ void mf_crypto1_encrypt(struct Crypto1State *pcs, uint8_t *data, uint16_t len, u for (i = 0; i < len; i++) { bt = data[i]; data[i] = crypto1_byte(pcs, 0x00, 0) ^ data[i]; - if ( ( i & 0x0007 ) == 0) + if ((i & 0x0007) == 0) par[ i >> 3 ] = 0; - par[ i >> 3 ] |= (((filter(pcs->odd) ^ oddparity8(bt)) & 0x01)<<(7-(i&0x0007))); + par[ i >> 3 ] |= (((filter(pcs->odd) ^ oddparity8(bt)) & 0x01) << (7 - (i & 0x0007))); } } -uint8_t mf_crypto1_encrypt4bit(struct Crypto1State *pcs, uint8_t data) { +uint8_t mf_crypto1_encrypt4bit(struct Crypto1State *pcs, uint8_t data) +{ uint8_t bt = 0; bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data, 0)) << 0; bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data, 1)) << 1; @@ -58,22 +62,24 @@ uint8_t mf_crypto1_encrypt4bit(struct Crypto1State *pcs, uint8_t data) { } // send X byte basic commands -int mifare_sendcmd(uint8_t cmd, uint8_t* data, uint8_t data_size, uint8_t* answer, uint8_t *answer_parity, uint32_t *timing) { - uint8_t dcmd[data_size+3]; +int mifare_sendcmd(uint8_t cmd, uint8_t *data, uint8_t data_size, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) +{ + uint8_t dcmd[data_size + 3]; dcmd[0] = cmd; - memcpy(dcmd+1, data, data_size); - AddCrc14A(dcmd, data_size+1); + memcpy(dcmd + 1, data, data_size); + AddCrc14A(dcmd, data_size + 1); ReaderTransmit(dcmd, sizeof(dcmd), timing); int len = ReaderReceive(answer, answer_parity); - if(!len) { + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("%02X Cmd failed. Card timeout.", cmd); - len = ReaderReceive(answer,answer_parity); + len = ReaderReceive(answer, answer_parity); } return len; } // send 2 byte commands -int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) { +int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) +{ uint16_t pos, res; uint8_t dcmd[4] = {cmd, data, 0x00, 0x00}; uint8_t ecmd[4] = {0x00, 0x00, 0x00, 0x00}; @@ -85,7 +91,7 @@ int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, par[0] = 0; for (pos = 0; pos < 4; pos++) { ecmd[pos] = crypto1_byte(pcs, 0x00, 0) ^ dcmd[pos]; - par[0] |= (((filter(pcs->odd) ^ oddparity8(dcmd[pos])) & 0x01) << (7-pos)); + par[0] |= (((filter(pcs->odd) ^ oddparity8(dcmd[pos])) & 0x01) << (7 - pos)); } ReaderTransmitPar(ecmd, sizeof(ecmd), par, timing); } else { @@ -113,21 +119,23 @@ int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, } // mifare classic commands -int mifare_classic_auth(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested) { +int mifare_classic_auth(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested) +{ return mifare_classic_authex(pcs, uid, blockNo, keyType, ui64Key, isNested, NULL, NULL); } -int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested, uint32_t *ntptr, uint32_t *timing) { +int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested, uint32_t *ntptr, uint32_t *timing) +{ int len; uint32_t pos, nt, ntpp; // Supplied tag nonce uint8_t par[1] = {0x00}; uint8_t nr[4]; - uint8_t mf_nr_ar[] = { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }; + uint8_t mf_nr_ar[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00}; uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00}; // "random" reader nonce: - num_to_bytes( prng_successor( GetTickCount(), 32), 4, nr); + num_to_bytes(prng_successor(GetTickCount(), 32), 4, nr); // Transmit MIFARE_CLASSIC_AUTH len = mifare_sendcmd_short(pcs, isNested, 0x60 + (keyType & 0x01), blockNo, receivedAnswer, receivedAnswerPar, timing); @@ -163,7 +171,7 @@ int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockN par[0] = 0; for (pos = 0; pos < 4; pos++) { mf_nr_ar[pos] = crypto1_byte(pcs, nr[pos], 0) ^ nr[pos]; - par[0] |= (((filter(pcs->odd) ^ oddparity8(nr[pos])) & 0x01) << (7-pos)); + par[0] |= (((filter(pcs->odd) ^ oddparity8(nr[pos])) & 0x01) << (7 - pos)); } // Skip 32 bits in pseudo random generator @@ -171,9 +179,9 @@ int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockN // ar+parity for (pos = 4; pos < 8; pos++) { - nt = prng_successor(nt,8); - mf_nr_ar[pos] = crypto1_byte(pcs,0x00,0) ^ (nt & 0xff); - par[0] |= (((filter(pcs->odd) ^ oddparity8(nt & 0xff)) & 0x01) << (7-pos)); + nt = prng_successor(nt, 8); + mf_nr_ar[pos] = crypto1_byte(pcs, 0x00, 0) ^ (nt & 0xff); + par[0] |= (((filter(pcs->odd) ^ oddparity8(nt & 0xff)) & 0x01) << (7 - pos)); } // Transmit reader nonce and reader answer @@ -186,7 +194,7 @@ int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockN return 2; } - ntpp = prng_successor(nt, 32) ^ crypto1_word(pcs, 0,0); + ntpp = prng_successor(nt, 32) ^ crypto1_word(pcs, 0, 0); if (ntpp != bytes_to_num(receivedAnswer, 4)) { if (MF_DBGLEVEL >= MF_DBG_EXTENDED) Dbprintf("Authentication failed. Error card response."); @@ -195,7 +203,8 @@ int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockN return 0; } -int mifare_classic_readblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) { +int mifare_classic_readblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) +{ int len; uint8_t bt[2] = {0x00, 0x00}; @@ -224,7 +233,8 @@ int mifare_classic_readblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blo } // mifare ultralight commands -int mifare_ul_ev1_auth(uint8_t *keybytes, uint8_t *pack){ +int mifare_ul_ev1_auth(uint8_t *keybytes, uint8_t *pack) +{ uint16_t len = 0; uint8_t resp[4] = {0x00, 0x00, 0x00, 0x00}; @@ -243,16 +253,17 @@ int mifare_ul_ev1_auth(uint8_t *keybytes, uint8_t *pack){ } if (MF_DBGLEVEL >= MF_DBG_EXTENDED) - Dbprintf("Auth Resp: %02x%02x%02x%02x", resp[0],resp[1],resp[2],resp[3]); + Dbprintf("Auth Resp: %02x%02x%02x%02x", resp[0], resp[1], resp[2], resp[3]); memcpy(pack, resp, 4); return 1; } -int mifare_ultra_auth(uint8_t *keybytes){ +int mifare_ultra_auth(uint8_t *keybytes) +{ /// 3des2k - uint8_t random_a[8] = {1,1,1,1,1,1,1,1}; + uint8_t random_a[8] = {1, 1, 1, 1, 1, 1, 1, 1}; uint8_t random_b[8] = {0x00}; uint8_t enc_random_b[8] = {0x00}; uint8_t rnd_ab[16] = {0x00}; @@ -262,36 +273,36 @@ int mifare_ultra_auth(uint8_t *keybytes){ uint16_t len = 0; uint8_t resp[19] = {0x00}; - uint8_t respPar[3] = {0,0,0}; + uint8_t respPar[3] = {0, 0, 0}; // REQUEST AUTHENTICATION - len = mifare_sendcmd_short(NULL, 1, MIFARE_ULC_AUTH_1, 0x00, resp, respPar ,NULL); + len = mifare_sendcmd_short(NULL, 1, MIFARE_ULC_AUTH_1, 0x00, resp, respPar, NULL); if (len != 11) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Cmd Error: %02x", resp[0]); return 0; } // tag nonce. - memcpy(enc_random_b,resp+1,8); + memcpy(enc_random_b, resp + 1, 8); // decrypt nonce. - tdes_2key_dec((void*)random_b, (void*)enc_random_b, sizeof(random_b), (const void*)key, IV ); - rol(random_b,8); - memcpy(rnd_ab ,random_a,8); - memcpy(rnd_ab+8,random_b,8); + tdes_2key_dec((void *)random_b, (void *)enc_random_b, sizeof(random_b), (const void *)key, IV); + rol(random_b, 8); + memcpy(rnd_ab, random_a, 8); + memcpy(rnd_ab + 8, random_b, 8); if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("enc_B: %02x %02x %02x %02x %02x %02x %02x %02x", - enc_random_b[0],enc_random_b[1],enc_random_b[2],enc_random_b[3],enc_random_b[4],enc_random_b[5],enc_random_b[6],enc_random_b[7]); + enc_random_b[0], enc_random_b[1], enc_random_b[2], enc_random_b[3], enc_random_b[4], enc_random_b[5], enc_random_b[6], enc_random_b[7]); Dbprintf(" B: %02x %02x %02x %02x %02x %02x %02x %02x", - random_b[0],random_b[1],random_b[2],random_b[3],random_b[4],random_b[5],random_b[6],random_b[7]); + random_b[0], random_b[1], random_b[2], random_b[3], random_b[4], random_b[5], random_b[6], random_b[7]); Dbprintf("rnd_ab: %02x %02x %02x %02x %02x %02x %02x %02x", - rnd_ab[0],rnd_ab[1],rnd_ab[2],rnd_ab[3],rnd_ab[4],rnd_ab[5],rnd_ab[6],rnd_ab[7]); + rnd_ab[0], rnd_ab[1], rnd_ab[2], rnd_ab[3], rnd_ab[4], rnd_ab[5], rnd_ab[6], rnd_ab[7]); Dbprintf("rnd_ab: %02x %02x %02x %02x %02x %02x %02x %02x", - rnd_ab[8],rnd_ab[9],rnd_ab[10],rnd_ab[11],rnd_ab[12],rnd_ab[13],rnd_ab[14],rnd_ab[15] ); + rnd_ab[8], rnd_ab[9], rnd_ab[10], rnd_ab[11], rnd_ab[12], rnd_ab[13], rnd_ab[14], rnd_ab[15]); } // encrypt out, in, length, key, iv @@ -303,38 +314,39 @@ int mifare_ultra_auth(uint8_t *keybytes){ return 0; } - uint8_t enc_resp[8] = { 0,0,0,0,0,0,0,0 }; - uint8_t resp_random_a[8] = { 0,0,0,0,0,0,0,0 }; - memcpy(enc_resp, resp+1, 8); + uint8_t enc_resp[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + uint8_t resp_random_a[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + memcpy(enc_resp, resp + 1, 8); // decrypt out, in, length, key, iv tdes_2key_dec(resp_random_a, enc_resp, 8, key, enc_random_b); - if ( memcmp(resp_random_a, random_a, 8) != 0 ) { + if (memcmp(resp_random_a, random_a, 8) != 0) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("failed authentication"); return 0; } if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("e_AB: %02x %02x %02x %02x %02x %02x %02x %02x", - rnd_ab[0],rnd_ab[1],rnd_ab[2],rnd_ab[3], - rnd_ab[4],rnd_ab[5],rnd_ab[6],rnd_ab[7]); + rnd_ab[0], rnd_ab[1], rnd_ab[2], rnd_ab[3], + rnd_ab[4], rnd_ab[5], rnd_ab[6], rnd_ab[7]); Dbprintf("e_AB: %02x %02x %02x %02x %02x %02x %02x %02x", - rnd_ab[8],rnd_ab[9],rnd_ab[10],rnd_ab[11], - rnd_ab[12],rnd_ab[13],rnd_ab[14],rnd_ab[15]); + rnd_ab[8], rnd_ab[9], rnd_ab[10], rnd_ab[11], + rnd_ab[12], rnd_ab[13], rnd_ab[14], rnd_ab[15]); Dbprintf("a: %02x %02x %02x %02x %02x %02x %02x %02x", - random_a[0],random_a[1],random_a[2],random_a[3], - random_a[4],random_a[5],random_a[6],random_a[7]); + random_a[0], random_a[1], random_a[2], random_a[3], + random_a[4], random_a[5], random_a[6], random_a[7]); Dbprintf("b: %02x %02x %02x %02x %02x %02x %02x %02x", - resp_random_a[0],resp_random_a[1],resp_random_a[2],resp_random_a[3], - resp_random_a[4],resp_random_a[5],resp_random_a[6],resp_random_a[7]); + resp_random_a[0], resp_random_a[1], resp_random_a[2], resp_random_a[3], + resp_random_a[4], resp_random_a[5], resp_random_a[6], resp_random_a[7]); } return 1; } -int mifare_ultra_readblockEx(uint8_t blockNo, uint8_t *blockData) { +int mifare_ultra_readblockEx(uint8_t blockNo, uint8_t *blockData) +{ uint16_t len = 0; uint8_t bt[2] = {0x00, 0x00}; uint8_t receivedAnswer[MAX_FRAME_SIZE] = {0x00}; @@ -360,15 +372,16 @@ int mifare_ultra_readblockEx(uint8_t blockNo, uint8_t *blockData) { memcpy(blockData, receivedAnswer, 14); return 0; } -int mifare_ultra_readblock(uint8_t blockNo, uint8_t *blockData) { - #define MFU_MAX_RETRIES 5 +int mifare_ultra_readblock(uint8_t blockNo, uint8_t *blockData) +{ +#define MFU_MAX_RETRIES 5 uint8_t res; for (uint8_t retries = 0; retries < MFU_MAX_RETRIES; ++retries) { res = mifare_ultra_readblockEx(blockNo, blockData); // break if OK, or NACK. - switch ( res ) { + switch (res) { case 0: case 1: return res; @@ -379,7 +392,8 @@ int mifare_ultra_readblock(uint8_t blockNo, uint8_t *blockData) { return res; } -int mifare_classic_writeblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) { +int mifare_classic_writeblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) +{ // variables uint16_t len = 0; uint32_t pos = 0; @@ -404,7 +418,7 @@ int mifare_classic_writeblock(struct Crypto1State *pcs, uint32_t uid, uint8_t bl // crypto for (pos = 0; pos < 18; pos++) { d_block_enc[pos] = crypto1_byte(pcs, 0x00, 0) ^ d_block[pos]; - par[pos>>3] |= (((filter(pcs->odd) ^ oddparity8(d_block[pos])) & 0x01) << (7 - (pos&0x0007))); + par[pos >> 3] |= (((filter(pcs->odd) ^ oddparity8(d_block[pos])) & 0x01) << (7 - (pos & 0x0007))); } ReaderTransmitPar(d_block_enc, sizeof(d_block_enc), par, NULL); @@ -457,25 +471,27 @@ int mifare_ultra_writeblock_compat(uint8_t blockNo, uint8_t *blockData) { } */ -int mifare_ultra_writeblock(uint8_t blockNo, uint8_t *blockData) { +int mifare_ultra_writeblock(uint8_t blockNo, uint8_t *blockData) +{ uint16_t len = 0; uint8_t block[5] = {blockNo, 0x00, 0x00, 0x00, 0x00 }; uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00}; uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00}; // command MIFARE_CLASSIC_WRITEBLOCK - memcpy(block+1, blockData, 4); + memcpy(block + 1, blockData, 4); - len = mifare_sendcmd( MIFARE_ULC_WRITE, block, sizeof(block), receivedAnswer, receivedAnswerPar, NULL); + len = mifare_sendcmd(MIFARE_ULC_WRITE, block, sizeof(block), receivedAnswer, receivedAnswerPar, NULL); if (receivedAnswer[0] != 0x0A) { // 0x0a - ACK if (MF_DBGLEVEL >= MF_DBG_ERROR) - Dbprintf("Cmd Send Error: %02x %d", receivedAnswer[0],len); + Dbprintf("Cmd Send Error: %02x %d", receivedAnswer[0], len); return 1; } return 0; } -int mifare_classic_halt_ex(struct Crypto1State *pcs) { +int mifare_classic_halt_ex(struct Crypto1State *pcs) +{ uint8_t receivedAnswer[4] = {0x00, 0x00, 0x00, 0x00}; uint16_t len = mifare_sendcmd_short(pcs, (pcs == NULL) ? CRYPT_NONE : CRYPT_ALL, ISO14443A_CMD_HALT, 0x00, receivedAnswer, NULL, NULL); if (len != 0) { @@ -484,11 +500,13 @@ int mifare_classic_halt_ex(struct Crypto1State *pcs) { } return 0; } -int mifare_classic_halt(struct Crypto1State *pcs, uint32_t uid) { +int mifare_classic_halt(struct Crypto1State *pcs, uint32_t uid) +{ return mifare_classic_halt_ex(pcs); } -int mifare_ultra_halt() { +int mifare_ultra_halt() +{ uint16_t len = 0; uint8_t receivedAnswer[4] = {0x00, 0x00, 0x00, 0x00}; len = mifare_sendcmd_short(NULL, CRYPT_NONE, ISO14443A_CMD_HALT, 0x00, receivedAnswer, NULL, NULL); @@ -502,56 +520,64 @@ int mifare_ultra_halt() { // Mifare Memory Structure: up to 32 Sectors with 4 blocks each (1k and 2k cards), // plus evtl. 8 sectors with 16 blocks each (4k cards) -uint8_t NumBlocksPerSector(uint8_t sectorNo) { +uint8_t NumBlocksPerSector(uint8_t sectorNo) +{ return (sectorNo < 32) ? 4 : 16; } -uint8_t FirstBlockOfSector(uint8_t sectorNo) { +uint8_t FirstBlockOfSector(uint8_t sectorNo) +{ if (sectorNo < 32) return sectorNo * 4; else - return 32*4 + (sectorNo - 32) * 16; + return 32 * 4 + (sectorNo - 32) * 16; } // work with emulator memory -void emlSetMem(uint8_t *data, int blockNum, int blocksCount) { +void emlSetMem(uint8_t *data, int blockNum, int blocksCount) +{ emlSetMem_xt(data, blockNum, blocksCount, 16); } -void emlSetMem_xt(uint8_t *data, int blockNum, int blocksCount, int blockBtWidth) { - uint8_t* emCARD = BigBuf_get_EM_addr(); +void emlSetMem_xt(uint8_t *data, int blockNum, int blocksCount, int blockBtWidth) +{ + uint8_t *emCARD = BigBuf_get_EM_addr(); memcpy(emCARD + blockNum * blockBtWidth, data, blocksCount * blockBtWidth); } -void emlGetMem(uint8_t *data, int blockNum, int blocksCount) { - uint8_t* emCARD = BigBuf_get_EM_addr(); +void emlGetMem(uint8_t *data, int blockNum, int blocksCount) +{ + uint8_t *emCARD = BigBuf_get_EM_addr(); memcpy(data, emCARD + blockNum * 16, blocksCount * 16); } -void emlGetMemBt(uint8_t *data, int bytePtr, int byteCount) { - uint8_t* emCARD = BigBuf_get_EM_addr(); +void emlGetMemBt(uint8_t *data, int bytePtr, int byteCount) +{ + uint8_t *emCARD = BigBuf_get_EM_addr(); memcpy(data, emCARD + bytePtr, byteCount); } -int emlCheckValBl(int blockNum) { - uint8_t* emCARD = BigBuf_get_EM_addr(); - uint8_t* data = emCARD + blockNum * 16; +int emlCheckValBl(int blockNum) +{ + uint8_t *emCARD = BigBuf_get_EM_addr(); + uint8_t *data = emCARD + blockNum * 16; if ((data[0] != (data[4] ^ 0xff)) || (data[0] != data[8]) || - (data[1] != (data[5] ^ 0xff)) || (data[1] != data[9]) || - (data[2] != (data[6] ^ 0xff)) || (data[2] != data[10]) || - (data[3] != (data[7] ^ 0xff)) || (data[3] != data[11]) || - (data[12] != (data[13] ^ 0xff)) || (data[12] != data[14]) || - (data[12] != (data[15] ^ 0xff)) - ) + (data[1] != (data[5] ^ 0xff)) || (data[1] != data[9]) || + (data[2] != (data[6] ^ 0xff)) || (data[2] != data[10]) || + (data[3] != (data[7] ^ 0xff)) || (data[3] != data[11]) || + (data[12] != (data[13] ^ 0xff)) || (data[12] != data[14]) || + (data[12] != (data[15] ^ 0xff)) + ) return 1; return 0; } -int emlGetValBl(uint32_t *blReg, uint8_t *blBlock, int blockNum) { - uint8_t* emCARD = BigBuf_get_EM_addr(); - uint8_t* data = emCARD + blockNum * 16; +int emlGetValBl(uint32_t *blReg, uint8_t *blBlock, int blockNum) +{ + uint8_t *emCARD = BigBuf_get_EM_addr(); + uint8_t *data = emCARD + blockNum * 16; if (emlCheckValBl(blockNum)) return 1; @@ -561,9 +587,10 @@ int emlGetValBl(uint32_t *blReg, uint8_t *blBlock, int blockNum) { return 0; } -int emlSetValBl(uint32_t blReg, uint8_t blBlock, int blockNum) { - uint8_t* emCARD = BigBuf_get_EM_addr(); - uint8_t* data = emCARD + blockNum * 16; +int emlSetValBl(uint32_t blReg, uint8_t blBlock, int blockNum) +{ + uint8_t *emCARD = BigBuf_get_EM_addr(); + uint8_t *data = emCARD + blockNum * 16; memcpy(data + 0, &blReg, 4); memcpy(data + 8, &blReg, 4); @@ -578,21 +605,23 @@ int emlSetValBl(uint32_t blReg, uint8_t blBlock, int blockNum) { return 0; } -uint64_t emlGetKey(int sectorNum, int keyType) { +uint64_t emlGetKey(int sectorNum, int keyType) +{ uint8_t key[6] = {0x00}; - uint8_t* emCARD = BigBuf_get_EM_addr(); + uint8_t *emCARD = BigBuf_get_EM_addr(); memcpy(key, emCARD + 16 * (FirstBlockOfSector(sectorNum) + NumBlocksPerSector(sectorNum) - 1) + keyType * 10, 6); return bytes_to_num(key, 6); } -void emlClearMem(void) { +void emlClearMem(void) +{ const uint8_t trailer[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x07, 0x80, 0x69, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; const uint8_t uid[] = {0xe6, 0x84, 0x87, 0xf3, 0x16, 0x88, 0x04, 0x00, 0x46, 0x8e, 0x45, 0x55, 0x4d, 0x70, 0x41, 0x04}; - uint8_t* emCARD = BigBuf_get_EM_addr(); + uint8_t *emCARD = BigBuf_get_EM_addr(); memset(emCARD, 0, CARD_MEMORY_SIZE); // fill sectors trailer data - for(uint16_t b = 3; b < 256; ((b < 127) ? (b += 4) : (b += 16))) + for (uint16_t b = 3; b < 256; ((b < 127) ? (b += 4) : (b += 16))) emlSetMem((uint8_t *)trailer, b, 1); // uid @@ -602,43 +631,46 @@ void emlClearMem(void) { // Mifare desfire commands -int mifare_sendcmd_special(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t* data, uint8_t* answer, uint8_t *answer_parity, uint32_t *timing) { +int mifare_sendcmd_special(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t *data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) +{ uint8_t dcmd[5] = {cmd, data[0], data[1], 0x00, 0x00}; AddCrc14A(dcmd, 3); ReaderTransmit(dcmd, sizeof(dcmd), NULL); int len = ReaderReceive(answer, answer_parity); - if(!len) { + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Authentication failed. Card timeout."); return 1; } return len; } -int mifare_sendcmd_special2(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t* data, uint8_t* answer,uint8_t *answer_parity, uint32_t *timing) { +int mifare_sendcmd_special2(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t *data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) +{ uint8_t dcmd[20] = {0x00}; dcmd[0] = cmd; - memcpy(dcmd+1,data,17); + memcpy(dcmd + 1, data, 17); AddCrc14A(dcmd, 18); ReaderTransmit(dcmd, sizeof(dcmd), NULL); int len = ReaderReceive(answer, answer_parity); - if(!len){ + if (!len) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Authentication failed. Card timeout."); return 1; } return len; } -int mifare_desfire_des_auth1(uint32_t uid, uint8_t *blockData){ +int mifare_desfire_des_auth1(uint32_t uid, uint8_t *blockData) +{ int len; // load key, keynumber - uint8_t data[2]={MFDES_AUTHENTICATE, 0x00}; + uint8_t data[2] = {MFDES_AUTHENTICATE, 0x00}; uint8_t receivedAnswer[MAX_FRAME_SIZE] = {0x00}; uint8_t receivedAnswerPar[MAX_PARITY_SIZE] = {0x00}; - len = mifare_sendcmd_special(NULL, 1, 0x02, data, receivedAnswer,receivedAnswerPar,NULL); + len = mifare_sendcmd_special(NULL, 1, 0x02, data, receivedAnswer, receivedAnswerPar, NULL); if (len == 1) { if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Cmd Error: %02x", receivedAnswer[0]); @@ -648,26 +680,27 @@ int mifare_desfire_des_auth1(uint32_t uid, uint8_t *blockData){ if (len == 12) { if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("Auth1 Resp: %02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x", - receivedAnswer[0],receivedAnswer[1],receivedAnswer[2],receivedAnswer[3],receivedAnswer[4], - receivedAnswer[5],receivedAnswer[6],receivedAnswer[7],receivedAnswer[8],receivedAnswer[9], - receivedAnswer[10],receivedAnswer[11]); - } - memcpy(blockData, receivedAnswer, 12); - return 0; + receivedAnswer[0], receivedAnswer[1], receivedAnswer[2], receivedAnswer[3], receivedAnswer[4], + receivedAnswer[5], receivedAnswer[6], receivedAnswer[7], receivedAnswer[8], receivedAnswer[9], + receivedAnswer[10], receivedAnswer[11]); + } + memcpy(blockData, receivedAnswer, 12); + return 0; } return 1; } -int mifare_desfire_des_auth2(uint32_t uid, uint8_t *key, uint8_t *blockData){ +int mifare_desfire_des_auth2(uint32_t uid, uint8_t *key, uint8_t *blockData) +{ int len; uint8_t data[17] = {MFDES_AUTHENTICATION_FRAME}; - memcpy(data+1,key,16); + memcpy(data + 1, key, 16); uint8_t receivedAnswer[MAX_FRAME_SIZE] = {0x00}; uint8_t receivedAnswerPar[MAX_PARITY_SIZE] = {0x00}; - len = mifare_sendcmd_special2(NULL, 1, 0x03, data, receivedAnswer, receivedAnswerPar ,NULL); + len = mifare_sendcmd_special2(NULL, 1, 0x03, data, receivedAnswer, receivedAnswerPar, NULL); if ((receivedAnswer[0] == 0x03) && (receivedAnswer[1] == 0xae)) { if (MF_DBGLEVEL >= MF_DBG_ERROR) @@ -675,13 +708,13 @@ int mifare_desfire_des_auth2(uint32_t uid, uint8_t *key, uint8_t *blockData){ return 1; } - if (len == 12){ + if (len == 12) { if (MF_DBGLEVEL >= MF_DBG_EXTENDED) { Dbprintf("Auth2 Resp: %02x%02x%02x%02x%02x%02x%02x%02x%02x%02x", - receivedAnswer[0],receivedAnswer[1],receivedAnswer[2],receivedAnswer[3],receivedAnswer[4], - receivedAnswer[5],receivedAnswer[6],receivedAnswer[7],receivedAnswer[8],receivedAnswer[9], - receivedAnswer[10],receivedAnswer[11]); - } + receivedAnswer[0], receivedAnswer[1], receivedAnswer[2], receivedAnswer[3], receivedAnswer[4], + receivedAnswer[5], receivedAnswer[6], receivedAnswer[7], receivedAnswer[8], receivedAnswer[9], + receivedAnswer[10], receivedAnswer[11]); + } memcpy(blockData, receivedAnswer, 12); return 0; } diff --git a/armsrc/mifareutil.h b/armsrc/mifareutil.h index ad01a7078..971ba77f9 100644 --- a/armsrc/mifareutil.h +++ b/armsrc/mifareutil.h @@ -58,12 +58,12 @@ #define cardSTATE_TO_IDLE() cardSTATE = MFEMUL_IDLE; LED_B_OFF(); LED_C_OFF(); //functions -int mifare_sendcmd(uint8_t cmd, uint8_t *data, uint8_t data_size, uint8_t* answer, uint8_t *answer_parity, uint32_t *timing); -int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t data, uint8_t* answer, uint8_t *answer_parity, uint32_t *timing); +int mifare_sendcmd(uint8_t cmd, uint8_t *data, uint8_t data_size, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing); +int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing); // mifare classic int mifare_classic_auth(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested); -int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested, uint32_t * ntptr, uint32_t *timing); +int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested, uint32_t *ntptr, uint32_t *timing); int mifare_classic_readblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData); int mifare_classic_halt(struct Crypto1State *pcs, uint32_t uid); int mifare_classic_halt_ex(struct Crypto1State *pcs); @@ -78,8 +78,8 @@ int mifare_ultra_writeblock(uint8_t blockNo, uint8_t *blockData); int mifare_ultra_halt(); // desfire -int mifare_sendcmd_special(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t* data, uint8_t* answer, uint8_t *answer_parity, uint32_t *timing); -int mifare_sendcmd_special2(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t* data, uint8_t* answer,uint8_t *answer_parity, uint32_t *timing); +int mifare_sendcmd_special(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t *data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing); +int mifare_sendcmd_special2(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t *data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing); int mifare_desfire_des_auth1(uint32_t uid, uint8_t *blockData); int mifare_desfire_des_auth2(uint32_t uid, uint8_t *key, uint8_t *blockData); diff --git a/armsrc/optimized_cipher.c b/armsrc/optimized_cipher.c index f8ddc19d2..377e6f439 100644 --- a/armsrc/optimized_cipher.c +++ b/armsrc/optimized_cipher.c @@ -96,7 +96,8 @@ uint8_t xopt__select(bool x, bool y, uint8_t r) } */ -void opt_successor(const uint8_t* k, State *s, bool y, State* successor) { +void opt_successor(const uint8_t *k, State *s, bool y, State *successor) +{ uint8_t Tt = 1 & opt_T(s); successor->t = (s->t >> 1); @@ -105,11 +106,12 @@ void opt_successor(const uint8_t* k, State *s, bool y, State* successor) { successor->b = s->b >> 1; successor->b |= (opt_B(s) ^ (s->r & 0x1)) << 7; - successor->r = (k[opt__select(Tt,y,s->r)] ^ successor->b) + s->l ; - successor->l = successor->r+s->r; + successor->r = (k[opt__select(Tt, y, s->r)] ^ successor->b) + s->l ; + successor->l = successor->r + s->r; } -void opt_suc(const uint8_t* k,State* s, uint8_t *in, uint8_t length, bool add32Zeroes) { +void opt_suc(const uint8_t *k, State *s, uint8_t *in, uint8_t length, bool add32Zeroes) +{ State x2; int i; uint8_t head = 0; @@ -148,12 +150,13 @@ void opt_suc(const uint8_t* k,State* s, uint8_t *in, uint8_t length, bool add32Z } } -void opt_output(const uint8_t* k,State* s, uint8_t *buffer) { +void opt_output(const uint8_t *k, State *s, uint8_t *buffer) +{ uint8_t times = 0; uint8_t bout = 0; - State temp = {0,0,0,0}; - for ( ; times < 4; times++) { - bout =0; + State temp = {0, 0, 0, 0}; + for (; times < 4; times++) { + bout = 0; bout |= (s->r & 0x4) << 5; opt_successor(k, s, 0, &temp); bout |= (temp.r & 0x4) << 4; @@ -174,54 +177,59 @@ void opt_output(const uint8_t* k,State* s, uint8_t *buffer) { } } -void opt_MAC(uint8_t* k, uint8_t* input, uint8_t* out) { +void opt_MAC(uint8_t *k, uint8_t *input, uint8_t *out) +{ State _init = { - ((k[0] ^ 0x4c) + 0xEC) & 0xFF,// l - ((k[0] ^ 0x4c) + 0x21) & 0xFF,// r - 0x4c, // b - 0xE012 // t - }; + ((k[0] ^ 0x4c) + 0xEC) & 0xFF,// l + ((k[0] ^ 0x4c) + 0x21) & 0xFF,// r + 0x4c, // b + 0xE012 // t + }; - opt_suc(k,&_init,input,12, false); - opt_output(k,&_init, out); + opt_suc(k, &_init, input, 12, false); + opt_output(k, &_init, out); } -uint8_t rev_byte(uint8_t b) { +uint8_t rev_byte(uint8_t b) +{ b = (b & 0xF0) >> 4 | (b & 0x0F) << 4; b = (b & 0xCC) >> 2 | (b & 0x33) << 2; b = (b & 0xAA) >> 1 | (b & 0x55) << 1; - return b; + return b; } -void opt_reverse_arraybytecpy(uint8_t* dest, uint8_t *src, size_t len) { +void opt_reverse_arraybytecpy(uint8_t *dest, uint8_t *src, size_t len) +{ uint8_t i; - for ( i =0; i< len ; i++) + for (i = 0; i < len ; i++) dest[i] = rev_byte(src[i]); } -void opt_doReaderMAC(uint8_t *cc_nr_p, uint8_t *div_key_p, uint8_t mac[4]) { +void opt_doReaderMAC(uint8_t *cc_nr_p, uint8_t *div_key_p, uint8_t mac[4]) +{ static uint8_t cc_nr[12]; opt_reverse_arraybytecpy(cc_nr, cc_nr_p, 12); - uint8_t dest [] = {0,0,0,0,0,0,0,0}; + uint8_t dest [] = {0, 0, 0, 0, 0, 0, 0, 0}; opt_MAC(div_key_p, cc_nr, dest); //The output MAC must also be reversed opt_reverse_arraybytecpy(mac, dest, 4); return; } -void opt_doTagMAC(uint8_t *cc_p, const uint8_t *div_key_p, uint8_t mac[4]) { - static uint8_t cc_nr[8+4+4]; +void opt_doTagMAC(uint8_t *cc_p, const uint8_t *div_key_p, uint8_t mac[4]) +{ + static uint8_t cc_nr[8 + 4 + 4]; opt_reverse_arraybytecpy(cc_nr, cc_p, 12); State _init = { - ((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l - ((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r - 0x4c, // b - 0xE012 // t - }; + ((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l + ((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r + 0x4c, // b + 0xE012 // t + }; opt_suc(div_key_p, &_init, cc_nr, 12, true); - uint8_t dest [] = {0,0,0,0}; + uint8_t dest [] = {0, 0, 0, 0}; opt_output(div_key_p, &_init, dest); //The output MAC must also be reversed - opt_reverse_arraybytecpy(mac, dest,4); + opt_reverse_arraybytecpy(mac, dest, 4); return; } @@ -233,15 +241,16 @@ void opt_doTagMAC(uint8_t *cc_p, const uint8_t *div_key_p, uint8_t mac[4]) { * @param div_key_p * @return the cipher state */ -State opt_doTagMAC_1(uint8_t *cc_p, const uint8_t *div_key_p) { +State opt_doTagMAC_1(uint8_t *cc_p, const uint8_t *div_key_p) +{ static uint8_t cc_nr[8]; opt_reverse_arraybytecpy(cc_nr, cc_p, 8); State _init = { - ((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l - ((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r - 0x4c, // b - 0xE012 // t - }; + ((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l + ((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r + 0x4c, // b + 0xE012 // t + }; opt_suc(div_key_p, &_init, cc_nr, 8, false); return _init; } @@ -254,14 +263,15 @@ State opt_doTagMAC_1(uint8_t *cc_p, const uint8_t *div_key_p) { * @param mac - where to store the MAC * @param div_key_p - the key to use */ -void opt_doTagMAC_2(State _init, uint8_t* nr, uint8_t mac[4], const uint8_t* div_key_p) { +void opt_doTagMAC_2(State _init, uint8_t *nr, uint8_t mac[4], const uint8_t *div_key_p) +{ static uint8_t _nr[4]; opt_reverse_arraybytecpy(_nr, nr, 4); - opt_suc(div_key_p, &_init,_nr, 4, true); + opt_suc(div_key_p, &_init, _nr, 4, true); - uint8_t dest [] = {0,0,0,0}; + uint8_t dest [] = {0, 0, 0, 0}; opt_output(div_key_p, &_init, dest); //The output MAC must also be reversed - opt_reverse_arraybytecpy(mac, dest,4); + opt_reverse_arraybytecpy(mac, dest, 4); return; } diff --git a/armsrc/optimized_cipher.h b/armsrc/optimized_cipher.h index ad4649d3f..9bf0516de 100644 --- a/armsrc/optimized_cipher.h +++ b/armsrc/optimized_cipher.h @@ -46,6 +46,6 @@ State opt_doTagMAC_1(uint8_t *cc_p, const uint8_t *div_key_p); * @param mac - where to store the MAC * @param div_key_p - the key to use */ -void opt_doTagMAC_2(State _init, uint8_t* nr, uint8_t mac[4], const uint8_t* div_key_p); +void opt_doTagMAC_2(State _init, uint8_t *nr, uint8_t mac[4], const uint8_t *div_key_p); #endif // OPTIMIZED_CIPHER_H diff --git a/armsrc/pcf7931.c b/armsrc/pcf7931.c index be56bc37f..568083d7d 100644 --- a/armsrc/pcf7931.c +++ b/armsrc/pcf7931.c @@ -3,13 +3,14 @@ #define T0_PCF 8 //period for the pcf7931 in us #define ALLOC 16 -size_t DemodPCF7931(uint8_t **outBlocks) { +size_t DemodPCF7931(uint8_t **outBlocks) +{ uint8_t bits[256] = {0x00}; uint8_t blocks[8][16]; 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; @@ -18,7 +19,7 @@ size_t DemodPCF7931(uint8_t **outBlocks) { int pmc, block_done; int lc, warnings = 0; size_t num_blocks = 0; - int lmin=128, lmax=128; + int lmin = 128, lmax = 128; uint8_t dir; //clear read buffer BigBuf_Clear_keep_EM(); @@ -32,16 +33,16 @@ size_t DemodPCF7931(uint8_t **outBlocks) { i = 2; /* Find first local max/min */ - if(dest[1] > dest[0]) { - while(i < GraphTraceLen) { - if( !(dest[i] > dest[i-1]) && dest[i] > lmax) + if (dest[1] > dest[0]) { + while (i < GraphTraceLen) { + if (!(dest[i] > dest[i - 1]) && dest[i] > lmax) break; i++; } dir = 0; } else { - while(i < GraphTraceLen) { - if( !(dest[i] < dest[i-1]) && dest[i] < lmin) + while (i < GraphTraceLen) { + if (!(dest[i] < dest[i - 1]) && dest[i] < lmin) break; i++; } @@ -54,38 +55,37 @@ size_t DemodPCF7931(uint8_t **outBlocks) { 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)) { + 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; // Switch depending on lc length: // Tolerance is 1/8 of clock rate (arbitrary) - if (ABS(lc-clock/4) < tolerance) { + if (ABS(lc - clock / 4) < tolerance) { // 16T0 - if((i - pmc) == lc) { /* 16T0 was previous one */ + if ((i - pmc) == lc) { /* 16T0 was previous one */ /* It's a PMC ! */ - i += (128+127+16+32+33+16)-1; + i += (128 + 127 + 16 + 32 + 33 + 16) - 1; lastval = i; pmc = 0; block_done = 1; } else { pmc = i; } - } else if (ABS(lc-clock/2) < tolerance) { + } else if (ABS(lc - clock / 2) < tolerance) { // 32TO - if((i - pmc) == lc) { /* 16T0 was previous one */ + if ((i - pmc) == lc) { /* 16T0 was previous one */ /* It's a PMC ! */ - i += (128+127+16+32+33)-1; + i += (128 + 127 + 16 + 32 + 33) - 1; lastval = i; pmc = 0; block_done = 1; - } else if(half_switch == 1) { + } else if (half_switch == 1) { bits[bitidx++] = 0; half_switch = 0; - } - else + } else half_switch++; - } else if (ABS(lc-clock) < tolerance) { + } else if (ABS(lc - clock) < tolerance) { // 64TO bits[bitidx++] = 1; } else { @@ -96,19 +96,19 @@ size_t DemodPCF7931(uint8_t **outBlocks) { } } - if(block_done == 1) { - if(bitidx == 128) { - for(j = 0; j < 16; ++j) { + 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] - ; + 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++; } @@ -116,44 +116,44 @@ size_t DemodPCF7931(uint8_t **outBlocks) { block_done = 0; half_switch = 0; } - if(i < GraphTraceLen) - dir =(dest[i-1] > dest[i]) ? 0 : 1; + if (i < GraphTraceLen) + dir = (dest[i - 1] > dest[i]) ? 0 : 1; } - if(bitidx==255) - bitidx=0; + if (bitidx == 255) + bitidx = 0; warnings = 0; - if(num_blocks == 4) break; + if (num_blocks == 4) break; } - memcpy(outBlocks, blocks, 16*num_blocks); + memcpy(outBlocks, blocks, 16 * num_blocks); return num_blocks; } -bool 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)) + 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 true; - } - else if (block[7] == 0x00) - { - if (!memcmp(block+9, "\x00\x00\x00\x00\x00\x00\x00", 7)) + } else if (block[7] == 0x00) { + if (!memcmp(block + 9, "\x00\x00\x00\x00\x00\x00\x00", 7)) return true; } return false; } -bool 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) + if ((block[14] & 0x7f) <= 9 && block[15] <= 9) return true; return false; } -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 @@ -168,17 +168,17 @@ void ReadPCF7931() { int errors = 0; // error counter int tries = 0; // tries counter - memset(memory_blocks, 0, 8*17*sizeof(uint8_t)); - memset(single_blocks, 0, 8*17*sizeof(uint8_t)); + memset(memory_blocks, 0, 8 * 17 * sizeof(uint8_t)); + memset(single_blocks, 0, 8 * 17 * sizeof(uint8_t)); int i = 0, j = 0; do { i = 0; - memset(tmp_blocks, 0, 4*16*sizeof(uint8_t)); - n = DemodPCF7931((uint8_t**)tmp_blocks); - if(!n) + memset(tmp_blocks, 0, 4 * 16 * sizeof(uint8_t)); + n = DemodPCF7931((uint8_t **)tmp_blocks); + if (!n) ++errors; // exit if no block is received @@ -187,7 +187,7 @@ void ReadPCF7931() { return; } // exit if too many errors during reading - if (tries > 50 && (2*errors > tries)) { + if (tries > 50 && (2 * errors > tries)) { Dbprintf("Error reading the tag"); Dbprintf("Here is the partial content"); goto end; @@ -218,12 +218,12 @@ void ReadPCF7931() { 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) { + if (!found_0_1) { while (i < n - 1) { - if (IsBlock0PCF7931(tmp_blocks[i]) && IsBlock1PCF7931(tmp_blocks[i+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); + memcpy(memory_blocks[1], tmp_blocks[i + 1], 16); memory_blocks[0][ALLOC] = memory_blocks[1][ALLOC] = 1; // block 1 tells how many blocks are going to be sent max_blocks = MAX((memory_blocks[1][14] & 0x7f), memory_blocks[1][15]) + 1; @@ -244,26 +244,26 @@ void ReadPCF7931() { } 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]) { - memcpy(memory_blocks[j+1], tmp_blocks[i+1], 16); - memory_blocks[j+1][ALLOC] = 1; + 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)) { + 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 (!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 { - memcpy(memory_blocks[j-1], tmp_blocks[i], 16); - memory_blocks[j-1][ALLOC] = 1; + memcpy(memory_blocks[j - 1], tmp_blocks[i], 16); + memory_blocks[j - 1][ALLOC] = 1; } if (++found_blocks >= max_blocks) goto end; } @@ -277,10 +277,9 @@ void ReadPCF7931() { Dbprintf("Button pressed, stopping."); goto end; } - } - while (found_blocks != max_blocks); + } while (found_blocks != max_blocks); - end: +end: Dbprintf("-----------------------------------------"); Dbprintf("Memory content:"); Dbprintf("-----------------------------------------"); @@ -301,10 +300,11 @@ void ReadPCF7931() { Dbprintf("-----------------------------------------"); } - cmd_send(CMD_ACK,0,0,0,0,0); + 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; @@ -312,8 +312,8 @@ static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int3 //BUILD OF THE DATA FRAME //alimentation of the tag (time for initializing) - AddPatternPCF7931(init_delay, 0, 8192/2*T0_PCF, tab); - AddPatternPCF7931(8192/2*T0_PCF + 319*T0_PCF+70, 3*T0_PCF, 29*T0_PCF, tab); + AddPatternPCF7931(init_delay, 0, 8192 / 2 * T0_PCF, tab); + AddPatternPCF7931(8192 / 2 * T0_PCF + 319 * T0_PCF + 70, 3 * T0_PCF, 29 * T0_PCF, tab); //password indication bit AddBitPCF7931(1, tab, l, p); //password (on 56 bits) @@ -329,35 +329,31 @@ static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int3 //block adress on 6 bits for (u = 0; u < 6; ++u) { - if (address&(1< 0xFFFF){ - tab[u] -= 0xFFFF; - comp = 0; + if (tab[u] > 0xFFFF) { + tab[u] -= 0xFFFF; + comp = 0; } - } + } SendCmdPCF7931(tab); } @@ -390,7 +386,8 @@ static void RealWritePCF7931(uint8_t *pass, uint16_t init_delay, int32_t l, int3 @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); @@ -400,7 +397,7 @@ void WritePCF7931(uint8_t pass1, uint8_t pass2, uint8_t pass3, uint8_t pass4, ui uint8_t password[7] = {pass1, pass2, pass3, pass4, pass5, pass6, pass7}; - RealWritePCF7931 (password, init_delay, l, p, address, byte, data); + RealWritePCF7931(password, init_delay, l, p, address, byte, data); } @@ -408,14 +405,15 @@ 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){ - uint16_t u=0, tempo=0; +void SendCmdPCF7931(uint32_t *tab) +{ + uint16_t u = 0, tempo = 0; Dbprintf("Sending data frame..."); FpgaDownloadAndGo(FPGA_BITSTREAM_LF); FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz - FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU ); + FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU); LED_A_ON(); @@ -432,7 +430,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]) @@ -440,12 +438,12 @@ void SendCmdPCF7931(uint32_t * tab){ // stop modulating antenna LOW(GPIO_SSC_DOUT); - while (tempo != tab[u+1]) + while (tempo != tab[u + 1]) tempo = AT91C_BASE_TC0->TC_CV; // modulate antenna HIGH(GPIO_SSC_DOUT); - while (tempo != tab[u+2]) + while (tempo != tab[u + 2]) tempo = AT91C_BASE_TC0->TC_CV; } @@ -464,13 +462,14 @@ 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 << u)) { //bit is 1 - if( AddBitPCF7931(1, tab, l, p)==1) return 1; + if (AddBitPCF7931(1, tab, l, p) == 1) return 1; } else { //bit is 0 - if (AddBitPCF7931(0, tab, l, p)==1) return 1; + if (AddBitPCF7931(0, tab, l, p) == 1) return 1; } } @@ -483,30 +482,31 @@ bool AddBytePCF7931(uint8_t byte, uint32_t * tab, int32_t l, int32_t p){ * @param l : offset on low pulse width * @param p : offset on low pulse positioning */ -bool AddBitPCF7931(bool b, uint32_t * tab, int32_t l, int32_t p){ +bool AddBitPCF7931(bool b, uint32_t *tab, int32_t l, int32_t p) +{ uint8_t u = 0; //we put the cursor at the last value of the array - for ( u = 0; tab[u] != 0; u += 3 ) { } + for (u = 0; tab[u] != 0; u += 3) { } - if ( b == 1 ) { //add a bit 1 - if ( u == 0 ) + if (b == 1) { //add a bit 1 + if (u == 0) tab[u] = 34 * T0_PCF + p; else - tab[u] = 34 * T0_PCF + tab[u-1] + p; + tab[u] = 34 * T0_PCF + tab[u - 1] + p; - tab[u+1] = 6 * T0_PCF + tab[u] + l; - tab[u+2] = 88 * T0_PCF + tab[u+1] - l - p; + tab[u + 1] = 6 * T0_PCF + tab[u] + l; + tab[u + 2] = 88 * T0_PCF + tab[u + 1] - l - p; return 0; } else { //add a bit 0 - if ( u == 0 ) + if (u == 0) tab[u] = 98 * T0_PCF + p; else - tab[u] = 98 * T0_PCF + tab[u-1] + p; + tab[u] = 98 * T0_PCF + tab[u - 1] + p; - tab[u+1] = 6 * T0_PCF + tab[u] + l; - tab[u+2] = 24 * T0_PCF + tab[u+1] - l - p; + tab[u + 1] = 6 * T0_PCF + tab[u] + l; + tab[u + 2] = 24 * T0_PCF + tab[u + 1] - l - p; return 0; } return 1; @@ -518,13 +518,14 @@ bool AddBitPCF7931(bool b, uint32_t * tab, int32_t l, int32_t p){ * @param c : delay of the last high pulse * @param tab : array of the data frame */ -bool AddPatternPCF7931(uint32_t a, uint32_t b, uint32_t c, uint32_t * tab){ +bool AddPatternPCF7931(uint32_t a, uint32_t b, uint32_t c, uint32_t *tab) +{ uint32_t u = 0; - for(u = 0; tab[u] != 0; u += 3){} //we put the cursor at the last value of the array + for (u = 0; tab[u] != 0; u += 3) {} //we put the cursor at the last value of the array - tab[u] = (u == 0) ? a : a + tab[u-1]; - tab[u+1] = b + tab[u]; - tab[u+2] = c + tab[u+1]; + tab[u] = (u == 0) ? a : a + tab[u - 1]; + tab[u + 1] = b + tab[u]; + tab[u + 2] = c + tab[u + 1]; return 0; } \ No newline at end of file diff --git a/armsrc/pcf7931.h b/armsrc/pcf7931.h index d87082b0f..bed33dadc 100644 --- a/armsrc/pcf7931.h +++ b/armsrc/pcf7931.h @@ -11,10 +11,10 @@ size_t DemodPCF7931(uint8_t **outBlocks); bool IsBlock0PCF7931(uint8_t *Block); bool IsBlock1PCF7931(uint8_t *Block); void ReadPCF7931(); -void SendCmdPCF7931(uint32_t * tab); -bool AddBytePCF7931(uint8_t byte, uint32_t * tab, int32_t l, int32_t p); -bool AddBitPCF7931(bool b, uint32_t * tab, int32_t l, int32_t p); -bool AddPatternPCF7931(uint32_t a, uint32_t b, uint32_t c, uint32_t * tab); +void SendCmdPCF7931(uint32_t *tab); +bool AddBytePCF7931(uint8_t byte, uint32_t *tab, int32_t l, int32_t p); +bool AddBitPCF7931(bool b, uint32_t *tab, int32_t l, int32_t p); +bool AddPatternPCF7931(uint32_t a, uint32_t b, uint32_t c, uint32_t *tab); 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); #endif \ No newline at end of file diff --git a/armsrc/printf.c b/armsrc/printf.c index ff48dec1c..a355fe2ff 100644 --- a/armsrc/printf.c +++ b/armsrc/printf.c @@ -128,282 +128,301 @@ kvsprintf(char const *fmt, void *arg, int radix, va_list ap) for (;;) { padc = ' '; width = 0; - while ((ch = (u_char)*fmt++) != '%' || stop) { + while ((ch = (u_char) * fmt++) != '%' || stop) { PCHAR(ch); if (ch == '\0') return (retval); } percent = fmt - 1; - qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0; - sign = 0; dot = 0; dwidth = 0; upper = 0; - cflag = 0; hflag = 0; jflag = 0; tflag = 0; zflag = 0; -reswitch: switch (ch = (u_char)*fmt++) { - case '.': - dot = 1; - goto reswitch; - case '#': - sharpflag = 1; - goto reswitch; - case '+': - sign = 1; - goto reswitch; - case '-': - ladjust = 1; - goto reswitch; - case '%': - PCHAR(ch); - break; - case '*': - if (!dot) { - width = va_arg(ap, int); - if (width < 0) { - ladjust = !ladjust; - width = -width; - } - } else { - dwidth = va_arg(ap, int); - } - goto reswitch; - case '0': - if (!dot) { - padc = '0'; + qflag = 0; + lflag = 0; + ladjust = 0; + sharpflag = 0; + neg = 0; + sign = 0; + dot = 0; + dwidth = 0; + upper = 0; + cflag = 0; + hflag = 0; + jflag = 0; + tflag = 0; + zflag = 0; +reswitch: + switch (ch = (u_char) * fmt++) { + case '.': + dot = 1; goto reswitch; - } - case '1': case '2': case '3': case '4': - case '5': case '6': case '7': case '8': case '9': + case '#': + sharpflag = 1; + goto reswitch; + case '+': + sign = 1; + goto reswitch; + case '-': + ladjust = 1; + goto reswitch; + case '%': + PCHAR(ch); + break; + case '*': + if (!dot) { + width = va_arg(ap, int); + if (width < 0) { + ladjust = !ladjust; + width = -width; + } + } else { + dwidth = va_arg(ap, int); + } + goto reswitch; + case '0': + if (!dot) { + padc = '0'; + goto reswitch; + } + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': for (n = 0;; ++fmt) { n = n * 10 + ch - '0'; ch = *fmt; if (ch < '0' || ch > '9') break; } - if (dot) - dwidth = n; - else - width = n; - goto reswitch; - case 'b': - num = (u_int)va_arg(ap, int); - p = va_arg(ap, char *); - for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;) - PCHAR(*q--); + if (dot) + dwidth = n; + else + width = n; + goto reswitch; + case 'b': + num = (u_int)va_arg(ap, int); + p = va_arg(ap, char *); + for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;) + PCHAR(*q--); - if (num == 0) - break; + if (num == 0) + break; - for (tmp = 0; *p;) { - n = *p++; - if (num & (1 << (n - 1))) { - PCHAR(tmp ? ',' : '<'); - for (; (n = *p) > ' '; ++p) - PCHAR(n); - tmp = 1; - } else - for (; *p > ' '; ++p) - continue; - } - if (tmp) - PCHAR('>'); - break; - case 'c': - PCHAR(va_arg(ap, int)); - break; - case 'D': - up = va_arg(ap, u_char *); - p = va_arg(ap, char *); - if (!width) - width = 16; - while(width--) { - PCHAR(hex2ascii(*up >> 4)); - PCHAR(hex2ascii(*up & 0x0f)); - up++; - if (width) - for (q=p;*q;q++) - PCHAR(*q); - } - break; - case 'd': - case 'i': - base = 10; - sign = 1; - goto handle_sign; - case 'h': - if (hflag) { - hflag = 0; - cflag = 1; - } else - hflag = 1; - goto reswitch; - case 'j': - jflag = 1; - goto reswitch; - case 'l': - if (lflag) { - lflag = 0; - qflag = 1; - } else - lflag = 1; - goto reswitch; - case 'n': - if (jflag) - *(va_arg(ap, intmax_t *)) = retval; - else if (qflag) - *(va_arg(ap, quad_t *)) = retval; - else if (lflag) - *(va_arg(ap, long *)) = retval; - else if (zflag) - *(va_arg(ap, size_t *)) = retval; - else if (hflag) - *(va_arg(ap, short *)) = retval; - else if (cflag) - *(va_arg(ap, char *)) = retval; - else - *(va_arg(ap, int *)) = retval; - break; - case 'o': - base = 8; - goto handle_nosign; - case 'p': - base = 16; - sharpflag = (width == 0); - sign = 0; - num = (uintptr_t)va_arg(ap, void *); - goto number; - case 'q': - qflag = 1; - goto reswitch; - case 'r': - base = radix; - if (sign) - goto handle_sign; - goto handle_nosign; - case 's': - p = va_arg(ap, char *); - if (p == NULL) - p = "(null)"; - if (!dot) - n = strlen (p); - else - for (n = 0; n < dwidth && p[n]; n++) - continue; - - width -= n; - - if (!ladjust && width > 0) - while (width--) - PCHAR(padc); - while (n--) - PCHAR(*p++); - if (ladjust && width > 0) - while (width--) - PCHAR(padc); - break; - case 't': - tflag = 1; - goto reswitch; - case 'u': - base = 10; - goto handle_nosign; - case 'X': - upper = 1; - case 'x': - base = 16; - goto handle_nosign; - case 'y': - base = 16; - sign = 1; - goto handle_sign; - case 'z': - zflag = 1; - goto reswitch; -handle_nosign: - sign = 0; - if (jflag) - num = va_arg(ap, uintmax_t); - else if (qflag) - num = va_arg(ap, u_quad_t); - else if (tflag) - num = va_arg(ap, ptrdiff_t); - else if (lflag) - num = va_arg(ap, u_long); - else if (zflag) - num = va_arg(ap, size_t); - else if (hflag) - num = (u_short)va_arg(ap, int); - else if (cflag) - num = (u_char)va_arg(ap, int); - else - num = va_arg(ap, u_int); - goto number; -handle_sign: - if (jflag) - num = va_arg(ap, intmax_t); - else if (qflag) - num = va_arg(ap, quad_t); - else if (tflag) - num = va_arg(ap, ptrdiff_t); - else if (lflag) - num = va_arg(ap, long); - else if (zflag) - num = va_arg(ap, ssize_t); - else if (hflag) - num = (short)va_arg(ap, int); - else if (cflag) - num = (char)va_arg(ap, int); - else - num = va_arg(ap, int); -number: - if (sign && (intmax_t)num < 0) { - neg = 1; - num = -(intmax_t)num; - } - p = ksprintn(nbuf, num, base, &tmp, upper); - if (sharpflag && num != 0) { - if (base == 8) - tmp++; - else if (base == 16) - tmp += 2; - } - if (neg) - tmp++; - - if (!ladjust && padc != '0' && width - && (width -= tmp) > 0) - while (width--) - PCHAR(padc); - if (neg) - PCHAR('-'); - if (sharpflag && num != 0) { - if (base == 8) { - PCHAR('0'); - } else if (base == 16) { - PCHAR('0'); - PCHAR('x'); + for (tmp = 0; *p;) { + n = *p++; + if (num & (1 << (n - 1))) { + PCHAR(tmp ? ',' : '<'); + for (; (n = *p) > ' '; ++p) + PCHAR(n); + tmp = 1; + } else + for (; *p > ' '; ++p) + continue; } - } - if (!ladjust && width && (width -= tmp) > 0) - while (width--) - PCHAR(padc); + if (tmp) + PCHAR('>'); + break; + case 'c': + PCHAR(va_arg(ap, int)); + break; + case 'D': + up = va_arg(ap, u_char *); + p = va_arg(ap, char *); + if (!width) + width = 16; + while (width--) { + PCHAR(hex2ascii(*up >> 4)); + PCHAR(hex2ascii(*up & 0x0f)); + up++; + if (width) + for (q = p; *q; q++) + PCHAR(*q); + } + break; + case 'd': + case 'i': + base = 10; + sign = 1; + goto handle_sign; + case 'h': + if (hflag) { + hflag = 0; + cflag = 1; + } else + hflag = 1; + goto reswitch; + case 'j': + jflag = 1; + goto reswitch; + case 'l': + if (lflag) { + lflag = 0; + qflag = 1; + } else + lflag = 1; + goto reswitch; + case 'n': + if (jflag) + *(va_arg(ap, intmax_t *)) = retval; + else if (qflag) + *(va_arg(ap, quad_t *)) = retval; + else if (lflag) + *(va_arg(ap, long *)) = retval; + else if (zflag) + *(va_arg(ap, size_t *)) = retval; + else if (hflag) + *(va_arg(ap, short *)) = retval; + else if (cflag) + *(va_arg(ap, char *)) = retval; + else + *(va_arg(ap, int *)) = retval; + break; + case 'o': + base = 8; + goto handle_nosign; + case 'p': + base = 16; + sharpflag = (width == 0); + sign = 0; + num = (uintptr_t)va_arg(ap, void *); + goto number; + case 'q': + qflag = 1; + goto reswitch; + case 'r': + base = radix; + if (sign) + goto handle_sign; + goto handle_nosign; + case 's': + p = va_arg(ap, char *); + if (p == NULL) + p = "(null)"; + if (!dot) + n = strlen(p); + else + for (n = 0; n < dwidth && p[n]; n++) + continue; - while (*p) - PCHAR(*p--); + width -= n; - if (ladjust && width && (width -= tmp) > 0) - while (width--) - PCHAR(padc); + if (!ladjust && width > 0) + while (width--) + PCHAR(padc); + while (n--) + PCHAR(*p++); + if (ladjust && width > 0) + while (width--) + PCHAR(padc); + break; + case 't': + tflag = 1; + goto reswitch; + case 'u': + base = 10; + goto handle_nosign; + case 'X': + upper = 1; + case 'x': + base = 16; + goto handle_nosign; + case 'y': + base = 16; + sign = 1; + goto handle_sign; + case 'z': + zflag = 1; + goto reswitch; +handle_nosign: + sign = 0; + if (jflag) + num = va_arg(ap, uintmax_t); + else if (qflag) + num = va_arg(ap, u_quad_t); + else if (tflag) + num = va_arg(ap, ptrdiff_t); + else if (lflag) + num = va_arg(ap, u_long); + else if (zflag) + num = va_arg(ap, size_t); + else if (hflag) + num = (u_short)va_arg(ap, int); + else if (cflag) + num = (u_char)va_arg(ap, int); + else + num = va_arg(ap, u_int); + goto number; +handle_sign: + if (jflag) + num = va_arg(ap, intmax_t); + else if (qflag) + num = va_arg(ap, quad_t); + else if (tflag) + num = va_arg(ap, ptrdiff_t); + else if (lflag) + num = va_arg(ap, long); + else if (zflag) + num = va_arg(ap, ssize_t); + else if (hflag) + num = (short)va_arg(ap, int); + else if (cflag) + num = (char)va_arg(ap, int); + else + num = va_arg(ap, int); +number: + if (sign && (intmax_t)num < 0) { + neg = 1; + num = -(intmax_t)num; + } + p = ksprintn(nbuf, num, base, &tmp, upper); + if (sharpflag && num != 0) { + if (base == 8) + tmp++; + else if (base == 16) + tmp += 2; + } + if (neg) + tmp++; - break; - default: - while (percent < fmt) - PCHAR(*percent++); - /* - * Since we ignore an formatting argument it is no - * longer safe to obey the remaining formatting - * arguments as the arguments will no longer match - * the format specs. - */ - stop = 1; - break; + if (!ladjust && padc != '0' && width + && (width -= tmp) > 0) + while (width--) + PCHAR(padc); + if (neg) + PCHAR('-'); + if (sharpflag && num != 0) { + if (base == 8) { + PCHAR('0'); + } else if (base == 16) { + PCHAR('0'); + PCHAR('x'); + } + } + if (!ladjust && width && (width -= tmp) > 0) + while (width--) + PCHAR(padc); + + while (*p) + PCHAR(*p--); + + if (ladjust && width && (width -= tmp) > 0) + while (width--) + PCHAR(padc); + + break; + default: + while (percent < fmt) + PCHAR(*percent++); + /* + * Since we ignore an formatting argument it is no + * longer safe to obey the remaining formatting + * arguments as the arguments will no longer match + * the format specs. + */ + stop = 1; + break; } } PCHAR(0); diff --git a/armsrc/printf.h b/armsrc/printf.h index 4060fcd1c..21d9c8fa4 100644 --- a/armsrc/printf.h +++ b/armsrc/printf.h @@ -15,8 +15,8 @@ #include #include "string.h" -int kvsprintf(const char *format, void *arg, int radix, va_list ap) __attribute__ ((format (printf, 1, 0))); -int vsprintf(char *str, const char *format, va_list ap) __attribute__ ((format (printf, 2, 0))); -int sprintf(char *str, const char *format, ...) __attribute__ ((format (printf, 2, 3))); +int kvsprintf(const char *format, void *arg, int radix, va_list ap) __attribute__((format(printf, 1, 0))); +int vsprintf(char *str, const char *format, va_list ap) __attribute__((format(printf, 2, 0))); +int sprintf(char *str, const char *format, ...) __attribute__((format(printf, 2, 3))); #endif diff --git a/armsrc/start.c b/armsrc/start.c index dcec6f59a..df5e30506 100644 --- a/armsrc/start.c +++ b/armsrc/start.c @@ -26,7 +26,7 @@ static voidpf inflate_malloc(voidpf opaque, uInt items, uInt size) uint8_t *allocated_memory; allocated_memory = next_free_memory; - next_free_memory += items*size; + next_free_memory += items * size; return allocated_memory; } @@ -71,7 +71,7 @@ void __attribute__((section(".startos"))) Vector(void) /* Set up (that is: clear) BSS. */ dst = &__bss_start__; end = &__bss_end__; - while(dst < end) *dst++ = 0; + while (dst < end) *dst++ = 0; // Set up data segment: Copy from flash to ram // src = &__data_src_start__; diff --git a/armsrc/string.c b/armsrc/string.c index e63ba4b05..5f188fbc4 100644 --- a/armsrc/string.c +++ b/armsrc/string.c @@ -13,7 +13,7 @@ void *memcpy(void *dest, const void *src, int len) { uint8_t *d = dest; const uint8_t *s = src; - while((len--) > 0) { + while ((len--) > 0) { *d = *s; d++; s++; @@ -24,7 +24,7 @@ void *memcpy(void *dest, const void *src, int len) void *memset(void *dest, int c, int len) { uint8_t *d = dest; - while((len--) > 0) { + while ((len--) > 0) { *d = c; d++; } @@ -36,8 +36,8 @@ int memcmp(const void *av, const void *bv, int len) const uint8_t *a = av; const uint8_t *b = bv; - while((len--) > 0) { - if(*a != *b) { + while ((len--) > 0) { + if (*a != *b) { return *a - *b; } a++; @@ -46,22 +46,23 @@ int memcmp(const void *av, const void *bv, int len) return 0; } -void memxor(uint8_t * dest, uint8_t * src, size_t len) { - for( ; len > 0; len--,dest++,src++) - *dest ^= *src; +void memxor(uint8_t *dest, uint8_t *src, size_t len) +{ + for (; len > 0; len--, dest++, src++) + *dest ^= *src; } int strlen(const char *str) { int l = 0; - while(*str) { + while (*str) { l++; str++; } return l; } -char* strncat(char *dest, const char *src, unsigned int n) +char *strncat(char *dest, const char *src, unsigned int n) { unsigned int dest_len = strlen(dest); unsigned int i; @@ -73,7 +74,7 @@ char* strncat(char *dest, const char *src, unsigned int n) return dest; } -char* strcat(char *dest, const char *src) +char *strcat(char *dest, const char *src) { unsigned int dest_len = strlen(dest); unsigned int i; @@ -91,7 +92,7 @@ void strreverse(char s[]) { int c, i, j; - for (i = 0, j = strlen(s)-1; i> 10; // Borrow a PWM unit for my real-time clock @@ -24,7 +25,7 @@ void SpinDelayUs(int us) { uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; - for(;;) { + for (;;) { uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; if (now == (uint16_t)(start + ticks)) return; @@ -33,9 +34,10 @@ void SpinDelayUs(int us) { } } -void SpinDelay(int ms) { - // convert to uS and call microsecond delay function - SpinDelayUs(ms*1000); +void SpinDelay(int ms) +{ + // convert to uS and call microsecond delay function + SpinDelayUs(ms * 1000); } // ------------------------------------------------------------------------- // timer lib @@ -46,26 +48,29 @@ void SpinDelay(int ms) { // SpinDelay(1000); // ti = GetTickCount() - ti; // Dbprintf("timer(1s): %d t=%d", ti, GetTickCount()); -void StartTickCount(void) { +void StartTickCount(void) +{ // This timer is based on the slow clock. The slow clock frequency is between 22kHz and 40kHz. // We can determine the actual slow clock frequency by looking at the Main Clock Frequency Register. uint16_t mainf = AT91C_BASE_PMC->PMC_MCFR & 0xffff; // = 16 * main clock frequency (16MHz) / slow clock frequency // set RealTimeCounter divider to count at 1kHz: - AT91C_BASE_RTTC->RTTC_RTMR = AT91C_RTTC_RTTRST | ((256000 + (mainf/2)) / mainf); + AT91C_BASE_RTTC->RTTC_RTMR = AT91C_RTTC_RTTRST | ((256000 + (mainf / 2)) / mainf); // note: worst case precision is approx 2.5% } /* * Get the current count. */ -uint32_t RAMFUNC GetTickCount(void){ +uint32_t RAMFUNC GetTickCount(void) +{ return AT91C_BASE_RTTC->RTTC_RTVR;// was * 2; } // ------------------------------------------------------------------------- // microseconds timer // ------------------------------------------------------------------------- -void StartCountUS(void) { +void StartCountUS(void) +{ AT91C_BASE_PMC->PMC_PCER |= (1 << AT91C_ID_TC0) | (1 << AT91C_ID_TC1); AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_NONE | AT91C_TCB_TC1XC1S_TIOA0 | AT91C_TCB_TC2XC2S_NONE; @@ -73,8 +78,8 @@ void StartCountUS(void) { // tick=1.5mks AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS; // timer disable AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | // MCK(48MHz) / 32 - AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_ACPA_CLEAR | - AT91C_TC_ACPC_SET | AT91C_TC_ASWTRG_SET; + AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_ACPA_CLEAR | + AT91C_TC_ACPC_SET | AT91C_TC_ASWTRG_SET; AT91C_BASE_TC0->TC_RA = 1; AT91C_BASE_TC0->TC_RC = 0xBFFF + 1; // 0xC000 @@ -88,7 +93,8 @@ void StartCountUS(void) { while (AT91C_BASE_TC1->TC_CV > 0); } -uint32_t RAMFUNC GetCountUS(void){ +uint32_t RAMFUNC GetCountUS(void) +{ //return (AT91C_BASE_TC1->TC_CV * 0x8000) + ((AT91C_BASE_TC0->TC_CV / 15) * 10); // By suggestion from PwPiwi, http://www.proxmark.org/forum/viewtopic.php?pid=17548#p17548 return ((uint32_t)AT91C_BASE_TC1->TC_CV) * 0x8000 + (((uint32_t)AT91C_BASE_TC0->TC_CV) * 2) / 3; @@ -97,40 +103,41 @@ uint32_t RAMFUNC GetCountUS(void){ // ------------------------------------------------------------------------- // Timer for iso14443 commands. Uses ssp_clk from FPGA // ------------------------------------------------------------------------- -void StartCountSspClk(void) { +void StartCountSspClk(void) +{ AT91C_BASE_PMC->PMC_PCER |= (1 << AT91C_ID_TC0) | (1 << AT91C_ID_TC1) | (1 << AT91C_ID_TC2); // Enable Clock to all timers AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_TIOA1 // XC0 Clock = TIOA1 - | AT91C_TCB_TC1XC1S_NONE // XC1 Clock = none - | AT91C_TCB_TC2XC2S_TIOA0; // XC2 Clock = TIOA0 + | AT91C_TCB_TC1XC1S_NONE // XC1 Clock = none + | AT91C_TCB_TC2XC2S_TIOA0; // XC2 Clock = TIOA0 // configure TC1 to create a short pulse on TIOA1 when a rising edge on TIOB1 (= ssp_clk from FPGA) occurs: AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // disable TC1 AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK // TC1 Clock = MCK(48MHz)/2 = 24MHz - | AT91C_TC_CPCSTOP // Stop clock on RC compare - | AT91C_TC_EEVTEDG_RISING // Trigger on rising edge of Event - | AT91C_TC_EEVT_TIOB // Event-Source: TIOB1 (= ssp_clk from FPGA = 13,56MHz/16) - | AT91C_TC_ENETRG // Enable external trigger event - | AT91C_TC_WAVESEL_UP // Upmode without automatic trigger on RC compare - | AT91C_TC_WAVE // Waveform Mode - | AT91C_TC_AEEVT_SET // Set TIOA1 on external event - | AT91C_TC_ACPC_CLEAR; // Clear TIOA1 on RC Compare + | AT91C_TC_CPCSTOP // Stop clock on RC compare + | AT91C_TC_EEVTEDG_RISING // Trigger on rising edge of Event + | AT91C_TC_EEVT_TIOB // Event-Source: TIOB1 (= ssp_clk from FPGA = 13,56MHz/16) + | AT91C_TC_ENETRG // Enable external trigger event + | AT91C_TC_WAVESEL_UP // Upmode without automatic trigger on RC compare + | AT91C_TC_WAVE // Waveform Mode + | AT91C_TC_AEEVT_SET // Set TIOA1 on external event + | AT91C_TC_ACPC_CLEAR; // Clear TIOA1 on RC Compare AT91C_BASE_TC1->TC_RC = 0x04; // RC Compare value = 0x04 // use TC0 to count TIOA1 pulses AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS; // disable TC0 AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_XC0 // TC0 clock = XC0 clock = TIOA1 - | AT91C_TC_WAVE // Waveform Mode - | AT91C_TC_WAVESEL_UP // just count - | AT91C_TC_ACPA_CLEAR // Clear TIOA0 on RA Compare - | AT91C_TC_ACPC_SET; // Set TIOA0 on RC Compare + | AT91C_TC_WAVE // Waveform Mode + | AT91C_TC_WAVESEL_UP // just count + | AT91C_TC_ACPA_CLEAR // Clear TIOA0 on RA Compare + | AT91C_TC_ACPC_SET; // Set TIOA0 on RC Compare AT91C_BASE_TC0->TC_RA = 1; // RA Compare value = 1; pulse width to TC2 AT91C_BASE_TC0->TC_RC = 0; // RC Compare value = 0; increment TC2 on overflow // use TC2 to count TIOA0 pulses (giving us a 32bit counter (TC0/TC2) clocked by ssp_clk) AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKDIS; // disable TC2 AT91C_BASE_TC2->TC_CMR = AT91C_TC_CLKS_XC2 // TC2 clock = XC2 clock = TIOA0 - | AT91C_TC_WAVE // Waveform Mode - | AT91C_TC_WAVESEL_UP; // just count + | AT91C_TC_WAVE // Waveform Mode + | AT91C_TC_WAVESEL_UP; // just count AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; // enable and reset TC0 AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; // enable and reset TC1 @@ -138,9 +145,9 @@ void StartCountSspClk(void) { // synchronize the counter with the ssp_frame signal. // Note: FPGA must be in any iso14443 mode, otherwise the frame signal would not be present - while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME)); // wait for ssp_frame to go high (start of frame) - while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME); // wait for ssp_frame to be low - while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); // wait for ssp_clk to go high + while (!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME)); // wait for ssp_frame to go high (start of frame) + while (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME); // wait for ssp_frame to be low + while (!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); // wait for ssp_clk to go high // note: up to now two ssp_clk rising edges have passed since the rising edge of ssp_frame // it is now safe to assert a sync signal. This sets all timers to 0 on next active clock edge @@ -154,14 +161,16 @@ void StartCountSspClk(void) { // Therefore need to wait quite some time before we can use the counter. while (AT91C_BASE_TC2->TC_CV > 0); } -void ResetSspClk(void) { +void ResetSspClk(void) +{ //enable clock of timer and software trigger AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; while (AT91C_BASE_TC2->TC_CV > 0); } -uint32_t RAMFUNC GetCountSspClk(void) { +uint32_t RAMFUNC GetCountSspClk(void) +{ uint32_t tmp_count = (AT91C_BASE_TC2->TC_CV << 16) | AT91C_BASE_TC0->TC_CV; if ((tmp_count & 0x0000ffff) == 0) //small chance that we may have missed an increment in TC2 return (AT91C_BASE_TC2->TC_CV << 16); @@ -172,7 +181,8 @@ uint32_t RAMFUNC GetCountSspClk(void) { // Timer for bitbanging, or LF stuff when you need a very precis timer // 1us = 1.5ticks // ------------------------------------------------------------------------- -void StartTicks(void){ +void StartTicks(void) +{ // initialization of the timer AT91C_BASE_PMC->PMC_PCER |= (1 << AT91C_ID_TC0) | (1 << AT91C_ID_TC1); AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_NONE | AT91C_TCB_TC1XC1S_TIOA0 | AT91C_TCB_TC2XC2S_NONE; @@ -205,36 +215,41 @@ void StartTicks(void){ while (AT91C_BASE_TC0->TC_CV > 0); } -uint32_t GetTicks(void) { +uint32_t GetTicks(void) +{ uint32_t hi, lo; do { hi = AT91C_BASE_TC1->TC_CV; lo = AT91C_BASE_TC0->TC_CV; - } while(hi != AT91C_BASE_TC1->TC_CV); + } while (hi != AT91C_BASE_TC1->TC_CV); return (hi << 16) | lo; } // Wait - Spindelay in ticks. // if called with a high number, this will trigger the WDT... -void WaitTicks(uint32_t ticks){ - if ( ticks == 0 ) return; +void WaitTicks(uint32_t ticks) +{ + if (ticks == 0) return; ticks += GetTicks(); while (GetTicks() < ticks); } // Wait / Spindelay in us (microseconds) // 1us = 1.5ticks. -void WaitUS(uint16_t us){ - WaitTicks( (uint32_t)us * 3/2 ); +void WaitUS(uint16_t us) +{ + WaitTicks((uint32_t)us * 3 / 2); } -void WaitMS(uint16_t ms){ - WaitTicks( (uint32_t)ms * 1500 ); +void WaitMS(uint16_t ms) +{ + WaitTicks((uint32_t)ms * 1500); } // stop clock -void StopTicks(void){ +void StopTicks(void) +{ AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS; AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; } diff --git a/armsrc/util.c b/armsrc/util.c index e3027c997..76b5d96cb 100644 --- a/armsrc/util.c +++ b/armsrc/util.c @@ -9,8 +9,9 @@ //----------------------------------------------------------------------------- #include "util.h" -size_t nbytes(size_t nbits) { - return (nbits >> 3)+((nbits % 8) > 0); +size_t nbytes(size_t nbits) +{ + return (nbits >> 3) + ((nbits % 8) > 0); } /* @@ -18,22 +19,25 @@ size_t nbytes(size_t nbits) { Returns the value v with the bottom b [0,32] bits reflected. Example: reflect(0x3e23L,3) == 0x3e26 */ -uint32_t reflect(uint32_t v, int b) { +uint32_t reflect(uint32_t v, int b) +{ uint32_t t = v; - for ( int i = 0; i < b; ++i) { + for (int i = 0; i < b; ++i) { if (t & 1) - v |= BITMASK((b-1)-i); + v |= BITMASK((b - 1) - i); else - v &= ~BITMASK((b-1)-i); - t>>=1; + v &= ~BITMASK((b - 1) - i); + t >>= 1; } return v; } -uint8_t reflect8(uint8_t b) { +uint8_t reflect8(uint8_t b) +{ return ((b * 0x80200802ULL) & 0x0884422110ULL) * 0x0101010101ULL >> 32; } -uint16_t reflect16(uint16_t b) { +uint16_t reflect16(uint16_t b) +{ uint16_t v = 0; v |= (b & 0x8000) >> 15; v |= (b & 0x4000) >> 13; @@ -55,14 +59,16 @@ uint16_t reflect16(uint16_t b) { return v; } -void num_to_bytes(uint64_t n, size_t len, uint8_t* dest) { +void num_to_bytes(uint64_t n, size_t len, uint8_t *dest) +{ while (len--) { dest[len] = (uint8_t) n; n >>= 8; } } -uint64_t bytes_to_num(uint8_t* src, size_t len) { +uint64_t bytes_to_num(uint8_t *src, size_t len) +{ uint64_t num = 0; while (len--) { num = (num << 8) | (*src); @@ -72,56 +78,93 @@ uint64_t bytes_to_num(uint8_t* src, size_t len) { } // RotateLeft - Ultralight, Desfire -void rol(uint8_t *data, const size_t len) { +void rol(uint8_t *data, const size_t len) +{ uint8_t first = data[0]; - for (size_t i = 0; i < len-1; i++) { - data[i] = data[i+1]; + for (size_t i = 0; i < len - 1; i++) { + data[i] = data[i + 1]; } - data[len-1] = first; + data[len - 1] = first; } -void lsl (uint8_t *data, size_t len) { +void lsl(uint8_t *data, size_t len) +{ for (size_t n = 0; n < len - 1; n++) { - data[n] = (data[n] << 1) | (data[n+1] >> 7); + data[n] = (data[n] << 1) | (data[n + 1] >> 7); } data[len - 1] <<= 1; } -int32_t le24toh (uint8_t data[3]) { +int32_t le24toh(uint8_t data[3]) +{ return (data[2] << 16) | (data[1] << 8) | data[0]; } //convert hex digit to integer -uint8_t hex2int(char hexchar){ - switch(hexchar){ - case '0': return 0; break; - case '1': return 1; break; - case '2': return 2; break; - case '3': return 3; break; - case '4': return 4; break; - case '5': return 5; break; - case '6': return 6; break; - case '7': return 7; break; - case '8': return 8; break; - case '9': return 9; break; +uint8_t hex2int(char hexchar) +{ + switch (hexchar) { + case '0': + return 0; + break; + case '1': + return 1; + break; + case '2': + return 2; + break; + case '3': + return 3; + break; + case '4': + return 4; + break; + case '5': + return 5; + break; + case '6': + return 6; + break; + case '7': + return 7; + break; + case '8': + return 8; + break; + case '9': + return 9; + break; case 'a': - case 'A': return 10; break; + case 'A': + return 10; + break; case 'b': - case 'B': return 11; break; + case 'B': + return 11; + break; case 'c': - case 'C': return 12; break; + case 'C': + return 12; + break; case 'd': - case 'D': return 13; break; + case 'D': + return 13; + break; case 'e': - case 'E': return 14; break; + case 'E': + return 14; + break; case 'f': - case 'F': return 15; break; + case 'F': + return 15; + break; default: return 0; } } -void LEDsoff() { +void LEDsoff() +{ LED_A_OFF(); LED_B_OFF(); LED_C_OFF(); @@ -129,7 +172,8 @@ void LEDsoff() { } // LEDs: R(C) O(A) G(B) -- R(D) [1, 2, 4 and 8] -void LED(int led, int ms) { +void LED(int led, int ms) +{ if (led & LED_RED) LED_C_ON(); if (led & LED_ORANGE) @@ -154,7 +198,8 @@ void LED(int led, int ms) { LED_D_OFF(); } -void SpinOff(uint32_t pause) { +void SpinOff(uint32_t pause) +{ LED_A_OFF(); LED_B_OFF(); LED_C_OFF(); @@ -163,29 +208,30 @@ void SpinOff(uint32_t pause) { } // 0=A, 1=B, 2=C, 3=D -void SpinErr(uint8_t led, uint32_t speed, uint8_t times) { +void SpinErr(uint8_t led, uint32_t speed, uint8_t times) +{ SpinOff(speed); - NTIME(times) - { + NTIME(times) { switch (led) { - case 0: - LED_A_INV(); - break; - case 1: - LED_B_INV(); - break; - case 2: - LED_C_INV(); - break; - case 3: - LED_D_INV(); - break; + case 0: + LED_A_INV(); + break; + case 1: + LED_B_INV(); + break; + case 2: + LED_C_INV(); + break; + case 3: + LED_D_INV(); + break; } SpinDelay(speed); } } -void SpinDown(uint32_t speed) { +void SpinDown(uint32_t speed) +{ SpinOff(speed); LED_D_ON(); SpinDelay(speed); @@ -201,7 +247,8 @@ void SpinDown(uint32_t speed) { LED_A_OFF(); } -void SpinUp(uint32_t speed) { +void SpinUp(uint32_t speed) +{ SpinOff(speed); LED_A_ON(); SpinDelay(speed); @@ -222,7 +269,8 @@ void SpinUp(uint32_t speed) { // not clicked, or held down (for ms || 1sec) // In general, don't use this function unless you expect a // double click, otherwise it will waste 500ms -- use BUTTON_HELD instead -int BUTTON_CLICKED(int ms) { +int BUTTON_CLICKED(int ms) +{ // Up to 500ms in between clicks to mean a double click int ticks = (48000 * (ms ? ms : 1000)) >> 10; @@ -240,16 +288,13 @@ int BUTTON_CLICKED(int ms) { uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; int letoff = 0; - for(;;) - { + for (;;) { uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; // We haven't let off the button yet - if (!letoff) - { + if (!letoff) { // We just let it off! - if (!BUTTON_PRESS()) - { + if (!BUTTON_PRESS()) { letoff = 1; // reset our timer for 500ms @@ -270,11 +315,10 @@ int BUTTON_CLICKED(int ms) { if (BUTTON_PRESS()) return BUTTON_DOUBLE_CLICK; - // Have we ran out of time to double click? - else - if (now == (uint16_t)(start + ticks)) - // At least we did a single click - return BUTTON_SINGLE_CLICK; + // Have we ran out of time to double click? + else if (now == (uint16_t)(start + ticks)) + // At least we did a single click + return BUTTON_SINGLE_CLICK; WDT_HIT(); } @@ -284,7 +328,8 @@ int BUTTON_CLICKED(int ms) { } // Determine if a button is held down -int BUTTON_HELD(int ms) { +int BUTTON_HELD(int ms) +{ // If button is held for one second int ticks = (48000 * (ms ? ms : 1000)) >> 10; @@ -301,8 +346,7 @@ int BUTTON_HELD(int ms) { uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; - for(;;) - { + for (;;) { uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; // As soon as our button let go, we didn't hold long enough @@ -310,9 +354,8 @@ int BUTTON_HELD(int ms) { return BUTTON_SINGLE_CLICK; // Have we waited the full second? - else - if (now == (uint16_t)(start + ticks)) - return BUTTON_HOLD; + else if (now == (uint16_t)(start + ticks)) + return BUTTON_HOLD; WDT_HIT(); } @@ -326,10 +369,11 @@ int BUTTON_HELD(int ms) { * verifies the magic properties, then stores a formatted string, prefixed by * prefix in dst. */ -void FormatVersionInformation(char *dst, int len, const char *prefix, void *version_information) { - struct version_information *v = (struct version_information*)version_information; +void FormatVersionInformation(char *dst, int len, const char *prefix, void *version_information) +{ + struct version_information *v = (struct version_information *)version_information; dst[0] = 0; - strncat(dst, prefix, len-1); + strncat(dst, prefix, len - 1); if (v->magic != VERSION_INFORMATION_MAGIC) { strncat(dst, "Missing/Invalid version information\n", len - strlen(dst) - 1); return; diff --git a/armsrc/util.h b/armsrc/util.h index bca073273..19b104f51 100644 --- a/armsrc/util.h +++ b/armsrc/util.h @@ -49,11 +49,11 @@ extern uint32_t reflect(uint32_t v, int b); // used in crc.c ... extern uint8_t reflect8(uint8_t b); // dedicated 8bit reversal extern uint16_t reflect16(uint16_t b); // dedicated 16bit reversal -void num_to_bytes(uint64_t n, size_t len, uint8_t* dest); -uint64_t bytes_to_num(uint8_t* src, size_t len); +void num_to_bytes(uint64_t n, size_t len, uint8_t *dest); +uint64_t bytes_to_num(uint8_t *src, size_t len); void rol(uint8_t *data, const size_t len); -void lsl (uint8_t *data, size_t len); -int32_t le24toh (uint8_t data[3]); +void lsl(uint8_t *data, size_t len); +int32_t le24toh(uint8_t data[3]); uint8_t hex2int(char hexchar); void LED(int led, int ms); diff --git a/armsrc/vtsend.c b/armsrc/vtsend.c index e851d4334..666e1aef7 100644 --- a/armsrc/vtsend.c +++ b/armsrc/vtsend.c @@ -37,13 +37,15 @@ //#define UART_WRITE(P, BUF, SIZ) (P)->uart_write(BUF, SIZ, (P)->extobj) #define UART_WRITE(BUF) DbprintfEx(FLAG_RAWPRINT, "%s", BUF) -int vtsend_init(vtsend_t *p, VTSEND_SERIAL_WRITE uart_write, void *extobj) { +int vtsend_init(vtsend_t *p, VTSEND_SERIAL_WRITE uart_write, void *extobj) +{ p->uart_write = uart_write; p->extobj = extobj; return 0; } -int vtsend_cursor_position(vtsend_t *p, const int column, const int line) { +int vtsend_cursor_position(vtsend_t *p, const int column, const int line) +{ char buf[1 + 8]; buf[0] = ESC; buf[1] = '['; @@ -58,7 +60,8 @@ int vtsend_cursor_position(vtsend_t *p, const int column, const int line) { return 0; } -int vtsend_cursor_up(vtsend_t *p, const int n) { +int vtsend_cursor_up(vtsend_t *p, const int n) +{ char buf[1 + 5]; buf[0] = ESC; buf[1] = '['; @@ -71,7 +74,8 @@ int vtsend_cursor_up(vtsend_t *p, const int n) { return 0; } -int vtsend_cursor_down(vtsend_t *p, const int n) { +int vtsend_cursor_down(vtsend_t *p, const int n) +{ char buf[1 + 5]; buf[0] = ESC; buf[1] = '['; @@ -84,7 +88,8 @@ int vtsend_cursor_down(vtsend_t *p, const int n) { return 0; } -int vtsend_cursor_forward(vtsend_t *p, const int n) { +int vtsend_cursor_forward(vtsend_t *p, const int n) +{ char buf[1 + 5]; buf[0] = ESC; buf[1] = '['; @@ -97,7 +102,8 @@ int vtsend_cursor_forward(vtsend_t *p, const int n) { return 0; } -int vtsend_cursor_backward(vtsend_t *p, const int n) { +int vtsend_cursor_backward(vtsend_t *p, const int n) +{ char buf[1 + 5]; buf[0] = ESC; buf[1] = '['; @@ -110,7 +116,8 @@ int vtsend_cursor_backward(vtsend_t *p, const int n) { return 0; } -int vtsend_cursor_position_save(vtsend_t *p) { +int vtsend_cursor_position_save(vtsend_t *p) +{ char buf[1 + 3]; buf[0] = ESC; buf[1] = '['; @@ -121,7 +128,8 @@ int vtsend_cursor_position_save(vtsend_t *p) { return 0; } -int vtsend_cursor_position_restore(vtsend_t *p) { +int vtsend_cursor_position_restore(vtsend_t *p) +{ char buf[1 + 3]; buf[0] = ESC; buf[1] = '['; @@ -132,7 +140,8 @@ int vtsend_cursor_position_restore(vtsend_t *p) { return 0; } -int vtsend_erase_display(vtsend_t *p) { +int vtsend_erase_display(vtsend_t *p) +{ char buf[1 + 4]; buf[0] = ESC; buf[1] = '['; @@ -144,7 +153,8 @@ int vtsend_erase_display(vtsend_t *p) { return 0; } -int vtsend_erase_line(vtsend_t *p) { +int vtsend_erase_line(vtsend_t *p) +{ char buf[1 + 4]; buf[0] = ESC; buf[1] = '['; @@ -156,7 +166,8 @@ int vtsend_erase_line(vtsend_t *p) { return 0; } -int vtsend_set_color_foreground(vtsend_t *p, const int color) { +int vtsend_set_color_foreground(vtsend_t *p, const int color) +{ char buf[1 + 5]; buf[0] = ESC; buf[1] = '['; @@ -169,7 +180,8 @@ int vtsend_set_color_foreground(vtsend_t *p, const int color) { return 0; } -int vtsend_set_color_background(vtsend_t *p, const int color) { +int vtsend_set_color_background(vtsend_t *p, const int color) +{ char buf[1 + 5]; buf[0] = ESC; buf[1] = '['; @@ -182,7 +194,8 @@ int vtsend_set_color_background(vtsend_t *p, const int color) { return 0; } -int vtsend_set_attribute(vtsend_t *p, const int attr) { +int vtsend_set_attribute(vtsend_t *p, const int attr) +{ char buf[1 + 5]; buf[0] = ESC; buf[1] = '['; @@ -195,7 +208,8 @@ int vtsend_set_attribute(vtsend_t *p, const int attr) { return 0; } -int vtsend_set_scroll_region(vtsend_t *p, const int top, const int bottom) { +int vtsend_set_scroll_region(vtsend_t *p, const int top, const int bottom) +{ char buf[1 + 8]; buf[0] = ESC; buf[1] = '['; @@ -211,7 +225,8 @@ int vtsend_set_scroll_region(vtsend_t *p, const int top, const int bottom) { return 0; } -int vtsend_set_cursor(vtsend_t *p, const int visible) { +int vtsend_set_cursor(vtsend_t *p, const int visible) +{ if (visible) { char buf[1 + 6]; buf[0] = ESC; @@ -238,7 +253,8 @@ int vtsend_set_cursor(vtsend_t *p, const int visible) { return 0; } -int vtsend_reset(vtsend_t *p) { +int vtsend_reset(vtsend_t *p) +{ char buf[1 + 2]; buf[0] = ESC; buf[1] = 'c'; @@ -248,7 +264,8 @@ int vtsend_reset(vtsend_t *p) { return 0; } -int vtsend_draw_box(vtsend_t *p, const int x1, const int y1, const int x2, const int y2) { +int vtsend_draw_box(vtsend_t *p, const int x1, const int y1, const int x2, const int y2) +{ int i; vtsend_cursor_position(p, x1, y1); @@ -268,7 +285,8 @@ int vtsend_draw_box(vtsend_t *p, const int x1, const int y1, const int x2, const return 0; } -int vtsend_fill_box(vtsend_t *p, const int x1, const int y1, const int x2, const int y2) { +int vtsend_fill_box(vtsend_t *p, const int x1, const int y1, const int x2, const int y2) +{ int i, j; for (i = y1; i <= y2; i++) { vtsend_cursor_position(p, x1, i); diff --git a/armsrc/vtsend.h b/armsrc/vtsend.h index 784ff3ebc..4dcb0c62b 100644 --- a/armsrc/vtsend.h +++ b/armsrc/vtsend.h @@ -78,11 +78,11 @@ int vtsend_set_cursor(vtsend_t *p, const int visible); int vtsend_reset(vtsend_t *p); int vtsend_draw_box( - vtsend_t *p, - const int x1, const int y1, const int x2, const int y2); + vtsend_t *p, + const int x1, const int y1, const int x2, const int y2); int vtsend_fill_box( - vtsend_t *p, - const int x1, const int y1, const int x2, const int y2); + vtsend_t *p, + const int x1, const int y1, const int x2, const int y2); #ifdef __cplusplus } diff --git a/bootrom/bootrom.c b/bootrom/bootrom.c index fda79f905..ec1922cb5 100644 --- a/bootrom/bootrom.c +++ b/bootrom/bootrom.c @@ -15,15 +15,17 @@ unsigned int start_addr, end_addr, bootrom_unlocked; extern char _bootrom_start, _bootrom_end, _flash_start, _flash_end; extern uint32_t _osimage_entry; -void DbpString(char *str) { +void DbpString(char *str) +{ byte_t len = 0; while (str[len] != 0x00) len++; - cmd_send(CMD_DEBUG_PRINT_STRING, len, 0, 0, (byte_t*)str, len); + cmd_send(CMD_DEBUG_PRINT_STRING, len, 0, 0, (byte_t *)str, len); } -static void ConfigClocks(void) { +static void ConfigClocks(void) +{ // we are using a 16 MHz crystal as the basis for everything // slow clock runs at 32Khz typical regardless of crystal @@ -32,12 +34,12 @@ static void ConfigClocks(void) { // enable the clock to the following peripherals AT91C_BASE_PMC->PMC_PCER = - (1<PMC_SR & AT91C_PMC_MOSCS) ) {}; + while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MOSCS)) {}; // PLL output clock frequency in range 80 - 160 MHz needs CKGR_PLL = 00 // PLL output clock frequency in range 150 - 180 MHz needs CKGR_PLL = 10 @@ -63,7 +65,7 @@ static void ConfigClocks(void) { PMC_PLL_USB_DIVISOR(1); // wait for PLL to lock - while ( !(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_LOCK) ) {}; + while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_LOCK)) {}; // we want a master clock (MCK) to be PLL clock / 2 = 96Mhz / 2 = 48Mhz // datasheet recommends that this register is programmed in two operations @@ -71,84 +73,90 @@ static void ConfigClocks(void) { AT91C_BASE_PMC->PMC_MCKR = AT91C_PMC_PRES_CLK_2; // wait for main clock ready signal - while ( !(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY) ) {}; + while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY)) {}; // set the source to PLL AT91C_BASE_PMC->PMC_MCKR = AT91C_PMC_PRES_CLK_2 | AT91C_PMC_CSS_PLL_CLK; // wait for main clock ready signal - while ( !(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY) ) {}; + while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY)) {}; } -static void Fatal(void) { - for(;;) {}; +static void Fatal(void) +{ + for (;;) {}; } -void UsbPacketReceived(uint8_t *packet, int len) { +void UsbPacketReceived(uint8_t *packet, int len) +{ int i, dont_ack = 0; - UsbCommand* c = (UsbCommand *)packet; + UsbCommand *c = (UsbCommand *)packet; volatile uint32_t *p; //if ( len != sizeof(UsbCommand)) Fatal(); uint32_t arg0 = (uint32_t)c->arg[0]; - switch(c->cmd) { + switch (c->cmd) { case CMD_DEVICE_INFO: { dont_ack = 1; arg0 = DEVICE_INFO_FLAG_BOOTROM_PRESENT | DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM | - DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH; - if(common_area.flags.osimage_present) + DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH; + if (common_area.flags.osimage_present) arg0 |= DEVICE_INFO_FLAG_OSIMAGE_PRESENT; - cmd_send(CMD_DEVICE_INFO,arg0,1,2,0,0); - } break; + cmd_send(CMD_DEVICE_INFO, arg0, 1, 2, 0, 0); + } + break; case CMD_SETUP_WRITE: { /* The temporary write buffer of the embedded flash controller is mapped to the * whole memory region, only the last 8 bits are decoded. */ p = (volatile uint32_t *)&_flash_start; - for(i = 0; i < 12; i++) - p[i+arg0] = c->d.asDwords[i]; - } break; + for (i = 0; i < 12; i++) + p[i + arg0] = c->d.asDwords[i]; + } + break; case CMD_FINISH_WRITE: { - uint32_t* flash_mem = (uint32_t*)(&_flash_start); - for ( int j=0; j<2; j++) { - for(i = 0+(64*j); i < 64+(64*j); i++) { + uint32_t *flash_mem = (uint32_t *)(&_flash_start); + for (int j = 0; j < 2; j++) { + for (i = 0 + (64 * j); i < 64 + (64 * j); i++) { flash_mem[i] = c->d.asDwords[i]; } - uint32_t flash_address = arg0 + (0x100*j); + uint32_t flash_address = arg0 + (0x100 * j); /* Check that the address that we are supposed to write to is within our allowed region */ - if( ((flash_address + AT91C_IFLASH_PAGE_SIZE - 1) >= end_addr) || (flash_address < start_addr) ) { + if (((flash_address + AT91C_IFLASH_PAGE_SIZE - 1) >= end_addr) || (flash_address < start_addr)) { /* Disallow write */ dont_ack = 1; - cmd_send(CMD_NACK,0,0,0,0,0); + cmd_send(CMD_NACK, 0, 0, 0, 0, 0); } else { uint32_t page_n = (flash_address - ((uint32_t)flash_mem)) / AT91C_IFLASH_PAGE_SIZE; /* Translate address to flash page and do flash, update here for the 512k part */ AT91C_BASE_EFC0->EFC_FCR = MC_FLASH_COMMAND_KEY | - MC_FLASH_COMMAND_PAGEN(page_n) | - AT91C_MC_FCMD_START_PROG; + MC_FLASH_COMMAND_PAGEN(page_n) | + AT91C_MC_FCMD_START_PROG; } // Wait until flashing of page finishes uint32_t sr; - while(!((sr = AT91C_BASE_EFC0->EFC_FSR) & AT91C_MC_FRDY)); - if(sr & (AT91C_MC_LOCKE | AT91C_MC_PROGE)) { + while (!((sr = AT91C_BASE_EFC0->EFC_FSR) & AT91C_MC_FRDY)); + if (sr & (AT91C_MC_LOCKE | AT91C_MC_PROGE)) { dont_ack = 1; - cmd_send(CMD_NACK,sr,0,0,0,0); + cmd_send(CMD_NACK, sr, 0, 0, 0, 0); } } - } break; + } + break; case CMD_HARDWARE_RESET: { usb_disable(); AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST; - } break; + } + break; case CMD_START_FLASH: { if (c->arg[2] == START_FLASH_MAGIC) @@ -166,28 +174,31 @@ void UsbPacketReceived(uint8_t *packet, int len) { /* Only allow command if the bootrom is unlocked, or the parameters are outside of the protected * bootrom area. In any case they must be within the flash area. */ - if( (bootrom_unlocked || ((cmd_start >= prot_end) || (cmd_end < prot_start))) && + if ((bootrom_unlocked || ((cmd_start >= prot_end) || (cmd_end < prot_start))) && (cmd_start >= allow_start) && - (cmd_end <= allow_end) ) { + (cmd_end <= allow_end)) { start_addr = cmd_start; end_addr = cmd_end; } else { start_addr = end_addr = 0; dont_ack = 1; - cmd_send(CMD_NACK,0,0,0,0,0); + cmd_send(CMD_NACK, 0, 0, 0, 0, 0); } - } break; + } + break; default: { Fatal(); - } break; + } + break; } if (!dont_ack) - cmd_send(CMD_ACK,arg0,0,0,0,0); + cmd_send(CMD_ACK, arg0, 0, 0, 0, 0); } -static void flash_mode(int externally_entered) { +static void flash_mode(int externally_entered) +{ start_addr = 0; end_addr = 0; bootrom_unlocked = 0; @@ -196,14 +207,14 @@ static void flash_mode(int externally_entered) { usb_enable(); // wait for reset to be complete? - for (volatile size_t i=0; i<0x100000; i++) {}; + for (volatile size_t i = 0; i < 0x100000; i++) {}; - for(;;) { + for (;;) { WDT_HIT(); // Check if there is a usb packet available if (usb_poll_validate_length()) { - if (usb_read(rx, sizeof(rx)) ) + if (usb_read(rx, sizeof(rx))) UsbPacketReceived(rx, sizeof(rx)); } @@ -212,7 +223,7 @@ static void flash_mode(int externally_entered) { usb_disable(); LED_B_ON(); AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST; - for(;;) {}; + for (;;) {}; } if (externally_entered && BUTTON_PRESS()) { /* Let the user's button press override the automatic leave */ @@ -221,7 +232,8 @@ static void flash_mode(int externally_entered) { } } -void BootROM(void) { +void BootROM(void) +{ //------------ // First set up all the I/O pins; GPIOs configured directly, other ones // just need to be assigned to the appropriate peripheral. @@ -246,7 +258,7 @@ void BootROM(void) { GPIO_MUXSEL_LORAW | GPIO_RELAY | GPIO_NVDD_ON; - // (and add GPIO_FPGA_ON) + // (and add GPIO_FPGA_ON) // These pins are outputs AT91C_BASE_PIOA->PIO_OER = GPIO_LED_A | @@ -274,8 +286,8 @@ void BootROM(void) { AT91C_BASE_EFC0->EFC_FMR = AT91C_MC_FWS_1FWS | MC_FLASH_MODE_MASTER_CLK_IN_MHZ(48); // 9 = 256, 10+ is 512kb - uint8_t id = ( *(AT91C_DBGU_CIDR) & 0xF00) >> 8; - if ( id > 9 ) + uint8_t id = (*(AT91C_DBGU_CIDR) & 0xF00) >> 8; + if (id > 9) AT91C_BASE_EFC1->EFC_FMR = AT91C_MC_FWS_1FWS | MC_FLASH_MODE_MASTER_CLK_IN_MHZ(48); // Initialize all system clocks @@ -285,23 +297,23 @@ void BootROM(void) { int common_area_present = 0; switch (AT91C_BASE_RSTC->RSTC_RSR & AT91C_RSTC_RSTTYP) { - case AT91C_RSTC_RSTTYP_WATCHDOG: - case AT91C_RSTC_RSTTYP_SOFTWARE: - case AT91C_RSTC_RSTTYP_USER: - /* In these cases the common_area in RAM should be ok, retain it if it's there */ - if(common_area.magic == COMMON_AREA_MAGIC && common_area.version == 1) - common_area_present = 1; - break; - default: /* Otherwise, initialize it from scratch */ - break; + case AT91C_RSTC_RSTTYP_WATCHDOG: + case AT91C_RSTC_RSTTYP_SOFTWARE: + case AT91C_RSTC_RSTTYP_USER: + /* In these cases the common_area in RAM should be ok, retain it if it's there */ + if (common_area.magic == COMMON_AREA_MAGIC && common_area.version == 1) + common_area_present = 1; + break; + default: /* Otherwise, initialize it from scratch */ + break; } - if (!common_area_present){ + if (!common_area_present) { /* Common area not ok, initialize it */ int i; /* Makeshift memset, no need to drag util.c into this */ - for(i=0; iaesKey, 128 ); + mbedtls_aes_setkey_enc(&aes, keys->aesKey, 128); memset(nonce_counter, 0, sizeof(nonce_counter)); memset(stream_block, 0, sizeof(stream_block)); memcpy(nonce_counter, keys->aesIV, sizeof(nonce_counter)); - mbedtls_aes_crypt_ctr( &aes, 0x188, &nc_off, nonce_counter, stream_block, in + 0x02C, out + 0x02C ); + mbedtls_aes_crypt_ctr(&aes, 0x188, &nc_off, nonce_counter, stream_block, in + 0x02C, out + 0x02C); memcpy(out + 0x000, in + 0x000, 0x008); // Data signature NOT copied @@ -46,7 +49,8 @@ void nfc3d_amiibo_cipher(const nfc3d_keygen_derivedkeys * keys, const uint8_t * memcpy(out + 0x1D4, in + 0x1D4, 0x034); } -void nfc3d_amiibo_tag_to_internal(const uint8_t * tag, uint8_t * intl) { +void nfc3d_amiibo_tag_to_internal(const uint8_t *tag, uint8_t *intl) +{ memcpy(intl + 0x000, tag + 0x008, 0x008); memcpy(intl + 0x008, tag + 0x080, 0x020); memcpy(intl + 0x028, tag + 0x010, 0x024); @@ -56,7 +60,8 @@ void nfc3d_amiibo_tag_to_internal(const uint8_t * tag, uint8_t * intl) { memcpy(intl + 0x1DC, tag + 0x054, 0x02C); } -void nfc3d_amiibo_internal_to_tag(const uint8_t * intl, uint8_t * tag) { +void nfc3d_amiibo_internal_to_tag(const uint8_t *intl, uint8_t *tag) +{ memcpy(tag + 0x008, intl + 0x000, 0x008); memcpy(tag + 0x080, intl + 0x008, 0x020); memcpy(tag + 0x010, intl + 0x028, 0x024); @@ -66,7 +71,8 @@ void nfc3d_amiibo_internal_to_tag(const uint8_t * intl, uint8_t * tag) { memcpy(tag + 0x054, intl + 0x1DC, 0x02C); } -bool nfc3d_amiibo_unpack(const nfc3d_amiibo_keys * amiiboKeys, const uint8_t * tag, uint8_t * plain) { +bool nfc3d_amiibo_unpack(const nfc3d_amiibo_keys *amiiboKeys, const uint8_t *tag, uint8_t *plain) +{ uint8_t internal[NFC3D_AMIIBO_SIZE]; nfc3d_keygen_derivedkeys dataKeys; nfc3d_keygen_derivedkeys tagKeys; @@ -82,19 +88,20 @@ bool nfc3d_amiibo_unpack(const nfc3d_amiibo_keys * amiiboKeys, const uint8_t * t nfc3d_amiibo_cipher(&dataKeys, internal, plain); // Regenerate tag HMAC. Note: order matters, data HMAC depends on tag HMAC! - mbedtls_md_hmac( mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), tagKeys.hmacKey, sizeof(tagKeys.hmacKey), - plain + 0x1D4, 0x34, plain + HMAC_POS_TAG ); + mbedtls_md_hmac(mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), tagKeys.hmacKey, sizeof(tagKeys.hmacKey), + plain + 0x1D4, 0x34, plain + HMAC_POS_TAG); // Regenerate data HMAC - mbedtls_md_hmac( mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), dataKeys.hmacKey, sizeof(dataKeys.hmacKey), - plain + 0x029, 0x1DF, plain + HMAC_POS_DATA ); + mbedtls_md_hmac(mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), dataKeys.hmacKey, sizeof(dataKeys.hmacKey), + plain + 0x029, 0x1DF, plain + HMAC_POS_DATA); return - memcmp(plain + HMAC_POS_DATA, internal + HMAC_POS_DATA, 32) == 0 && - memcmp(plain + HMAC_POS_TAG, internal + HMAC_POS_TAG, 32) == 0; + memcmp(plain + HMAC_POS_DATA, internal + HMAC_POS_DATA, 32) == 0 && + memcmp(plain + HMAC_POS_TAG, internal + HMAC_POS_TAG, 32) == 0; } -void nfc3d_amiibo_pack(const nfc3d_amiibo_keys * amiiboKeys, const uint8_t * plain, uint8_t * tag) { +void nfc3d_amiibo_pack(const nfc3d_amiibo_keys *amiiboKeys, const uint8_t *plain, uint8_t *tag) +{ uint8_t cipher[NFC3D_AMIIBO_SIZE]; nfc3d_keygen_derivedkeys tagKeys; nfc3d_keygen_derivedkeys dataKeys; @@ -104,24 +111,24 @@ void nfc3d_amiibo_pack(const nfc3d_amiibo_keys * amiiboKeys, const uint8_t * pla nfc3d_amiibo_keygen(&amiiboKeys->data, plain, &dataKeys); // Generate tag HMAC - mbedtls_md_hmac( mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), tagKeys.hmacKey, sizeof(tagKeys.hmacKey), - plain + 0x1D4, 0x34, cipher + HMAC_POS_TAG ); + mbedtls_md_hmac(mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), tagKeys.hmacKey, sizeof(tagKeys.hmacKey), + plain + 0x1D4, 0x34, cipher + HMAC_POS_TAG); // Init mbedtls HMAC context mbedtls_md_context_t ctx; - mbedtls_md_init( &ctx ); - mbedtls_md_setup( &ctx, mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), 1 ); + mbedtls_md_init(&ctx); + mbedtls_md_setup(&ctx, mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), 1); // Generate data HMAC - mbedtls_md_hmac_starts( &ctx, dataKeys.hmacKey, sizeof(dataKeys.hmacKey) ); - mbedtls_md_hmac_update( &ctx, plain + 0x029, 0x18B ); // Data - mbedtls_md_hmac_update( &ctx, cipher + HMAC_POS_TAG, 0x20 ); // Tag HMAC - mbedtls_md_hmac_update( &ctx, plain + 0x1D4, 0x34 ); // Here be dragons + mbedtls_md_hmac_starts(&ctx, dataKeys.hmacKey, sizeof(dataKeys.hmacKey)); + mbedtls_md_hmac_update(&ctx, plain + 0x029, 0x18B); // Data + mbedtls_md_hmac_update(&ctx, cipher + HMAC_POS_TAG, 0x20); // Tag HMAC + mbedtls_md_hmac_update(&ctx, plain + 0x1D4, 0x34); // Here be dragons - mbedtls_md_hmac_finish( &ctx, cipher + HMAC_POS_DATA ); + mbedtls_md_hmac_finish(&ctx, cipher + HMAC_POS_DATA); // HMAC cleanup - mbedtls_md_free( &ctx ); + mbedtls_md_free(&ctx); // Encrypt nfc3d_amiibo_cipher(&dataKeys, plain, cipher); @@ -130,8 +137,9 @@ void nfc3d_amiibo_pack(const nfc3d_amiibo_keys * amiiboKeys, const uint8_t * pla nfc3d_amiibo_internal_to_tag(cipher, tag); } -bool nfc3d_amiibo_load_keys(nfc3d_amiibo_keys * amiiboKeys, const char * path) { - FILE * f = fopen(path, "rb"); +bool nfc3d_amiibo_load_keys(nfc3d_amiibo_keys *amiiboKeys, const char *path) +{ + FILE *f = fopen(path, "rb"); if (!f) { return false; } @@ -152,7 +160,8 @@ bool nfc3d_amiibo_load_keys(nfc3d_amiibo_keys * amiiboKeys, const char * path) { return true; } -void nfc3d_amiibo_copy_app_data(const uint8_t * src, uint8_t * dst) { +void nfc3d_amiibo_copy_app_data(const uint8_t *src, uint8_t *dst) +{ //uint16_t *ami_nb_wr = (uint16_t*)(dst + 0x29); @@ -162,8 +171,8 @@ void nfc3d_amiibo_copy_app_data(const uint8_t * src, uint8_t * dst) { //*ami_nb_wr = htobe16(be16toh(*ami_nb_wr) + 1); //*cfg_nb_wr = htobe16(be16toh(*cfg_nb_wr) + 1); - uint16_t ami_nb_wr = ((uint16_t)bytes_to_num(dst + 0x29, 2)) + 1; - uint16_t cfg_nb_wr = ((uint16_t)bytes_to_num(dst + 0xB4, 2)) + 1; + uint16_t ami_nb_wr = ((uint16_t)bytes_to_num(dst + 0x29, 2)) + 1; + uint16_t cfg_nb_wr = ((uint16_t)bytes_to_num(dst + 0xB4, 2)) + 1; num_to_bytes(ami_nb_wr, 2, dst + 0x29); num_to_bytes(cfg_nb_wr, 2, dst + 0xB4); diff --git a/client/amiitool/amiibo.h b/client/amiitool/amiibo.h index 5b45e9b98..7b56a2c0a 100644 --- a/client/amiitool/amiibo.h +++ b/client/amiitool/amiibo.h @@ -24,9 +24,9 @@ typedef struct { } nfc3d_amiibo_keys; #pragma pack() -bool nfc3d_amiibo_unpack(const nfc3d_amiibo_keys * amiiboKeys, const uint8_t * tag, uint8_t * plain); -void nfc3d_amiibo_pack(const nfc3d_amiibo_keys * amiiboKeys, const uint8_t * plain, uint8_t * tag); -bool nfc3d_amiibo_load_keys(nfc3d_amiibo_keys * amiiboKeys, const char * path); -void nfc3d_amiibo_copy_app_data(const uint8_t * src, uint8_t * dst); +bool nfc3d_amiibo_unpack(const nfc3d_amiibo_keys *amiiboKeys, const uint8_t *tag, uint8_t *plain); +void nfc3d_amiibo_pack(const nfc3d_amiibo_keys *amiiboKeys, const uint8_t *plain, uint8_t *tag); +bool nfc3d_amiibo_load_keys(nfc3d_amiibo_keys *amiiboKeys, const char *path); +void nfc3d_amiibo_copy_app_data(const uint8_t *src, uint8_t *dst); #endif diff --git a/client/amiitool/amiitool.c b/client/amiitool/amiitool.c index 2643ffa36..9d30e4cb7 100644 --- a/client/amiitool/amiitool.c +++ b/client/amiitool/amiitool.c @@ -12,27 +12,29 @@ #define NTAG215_SIZE 540 -static char * self; +static char *self; -void amiitool_usage() { +void amiitool_usage() +{ fprintf(stderr, - "amiitool build %i (commit %s-%08x)\n" - "by Marcos Del Sol Vives \n" - "\n" - "Usage: %s (-e|-d|-c) -k keyfile [-i input] [-s input2] [-o output]\n" - " -e encrypt and sign amiibo\n" - " -d decrypt and test amiibo\n" - " -c decrypt, copy AppData and encrypt amiibo\n" - " -k key set file. For retail amiibo, use \"retail unfixed\" key set\n" - " -i input file. If not specified, stdin will be used.\n" - " -s input save file, save from this file will replace input file ones.\n" - " -o output file. If not specified, stdout will be used.\n" - " -l decrypt files with invalid signatures.\n", - , self - ); + "amiitool build %i (commit %s-%08x)\n" + "by Marcos Del Sol Vives \n" + "\n" + "Usage: %s (-e|-d|-c) -k keyfile [-i input] [-s input2] [-o output]\n" + " -e encrypt and sign amiibo\n" + " -d decrypt and test amiibo\n" + " -c decrypt, copy AppData and encrypt amiibo\n" + " -k key set file. For retail amiibo, use \"retail unfixed\" key set\n" + " -i input file. If not specified, stdin will be used.\n" + " -s input save file, save from this file will replace input file ones.\n" + " -o output file. If not specified, stdout will be used.\n" + " -l decrypt files with invalid signatures.\n", + , self + ); } -static bool LoadAmiikey(nfc3d_amiibo_keys keys, char* keyfile) { +static bool LoadAmiikey(nfc3d_amiibo_keys keys, char *keyfile) +{ if (!nfc3d_amiibo_load_keys(&keys, keyfile)) { PrintAndLogEx(ERR, "Could not load keys from '%s'", keyfile); @@ -41,13 +43,14 @@ static bool LoadAmiikey(nfc3d_amiibo_keys keys, char* keyfile) { return true; } -int main(int argc, char ** argv) { +int main(int argc, char **argv) +{ self = argv[0]; - char * infile = NULL; - char * savefile = NULL; - char * outfile = NULL; - char * keyfile = NULL; + char *infile = NULL; + char *savefile = NULL; + char *outfile = NULL; + char *keyfile = NULL; char op = '\0'; bool lenient = false; @@ -88,7 +91,7 @@ int main(int argc, char ** argv) { uint8_t original[NTAG215_SIZE]; uint8_t modified[NFC3D_AMIIBO_SIZE]; - FILE * f = stdin; + FILE *f = stdin; if (infile) { f = fopen(infile, "rb"); if (!f) { diff --git a/client/amiitool/drbg.c b/client/amiitool/drbg.c index aaf0acef5..e350ffdf3 100644 --- a/client/amiitool/drbg.c +++ b/client/amiitool/drbg.c @@ -10,7 +10,8 @@ #include #include -void nfc3d_drbg_init(nfc3d_drbg_ctx * ctx, const uint8_t * hmacKey, size_t hmacKeySize, const uint8_t * seed, size_t seedSize) { +void nfc3d_drbg_init(nfc3d_drbg_ctx *ctx, const uint8_t *hmacKey, size_t hmacKeySize, const uint8_t *seed, size_t seedSize) +{ assert(ctx != NULL); assert(hmacKey != NULL); assert(seed != NULL); @@ -30,7 +31,8 @@ void nfc3d_drbg_init(nfc3d_drbg_ctx * ctx, const uint8_t * hmacKey, size_t hmacK mbedtls_md_hmac_starts(&ctx->hmacCtx, hmacKey, hmacKeySize); } -void nfc3d_drbg_step(nfc3d_drbg_ctx * ctx, uint8_t * output) { +void nfc3d_drbg_step(nfc3d_drbg_ctx *ctx, uint8_t *output) +{ assert(ctx != NULL); assert(output != NULL); @@ -51,12 +53,14 @@ void nfc3d_drbg_step(nfc3d_drbg_ctx * ctx, uint8_t * output) { mbedtls_md_hmac_finish(&ctx->hmacCtx, output); } -void nfc3d_drbg_cleanup(nfc3d_drbg_ctx * ctx) { +void nfc3d_drbg_cleanup(nfc3d_drbg_ctx *ctx) +{ assert(ctx != NULL); mbedtls_md_free(&ctx->hmacCtx); } -void nfc3d_drbg_generate_bytes(const uint8_t * hmacKey, size_t hmacKeySize, const uint8_t * seed, size_t seedSize, uint8_t * output, size_t outputSize) { +void nfc3d_drbg_generate_bytes(const uint8_t *hmacKey, size_t hmacKeySize, const uint8_t *seed, size_t seedSize, uint8_t *output, size_t outputSize) +{ uint8_t temp[NFC3D_DRBG_OUTPUT_SIZE]; nfc3d_drbg_ctx rngCtx; diff --git a/client/amiitool/drbg.h b/client/amiitool/drbg.h index 7b5bddc86..d77d030ac 100644 --- a/client/amiitool/drbg.h +++ b/client/amiitool/drbg.h @@ -24,10 +24,10 @@ typedef struct { size_t bufferSize; } nfc3d_drbg_ctx; -void nfc3d_drbg_init(nfc3d_drbg_ctx * ctx, const uint8_t * hmacKey, size_t hmacKeySize, const uint8_t * seed, size_t seedSize); -void nfc3d_drbg_step(nfc3d_drbg_ctx * ctx, uint8_t * output); -void nfc3d_drbg_cleanup(nfc3d_drbg_ctx * ctx); -void nfc3d_drbg_generate_bytes(const uint8_t * hmacKey, size_t hmacKeySize, const uint8_t * seed, size_t seedSize, uint8_t * output, size_t outputSize); +void nfc3d_drbg_init(nfc3d_drbg_ctx *ctx, const uint8_t *hmacKey, size_t hmacKeySize, const uint8_t *seed, size_t seedSize); +void nfc3d_drbg_step(nfc3d_drbg_ctx *ctx, uint8_t *output); +void nfc3d_drbg_cleanup(nfc3d_drbg_ctx *ctx); +void nfc3d_drbg_generate_bytes(const uint8_t *hmacKey, size_t hmacKeySize, const uint8_t *seed, size_t seedSize, uint8_t *output, size_t outputSize); #endif diff --git a/client/amiitool/keygen.c b/client/amiitool/keygen.c index 1c0a91344..52f9c0069 100644 --- a/client/amiitool/keygen.c +++ b/client/amiitool/keygen.c @@ -10,13 +10,14 @@ #include #include -void nfc3d_keygen_prepare_seed(const nfc3d_keygen_masterkeys * baseKeys, const uint8_t * baseSeed, uint8_t * output, size_t * outputSize) { +void nfc3d_keygen_prepare_seed(const nfc3d_keygen_masterkeys *baseKeys, const uint8_t *baseSeed, uint8_t *output, size_t *outputSize) +{ assert(baseKeys != NULL); assert(baseSeed != NULL); assert(output != NULL); assert(outputSize != NULL); - uint8_t * start = output; + uint8_t *start = output; // 1: Copy whole type string output = memccpy(output, baseKeys->typeString, '\0', sizeof(baseKeys->typeString)); @@ -44,7 +45,8 @@ void nfc3d_keygen_prepare_seed(const nfc3d_keygen_masterkeys * baseKeys, const u *outputSize = output - start; } -void nfc3d_keygen(const nfc3d_keygen_masterkeys * baseKeys, const uint8_t * baseSeed, nfc3d_keygen_derivedkeys * derivedKeys) { +void nfc3d_keygen(const nfc3d_keygen_masterkeys *baseKeys, const uint8_t *baseSeed, nfc3d_keygen_derivedkeys *derivedKeys) +{ uint8_t preparedSeed[NFC3D_DRBG_MAX_SEED_SIZE]; size_t preparedSeedSize; diff --git a/client/amiitool/keygen.h b/client/amiitool/keygen.h index 5e371074e..4d1c21cea 100644 --- a/client/amiitool/keygen.h +++ b/client/amiitool/keygen.h @@ -29,6 +29,6 @@ typedef struct { } nfc3d_keygen_derivedkeys; #pragma pack() -void nfc3d_keygen(const nfc3d_keygen_masterkeys * baseKeys, const uint8_t * baseSeed, nfc3d_keygen_derivedkeys * derivedKeys); +void nfc3d_keygen(const nfc3d_keygen_masterkeys *baseKeys, const uint8_t *baseSeed, nfc3d_keygen_derivedkeys *derivedKeys); #endif diff --git a/client/cli.c b/client/cli.c index e7f8324ca..66c24b6db 100644 --- a/client/cli.c +++ b/client/cli.c @@ -13,46 +13,45 @@ #include "cmdmain.h" #define HANDLE_ERROR if (error_occured) { \ - error_occured = 0;\ - break;\ -} + error_occured = 0;\ + break;\ + } int main(int argc, char **argv) { - if (argc != 3 && argc != 4) - { - printf("\n\tusage: cli [logfile (default cli.log)]\n"); - printf("\n"); - printf("\texample: cli hi14asnoop hi14alist h14a.log\n"); - printf("\n"); - return -1; - } - - usb_init(); - if (argc == 4) - SetLogFilename(argv[3]); - else - SetLogFilename("cli.log"); - - return_on_error = 1; - - while (1) { - while (!OpenProxmark()) { sleep(1); } - while (1) { - UsbCommand cmdbuf; - CommandReceived(argv[1]); - HANDLE_ERROR; - ReceiveCommand(&cmdbuf); - HANDLE_ERROR; - for (int i = 0; i < 5; ++i) { - ReceiveCommandPoll(&cmdbuf); - } - HANDLE_ERROR; - CommandReceived(argv[2]); - HANDLE_ERROR; + if (argc != 3 && argc != 4) { + printf("\n\tusage: cli [logfile (default cli.log)]\n"); + printf("\n"); + printf("\texample: cli hi14asnoop hi14alist h14a.log\n"); + printf("\n"); + return -1; } - } - CloseProxmark(); - return 0; + usb_init(); + if (argc == 4) + SetLogFilename(argv[3]); + else + SetLogFilename("cli.log"); + + return_on_error = 1; + + while (1) { + while (!OpenProxmark()) { sleep(1); } + while (1) { + UsbCommand cmdbuf; + CommandReceived(argv[1]); + HANDLE_ERROR; + ReceiveCommand(&cmdbuf); + HANDLE_ERROR; + for (int i = 0; i < 5; ++i) { + ReceiveCommandPoll(&cmdbuf); + } + HANDLE_ERROR; + CommandReceived(argv[2]); + HANDLE_ERROR; + } + } + + CloseProxmark(); + return 0; } diff --git a/client/cliparser/argtable3.c b/client/cliparser/argtable3.c index 63e847be1..a982c4084 100644 --- a/client/cliparser/argtable3.c +++ b/client/cliparser/argtable3.c @@ -34,9 +34,9 @@ // so you have to use _istspace instead of space #ifdef UNICODE #include - #define ISSPACE _istspace +#define ISSPACE _istspace #else - #define ISSPACE isspace +#define ISSPACE isspace #endif /******************************************************************************* @@ -79,8 +79,7 @@ extern "C" { #endif -enum -{ +enum { EMINCOUNT = 1, EMAXCOUNT, EBADINT, @@ -101,13 +100,13 @@ enum __pragma(warning(push)) \ __pragma(warning(disable:4127)) \ do { if (ARG_ENABLE_TRACE) dbg_printf x; } while (0) \ - __pragma(warning(pop)) + __pragma(warning(pop)) #define ARG_LOG(x) \ __pragma(warning(push)) \ __pragma(warning(disable:4127)) \ do { if (ARG_ENABLE_LOG) dbg_printf x; } while (0) \ - __pragma(warning(pop)) + __pragma(warning(pop)) #else #define ARG_TRACE(x) \ do { if (ARG_ENABLE_TRACE) dbg_printf x; } while (0) @@ -254,12 +253,12 @@ char *optarg; /* argument associated with option */ #define EMSG "" -static int getopt_internal(int, char * const *, const char *, - const struct option *, int *, int); -static int parse_long_options(char * const *, const char *, - const struct option *, int *, int); +static int getopt_internal(int, char *const *, const char *, + const struct option *, int *, int); +static int parse_long_options(char *const *, const char *, + const struct option *, int *, int); static int gcd(int, int); -static void permute_args(int, int, int, char * const *); +static void permute_args(int, int, int, char *const *); static char *place = EMSG; /* option letter processing */ @@ -344,7 +343,7 @@ gcd(int a, int b) */ static void permute_args(int panonopt_start, int panonopt_end, int opt_end, - char * const *nargv) + char *const *nargv) { int cstart, cyclelen, i, j, ncycle, nnonopts, nopts, pos; char *swap; @@ -358,7 +357,7 @@ permute_args(int panonopt_start, int panonopt_end, int opt_end, cyclelen = (opt_end - panonopt_start) / ncycle; for (i = 0; i < ncycle; i++) { - cstart = panonopt_end+i; + cstart = panonopt_end + i; pos = cstart; for (j = 0; j < cyclelen; j++) { if (pos >= panonopt_end) @@ -380,8 +379,8 @@ permute_args(int panonopt_start, int panonopt_end, int opt_end, * Returns -1 if short_too is set and the option does not match long_options. */ static int -parse_long_options(char * const *nargv, const char *options, - const struct option *long_options, int *idx, int short_too) +parse_long_options(char *const *nargv, const char *options, + const struct option *long_options, int *idx, int short_too) { char *current_argv, *has_equal; size_t current_argv_len; @@ -402,7 +401,7 @@ parse_long_options(char * const *nargv, const char *options, for (i = 0; long_options[i].name; i++) { /* find matching long option */ if (strncmp(current_argv, long_options[i].name, - current_argv_len)) + current_argv_len)) continue; if (strlen(long_options[i].name) == current_argv_len) { @@ -423,7 +422,7 @@ parse_long_options(char * const *nargv, const char *options, /* ambiguous abbreviation */ if (PRINT_ERROR) warnx(ambig, (int)current_argv_len, - current_argv); + current_argv); optopt = 0; return (BADCH); } @@ -433,7 +432,7 @@ parse_long_options(char * const *nargv, const char *options, && has_equal) { if (PRINT_ERROR) warnx(noarg, (int)current_argv_len, - current_argv); + current_argv); /* * XXX: GNU sets optopt to val regardless of flag */ @@ -448,7 +447,7 @@ parse_long_options(char * const *nargv, const char *options, if (has_equal) optarg = has_equal; else if (long_options[match].has_arg == - required_argument) { + required_argument) { /* * optional argument doesn't use next nargv */ @@ -463,7 +462,7 @@ parse_long_options(char * const *nargv, const char *options, */ if (PRINT_ERROR) warnx(recargstring, - current_argv); + current_argv); /* * XXX: GNU sets optopt to val regardless of flag */ @@ -498,14 +497,14 @@ parse_long_options(char * const *nargv, const char *options, * Parse argc/argv argument vector. Called by user level routines. */ static int -getopt_internal(int nargc, char * const *nargv, const char *options, - const struct option *long_options, int *idx, int flags) +getopt_internal(int nargc, char *const *nargv, const char *options, + const struct option *long_options, int *idx, int flags) { char *oli; /* option letter list index */ int optchar, short_too; static int posixly_correct = -1; #ifdef __STDC_WANT_SECURE_LIB__ - char* buffer = NULL; + char *buffer = NULL; size_t buffer_size = 0; errno_t err = 0; #endif @@ -522,7 +521,7 @@ getopt_internal(int nargc, char * const *nargv, const char *options, if (posixly_correct == -1) { err = _dupenv_s(&buffer, &buffer_size, "POSIXLY_CORRECT") == 0; posixly_correct = buffer != NULL; - if(buffer != NULL && err == 0) { + if (buffer != NULL && err == 0) { free(buffer); } } @@ -555,10 +554,9 @@ start: if (nonopt_end != -1) { /* do permutation, if we have to */ permute_args(nonopt_start, nonopt_end, - optind, nargv); + optind, nargv); optind -= nonopt_end - nonopt_start; - } - else if (nonopt_start != -1) { + } else if (nonopt_start != -1) { /* * If we skipped non-options, set optind * to the first of them. @@ -591,9 +589,9 @@ start: nonopt_start = optind; else if (nonopt_end != -1) { permute_args(nonopt_start, nonopt_end, - optind, nargv); + optind, nargv); nonopt_start = optind - - (nonopt_end - nonopt_start); + (nonopt_end - nonopt_start); nonopt_end = -1; } optind++; @@ -615,7 +613,7 @@ start: */ if (nonopt_end != -1) { permute_args(nonopt_start, nonopt_end, - optind, nargv); + optind, nargv); optind -= nonopt_end - nonopt_start; } nonopt_start = nonopt_end = -1; @@ -638,14 +636,14 @@ start: short_too = 1; /* could be short option too */ optchar = parse_long_options(nargv, options, long_options, - idx, short_too); + idx, short_too); if (optchar != -1) { place = EMSG; return (optchar); } } - if ((optchar = (int)*place++) == (int)':' || + if ((optchar = (int) * place++) == (int)':' || (optchar == (int)'-' && *place != '\0') || (oli = strchr(options, optchar)) == NULL) { /* @@ -675,7 +673,7 @@ start: } else /* white space */ place = nargv[optind]; optchar = parse_long_options(nargv, options, long_options, - idx, 0); + idx, 0); place = EMSG; return (optchar); } @@ -711,7 +709,7 @@ start: * [eventually this will replace the BSD getopt] */ int -getopt(int nargc, char * const *nargv, const char *options) +getopt(int nargc, char *const *nargv, const char *options) { /* @@ -731,12 +729,12 @@ getopt(int nargc, char * const *nargv, const char *options) * Parse argc/argv argument vector. */ int -getopt_long(int nargc, char * const *nargv, const char *options, - const struct option *long_options, int *idx) +getopt_long(int nargc, char *const *nargv, const char *options, + const struct option *long_options, int *idx) { return (getopt_internal(nargc, nargv, options, long_options, idx, - FLAG_PERMUTE)); + FLAG_PERMUTE)); } /* @@ -744,12 +742,12 @@ getopt_long(int nargc, char * const *nargv, const char *options, * Parse argc/argv argument vector. */ int -getopt_long_only(int nargc, char * const *nargv, const char *options, - const struct option *long_options, int *idx) +getopt_long_only(int nargc, char *const *nargv, const char *options, + const struct option *long_options, int *idx) { return (getopt_internal(nargc, nargv, options, long_options, idx, - FLAG_PERMUTE|FLAG_LONGONLY)); + FLAG_PERMUTE | FLAG_LONGONLY)); } /******************************************************************************* * This file is part of the argtable3 library. @@ -787,7 +785,7 @@ getopt_long_only(int nargc, char * const *nargv, const char *options, #include "argtable3.h" -char * arg_strptime(const char *buf, const char *fmt, struct tm *tm); +char *arg_strptime(const char *buf, const char *fmt, struct tm *tm); static void arg_date_resetfn(struct arg_date *parent) @@ -801,17 +799,12 @@ static int arg_date_scanfn(struct arg_date *parent, const char *argval) { int errorcode = 0; - if (parent->count == parent->hdr.maxcount) - { + if (parent->count == parent->hdr.maxcount) { errorcode = EMAXCOUNT; - } - else if (!argval) - { + } else if (!argval) { /* no argument value was given, leave parent->tmval[] unaltered but still count it */ parent->count++; - } - else - { + } else { const char *pend; struct tm tm = parent->tmval[parent->count]; @@ -852,38 +845,36 @@ static void arg_date_errorfn( argval = argval ? argval : ""; fprintf(fp, "%s: ", progname); - switch(errorcode) - { - case EMINCOUNT: - fputs("missing option ", fp); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + switch (errorcode) { + case EMINCOUNT: + fputs("missing option ", fp); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; - case EMAXCOUNT: - fputs("excess option ", fp); - arg_print_option(fp, shortopts, longopts, argval, "\n"); - break; + case EMAXCOUNT: + fputs("excess option ", fp); + arg_print_option(fp, shortopts, longopts, argval, "\n"); + break; - case EBADDATE: - { - struct tm tm; - char buff[200]; + case EBADDATE: { + struct tm tm; + char buff[200]; - fprintf(fp, "illegal timestamp format \"%s\"\n", argval); - memset(&tm, 0, sizeof(tm)); - arg_strptime("1999-12-31 23:59:59", "%F %H:%M:%S", &tm); - strftime(buff, sizeof(buff), parent->format, &tm); - printf("correct format is \"%s\"\n", buff); - break; - } + fprintf(fp, "illegal timestamp format \"%s\"\n", argval); + memset(&tm, 0, sizeof(tm)); + arg_strptime("1999-12-31 23:59:59", "%F %H:%M:%S", &tm); + strftime(buff, sizeof(buff), parent->format, &tm); + printf("correct format is \"%s\"\n", buff); + break; + } } } -struct arg_date * arg_date0( - const char * shortopts, - const char * longopts, - const char * format, +struct arg_date *arg_date0( + const char *shortopts, + const char *longopts, + const char *format, const char *datatype, const char *glossary) { @@ -891,10 +882,10 @@ struct arg_date * arg_date0( } -struct arg_date * arg_date1( - const char * shortopts, - const char * longopts, - const char * format, +struct arg_date *arg_date1( + const char *shortopts, + const char *longopts, + const char *format, const char *datatype, const char *glossary) { @@ -902,10 +893,10 @@ struct arg_date * arg_date1( } -struct arg_date * arg_daten( - const char * shortopts, - const char * longopts, - const char * format, +struct arg_date *arg_daten( + const char *shortopts, + const char *longopts, + const char *format, const char *datatype, int mincount, int maxcount, @@ -922,13 +913,12 @@ struct arg_date * arg_daten( format = "%x"; nbytes = sizeof(struct arg_date) /* storage for struct arg_date */ - + maxcount * sizeof(struct tm); /* storage for tmval[maxcount] array */ + + maxcount * sizeof(struct tm); /* storage for tmval[maxcount] array */ /* allocate storage for the arg_date struct + tmval[] array. */ /* we use calloc because we want the tmval[] array zero filled. */ result = (struct arg_date *)calloc(1, nbytes); - if (result) - { + if (result) { /* init the arg_hdr struct */ result->hdr.flag = ARG_HASVALUE; result->hdr.shortopts = shortopts; @@ -1038,12 +1028,10 @@ static int arg_strcasecmp(const char *s1, const char *s2) static int arg_strncasecmp(const char *s1, const char *s2, size_t n) { - if (n != 0) - { + if (n != 0) { const unsigned char *us1 = (const unsigned char *)s1; const unsigned char *us2 = (const unsigned char *)s2; - do - { + do { if (tolower(*us1) != tolower(*us2++)) return tolower(*us1) - tolower(*--us2); @@ -1056,7 +1044,7 @@ static int arg_strncasecmp(const char *s1, const char *s2, size_t n) } -char * arg_strptime(const char *buf, const char *fmt, struct tm *tm) +char *arg_strptime(const char *buf, const char *fmt, struct tm *tm) { char c; const char *bp; @@ -1083,275 +1071,274 @@ char * arg_strptime(const char *buf, const char *fmt, struct tm *tm) again: - switch (c = *fmt++) - { - case '%': /* "%%" is converted to "%". */ + switch (c = *fmt++) { + case '%': /* "%%" is converted to "%". */ literal: - if (c != *bp++) - return (0); - break; - - /* - * "Alternative" modifiers. Just set the appropriate flag - * and start over again. - */ - case 'E': /* "%E?" alternative conversion modifier. */ - LEGAL_ALT(0); - alt_format |= ALT_E; - goto again; - - case 'O': /* "%O?" alternative conversion modifier. */ - LEGAL_ALT(0); - alt_format |= ALT_O; - goto again; - - /* - * "Complex" conversion rules, implemented through recursion. - */ - case 'c': /* Date and time, using the locale's format. */ - LEGAL_ALT(ALT_E); - bp = arg_strptime(bp, "%x %X", tm); - if (!bp) - return (0); - break; - - case 'D': /* The date as "%m/%d/%y". */ - LEGAL_ALT(0); - bp = arg_strptime(bp, "%m/%d/%y", tm); - if (!bp) - return (0); - break; - - case 'R': /* The time as "%H:%M". */ - LEGAL_ALT(0); - bp = arg_strptime(bp, "%H:%M", tm); - if (!bp) - return (0); - break; - - case 'r': /* The time in 12-hour clock representation. */ - LEGAL_ALT(0); - bp = arg_strptime(bp, "%I:%M:%S %p", tm); - if (!bp) - return (0); - break; - - case 'T': /* The time as "%H:%M:%S". */ - LEGAL_ALT(0); - bp = arg_strptime(bp, "%H:%M:%S", tm); - if (!bp) - return (0); - break; - - case 'X': /* The time, using the locale's format. */ - LEGAL_ALT(ALT_E); - bp = arg_strptime(bp, "%H:%M:%S", tm); - if (!bp) - return (0); - break; - - case 'x': /* The date, using the locale's format. */ - LEGAL_ALT(ALT_E); - bp = arg_strptime(bp, "%m/%d/%y", tm); - if (!bp) - return (0); - break; - - /* - * "Elementary" conversion rules. - */ - case 'A': /* The day of week, using the locale's form. */ - case 'a': - LEGAL_ALT(0); - for (i = 0; i < 7; i++) { - /* Full name. */ - len = strlen(day[i]); - if (arg_strncasecmp(day[i], bp, len) == 0) - break; - - /* Abbreviated name. */ - len = strlen(abday[i]); - if (arg_strncasecmp(abday[i], bp, len) == 0) - break; - } - - /* Nothing matched. */ - if (i == 7) - return (0); - - tm->tm_wday = i; - bp += len; - break; - - case 'B': /* The month, using the locale's form. */ - case 'b': - case 'h': - LEGAL_ALT(0); - for (i = 0; i < 12; i++) { - /* Full name. */ - len = strlen(mon[i]); - if (arg_strncasecmp(mon[i], bp, len) == 0) - break; - - /* Abbreviated name. */ - len = strlen(abmon[i]); - if (arg_strncasecmp(abmon[i], bp, len) == 0) - break; - } - - /* Nothing matched. */ - if (i == 12) - return (0); - - tm->tm_mon = i; - bp += len; - break; - - case 'C': /* The century number. */ - LEGAL_ALT(ALT_E); - if (!(conv_num(&bp, &i, 0, 99))) - return (0); - - if (split_year) { - tm->tm_year = (tm->tm_year % 100) + (i * 100); - } else { - tm->tm_year = i * 100; - split_year = 1; - } - break; - - case 'd': /* The day of month. */ - case 'e': - LEGAL_ALT(ALT_O); - if (!(conv_num(&bp, &tm->tm_mday, 1, 31))) - return (0); - break; - - case 'k': /* The hour (24-hour clock representation). */ - LEGAL_ALT(0); - /* FALLTHROUGH */ - case 'H': - LEGAL_ALT(ALT_O); - if (!(conv_num(&bp, &tm->tm_hour, 0, 23))) - return (0); - break; - - case 'l': /* The hour (12-hour clock representation). */ - LEGAL_ALT(0); - /* FALLTHROUGH */ - case 'I': - LEGAL_ALT(ALT_O); - if (!(conv_num(&bp, &tm->tm_hour, 1, 12))) - return (0); - if (tm->tm_hour == 12) - tm->tm_hour = 0; - break; - - case 'j': /* The day of year. */ - LEGAL_ALT(0); - if (!(conv_num(&bp, &i, 1, 366))) - return (0); - tm->tm_yday = i - 1; - break; - - case 'M': /* The minute. */ - LEGAL_ALT(ALT_O); - if (!(conv_num(&bp, &tm->tm_min, 0, 59))) - return (0); - break; - - case 'm': /* The month. */ - LEGAL_ALT(ALT_O); - if (!(conv_num(&bp, &i, 1, 12))) - return (0); - tm->tm_mon = i - 1; - break; - - case 'p': /* The locale's equivalent of AM/PM. */ - LEGAL_ALT(0); - /* AM? */ - if (arg_strcasecmp(am_pm[0], bp) == 0) { - if (tm->tm_hour > 11) + if (c != *bp++) return (0); - - bp += strlen(am_pm[0]); break; - } - /* PM? */ - else if (arg_strcasecmp(am_pm[1], bp) == 0) { - if (tm->tm_hour > 11) - return (0); - tm->tm_hour += 12; - bp += strlen(am_pm[1]); - break; - } - - /* Nothing matched. */ - return (0); - - case 'S': /* The seconds. */ - LEGAL_ALT(ALT_O); - if (!(conv_num(&bp, &tm->tm_sec, 0, 61))) - return (0); - break; - - case 'U': /* The week of year, beginning on sunday. */ - case 'W': /* The week of year, beginning on monday. */ - LEGAL_ALT(ALT_O); /* - * XXX This is bogus, as we can not assume any valid - * information present in the tm structure at this - * point to calculate a real value, so just check the - * range for now. + * "Alternative" modifiers. Just set the appropriate flag + * and start over again. */ - if (!(conv_num(&bp, &i, 0, 53))) - return (0); - break; + case 'E': /* "%E?" alternative conversion modifier. */ + LEGAL_ALT(0); + alt_format |= ALT_E; + goto again; - case 'w': /* The day of week, beginning on sunday. */ - LEGAL_ALT(ALT_O); - if (!(conv_num(&bp, &tm->tm_wday, 0, 6))) - return (0); - break; + case 'O': /* "%O?" alternative conversion modifier. */ + LEGAL_ALT(0); + alt_format |= ALT_O; + goto again; - case 'Y': /* The year. */ - LEGAL_ALT(ALT_E); - if (!(conv_num(&bp, &i, 0, 9999))) - return (0); - - tm->tm_year = i - TM_YEAR_BASE; - break; - - case 'y': /* The year within 100 years of the epoch. */ - LEGAL_ALT(ALT_E | ALT_O); - if (!(conv_num(&bp, &i, 0, 99))) - return (0); - - if (split_year) { - tm->tm_year = ((tm->tm_year / 100) * 100) + i; + /* + * "Complex" conversion rules, implemented through recursion. + */ + case 'c': /* Date and time, using the locale's format. */ + LEGAL_ALT(ALT_E); + bp = arg_strptime(bp, "%x %X", tm); + if (!bp) + return (0); break; - } - split_year = 1; - if (i <= 68) - tm->tm_year = i + 2000 - TM_YEAR_BASE; - else - tm->tm_year = i + 1900 - TM_YEAR_BASE; - break; - /* - * Miscellaneous conversions. - */ - case 'n': /* Any kind of white-space. */ - case 't': - LEGAL_ALT(0); - while (ISSPACE(*bp)) - bp++; - break; + case 'D': /* The date as "%m/%d/%y". */ + LEGAL_ALT(0); + bp = arg_strptime(bp, "%m/%d/%y", tm); + if (!bp) + return (0); + break; + + case 'R': /* The time as "%H:%M". */ + LEGAL_ALT(0); + bp = arg_strptime(bp, "%H:%M", tm); + if (!bp) + return (0); + break; + + case 'r': /* The time in 12-hour clock representation. */ + LEGAL_ALT(0); + bp = arg_strptime(bp, "%I:%M:%S %p", tm); + if (!bp) + return (0); + break; + + case 'T': /* The time as "%H:%M:%S". */ + LEGAL_ALT(0); + bp = arg_strptime(bp, "%H:%M:%S", tm); + if (!bp) + return (0); + break; + + case 'X': /* The time, using the locale's format. */ + LEGAL_ALT(ALT_E); + bp = arg_strptime(bp, "%H:%M:%S", tm); + if (!bp) + return (0); + break; + + case 'x': /* The date, using the locale's format. */ + LEGAL_ALT(ALT_E); + bp = arg_strptime(bp, "%m/%d/%y", tm); + if (!bp) + return (0); + break; + + /* + * "Elementary" conversion rules. + */ + case 'A': /* The day of week, using the locale's form. */ + case 'a': + LEGAL_ALT(0); + for (i = 0; i < 7; i++) { + /* Full name. */ + len = strlen(day[i]); + if (arg_strncasecmp(day[i], bp, len) == 0) + break; + + /* Abbreviated name. */ + len = strlen(abday[i]); + if (arg_strncasecmp(abday[i], bp, len) == 0) + break; + } + + /* Nothing matched. */ + if (i == 7) + return (0); + + tm->tm_wday = i; + bp += len; + break; + + case 'B': /* The month, using the locale's form. */ + case 'b': + case 'h': + LEGAL_ALT(0); + for (i = 0; i < 12; i++) { + /* Full name. */ + len = strlen(mon[i]); + if (arg_strncasecmp(mon[i], bp, len) == 0) + break; + + /* Abbreviated name. */ + len = strlen(abmon[i]); + if (arg_strncasecmp(abmon[i], bp, len) == 0) + break; + } + + /* Nothing matched. */ + if (i == 12) + return (0); + + tm->tm_mon = i; + bp += len; + break; + + case 'C': /* The century number. */ + LEGAL_ALT(ALT_E); + if (!(conv_num(&bp, &i, 0, 99))) + return (0); + + if (split_year) { + tm->tm_year = (tm->tm_year % 100) + (i * 100); + } else { + tm->tm_year = i * 100; + split_year = 1; + } + break; + + case 'd': /* The day of month. */ + case 'e': + LEGAL_ALT(ALT_O); + if (!(conv_num(&bp, &tm->tm_mday, 1, 31))) + return (0); + break; + + case 'k': /* The hour (24-hour clock representation). */ + LEGAL_ALT(0); + /* FALLTHROUGH */ + case 'H': + LEGAL_ALT(ALT_O); + if (!(conv_num(&bp, &tm->tm_hour, 0, 23))) + return (0); + break; + + case 'l': /* The hour (12-hour clock representation). */ + LEGAL_ALT(0); + /* FALLTHROUGH */ + case 'I': + LEGAL_ALT(ALT_O); + if (!(conv_num(&bp, &tm->tm_hour, 1, 12))) + return (0); + if (tm->tm_hour == 12) + tm->tm_hour = 0; + break; + + case 'j': /* The day of year. */ + LEGAL_ALT(0); + if (!(conv_num(&bp, &i, 1, 366))) + return (0); + tm->tm_yday = i - 1; + break; + + case 'M': /* The minute. */ + LEGAL_ALT(ALT_O); + if (!(conv_num(&bp, &tm->tm_min, 0, 59))) + return (0); + break; + + case 'm': /* The month. */ + LEGAL_ALT(ALT_O); + if (!(conv_num(&bp, &i, 1, 12))) + return (0); + tm->tm_mon = i - 1; + break; + + case 'p': /* The locale's equivalent of AM/PM. */ + LEGAL_ALT(0); + /* AM? */ + if (arg_strcasecmp(am_pm[0], bp) == 0) { + if (tm->tm_hour > 11) + return (0); + + bp += strlen(am_pm[0]); + break; + } + /* PM? */ + else if (arg_strcasecmp(am_pm[1], bp) == 0) { + if (tm->tm_hour > 11) + return (0); + + tm->tm_hour += 12; + bp += strlen(am_pm[1]); + break; + } + + /* Nothing matched. */ + return (0); + + case 'S': /* The seconds. */ + LEGAL_ALT(ALT_O); + if (!(conv_num(&bp, &tm->tm_sec, 0, 61))) + return (0); + break; + + case 'U': /* The week of year, beginning on sunday. */ + case 'W': /* The week of year, beginning on monday. */ + LEGAL_ALT(ALT_O); + /* + * XXX This is bogus, as we can not assume any valid + * information present in the tm structure at this + * point to calculate a real value, so just check the + * range for now. + */ + if (!(conv_num(&bp, &i, 0, 53))) + return (0); + break; + + case 'w': /* The day of week, beginning on sunday. */ + LEGAL_ALT(ALT_O); + if (!(conv_num(&bp, &tm->tm_wday, 0, 6))) + return (0); + break; + + case 'Y': /* The year. */ + LEGAL_ALT(ALT_E); + if (!(conv_num(&bp, &i, 0, 9999))) + return (0); + + tm->tm_year = i - TM_YEAR_BASE; + break; + + case 'y': /* The year within 100 years of the epoch. */ + LEGAL_ALT(ALT_E | ALT_O); + if (!(conv_num(&bp, &i, 0, 99))) + return (0); + + if (split_year) { + tm->tm_year = ((tm->tm_year / 100) * 100) + i; + break; + } + split_year = 1; + if (i <= 68) + tm->tm_year = i + 2000 - TM_YEAR_BASE; + else + tm->tm_year = i + 1900 - TM_YEAR_BASE; + break; + + /* + * Miscellaneous conversions. + */ + case 'n': /* Any kind of white-space. */ + case 't': + LEGAL_ALT(0); + while (ISSPACE(*bp)) + bp++; + break; - default: /* Unknown/unsupported conversion. */ - return (0); + default: /* Unknown/unsupported conversion. */ + return (0); } @@ -1430,20 +1417,15 @@ static int arg_dbl_scanfn(struct arg_dbl *parent, const char *argval) { int errorcode = 0; - if (parent->count == parent->hdr.maxcount) - { + if (parent->count == parent->hdr.maxcount) { /* maximum number of arguments exceeded */ errorcode = EMAXCOUNT; - } - else if (!argval) - { + } else if (!argval) { /* a valid argument with no argument value was given. */ /* This happens when an optional argument value was invoked. */ /* leave parent argument value unaltered but still count the argument. */ parent->count++; - } - else - { + } else { double val; char *end; @@ -1486,29 +1468,28 @@ static void arg_dbl_errorfn( argval = argval ? argval : ""; fprintf(fp, "%s: ", progname); - switch(errorcode) - { - case EMINCOUNT: - fputs("missing option ", fp); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + switch (errorcode) { + case EMINCOUNT: + fputs("missing option ", fp); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; - case EMAXCOUNT: - fputs("excess option ", fp); - arg_print_option(fp, shortopts, longopts, argval, "\n"); - break; + case EMAXCOUNT: + fputs("excess option ", fp); + arg_print_option(fp, shortopts, longopts, argval, "\n"); + break; - case EBADDOUBLE: - fprintf(fp, "invalid argument \"%s\" to option ", argval); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + case EBADDOUBLE: + fprintf(fp, "invalid argument \"%s\" to option ", argval); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; } } -struct arg_dbl * arg_dbl0( - const char * shortopts, - const char * longopts, +struct arg_dbl *arg_dbl0( + const char *shortopts, + const char *longopts, const char *datatype, const char *glossary) { @@ -1516,9 +1497,9 @@ struct arg_dbl * arg_dbl0( } -struct arg_dbl * arg_dbl1( - const char * shortopts, - const char * longopts, +struct arg_dbl *arg_dbl1( + const char *shortopts, + const char *longopts, const char *datatype, const char *glossary) { @@ -1526,9 +1507,9 @@ struct arg_dbl * arg_dbl1( } -struct arg_dbl * arg_dbln( - const char * shortopts, - const char * longopts, +struct arg_dbl *arg_dbln( + const char *shortopts, + const char *longopts, const char *datatype, int mincount, int maxcount, @@ -1544,8 +1525,7 @@ struct arg_dbl * arg_dbln( + (maxcount + 1) * sizeof(double); /* storage for dval[maxcount] array plus one extra for padding to memory boundary */ result = (struct arg_dbl *)malloc(nbytes); - if (result) - { + if (result) { size_t addr; size_t rem; @@ -1634,33 +1614,32 @@ static void arg_end_errorfn( argval = argval ? argval : ""; fprintf(fp, "%s: ", progname); - switch(error) - { - case ARG_ELIMIT: - fputs("too many errors to display", fp); - break; - case ARG_EMALLOC: - fputs("insufficent memory", fp); - break; - case ARG_ENOMATCH: - fprintf(fp, "unexpected argument \"%s\"", argval); - break; - case ARG_EMISSARG: - fprintf(fp, "option \"%s\" requires an argument", argval); - break; - case ARG_ELONGOPT: - fprintf(fp, "invalid option \"%s\"", argval); - break; - default: - fprintf(fp, "invalid option \"-%c\"", error); - break; + switch (error) { + case ARG_ELIMIT: + fputs("too many errors to display", fp); + break; + case ARG_EMALLOC: + fputs("insufficent memory", fp); + break; + case ARG_ENOMATCH: + fprintf(fp, "unexpected argument \"%s\"", argval); + break; + case ARG_EMISSARG: + fprintf(fp, "option \"%s\" requires an argument", argval); + break; + case ARG_ELONGOPT: + fprintf(fp, "invalid option \"%s\"", argval); + break; + default: + fprintf(fp, "invalid option \"-%c\"", error); + break; } fputc('\n', fp); } -struct arg_end * arg_end(int maxcount) +struct arg_end *arg_end(int maxcount) { size_t nbytes; struct arg_end *result; @@ -1671,8 +1650,7 @@ struct arg_end * arg_end(int maxcount) + maxcount * sizeof(char *); /* storage for char* argval[maxcount] array */ result = (struct arg_end *)malloc(nbytes); - if (result) - { + if (result) { /* init the arg_hdr struct */ result->hdr.flag = ARG_TERMINATOR; result->hdr.shortopts = NULL; @@ -1691,10 +1669,10 @@ struct arg_end * arg_end(int maxcount) result->error = (int *)(result + 1); /* store parent[maxcount] array immediately after error[] array */ - result->parent = (void * *)(result->error + maxcount ); + result->parent = (void * *)(result->error + maxcount); /* store argval[maxcount] array immediately after parent[] array */ - result->argval = (const char * *)(result->parent + maxcount ); + result->argval = (const char * *)(result->parent + maxcount); } ARG_TRACE(("arg_end(%d) returns %p\n", maxcount, result)); @@ -1702,12 +1680,11 @@ struct arg_end * arg_end(int maxcount) } -void arg_print_errors(FILE * fp, struct arg_end * end, const char * progname) +void arg_print_errors(FILE *fp, struct arg_end *end, const char *progname) { int i; ARG_TRACE(("arg_errors()\n")); - for (i = 0; i < end->count; i++) - { + for (i = 0; i < end->count; i++) { struct arg_hdr *errorparent = (struct arg_hdr *)(end->parent[i]); if (errorparent->errorfn) errorparent->errorfn(end->parent[i], @@ -1769,7 +1746,7 @@ static void arg_file_resetfn(struct arg_file *parent) /* Returns ptr to the base filename within *filename */ -static const char * arg_basename(const char *filename) +static const char *arg_basename(const char *filename) { const char *result = NULL, *result1, *result2; @@ -1789,7 +1766,7 @@ static const char * arg_basename(const char *filename) result = filename; /* neither file separator was found so basename is the whole filename */ /* special cases of "." and ".." are not considered basenames */ - if (result && ( strcmp(".", result) == 0 || strcmp("..", result) == 0 )) + if (result && (strcmp(".", result) == 0 || strcmp("..", result) == 0)) result = filename + strlen(filename); return result; @@ -1797,7 +1774,7 @@ static const char * arg_basename(const char *filename) /* Returns ptr to the file extension within *basename */ -static const char * arg_extension(const char *basename) +static const char *arg_extension(const char *basename) { /* find the last occurrence of '.' in basename */ const char *result = (basename ? strrchr(basename, '.') : NULL); @@ -1822,20 +1799,15 @@ static int arg_file_scanfn(struct arg_file *parent, const char *argval) { int errorcode = 0; - if (parent->count == parent->hdr.maxcount) - { + if (parent->count == parent->hdr.maxcount) { /* maximum number of arguments exceeded */ errorcode = EMAXCOUNT; - } - else if (!argval) - { + } else if (!argval) { /* a valid argument with no argument value was given. */ /* This happens when an optional argument value was invoked. */ /* leave parent arguiment value unaltered but still count the argument. */ parent->count++; - } - else - { + } else { parent->filename[parent->count] = argval; parent->basename[parent->count] = arg_basename(argval); parent->extension[parent->count] = @@ -1872,27 +1844,26 @@ static void arg_file_errorfn( argval = argval ? argval : ""; fprintf(fp, "%s: ", progname); - switch(errorcode) - { - case EMINCOUNT: - fputs("missing option ", fp); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + switch (errorcode) { + case EMINCOUNT: + fputs("missing option ", fp); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; - case EMAXCOUNT: - fputs("excess option ", fp); - arg_print_option(fp, shortopts, longopts, argval, "\n"); - break; + case EMAXCOUNT: + fputs("excess option ", fp); + arg_print_option(fp, shortopts, longopts, argval, "\n"); + break; - default: - fprintf(fp, "unknown error at \"%s\"\n", argval); + default: + fprintf(fp, "unknown error at \"%s\"\n", argval); } } -struct arg_file * arg_file0( - const char * shortopts, - const char * longopts, +struct arg_file *arg_file0( + const char *shortopts, + const char *longopts, const char *datatype, const char *glossary) { @@ -1900,9 +1871,9 @@ struct arg_file * arg_file0( } -struct arg_file * arg_file1( - const char * shortopts, - const char * longopts, +struct arg_file *arg_file1( + const char *shortopts, + const char *longopts, const char *datatype, const char *glossary) { @@ -1910,9 +1881,9 @@ struct arg_file * arg_file1( } -struct arg_file * arg_filen( - const char * shortopts, - const char * longopts, +struct arg_file *arg_filen( + const char *shortopts, + const char *longopts, const char *datatype, int mincount, int maxcount, @@ -1930,8 +1901,7 @@ struct arg_file * arg_filen( + sizeof(char *) * maxcount; /* storage for extension[maxcount] array */ result = (struct arg_file *)malloc(nbytes); - if (result) - { + if (result) { int i; /* init the arg_hdr struct */ @@ -1955,8 +1925,7 @@ struct arg_file * arg_filen( result->count = 0; /* foolproof the string pointers by initialising them with empty strings */ - for (i = 0; i < maxcount; i++) - { + for (i = 0; i < maxcount; i++) { result->filename[i] = ""; result->basename[i] = ""; result->extension[i] = ""; @@ -2020,7 +1989,7 @@ static void arg_int_resetfn(struct arg_int *parent) /* eg: to parse oct str="+0o12324", specify X='O' and base=8. */ /* eg: to parse bin str="-0B01010", specify X='B' and base=2. */ /* Failure of conversion is indicated by result where *endptr==str. */ -static long int strtol0X(const char * str, +static long int strtol0X(const char *str, const char * *endptr, char X, int base) @@ -2035,32 +2004,29 @@ static long int strtol0X(const char * str, /* printf("1) %s\n",ptr); */ /* scan optional sign character */ - switch (*ptr) - { - case '+': - ptr++; - s = 1; - break; - case '-': - ptr++; - s = -1; - break; - default: - s = 1; - break; + switch (*ptr) { + case '+': + ptr++; + s = 1; + break; + case '-': + ptr++; + s = -1; + break; + default: + s = 1; + break; } /* printf("2) %s\n",ptr); */ /* '0X' prefix */ - if ((*ptr++) != '0') - { + if ((*ptr++) != '0') { /* printf("failed to detect '0'\n"); */ *endptr = str; return 0; } /* printf("3) %s\n",ptr); */ - if (toupper(*ptr++) != toupper(X)) - { + if (toupper(*ptr++) != toupper(X)) { /* printf("failed to detect '%c'\n",X); */ *endptr = str; return 0; @@ -2069,8 +2035,7 @@ static long int strtol0X(const char * str, /* attempt conversion on remainder of string using strtol() */ val = strtol(ptr, (char * *)endptr, base); - if (*endptr == ptr) - { + if (*endptr == ptr) { /* conversion failed */ *endptr = str; return 0; @@ -2086,8 +2051,7 @@ static long int strtol0X(const char * str, static int detectsuffix(const char *str, const char *suffix) { /* scan pairwise through strings until mismatch detected */ - while( toupper(*str) == toupper(*suffix) ) - { + while (toupper(*str) == toupper(*suffix)) { /* printf("'%c' '%c'\n", *str, *suffix); */ /* return 1 (success) if match persists until the string terminator */ @@ -2117,39 +2081,30 @@ static int arg_int_scanfn(struct arg_int *parent, const char *argval) { int errorcode = 0; - if (parent->count == parent->hdr.maxcount) - { + if (parent->count == parent->hdr.maxcount) { /* maximum number of arguments exceeded */ errorcode = EMAXCOUNT; - } - else if (!argval) - { + } else if (!argval) { /* a valid argument with no argument value was given. */ /* This happens when an optional argument value was invoked. */ /* leave parent arguiment value unaltered but still count the argument. */ parent->count++; - } - else - { + } else { long int val; const char *end; /* attempt to extract hex integer (eg: +0x123) from argval into val conversion */ val = strtol0X(argval, &end, 'X', 16); - if (end == argval) - { + if (end == argval) { /* hex failed, attempt octal conversion (eg +0o123) */ val = strtol0X(argval, &end, 'O', 8); - if (end == argval) - { + if (end == argval) { /* octal failed, attempt binary conversion (eg +0B101) */ val = strtol0X(argval, &end, 'B', 2); - if (end == argval) - { + if (end == argval) { /* binary failed, attempt decimal conversion with no prefix (eg 1234) */ val = strtol(argval, (char * *)&end, 10); - if (end == argval) - { + if (end == argval) { /* all supported number formats failed */ return EBADINT; } @@ -2159,7 +2114,7 @@ static int arg_int_scanfn(struct arg_int *parent, const char *argval) /* Safety check for integer overflow. WARNING: this check */ /* achieves nothing on machines where size(int)==size(long). */ - if ( val > INT_MAX || val < INT_MIN ) + if (val > INT_MAX || val < INT_MIN) #ifdef __STDC_WANT_SECURE_LIB__ errorcode = EOVERFLOW_; #else @@ -2168,9 +2123,8 @@ static int arg_int_scanfn(struct arg_int *parent, const char *argval) /* Detect any suffixes (KB,MB,GB) and multiply argument value appropriately. */ /* We need to be mindful of integer overflows when using such big numbers. */ - if (detectsuffix(end, "KB")) /* kilobytes */ - { - if ( val > (INT_MAX / 1024) || val < (INT_MIN / 1024) ) + if (detectsuffix(end, "KB")) { /* kilobytes */ + if (val > (INT_MAX / 1024) || val < (INT_MIN / 1024)) #ifdef __STDC_WANT_SECURE_LIB__ errorcode = EOVERFLOW_; /* Overflow would occur if we proceed */ #else @@ -2178,10 +2132,8 @@ static int arg_int_scanfn(struct arg_int *parent, const char *argval) #endif else val *= 1024; /* 1KB = 1024 */ - } - else if (detectsuffix(end, "MB")) /* megabytes */ - { - if ( val > (INT_MAX / 1048576) || val < (INT_MIN / 1048576) ) + } else if (detectsuffix(end, "MB")) { /* megabytes */ + if (val > (INT_MAX / 1048576) || val < (INT_MIN / 1048576)) #ifdef __STDC_WANT_SECURE_LIB__ errorcode = EOVERFLOW_; /* Overflow would occur if we proceed */ #else @@ -2189,10 +2141,8 @@ static int arg_int_scanfn(struct arg_int *parent, const char *argval) #endif else val *= 1048576; /* 1MB = 1024*1024 */ - } - else if (detectsuffix(end, "GB")) /* gigabytes */ - { - if ( val > (INT_MAX / 1073741824) || val < (INT_MIN / 1073741824) ) + } else if (detectsuffix(end, "GB")) { /* gigabytes */ + if (val > (INT_MAX / 1073741824) || val < (INT_MIN / 1073741824)) #ifdef __STDC_WANT_SECURE_LIB__ errorcode = EOVERFLOW_; /* Overflow would occur if we proceed */ #else @@ -2200,8 +2150,7 @@ static int arg_int_scanfn(struct arg_int *parent, const char *argval) #endif else val *= 1073741824; /* 1GB = 1024*1024*1024 */ - } - else if (!detectsuffix(end, "")) + } else if (!detectsuffix(end, "")) errorcode = EBADINT; /* invalid suffix detected */ /* if success then store result in parent->ival[] array */ @@ -2237,37 +2186,36 @@ static void arg_int_errorfn( argval = argval ? argval : ""; fprintf(fp, "%s: ", progname); - switch(errorcode) - { - case EMINCOUNT: - fputs("missing option ", fp); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + switch (errorcode) { + case EMINCOUNT: + fputs("missing option ", fp); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; - case EMAXCOUNT: - fputs("excess option ", fp); - arg_print_option(fp, shortopts, longopts, argval, "\n"); - break; + case EMAXCOUNT: + fputs("excess option ", fp); + arg_print_option(fp, shortopts, longopts, argval, "\n"); + break; - case EBADINT: - fprintf(fp, "invalid argument \"%s\" to option ", argval); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + case EBADINT: + fprintf(fp, "invalid argument \"%s\" to option ", argval); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; #ifdef __STDC_WANT_SECURE_LIB__ - case EOVERFLOW_: + case EOVERFLOW_: #else - case EOVERFLOW: + case EOVERFLOW: #endif - fputs("integer overflow at option ", fp); - arg_print_option(fp, shortopts, longopts, datatype, " "); - fprintf(fp, "(%s is too large)\n", argval); - break; + fputs("integer overflow at option ", fp); + arg_print_option(fp, shortopts, longopts, datatype, " "); + fprintf(fp, "(%s is too large)\n", argval); + break; } } -struct arg_int * arg_int0( +struct arg_int *arg_int0( const char *shortopts, const char *longopts, const char *datatype, @@ -2277,7 +2225,7 @@ struct arg_int * arg_int0( } -struct arg_int * arg_int1( +struct arg_int *arg_int1( const char *shortopts, const char *longopts, const char *datatype, @@ -2287,7 +2235,7 @@ struct arg_int * arg_int1( } -struct arg_int * arg_intn( +struct arg_int *arg_intn( const char *shortopts, const char *longopts, const char *datatype, @@ -2305,8 +2253,7 @@ struct arg_int * arg_intn( + maxcount * sizeof(int); /* storage for ival[maxcount] array */ result = (struct arg_int *)malloc(nbytes); - if (result) - { + if (result) { /* init the arg_hdr struct */ result->hdr.flag = ARG_HASVALUE; result->hdr.shortopts = shortopts; @@ -2374,7 +2321,7 @@ static void arg_lit_resetfn(struct arg_lit *parent) static int arg_lit_scanfn(struct arg_lit *parent, const char *argval) { int errorcode = 0; - if (parent->count < parent->hdr.maxcount ) + if (parent->count < parent->hdr.maxcount) parent->count++; else errorcode = EMAXCOUNT; @@ -2404,18 +2351,17 @@ static void arg_lit_errorfn( const char *longopts = parent->hdr.longopts; const char *datatype = parent->hdr.datatype; - switch(errorcode) - { - case EMINCOUNT: - fprintf(fp, "%s: missing option ", progname); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - fprintf(fp, "\n"); - break; + switch (errorcode) { + case EMINCOUNT: + fprintf(fp, "%s: missing option ", progname); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + fprintf(fp, "\n"); + break; - case EMAXCOUNT: - fprintf(fp, "%s: extraneous option ", progname); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + case EMAXCOUNT: + fprintf(fp, "%s: extraneous option ", progname); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; } ARG_TRACE(("%s:errorfn(%p, %p, %d, %s, %s)\n", __FILE__, parent, fp, @@ -2423,16 +2369,16 @@ static void arg_lit_errorfn( } -struct arg_lit * arg_lit0( - const char * shortopts, - const char * longopts, - const char * glossary) +struct arg_lit *arg_lit0( + const char *shortopts, + const char *longopts, + const char *glossary) { return arg_litn(shortopts, longopts, 0, 1, glossary); } -struct arg_lit * arg_lit1( +struct arg_lit *arg_lit1( const char *shortopts, const char *longopts, const char *glossary) @@ -2441,7 +2387,7 @@ struct arg_lit * arg_lit1( } -struct arg_lit * arg_litn( +struct arg_lit *arg_litn( const char *shortopts, const char *longopts, int mincount, @@ -2454,8 +2400,7 @@ struct arg_lit * arg_litn( maxcount = (maxcount < mincount) ? mincount : maxcount; result = (struct arg_lit *)malloc(sizeof(struct arg_lit)); - if (result) - { + if (result) { /* init the arg_hdr struct */ result->hdr.flag = 0; result->hdr.shortopts = shortopts; @@ -2514,8 +2459,7 @@ struct arg_lit * arg_litn( struct arg_rem *arg_rem(const char *datatype, const char *glossary) { struct arg_rem *result = (struct arg_rem *)malloc(sizeof(struct arg_rem)); - if (result) - { + if (result) { result->hdr.flag = 0; result->hdr.shortopts = NULL; result->hdr.longopts = NULL; @@ -2636,11 +2580,11 @@ typedef struct { TREX_API TRex *trex_compile(const TRexChar *pattern, const TRexChar **error, int flags); TREX_API void trex_free(TRex *exp); -TREX_API TRexBool trex_match(TRex* exp, const TRexChar* text); -TREX_API TRexBool trex_search(TRex* exp, const TRexChar* text, const TRexChar** out_begin, const TRexChar** out_end); -TREX_API TRexBool trex_searchrange(TRex* exp, const TRexChar* text_begin, const TRexChar* text_end, const TRexChar** out_begin, const TRexChar** out_end); -TREX_API int trex_getsubexpcount(TRex* exp); -TREX_API TRexBool trex_getsubexp(TRex* exp, int n, TRexMatch *subexp); +TREX_API TRexBool trex_match(TRex *exp, const TRexChar *text); +TREX_API TRexBool trex_search(TRex *exp, const TRexChar *text, const TRexChar **out_begin, const TRexChar **out_end); +TREX_API TRexBool trex_searchrange(TRex *exp, const TRexChar *text_begin, const TRexChar *text_end, const TRexChar **out_begin, const TRexChar **out_end); +TREX_API int trex_getsubexpcount(TRex *exp); +TREX_API TRexBool trex_getsubexp(TRex *exp, int n, TRexMatch *subexp); #ifdef __cplusplus } @@ -2650,8 +2594,7 @@ TREX_API TRexBool trex_getsubexp(TRex* exp, int n, TRexMatch *subexp); -struct privhdr -{ +struct privhdr { const char *pattern; int flags; }; @@ -2670,20 +2613,15 @@ static int arg_rex_scanfn(struct arg_rex *parent, const char *argval) TRex *rex = NULL; TRexBool is_match = TRex_False; - if (parent->count == parent->hdr.maxcount ) - { + if (parent->count == parent->hdr.maxcount) { /* maximum number of arguments exceeded */ errorcode = EMAXCOUNT; - } - else if (!argval) - { + } else if (!argval) { /* a valid argument with no argument value was given. */ /* This happens when an optional argument value was invoked. */ /* leave parent argument value unaltered but still count the argument. */ parent->count++; - } - else - { + } else { struct privhdr *priv = (struct privhdr *)parent->hdr.priv; /* test the current argument value for a match with the regular expression */ @@ -2699,7 +2637,7 @@ static int arg_rex_scanfn(struct arg_rex *parent, const char *argval) trex_free(rex); } - ARG_TRACE(("%s:scanfn(%p) returns %d\n",__FILE__,parent,errorcode)); + ARG_TRACE(("%s:scanfn(%p) returns %d\n", __FILE__, parent, errorcode)); return errorcode; } @@ -2716,10 +2654,10 @@ static int arg_rex_checkfn(struct arg_rex *parent) } static void arg_rex_errorfn(struct arg_rex *parent, - FILE *fp, - int errorcode, - const char *argval, - const char *progname) + FILE *fp, + int errorcode, + const char *argval, + const char *progname) { const char *shortopts = parent->hdr.shortopts; const char *longopts = parent->hdr.longopts; @@ -2729,40 +2667,38 @@ static void arg_rex_errorfn(struct arg_rex *parent, argval = argval ? argval : ""; fprintf(fp, "%s: ", progname); - switch(errorcode) - { - case EMINCOUNT: - fputs("missing option ", fp); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + switch (errorcode) { + case EMINCOUNT: + fputs("missing option ", fp); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; - case EMAXCOUNT: - fputs("excess option ", fp); - arg_print_option(fp, shortopts, longopts, argval, "\n"); - break; + case EMAXCOUNT: + fputs("excess option ", fp); + arg_print_option(fp, shortopts, longopts, argval, "\n"); + break; - case EREGNOMATCH: - fputs("illegal value ", fp); - arg_print_option(fp, shortopts, longopts, argval, "\n"); - break; + case EREGNOMATCH: + fputs("illegal value ", fp); + arg_print_option(fp, shortopts, longopts, argval, "\n"); + break; - default: - { - //char errbuff[256]; - //regerror(errorcode, NULL, errbuff, sizeof(errbuff)); - //printf("%s\n", errbuff); - } - break; + default: { + //char errbuff[256]; + //regerror(errorcode, NULL, errbuff, sizeof(errbuff)); + //printf("%s\n", errbuff); + } + break; } } -struct arg_rex * arg_rex0(const char * shortopts, - const char * longopts, - const char * pattern, - const char *datatype, - int flags, - const char *glossary) +struct arg_rex *arg_rex0(const char *shortopts, + const char *longopts, + const char *pattern, + const char *datatype, + int flags, + const char *glossary) { return arg_rexn(shortopts, longopts, @@ -2774,12 +2710,12 @@ struct arg_rex * arg_rex0(const char * shortopts, glossary); } -struct arg_rex * arg_rex1(const char * shortopts, - const char * longopts, - const char * pattern, - const char *datatype, - int flags, - const char *glossary) +struct arg_rex *arg_rex1(const char *shortopts, + const char *longopts, + const char *pattern, + const char *datatype, + int flags, + const char *glossary) { return arg_rexn(shortopts, longopts, @@ -2792,14 +2728,14 @@ struct arg_rex * arg_rex1(const char * shortopts, } -struct arg_rex * arg_rexn(const char * shortopts, - const char * longopts, - const char * pattern, - const char *datatype, - int mincount, - int maxcount, - int flags, - const char *glossary) +struct arg_rex *arg_rexn(const char *shortopts, + const char *longopts, + const char *pattern, + const char *datatype, + int mincount, + int maxcount, + int flags, + const char *glossary) { size_t nbytes; struct arg_rex *result; @@ -2808,8 +2744,7 @@ struct arg_rex * arg_rexn(const char * shortopts, const TRexChar *error = NULL; TRex *rex = NULL; - if (!pattern) - { + if (!pattern) { printf( "argtable: ERROR - illegal regular expression pattern \"(NULL)\"\n"); printf("argtable: Bad argument table.\n"); @@ -2862,8 +2797,7 @@ struct arg_rex * arg_rexn(const char * shortopts, */ rex = trex_compile(priv->pattern, &error, priv->flags); - if (rex == NULL) - { + if (rex == NULL) { ARG_LOG(("argtable: %s \"%s\"\n", error ? error : _TREXC("undefined"), priv->pattern)); ARG_LOG(("argtable: Bad argument table.\n")); } @@ -2897,12 +2831,11 @@ struct arg_rex * arg_rexn(const char * shortopts, #ifdef _DEBUG #include -static const TRexChar *g_nnames[] = -{ - _SC("NONE"),_SC("OP_GREEDY"), _SC("OP_OR"), - _SC("OP_EXPR"),_SC("OP_NOCAPEXPR"),_SC("OP_DOT"), _SC("OP_CLASS"), - _SC("OP_CCLASS"),_SC("OP_NCLASS"),_SC("OP_RANGE"),_SC("OP_CHAR"), - _SC("OP_EOL"),_SC("OP_BOL"),_SC("OP_WB") +static const TRexChar *g_nnames[] = { + _SC("NONE"), _SC("OP_GREEDY"), _SC("OP_OR"), + _SC("OP_EXPR"), _SC("OP_NOCAPEXPR"), _SC("OP_DOT"), _SC("OP_CLASS"), + _SC("OP_CCLASS"), _SC("OP_NCLASS"), _SC("OP_RANGE"), _SC("OP_CHAR"), + _SC("OP_EOL"), _SC("OP_BOL"), _SC("OP_WB") }; #endif @@ -2932,14 +2865,14 @@ static const TRexChar *g_nnames[] = typedef int TRexNodeType; -typedef struct tagTRexNode{ +typedef struct tagTRexNode { TRexNodeType type; int left; int right; int next; -}TRexNode; +} TRexNode; -struct TRex{ +struct TRex { const TRexChar *_eol; const TRexChar *_bol; const TRexChar *_p; @@ -2964,9 +2897,9 @@ static int trex_newnode(TRex *exp, TRexNodeType type) int newid; n.type = type; n.next = n.right = n.left = -1; - if(type == OP_EXPR) + if (type == OP_EXPR) n.right = exp->_nsubexpr++; - if(exp->_nallocated < (exp->_nsize + 1)) { + if (exp->_nallocated < (exp->_nsize + 1)) { exp->_nallocated *= 2; exp->_nodes = (TRexNode *)realloc(exp->_nodes, exp->_nallocated * sizeof(TRexNode)); } @@ -2975,117 +2908,151 @@ static int trex_newnode(TRex *exp, TRexNodeType type) return (int)newid; } -static void trex_error(TRex *exp,const TRexChar *error) +static void trex_error(TRex *exp, const TRexChar *error) { - if(exp->_error) *exp->_error = error; - longjmp(*((jmp_buf*)exp->_jmpbuf),-1); + if (exp->_error) *exp->_error = error; + longjmp(*((jmp_buf *)exp->_jmpbuf), -1); } -static void trex_expect(TRex *exp, int n){ - if((*exp->_p) != n) +static void trex_expect(TRex *exp, int n) +{ + if ((*exp->_p) != n) trex_error(exp, _SC("expected paren")); exp->_p++; } static TRexChar trex_escapechar(TRex *exp) { - if(*exp->_p == TREX_SYMBOL_ESCAPE_CHAR){ + if (*exp->_p == TREX_SYMBOL_ESCAPE_CHAR) { exp->_p++; - switch(*exp->_p) { - case 'v': exp->_p++; return '\v'; - case 'n': exp->_p++; return '\n'; - case 't': exp->_p++; return '\t'; - case 'r': exp->_p++; return '\r'; - case 'f': exp->_p++; return '\f'; - default: return (*exp->_p++); + switch (*exp->_p) { + case 'v': + exp->_p++; + return '\v'; + case 'n': + exp->_p++; + return '\n'; + case 't': + exp->_p++; + return '\t'; + case 'r': + exp->_p++; + return '\r'; + case 'f': + exp->_p++; + return '\f'; + default: + return (*exp->_p++); } - } else if(!scisprint(*exp->_p)) trex_error(exp,_SC("letter expected")); + } else if (!scisprint(*exp->_p)) trex_error(exp, _SC("letter expected")); return (*exp->_p++); } -static int trex_charclass(TRex *exp,int classid) +static int trex_charclass(TRex *exp, int classid) { - int n = trex_newnode(exp,OP_CCLASS); + int n = trex_newnode(exp, OP_CCLASS); exp->_nodes[n].left = classid; return n; } -static int trex_charnode(TRex *exp,TRexBool isclass) +static int trex_charnode(TRex *exp, TRexBool isclass) { TRexChar t; - if(*exp->_p == TREX_SYMBOL_ESCAPE_CHAR) { + if (*exp->_p == TREX_SYMBOL_ESCAPE_CHAR) { exp->_p++; - switch(*exp->_p) { - case 'n': exp->_p++; return trex_newnode(exp,'\n'); - case 't': exp->_p++; return trex_newnode(exp,'\t'); - case 'r': exp->_p++; return trex_newnode(exp,'\r'); - case 'f': exp->_p++; return trex_newnode(exp,'\f'); - case 'v': exp->_p++; return trex_newnode(exp,'\v'); - case 'a': case 'A': case 'w': case 'W': case 's': case 'S': - case 'd': case 'D': case 'x': case 'X': case 'c': case 'C': - case 'p': case 'P': case 'l': case 'u': - { - t = *exp->_p; exp->_p++; - return trex_charclass(exp,t); - } + switch (*exp->_p) { + case 'n': + exp->_p++; + return trex_newnode(exp, '\n'); + case 't': + exp->_p++; + return trex_newnode(exp, '\t'); + case 'r': + exp->_p++; + return trex_newnode(exp, '\r'); + case 'f': + exp->_p++; + return trex_newnode(exp, '\f'); + case 'v': + exp->_p++; + return trex_newnode(exp, '\v'); + case 'a': + case 'A': + case 'w': + case 'W': + case 's': + case 'S': + case 'd': + case 'D': + case 'x': + case 'X': + case 'c': + case 'C': + case 'p': + case 'P': + case 'l': + case 'u': { + t = *exp->_p; + exp->_p++; + return trex_charclass(exp, t); + } case 'b': case 'B': - if(!isclass) { - int node = trex_newnode(exp,OP_WB); + if (!isclass) { + int node = trex_newnode(exp, OP_WB); exp->_nodes[node].left = *exp->_p; exp->_p++; return node; } //else default default: - t = *exp->_p; exp->_p++; - return trex_newnode(exp,t); + t = *exp->_p; + exp->_p++; + return trex_newnode(exp, t); } - } - else if(!scisprint(*exp->_p)) { + } else if (!scisprint(*exp->_p)) { - trex_error(exp,_SC("letter expected")); + trex_error(exp, _SC("letter expected")); } - t = *exp->_p; exp->_p++; - return trex_newnode(exp,t); + t = *exp->_p; + exp->_p++; + return trex_newnode(exp, t); } static int trex_class(TRex *exp) { int ret = -1; - int first = -1,chain; - if(*exp->_p == TREX_SYMBOL_BEGINNING_OF_STRING){ - ret = trex_newnode(exp,OP_NCLASS); + int first = -1, chain; + if (*exp->_p == TREX_SYMBOL_BEGINNING_OF_STRING) { + ret = trex_newnode(exp, OP_NCLASS); exp->_p++; - }else ret = trex_newnode(exp,OP_CLASS); + } else ret = trex_newnode(exp, OP_CLASS); - if(*exp->_p == ']') trex_error(exp,_SC("empty class")); + if (*exp->_p == ']') trex_error(exp, _SC("empty class")); chain = ret; - while(*exp->_p != ']' && exp->_p != exp->_eol) { - if(*exp->_p == '-' && first != -1){ - int r,t; - if(*exp->_p++ == ']') trex_error(exp,_SC("unfinished range")); - r = trex_newnode(exp,OP_RANGE); - if(first>*exp->_p) trex_error(exp,_SC("invalid range")); - if(exp->_nodes[first].type == OP_CCLASS) trex_error(exp,_SC("cannot use character classes in ranges")); + while (*exp->_p != ']' && exp->_p != exp->_eol) { + if (*exp->_p == '-' && first != -1) { + int r, t; + if (*exp->_p++ == ']') trex_error(exp, _SC("unfinished range")); + r = trex_newnode(exp, OP_RANGE); + if (first > *exp->_p) trex_error(exp, _SC("invalid range")); + if (exp->_nodes[first].type == OP_CCLASS) trex_error(exp, _SC("cannot use character classes in ranges")); exp->_nodes[r].left = exp->_nodes[first].type; t = trex_escapechar(exp); exp->_nodes[r].right = t; exp->_nodes[chain].next = r; chain = r; first = -1; - } - else{ - if(first!=-1){ + } else { + if (first != -1) { int c = first; exp->_nodes[chain].next = c; chain = c; - first = trex_charnode(exp,TRex_True); - } - else{ - first = trex_charnode(exp,TRex_True); + first = trex_charnode(exp, TRex_True); + } else { + first = trex_charnode(exp, TRex_True); } } } - if(first!=-1){ + if (first != -1) { int c = first; exp->_nodes[chain].next = c; chain = c; @@ -3099,12 +3066,12 @@ static int trex_class(TRex *exp) static int trex_parsenumber(TRex *exp) { - int ret = *exp->_p-'0'; + int ret = *exp->_p - '0'; int positions = 10; exp->_p++; - while(isdigit(*exp->_p)) { - ret = ret*10+(*exp->_p++-'0'); - if(positions==1000000000) trex_error(exp,_SC("overflow in numeric constant")); + while (isdigit(*exp->_p)) { + ret = ret * 10 + (*exp->_p++ -'0'); + if (positions == 1000000000) trex_error(exp, _SC("overflow in numeric constant")); positions *= 10; }; return ret; @@ -3113,78 +3080,98 @@ static int trex_parsenumber(TRex *exp) static int trex_element(TRex *exp) { int ret = -1; - switch(*exp->_p) - { - case '(': { - int expr,newn; - exp->_p++; - - - if(*exp->_p =='?') { + switch (*exp->_p) { + case '(': { + int expr, newn; exp->_p++; - trex_expect(exp,':'); - expr = trex_newnode(exp,OP_NOCAPEXPR); + + + if (*exp->_p == '?') { + exp->_p++; + trex_expect(exp, ':'); + expr = trex_newnode(exp, OP_NOCAPEXPR); + } else + expr = trex_newnode(exp, OP_EXPR); + newn = trex_list(exp); + exp->_nodes[expr].left = newn; + ret = expr; + trex_expect(exp, ')'); } - else - expr = trex_newnode(exp,OP_EXPR); - newn = trex_list(exp); - exp->_nodes[expr].left = newn; - ret = expr; - trex_expect(exp,')'); - } - break; - case '[': - exp->_p++; - ret = trex_class(exp); - trex_expect(exp,']'); - break; - case TREX_SYMBOL_END_OF_STRING: exp->_p++; ret = trex_newnode(exp,OP_EOL);break; - case TREX_SYMBOL_ANY_CHAR: exp->_p++; ret = trex_newnode(exp,OP_DOT);break; - default: - ret = trex_charnode(exp,TRex_False); break; + case '[': + exp->_p++; + ret = trex_class(exp); + trex_expect(exp, ']'); + break; + case TREX_SYMBOL_END_OF_STRING: + exp->_p++; + ret = trex_newnode(exp, OP_EOL); + break; + case TREX_SYMBOL_ANY_CHAR: + exp->_p++; + ret = trex_newnode(exp, OP_DOT); + break; + default: + ret = trex_charnode(exp, TRex_False); + break; } { TRexBool isgreedy = TRex_False; unsigned short p0 = 0, p1 = 0; - switch(*exp->_p){ - case TREX_SYMBOL_GREEDY_ZERO_OR_MORE: p0 = 0; p1 = 0xFFFF; exp->_p++; isgreedy = TRex_True; break; - case TREX_SYMBOL_GREEDY_ONE_OR_MORE: p0 = 1; p1 = 0xFFFF; exp->_p++; isgreedy = TRex_True; break; - case TREX_SYMBOL_GREEDY_ZERO_OR_ONE: p0 = 0; p1 = 1; exp->_p++; isgreedy = TRex_True; break; + switch (*exp->_p) { + case TREX_SYMBOL_GREEDY_ZERO_OR_MORE: + p0 = 0; + p1 = 0xFFFF; + exp->_p++; + isgreedy = TRex_True; + break; + case TREX_SYMBOL_GREEDY_ONE_OR_MORE: + p0 = 1; + p1 = 0xFFFF; + exp->_p++; + isgreedy = TRex_True; + break; + case TREX_SYMBOL_GREEDY_ZERO_OR_ONE: + p0 = 0; + p1 = 1; + exp->_p++; + isgreedy = TRex_True; + break; case '{': exp->_p++; - if(!isdigit(*exp->_p)) trex_error(exp,_SC("number expected")); + if (!isdigit(*exp->_p)) trex_error(exp, _SC("number expected")); p0 = (unsigned short)trex_parsenumber(exp); /*******************************/ - switch(*exp->_p) { - case '}': - p1 = p0; exp->_p++; - break; - case ',': - exp->_p++; - p1 = 0xFFFF; - if(isdigit(*exp->_p)){ - p1 = (unsigned short)trex_parsenumber(exp); + switch (*exp->_p) { + case '}': + p1 = p0; + exp->_p++; + break; + case ',': + exp->_p++; + p1 = 0xFFFF; + if (isdigit(*exp->_p)) { + p1 = (unsigned short)trex_parsenumber(exp); + } + trex_expect(exp, '}'); + break; + default: + trex_error(exp, _SC(", or } expected")); } - trex_expect(exp,'}'); + /*******************************/ + isgreedy = TRex_True; break; - default: - trex_error(exp,_SC(", or } expected")); - } - /*******************************/ - isgreedy = TRex_True; - break; } - if(isgreedy) { - int nnode = trex_newnode(exp,OP_GREEDY); + if (isgreedy) { + int nnode = trex_newnode(exp, OP_GREEDY); exp->_nodes[nnode].left = ret; - exp->_nodes[nnode].right = ((p0)<<16)|p1; + exp->_nodes[nnode].right = ((p0) << 16) | p1; ret = nnode; } } - if((*exp->_p != TREX_SYMBOL_BRANCH) && (*exp->_p != ')') && (*exp->_p != TREX_SYMBOL_GREEDY_ZERO_OR_MORE) && (*exp->_p != TREX_SYMBOL_GREEDY_ONE_OR_MORE) && (*exp->_p != '\0')) { + if ((*exp->_p != TREX_SYMBOL_BRANCH) && (*exp->_p != ')') && (*exp->_p != TREX_SYMBOL_GREEDY_ZERO_OR_MORE) && (*exp->_p != TREX_SYMBOL_GREEDY_ONE_OR_MORE) && (*exp->_p != '\0')) { int nnode = trex_element(exp); exp->_nodes[ret].next = nnode; } @@ -3194,21 +3181,20 @@ static int trex_element(TRex *exp) static int trex_list(TRex *exp) { - int ret=-1,e; - if(*exp->_p == TREX_SYMBOL_BEGINNING_OF_STRING) { + int ret = -1, e; + if (*exp->_p == TREX_SYMBOL_BEGINNING_OF_STRING) { exp->_p++; - ret = trex_newnode(exp,OP_BOL); + ret = trex_newnode(exp, OP_BOL); } e = trex_element(exp); - if(ret != -1) { + if (ret != -1) { exp->_nodes[ret].next = e; - } - else ret = e; + } else ret = e; - if(*exp->_p == TREX_SYMBOL_BRANCH) { - int temp,tright; + if (*exp->_p == TREX_SYMBOL_BRANCH) { + int temp, tright; exp->_p++; - temp = trex_newnode(exp,OP_OR); + temp = trex_newnode(exp, OP_OR); exp->_nodes[temp].left = ret; tright = trex_list(exp); exp->_nodes[temp].right = tright; @@ -3217,143 +3203,151 @@ static int trex_list(TRex *exp) return ret; } -static TRexBool trex_matchcclass(int cclass,TRexChar c) +static TRexBool trex_matchcclass(int cclass, TRexChar c) { - switch(cclass) { - case 'a': return isalpha(c)?TRex_True:TRex_False; - case 'A': return !isalpha(c)?TRex_True:TRex_False; - case 'w': return (isalnum(c) || c == '_')?TRex_True:TRex_False; - case 'W': return (!isalnum(c) && c != '_')?TRex_True:TRex_False; - case 's': return ISSPACE(c)?TRex_True:TRex_False; - case 'S': return !ISSPACE(c)?TRex_True:TRex_False; - case 'd': return isdigit(c)?TRex_True:TRex_False; - case 'D': return !isdigit(c)?TRex_True:TRex_False; - case 'x': return isxdigit(c)?TRex_True:TRex_False; - case 'X': return !isxdigit(c)?TRex_True:TRex_False; - case 'c': return iscntrl(c)?TRex_True:TRex_False; - case 'C': return !iscntrl(c)?TRex_True:TRex_False; - case 'p': return ispunct(c)?TRex_True:TRex_False; - case 'P': return !ispunct(c)?TRex_True:TRex_False; - case 'l': return islower(c)?TRex_True:TRex_False; - case 'u': return isupper(c)?TRex_True:TRex_False; + switch (cclass) { + case 'a': + return isalpha(c) ? TRex_True : TRex_False; + case 'A': + return !isalpha(c) ? TRex_True : TRex_False; + case 'w': + return (isalnum(c) || c == '_') ? TRex_True : TRex_False; + case 'W': + return (!isalnum(c) && c != '_') ? TRex_True : TRex_False; + case 's': + return ISSPACE(c) ? TRex_True : TRex_False; + case 'S': + return !ISSPACE(c) ? TRex_True : TRex_False; + case 'd': + return isdigit(c) ? TRex_True : TRex_False; + case 'D': + return !isdigit(c) ? TRex_True : TRex_False; + case 'x': + return isxdigit(c) ? TRex_True : TRex_False; + case 'X': + return !isxdigit(c) ? TRex_True : TRex_False; + case 'c': + return iscntrl(c) ? TRex_True : TRex_False; + case 'C': + return !iscntrl(c) ? TRex_True : TRex_False; + case 'p': + return ispunct(c) ? TRex_True : TRex_False; + case 'P': + return !ispunct(c) ? TRex_True : TRex_False; + case 'l': + return islower(c) ? TRex_True : TRex_False; + case 'u': + return isupper(c) ? TRex_True : TRex_False; } return TRex_False; /*cannot happen*/ } -static TRexBool trex_matchclass(TRex* exp,TRexNode *node,TRexChar c) +static TRexBool trex_matchclass(TRex *exp, TRexNode *node, TRexChar c) { do { - switch(node->type) { + switch (node->type) { case OP_RANGE: - if (exp->_flags & TREX_ICASE) - { - if(c >= toupper(node->left) && c <= toupper(node->right)) return TRex_True; - if(c >= tolower(node->left) && c <= tolower(node->right)) return TRex_True; - } - else - { - if(c >= node->left && c <= node->right) return TRex_True; + if (exp->_flags & TREX_ICASE) { + if (c >= toupper(node->left) && c <= toupper(node->right)) return TRex_True; + if (c >= tolower(node->left) && c <= tolower(node->right)) return TRex_True; + } else { + if (c >= node->left && c <= node->right) return TRex_True; } break; case OP_CCLASS: - if(trex_matchcclass(node->left,c)) return TRex_True; + if (trex_matchcclass(node->left, c)) return TRex_True; break; default: - if (exp->_flags & TREX_ICASE) - { + if (exp->_flags & TREX_ICASE) { if (c == tolower(node->type) || c == toupper(node->type)) return TRex_True; - } - else - { - if(c == node->type)return TRex_True; + } else { + if (c == node->type)return TRex_True; } } - } while((node->next != -1) && (node = &exp->_nodes[node->next])); + } while ((node->next != -1) && (node = &exp->_nodes[node->next])); return TRex_False; } -static const TRexChar *trex_matchnode(TRex* exp,TRexNode *node,const TRexChar *str,TRexNode *next) +static const TRexChar *trex_matchnode(TRex *exp, TRexNode *node, const TRexChar *str, TRexNode *next) { TRexNodeType type = node->type; - switch(type) { - case OP_GREEDY: { - //TRexNode *greedystop = (node->next != -1) ? &exp->_nodes[node->next] : NULL; - TRexNode *greedystop = NULL; - int p0 = (node->right >> 16)&0x0000FFFF, p1 = node->right&0x0000FFFF, nmaches = 0; - const TRexChar *s=str, *good = str; + switch (type) { + case OP_GREEDY: { + //TRexNode *greedystop = (node->next != -1) ? &exp->_nodes[node->next] : NULL; + TRexNode *greedystop = NULL; + int p0 = (node->right >> 16) & 0x0000FFFF, p1 = node->right & 0x0000FFFF, nmaches = 0; + const TRexChar *s = str, *good = str; - if(node->next != -1) { - greedystop = &exp->_nodes[node->next]; - } - else { - greedystop = next; - } - - while((nmaches == 0xFFFF || nmaches < p1)) { - - const TRexChar *stop; - if(!(s = trex_matchnode(exp,&exp->_nodes[node->left],s,greedystop))) - break; - nmaches++; - good=s; - if(greedystop) { - //checks that 0 matches satisfy the expression(if so skips) - //if not would always stop(for instance if is a '?') - if(greedystop->type != OP_GREEDY || - (greedystop->type == OP_GREEDY && ((greedystop->right >> 16)&0x0000FFFF) != 0)) - { - TRexNode *gnext = NULL; - if(greedystop->next != -1) { - gnext = &exp->_nodes[greedystop->next]; - }else if(next && next->next != -1){ - gnext = &exp->_nodes[next->next]; - } - stop = trex_matchnode(exp,greedystop,s,gnext); - if(stop) { - //if satisfied stop it - if(p0 == p1 && p0 == nmaches) break; - else if(nmaches >= p0 && p1 == 0xFFFF) break; - else if(nmaches >= p0 && nmaches <= p1) break; - } - } + if (node->next != -1) { + greedystop = &exp->_nodes[node->next]; + } else { + greedystop = next; } - if(s >= exp->_eol) - break; + while ((nmaches == 0xFFFF || nmaches < p1)) { + + const TRexChar *stop; + if (!(s = trex_matchnode(exp, &exp->_nodes[node->left], s, greedystop))) + break; + nmaches++; + good = s; + if (greedystop) { + //checks that 0 matches satisfy the expression(if so skips) + //if not would always stop(for instance if is a '?') + if (greedystop->type != OP_GREEDY || + (greedystop->type == OP_GREEDY && ((greedystop->right >> 16) & 0x0000FFFF) != 0)) { + TRexNode *gnext = NULL; + if (greedystop->next != -1) { + gnext = &exp->_nodes[greedystop->next]; + } else if (next && next->next != -1) { + gnext = &exp->_nodes[next->next]; + } + stop = trex_matchnode(exp, greedystop, s, gnext); + if (stop) { + //if satisfied stop it + if (p0 == p1 && p0 == nmaches) break; + else if (nmaches >= p0 && p1 == 0xFFFF) break; + else if (nmaches >= p0 && nmaches <= p1) break; + } + } + } + + if (s >= exp->_eol) + break; + } + if (p0 == p1 && p0 == nmaches) return good; + else if (nmaches >= p0 && p1 == 0xFFFF) return good; + else if (nmaches >= p0 && nmaches <= p1) return good; + return NULL; } - if(p0 == p1 && p0 == nmaches) return good; - else if(nmaches >= p0 && p1 == 0xFFFF) return good; - else if(nmaches >= p0 && nmaches <= p1) return good; - return NULL; - } - case OP_OR: { + case OP_OR: { const TRexChar *asd = str; - TRexNode *temp=&exp->_nodes[node->left]; - while( (asd = trex_matchnode(exp,temp,asd,NULL)) ) { - if(temp->next != -1) + TRexNode *temp = &exp->_nodes[node->left]; + while ((asd = trex_matchnode(exp, temp, asd, NULL))) { + if (temp->next != -1) temp = &exp->_nodes[temp->next]; else return asd; } asd = str; temp = &exp->_nodes[node->right]; - while( (asd = trex_matchnode(exp,temp,asd,NULL)) ) { - if(temp->next != -1) + while ((asd = trex_matchnode(exp, temp, asd, NULL))) { + if (temp->next != -1) temp = &exp->_nodes[temp->next]; else return asd; } return NULL; break; - } - case OP_EXPR: - case OP_NOCAPEXPR:{ + } + case OP_EXPR: + case OP_NOCAPEXPR: { TRexNode *n = &exp->_nodes[node->left]; const TRexChar *cur = str; int capture = -1; - if(node->type != OP_NOCAPEXPR && node->right == exp->_currsubexp) { + if (node->type != OP_NOCAPEXPR && node->right == exp->_currsubexp) { capture = exp->_currsubexp; exp->_matches[capture].begin = cur; exp->_currsubexp++; @@ -3361,71 +3355,68 @@ static const TRexChar *trex_matchnode(TRex* exp,TRexNode *node,const TRexChar *s do { TRexNode *subnext = NULL; - if(n->next != -1) { + if (n->next != -1) { subnext = &exp->_nodes[n->next]; - }else { + } else { subnext = next; } - if(!(cur = trex_matchnode(exp,n,cur,subnext))) { - if(capture != -1){ + if (!(cur = trex_matchnode(exp, n, cur, subnext))) { + if (capture != -1) { exp->_matches[capture].begin = 0; exp->_matches[capture].len = 0; } return NULL; } - } while((n->next != -1) && (n = &exp->_nodes[n->next])); + } while ((n->next != -1) && (n = &exp->_nodes[n->next])); - if(capture != -1) + if (capture != -1) exp->_matches[capture].len = (int)(cur - exp->_matches[capture].begin); return cur; - } - case OP_WB: - if((str == exp->_bol && !ISSPACE(*str)) - || ((str == exp->_eol && !ISSPACE(*(str-1)))) - || ((!ISSPACE(*str) && ISSPACE(*(str+1)))) - || ((ISSPACE(*str) && !ISSPACE(*(str+1)))) ) { - return (node->left == 'b')?str:NULL; } - return (node->left == 'b')?NULL:str; - case OP_BOL: - if(str == exp->_bol) return str; - return NULL; - case OP_EOL: - if(str == exp->_eol) return str; - return NULL; - case OP_DOT: - str++; - return str; - case OP_NCLASS: - case OP_CLASS: - if(trex_matchclass(exp,&exp->_nodes[node->left],*str)?(type == OP_CLASS?TRex_True:TRex_False):(type == OP_NCLASS?TRex_True:TRex_False)) { - str++; + case OP_WB: + if ((str == exp->_bol && !ISSPACE(*str)) + || ((str == exp->_eol && !ISSPACE(*(str - 1)))) + || ((!ISSPACE(*str) && ISSPACE(*(str + 1)))) + || ((ISSPACE(*str) && !ISSPACE(*(str + 1))))) { + return (node->left == 'b') ? str : NULL; + } + return (node->left == 'b') ? NULL : str; + case OP_BOL: + if (str == exp->_bol) return str; + return NULL; + case OP_EOL: + if (str == exp->_eol) return str; + return NULL; + case OP_DOT: + str++; return str; - } - return NULL; - case OP_CCLASS: - if(trex_matchcclass(node->left,*str)) { - str++; + case OP_NCLASS: + case OP_CLASS: + if (trex_matchclass(exp, &exp->_nodes[node->left], *str) ? (type == OP_CLASS ? TRex_True : TRex_False) : (type == OP_NCLASS ? TRex_True : TRex_False)) { + str++; + return str; + } + return NULL; + case OP_CCLASS: + if (trex_matchcclass(node->left, *str)) { + str++; + return str; + } + return NULL; + default: /* char */ + if (exp->_flags & TREX_ICASE) { + if (*str != tolower(node->type) && *str != toupper(node->type)) return NULL; + } else { + if (*str != node->type) return NULL; + } + str++; return str; - } - return NULL; - default: /* char */ - if (exp->_flags & TREX_ICASE) - { - if(*str != tolower(node->type) && *str != toupper(node->type)) return NULL; - } - else - { - if (*str != node->type) return NULL; - } - str++; - return str; } return NULL; } /* public api */ -TRex *trex_compile(const TRexChar *pattern,const TRexChar **error,int flags) +TRex *trex_compile(const TRexChar *pattern, const TRexChar **error, int flags) { TRex *exp = (TRex *)malloc(sizeof(TRex)); exp->_eol = exp->_bol = NULL; @@ -3435,36 +3426,35 @@ TRex *trex_compile(const TRexChar *pattern,const TRexChar **error,int flags) exp->_nsize = 0; exp->_matches = 0; exp->_nsubexpr = 0; - exp->_first = trex_newnode(exp,OP_EXPR); + exp->_first = trex_newnode(exp, OP_EXPR); exp->_error = error; exp->_jmpbuf = malloc(sizeof(jmp_buf)); exp->_flags = flags; - if(setjmp(*((jmp_buf*)exp->_jmpbuf)) == 0) { + if (setjmp(*((jmp_buf *)exp->_jmpbuf)) == 0) { int res = trex_list(exp); exp->_nodes[exp->_first].left = res; - if(*exp->_p!='\0') - trex_error(exp,_SC("unexpected character")); + if (*exp->_p != '\0') + trex_error(exp, _SC("unexpected character")); #ifdef _DEBUG { - int nsize,i; + int nsize, i; TRexNode *t; nsize = exp->_nsize; t = &exp->_nodes[0]; scprintf(_SC("\n")); - for(i = 0;i < nsize; i++) { - if(exp->_nodes[i].type>MAX_CHAR) - scprintf(_SC("[%02d] %10s "),i,g_nnames[exp->_nodes[i].type-MAX_CHAR]); + for (i = 0; i < nsize; i++) { + if (exp->_nodes[i].type > MAX_CHAR) + scprintf(_SC("[%02d] %10s "), i, g_nnames[exp->_nodes[i].type - MAX_CHAR]); else - scprintf(_SC("[%02d] %10c "),i,exp->_nodes[i].type); - scprintf(_SC("left %02d right %02d next %02d\n"),exp->_nodes[i].left,exp->_nodes[i].right,exp->_nodes[i].next); + scprintf(_SC("[%02d] %10c "), i, exp->_nodes[i].type); + scprintf(_SC("left %02d right %02d next %02d\n"), exp->_nodes[i].left, exp->_nodes[i].right, exp->_nodes[i].next); } scprintf(_SC("\n")); } #endif exp->_matches = (TRexMatch *) malloc(exp->_nsubexpr * sizeof(TRexMatch)); - memset(exp->_matches,0,exp->_nsubexpr * sizeof(TRexMatch)); - } - else{ + memset(exp->_matches, 0, exp->_nsubexpr * sizeof(TRexMatch)); + } else { trex_free(exp); return NULL; } @@ -3473,68 +3463,68 @@ TRex *trex_compile(const TRexChar *pattern,const TRexChar **error,int flags) void trex_free(TRex *exp) { - if(exp) { - if(exp->_nodes) free(exp->_nodes); - if(exp->_jmpbuf) free(exp->_jmpbuf); - if(exp->_matches) free(exp->_matches); + if (exp) { + if (exp->_nodes) free(exp->_nodes); + if (exp->_jmpbuf) free(exp->_jmpbuf); + if (exp->_matches) free(exp->_matches); free(exp); } } -TRexBool trex_match(TRex* exp,const TRexChar* text) +TRexBool trex_match(TRex *exp, const TRexChar *text) { - const TRexChar* res = NULL; + const TRexChar *res = NULL; exp->_bol = text; exp->_eol = text + scstrlen(text); exp->_currsubexp = 0; - res = trex_matchnode(exp,exp->_nodes,text,NULL); - if(res == NULL || res != exp->_eol) + res = trex_matchnode(exp, exp->_nodes, text, NULL); + if (res == NULL || res != exp->_eol) return TRex_False; return TRex_True; } -TRexBool trex_searchrange(TRex* exp,const TRexChar* text_begin,const TRexChar* text_end,const TRexChar** out_begin, const TRexChar** out_end) +TRexBool trex_searchrange(TRex *exp, const TRexChar *text_begin, const TRexChar *text_end, const TRexChar **out_begin, const TRexChar **out_end) { const TRexChar *cur = NULL; int node = exp->_first; - if(text_begin >= text_end) return TRex_False; + if (text_begin >= text_end) return TRex_False; exp->_bol = text_begin; exp->_eol = text_end; do { cur = text_begin; - while(node != -1) { + while (node != -1) { exp->_currsubexp = 0; - cur = trex_matchnode(exp,&exp->_nodes[node],cur,NULL); - if(!cur) + cur = trex_matchnode(exp, &exp->_nodes[node], cur, NULL); + if (!cur) break; node = exp->_nodes[node].next; } text_begin++; - } while(cur == NULL && text_begin != text_end); + } while (cur == NULL && text_begin != text_end); - if(cur == NULL) + if (cur == NULL) return TRex_False; --text_begin; - if(out_begin) *out_begin = text_begin; - if(out_end) *out_end = cur; + if (out_begin) *out_begin = text_begin; + if (out_end) *out_end = cur; return TRex_True; } -TRexBool trex_search(TRex* exp,const TRexChar* text, const TRexChar** out_begin, const TRexChar** out_end) +TRexBool trex_search(TRex *exp, const TRexChar *text, const TRexChar **out_begin, const TRexChar **out_end) { - return trex_searchrange(exp,text,text + scstrlen(text),out_begin,out_end); + return trex_searchrange(exp, text, text + scstrlen(text), out_begin, out_end); } -int trex_getsubexpcount(TRex* exp) +int trex_getsubexpcount(TRex *exp) { return exp->_nsubexpr; } -TRexBool trex_getsubexp(TRex* exp, int n, TRexMatch *subexp) +TRexBool trex_getsubexp(TRex *exp, int n, TRexMatch *subexp) { - if( n<0 || n >= exp->_nsubexpr) return TRex_False; + if (n < 0 || n >= exp->_nsubexpr) return TRex_False; *subexp = exp->_matches[n]; return TRex_True; } @@ -3584,20 +3574,15 @@ static int arg_str_scanfn(struct arg_str *parent, const char *argval) { int errorcode = 0; - if (parent->count == parent->hdr.maxcount) - { + if (parent->count == parent->hdr.maxcount) { /* maximum number of arguments exceeded */ errorcode = EMAXCOUNT; - } - else if (!argval) - { + } else if (!argval) { /* a valid argument with no argument value was given. */ /* This happens when an optional argument value was invoked. */ /* leave parent arguiment value unaltered but still count the argument. */ parent->count++; - } - else - { + } else { parent->sval[parent->count++] = argval; } @@ -3630,22 +3615,21 @@ static void arg_str_errorfn( argval = argval ? argval : ""; fprintf(fp, "%s: ", progname); - switch(errorcode) - { - case EMINCOUNT: - fputs("missing option ", fp); - arg_print_option(fp, shortopts, longopts, datatype, "\n"); - break; + switch (errorcode) { + case EMINCOUNT: + fputs("missing option ", fp); + arg_print_option(fp, shortopts, longopts, datatype, "\n"); + break; - case EMAXCOUNT: - fputs("excess option ", fp); - arg_print_option(fp, shortopts, longopts, argval, "\n"); - break; + case EMAXCOUNT: + fputs("excess option ", fp); + arg_print_option(fp, shortopts, longopts, argval, "\n"); + break; } } -struct arg_str * arg_str0( +struct arg_str *arg_str0( const char *shortopts, const char *longopts, const char *datatype, @@ -3655,7 +3639,7 @@ struct arg_str * arg_str0( } -struct arg_str * arg_str1( +struct arg_str *arg_str1( const char *shortopts, const char *longopts, const char *datatype, @@ -3665,7 +3649,7 @@ struct arg_str * arg_str1( } -struct arg_str * arg_strn( +struct arg_str *arg_strn( const char *shortopts, const char *longopts, const char *datatype, @@ -3685,8 +3669,7 @@ struct arg_str * arg_strn( + maxcount * sizeof(char *); /* storage for sval[maxcount] array */ result = (struct arg_str *)malloc(nbytes); - if (result) - { + if (result) { int i; /* init the arg_hdr struct */ @@ -3759,15 +3742,12 @@ void arg_register_error(struct arg_end *end, const char *argval) { /* printf("arg_register_error(%p,%p,%d,%s)\n",end,parent,error,argval); */ - if (end->count < end->hdr.maxcount) - { + if (end->count < end->hdr.maxcount) { end->error[end->count] = error; end->parent[end->count] = parent; end->argval[end->count] = argval; end->count++; - } - else - { + } else { end->error[end->hdr.maxcount - 1] = ARG_ELIMIT; end->parent[end->hdr.maxcount - 1] = end; end->argval[end->hdr.maxcount - 1] = NULL; @@ -3783,8 +3763,7 @@ static int find_shortoption(struct arg_hdr * *table, char shortopt) { int tabindex; - for(tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) - { + for (tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) { if (table[tabindex]->shortopts && strchr(table[tabindex]->shortopts, shortopt)) return tabindex; @@ -3793,8 +3772,7 @@ int find_shortoption(struct arg_hdr * *table, char shortopt) } -struct longoptions -{ +struct longoptions { int getoptval; int noptions; struct option *options; @@ -3802,13 +3780,12 @@ struct longoptions #if 0 static -void dump_longoptions(struct longoptions * longoptions) +void dump_longoptions(struct longoptions *longoptions) { int i; printf("getoptval = %d\n", longoptions->getoptval); printf("noptions = %d\n", longoptions->noptions); - for (i = 0; i < longoptions->noptions; i++) - { + for (i = 0; i < longoptions->noptions; i++) { printf("options[%d].name = \"%s\"\n", i, longoptions->options[i].name); @@ -3820,7 +3797,7 @@ void dump_longoptions(struct longoptions * longoptions) #endif static -struct longoptions * alloc_longoptions(struct arg_hdr * *table) +struct longoptions *alloc_longoptions(struct arg_hdr * *table) { struct longoptions *result; size_t nbytes; @@ -3839,16 +3816,14 @@ struct longoptions * alloc_longoptions(struct arg_hdr * *table) * and return that count in logoptlen. */ tabindex = 0; - do - { + do { const char *longopts = table[tabindex]->longopts; longoptlen += (longopts ? strlen(longopts) : 0) + 1; - while (longopts) - { + while (longopts) { noptions++; longopts = strchr(longopts + 1, ','); } - } while(!(table[tabindex++]->flag & ARG_TERMINATOR)); + } while (!(table[tabindex++]->flag & ARG_TERMINATOR)); /*printf("%d long options consuming %d chars in total\n",noptions,longoptlen);*/ @@ -3858,8 +3833,7 @@ struct longoptions * alloc_longoptions(struct arg_hdr * *table) + sizeof(struct option) * noptions + longoptlen; result = (struct longoptions *)malloc(nbytes); - if (result) - { + if (result) { int option_index = 0; char *store; @@ -3868,12 +3842,10 @@ struct longoptions * alloc_longoptions(struct arg_hdr * *table) result->options = (struct option *)(result + 1); store = (char *)(result->options + noptions); - for(tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) - { + for (tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) { const char *longopts = table[tabindex]->longopts; - while(longopts && *longopts) - { + while (longopts && *longopts) { char *storestart = store; /* copy progressive longopt strings into the store */ @@ -3909,34 +3881,30 @@ struct longoptions * alloc_longoptions(struct arg_hdr * *table) } static -char * alloc_shortoptions(struct arg_hdr * *table) +char *alloc_shortoptions(struct arg_hdr * *table) { char *result; size_t len = 2; int tabindex; /* determine the total number of option chars required */ - for(tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) - { + for (tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) { struct arg_hdr *hdr = table[tabindex]; len += 3 * (hdr->shortopts ? strlen(hdr->shortopts) : 0); } result = malloc(len); - if (result) - { + if (result) { char *res = result; /* add a leading ':' so getopt return codes distinguish */ /* unrecognised option and options missing argument values */ *res++ = ':'; - for(tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) - { + for (tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) { struct arg_hdr *hdr = table[tabindex]; const char *shortopts = hdr->shortopts; - while(shortopts && *shortopts) - { + while (shortopts && *shortopts) { *res++ = *shortopts++; if (hdr->flag & ARG_HASVALUE) *res++ = ':'; @@ -3980,8 +3948,7 @@ void arg_parse_tagged(int argc, /* if the allocs fail then put an error msg in the last table entry. */ longoptions = alloc_longoptions(table); shortoptions = alloc_shortoptions(table); - if (!longoptions || !shortoptions) - { + if (!longoptions || !shortoptions) { /* one or both memory allocs failed */ arg_register_error(endtable, endtable, ARG_EMALLOC, NULL); /* free anything that was allocated (this is null safe) */ @@ -3997,92 +3964,81 @@ void arg_parse_tagged(int argc, opterr = 0; /* fetch and process args using getopt_long */ - while( (copt = + while ((copt = getopt_long(argc, argv, shortoptions, longoptions->options, - NULL)) != -1) - { + NULL)) != -1) { /* printf("optarg='%s'\n",optarg); printf("optind=%d\n",optind); printf("copt=%c\n",(char)copt); printf("optopt=%c (%d)\n",optopt, (int)(optopt)); */ - switch(copt) - { - case 0: - { - int tabindex = longoptions->getoptval; - void *parent = table[tabindex]->parent; - /*printf("long option detected from argtable[%d]\n", tabindex);*/ - if (optarg && optarg[0] == 0 && - (table[tabindex]->flag & ARG_HASVALUE)) - { - /* printf(": long option %s requires an argument\n",argv[optind-1]); */ - arg_register_error(endtable, endtable, ARG_EMISSARG, - argv[optind - 1]); - /* continue to scan the (empty) argument value to enforce argument count checking */ - } - if (table[tabindex]->scanfn) - { - int errorcode = table[tabindex]->scanfn(parent, optarg); - if (errorcode != 0) - arg_register_error(endtable, parent, errorcode, optarg); - } - } - break; - - case '?': - /* - * getopt_long() found an unrecognised short option. - * if it was a short option its value is in optopt - * if it was a long option then optopt=0 - */ - switch (optopt) - { - case 0: - /*printf("?0 unrecognised long option %s\n",argv[optind-1]);*/ - arg_register_error(endtable, endtable, ARG_ELONGOPT, - argv[optind - 1]); - break; - default: - /*printf("?* unrecognised short option '%c'\n",optopt);*/ - arg_register_error(endtable, endtable, optopt, NULL); - break; - } - break; - - case ':': - /* - * getopt_long() found an option with its argument missing. - */ - /*printf(": option %s requires an argument\n",argv[optind-1]); */ - arg_register_error(endtable, endtable, ARG_EMISSARG, - argv[optind - 1]); - break; - - default: - { - /* getopt_long() found a valid short option */ - int tabindex = find_shortoption(table, (char)copt); - /*printf("short option detected from argtable[%d]\n", tabindex);*/ - if (tabindex == -1) - { - /* should never get here - but handle it just in case */ - /*printf("unrecognised short option %d\n",copt);*/ - arg_register_error(endtable, endtable, copt, NULL); - } - else - { - if (table[tabindex]->scanfn) - { - void *parent = table[tabindex]->parent; + switch (copt) { + case 0: { + int tabindex = longoptions->getoptval; + void *parent = table[tabindex]->parent; + /*printf("long option detected from argtable[%d]\n", tabindex);*/ + if (optarg && optarg[0] == 0 && + (table[tabindex]->flag & ARG_HASVALUE)) { + /* printf(": long option %s requires an argument\n",argv[optind-1]); */ + arg_register_error(endtable, endtable, ARG_EMISSARG, + argv[optind - 1]); + /* continue to scan the (empty) argument value to enforce argument count checking */ + } + if (table[tabindex]->scanfn) { int errorcode = table[tabindex]->scanfn(parent, optarg); if (errorcode != 0) arg_register_error(endtable, parent, errorcode, optarg); } } break; - } + + case '?': + /* + * getopt_long() found an unrecognised short option. + * if it was a short option its value is in optopt + * if it was a long option then optopt=0 + */ + switch (optopt) { + case 0: + /*printf("?0 unrecognised long option %s\n",argv[optind-1]);*/ + arg_register_error(endtable, endtable, ARG_ELONGOPT, + argv[optind - 1]); + break; + default: + /*printf("?* unrecognised short option '%c'\n",optopt);*/ + arg_register_error(endtable, endtable, optopt, NULL); + break; + } + break; + + case ':': + /* + * getopt_long() found an option with its argument missing. + */ + /*printf(": option %s requires an argument\n",argv[optind-1]); */ + arg_register_error(endtable, endtable, ARG_EMISSARG, + argv[optind - 1]); + break; + + default: { + /* getopt_long() found a valid short option */ + int tabindex = find_shortoption(table, (char)copt); + /*printf("short option detected from argtable[%d]\n", tabindex);*/ + if (tabindex == -1) { + /* should never get here - but handle it just in case */ + /*printf("unrecognised short option %d\n",copt);*/ + arg_register_error(endtable, endtable, copt, NULL); + } else { + if (table[tabindex]->scanfn) { + void *parent = table[tabindex]->parent; + int errorcode = table[tabindex]->scanfn(parent, optarg); + if (errorcode != 0) + arg_register_error(endtable, parent, errorcode, optarg); + } + } + break; + } } } @@ -4103,29 +4059,25 @@ void arg_parse_untagged(int argc, void *parentlast = NULL; /*printf("arg_parse_untagged(%d,%p,%p,%p)\n",argc,argv,table,endtable);*/ - while (!(table[tabindex]->flag & ARG_TERMINATOR)) - { + while (!(table[tabindex]->flag & ARG_TERMINATOR)) { void *parent; int errorcode; /* if we have exhausted our argv[optind] entries then we have finished */ - if (optind >= argc) - { + if (optind >= argc) { /*printf("arg_parse_untagged(): argv[] exhausted\n");*/ return; } /* skip table entries with non-null long or short options (they are not untagged entries) */ - if (table[tabindex]->longopts || table[tabindex]->shortopts) - { + if (table[tabindex]->longopts || table[tabindex]->shortopts) { /*printf("arg_parse_untagged(): skipping argtable[%d] (tagged argument)\n",tabindex);*/ tabindex++; continue; } /* skip table entries with NULL scanfn */ - if (!(table[tabindex]->scanfn)) - { + if (!(table[tabindex]->scanfn)) { /*printf("arg_parse_untagged(): skipping argtable[%d] (NULL scanfn)\n",tabindex);*/ tabindex++; continue; @@ -4136,17 +4088,14 @@ void arg_parse_untagged(int argc, /* try again with the next table[] entry. */ parent = table[tabindex]->parent; errorcode = table[tabindex]->scanfn(parent, argv[optind]); - if (errorcode == 0) - { + if (errorcode == 0) { /* success, move onto next argv[optind] but stay with same table[tabindex] */ /*printf("arg_parse_untagged(): argtable[%d] successfully matched\n",tabindex);*/ optind++; /* clear the last tentative error */ errorlast = 0; - } - else - { + } else { /* failure, try same argv[optind] with next table[tabindex] entry */ /*printf("arg_parse_untagged(): argtable[%d] failed match\n",tabindex);*/ tabindex++; @@ -4160,16 +4109,14 @@ void arg_parse_untagged(int argc, } /* if a tenative error still remains at this point then register it as a proper error */ - if (errorlast) - { + if (errorlast) { arg_register_error(endtable, parentlast, errorlast, optarglast); optind++; } /* only get here when not all argv[] entries were consumed */ /* register an error for each unused argv[] entry */ - while (optind < argc) - { + while (optind < argc) { /*printf("arg_parse_untagged(): argv[%d]=\"%s\" not consumed\n",optind,argv[optind]);*/ arg_register_error(endtable, endtable, ARG_ENOMATCH, argv[optind++]); } @@ -4183,16 +4130,14 @@ void arg_parse_check(struct arg_hdr * *table, struct arg_end *endtable) { int tabindex = 0; /* printf("arg_parse_check()\n"); */ - do - { - if (table[tabindex]->checkfn) - { + do { + if (table[tabindex]->checkfn) { void *parent = table[tabindex]->parent; int errorcode = table[tabindex]->checkfn(parent); if (errorcode != 0) arg_register_error(endtable, parent, errorcode, NULL); } - } while(!(table[tabindex++]->flag & ARG_TERMINATOR)); + } while (!(table[tabindex++]->flag & ARG_TERMINATOR)); } @@ -4202,11 +4147,10 @@ void arg_reset(void * *argtable) struct arg_hdr * *table = (struct arg_hdr * *)argtable; int tabindex = 0; /*printf("arg_reset(%p)\n",argtable);*/ - do - { + do { if (table[tabindex]->resetfn) table[tabindex]->resetfn(table[tabindex]->parent); - } while(!(table[tabindex++]->flag & ARG_TERMINATOR)); + } while (!(table[tabindex++]->flag & ARG_TERMINATOR)); } @@ -4229,8 +4173,7 @@ int arg_parse(int argc, char * *argv, void * *argtable) /* Special case of argc==0. This can occur on Texas Instruments DSP. */ /* Failure to trap this case results in an unwanted NULL result from */ /* the malloc for argvcopy (next code block). */ - if (argc == 0) - { + if (argc == 0) { /* We must still perform post-parse checks despite the absence of command line arguments */ arg_parse_check(table, endtable); @@ -4239,8 +4182,7 @@ int arg_parse(int argc, char * *argv, void * *argtable) } argvcopy = (char **)malloc(sizeof(char *) * (argc + 1)); - if (argvcopy) - { + if (argvcopy) { int i; /* @@ -4265,9 +4207,7 @@ int arg_parse(int argc, char * *argv, void * *argtable) /* release the local copt of argv[] */ free(argvcopy); - } - else - { + } else { /* memory alloc failed */ arg_register_error(endtable, endtable, ARG_EMALLOC, NULL); } @@ -4303,11 +4243,11 @@ void arg_cat(char * *pdest, const char *src, size_t *pndest) char *end = dest + *pndest; /*locate null terminator of dest string */ - while(dest < end && *dest != 0) + while (dest < end && *dest != 0) dest++; /* concat src string to dest string */ - while(dest < end && *src != 0) + while (dest < end && *src != 0) *dest++ = *src++; /* null terminate dest string */ @@ -4327,8 +4267,7 @@ void arg_cat_option(char *dest, const char *datatype, int optvalue) { - if (shortopts) - { + if (shortopts) { char option[3]; /* note: option array[] is initialiazed dynamically here to satisfy */ @@ -4338,21 +4277,16 @@ void arg_cat_option(char *dest, option[2] = 0; arg_cat(&dest, option, &ndest); - if (datatype) - { + if (datatype) { arg_cat(&dest, " ", &ndest); - if (optvalue) - { + if (optvalue) { arg_cat(&dest, "[", &ndest); arg_cat(&dest, datatype, &ndest); arg_cat(&dest, "]", &ndest); - } - else + } else arg_cat(&dest, datatype, &ndest); } - } - else if (longopts) - { + } else if (longopts) { size_t ncspn; /* add "--" tag prefix */ @@ -4366,28 +4300,21 @@ void arg_cat_option(char *dest, strncat(dest, longopts, (ncspn < ndest) ? ncspn : ndest); #endif - if (datatype) - { + if (datatype) { arg_cat(&dest, "=", &ndest); - if (optvalue) - { + if (optvalue) { arg_cat(&dest, "[", &ndest); arg_cat(&dest, datatype, &ndest); arg_cat(&dest, "]", &ndest); - } - else + } else arg_cat(&dest, datatype, &ndest); } - } - else if (datatype) - { - if (optvalue) - { + } else if (datatype) { + if (optvalue) { arg_cat(&dest, "[", &ndest); arg_cat(&dest, datatype, &ndest); arg_cat(&dest, "]", &ndest); - } - else + } else arg_cat(&dest, datatype, &ndest); } } @@ -4403,11 +4330,9 @@ void arg_cat_optionv(char *dest, { separator = separator ? separator : ""; - if (shortopts) - { + if (shortopts) { const char *c = shortopts; - while(*c) - { + while (*c) { /* "-a|-b|-c" */ char shortopt[3]; @@ -4427,11 +4352,9 @@ void arg_cat_optionv(char *dest, if (shortopts && longopts) arg_cat(&dest, separator, &ndest); - if (longopts) - { + if (longopts) { const char *c = longopts; - while(*c) - { + while (*c) { size_t ncspn; /* add "--" tag prefix */ @@ -4447,28 +4370,24 @@ void arg_cat_optionv(char *dest, c += ncspn; /* add given separator in place of comma */ - if (*c == ',') - { + if (*c == ',') { arg_cat(&dest, separator, &ndest); c++; } } } - if (datatype) - { + if (datatype) { if (longopts) arg_cat(&dest, "=", &ndest); else if (shortopts) arg_cat(&dest, " ", &ndest); - if (optvalue) - { + if (optvalue) { arg_cat(&dest, "[", &ndest); arg_cat(&dest, datatype, &ndest); arg_cat(&dest, "]", &ndest); - } - else + } else arg_cat(&dest, datatype, &ndest); } } @@ -4512,10 +4431,9 @@ void arg_print_gnuswitch(FILE *fp, struct arg_hdr * *table) char *suffix = ""; /* print all mandatory switches that are without argument values */ - for(tabindex = 0; - table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); - tabindex++) - { + for (tabindex = 0; + table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); + tabindex++) { /* skip optional options */ if (table[tabindex]->mincount < 1) continue; @@ -4535,10 +4453,9 @@ void arg_print_gnuswitch(FILE *fp, struct arg_hdr * *table) } /* print all optional switches that are without argument values */ - for(tabindex = 0; - table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); - tabindex++) - { + for (tabindex = 0; + table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); + tabindex++) { /* skip mandatory args */ if (table[tabindex]->mincount > 0) continue; @@ -4570,10 +4487,9 @@ void arg_print_syntax(FILE *fp, void * *argtable, const char *suffix) arg_print_gnuswitch(fp, table); /* print remaining options in abbreviated style */ - for(tabindex = 0; - table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); - tabindex++) - { + for (tabindex = 0; + table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); + tabindex++) { char syntax[200] = ""; const char *shortopts, *longopts, *datatype; @@ -4592,26 +4508,24 @@ void arg_print_syntax(FILE *fp, void * *argtable, const char *suffix) datatype, table[tabindex]->flag & ARG_HASOPTVALUE); - if (strlen(syntax) > 0) - { + if (strlen(syntax) > 0) { /* print mandatory instances of this option */ for (i = 0; i < table[tabindex]->mincount; i++) fprintf(fp, " %s", syntax); /* print optional instances enclosed in "[..]" */ - switch ( table[tabindex]->maxcount - table[tabindex]->mincount ) - { - case 0: - break; - case 1: - fprintf(fp, " [%s]", syntax); - break; - case 2: - fprintf(fp, " [%s] [%s]", syntax, syntax); - break; - default: - fprintf(fp, " [%s]...", syntax); - break; + switch (table[tabindex]->maxcount - table[tabindex]->mincount) { + case 0: + break; + case 1: + fprintf(fp, " [%s]", syntax); + break; + case 2: + fprintf(fp, " [%s] [%s]", syntax, syntax); + break; + default: + fprintf(fp, " [%s]...", syntax); + break; } } } @@ -4627,10 +4541,9 @@ void arg_print_syntaxv(FILE *fp, void * *argtable, const char *suffix) int i, tabindex; /* print remaining options in abbreviated style */ - for(tabindex = 0; - table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); - tabindex++) - { + for (tabindex = 0; + table[tabindex] && !(table[tabindex]->flag & ARG_TERMINATOR); + tabindex++) { char syntax[200] = ""; const char *shortopts, *longopts, *datatype; @@ -4650,19 +4563,18 @@ void arg_print_syntaxv(FILE *fp, void * *argtable, const char *suffix) fprintf(fp, " %s", syntax); /* print optional args enclosed in "[..]" */ - switch ( table[tabindex]->maxcount - table[tabindex]->mincount ) - { - case 0: - break; - case 1: - fprintf(fp, " [%s]", syntax); - break; - case 2: - fprintf(fp, " [%s] [%s]", syntax, syntax); - break; - default: - fprintf(fp, " [%s]...", syntax); - break; + switch (table[tabindex]->maxcount - table[tabindex]->mincount) { + case 0: + break; + case 1: + fprintf(fp, " [%s]", syntax); + break; + case 2: + fprintf(fp, " [%s] [%s]", syntax, syntax); + break; + default: + fprintf(fp, " [%s]...", syntax); + break; } } @@ -4677,10 +4589,8 @@ void arg_print_glossary(FILE *fp, void * *argtable, const char *format) int tabindex; format = format ? format : " %-20s %s\n"; - for (tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) - { - if (table[tabindex]->glossary) - { + for (tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) { + if (table[tabindex]->glossary) { char syntax[200] = ""; const char *shortopts = table[tabindex]->shortopts; const char *longopts = table[tabindex]->longopts; @@ -4732,35 +4642,34 @@ void arg_print_glossary(FILE *fp, void * *argtable, const char *format) * Author: Uli Fouquet */ static -void arg_print_formatted( FILE *fp, - const unsigned lmargin, - const unsigned rmargin, - const char *text ) +void arg_print_formatted(FILE *fp, + const unsigned lmargin, + const unsigned rmargin, + const char *text) { - const unsigned textlen = (unsigned)strlen( text ); + const unsigned textlen = (unsigned)strlen(text); unsigned line_start = 0; unsigned line_end = textlen + 1; const unsigned colwidth = (rmargin - lmargin) + 1; /* Someone doesn't like us... */ - if ( line_end < line_start ) - { fprintf( fp, "%s\n", text ); } + if (line_end < line_start) + { fprintf(fp, "%s\n", text); } - while (line_end - 1 > line_start ) - { + while (line_end - 1 > line_start) { /* Eat leading whitespaces. This is essential because while wrapping lines, there will often be a whitespace at beginning of line */ - while ( ISSPACE(*(text + line_start)) ) + while (ISSPACE(*(text + line_start))) { line_start++; } - if ((line_end - line_start) > colwidth ) + if ((line_end - line_start) > colwidth) { line_end = line_start + colwidth; } /* Find last whitespace, that fits into line */ - while ( ( line_end > line_start ) - && ( line_end - line_start > colwidth ) - && !ISSPACE(*(text + line_end))) + while ((line_end > line_start) + && (line_end - line_start > colwidth) + && !ISSPACE(*(text + line_end))) { line_end--; } /* Do not print trailing whitespace. If this text @@ -4769,20 +4678,18 @@ void arg_print_formatted( FILE *fp, line_end--; /* Output line of text */ - while ( line_start < line_end ) - { - fputc(*(text + line_start), fp ); + while (line_start < line_end) { + fputc(*(text + line_start), fp); line_start++; } - fputc( '\n', fp ); + fputc('\n', fp); /* Initialize another line */ - if ( line_end + 1 < textlen ) - { + if (line_end + 1 < textlen) { unsigned i; - for (i = 0; i < lmargin; i++ ) - { fputc( ' ', fp ); } + for (i = 0; i < lmargin; i++) + { fputc(' ', fp); } line_end = textlen; } @@ -4802,25 +4709,22 @@ void arg_print_formatted( FILE *fp, * * Contributed by Uli Fouquet */ -void arg_print_glossary_gnu(FILE *fp, void * *argtable ) +void arg_print_glossary_gnu(FILE *fp, void * *argtable) { struct arg_hdr * *table = (struct arg_hdr * *)argtable; int tabindex; - for(tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) - { - if (table[tabindex]->glossary) - { + for (tabindex = 0; !(table[tabindex]->flag & ARG_TERMINATOR); tabindex++) { + if (table[tabindex]->glossary) { char syntax[200] = ""; const char *shortopts = table[tabindex]->shortopts; const char *longopts = table[tabindex]->longopts; const char *datatype = table[tabindex]->datatype; const char *glossary = table[tabindex]->glossary; - if ( !shortopts && longopts ) - { + if (!shortopts && longopts) { /* Indent trailing line by 4 spaces... */ - memset( syntax, ' ', 4 ); + memset(syntax, ' ', 4); *(syntax + 4) = '\0'; } @@ -4833,18 +4737,17 @@ void arg_print_glossary_gnu(FILE *fp, void * *argtable ) ", "); /* If syntax fits not into column, print glossary in new line... */ - if ( strlen(syntax) > 25 ) - { - fprintf( fp, " %-25s %s\n", syntax, "" ); + if (strlen(syntax) > 25) { + fprintf(fp, " %-25s %s\n", syntax, ""); *syntax = '\0'; } - fprintf( fp, " %-25s ", syntax ); - arg_print_formatted( fp, 28, 79, glossary ); + fprintf(fp, " %-25s ", syntax); + arg_print_formatted(fp, 28, 79, glossary); } } /* for each table entry */ - fputc( '\n', fp ); + fputc('\n', fp); } @@ -4862,12 +4765,11 @@ int arg_nullcheck(void * *argtable) return 1; tabindex = 0; - do - { + do { /*printf("argtable[%d]=%p\n",tabindex,argtable[tabindex]);*/ if (!table[tabindex]) return 1; - } while(!(table[tabindex++]->flag & ARG_TERMINATOR)); + } while (!(table[tabindex++]->flag & ARG_TERMINATOR)); return 0; } @@ -4890,8 +4792,7 @@ void arg_free(void * *argtable) int tabindex = 0; int flag; /*printf("arg_free(%p)\n",argtable);*/ - do - { + do { /* if we encounter a NULL entry then somewhat incorrectly we presume we have come to the end of the array. It isnt strictly true because @@ -4905,7 +4806,7 @@ void arg_free(void * *argtable) free(table[tabindex]); table[tabindex++] = NULL; - } while(!(flag & ARG_TERMINATOR)); + } while (!(flag & ARG_TERMINATOR)); } /* frees each non-NULL element of argtable[], where n is the size of the number of entries in the array */ @@ -4914,8 +4815,7 @@ void arg_freetable(void * *argtable, size_t n) struct arg_hdr * *table = (struct arg_hdr * *)argtable; size_t tabindex = 0; /*printf("arg_freetable(%p)\n",argtable);*/ - for (tabindex = 0; tabindex < n; tabindex++) - { + for (tabindex = 0; tabindex < n; tabindex++) { if (table[tabindex] == NULL) continue; diff --git a/client/cliparser/argtable3.h b/client/cliparser/argtable3.h index 452d0283a..a442ec0d5 100644 --- a/client/cliparser/argtable3.h +++ b/client/cliparser/argtable3.h @@ -41,16 +41,15 @@ extern "C" { #define ARG_REX_ICASE 1 /* bit masks for arg_hdr.flag */ -enum -{ - ARG_TERMINATOR=0x1, - ARG_HASVALUE=0x2, - ARG_HASOPTVALUE=0x4 +enum { + ARG_TERMINATOR = 0x1, + ARG_HASVALUE = 0x2, + ARG_HASOPTVALUE = 0x4 }; typedef void (arg_resetfn)(void *parent); -typedef int (arg_scanfn)(void *parent, const char *argval); -typedef int (arg_checkfn)(void *parent); +typedef int (arg_scanfn)(void *parent, const char *argval); +typedef int (arg_checkfn)(void *parent); typedef void (arg_errorfn)(void *parent, FILE *fp, int error, const char *argval, const char *progname); @@ -71,8 +70,7 @@ typedef void (arg_errorfn)(void *parent, FILE *fp, int error, const char *argval * if desired, but the original intention is for them to be set by the * constructor and left unaltered. */ -struct arg_hdr -{ +struct arg_hdr { char flag; /* Modifier flags: ARG_TERMINATOR, ARG_HASVALUE. */ const char *shortopts; /* String defining the short options */ const char *longopts; /* String defiing the long options */ @@ -88,47 +86,40 @@ struct arg_hdr void *priv; /* Pointer to private header data for use by arg_xxx functions */ }; -struct arg_rem -{ +struct arg_rem { struct arg_hdr hdr; /* The mandatory argtable header struct */ }; -struct arg_lit -{ +struct arg_lit { struct arg_hdr hdr; /* The mandatory argtable header struct */ int count; /* Number of matching command line args */ }; -struct arg_int -{ +struct arg_int { struct arg_hdr hdr; /* The mandatory argtable header struct */ int count; /* Number of matching command line args */ int *ival; /* Array of parsed argument values */ }; -struct arg_dbl -{ +struct arg_dbl { struct arg_hdr hdr; /* The mandatory argtable header struct */ int count; /* Number of matching command line args */ double *dval; /* Array of parsed argument values */ }; -struct arg_str -{ +struct arg_str { struct arg_hdr hdr; /* The mandatory argtable header struct */ int count; /* Number of matching command line args */ const char **sval; /* Array of parsed argument values */ }; -struct arg_rex -{ +struct arg_rex { struct arg_hdr hdr; /* The mandatory argtable header struct */ int count; /* Number of matching command line args */ const char **sval; /* Array of parsed argument values */ }; -struct arg_file -{ +struct arg_file { struct arg_hdr hdr; /* The mandatory argtable header struct */ int count; /* Number of matching command line args*/ const char **filename; /* Array of parsed filenames (eg: /home/foo.bar) */ @@ -136,17 +127,15 @@ struct arg_file const char **extension; /* Array of parsed extensions (eg: .bar) */ }; -struct arg_date -{ +struct arg_date { struct arg_hdr hdr; /* The mandatory argtable header struct */ const char *format; /* strptime format string used to parse the date */ int count; /* Number of matching command line args */ struct tm *tmval; /* Array of parsed time values */ }; -enum {ARG_ELIMIT=1, ARG_EMALLOC, ARG_ENOMATCH, ARG_ELONGOPT, ARG_EMISSARG}; -struct arg_end -{ +enum {ARG_ELIMIT = 1, ARG_EMALLOC, ARG_ENOMATCH, ARG_ELONGOPT, ARG_EMISSARG}; +struct arg_end { struct arg_hdr hdr; /* The mandatory argtable header struct */ int count; /* Number of errors encountered */ int *error; /* Array of error codes */ @@ -157,132 +146,132 @@ struct arg_end /**** arg_xxx constructor functions *********************************/ -struct arg_rem* arg_rem(const char* datatype, const char* glossary); +struct arg_rem *arg_rem(const char *datatype, const char *glossary); -struct arg_lit* arg_lit0(const char* shortopts, - const char* longopts, - const char* glossary); -struct arg_lit* arg_lit1(const char* shortopts, - const char* longopts, - const char *glossary); -struct arg_lit* arg_litn(const char* shortopts, - const char* longopts, - int mincount, - int maxcount, - const char *glossary); +struct arg_lit *arg_lit0(const char *shortopts, + const char *longopts, + const char *glossary); +struct arg_lit *arg_lit1(const char *shortopts, + const char *longopts, + const char *glossary); +struct arg_lit *arg_litn(const char *shortopts, + const char *longopts, + int mincount, + int maxcount, + const char *glossary); -struct arg_key* arg_key0(const char* keyword, - int flags, - const char* glossary); -struct arg_key* arg_key1(const char* keyword, - int flags, - const char* glossary); -struct arg_key* arg_keyn(const char* keyword, - int flags, - int mincount, - int maxcount, - const char* glossary); +struct arg_key *arg_key0(const char *keyword, + int flags, + const char *glossary); +struct arg_key *arg_key1(const char *keyword, + int flags, + const char *glossary); +struct arg_key *arg_keyn(const char *keyword, + int flags, + int mincount, + int maxcount, + const char *glossary); -struct arg_int* arg_int0(const char* shortopts, - const char* longopts, - const char* datatype, - const char* glossary); -struct arg_int* arg_int1(const char* shortopts, - const char* longopts, - const char* datatype, - const char *glossary); -struct arg_int* arg_intn(const char* shortopts, - const char* longopts, - const char *datatype, - int mincount, - int maxcount, - const char *glossary); +struct arg_int *arg_int0(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_int *arg_int1(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_int *arg_intn(const char *shortopts, + const char *longopts, + const char *datatype, + int mincount, + int maxcount, + const char *glossary); -struct arg_dbl* arg_dbl0(const char* shortopts, - const char* longopts, - const char* datatype, - const char* glossary); -struct arg_dbl* arg_dbl1(const char* shortopts, - const char* longopts, - const char* datatype, - const char *glossary); -struct arg_dbl* arg_dbln(const char* shortopts, - const char* longopts, - const char *datatype, - int mincount, - int maxcount, - const char *glossary); +struct arg_dbl *arg_dbl0(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_dbl *arg_dbl1(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_dbl *arg_dbln(const char *shortopts, + const char *longopts, + const char *datatype, + int mincount, + int maxcount, + const char *glossary); -struct arg_str* arg_str0(const char* shortopts, - const char* longopts, - const char* datatype, - const char* glossary); -struct arg_str* arg_str1(const char* shortopts, - const char* longopts, - const char* datatype, - const char *glossary); -struct arg_str* arg_strn(const char* shortopts, - const char* longopts, - const char* datatype, - int mincount, - int maxcount, - const char *glossary); +struct arg_str *arg_str0(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_str *arg_str1(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_str *arg_strn(const char *shortopts, + const char *longopts, + const char *datatype, + int mincount, + int maxcount, + const char *glossary); -struct arg_rex* arg_rex0(const char* shortopts, - const char* longopts, - const char* pattern, - const char* datatype, - int flags, - const char* glossary); -struct arg_rex* arg_rex1(const char* shortopts, - const char* longopts, - const char* pattern, - const char* datatype, - int flags, - const char *glossary); -struct arg_rex* arg_rexn(const char* shortopts, - const char* longopts, - const char* pattern, - const char* datatype, - int mincount, - int maxcount, - int flags, - const char *glossary); +struct arg_rex *arg_rex0(const char *shortopts, + const char *longopts, + const char *pattern, + const char *datatype, + int flags, + const char *glossary); +struct arg_rex *arg_rex1(const char *shortopts, + const char *longopts, + const char *pattern, + const char *datatype, + int flags, + const char *glossary); +struct arg_rex *arg_rexn(const char *shortopts, + const char *longopts, + const char *pattern, + const char *datatype, + int mincount, + int maxcount, + int flags, + const char *glossary); -struct arg_file* arg_file0(const char* shortopts, - const char* longopts, - const char* datatype, - const char* glossary); -struct arg_file* arg_file1(const char* shortopts, - const char* longopts, - const char* datatype, - const char *glossary); -struct arg_file* arg_filen(const char* shortopts, - const char* longopts, - const char* datatype, - int mincount, - int maxcount, - const char *glossary); +struct arg_file *arg_file0(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_file *arg_file1(const char *shortopts, + const char *longopts, + const char *datatype, + const char *glossary); +struct arg_file *arg_filen(const char *shortopts, + const char *longopts, + const char *datatype, + int mincount, + int maxcount, + const char *glossary); -struct arg_date* arg_date0(const char* shortopts, - const char* longopts, - const char* format, - const char* datatype, - const char* glossary); -struct arg_date* arg_date1(const char* shortopts, - const char* longopts, - const char* format, - const char* datatype, - const char *glossary); -struct arg_date* arg_daten(const char* shortopts, - const char* longopts, - const char* format, - const char* datatype, - int mincount, - int maxcount, - const char *glossary); +struct arg_date *arg_date0(const char *shortopts, + const char *longopts, + const char *format, + const char *datatype, + const char *glossary); +struct arg_date *arg_date1(const char *shortopts, + const char *longopts, + const char *format, + const char *datatype, + const char *glossary); +struct arg_date *arg_daten(const char *shortopts, + const char *longopts, + const char *format, + const char *datatype, + int mincount, + int maxcount, + const char *glossary); -struct arg_end* arg_end(int maxerrors); +struct arg_end *arg_end(int maxerrors); /**** other functions *******************************************/ @@ -293,7 +282,7 @@ void arg_print_syntax(FILE *fp, void **argtable, const char *suffix); void arg_print_syntaxv(FILE *fp, void **argtable, const char *suffix); void arg_print_glossary(FILE *fp, void **argtable, const char *format); void arg_print_glossary_gnu(FILE *fp, void **argtable); -void arg_print_errors(FILE* fp, struct arg_end* end, const char* progname); +void arg_print_errors(FILE *fp, struct arg_end *end, const char *progname); void arg_freetable(void **argtable, size_t n); /**** deprecated functions, for back-compatibility only ********/ diff --git a/client/cliparser/cliparser.c b/client/cliparser/cliparser.c index e0c853d23..5383712b0 100644 --- a/client/cliparser/cliparser.c +++ b/client/cliparser/cliparser.c @@ -19,7 +19,8 @@ char *programHint = NULL; char *programHelp = NULL; char buf[500] = {0}; -int CLIParserInit(char *vprogramName, char *vprogramHint, char *vprogramHelp) { +int CLIParserInit(char *vprogramName, char *vprogramHint, char *vprogramHelp) +{ argtable = NULL; argtableLen = 0; programName = vprogramName; @@ -30,7 +31,8 @@ int CLIParserInit(char *vprogramName, char *vprogramHint, char *vprogramHelp) { return 0; } -int CLIParserParseArg(int argc, char **argv, void* vargtable[], size_t vargtableLen, bool allowEmptyExec) { +int CLIParserParseArg(int argc, char **argv, void *vargtable[], size_t vargtableLen, bool allowEmptyExec) +{ int nerrors; argtable = vargtable; @@ -46,7 +48,7 @@ int CLIParserParseArg(int argc, char **argv, void* vargtable[], size_t vargtable nerrors = arg_parse(argc, argv, argtable); /* special case: '--help' takes precedence over error reporting */ - if ((argc < 2 && !allowEmptyExec) ||((struct arg_lit *)argtable[0])->count > 0) { // help must be the first record + if ((argc < 2 && !allowEmptyExec) || ((struct arg_lit *)argtable[0])->count > 0) { // help must be the first record printf("Usage: %s", programName); arg_print_syntaxv(stdout, argtable, "\n"); if (programHint) @@ -79,11 +81,13 @@ enum ParserState { #define isSpace(c)(c == ' ' || c == '\t') -int CLIParserParseString(const char* str, void* vargtable[], size_t vargtableLen, bool allowEmptyExec) { +int CLIParserParseString(const char *str, void *vargtable[], size_t vargtableLen, bool allowEmptyExec) +{ return CLIParserParseStringEx(str, vargtable, vargtableLen, allowEmptyExec, false); } -int CLIParserParseStringEx(const char* str, void* vargtable[], size_t vargtableLen, bool allowEmptyExec, bool clueData) { +int CLIParserParseStringEx(const char *str, void *vargtable[], size_t vargtableLen, bool allowEmptyExec, bool clueData) +{ int argc = 0; char *argv[200] = {NULL}; @@ -101,9 +105,9 @@ int CLIParserParseStringEx(const char* str, void* vargtable[], size_t vargtableL // parse params for (int i = 0; i < len; i++) { - switch(state){ + switch (state) { case PS_FIRST: // first char - if (!clueData || str[i] == '-'){ // first char before space is '-' - next element - option OR not "clueData" for not-option fields + if (!clueData || str[i] == '-') { // first char before space is '-' - next element - option OR not "clueData" for not-option fields state = PS_OPTION; if (spaceptr) { @@ -125,7 +129,7 @@ int CLIParserParseStringEx(const char* str, void* vargtable[], size_t vargtableL bufptr++; break; case PS_OPTION: - if (isSpace(str[i])){ + if (isSpace(str[i])) { state = PS_FIRST; *bufptr = 0x00; @@ -143,7 +147,8 @@ int CLIParserParseStringEx(const char* str, void* vargtable[], size_t vargtableL return CLIParserParseArg(argc, argv, vargtable, vargtableLen, allowEmptyExec); } -void CLIParserFree() { +void CLIParserFree() +{ arg_freetable(argtable, argtableLen); argtable = NULL; @@ -151,7 +156,8 @@ void CLIParserFree() { } // convertors -int CLIParamHexToBuf(struct arg_str *argstr, uint8_t *data, int maxdatalen, int *datalen) { +int CLIParamHexToBuf(struct arg_str *argstr, uint8_t *data, int maxdatalen, int *datalen) +{ *datalen = 0; int ibuf = 0; @@ -160,22 +166,23 @@ int CLIParamHexToBuf(struct arg_str *argstr, uint8_t *data, int maxdatalen, int if (res || !ibuf) return res; - switch(param_gethex_to_eol((char *)buf, 0, data, maxdatalen, datalen)) { - case 1: - printf("Parameter error: Invalid HEX value.\n"); - return 1; - case 2: - printf("Parameter error: parameter too large.\n"); - return 2; - case 3: - printf("Parameter error: Hex string must have even number of digits.\n"); - return 3; + switch (param_gethex_to_eol((char *)buf, 0, data, maxdatalen, datalen)) { + case 1: + printf("Parameter error: Invalid HEX value.\n"); + return 1; + case 2: + printf("Parameter error: parameter too large.\n"); + return 2; + case 3: + printf("Parameter error: Hex string must have even number of digits.\n"); + return 3; } return 0; } -int CLIParamStrToBuf(struct arg_str *argstr, uint8_t *data, int maxdatalen, int *datalen) { +int CLIParamStrToBuf(struct arg_str *argstr, uint8_t *data, int maxdatalen, int *datalen) +{ *datalen = 0; if (!argstr->count) return 0; diff --git a/client/cliparser/cliparser.h b/client/cliparser/cliparser.h index 05910ea43..d42cd50b6 100644 --- a/client/cliparser/cliparser.h +++ b/client/cliparser/cliparser.h @@ -32,9 +32,9 @@ #define CLIGetStrWithReturn(paramnum, data, datalen) if (CLIParamStrToBuf(arg_get_str(paramnum), data, sizeof(data), datalen)) {CLIParserFree();return 1;} extern int CLIParserInit(char *vprogramName, char *vprogramHint, char *vprogramHelp); -extern int CLIParserParseString(const char* str, void* argtable[], size_t vargtableLen, bool allowEmptyExec); -extern int CLIParserParseStringEx(const char* str, void* vargtable[], size_t vargtableLen, bool allowEmptyExec, bool clueData); -extern int CLIParserParseArg(int argc, char **argv, void* argtable[], size_t vargtableLen, bool allowEmptyExec); +extern int CLIParserParseString(const char *str, void *argtable[], size_t vargtableLen, bool allowEmptyExec); +extern int CLIParserParseStringEx(const char *str, void *vargtable[], size_t vargtableLen, bool allowEmptyExec, bool clueData); +extern int CLIParserParseArg(int argc, char **argv, void *argtable[], size_t vargtableLen, bool allowEmptyExec); extern void CLIParserFree(); extern int CLIParamHexToBuf(struct arg_str *argstr, uint8_t *data, int maxdatalen, int *datalen); diff --git a/client/cliparser/getopt.h b/client/cliparser/getopt.h index c036ff686..308acf3ce 100644 --- a/client/cliparser/getopt.h +++ b/client/cliparser/getopt.h @@ -60,13 +60,13 @@ struct option { }; __BEGIN_DECLS -int getopt_long(int, char * const *, const char *, - const struct option *, int *); -int getopt_long_only(int, char * const *, const char *, - const struct option *, int *); +int getopt_long(int, char *const *, const char *, + const struct option *, int *); +int getopt_long_only(int, char *const *, const char *, + const struct option *, int *); #ifndef _GETOPT_DECLARED #define _GETOPT_DECLARED -int getopt(int, char * const [], const char *); +int getopt(int, char *const [], const char *); extern char *optarg; /* getopt(3) external variables */ extern int optind, opterr, optopt; diff --git a/client/cmdanalyse.c b/client/cmdanalyse.c index c0e455fe2..9b326f961 100644 --- a/client/cmdanalyse.c +++ b/client/cmdanalyse.c @@ -11,7 +11,8 @@ static int CmdHelp(const char *Cmd); -int usage_analyse_lcr(void) { +int usage_analyse_lcr(void) +{ PrintAndLogEx(NORMAL, "Specifying the bytes of a UID with a known LRC will find the last byte value"); PrintAndLogEx(NORMAL, "needed to generate that LRC with a rolling XOR. All bytes should be specified in HEX."); PrintAndLogEx(NORMAL, ""); @@ -25,7 +26,8 @@ int usage_analyse_lcr(void) { PrintAndLogEx(NORMAL, "expected output: Target (BA) requires final LRC XOR byte value: 5A"); return 0; } -int usage_analyse_checksum(void) { +int usage_analyse_checksum(void) +{ PrintAndLogEx(NORMAL, "The bytes will be added with eachother and than limited with the applied mask"); PrintAndLogEx(NORMAL, "Finally compute ones' complement of the least significant bytes"); PrintAndLogEx(NORMAL, ""); @@ -41,7 +43,8 @@ int usage_analyse_checksum(void) { PrintAndLogEx(NORMAL, "expected output: 0x61"); return 0; } -int usage_analyse_crc(void){ +int usage_analyse_crc(void) +{ PrintAndLogEx(NORMAL, "A stub method to test different crc implementations inside the PM3 sourcecode. Just because you figured out the poly, doesn't mean you get the desired output"); PrintAndLogEx(NORMAL, ""); PrintAndLogEx(NORMAL, "Usage: analyse crc [h] "); @@ -53,7 +56,8 @@ int usage_analyse_crc(void){ PrintAndLogEx(NORMAL, " analyse crc 137AF00A0A0D"); return 0; } -int usage_analyse_nuid(void){ +int usage_analyse_nuid(void) +{ PrintAndLogEx(NORMAL, "Generate 4byte NUID from 7byte UID"); PrintAndLogEx(NORMAL, ""); PrintAndLogEx(NORMAL, "Usage: analyse hid [h] "); @@ -65,7 +69,8 @@ int usage_analyse_nuid(void){ PrintAndLogEx(NORMAL, " analyse nuid 11223344556677"); return 0; } -int usage_analyse_a(void) { +int usage_analyse_a(void) +{ PrintAndLogEx(NORMAL, "Iceman's personal garbage test command"); PrintAndLogEx(NORMAL, ""); PrintAndLogEx(NORMAL, "Usage: analyse a [h] d "); @@ -78,7 +83,8 @@ int usage_analyse_a(void) { return 0; } -static uint8_t calculateLRC( uint8_t* bytes, uint8_t len) { +static uint8_t calculateLRC(uint8_t *bytes, uint8_t len) +{ uint8_t LRC = 0; for (uint8_t i = 0; i < len; i++) LRC ^= bytes[i]; @@ -102,7 +108,8 @@ static uint16_t shiftadd ( uint8_t* bytes, uint8_t len){ return 0; } */ -static uint16_t calcSumCrumbAdd( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumCrumbAdd(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum += CRUMB(bytes[i], 0); @@ -113,10 +120,12 @@ static uint16_t calcSumCrumbAdd( uint8_t* bytes, uint8_t len, uint32_t mask) { sum &= mask; return sum; } -static uint16_t calcSumCrumbAddOnes( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumCrumbAddOnes(uint8_t *bytes, uint8_t len, uint32_t mask) +{ return (~calcSumCrumbAdd(bytes, len, mask) & mask); } -static uint16_t calcSumNibbleAdd( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumNibbleAdd(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum += NIBBLE_LOW(bytes[i]); @@ -125,10 +134,12 @@ static uint16_t calcSumNibbleAdd( uint8_t* bytes, uint8_t len, uint32_t mask) { sum &= mask; return sum; } -static uint16_t calcSumNibbleAddOnes( uint8_t* bytes, uint8_t len, uint32_t mask){ +static uint16_t calcSumNibbleAddOnes(uint8_t *bytes, uint8_t len, uint32_t mask) +{ return (~calcSumNibbleAdd(bytes, len, mask) & mask); } -static uint16_t calcSumCrumbXor( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumCrumbXor(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum ^= CRUMB(bytes[i], 0); @@ -139,7 +150,8 @@ static uint16_t calcSumCrumbXor( uint8_t* bytes, uint8_t len, uint32_t mask) { sum &= mask; return sum; } -static uint16_t calcSumNibbleXor( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumNibbleXor(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum ^= NIBBLE_LOW(bytes[i]); @@ -148,7 +160,8 @@ static uint16_t calcSumNibbleXor( uint8_t* bytes, uint8_t len, uint32_t mask) { sum &= mask; return sum; } -static uint16_t calcSumByteXor( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumByteXor(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum ^= bytes[i]; @@ -156,7 +169,8 @@ static uint16_t calcSumByteXor( uint8_t* bytes, uint8_t len, uint32_t mask) { sum &= mask; return sum; } -static uint16_t calcSumByteAdd( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumByteAdd(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum += bytes[i]; @@ -165,11 +179,13 @@ static uint16_t calcSumByteAdd( uint8_t* bytes, uint8_t len, uint32_t mask) { return sum; } // Ones complement -static uint16_t calcSumByteAddOnes( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumByteAddOnes(uint8_t *bytes, uint8_t len, uint32_t mask) +{ return (~calcSumByteAdd(bytes, len, mask) & mask); } -static uint16_t calcSumByteSub( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumByteSub(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint8_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum -= bytes[i]; @@ -177,10 +193,12 @@ static uint16_t calcSumByteSub( uint8_t* bytes, uint8_t len, uint32_t mask) { sum &= mask; return sum; } -static uint16_t calcSumByteSubOnes( uint8_t* bytes, uint8_t len, uint32_t mask){ +static uint16_t calcSumByteSubOnes(uint8_t *bytes, uint8_t len, uint32_t mask) +{ return (~calcSumByteSub(bytes, len, mask) & mask); } -static uint16_t calcSumNibbleSub( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumNibbleSub(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint8_t sum = 0; for (uint8_t i = 0; i < len; i++) { sum -= NIBBLE_LOW(bytes[i]); @@ -189,14 +207,16 @@ static uint16_t calcSumNibbleSub( uint8_t* bytes, uint8_t len, uint32_t mask) { sum &= mask; return sum; } -static uint16_t calcSumNibbleSubOnes( uint8_t* bytes, uint8_t len, uint32_t mask) { +static uint16_t calcSumNibbleSubOnes(uint8_t *bytes, uint8_t len, uint32_t mask) +{ return (~calcSumNibbleSub(bytes, len, mask) & mask); } // BSD shift checksum 8bit version -static uint16_t calcBSDchecksum8( uint8_t* bytes, uint8_t len, uint32_t mask){ +static uint16_t calcBSDchecksum8(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; - for(uint8_t i = 0; i < len; i++){ + for (uint8_t i = 0; i < len; i++) { sum = ((sum & 0xFF) >> 1) | ((sum & 0x1) << 7); // rotate accumulator sum += bytes[i]; // add next byte sum &= 0xFF; // @@ -205,9 +225,10 @@ static uint16_t calcBSDchecksum8( uint8_t* bytes, uint8_t len, uint32_t mask){ return sum; } // BSD shift checksum 4bit version -static uint16_t calcBSDchecksum4( uint8_t* bytes, uint8_t len, uint32_t mask){ +static uint16_t calcBSDchecksum4(uint8_t *bytes, uint8_t len, uint32_t mask) +{ uint16_t sum = 0; - for(uint8_t i = 0; i < len; i++){ + for (uint8_t i = 0; i < len; i++) { sum = ((sum & 0xF) >> 1) | ((sum & 0x1) << 3); // rotate accumulator sum += NIBBLE_HIGH(bytes[i]); // add high nibble sum &= 0xF; // @@ -220,7 +241,8 @@ static uint16_t calcBSDchecksum4( uint8_t* bytes, uint8_t len, uint32_t mask){ } // measuring LFSR maximum length -int CmdAnalyseLfsr(const char *Cmd){ +int CmdAnalyseLfsr(const char *Cmd) +{ uint16_t start_state = 0; /* Any nonzero start state will work. */ uint16_t lfsr = start_state; @@ -230,7 +252,7 @@ int CmdAnalyseLfsr(const char *Cmd){ uint8_t find = param_get8ex(Cmd, 1, 0, 16); PrintAndLogEx(NORMAL, "LEGIC LFSR IV 0x%02X: \n", iv); - PrintAndLogEx(NORMAL, " bit# | lfsr | ^0x40 | 0x%02X ^ lfsr \n",find); + PrintAndLogEx(NORMAL, " bit# | lfsr | ^0x40 | 0x%02X ^ lfsr \n", find); for (uint8_t i = 0x01; i < 0x30; i += 1) { //period = 0; @@ -238,50 +260,52 @@ int CmdAnalyseLfsr(const char *Cmd){ legic_prng_forward(i); lfsr = legic_prng_get_bits(12); - PrintAndLogEx(NORMAL, " %02X | %03X | %03X | %03X \n",i, lfsr, 0x40 ^ lfsr, find ^ lfsr); + PrintAndLogEx(NORMAL, " %02X | %03X | %03X | %03X \n", i, lfsr, 0x40 ^ lfsr, find ^ lfsr); } return 0; } -int CmdAnalyseLCR(const char *Cmd) { +int CmdAnalyseLCR(const char *Cmd) +{ uint8_t data[50]; char cmdp = tolower(param_getchar(Cmd, 0)); - if (strlen(Cmd) == 0|| cmdp == 'h') return usage_analyse_lcr(); + if (strlen(Cmd) == 0 || cmdp == 'h') return usage_analyse_lcr(); int len = 0; switch (param_gethex_to_eol(Cmd, 0, data, sizeof(data), &len)) { - case 1: - PrintAndLogEx(WARNING, "Invalid HEX value."); - return 1; - case 2: - PrintAndLogEx(WARNING, "Too many bytes. Max %d bytes", sizeof(data)); - return 1; - case 3: - PrintAndLogEx(WARNING, "Hex must have even number of digits."); - return 1; + case 1: + PrintAndLogEx(WARNING, "Invalid HEX value."); + return 1; + case 2: + PrintAndLogEx(WARNING, "Too many bytes. Max %d bytes", sizeof(data)); + return 1; + case 3: + PrintAndLogEx(WARNING, "Hex must have even number of digits."); + return 1; } uint8_t finalXor = calculateLRC(data, len); - PrintAndLogEx(NORMAL, "Target [%02X] requires final LRC XOR byte value: 0x%02X",data[len-1] ,finalXor); + PrintAndLogEx(NORMAL, "Target [%02X] requires final LRC XOR byte value: 0x%02X", data[len - 1], finalXor); return 0; } -int CmdAnalyseCRC(const char *Cmd) { +int CmdAnalyseCRC(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) == 0 || cmdp == 'h') return usage_analyse_crc(); int len = strlen(Cmd); - if ( len & 1 ) return usage_analyse_crc(); + if (len & 1) return usage_analyse_crc(); // add 1 for null terminator. - uint8_t *data = calloc(len+1, sizeof(uint8_t)); - if ( !data ) return 1; + uint8_t *data = calloc(len + 1, sizeof(uint8_t)); + if (!data) return 1; - if ( param_gethex(Cmd, 0, data, len)) { + if (param_gethex(Cmd, 0, data, len)) { free(data); return usage_analyse_crc(); } len >>= 1; - PrintAndLogEx(NORMAL, "\nTests with (%d) | %s",len, sprint_hex(data, len)); + PrintAndLogEx(NORMAL, "\nTests with (%d) | %s", len, sprint_hex(data, len)); // 51 f5 7a d6 uint8_t uid[] = {0x51, 0xf5, 0x7a, 0xd6}; //12 34 56 @@ -292,7 +316,7 @@ int CmdAnalyseCRC(const char *Cmd) { PrintAndLogEx(NORMAL, "FeliCa | %X ", crc16_xmodem(data, len)); PrintAndLogEx(NORMAL, "\nTests of reflection. Current methods in source code"); - PrintAndLogEx(NORMAL, " reflect(0x3e23L,3) is %04X == 0x3e26", reflect(0x3e23L,3) ); + PrintAndLogEx(NORMAL, " reflect(0x3e23L,3) is %04X == 0x3e26", reflect(0x3e23L, 3)); PrintAndLogEx(NORMAL, " reflect8(0x80) is %02X == 0x01", reflect8(0x80)); PrintAndLogEx(NORMAL, " reflect16(0x8000) is %04X == 0x0001", reflect16(0xc6c6)); @@ -301,14 +325,14 @@ int CmdAnalyseCRC(const char *Cmd) { compute_crc(CRC_14443_B, data, len, &b1, &b2); uint16_t crcBB_1 = b1 << 8 | b2; uint16_t bbb = crc(CRC_14443_B, data, len); - PrintAndLogEx(NORMAL, "ISO14443 crc B | %04x == %04x \n", crcBB_1, bbb ); + PrintAndLogEx(NORMAL, "ISO14443 crc B | %04x == %04x \n", crcBB_1, bbb); // Test of CRC16, '123456789' string. // PrintAndLogEx(NORMAL, "\n\nStandard test with 31 32 33 34 35 36 37 38 39 '123456789'\n\n"); - uint8_t dataStr[] = { 0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39 }; + uint8_t dataStr[] = { 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39 }; legic8 = CRC8Legic(dataStr, sizeof(dataStr)); //these below has been tested OK. @@ -323,33 +347,33 @@ int CmdAnalyseCRC(const char *Cmd) { // input from commandline PrintAndLogEx(NORMAL, "CCITT | %X (29B1 expected)", crc(CRC_CCITT, dataStr, sizeof(dataStr))); - uint8_t poll[] = {0xb2,0x4d,0x12,0x01,0x01,0x2e,0x3d,0x17,0x26,0x47,0x80, 0x95,0x00,0xf1,0x00,0x00,0x00,0x01,0x43,0x00,0xb3,0x7f}; - PrintAndLogEx(NORMAL, "FeliCa | %04X (B37F expected)", crc(CRC_FELICA, poll+2, sizeof(poll)-4)); - PrintAndLogEx(NORMAL, "FeliCa | %04X (0000 expected)", crc(CRC_FELICA, poll+2, sizeof(poll)-2)); + uint8_t poll[] = {0xb2, 0x4d, 0x12, 0x01, 0x01, 0x2e, 0x3d, 0x17, 0x26, 0x47, 0x80, 0x95, 0x00, 0xf1, 0x00, 0x00, 0x00, 0x01, 0x43, 0x00, 0xb3, 0x7f}; + PrintAndLogEx(NORMAL, "FeliCa | %04X (B37F expected)", crc(CRC_FELICA, poll + 2, sizeof(poll) - 4)); + PrintAndLogEx(NORMAL, "FeliCa | %04X (0000 expected)", crc(CRC_FELICA, poll + 2, sizeof(poll) - 2)); uint8_t sel_corr[] = { 0x40, 0xe1, 0xe1, 0xff, 0xfe, 0x5f, 0x02, 0x3c, 0x43, 0x01}; - PrintAndLogEx(NORMAL, "iCLASS | %04x (0143 expected)", crc(CRC_ICLASS, sel_corr, sizeof(sel_corr)-2)); + PrintAndLogEx(NORMAL, "iCLASS | %04x (0143 expected)", crc(CRC_ICLASS, sel_corr, sizeof(sel_corr) - 2)); PrintAndLogEx(NORMAL, "---------------------------------------------------------------\n\n\n"); // ISO14443 crc A compute_crc(CRC_14443_A, dataStr, sizeof(dataStr), &b1, &b2); uint16_t crcAA = b1 << 8 | b2; - PrintAndLogEx(NORMAL, "ISO14443 crc A | %04x or %04x (BF05 expected)\n", crcAA, crc(CRC_14443_A, dataStr, sizeof(dataStr)) ); + PrintAndLogEx(NORMAL, "ISO14443 crc A | %04x or %04x (BF05 expected)\n", crcAA, crc(CRC_14443_A, dataStr, sizeof(dataStr))); // ISO14443 crc B compute_crc(CRC_14443_B, dataStr, sizeof(dataStr), &b1, &b2); uint16_t crcBB = b1 << 8 | b2; - PrintAndLogEx(NORMAL, "ISO14443 crc B | %04x or %04x (906E expected)\n", crcBB, crc(CRC_14443_B, dataStr, sizeof(dataStr)) ); + PrintAndLogEx(NORMAL, "ISO14443 crc B | %04x or %04x (906E expected)\n", crcBB, crc(CRC_14443_B, dataStr, sizeof(dataStr))); // ISO15693 crc (x.25) compute_crc(CRC_15693, dataStr, sizeof(dataStr), &b1, &b2); uint16_t crcCC = b1 << 8 | b2; - PrintAndLogEx(NORMAL, "ISO15693 crc X25| %04x or %04x (906E expected)\n", crcCC, crc(CRC_15693, dataStr, sizeof(dataStr)) ); + PrintAndLogEx(NORMAL, "ISO15693 crc X25| %04x or %04x (906E expected)\n", crcCC, crc(CRC_15693, dataStr, sizeof(dataStr))); // ICLASS compute_crc(CRC_ICLASS, dataStr, sizeof(dataStr), &b1, &b2); uint16_t crcDD = b1 << 8 | b2; - PrintAndLogEx(NORMAL, "ICLASS crc | %04x or %04x\n", crcDD, crc(CRC_ICLASS, dataStr, sizeof(dataStr)) ); + PrintAndLogEx(NORMAL, "ICLASS crc | %04x or %04x\n", crcDD, crc(CRC_ICLASS, dataStr, sizeof(dataStr))); // FeliCa compute_crc(CRC_FELICA, dataStr, sizeof(dataStr), &b1, &b2); @@ -359,7 +383,8 @@ int CmdAnalyseCRC(const char *Cmd) { free(data); return 0; } -int CmdAnalyseCHKSUM(const char *Cmd){ +int CmdAnalyseCHKSUM(const char *Cmd) +{ uint8_t data[50]; uint8_t cmdp = 0; @@ -369,36 +394,36 @@ int CmdAnalyseCHKSUM(const char *Cmd){ int len = 0; memset(data, 0x0, sizeof(data)); - while(param_getchar(Cmd, cmdp) != 0x00 && !errors) { - switch(param_getchar(Cmd, cmdp)) { - case 'b': - case 'B': - param_gethex_ex(Cmd, cmdp+1, data, &len); - if ( len%2 ) errors = true; - len >>= 1; - cmdp += 2; - break; - case 'm': - case 'M': - mask = param_get32ex(Cmd, cmdp+1, 0, 16); - cmdp += 2; - break; - case 'v': - case 'V': - useHeader = true; - cmdp++; - break; - case 'h': - case 'H': - return usage_analyse_checksum(); - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; + while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { + switch (param_getchar(Cmd, cmdp)) { + case 'b': + case 'B': + param_gethex_ex(Cmd, cmdp + 1, data, &len); + if (len % 2) errors = true; + len >>= 1; + cmdp += 2; + break; + case 'm': + case 'M': + mask = param_get32ex(Cmd, cmdp + 1, 0, 16); + cmdp += 2; + break; + case 'v': + case 'V': + useHeader = true; + cmdp++; + break; + case 'h': + case 'H': + return usage_analyse_checksum(); + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); + errors = true; + break; } } //Validations - if (errors || cmdp == 0 ) return usage_analyse_checksum(); + if (errors || cmdp == 0) return usage_analyse_checksum(); if (useHeader) { PrintAndLogEx(NORMAL, " add | sub | add 1's compl | sub 1's compl | xor"); @@ -407,51 +432,53 @@ int CmdAnalyseCHKSUM(const char *Cmd){ } PrintAndLogEx(NORMAL, "0x%X 0x%X 0x%X | 0x%X 0x%X | 0x%X 0x%X 0x%X | 0x%X 0x%X | 0x%X 0x%X 0x%X | 0x%X 0x%X |\n", calcSumByteAdd(data, len, mask) - , calcSumNibbleAdd(data, len, mask) - , calcSumCrumbAdd(data, len, mask) - , calcSumByteSub(data, len, mask) - , calcSumNibbleSub(data, len, mask) - , calcSumByteAddOnes(data, len, mask) - , calcSumNibbleAddOnes(data, len, mask) - , calcSumCrumbAddOnes(data, len, mask) - , calcSumByteSubOnes(data, len, mask) - , calcSumNibbleSubOnes(data, len, mask) - , calcSumByteXor(data, len, mask) - , calcSumNibbleXor(data, len, mask) - , calcSumCrumbXor(data, len, mask) - , calcBSDchecksum8(data, len, mask) - , calcBSDchecksum4(data, len, mask) - ); + , calcSumNibbleAdd(data, len, mask) + , calcSumCrumbAdd(data, len, mask) + , calcSumByteSub(data, len, mask) + , calcSumNibbleSub(data, len, mask) + , calcSumByteAddOnes(data, len, mask) + , calcSumNibbleAddOnes(data, len, mask) + , calcSumCrumbAddOnes(data, len, mask) + , calcSumByteSubOnes(data, len, mask) + , calcSumNibbleSubOnes(data, len, mask) + , calcSumByteXor(data, len, mask) + , calcSumNibbleXor(data, len, mask) + , calcSumCrumbXor(data, len, mask) + , calcBSDchecksum8(data, len, mask) + , calcBSDchecksum4(data, len, mask) + ); return 0; } -int CmdAnalyseDates(const char *Cmd){ +int CmdAnalyseDates(const char *Cmd) +{ // look for datestamps in a given array of bytes PrintAndLogEx(NORMAL, "To be implemented. Feel free to contribute!"); return 0; } -int CmdAnalyseTEASelfTest(const char *Cmd){ +int CmdAnalyseTEASelfTest(const char *Cmd) +{ uint8_t v[8], v_le[8]; memset(v, 0x00, sizeof(v)); memset(v_le, 0x00, sizeof(v_le)); - uint8_t* v_ptr = v_le; + uint8_t *v_ptr = v_le; uint8_t cmdlen = strlen(Cmd); - cmdlen = ( sizeof(v)<<2 < cmdlen ) ? sizeof(v)<<2 : cmdlen; + cmdlen = (sizeof(v) << 2 < cmdlen) ? sizeof(v) << 2 : cmdlen; - if ( param_gethex(Cmd, 0, v, cmdlen) > 0 ){ + if (param_gethex(Cmd, 0, v, cmdlen) > 0) { PrintAndLogEx(WARNING, "Can't read hex chars, uneven? :: %u", cmdlen); return 1; } - SwapEndian64ex(v , 8, 4, v_ptr); + SwapEndian64ex(v, 8, 4, v_ptr); // ENCRYPTION KEY: - uint8_t key[16] = {0x55,0xFE,0xF6,0x30,0x62,0xBF,0x0B,0xC1,0xC9,0xB3,0x7C,0x34,0x97,0x3E,0x29,0xFB }; + uint8_t key[16] = {0x55, 0xFE, 0xF6, 0x30, 0x62, 0xBF, 0x0B, 0xC1, 0xC9, 0xB3, 0x7C, 0x34, 0x97, 0x3E, 0x29, 0xFB }; uint8_t keyle[16]; - uint8_t* key_ptr = keyle; - SwapEndian64ex(key , sizeof(key), 4, key_ptr); + uint8_t *key_ptr = keyle; + SwapEndian64ex(key, sizeof(key), 4, key_ptr); PrintAndLogEx(NORMAL, "TEST LE enc| %s", sprint_hex(v_ptr, 8)); @@ -465,7 +492,8 @@ int CmdAnalyseTEASelfTest(const char *Cmd){ return 0; } -char* pb(uint32_t b) { +char *pb(uint32_t b) +{ static char buf1[33] = {0}; static char buf2[33] = {0}; static char *s; @@ -478,14 +506,15 @@ char* pb(uint32_t b) { memset(s, 0, sizeof(buf1)); uint32_t mask = 0x80000000; - for (uint8_t i=0; i<32;i++) { - s[i] = (mask & b)?'1':'0'; + for (uint8_t i = 0; i < 32; i++) { + s[i] = (mask & b) ? '1' : '0'; mask >>= 1; } return s; } -int CmdAnalyseA(const char *Cmd){ +int CmdAnalyseA(const char *Cmd) +{ int hexlen = 0; uint8_t cmdp = 0; @@ -494,24 +523,24 @@ int CmdAnalyseA(const char *Cmd){ while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'd': - param_gethex_ex(Cmd, cmdp+1, data, &hexlen); - hexlen >>= 1; - if ( hexlen != sizeof(data) ) { - PrintAndLogEx(WARNING, "Read %d bytes of %u", hexlen, sizeof(data) ); - } - cmdp += 2; - break; - case 'h': - return usage_analyse_a(); - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; + case 'd': + param_gethex_ex(Cmd, cmdp + 1, data, &hexlen); + hexlen >>= 1; + if (hexlen != sizeof(data)) { + PrintAndLogEx(WARNING, "Read %d bytes of %u", hexlen, sizeof(data)); + } + cmdp += 2; + break; + case 'h': + return usage_analyse_a(); + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); + errors = true; + break; } } //Validations - if (errors || cmdp == 0 ) return usage_analyse_a(); + if (errors || cmdp == 0) return usage_analyse_a(); UsbCommand c = {CMD_FPC_SEND, {0, 0, 0}}; @@ -536,7 +565,7 @@ int CmdAnalyseA(const char *Cmd){ // Sequence X followed by Sequence Y followed by Sequence Z (111100x1 11111111 00x11111) // we therefore look for a ...xx1111 11111111 00x11111xxxxxx... pattern // (12 '1's followed by 2 '0's, eventually followed by another '0', followed by 5 '1's) - # define SYNC_16BIT 0xB24D +# define SYNC_16BIT 0xB24D uint32_t shiftReg = param_get32ex(Cmd, 0, 0xb24d, 16); uint8_t bt = param_get8ex(Cmd, 1, 0xBB, 16); uint8_t byte_offset = 99; @@ -550,100 +579,100 @@ int CmdAnalyseA(const char *Cmd){ uint8_t n0 = 0, n1 = 0; - n0 = (rev & (uint8_t)(~(0xFF >> (8-4)))) >> 4; + n0 = (rev & (uint8_t)(~(0xFF >> (8 - 4)))) >> 4; n1 = (n1 << 4) | (rev & (uint8_t)(~(0xFF << 4))); - PrintAndLogEx(NORMAL, "rev %02X | %02X %s | %02X %s |\n", rev, n0, pb(n0), n1, pb(n1) ); + PrintAndLogEx(NORMAL, "rev %02X | %02X %s | %02X %s |\n", rev, n0, pb(n0), n1, pb(n1)); -/* -hex(0xb24d shr 0) 0xB24D 0b1011001001001101 -hex(0xb24d shr 1) 0x5926 -hex(0xb24d shr 2) 0x2C93 -*/ + /* + hex(0xb24d shr 0) 0xB24D 0b1011001001001101 + hex(0xb24d shr 1) 0x5926 + hex(0xb24d shr 2) 0x2C93 + */ -for ( int i =0; i< 16; i++) { - PrintAndLogEx(NORMAL, " (shiftReg >> %d) & 0xFFFF == %08x ---", i, (( shiftReg >> i) & 0xFFFF )); + for (int i = 0; i < 16; i++) { + PrintAndLogEx(NORMAL, " (shiftReg >> %d) & 0xFFFF == %08x ---", i, ((shiftReg >> i) & 0xFFFF)); - // kolla om SYNC_PATTERN finns. - if ((( shiftReg >> 7) & 0xFFFF ) == SYNC_16BIT) byte_offset = 7; - else if ((( shiftReg >> 6) & 0xFFFF ) == SYNC_16BIT) byte_offset = 6; - else if ((( shiftReg >> 5) & 0xFFFF ) == SYNC_16BIT) byte_offset = 5; - else if ((( shiftReg >> 4) & 0xFFFF ) == SYNC_16BIT) byte_offset = 4; - else if ((( shiftReg >> 3) & 0xFFFF ) == SYNC_16BIT) byte_offset = 3; - else if ((( shiftReg >> 2) & 0xFFFF ) == SYNC_16BIT) byte_offset = 2; - else if ((( shiftReg >> 1) & 0xFFFF ) == SYNC_16BIT) byte_offset = 1; - else if ((( shiftReg >> 0) & 0xFFFF ) == SYNC_16BIT) byte_offset = 0; + // kolla om SYNC_PATTERN finns. + if (((shiftReg >> 7) & 0xFFFF) == SYNC_16BIT) byte_offset = 7; + else if (((shiftReg >> 6) & 0xFFFF) == SYNC_16BIT) byte_offset = 6; + else if (((shiftReg >> 5) & 0xFFFF) == SYNC_16BIT) byte_offset = 5; + else if (((shiftReg >> 4) & 0xFFFF) == SYNC_16BIT) byte_offset = 4; + else if (((shiftReg >> 3) & 0xFFFF) == SYNC_16BIT) byte_offset = 3; + else if (((shiftReg >> 2) & 0xFFFF) == SYNC_16BIT) byte_offset = 2; + else if (((shiftReg >> 1) & 0xFFFF) == SYNC_16BIT) byte_offset = 1; + else if (((shiftReg >> 0) & 0xFFFF) == SYNC_16BIT) byte_offset = 0; - PrintAndLogEx(NORMAL, "Offset %u \n", byte_offset); - if ( byte_offset != 99 ) - break; + PrintAndLogEx(NORMAL, "Offset %u \n", byte_offset); + if (byte_offset != 99) + break; - shiftReg >>=1; -} + shiftReg >>= 1; + } uint8_t p1 = (rev & (uint8_t)(~(0xFF << byte_offset))); - PrintAndLogEx(NORMAL, "Offset %u | leftovers %02x %s \n", byte_offset, p1, pb(p1) ); + PrintAndLogEx(NORMAL, "Offset %u | leftovers %02x %s \n", byte_offset, p1, pb(p1)); /* -pm3 --> da hex2bin 4db2 0100110110110010 -*/ - return 0; -/* - // split byte into two parts. - uint8_t offset = 3, n0 = 0, n1 = 0; - rev = 0xB2; - for (uint8_t m=0; m<8; m++) { - offset = m; - n0 = (rev & (uint8_t)(~(0xFF >> (8-offset)))) >> offset; - n1 = (n1 << offset) | (rev & (uint8_t)(~(0xFF << offset))); - - PrintAndLogEx(NORMAL, "rev %02X | %02X %s | %02X %s |\n", rev, n0, pb(n0), n1, pb(n1) ); - n0 = 0, n1 = 0; - // PrintAndLogEx(NORMAL, " (0xFF >> offset) == %s |\n", pb( (0xFF >> offset)) ); - //PrintAndLogEx(NORMAL, "~(0xFF >> (8-offset)) == %s |\n", pb( (uint8_t)(~(0xFF >> (8-offset))) ) ); - //PrintAndLogEx(NORMAL, " rev & xxx == %s\n\n", pb( (rev & (uint8_t)(~(0xFF << offset))) )); - } -return 0; - // from A -- x bits into B and the rest into C. - - for ( uint8_t i=0; i<8; i++){ - PrintAndLogEx(NORMAL, "%u | %02X %s | %02X %s |\n", i, a, pb(a), b, pb(b) ); - b = a & (a & (0xFF >> (8-i))); - a >>=1; - } - + pm3 --> da hex2bin 4db2 0100110110110010 */ return 0; + /* + // split byte into two parts. + uint8_t offset = 3, n0 = 0, n1 = 0; + rev = 0xB2; + for (uint8_t m=0; m<8; m++) { + offset = m; + n0 = (rev & (uint8_t)(~(0xFF >> (8-offset)))) >> offset; + n1 = (n1 << offset) | (rev & (uint8_t)(~(0xFF << offset))); + + PrintAndLogEx(NORMAL, "rev %02X | %02X %s | %02X %s |\n", rev, n0, pb(n0), n1, pb(n1) ); + n0 = 0, n1 = 0; + // PrintAndLogEx(NORMAL, " (0xFF >> offset) == %s |\n", pb( (0xFF >> offset)) ); + //PrintAndLogEx(NORMAL, "~(0xFF >> (8-offset)) == %s |\n", pb( (uint8_t)(~(0xFF >> (8-offset))) ) ); + //PrintAndLogEx(NORMAL, " rev & xxx == %s\n\n", pb( (rev & (uint8_t)(~(0xFF << offset))) )); + } + return 0; + // from A -- x bits into B and the rest into C. + + for ( uint8_t i=0; i<8; i++){ + PrintAndLogEx(NORMAL, "%u | %02X %s | %02X %s |\n", i, a, pb(a), b, pb(b) ); + b = a & (a & (0xFF >> (8-i))); + a >>=1; + } + + */ + return 0; // 14443-A uint8_t u14_c[] = {0x09, 0x78, 0x00, 0x92, 0x02, 0x54, 0x13, 0x02, 0x04, 0x2d, 0xe8 }; // atqs w crc uint8_t u14_w[] = {0x09, 0x78, 0x00, 0x92, 0x02, 0x54, 0x13, 0x02, 0x04, 0x2d, 0xe7 }; // atqs w crc - PrintAndLogEx(FAILED, "14a check wrong crc | %s\n", (check_crc(CRC_14443_A, u14_w, sizeof(u14_w))) ? "YES": "NO" ); - PrintAndLogEx(SUCCESS, "14a check correct crc | %s\n", (check_crc(CRC_14443_A, u14_c, sizeof(u14_c))) ? "YES": "NO" ); + PrintAndLogEx(FAILED, "14a check wrong crc | %s\n", (check_crc(CRC_14443_A, u14_w, sizeof(u14_w))) ? "YES" : "NO"); + PrintAndLogEx(SUCCESS, "14a check correct crc | %s\n", (check_crc(CRC_14443_A, u14_c, sizeof(u14_c))) ? "YES" : "NO"); // 14443-B - uint8_t u14b[] = {0x05,0x00,0x08,0x39,0x73}; - PrintAndLogEx(NORMAL, "14b check crc | %s\n", (check_crc(CRC_14443_B, u14b, sizeof(u14b))) ? "YES": "NO"); + uint8_t u14b[] = {0x05, 0x00, 0x08, 0x39, 0x73}; + PrintAndLogEx(NORMAL, "14b check crc | %s\n", (check_crc(CRC_14443_B, u14b, sizeof(u14b))) ? "YES" : "NO"); // 15693 test - uint8_t u15_c[] = {0x05,0x00,0x08,0x39,0x73}; // correct - uint8_t u15_w[] = {0x05,0x00,0x08,0x39,0x72}; // wrong - PrintAndLogEx(FAILED, "15 check wrong crc | %s\n", (check_crc(CRC_15693, u15_w, sizeof(u15_w))) ? "YES": "NO"); - PrintAndLogEx(SUCCESS, "15 check correct crc | %s\n", (check_crc(CRC_15693, u15_c, sizeof(u15_c))) ? "YES": "NO"); + uint8_t u15_c[] = {0x05, 0x00, 0x08, 0x39, 0x73}; // correct + uint8_t u15_w[] = {0x05, 0x00, 0x08, 0x39, 0x72}; // wrong + PrintAndLogEx(FAILED, "15 check wrong crc | %s\n", (check_crc(CRC_15693, u15_w, sizeof(u15_w))) ? "YES" : "NO"); + PrintAndLogEx(SUCCESS, "15 check correct crc | %s\n", (check_crc(CRC_15693, u15_c, sizeof(u15_c))) ? "YES" : "NO"); // iCLASS test - wrong crc , swapped bytes. uint8_t iclass_w[] = { 0x40, 0xe1, 0xe1, 0xff, 0xfe, 0x5f, 0x02, 0x3c, 0x01, 0x43}; uint8_t iclass_c[] = { 0x40, 0xe1, 0xe1, 0xff, 0xfe, 0x5f, 0x02, 0x3c, 0x43, 0x01}; - PrintAndLogEx(FAILED, "iCLASS check wrong crc | %s\n", (check_crc(CRC_ICLASS, iclass_w, sizeof(iclass_w))) ? "YES": "NO"); - PrintAndLogEx(SUCCESS, "iCLASS check correct crc | %s\n", (check_crc(CRC_ICLASS, iclass_c, sizeof(iclass_c))) ? "YES": "NO"); + PrintAndLogEx(FAILED, "iCLASS check wrong crc | %s\n", (check_crc(CRC_ICLASS, iclass_w, sizeof(iclass_w))) ? "YES" : "NO"); + PrintAndLogEx(SUCCESS, "iCLASS check correct crc | %s\n", (check_crc(CRC_ICLASS, iclass_c, sizeof(iclass_c))) ? "YES" : "NO"); // FeliCa test - uint8_t felica_w[] = {0x12,0x01,0x01,0x2e,0x3d,0x17,0x26,0x47,0x80, 0x95,0x00,0xf1,0x00,0x00,0x00,0x01,0x43,0x00,0xb3,0x7e}; - uint8_t felica_c[] = {0x12,0x01,0x01,0x2e,0x3d,0x17,0x26,0x47,0x80, 0x95,0x00,0xf1,0x00,0x00,0x00,0x01,0x43,0x00,0xb3,0x7f}; - PrintAndLogEx(FAILED, "FeliCa check wrong crc | %s\n", (check_crc(CRC_FELICA, felica_w, sizeof(felica_w))) ? "YES": "NO"); - PrintAndLogEx(SUCCESS, "FeliCa check correct crc | %s\n", (check_crc(CRC_FELICA, felica_c, sizeof(felica_c))) ? "YES": "NO"); + uint8_t felica_w[] = {0x12, 0x01, 0x01, 0x2e, 0x3d, 0x17, 0x26, 0x47, 0x80, 0x95, 0x00, 0xf1, 0x00, 0x00, 0x00, 0x01, 0x43, 0x00, 0xb3, 0x7e}; + uint8_t felica_c[] = {0x12, 0x01, 0x01, 0x2e, 0x3d, 0x17, 0x26, 0x47, 0x80, 0x95, 0x00, 0xf1, 0x00, 0x00, 0x00, 0x01, 0x43, 0x00, 0xb3, 0x7f}; + PrintAndLogEx(FAILED, "FeliCa check wrong crc | %s\n", (check_crc(CRC_FELICA, felica_w, sizeof(felica_w))) ? "YES" : "NO"); + PrintAndLogEx(SUCCESS, "FeliCa check correct crc | %s\n", (check_crc(CRC_FELICA, felica_c, sizeof(felica_c))) ? "YES" : "NO"); PrintAndLogEx(NORMAL, "\n\n"); @@ -687,27 +716,35 @@ return 0; //uint64_t d1[] = {0x6e442129, 0x8f699195, 0x0000001, 0x090d0b0305020f02}; //uint64_t d2[] = {0x6e442129, 0x8f699195, 0x0000004, 0x00040f0f0305030e}; -/* -uid(3e172b29) nt(039b7bd2) ks(0c0e0f0505080800) nr(00000001) -uid(3e172b29) nt(039b7bd2) ks(0e06090d03000b0f) nr(00000002) -*/ + /* + uid(3e172b29) nt(039b7bd2) ks(0c0e0f0505080800) nr(00000001) + uid(3e172b29) nt(039b7bd2) ks(0e06090d03000b0f) nr(00000002) + */ uint64_t *keylistA = NULL, *keylistB = NULL; uint32_t keycountA = 0, keycountB = 0; // uint64_t d1[] = {0x3e172b29, 0x039b7bd2, 0x0000001, 0, 0x0c0e0f0505080800}; // uint64_t d2[] = {0x3e172b29, 0x039b7bd2, 0x0000002, 0, 0x0e06090d03000b0f}; -uint64_t d1[] = {0x6e442129, 0x8f699195, 0x0000001, 0, 0x090d0b0305020f02}; -uint64_t d2[] = {0x6e442129, 0x8f699195, 0x0000004, 0, 0x00040f0f0305030e}; + uint64_t d1[] = {0x6e442129, 0x8f699195, 0x0000001, 0, 0x090d0b0305020f02}; + uint64_t d2[] = {0x6e442129, 0x8f699195, 0x0000004, 0, 0x00040f0f0305030e}; - keycountA = nonce2key(d1[0], d1[1], d1[2], 0, d1[3], d1[4] ,&keylistA); + keycountA = nonce2key(d1[0], d1[1], d1[2], 0, d1[3], d1[4], &keylistA); keycountB = nonce2key(d2[0], d2[1], d2[2], 0, d2[3], d2[4], &keylistB); switch (keycountA) { - case 0: PrintAndLogEx(FAILED, "Key test A failed\n"); break; - case 1: PrintAndLogEx(SUCCESS, "KEY A | %012" PRIX64 " ", keylistA[0]); break; + case 0: + PrintAndLogEx(FAILED, "Key test A failed\n"); + break; + case 1: + PrintAndLogEx(SUCCESS, "KEY A | %012" PRIX64 " ", keylistA[0]); + break; } switch (keycountB) { - case 0: PrintAndLogEx(FAILED, "Key test B failed\n"); break; - case 1: PrintAndLogEx(SUCCESS, "KEY B | %012" PRIX64 " ", keylistB[0]); break; + case 0: + PrintAndLogEx(FAILED, "Key test B failed\n"); + break; + case 1: + PrintAndLogEx(SUCCESS, "KEY B | %012" PRIX64 " ", keylistB[0]); + break; } free(keylistA); @@ -843,7 +880,8 @@ uint64_t d2[] = {0x6e442129, 0x8f699195, 0x0000004, 0, 0x00040f0f0305030e}; return 0; } -void generate4bNUID(uint8_t *uid, uint8_t *nuid){ +void generate4bNUID(uint8_t *uid, uint8_t *nuid) +{ uint16_t crc; uint8_t b1, b2; @@ -857,12 +895,13 @@ void generate4bNUID(uint8_t *uid, uint8_t *nuid){ nuid[3] = crc & 0xFF; } -int CmdAnalyseNuid(const char *Cmd){ +int CmdAnalyseNuid(const char *Cmd) +{ uint8_t nuid[4] = {0}; uint8_t uid[7] = {0}; int len = 0; char cmdp = tolower(param_getchar(Cmd, 0)); - if (strlen(Cmd) == 0|| cmdp == 'h') return usage_analyse_nuid(); + if (strlen(Cmd) == 0 || cmdp == 'h') return usage_analyse_nuid(); /* src: https://www.nxp.com/docs/en/application-note/AN10927.pdf */ /* selftest1 UID 040D681AB52281 -> NUID 8F430FEF */ @@ -872,7 +911,7 @@ int CmdAnalyseNuid(const char *Cmd){ generate4bNUID(uid, nuid); bool test1 = (0 == memcmp(nuid, "\x8f\x43\x0f\xef", 4)); - PrintAndLogEx(SUCCESS, "Selftest1 %s\n", test1 ? _GREEN_(OK): _RED_(Fail)); + PrintAndLogEx(SUCCESS, "Selftest1 %s\n", test1 ? _GREEN_(OK) : _RED_(Fail)); memcpy(uid, "\x04\x18\x3f\x09\x32\x1b\x85", 7); generate4bNUID(uid, nuid); @@ -882,7 +921,7 @@ int CmdAnalyseNuid(const char *Cmd){ } param_gethex_ex(Cmd, 0, uid, &len); - if ( len%2 || len != 14) return usage_analyse_nuid(); + if (len % 2 || len != 14) return usage_analyse_nuid(); generate4bNUID(uid, nuid); @@ -903,13 +942,15 @@ static command_t CommandTable[] = { {NULL, NULL, 0, NULL} }; -int CmdAnalyse(const char *Cmd) { +int CmdAnalyse(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { +int CmdHelp(const char *Cmd) +{ CmdsHelp(CommandTable); return 0; } diff --git a/client/cmdcrc.c b/client/cmdcrc.c index 38d5a2005..eb46a524d 100644 --- a/client/cmdcrc.c +++ b/client/cmdcrc.c @@ -11,17 +11,18 @@ #define MAX_ARGS 20 -int split(char *str, char *arr[MAX_ARGS]){ +int split(char *str, char *arr[MAX_ARGS]) +{ int beginIndex = 0; int endIndex; int maxWords = MAX_ARGS; int wordCnt = 0; - while(1){ - while(isspace(str[beginIndex])) { + while (1) { + while (isspace(str[beginIndex])) { ++beginIndex; } - if(str[beginIndex] == '\0') { + if (str[beginIndex] == '\0') { break; } endIndex = beginIndex; @@ -39,7 +40,8 @@ int split(char *str, char *arr[MAX_ARGS]){ return wordCnt; } -int CmdCrc(const char *Cmd) { +int CmdCrc(const char *Cmd) +{ char name[] = {"reveng "}; char Cmd2[100 + 7]; memcpy(Cmd2, name, 7); @@ -52,7 +54,7 @@ int CmdCrc(const char *Cmd) { } else { reveng_main(argc, argv); } - for(int i = 0; i < argc; ++i) { + for (int i = 0; i < argc; ++i) { free(argv[i]); } return 0; @@ -60,7 +62,8 @@ int CmdCrc(const char *Cmd) { //returns array of model names and the count of models returning // as well as a width array for the width of each model -int GetModels(char *Models[], int *count, uint8_t *width){ +int GetModels(char *Models[], int *count, uint8_t *width) +{ /* default values */ static model_t model = MZERO; @@ -70,9 +73,9 @@ int GetModels(char *Models[], int *count, uint8_t *width){ model_t pset = model, *candmods, *mptr; /* stdin must be binary */ - #ifdef _WIN32 - _setmode(STDIN_FILENO, _O_BINARY); - #endif /* _WIN32 */ +#ifdef _WIN32 + _setmode(STDIN_FILENO, _O_BINARY); +#endif /* _WIN32 */ SETBMP(); @@ -80,7 +83,7 @@ int GetModels(char *Models[], int *count, uint8_t *width){ int Cnt = 0; if (width[0] == 0) { //reveng -D *count = mcount(); - if (!*count){ + if (!*count) { PrintAndLogEx(WARNING, "no preset models available"); return 0; } @@ -88,8 +91,8 @@ int GetModels(char *Models[], int *count, uint8_t *width){ mbynum(&model, mode); mcanon(&model); size_t size = (model.name && *model.name) ? strlen(model.name) : 7; - char *tmp = calloc(size+1, sizeof(char)); - if (tmp==NULL){ + char *tmp = calloc(size + 1, sizeof(char)); + if (tmp == NULL) { PrintAndLogEx(WARNING, "out of memory?"); return 0; } @@ -100,9 +103,9 @@ int GetModels(char *Models[], int *count, uint8_t *width){ mfree(&model); } else { //reveng -s - if (~model.flags & P_MULXN){ - PrintAndLogEx(WARNING, "cannot search for non-Williams compliant models"); - return 0; + if (~model.flags & P_MULXN) { + PrintAndLogEx(WARNING, "cannot search for non-Williams compliant models"); + return 0; } praloc(&model.spoly, (unsigned long)width[0]); praloc(&model.init, (unsigned long)width[0]); @@ -133,7 +136,7 @@ int GetModels(char *Models[], int *count, uint8_t *width){ mbynum(&pset, --psets); /* skip if different width, or refin or refout don't match */ - if( plen(pset.spoly) != width[0] || (model.flags ^ pset.flags) & (P_REFIN | P_REFOUT)) + if (plen(pset.spoly) != width[0] || (model.flags ^ pset.flags) & (P_REFIN | P_REFOUT)) continue; /* skip if the preset doesn't match specified parameters */ if (rflags & R_HAVEP && pcmp(&model.spoly, &pset.spoly)) @@ -167,8 +170,8 @@ int GetModels(char *Models[], int *count, uint8_t *width){ size_t size = (pset.name && *pset.name) ? strlen(pset.name) : 7; //PrintAndLogEx(NORMAL, "Size: %d, %s, count: %d",size,pset.name, Cnt); - char *tmp = calloc(size+1, sizeof(char)); - if (tmp == NULL){ + char *tmp = calloc(size + 1, sizeof(char)); + if (tmp == NULL) { PrintAndLogEx(WARNING, "out of memory?"); return 0; } @@ -197,12 +200,12 @@ int GetModels(char *Models[], int *count, uint8_t *width){ pfree(qptr); } } - if(uflags & C_NOBFS && ~rflags & R_HAVEP) { + if (uflags & C_NOBFS && ~rflags & R_HAVEP) { PrintAndLogEx(WARNING, "no models found"); return 0; } - if (!(model.flags & P_REFIN) != !(model.flags & P_REFOUT)){ + if (!(model.flags & P_REFIN) != !(model.flags & P_REFOUT)) { PrintAndLogEx(WARNING, "cannot search for crossed-endian models"); return 0; } @@ -230,7 +233,7 @@ int GetModels(char *Models[], int *count, uint8_t *width){ free(apolys); mfree(&model); - if (~uflags & C_RESULT){ + if (~uflags & C_RESULT) { PrintAndLogEx(WARNING, "no models found"); return 0; } @@ -245,7 +248,8 @@ int GetModels(char *Models[], int *count, uint8_t *width){ //endian = {0 = calc default endian input and output, b = big endian input and output, B = big endian output, r = right justified // l = little endian input and output, L = little endian output only, t = left justified} //result = calculated crc hex string -int RunModel(char *inModel, char *inHexStr, bool reverse, char endian, char *result){ +int RunModel(char *inModel, char *inHexStr, bool reverse, char endian, char *result) +{ /* default values */ static model_t model = MZERO; @@ -257,9 +261,9 @@ int RunModel(char *inModel, char *inHexStr, bool reverse, char endian, char *res char *string; // stdin must be binary - #ifdef _WIN32 - _setmode(STDIN_FILENO, _O_BINARY); - #endif /* _WIN32 */ +#ifdef _WIN32 + _setmode(STDIN_FILENO, _O_BINARY); +#endif /* _WIN32 */ SETBMP(); //set model @@ -268,7 +272,7 @@ int RunModel(char *inModel, char *inHexStr, bool reverse, char endian, char *res PrintAndLogEx(WARNING, "error: preset model '%s' not found. Use reveng -D to list presets. [%d]", inModel, c); return 0; } - if (c < 0){ + if (c < 0) { PrintAndLogEx(WARNING, "no preset models available"); return 0; } @@ -279,24 +283,24 @@ int RunModel(char *inModel, char *inHexStr, bool reverse, char endian, char *res case 'b': /* b big-endian (RefIn = false, RefOut = false ) */ model.flags &= ~P_REFIN; rflags |= R_HAVERI; - /* fall through: */ + /* fall through: */ case 'B': /* B big-endian output (RefOut = false) */ model.flags &= ~P_REFOUT; rflags |= R_HAVERO; mnovel(&model); - /* fall through: */ + /* fall through: */ case 'r': /* r right-justified */ model.flags |= P_RTJUST; break; case 'l': /* l little-endian input and output */ model.flags |= P_REFIN; rflags |= R_HAVERI; - /* fall through: */ + /* fall through: */ case 'L': /* L little-endian output */ model.flags |= P_REFOUT; rflags |= R_HAVERO; mnovel(&model); - /* fall through: */ + /* fall through: */ case 't': /* t left-justified */ model.flags &= ~P_RTJUST; break; @@ -353,9 +357,9 @@ int RunModel(char *inModel, char *inHexStr, bool reverse, char endian, char *res prev(&crc); string = ptostr(crc, model.flags, obperhx); - for (int i = 0; i < 50; i++){ + for (int i = 0; i < 50; i++) { result[i] = string[i]; - if (result[i]==0) break; + if (result[i] == 0) break; } free(string); pfree(&crc); @@ -364,7 +368,8 @@ int RunModel(char *inModel, char *inHexStr, bool reverse, char endian, char *res } //test call to RunModel -int CmdrevengTestC(const char *Cmd){ +int CmdrevengTestC(const char *Cmd) +{ int cmdp = 0; char inModel[30] = {0x00}; char inHexStr[30] = {0x00}; @@ -382,24 +387,26 @@ int CmdrevengTestC(const char *Cmd){ int ans = RunModel(inModel, inHexStr, reverse, endian, result); if (!ans) return 0; - PrintAndLogEx(SUCCESS, "result: %s",result); + PrintAndLogEx(SUCCESS, "result: %s", result); return 1; } //returns a calloced string (needs to be freed) -char *SwapEndianStr(const char *inStr, const size_t len, const uint8_t blockSize){ - char *tmp = calloc(len+1, sizeof(char)); - for (uint8_t block=0; block < (uint8_t)(len/blockSize); block++){ - for (size_t i = 0; i < blockSize; i+=2){ - tmp[i+(blockSize*block)] = inStr[(blockSize-1-i-1)+(blockSize*block)]; - tmp[i+(blockSize*block)+1] = inStr[(blockSize-1-i)+(blockSize*block)]; +char *SwapEndianStr(const char *inStr, const size_t len, const uint8_t blockSize) +{ + char *tmp = calloc(len + 1, sizeof(char)); + for (uint8_t block = 0; block < (uint8_t)(len / blockSize); block++) { + for (size_t i = 0; i < blockSize; i += 2) { + tmp[i + (blockSize * block)] = inStr[(blockSize - 1 - i - 1) + (blockSize * block)]; + tmp[i + (blockSize * block) + 1] = inStr[(blockSize - 1 - i) + (blockSize * block)]; } } return tmp; } // takes hex string in and searches for a matching result (hex string must include checksum) -int CmdrevengSearch(const char *Cmd){ +int CmdrevengSearch(const char *Cmd) +{ #define NMODELS 105 @@ -420,35 +427,35 @@ int CmdrevengSearch(const char *Cmd){ if (!ans) return 0; // try each model and get result - for (int i = 0; i < count; i++){ + for (int i = 0; i < count; i++) { /*if (found) { free(Models[i]); continue; }*/ // round up to # of characters in this model's crc - crcChars = ((width[i]+7)/8)*2; + crcChars = ((width[i] + 7) / 8) * 2; // can't test a model that has more crc digits than our data if (crcChars >= dataLen) continue; memset(result, 0, 30); - char *inCRC = calloc(crcChars+1, sizeof(char)); - memcpy(inCRC, inHexStr+(dataLen-crcChars), crcChars); + char *inCRC = calloc(crcChars + 1, sizeof(char)); + memcpy(inCRC, inHexStr + (dataLen - crcChars), crcChars); - char *outHex = calloc(dataLen-crcChars+1, sizeof(char)); - memcpy(outHex, inHexStr, dataLen-crcChars); + char *outHex = calloc(dataLen - crcChars + 1, sizeof(char)); + memcpy(outHex, inHexStr, dataLen - crcChars); - PrintAndLogEx(DEBUG, "DEBUG: dataLen: %d, crcChars: %d, Model: %s, CRC: %s, width: %d, outHex: %s",dataLen, crcChars, Models[i], inCRC, width[i], outHex); + PrintAndLogEx(DEBUG, "DEBUG: dataLen: %d, crcChars: %d, Model: %s, CRC: %s, width: %d, outHex: %s", dataLen, crcChars, Models[i], inCRC, width[i], outHex); ans = RunModel(Models[i], outHex, false, 0, result); if (ans) { // test for match - if (memcmp(result, inCRC, crcChars) == 0){ + if (memcmp(result, inCRC, crcChars) == 0) { PrintAndLogEx(SUCCESS, "\nfound possible match\nmodel: %s | value: %s\n", Models[i], result); //optional - stop searching if found... found = true; } else { - if (crcChars > 2){ + if (crcChars > 2) { char *swapEndian = SwapEndianStr(result, crcChars, crcChars); - if (memcmp(swapEndian, inCRC, crcChars) == 0){ + if (memcmp(swapEndian, inCRC, crcChars) == 0) { PrintAndLogEx(SUCCESS, "\nfound possible match\nmodel: %s | value endian swapped: %s\n", Models[i], swapEndian); // optional - stop searching if found... found = true; @@ -460,14 +467,14 @@ int CmdrevengSearch(const char *Cmd){ ans = RunModel(Models[i], outHex, true, 0, revResult); if (ans) { // test for match - if (memcmp(revResult, inCRC, crcChars) == 0){ + if (memcmp(revResult, inCRC, crcChars) == 0) { PrintAndLogEx(SUCCESS, "\nfound possible match\nmodel reversed: %s | value: %s\n", Models[i], revResult); // optional - stop searching if found... found = true; } else { - if (crcChars > 2){ + if (crcChars > 2) { char *swapEndian = SwapEndianStr(revResult, crcChars, crcChars); - if (memcmp(swapEndian, inCRC, crcChars) == 0){ + if (memcmp(swapEndian, inCRC, crcChars) == 0) { PrintAndLogEx(SUCCESS, "\nfound possible match\nmodel reversed: %s | value endian swapped: %s\n", Models[i], swapEndian); // optional - stop searching if found... found = true; diff --git a/client/cmddata.c b/client/cmddata.c index 9753701e5..1fd01a3ae 100644 --- a/client/cmddata.c +++ b/client/cmddata.c @@ -17,7 +17,8 @@ int g_DemodClock = 0; static int CmdHelp(const char *Cmd); -int usage_data_printdemodbuf(void){ +int usage_data_printdemodbuf(void) +{ PrintAndLogEx(NORMAL, "Usage: data printdemodbuffer x o l "); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " h This help"); @@ -26,7 +27,8 @@ int usage_data_printdemodbuf(void){ PrintAndLogEx(NORMAL, " l enter length to print in # of bits or hex characters respectively"); return 0; } -int usage_data_manrawdecode(void){ +int usage_data_manrawdecode(void) +{ PrintAndLogEx(NORMAL, "Usage: data manrawdecode [invert] [maxErr]"); PrintAndLogEx(NORMAL, " Takes 10 and 01 and converts to 0 and 1 respectively"); PrintAndLogEx(NORMAL, " --must have binary sequence in demodbuffer (run data askrawdemod first)"); @@ -36,7 +38,8 @@ int usage_data_manrawdecode(void){ PrintAndLogEx(NORMAL, " Example: data manrawdecode = decode manchester bitstream from the demodbuffer"); return 0; } -int usage_data_biphaserawdecode(void){ +int usage_data_biphaserawdecode(void) +{ PrintAndLogEx(NORMAL, "Usage: data biphaserawdecode [offset] [invert] [maxErr]"); PrintAndLogEx(NORMAL, " Converts 10 or 01 to 1 and 11 or 00 to 0"); PrintAndLogEx(NORMAL, " --must have binary sequence in demodbuffer (run data askrawdemod first)"); @@ -50,7 +53,8 @@ int usage_data_biphaserawdecode(void){ PrintAndLogEx(NORMAL, " Example: data biphaserawdecode 1 1 = decode biphase bitstream from the demodbuffer, set offset, and invert output"); return 0; } -int usage_data_rawdemod(void){ +int usage_data_rawdemod(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod [modulation] |"); PrintAndLogEx(NORMAL, " [modulation] as 2 char, 'ab' for ask/biphase, 'am' for ask/manchester, 'ar' for ask/raw, 'fs' for fsk, ..."); PrintAndLogEx(NORMAL, " 'nr' for nrz/direct, 'p1' for psk1, 'p2' for psk2"); @@ -67,7 +71,8 @@ int usage_data_rawdemod(void){ PrintAndLogEx(NORMAL, " : data rawdemod p2 = demod GraphBuffer using: psk2 - autodetect"); return 0; } -int usage_data_rawdemod_am(void){ +int usage_data_rawdemod_am(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod am [clock] [maxError] [maxLen] [amplify]"); PrintAndLogEx(NORMAL, " ['s'] optional, check for Sequence Terminator"); PrintAndLogEx(NORMAL, " [set clock as integer] optional, if not set, autodetect"); @@ -83,7 +88,8 @@ int usage_data_rawdemod_am(void){ PrintAndLogEx(NORMAL, " : data rawdemod am 64 1 0 = demod an ask/manchester tag from GraphBuffer using a clock of RF/64, inverting data and allowing 0 demod errors"); return 0; } -int usage_data_rawdemod_ab(void){ +int usage_data_rawdemod_ab(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod ab [offset] [clock] [maxError] [maxLen] "); PrintAndLogEx(NORMAL, " [offset], offset to begin biphase, default=0"); PrintAndLogEx(NORMAL, " [set clock as integer] optional, if not set, autodetect"); @@ -106,7 +112,8 @@ int usage_data_rawdemod_ab(void){ PrintAndLogEx(NORMAL, " : data rawdemod ab 0 64 1 0 0 a = demod an ask/biph tag from GraphBuffer using a clock of RF/64, inverting data and allowing 0 demod errors, and amp"); return 0; } -int usage_data_rawdemod_ar(void){ +int usage_data_rawdemod_ar(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod ar [clock] [maxError] [maxLen] [amplify]"); PrintAndLogEx(NORMAL, " [set clock as integer] optional, if not set, autodetect"); PrintAndLogEx(NORMAL, " , 1 to invert output"); @@ -123,7 +130,8 @@ int usage_data_rawdemod_ar(void){ PrintAndLogEx(NORMAL, " : data rawdemod ar 64 1 0 0 a = demod an ask tag from GraphBuffer using a clock of RF/64, inverting data and allowing 0 demod errors, and amp"); return 0; } -int usage_data_rawdemod_fs(void){ +int usage_data_rawdemod_fs(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod fs [clock] [fchigh] [fclow]"); PrintAndLogEx(NORMAL, " [set clock as integer] optional, omit for autodetect."); PrintAndLogEx(NORMAL, " , 1 for invert output, can be used even if the clock is omitted"); @@ -139,7 +147,8 @@ int usage_data_rawdemod_fs(void){ PrintAndLogEx(NORMAL, " : data rawdemod fs 50 1 10 8 = demod an fsk2a RF/50 tag from GraphBuffer"); return 0; } -int usage_data_rawdemod_nr(void){ +int usage_data_rawdemod_nr(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod nr [clock] <0|1> [maxError]"); PrintAndLogEx(NORMAL, " [set clock as integer] optional, if not set, autodetect."); PrintAndLogEx(NORMAL, " , 1 for invert output"); @@ -152,7 +161,8 @@ int usage_data_rawdemod_nr(void){ PrintAndLogEx(NORMAL, " : data rawdemod nr 64 1 0 = demod a nrz/direct tag from GraphBuffer using a clock of RF/64, inverting data and allowing 0 demod errors"); return 0; } -int usage_data_rawdemod_p1(void){ +int usage_data_rawdemod_p1(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod p1 [clock] <0|1> [maxError]"); PrintAndLogEx(NORMAL, " [set clock as integer] optional, if not set, autodetect."); PrintAndLogEx(NORMAL, " , 1 for invert output"); @@ -165,7 +175,8 @@ int usage_data_rawdemod_p1(void){ PrintAndLogEx(NORMAL, " : data rawdemod p1 64 1 0 = demod a psk1 tag from GraphBuffer using a clock of RF/64, inverting data and allowing 0 demod errors"); return 0; } -int usage_data_rawdemod_p2(void){ +int usage_data_rawdemod_p2(void) +{ PrintAndLogEx(NORMAL, "Usage: data rawdemod p2 [clock] <0|1> [maxError]"); PrintAndLogEx(NORMAL, " [set clock as integer] optional, if not set, autodetect."); PrintAndLogEx(NORMAL, " , 1 for invert output"); @@ -178,7 +189,8 @@ int usage_data_rawdemod_p2(void){ PrintAndLogEx(NORMAL, " : data rawdemod p2 64 1 0 = demod a psk2 tag from GraphBuffer using a clock of RF/64, inverting output and allowing 0 demod errors"); return 0; } -int usage_data_autocorr(void) { +int usage_data_autocorr(void) +{ PrintAndLogEx(NORMAL, "Autocorrelate is used to detect repeating sequences. We use it as detection of length in bits a message inside the signal is"); PrintAndLogEx(NORMAL, "Usage: data autocorr w [g]"); PrintAndLogEx(NORMAL, "Options:"); @@ -187,7 +199,8 @@ int usage_data_autocorr(void) { PrintAndLogEx(NORMAL, " g save back to GraphBuffer (overwrite)"); return 0; } -int usage_data_undecimate(void){ +int usage_data_undecimate(void) +{ PrintAndLogEx(NORMAL, "Usage: data undec [factor]"); PrintAndLogEx(NORMAL, "This function performs un-decimation, by repeating each sample N times"); PrintAndLogEx(NORMAL, "Options:"); @@ -196,7 +209,8 @@ int usage_data_undecimate(void){ PrintAndLogEx(NORMAL, "Example: 'data undec 3'"); return 0; } -int usage_data_detectclock(void){ +int usage_data_detectclock(void) +{ PrintAndLogEx(NORMAL, "Usage: data detectclock [modulation] "); PrintAndLogEx(NORMAL, " [modulation as char], specify the modulation type you want to detect the clock of"); PrintAndLogEx(NORMAL, " , specify the clock (optional - to get best start position only)"); @@ -208,39 +222,44 @@ int usage_data_detectclock(void){ PrintAndLogEx(NORMAL, " data detectclock n = detect the clock of an nrz/direct modulated wave in the GraphBuffer"); return 0; } -int usage_data_hex2bin(void){ +int usage_data_hex2bin(void) +{ PrintAndLogEx(NORMAL, "Usage: data hex2bin "); PrintAndLogEx(NORMAL, " This function will ignore all non-hexadecimal characters (but stop reading on whitespace)"); return 0; } -int usage_data_bin2hex(void){ +int usage_data_bin2hex(void) +{ PrintAndLogEx(NORMAL, "Usage: data bin2hex "); PrintAndLogEx(NORMAL, " This function will ignore all characters not 1 or 0 (but stop reading on whitespace)"); return 0; } -int usage_data_buffclear(void){ +int usage_data_buffclear(void) +{ PrintAndLogEx(NORMAL, "This function clears the bigbuff on deviceside"); PrintAndLogEx(NORMAL, "Usage: data buffclear [h]"); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " h This help"); return 0; } -int usage_data_fsktonrz() { - PrintAndLogEx(NORMAL, "Usage: data fsktonrz c l f "); - PrintAndLogEx(NORMAL, "Options:"); - PrintAndLogEx(NORMAL, " h This help"); - PrintAndLogEx(NORMAL, " c enter the a clock (omit to autodetect)"); - PrintAndLogEx(NORMAL, " l enter a field clock (omit to autodetect)"); - PrintAndLogEx(NORMAL, " f enter a field clock (omit to autodetect)"); - return 0; +int usage_data_fsktonrz() +{ + PrintAndLogEx(NORMAL, "Usage: data fsktonrz c l f "); + PrintAndLogEx(NORMAL, "Options:"); + PrintAndLogEx(NORMAL, " h This help"); + PrintAndLogEx(NORMAL, " c enter the a clock (omit to autodetect)"); + PrintAndLogEx(NORMAL, " l enter a field clock (omit to autodetect)"); + PrintAndLogEx(NORMAL, " f enter a field clock (omit to autodetect)"); + return 0; } //set the demod buffer with given array of binary (one bit per byte) //by marshmellow -void setDemodBuf(uint8_t *buf, size_t size, size_t start_idx) { +void setDemodBuf(uint8_t *buf, size_t size, size_t start_idx) +{ if (buf == NULL) return; - if ( size > MAX_DEMOD_BUF_LEN - start_idx) + if (size > MAX_DEMOD_BUF_LEN - start_idx) size = MAX_DEMOD_BUF_LEN - start_idx; for (size_t i = 0; i < size; i++) @@ -249,7 +268,8 @@ void setDemodBuf(uint8_t *buf, size_t size, size_t start_idx) { DemodBufferLen = size; } -bool getDemodBuf(uint8_t *buf, size_t *size) { +bool getDemodBuf(uint8_t *buf, size_t *size) +{ if (buf == NULL) return false; if (size == NULL) return false; if (*size == 0) return false; @@ -262,49 +282,56 @@ bool getDemodBuf(uint8_t *buf, size_t *size) { // include // Root mean square -double rms(double *v, size_t n) { +double rms(double *v, size_t n) +{ double sum = 0.0; - for(size_t i = 0; i < n; i++) + for (size_t i = 0; i < n; i++) sum += v[i] * v[i]; return sqrt(sum / n); } -int cmp_int( const void *a, const void *b) { - if (*(const int *)a < *(const int *)b) +int cmp_int(const void *a, const void *b) +{ + if (*(const int *)a < * (const int *)b) return -1; else return *(const int *)a > *(const int *)b; } -int cmp_uint8( const void *a, const void *b) { - if (*(const uint8_t *)a < *(const uint8_t *)b) +int cmp_uint8(const void *a, const void *b) +{ + if (*(const uint8_t *)a < * (const uint8_t *)b) return -1; else return *(const uint8_t *)a > *(const uint8_t *)b; } // Median of a array of values -double median_int( int *src, size_t size ) { - qsort( src, size, sizeof(int), cmp_int); - return 0.5 * ( src[size/2] + src[(size-1)/2]); +double median_int(int *src, size_t size) +{ + qsort(src, size, sizeof(int), cmp_int); + return 0.5 * (src[size / 2] + src[(size - 1) / 2]); } -double median_uint8( uint8_t *src, size_t size ) { - qsort( src, size, sizeof(uint8_t), cmp_uint8); - return 0.5 * ( src[size/2] + src[(size-1)/2]); +double median_uint8(uint8_t *src, size_t size) +{ + qsort(src, size, sizeof(uint8_t), cmp_uint8); + return 0.5 * (src[size / 2] + src[(size - 1) / 2]); } // function to compute mean for a series -static double compute_mean(const int *data, size_t n) { +static double compute_mean(const int *data, size_t n) +{ double mean = 0.0; - for (size_t i=0; i < n; i++) + for (size_t i = 0; i < n; i++) mean += data[i]; mean /= n; return mean; } // function to compute variance for a series -static double compute_variance(const int *data, size_t n) { +static double compute_variance(const int *data, size_t n) +{ double variance = 0.0; double mean = compute_mean(data, n); - for (size_t i=0; i < n; i++) - variance += pow(( data[i] - mean), 2.0); + for (size_t i = 0; i < n; i++) + variance += pow((data[i] - mean), 2.0); variance /= n; return variance; @@ -335,7 +362,8 @@ static double compute_autoc(const int *data, size_t n, int lag) { */ // option '1' to save DemodBuffer any other to restore -void save_restoreDB(uint8_t saveOpt) { +void save_restoreDB(uint8_t saveOpt) +{ static uint8_t SavedDB[MAX_DEMOD_BUF_LEN]; static size_t SavedDBlen; static bool DB_Saved = false; @@ -358,7 +386,8 @@ void save_restoreDB(uint8_t saveOpt) { } } -int CmdSetDebugMode(const char *Cmd) { +int CmdSetDebugMode(const char *Cmd) +{ int demod = 0; sscanf(Cmd, "%i", &demod); g_debugMode = (uint8_t)demod; @@ -367,7 +396,8 @@ int CmdSetDebugMode(const char *Cmd) { //by marshmellow // max output to 512 bits if we have more - should be plenty -void printDemodBuff(void) { +void printDemodBuff(void) +{ int len = DemodBufferLen; if (len < 1) { PrintAndLogEx(NORMAL, "(printDemodBuff) no bits found in demod buffer"); @@ -375,10 +405,11 @@ void printDemodBuff(void) { } if (len > 512) len = 512; - PrintAndLogEx(NORMAL, "%s", sprint_bin_break(DemodBuffer, len, 16) ); + PrintAndLogEx(NORMAL, "%s", sprint_bin_break(DemodBuffer, len, 16)); } -int CmdPrintDemodBuff(const char *Cmd) { +int CmdPrintDemodBuff(const char *Cmd) +{ char hex[512] = {0x00}; bool hexMode = false; bool errors = false; @@ -387,26 +418,26 @@ int CmdPrintDemodBuff(const char *Cmd) { char cmdp = 0; while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': - return usage_data_printdemodbuf(); - case 'x': - hexMode = true; - cmdp++; - break; - case 'o': - offset = param_get32ex(Cmd, cmdp+1, 0, 10); - if (!offset) errors = true; - cmdp += 2; - break; - case 'l': - length = param_get32ex(Cmd, cmdp+1, 512, 10); - if (!length) errors = true; - cmdp += 2; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; + case 'h': + return usage_data_printdemodbuf(); + case 'x': + hexMode = true; + cmdp++; + break; + case 'o': + offset = param_get32ex(Cmd, cmdp + 1, 0, 10); + if (!offset) errors = true; + cmdp += 2; + break; + case 'l': + length = param_get32ex(Cmd, cmdp + 1, 512, 10); + if (!length) errors = true; + cmdp += 2; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); + errors = true; + break; } } //Validations @@ -416,11 +447,11 @@ int CmdPrintDemodBuff(const char *Cmd) { PrintAndLogEx(NORMAL, "Demodbuffer is empty"); return 0; } - length = (length > (DemodBufferLen-offset)) ? DemodBufferLen-offset : length; + length = (length > (DemodBufferLen - offset)) ? DemodBufferLen - offset : length; int numBits = (length) & 0x00FFC; //make sure we don't exceed our string - if (hexMode){ - char *buf = (char *) (DemodBuffer + offset); + if (hexMode) { + char *buf = (char *)(DemodBuffer + offset); numBits = (numBits > sizeof(hex)) ? sizeof(hex) : numBits; numBits = binarraytohex(hex, buf, numBits); if (numBits == 0) { @@ -428,14 +459,15 @@ int CmdPrintDemodBuff(const char *Cmd) { } PrintAndLogEx(NORMAL, "DemodBuffer: %s", hex); } else { - PrintAndLogEx(NORMAL, "DemodBuffer:\n%s", sprint_bin_break(DemodBuffer+offset, numBits, 16)); + PrintAndLogEx(NORMAL, "DemodBuffer:\n%s", sprint_bin_break(DemodBuffer + offset, numBits, 16)); } return 1; } //by marshmellow //this function strictly converts >1 to 1 and <1 to 0 for each sample in the graphbuffer -int CmdGetBitStream(const char *Cmd) { +int CmdGetBitStream(const char *Cmd) +{ CmdHpf(Cmd); for (uint32_t i = 0; i < GraphTraceLen; i++) GraphBuffer[i] = (GraphBuffer[i] >= 1) ? 1 : 0; @@ -450,7 +482,8 @@ int CmdGetBitStream(const char *Cmd) { //verbose will print results and demoding messages //emSearch will auto search for EM410x format in bitstream //askType switches decode: ask/raw = 0, ask/manchester = 1 -int ASKDemod_ext(const char *Cmd, bool verbose, bool emSearch, uint8_t askType, bool *stCheck) { +int ASKDemod_ext(const char *Cmd, bool verbose, bool emSearch, uint8_t askType, bool *stCheck) +{ int invert = 0; int clk = 0; int maxErr = 100; @@ -503,12 +536,12 @@ int ASKDemod_ext(const char *Cmd, bool verbose, bool emSearch, uint8_t askType, int startIdx = 0; int errCnt = askdemod_ext(bits, &BitLen, &clk, &invert, maxErr, askamp, askType, &startIdx); - if (errCnt < 0 || BitLen < 16){ //if fatal error (or -1) - PrintAndLogEx(DEBUG, "DEBUG: (ASKDemod_ext) No data found errors:%d, invert:%c, bitlen:%d, clock:%d", errCnt, (invert)?'Y':'N', BitLen, clk); + if (errCnt < 0 || BitLen < 16) { //if fatal error (or -1) + PrintAndLogEx(DEBUG, "DEBUG: (ASKDemod_ext) No data found errors:%d, invert:%c, bitlen:%d, clock:%d", errCnt, (invert) ? 'Y' : 'N', BitLen, clk); return 0; } - if (errCnt > maxErr){ + if (errCnt > maxErr) { PrintAndLogEx(DEBUG, "DEBUG: (ASKDemod_ext) Too many errors found, errors:%d, bits:%d, clock:%d", errCnt, BitLen, clk); return 0; } @@ -519,13 +552,13 @@ int ASKDemod_ext(const char *Cmd, bool verbose, bool emSearch, uint8_t askType, setDemodBuf(bits, BitLen, 0); setClockGrid(clk, startIdx); - if (verbose || g_debugMode){ + if (verbose || g_debugMode) { if (errCnt > 0) - PrintAndLogEx(NORMAL, "# Errors during Demoding (shown as 7 in bit stream): %d",errCnt); + PrintAndLogEx(NORMAL, "# Errors during Demoding (shown as 7 in bit stream): %d", errCnt); if (askType) - PrintAndLogEx(NORMAL, "ASK/Manchester - Clock: %d - Decoded bitstream:",clk); + PrintAndLogEx(NORMAL, "ASK/Manchester - Clock: %d - Decoded bitstream:", clk); else - PrintAndLogEx(NORMAL, "ASK/Raw - Clock: %d - Decoded bitstream:",clk); + PrintAndLogEx(NORMAL, "ASK/Raw - Clock: %d - Decoded bitstream:", clk); // Now output the bitstream to the scrollback by line of 16 bits printDemodBuff(); } @@ -536,7 +569,8 @@ int ASKDemod_ext(const char *Cmd, bool verbose, bool emSearch, uint8_t askType, return 1; } -int ASKDemod(const char *Cmd, bool verbose, bool emSearch, uint8_t askType) { +int ASKDemod(const char *Cmd, bool verbose, bool emSearch, uint8_t askType) +{ bool st = false; return ASKDemod_ext(Cmd, verbose, emSearch, askType, &st); } @@ -562,7 +596,8 @@ int Cmdaskmandemod(const char *Cmd) //by marshmellow //manchester decode //stricktly take 10 and 01 and convert to 0 and 1 -int Cmdmandecoderaw(const char *Cmd) { +int Cmdmandecoderaw(const char *Cmd) +{ size_t size = 0; int high = 0, low = 0; int i = 0, errCnt = 0, invert = 0, maxErr = 20; @@ -573,15 +608,15 @@ int Cmdmandecoderaw(const char *Cmd) { uint8_t bits[MAX_DEMOD_BUF_LEN] = {0}; - for (; i < DemodBufferLen; ++i){ + for (; i < DemodBufferLen; ++i) { if (DemodBuffer[i] > high) high = DemodBuffer[i]; - else if(DemodBuffer[i] < low) + else if (DemodBuffer[i] < low) low = DemodBuffer[i]; bits[i] = DemodBuffer[i]; } - if (high > 7 || low < 0 ){ + if (high > 7 || low < 0) { PrintAndLogEx(WARNING, "Error: please raw demod the wave first then manchester raw decode"); return 0; } @@ -590,19 +625,19 @@ int Cmdmandecoderaw(const char *Cmd) { size = i; uint8_t alignPos = 0; errCnt = manrawdecode(bits, &size, invert, &alignPos); - if (errCnt >= maxErr){ - PrintAndLogEx(WARNING, "Too many errors: %d",errCnt); + if (errCnt >= maxErr) { + PrintAndLogEx(WARNING, "Too many errors: %d", errCnt); return 0; } - PrintAndLogEx(NORMAL, "Manchester Decoded - # errors:%d - data:",errCnt); + PrintAndLogEx(NORMAL, "Manchester Decoded - # errors:%d - data:", errCnt); PrintAndLogEx(NORMAL, "%s", sprint_bin_break(bits, size, 16)); - if (errCnt == 0){ + if (errCnt == 0) { uint64_t id = 0; uint32_t hi = 0; size_t idx = 0; - if (Em410xDecode(bits, &size, &idx, &hi, &id) == 1){ + if (Em410xDecode(bits, &size, &idx, &hi, &id) == 1) { //need to adjust to set bitstream back to manchester encoded data //setDemodBuf(bits, size, idx); printEM410x(hi, id); @@ -617,43 +652,44 @@ int Cmdmandecoderaw(const char *Cmd) { //takes 2 arguments "offset" default = 0 if 1 it will shift the decode by one bit // and "invert" default = 0 if 1 it will invert output // the argument offset allows us to manually shift if the output is incorrect - [EDIT: now auto detects] -int CmdBiphaseDecodeRaw(const char *Cmd) { +int CmdBiphaseDecodeRaw(const char *Cmd) +{ size_t size = 0; int offset = 0, invert = 0, maxErr = 20, errCnt = 0; char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) > 3 || cmdp == 'h') return usage_data_biphaserawdecode(); sscanf(Cmd, "%i %i %i", &offset, &invert, &maxErr); - if (DemodBufferLen == 0){ + if (DemodBufferLen == 0) { PrintAndLogEx(NORMAL, "DemodBuffer Empty - run 'data rawdemod ar' first"); return 0; } uint8_t bits[MAX_DEMOD_BUF_LEN] = {0}; size = sizeof(bits); - if ( !getDemodBuf(bits, &size) ) return 0; + if (!getDemodBuf(bits, &size)) return 0; errCnt = BiphaseRawDecode(bits, &size, &offset, invert); - if (errCnt < 0){ + if (errCnt < 0) { PrintAndLogEx(WARNING, "Error during decode:%d", errCnt); return 0; } - if (errCnt > maxErr){ - PrintAndLogEx(WARNING, "Too many errors attempting to decode: %d",errCnt); + if (errCnt > maxErr) { + PrintAndLogEx(WARNING, "Too many errors attempting to decode: %d", errCnt); return 0; } if (errCnt > 0) - PrintAndLogEx(WARNING, "# Errors found during Demod (shown as 7 in bit stream): %d",errCnt); + PrintAndLogEx(WARNING, "# Errors found during Demod (shown as 7 in bit stream): %d", errCnt); - PrintAndLogEx(NORMAL, "Biphase Decoded using offset: %d - # invert:%d - data:",offset,invert); + PrintAndLogEx(NORMAL, "Biphase Decoded using offset: %d - # invert:%d - data:", offset, invert); PrintAndLogEx(NORMAL, "%s", sprint_bin_break(bits, size, 16)); //remove first bit from raw demod if (offset) - setDemodBuf(DemodBuffer,DemodBufferLen-offset, offset); + setDemodBuf(DemodBuffer, DemodBufferLen - offset, offset); - setClockGrid(g_DemodClock, g_DemodStartIdx + g_DemodClock*offset/2); + setClockGrid(g_DemodClock, g_DemodStartIdx + g_DemodClock * offset / 2); return 1; } @@ -662,26 +698,26 @@ int CmdBiphaseDecodeRaw(const char *Cmd) { int ASKbiphaseDemod(const char *Cmd, bool verbose) { //ask raw demod GraphBuffer first - int offset=0, clk=0, invert=0, maxErr=0; + int offset = 0, clk = 0, invert = 0, maxErr = 0; sscanf(Cmd, "%i %i %i %i", &offset, &clk, &invert, &maxErr); uint8_t BitStream[MAX_DEMOD_BUF_LEN]; size_t size = getFromGraphBuf(BitStream); - if (size == 0 ) { + if (size == 0) { PrintAndLogEx(DEBUG, "DEBUG: no data in graphbuf"); return 0; } int startIdx = 0; //invert here inverts the ask raw demoded bits which has no effect on the demod, but we need the pointer int errCnt = askdemod_ext(BitStream, &size, &clk, &invert, maxErr, 0, 0, &startIdx); - if ( errCnt < 0 || errCnt > maxErr ) { + if (errCnt < 0 || errCnt > maxErr) { PrintAndLogEx(DEBUG, "DEBUG: no data or error found %d, clock: %d", errCnt, clk); return 0; } //attempt to Biphase decode BitStream errCnt = BiphaseRawDecode(BitStream, &size, &offset, invert); - if (errCnt < 0){ + if (errCnt < 0) { if (g_debugMode || verbose) PrintAndLogEx(DEBUG, "DEBUG: Error BiphaseRawDecode: %d", errCnt); return 0; } @@ -691,9 +727,9 @@ int ASKbiphaseDemod(const char *Cmd, bool verbose) } //success set DemodBuffer and return setDemodBuf(BitStream, size, 0); - setClockGrid(clk, startIdx + clk*offset/2); - if (g_debugMode || verbose){ - PrintAndLogEx(NORMAL, "Biphase Decoded using offset: %d - clock: %d - # errors:%d - data:",offset,clk,errCnt); + setClockGrid(clk, startIdx + clk * offset / 2); + if (g_debugMode || verbose) { + PrintAndLogEx(NORMAL, "Biphase Decoded using offset: %d - clock: %d - # errors:%d - data:", offset, clk, errCnt); printDemodBuff(); } return 1; @@ -716,9 +752,10 @@ int Cmdaskrawdemod(const char *Cmd) return ASKDemod(Cmd, true, false, 0); } -int AutoCorrelate(const int *in, int *out, size_t len, int window, bool SaveGrph, bool verbose) { +int AutoCorrelate(const int *in, int *out, size_t len, int window, bool SaveGrph, bool verbose) +{ // sanity check - if ( window > len ) window = len; + if (window > len) window = len; if (verbose) PrintAndLogEx(INFO, "performing %d correlations", GraphTraceLen - window); @@ -738,8 +775,8 @@ int AutoCorrelate(const int *in, int *out, size_t len, int window, bool SaveGrph for (int i = 0; i < len - window; ++i) { - for (size_t j=0; j < (len - i); j++) { - autocv += (in[j] - mean) * (in[j+i] - mean); + for (size_t j = 0; j < (len - i); j++) { + autocv += (in[j] - mean) * (in[j + i] - mean); } autocv = (1.0 / (len - i)) * autocv; @@ -749,8 +786,8 @@ int AutoCorrelate(const int *in, int *out, size_t len, int window, bool SaveGrph ac_value = autocv / variance; // keep track of which distance is repeating. - if ( ac_value > 1) { - correlation = i-lastmax; + if (ac_value > 1) { + correlation = i - lastmax; lastmax = i; } } @@ -758,26 +795,26 @@ int AutoCorrelate(const int *in, int *out, size_t len, int window, bool SaveGrph // int hi = 0, idx = 0; int distance = 0, hi_1 = 0, idx_1 = 0; - for (int i = 0; i <= len; ++i){ - if ( CorrelBuffer[i] > hi) { + for (int i = 0; i <= len; ++i) { + if (CorrelBuffer[i] > hi) { hi = CorrelBuffer[i]; idx = i; } } - for (int i = idx+1; i <= window; ++i){ - if ( CorrelBuffer[i] > hi_1 ) { + for (int i = idx + 1; i <= window; ++i) { + if (CorrelBuffer[i] > hi_1) { hi_1 = CorrelBuffer[i]; idx_1 = i; } } - int foo = ABS(hi-hi_1); - int bar = (int)((int)((hi+hi_1) / 2) * 0.03); - if ( verbose && foo < bar ) { + int foo = ABS(hi - hi_1); + int bar = (int)((int)((hi + hi_1) / 2) * 0.03); + if (verbose && foo < bar) { distance = idx_1 - idx; PrintAndLogEx(SUCCESS, "possible 3% visible correlation %4d samples", distance); - } else if (verbose && ( correlation > 1 ) ) { + } else if (verbose && (correlation > 1)) { PrintAndLogEx(SUCCESS, "possible correlation %4d samples", correlation); } else { PrintAndLogEx(FAILED, "no repeating pattern found, try increasing window size"); @@ -787,15 +824,14 @@ int AutoCorrelate(const int *in, int *out, size_t len, int window, bool SaveGrph if (SaveGrph) { //GraphTraceLen = GraphTraceLen - window; memcpy(out, CorrelBuffer, len * sizeof(int)); - if ( distance > 0) { + if (distance > 0) { setClockGrid(distance, idx); retval = distance; - } - else + } else setClockGrid(correlation, idx); CursorCPos = idx_1; - CursorDPos = idx_1+retval; + CursorDPos = idx_1 + retval; DemodBufferLen = 0; RepaintGraphWindow(); } @@ -803,7 +839,8 @@ int AutoCorrelate(const int *in, int *out, size_t len, int window, bool SaveGrph return retval; } -int CmdAutoCorr(const char *Cmd) { +int CmdAutoCorr(const char *Cmd) +{ uint32_t window = 4000; uint8_t cmdp = 0; @@ -819,7 +856,7 @@ int CmdAutoCorr(const char *Cmd) { cmdp++; break; case 'w': - window = param_get32ex(Cmd, cmdp+1, 4000, 10); + window = param_get32ex(Cmd, cmdp + 1, 4000, 10); if (window >= GraphTraceLen) { PrintAndLogEx(WARNING, "window must be smaller than trace (%d samples)", GraphTraceLen); errors = true; @@ -833,7 +870,7 @@ int CmdAutoCorr(const char *Cmd) { } } //Validations - if (errors || cmdp == 0 ) return usage_data_autocorr(); + if (errors || cmdp == 0) return usage_data_autocorr(); return AutoCorrelate(GraphBuffer, GraphBuffer, GraphTraceLen, window, updateGrph, true); } @@ -843,14 +880,14 @@ int CmdBitsamples(const char *Cmd) int cnt = 0; uint8_t got[12288]; - if (!GetFromDevice(BIG_BUF, got, sizeof(got), 0, NULL, 2500 , false)) { + if (!GetFromDevice(BIG_BUF, got, sizeof(got), 0, NULL, 2500, false)) { PrintAndLogEx(WARNING, "command execution time out"); return false; } for (int j = 0; j < sizeof(got); j++) { for (int k = 0; k < 8; k++) { - if(got[j] & (1 << (7 - k))) + if (got[j] & (1 << (7 - k))) GraphBuffer[cnt++] = 1; else GraphBuffer[cnt++] = 0; @@ -866,7 +903,7 @@ int CmdBuffClear(const char *Cmd) char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_data_buffclear(); - UsbCommand c = {CMD_BUFF_CLEAR, {0,0,0}}; + UsbCommand c = {CMD_BUFF_CLEAR, {0, 0, 0}}; clearCommandBuffer(); SendCommand(&c); ClearGraph(true); @@ -899,11 +936,10 @@ int CmdUndec(const char *Cmd) //We have memory, don't we? int swap[MAX_GRAPH_TRACE_LEN] = {0}; uint32_t g_index = 0, s_index = 0; - while(g_index < GraphTraceLen && s_index + factor < MAX_GRAPH_TRACE_LEN) - { + while (g_index < GraphTraceLen && s_index + factor < MAX_GRAPH_TRACE_LEN) { int count = 0; for (count = 0; count < factor && s_index + count < MAX_GRAPH_TRACE_LEN; count++) - swap[s_index+count] = GraphBuffer[g_index]; + swap[s_index + count] = GraphBuffer[g_index]; s_index += count; g_index++; } @@ -916,13 +952,14 @@ int CmdUndec(const char *Cmd) //by marshmellow //shift graph zero up or down based on input + or - -int CmdGraphShiftZero(const char *Cmd) { +int CmdGraphShiftZero(const char *Cmd) +{ int shift = 0, shiftedVal = 0; //set options from parameters entered with the command sscanf(Cmd, "%i", &shift); - for(int i = 0; i < GraphTraceLen; i++){ - if ( i+shift >= GraphTraceLen) + for (int i = 0; i < GraphTraceLen; i++) { + if (i + shift >= GraphTraceLen) shiftedVal = GraphBuffer[i]; else shiftedVal = GraphBuffer[i] + shift; @@ -937,14 +974,15 @@ int CmdGraphShiftZero(const char *Cmd) { return 0; } -int AskEdgeDetect(const int *in, int *out, int len, int threshold) { +int AskEdgeDetect(const int *in, int *out, int len, int threshold) +{ int last = 0; - for(int i = 1; i= threshold) //large jump up + for (int i = 1; i < len; i++) { + if (in[i] - in[i - 1] >= threshold) //large jump up last = 127; - else if (in[i] - in[i-1] <= -1 * threshold) //large jump down + else if (in[i] - in[i - 1] <= -1 * threshold) //large jump down last = -127; - out[i-1] = last; + out[i - 1] = last; } return 0; } @@ -953,7 +991,8 @@ int AskEdgeDetect(const int *in, int *out, int len, int threshold) { //use large jumps in read samples to identify edges of waves and then amplify that wave to max //similar to dirtheshold, threshold commands //takes a threshold length which is the measured length between two samples then determines an edge -int CmdAskEdgeDetect(const char *Cmd) { +int CmdAskEdgeDetect(const char *Cmd) +{ int thresLen = 25; int ans = 0; sscanf(Cmd, "%i", &thresLen); @@ -966,15 +1005,16 @@ int CmdAskEdgeDetect(const char *Cmd) { /* Print our clock rate */ // uses data from graphbuffer // adjusted to take char parameter for type of modulation to find the clock - by marshmellow. -int CmdDetectClockRate(const char *Cmd) { +int CmdDetectClockRate(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) > 6 || strlen(Cmd) == 0 || cmdp == 'h') return usage_data_detectclock(); int clock = 0; - switch ( cmdp ) { + switch (cmdp) { case 'a' : - clock = GetAskClock(Cmd+1, true); + clock = GetAskClock(Cmd + 1, true); break; case 'f' : clock = GetFskClock("", true); @@ -999,7 +1039,7 @@ char *GetFSKType(uint8_t fchigh, uint8_t fclow, uint8_t invert) memset(fType, 0x00, 8); char *fskType = fType; - if (fchigh == 10 && fclow == 8){ + if (fchigh == 10 && fclow == 8) { if (invert) memcpy(fskType, "FSK2a", 5); @@ -1023,7 +1063,8 @@ char *GetFSKType(uint8_t fchigh, uint8_t fclow, uint8_t invert) //fsk raw demod and print binary //takes 4 arguments - Clock, invert, fchigh, fclow //defaults: clock = 50, invert=1, fchigh=10, fclow=8 (RF/10 RF/8 (fsk2a)) -int FSKrawDemod(const char *Cmd, bool verbose) { +int FSKrawDemod(const char *Cmd, bool verbose) +{ //raw fsk demod no manchester decoding no start bit finding just get binary from wave uint8_t rfLen, invert, fchigh, fclow; @@ -1089,7 +1130,8 @@ int FSKrawDemod(const char *Cmd, bool verbose) { //fsk raw demod and print binary //takes 4 arguments - Clock, invert, fchigh, fclow //defaults: clock = 50, invert=1, fchigh=10, fclow=8 (RF/10 RF/8 (fsk2a)) -int CmdFSKrawdemod(const char *Cmd) { +int CmdFSKrawdemod(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) > 20 || cmdp == 'h') return usage_data_rawdemod_fs(); @@ -1098,7 +1140,8 @@ int CmdFSKrawdemod(const char *Cmd) { //by marshmellow //attempt to psk1 demod graph buffer -int PSKDemod(const char *Cmd, bool verbose) { +int PSKDemod(const char *Cmd, bool verbose) +{ int invert = 0, clk = 0, maxErr = 100; sscanf(Cmd, "%i %i %i", &clk, &invert, &maxErr); if (clk == 1) { @@ -1119,17 +1162,17 @@ int PSKDemod(const char *Cmd, bool verbose) { int errCnt = 0; int startIdx = 0; errCnt = pskRawDemod_ext(BitStream, &BitLen, &clk, &invert, &startIdx); - if (errCnt > maxErr){ + if (errCnt > maxErr) { if (g_debugMode || verbose) PrintAndLogEx(DEBUG, "DEBUG: (PSKdemod) Too many errors found, clk: %d, invert: %d, numbits: %d, errCnt: %d", clk, invert, BitLen, errCnt); return 0; } - if (errCnt < 0|| BitLen < 16){ //throw away static - allow 1 and -1 (in case of threshold command first) + if (errCnt < 0 || BitLen < 16) { //throw away static - allow 1 and -1 (in case of threshold command first) if (g_debugMode || verbose) PrintAndLogEx(DEBUG, "DEBUG: (PSKdemod) no data found, clk: %d, invert: %d, numbits: %d, errCnt: %d", clk, invert, BitLen, errCnt); return 0; } - if (verbose || g_debugMode){ - PrintAndLogEx(DEBUG, "DEBUG: (PSKdemod) Using Clock:%d, invert:%d, Bits Found:%d",clk, invert, BitLen); - if (errCnt > 0){ + if (verbose || g_debugMode) { + PrintAndLogEx(DEBUG, "DEBUG: (PSKdemod) Using Clock:%d, invert:%d, Bits Found:%d", clk, invert, BitLen); + if (errCnt > 0) { PrintAndLogEx(DEBUG, "DEBUG: (PSKdemod) errors during Demoding (shown as 7 in bit stream): %d", errCnt); } } @@ -1139,7 +1182,8 @@ int PSKDemod(const char *Cmd, bool verbose) { return 1; } -int CmdPSKIdteck(const char *Cmd) { +int CmdPSKIdteck(const char *Cmd) +{ if (!PSKDemod("", false)) { PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck PSKDemod failed"); @@ -1149,7 +1193,7 @@ int CmdPSKIdteck(const char *Cmd) { //get binary from PSK1 wave int idx = detectIdteck(DemodBuffer, &size); - if (idx < 0){ + if (idx < 0) { if (idx == -1) PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: not enough samples"); @@ -1160,7 +1204,7 @@ int CmdPSKIdteck(const char *Cmd) { else if (idx == -4) PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: size not correct: %d", size); else - PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: idx: %d",idx); + PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: idx: %d", idx); // if didn't find preamble try again inverting if (!PSKDemod("1", false)) { @@ -1168,7 +1212,7 @@ int CmdPSKIdteck(const char *Cmd) { return 0; } idx = detectIdteck(DemodBuffer, &size); - if (idx < 0){ + if (idx < 0) { if (idx == -1) PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: not enough samples"); @@ -1179,7 +1223,7 @@ int CmdPSKIdteck(const char *Cmd) { else if (idx == -4) PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: size not correct: %d", size); else - PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: idx: %d",idx); + PrintAndLogEx(DEBUG, "DEBUG: Error - Idteck: idx: %d", idx); return 0; } @@ -1189,7 +1233,7 @@ int CmdPSKIdteck(const char *Cmd) { //got a good demod uint32_t id = 0; uint32_t raw1 = bytebits_to_byte(DemodBuffer, 32); - uint32_t raw2 = bytebits_to_byte(DemodBuffer+32, 32); + uint32_t raw2 = bytebits_to_byte(DemodBuffer + 32, 32); //parity check (TBD) //checksum check (TBD) @@ -1203,12 +1247,13 @@ int CmdPSKIdteck(const char *Cmd) { // takes 3 arguments - clock, invert, maxErr as integers // attempts to demodulate nrz only // prints binary found and saves in demodbuffer for further commands -int NRZrawDemod(const char *Cmd, bool verbose) { +int NRZrawDemod(const char *Cmd, bool verbose) +{ int errCnt = 0, clkStartIdx = 0; int invert = 0, clk = 0, maxErr = 100; sscanf(Cmd, "%i %i %i", &clk, &invert, &maxErr); - if (clk == 1){ + if (clk == 1) { invert = 1; clk = 0; } @@ -1227,11 +1272,11 @@ int NRZrawDemod(const char *Cmd, bool verbose) { if (BitLen == 0) return 0; errCnt = nrzRawDemod(bits, &BitLen, &clk, &invert, &clkStartIdx); - if (errCnt > maxErr){ - PrintAndLogEx(DEBUG, "DEBUG: (NRZrawDemod) Too many errors found, clk: %d, invert: %d, numbits: %d, errCnt: %d",clk,invert,BitLen,errCnt); + if (errCnt > maxErr) { + PrintAndLogEx(DEBUG, "DEBUG: (NRZrawDemod) Too many errors found, clk: %d, invert: %d, numbits: %d, errCnt: %d", clk, invert, BitLen, errCnt); return 0; } - if (errCnt < 0 || BitLen < 16){ //throw away static - allow 1 and -1 (in case of threshold command first) + if (errCnt < 0 || BitLen < 16) { //throw away static - allow 1 and -1 (in case of threshold command first) PrintAndLogEx(DEBUG, "DEBUG: (NRZrawDemod) no data found, clk: %d, invert: %d, numbits: %d, errCnt: %d", clk, invert, BitLen, errCnt); return 0; } @@ -1250,7 +1295,8 @@ int NRZrawDemod(const char *Cmd, bool verbose) { return 1; } -int CmdNRZrawDemod(const char *Cmd) { +int CmdNRZrawDemod(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) > 16 || cmdp == 'h') return usage_data_rawdemod_nr(); @@ -1261,14 +1307,15 @@ int CmdNRZrawDemod(const char *Cmd) { // takes 3 arguments - clock, invert, maxErr as integers // attempts to demodulate psk only // prints binary found and saves in demodbuffer for further commands -int CmdPSK1rawDemod(const char *Cmd) { +int CmdPSK1rawDemod(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) > 16 || cmdp == 'h') return usage_data_rawdemod_p1(); int ans = PSKDemod(Cmd, true); //output - if (!ans){ - if (g_debugMode) PrintAndLogEx(WARNING, "Error demoding: %d",ans); + if (!ans) { + if (g_debugMode) PrintAndLogEx(WARNING, "Error demoding: %d", ans); return 0; } PrintAndLogEx(NORMAL, "PSK1 demoded bitstream:"); @@ -1279,13 +1326,14 @@ int CmdPSK1rawDemod(const char *Cmd) { // by marshmellow // takes same args as cmdpsk1rawdemod -int CmdPSK2rawDemod(const char *Cmd) { +int CmdPSK2rawDemod(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) > 16 || cmdp == 'h') return usage_data_rawdemod_p2(); int ans = PSKDemod(Cmd, true); - if (!ans){ - if (g_debugMode) PrintAndLogEx(WARNING, "Error demoding: %d",ans); + if (!ans) { + if (g_debugMode) PrintAndLogEx(WARNING, "Error demoding: %d", ans); return 0; } psk1TOpsk2(DemodBuffer, DemodBufferLen); @@ -1296,27 +1344,29 @@ int CmdPSK2rawDemod(const char *Cmd) { } // by marshmellow - combines all raw demod functions into one menu command -int CmdRawDemod(const char *Cmd) { +int CmdRawDemod(const char *Cmd) +{ int ans = 0; if (strlen(Cmd) > 35 || strlen(Cmd) < 2) return usage_data_rawdemod(); - str_lower( (char *)Cmd); + str_lower((char *)Cmd); - if (str_startswith(Cmd, "fs")) ans = CmdFSKrawdemod(Cmd+2); - else if(str_startswith(Cmd, "ab")) ans = Cmdaskbiphdemod(Cmd+2); - else if(str_startswith(Cmd, "am")) ans = Cmdaskmandemod(Cmd+2); - else if(str_startswith(Cmd, "ar")) ans = Cmdaskrawdemod(Cmd+2); - else if(str_startswith(Cmd, "nr")) ans = CmdNRZrawDemod(Cmd+2); - else if(str_startswith(Cmd, "p1")) ans = CmdPSK1rawDemod(Cmd+2); - else if(str_startswith(Cmd, "p2")) ans = CmdPSK2rawDemod(Cmd+2); + if (str_startswith(Cmd, "fs")) ans = CmdFSKrawdemod(Cmd + 2); + else if (str_startswith(Cmd, "ab")) ans = Cmdaskbiphdemod(Cmd + 2); + else if (str_startswith(Cmd, "am")) ans = Cmdaskmandemod(Cmd + 2); + else if (str_startswith(Cmd, "ar")) ans = Cmdaskrawdemod(Cmd + 2); + else if (str_startswith(Cmd, "nr")) ans = CmdNRZrawDemod(Cmd + 2); + else if (str_startswith(Cmd, "p1")) ans = CmdPSK1rawDemod(Cmd + 2); + else if (str_startswith(Cmd, "p2")) ans = CmdPSK2rawDemod(Cmd + 2); else PrintAndLogEx(WARNING, "Unknown modulation entered - see help ('h') for parameter structure"); return ans; } -void setClockGrid(int clk, int offset) { +void setClockGrid(int clk, int offset) +{ g_DemodStartIdx = offset; g_DemodClock = clk; PrintAndLogEx(DEBUG, "DEBUG: (setClockGrid) demodoffset %d, clk %d", offset, clk); @@ -1340,7 +1390,8 @@ void setClockGrid(int clk, int offset) { } } -int CmdGrid(const char *Cmd) { +int CmdGrid(const char *Cmd) +{ sscanf(Cmd, "%i %i", &PlotGridX, &PlotGridY); PlotGridXdefault = PlotGridX; PlotGridYdefault = PlotGridY; @@ -1348,16 +1399,18 @@ int CmdGrid(const char *Cmd) { return 0; } -int CmdSetGraphMarkers(const char *Cmd) { +int CmdSetGraphMarkers(const char *Cmd) +{ sscanf(Cmd, "%i %i", &CursorCPos, &CursorDPos); RepaintGraphWindow(); return 0; } -int CmdHexsamples(const char *Cmd) { +int CmdHexsamples(const char *Cmd) +{ int i, j, requested = 0, offset = 0; char string_buf[25]; - char* string_ptr = string_buf; + char *string_ptr = string_buf; uint8_t got[BIGBUF_SIZE]; sscanf(Cmd, "%i %i", &requested, &offset); @@ -1371,7 +1424,7 @@ int CmdHexsamples(const char *Cmd) { return 0; } - if ( !GetFromDevice(BIG_BUF, got, requested, offset, NULL, 2500, false)) { + if (!GetFromDevice(BIG_BUF, got, requested, offset, NULL, 2500, false)) { PrintAndLogEx(WARNING, "command execution time out"); return false; } @@ -1396,13 +1449,15 @@ int CmdHexsamples(const char *Cmd) { return 0; } -int CmdHide(const char *Cmd) { +int CmdHide(const char *Cmd) +{ HideGraphWindow(); return 0; } //zero mean GraphBuffer -int CmdHpf(const char *Cmd) { +int CmdHpf(const char *Cmd) +{ uint8_t bits[GraphTraceLen]; size_t size = getFromGraphBuf(bits); removeSignalOffset(bits, size); @@ -1415,34 +1470,37 @@ int CmdHpf(const char *Cmd) { return 0; } -bool _headBit( BitstreamOut *stream) { +bool _headBit(BitstreamOut *stream) +{ int bytepos = stream->position >> 3; // divide by 8 int bitpos = (stream->position++) & 7; // mask out 00000111 - return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1; + return (*(stream->buffer + bytepos) >> (7 - bitpos)) & 1; } -uint8_t getByte(uint8_t bits_per_sample, BitstreamOut* b) { +uint8_t getByte(uint8_t bits_per_sample, BitstreamOut *b) +{ uint8_t val = 0; - for(int i = 0 ; i < bits_per_sample; i++) - val |= (_headBit(b) << (7-i)); + for (int i = 0 ; i < bits_per_sample; i++) + val |= (_headBit(b) << (7 - i)); return val; } -int getSamples(int n, bool silent) { +int getSamples(int n, bool silent) +{ //If we get all but the last byte in bigbuf, // we don't have to worry about remaining trash // in the last byte in case the bits-per-sample // does not line up on byte boundaries - uint8_t got[BIGBUF_SIZE-1] = { 0 }; + uint8_t got[BIGBUF_SIZE - 1] = { 0 }; - if ( n == 0 || n > sizeof(got)) + if (n == 0 || n > sizeof(got)) n = sizeof(got); if (!silent) PrintAndLogEx(NORMAL, "Reading %d bytes from device memory\n", n); UsbCommand response; - if ( !GetFromDevice(BIG_BUF, got, n, 0, &response, 10000, true) ) { + if (!GetFromDevice(BIG_BUF, got, n, 0, &response, 10000, true)) { PrintAndLogEx(WARNING, "timeout while waiting for reply."); return 1; } @@ -1463,14 +1521,14 @@ int getSamples(int n, bool silent) { if (!silent) PrintAndLogEx(NORMAL, "Unpacking..."); BitstreamOut bout = { got, bits_per_sample * n, 0}; - int j =0; + int j = 0; for (j = 0; j * bits_per_sample < n * 8 && j < n; j++) { uint8_t sample = getByte(bits_per_sample, &bout); - GraphBuffer[j] = ((int) sample )- 128; + GraphBuffer[j] = ((int) sample) - 128; } GraphTraceLen = j; - if (!silent) PrintAndLogEx(NORMAL, "Unpacked %d samples" , j ); + if (!silent) PrintAndLogEx(NORMAL, "Unpacked %d samples", j); } else { for (int j = 0; j < n; j++) { @@ -1490,12 +1548,14 @@ int getSamples(int n, bool silent) { return 0; } -int CmdSamples(const char *Cmd) { +int CmdSamples(const char *Cmd) +{ int n = strtol(Cmd, NULL, 0); return getSamples(n, false); } -int CmdTuneSamples(const char *Cmd) { +int CmdTuneSamples(const char *Cmd) +{ #define NON_VOLTAGE 1000 #define LF_UNUSABLE_V 2000 #define LF_MARGINAL_V 10000 @@ -1512,13 +1572,14 @@ int CmdTuneSamples(const char *Cmd) { int timeout = 0; PrintAndLogEx(INFO, "\nmeasuring antenna characteristics, please wait..."); - UsbCommand c = {CMD_MEASURE_ANTENNA_TUNING, {0,0,0}}; + UsbCommand c = {CMD_MEASURE_ANTENNA_TUNING, {0, 0, 0}}; clearCommandBuffer(); SendCommand(&c); UsbCommand resp; while (!WaitForResponseTimeout(CMD_MEASURED_ANTENNA_TUNING, &resp, 2000)) { timeout++; - printf("."); fflush(stdout); + printf("."); + fflush(stdout); if (timeout > 7) { PrintAndLogEx(WARNING, "\nno response from Proxmark. Aborting..."); return 1; @@ -1533,45 +1594,45 @@ int CmdTuneSamples(const char *Cmd) { uint32_t peakf = resp.arg[2]; uint32_t peakv = resp.arg[2] >> 32; - if ( v_lf125 > NON_VOLTAGE ) - PrintAndLogEx(SUCCESS, "LF antenna: %5.2f V - 125.00 kHz", (v_lf125 * ANTENNA_ERROR)/1000.0); - if ( v_lf134 > NON_VOLTAGE ) - PrintAndLogEx(SUCCESS, "LF antenna: %5.2f V - 134.00 kHz", (v_lf134 * ANTENNA_ERROR)/1000.0); - if ( peakv > NON_VOLTAGE && peakf > 0 ) - PrintAndLogEx(SUCCESS, "LF optimal: %5.2f V - %6.2f kHz", (peakv * ANTENNA_ERROR)/1000.0, 12000.0/(peakf+1)); + if (v_lf125 > NON_VOLTAGE) + PrintAndLogEx(SUCCESS, "LF antenna: %5.2f V - 125.00 kHz", (v_lf125 * ANTENNA_ERROR) / 1000.0); + if (v_lf134 > NON_VOLTAGE) + PrintAndLogEx(SUCCESS, "LF antenna: %5.2f V - 134.00 kHz", (v_lf134 * ANTENNA_ERROR) / 1000.0); + if (peakv > NON_VOLTAGE && peakf > 0) + PrintAndLogEx(SUCCESS, "LF optimal: %5.2f V - %6.2f kHz", (peakv * ANTENNA_ERROR) / 1000.0, 12000.0 / (peakf + 1)); char judgement[20]; memset(judgement, 0, sizeof(judgement)); // LF evaluation if (peakv < LF_UNUSABLE_V) - sprintf(judgement, _RED_(UNUSABLE) ); + sprintf(judgement, _RED_(UNUSABLE)); else if (peakv < LF_MARGINAL_V) - sprintf(judgement, _YELLOW_(MARGINAL) ); + sprintf(judgement, _YELLOW_(MARGINAL)); else - sprintf(judgement, _GREEN_(OK) ); + sprintf(judgement, _GREEN_(OK)); PrintAndLogEx(NORMAL, "%sLF antenna is %s \n" - , (peakv < LF_UNUSABLE_V) ? _CYAN_([!]) : _GREEN_([+]) - , judgement - ); + , (peakv < LF_UNUSABLE_V) ? _CYAN_([!]) : _GREEN_([+]) + , judgement + ); // HF evaluation - if ( v_hf > NON_VOLTAGE ) - PrintAndLogEx(SUCCESS, "HF antenna: %5.2f V - 13.56 MHz", (v_hf * ANTENNA_ERROR)/1000.0); + if (v_hf > NON_VOLTAGE) + PrintAndLogEx(SUCCESS, "HF antenna: %5.2f V - 13.56 MHz", (v_hf * ANTENNA_ERROR) / 1000.0); memset(judgement, 0, sizeof(judgement)); if (v_hf < HF_UNUSABLE_V) - sprintf(judgement, _RED_(UNUSABLE) ); + sprintf(judgement, _RED_(UNUSABLE)); else if (v_hf < HF_MARGINAL_V) - sprintf(judgement, _YELLOW_(MARGINAL) ); + sprintf(judgement, _YELLOW_(MARGINAL)); else - sprintf(judgement, _GREEN_(OK) ); + sprintf(judgement, _GREEN_(OK)); PrintAndLogEx(NORMAL, "%sHF antenna is %s" - , (v_hf < HF_UNUSABLE_V) ? _CYAN_([!]) : _GREEN_([+]) - , judgement - ); + , (v_hf < HF_UNUSABLE_V) ? _CYAN_([!]) : _GREEN_([+]) + , judgement + ); // graph LF measurements // even here, these values has 3% error. @@ -1580,7 +1641,7 @@ int CmdTuneSamples(const char *Cmd) { GraphBuffer[i] = resp.d.asBytes[i] - 128; test += resp.d.asBytes[i]; } - if ( test > 0 ) { + if (test > 0) { PrintAndLogEx(SUCCESS, "\nDisplaying LF tuning graph. Divisor 89 is 134khz, 95 is 125khz.\n\n"); GraphTraceLen = 256; ShowGraphWindow(); @@ -1593,7 +1654,8 @@ int CmdTuneSamples(const char *Cmd) { return 0; } -int CmdLoad(const char *Cmd) { +int CmdLoad(const char *Cmd) +{ char filename[FILE_PATH_SIZE] = {0x00}; int len = 0; @@ -1609,11 +1671,11 @@ int CmdLoad(const char *Cmd) { GraphTraceLen = 0; char line[80]; - while (fgets(line, sizeof (line), f)) { + while (fgets(line, sizeof(line), f)) { GraphBuffer[GraphTraceLen] = atoi(line); GraphTraceLen++; - if ( GraphTraceLen >= MAX_GRAPH_TRACE_LEN ) + if (GraphTraceLen >= MAX_GRAPH_TRACE_LEN) break; } @@ -1629,20 +1691,21 @@ int CmdLoad(const char *Cmd) { setGraphBuf(bits, size); computeSignalProperties(bits, size); - setClockGrid(0,0); + setClockGrid(0, 0); DemodBufferLen = 0; RepaintGraphWindow(); return 0; } // trim graph from the end -int CmdLtrim(const char *Cmd) { +int CmdLtrim(const char *Cmd) +{ // sanitycheck if (GraphTraceLen <= 0) return 1; int ds = atoi(Cmd); for (int i = ds; i < GraphTraceLen; ++i) - GraphBuffer[i-ds] = GraphBuffer[i]; + GraphBuffer[i - ds] = GraphBuffer[i]; GraphTraceLen -= ds; RepaintGraphWindow(); @@ -1650,7 +1713,8 @@ int CmdLtrim(const char *Cmd) { } // trim graph from the beginning -int CmdRtrim(const char *Cmd) { +int CmdRtrim(const char *Cmd) +{ int ds = atoi(Cmd); @@ -1663,7 +1727,8 @@ int CmdRtrim(const char *Cmd) { } // trim graph (middle) piece -int CmdMtrim(const char *Cmd) { +int CmdMtrim(const char *Cmd) +{ int start = 0, stop = 0; sscanf(Cmd, "%i %i", &start, &stop); @@ -1674,12 +1739,13 @@ int CmdMtrim(const char *Cmd) { GraphTraceLen = stop - start; for (int i = 0; i < GraphTraceLen; i++) - GraphBuffer[i] = GraphBuffer[start+i]; + GraphBuffer[i] = GraphBuffer[start + i]; return 0; } -int CmdNorm(const char *Cmd) { +int CmdNorm(const char *Cmd) +{ int i; int max = INT_MIN, min = INT_MAX; @@ -1705,12 +1771,14 @@ int CmdNorm(const char *Cmd) { return 0; } -int CmdPlot(const char *Cmd) { +int CmdPlot(const char *Cmd) +{ ShowGraphWindow(); return 0; } -int CmdSave(const char *Cmd) { +int CmdSave(const char *Cmd) +{ int len = 0; char filename[FILE_PATH_SIZE] = {0x00}; @@ -1720,7 +1788,7 @@ int CmdSave(const char *Cmd) { memcpy(filename, Cmd, len); FILE *f = fopen(filename, "w"); - if(!f) { + if (!f) { PrintAndLogEx(WARNING, "couldn't open '%s'", filename); return 0; } @@ -1735,7 +1803,8 @@ int CmdSave(const char *Cmd) { return 0; } -int CmdScale(const char *Cmd) { +int CmdScale(const char *Cmd) +{ CursorScaleFactor = atoi(Cmd); if (CursorScaleFactor == 0) { PrintAndLogEx(FAILED, "bad, can't have zero scale"); @@ -1745,7 +1814,8 @@ int CmdScale(const char *Cmd) { return 0; } -int directionalThreshold(const int* in, int *out, size_t len, int8_t up, int8_t down) { +int directionalThreshold(const int *in, int *out, size_t len, int8_t up, int8_t down) +{ int lastValue = in[0]; @@ -1755,21 +1825,17 @@ int directionalThreshold(const int* in, int *out, size_t len, int8_t up, int8_t for (size_t i = 1; i < len; ++i) { // Apply first threshold to samples heading up - if (in[i] >= up && in[i] > lastValue) - { + if (in[i] >= up && in[i] > lastValue) { lastValue = out[i]; // Buffer last value as we overwrite it. out[i] = 1; } // Apply second threshold to samples heading down - else if (in[i] <= down && in[i] < lastValue) - { + else if (in[i] <= down && in[i] < lastValue) { lastValue = out[i]; // Buffer last value as we overwrite it. out[i] = -1; - } - else - { + } else { lastValue = out[i]; // Buffer last value as we overwrite it. - out[i] = out[i-1]; + out[i] = out[i - 1]; } } @@ -1778,7 +1844,8 @@ int directionalThreshold(const int* in, int *out, size_t len, int8_t up, int8_t return 0; } -int CmdDirectionalThreshold(const char *Cmd) { +int CmdDirectionalThreshold(const char *Cmd) +{ int8_t up = param_get8(Cmd, 0); int8_t down = param_get8(Cmd, 1); @@ -1796,7 +1863,8 @@ int CmdDirectionalThreshold(const char *Cmd) { return 0; } -int CmdZerocrossings(const char *Cmd) { +int CmdZerocrossings(const char *Cmd) +{ // Zero-crossings aren't meaningful unless the signal is zero-mean. CmdHpf(""); @@ -1832,23 +1900,24 @@ int CmdZerocrossings(const char *Cmd) { * @param Cmd * @return */ -int Cmdbin2hex(const char *Cmd) { +int Cmdbin2hex(const char *Cmd) +{ int bg = 0, en = 0; if (param_getptr(Cmd, &bg, &en, 0)) return usage_data_bin2hex(); //Number of digits supplied as argument size_t length = en - bg + 1; - size_t bytelen = (length+7) / 8; - uint8_t* arr = (uint8_t *) calloc(bytelen, sizeof(uint8_t)); + size_t bytelen = (length + 7) / 8; + uint8_t *arr = (uint8_t *) calloc(bytelen, sizeof(uint8_t)); memset(arr, 0, bytelen); BitstreamOut bout = { arr, 0, 0 }; for (; bg <= en; bg++) { char c = Cmd[bg]; - if( c == '1') + if (c == '1') pushBit(&bout, 1); - else if( c == '0') + else if (c == '0') pushBit(&bout, 0); else PrintAndLogEx(NORMAL, "Ignoring '%c'", c); @@ -1862,11 +1931,12 @@ int Cmdbin2hex(const char *Cmd) { return 0; } -int Cmdhex2bin(const char *Cmd) { +int Cmdhex2bin(const char *Cmd) +{ int bg = 0, en = 0; if (param_getptr(Cmd, &bg, &en, 0)) return usage_data_hex2bin(); - while (bg <= en ) { + while (bg <= en) { char x = Cmd[bg++]; // capitalize if (x >= 'a' && x <= 'f') @@ -1882,36 +1952,37 @@ int Cmdhex2bin(const char *Cmd) { //Uses printf instead of PrintAndLog since the latter // adds linebreaks to each printout - this way was more convenient since we don't have to // allocate a string and write to that first... - for(int i = 0 ; i < 4 ; ++i) - PrintAndLogEx(NORMAL, "%d",(x >> (3 - i)) & 1); + for (int i = 0 ; i < 4 ; ++i) + PrintAndLogEx(NORMAL, "%d", (x >> (3 - i)) & 1); } PrintAndLogEx(NORMAL, "\n"); return 0; } - /* // example of FSK2 RF/50 Tones - static const int LowTone[] = { - 1, 1, 1, 1, 1, -1, -1, -1, -1, -1, - 1, 1, 1, 1, 1, -1, -1, -1, -1, -1, - 1, 1, 1, 1, 1, -1, -1, -1, -1, -1, - 1, 1, 1, 1, 1, -1, -1, -1, -1, -1, - 1, 1, 1, 1, 1, -1, -1, -1, -1, -1 - }; - static const int HighTone[] = { - 1, 1, 1, 1, 1, -1, -1, -1, -1, // note one extra 1 to padd due to 50/8 remainder (1/2 the remainder) - 1, 1, 1, 1, -1, -1, -1, -1, - 1, 1, 1, 1, -1, -1, -1, -1, - 1, 1, 1, 1, -1, -1, -1, -1, - 1, 1, 1, 1, -1, -1, -1, -1, - 1, 1, 1, 1, -1, -1, -1, -1, -1, // note one extra -1 to padd due to 50/8 remainder - }; - */ -void GetHiLoTone(int *LowTone, int *HighTone, int clk, int LowToneFC, int HighToneFC) { - int i,j=0; - int Left_Modifier = ((clk % LowToneFC) % 2) + ((clk % LowToneFC)/2); +/* // example of FSK2 RF/50 Tones +static const int LowTone[] = { +1, 1, 1, 1, 1, -1, -1, -1, -1, -1, +1, 1, 1, 1, 1, -1, -1, -1, -1, -1, +1, 1, 1, 1, 1, -1, -1, -1, -1, -1, +1, 1, 1, 1, 1, -1, -1, -1, -1, -1, +1, 1, 1, 1, 1, -1, -1, -1, -1, -1 +}; +static const int HighTone[] = { +1, 1, 1, 1, 1, -1, -1, -1, -1, // note one extra 1 to padd due to 50/8 remainder (1/2 the remainder) +1, 1, 1, 1, -1, -1, -1, -1, +1, 1, 1, 1, -1, -1, -1, -1, +1, 1, 1, 1, -1, -1, -1, -1, +1, 1, 1, 1, -1, -1, -1, -1, +1, 1, 1, 1, -1, -1, -1, -1, -1, // note one extra -1 to padd due to 50/8 remainder +}; +*/ +void GetHiLoTone(int *LowTone, int *HighTone, int clk, int LowToneFC, int HighToneFC) +{ + int i, j = 0; + int Left_Modifier = ((clk % LowToneFC) % 2) + ((clk % LowToneFC) / 2); int Right_Modifier = (clk % LowToneFC) / 2; //int HighToneMod = clk mod HighToneFC; - int LeftHalfFCCnt = (LowToneFC % 2) + (LowToneFC/2); //truncate + int LeftHalfFCCnt = (LowToneFC % 2) + (LowToneFC / 2); //truncate int FCs_per_clk = clk / LowToneFC; // need to correctly split up the clock to field clocks. @@ -1927,21 +1998,21 @@ void GetHiLoTone(int *LowTone, int *HighTone, int clk, int LowToneFC, int HighTo for (i = 0; i < (FCs_per_clk); i++) { // loop # of samples per field clock for (j = 0; j < LowToneFC; j++) { - LowTone[ (i * LowToneFC) + Left_Modifier + j] = ( j < LeftHalfFCCnt ) ? 1 : -1; + LowTone[(i * LowToneFC) + Left_Modifier + j] = (j < LeftHalfFCCnt) ? 1 : -1; } } int k; // add last -1 modifiers for (k = 0; k < Right_Modifier; k++) { - LowTone[ ( (i-1) * LowToneFC) + Left_Modifier + j + k] = -1; + LowTone[((i - 1) * LowToneFC) + Left_Modifier + j + k] = -1; } // now do hightone - Left_Modifier = ((clk % HighToneFC) % 2) + ((clk % HighToneFC)/2); + Left_Modifier = ((clk % HighToneFC) % 2) + ((clk % HighToneFC) / 2); Right_Modifier = (clk % HighToneFC) / 2; - LeftHalfFCCnt = (HighToneFC % 2) + (HighToneFC/2); //truncate - FCs_per_clk = clk/HighToneFC; + LeftHalfFCCnt = (HighToneFC % 2) + (HighToneFC / 2); //truncate + FCs_per_clk = clk / HighToneFC; for (i = 0; i < Left_Modifier; i++) { HighTone[i] = 1; @@ -1951,17 +2022,17 @@ void GetHiLoTone(int *LowTone, int *HighTone, int clk, int LowToneFC, int HighTo for (i = 0; i < (FCs_per_clk); i++) { // loop # of samples per field clock for (j = 0; j < HighToneFC; j++) { - HighTone[(i * HighToneFC) + Left_Modifier + j] = ( j < LeftHalfFCCnt ) ? 1 : -1; + HighTone[(i * HighToneFC) + Left_Modifier + j] = (j < LeftHalfFCCnt) ? 1 : -1; } } // add last -1 modifiers for (k = 0; k < Right_Modifier; k++) { - PrintAndLogEx(NORMAL, "(i-1)*HighToneFC+lm+j+k %i", ((i-1) * HighToneFC) + Left_Modifier + j + k); - HighTone[ ( (i-1) * HighToneFC) + Left_Modifier + j + k] = -1; + PrintAndLogEx(NORMAL, "(i-1)*HighToneFC+lm+j+k %i", ((i - 1) * HighToneFC) + Left_Modifier + j + k); + HighTone[((i - 1) * HighToneFC) + Left_Modifier + j + k] = -1; } if (g_debugMode == 2) { - for ( i = 0; i < clk; i++) { + for (i = 0; i < clk; i++) { PrintAndLogEx(NORMAL, "Low: %i, High: %i", LowTone[i], HighTone[i]); } } @@ -1969,19 +2040,20 @@ void GetHiLoTone(int *LowTone, int *HighTone, int clk, int LowToneFC, int HighTo //old CmdFSKdemod adapted by marshmellow //converts FSK to clear NRZ style wave. (or demodulates) -int FSKToNRZ(int *data, int *dataLen, int clk, int LowToneFC, int HighToneFC) { +int FSKToNRZ(int *data, int *dataLen, int clk, int LowToneFC, int HighToneFC) +{ uint8_t ans = 0; if (clk == 0 || LowToneFC == 0 || HighToneFC == 0) { - int firstClockEdge=0; + int firstClockEdge = 0; ans = fskClocks((uint8_t *) &LowToneFC, (uint8_t *) &HighToneFC, (uint8_t *) &clk, &firstClockEdge); if (g_debugMode > 1) { - PrintAndLog ("DEBUG FSKtoNRZ: detected clocks: fc_low %i, fc_high %i, clk %i, firstClockEdge %i, ans %u", LowToneFC, HighToneFC, clk, firstClockEdge, ans); + PrintAndLog("DEBUG FSKtoNRZ: detected clocks: fc_low %i, fc_high %i, clk %i, firstClockEdge %i, ans %u", LowToneFC, HighToneFC, clk, firstClockEdge, ans); } } // currently only know fsk modulations with field clocks < 10 samples and > 4 samples. filter out to remove false positives (and possibly destroying ask/psk modulated waves...) if (ans == 0 || clk == 0 || LowToneFC == 0 || HighToneFC == 0 || LowToneFC > 10 || HighToneFC < 4) { if (g_debugMode > 1) { - PrintAndLog ("DEBUG FSKtoNRZ: no fsk clocks found"); + PrintAndLog("DEBUG FSKtoNRZ: no fsk clocks found"); } return 0; } @@ -2010,15 +2082,15 @@ int FSKToNRZ(int *data, int *dataLen, int clk, int LowToneFC, int HighToneFC) { // now we have the abs( [average sample value per clk] * 100 ) for each tone // loop through again [all samples] - clk - 16 // note why 16??? is 16 the largest FC? changed to LowToneFC as that should be the > fc - for(i = 0; i < *dataLen - clk - LowToneFC; ++i) { + for (i = 0; i < *dataLen - clk - LowToneFC; ++i) { int lowTot = 0, highTot = 0; // sum a field clock width of abs( [average sample values per clk] * 100) for each tone for (j = 0; j < LowToneFC; ++j) { //10 for fsk2 - lowTot += (data[i + j] & 0xffff); + lowTot += (data[i + j] & 0xffff); } for (j = 0; j < HighToneFC; j++) { //8 for fsk2 - highTot += (data[i + j] >> 16); + highTot += (data[i + j] >> 16); } // subtract the sum of lowTone averages by the sum of highTone averages as it @@ -2030,7 +2102,8 @@ int FSKToNRZ(int *data, int *dataLen, int clk, int LowToneFC, int HighToneFC) { return 0; } -int CmdFSKToNRZ(const char *Cmd) { +int CmdFSKToNRZ(const char *Cmd) +{ // take clk, fc_low, fc_high // blank = auto; bool errors = false; @@ -2038,26 +2111,26 @@ int CmdFSKToNRZ(const char *Cmd) { int clk = 0, fc_low = 10, fc_high = 8; while (param_getchar(Cmd, cmdp) != 0x00) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': - return usage_data_fsktonrz(); - case 'c': - clk = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - case 'f': - fc_high = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - case 'l': - fc_low = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; + case 'h': + return usage_data_fsktonrz(); + case 'c': + clk = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + case 'f': + fc_high = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + case 'l': + fc_low = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); + errors = true; + break; } - if(errors) break; + if (errors) break; } //Validations if (errors) return usage_data_fsktonrz(); @@ -2070,7 +2143,8 @@ int CmdFSKToNRZ(const char *Cmd) { return ans; } -int CmdDataIIR(const char *Cmd){ +int CmdDataIIR(const char *Cmd) +{ uint8_t k = param_get8(Cmd, 0); //iceIIR_Butterworth(GraphBuffer, GraphTraceLen); iceSimple_Filter(GraphBuffer, GraphTraceLen, k); @@ -2087,7 +2161,7 @@ static command_t CommandTable[] = { {"help", CmdHelp, 1, "This help"}, {"askedgedetect", CmdAskEdgeDetect, 1, "[threshold] Adjust Graph for manual ASK demod using the length of sample differences to detect the edge of a wave (use 20-45, def:25)"}, {"autocorr", CmdAutoCorr, 1, "[window length] [g] -- Autocorrelation over window - g to save back to GraphBuffer (overwrite)"}, - {"biphaserawdecode",CmdBiphaseDecodeRaw,1, "[offset] [invert<0|1>] [maxErr] -- Biphase decode bin stream in DemodBuffer (offset = 0|1 bits to shift the decode start)"}, + {"biphaserawdecode", CmdBiphaseDecodeRaw, 1, "[offset] [invert<0|1>] [maxErr] -- Biphase decode bin stream in DemodBuffer (offset = 0|1 bits to shift the decode start)"}, {"bin2hex", Cmdbin2hex, 1, " -- Converts binary to hexadecimal"}, {"bitsamples", CmdBitsamples, 0, "Get raw samples as bitstring"}, {"buffclear", CmdBuffClear, 1, "Clears bigbuff on deviceside and graph window"}, @@ -2108,7 +2182,7 @@ static command_t CommandTable[] = { {"manrawdecode", Cmdmandecoderaw, 1, "[invert] [maxErr] -- Manchester decode binary stream in DemodBuffer"}, {"norm", CmdNorm, 1, "Normalize max/min to +/-128"}, {"plot", CmdPlot, 1, "Show graph window (hit 'h' in window for keystroke help)"}, - {"printdemodbuffer",CmdPrintDemodBuff, 1, "[x] [o] [l] -- print the data in the DemodBuffer - 'x' for hex output"}, + {"printdemodbuffer", CmdPrintDemodBuff, 1, "[x] [o] [l] -- print the data in the DemodBuffer - 'x' for hex output"}, {"rawdemod", CmdRawDemod, 1, "[modulation] ... -see help (h option) -- Demodulate the data in the GraphBuffer and output binary"}, {"samples", CmdSamples, 0, "[512 - 40000] -- Get raw samples for graph window (GraphBuffer)"}, {"save", CmdSave, 1, " -- Save trace (from graph window)"}, @@ -2124,13 +2198,15 @@ static command_t CommandTable[] = { {NULL, NULL, 0, NULL} }; -int CmdData(const char *Cmd) { +int CmdData(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { +int CmdHelp(const char *Cmd) +{ CmdsHelp(CommandTable); return 0; } diff --git a/client/cmddata.h b/client/cmddata.h index 605bc3298..86f19d81d 100644 --- a/client/cmddata.h +++ b/client/cmddata.h @@ -32,7 +32,7 @@ #include "loclass/cipherutils.h" // for decimating samples in getsamples #include "cmdlfem4x.h" // askem410xdecode -command_t * CmdDataCommands(); +command_t *CmdDataCommands(); int CmdData(const char *Cmd); void printDemodBuff(void); @@ -83,7 +83,7 @@ int PSKDemod(const char *Cmd, bool verbose); int NRZrawDemod(const char *Cmd, bool verbose); int getSamples(int n, bool silent); void setClockGrid(int clk, int offset); -int directionalThreshold(const int* in, int *out, size_t len, int8_t up, int8_t down); +int directionalThreshold(const int *in, int *out, size_t len, int8_t up, int8_t down); extern int AskEdgeDetect(const int *in, int *out, int len, int threshold); int CmdDataIIR(const char *Cmd); diff --git a/client/cmdflashmem.c b/client/cmdflashmem.c index f6a6e8518..68faf826e 100644 --- a/client/cmdflashmem.c +++ b/client/cmdflashmem.c @@ -26,7 +26,8 @@ static int CmdHelp(const char *Cmd); -int usage_flashmem_spibaud(void){ +int usage_flashmem_spibaud(void) +{ PrintAndLogEx(NORMAL, "Usage: mem spibaud [h] "); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " h this help"); @@ -40,7 +41,8 @@ int usage_flashmem_spibaud(void){ return 0; } -int usage_flashmem_read(void){ +int usage_flashmem_read(void) +{ PrintAndLogEx(NORMAL, "Read flash memory on device"); PrintAndLogEx(NORMAL, "Usage: mem read o l "); PrintAndLogEx(NORMAL, " o : offset in memory"); @@ -51,7 +53,8 @@ int usage_flashmem_read(void){ PrintAndLogEx(NORMAL, " mem read o 1024 l 10"); // read 10 bytes starting at offset 1024 return 0; } -int usage_flashmem_load(void){ +int usage_flashmem_load(void) +{ PrintAndLogEx(NORMAL, "Loads binary file into flash memory on device"); PrintAndLogEx(NORMAL, "Usage: mem load o f m t i"); PrintAndLogEx(NORMAL, " o : offset in memory"); @@ -68,7 +71,8 @@ int usage_flashmem_load(void){ PrintAndLogEx(NORMAL, " mem load f default_iclass_keys i"); return 0; } -int usage_flashmem_save(void){ +int usage_flashmem_save(void) +{ PrintAndLogEx(NORMAL, "Saves flash memory on device into the file"); PrintAndLogEx(NORMAL, " Usage: mem save o l f "); PrintAndLogEx(NORMAL, " o : offset in memory"); @@ -81,7 +85,8 @@ int usage_flashmem_save(void){ PrintAndLogEx(NORMAL, " mem save f myfile o 1024 l 4096"); // downlowd 4096 bytes from offset 1024 to file myfile return 0; } -int usage_flashmem_wipe(void){ +int usage_flashmem_wipe(void) +{ PrintAndLogEx(WARNING, "[OBS] use with caution."); PrintAndLogEx(NORMAL, "Wipe flash memory on device, which fills memory with 0xFF\n"); @@ -95,7 +100,8 @@ int usage_flashmem_wipe(void){ PrintAndLogEx(NORMAL, " mem wipe p 0"); // wipes first page. return 0; } -int usage_flashmem_info(void){ +int usage_flashmem_info(void) +{ PrintAndLogEx(NORMAL, "Collect signature and verify it from flash memory\n"); PrintAndLogEx(NORMAL, " Usage: mem info [h|s|w]"); PrintAndLogEx(NORMAL, " s : create a signature"); @@ -107,7 +113,8 @@ int usage_flashmem_info(void){ return 0; } -int CmdFlashMemRead(const char *Cmd) { +int CmdFlashMemRead(const char *Cmd) +{ uint8_t cmdp = 0; bool errors = false; @@ -115,25 +122,25 @@ int CmdFlashMemRead(const char *Cmd) { while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'o': - start_index = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - case 'l': - len = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - case 'h': - return usage_flashmem_read(); - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; + case 'o': + start_index = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + case 'l': + len = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + case 'h': + return usage_flashmem_read(); + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); + errors = true; + break; } } //Validations - if (errors || cmdp == 0 ) return usage_flashmem_read(); + if (errors || cmdp == 0) return usage_flashmem_read(); if (start_index + len > FLASH_MEM_MAX_SIZE) { PrintAndLogDevice(WARNING, "error, start_index + length is larger than available memory"); @@ -146,19 +153,21 @@ int CmdFlashMemRead(const char *Cmd) { return 0; } -int CmdFlashmemSpiBaudrate(const char *Cmd) { +int CmdFlashmemSpiBaudrate(const char *Cmd) +{ char ctmp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) < 1 || ctmp == 'h') return usage_flashmem_spibaud(); uint32_t baudrate = param_get32ex(Cmd, 0, 0, 10); baudrate = baudrate * 1000000; - if (baudrate != FLASH_BAUD && baudrate != FLASH_MINBAUD ) return usage_flashmem_spibaud(); + if (baudrate != FLASH_BAUD && baudrate != FLASH_MINBAUD) return usage_flashmem_spibaud(); UsbCommand c = {CMD_FLASHMEM_SET_SPIBAUDRATE, {baudrate, 0, 0}}; SendCommand(&c); return 0; } -int CmdFlashMemLoad(const char *Cmd){ +int CmdFlashMemLoad(const char *Cmd) +{ uint32_t start_index = 0; char filename[FILE_PATH_SIZE] = {0}; @@ -168,41 +177,41 @@ int CmdFlashMemLoad(const char *Cmd){ while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': - return usage_flashmem_load(); - case 'f': - if ( param_getstr(Cmd, cmdp+1, filename, FILE_PATH_SIZE) >= FILE_PATH_SIZE ) { - PrintAndLogEx(FAILED, "Filename too long"); + case 'h': + return usage_flashmem_load(); + case 'f': + if (param_getstr(Cmd, cmdp + 1, filename, FILE_PATH_SIZE) >= FILE_PATH_SIZE) { + PrintAndLogEx(FAILED, "Filename too long"); + errors = true; + break; + } + cmdp += 2; + break; + case 'o': + start_index = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + case 'm': + d = DICTIONARY_MIFARE; + cmdp++; + break; + case 't': + d = DICTIONARY_T55XX; + cmdp++; + break; + case 'i': + d = DICTIONARY_ICLASS; + cmdp++; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); errors = true; break; - } - cmdp += 2; - break; - case 'o': - start_index = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - case 'm': - d = DICTIONARY_MIFARE; - cmdp++; - break; - case 't': - d = DICTIONARY_T55XX; - cmdp++; - break; - case 'i': - d = DICTIONARY_ICLASS; - cmdp++; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; } } //Validations - if (errors || cmdp == 0 ) return usage_flashmem_load(); + if (errors || cmdp == 0) return usage_flashmem_load(); size_t datalen = 0; uint16_t keycount = 0; @@ -212,8 +221,8 @@ int CmdFlashMemLoad(const char *Cmd){ switch (d) { case DICTIONARY_MIFARE: start_index = DEFAULT_MF_KEYS_OFFSET; - res = loadFileDICTIONARY(filename, "dic", data+2, &datalen, 6, &keycount ); - if ( res || !keycount) { + res = loadFileDICTIONARY(filename, "dic", data + 2, &datalen, 6, &keycount); + if (res || !keycount) { free(data); return 1; } @@ -223,8 +232,8 @@ int CmdFlashMemLoad(const char *Cmd){ break; case DICTIONARY_T55XX: start_index = DEFAULT_T55XX_KEYS_OFFSET; - res = loadFileDICTIONARY(filename, "dic", data+2, &datalen, 4, &keycount ); - if ( res || !keycount) { + res = loadFileDICTIONARY(filename, "dic", data + 2, &datalen, 4, &keycount); + if (res || !keycount) { free(data); return 1; } @@ -234,8 +243,8 @@ int CmdFlashMemLoad(const char *Cmd){ break; case DICTIONARY_ICLASS: start_index = DEFAULT_ICLASS_KEYS_OFFSET; - res = loadFileDICTIONARY(filename, "dic", data+2, &datalen, 8, &keycount ); - if ( res || !keycount) { + res = loadFileDICTIONARY(filename, "dic", data + 2, &datalen, 8, &keycount); + if (res || !keycount) { free(data); return 1; } @@ -247,7 +256,7 @@ int CmdFlashMemLoad(const char *Cmd){ res = loadFile(filename, "bin", data, &datalen); //int res = loadFileEML( filename, "eml", data, &datalen); - if ( res ) { + if (res) { free(data); return 1; } @@ -266,7 +275,7 @@ int CmdFlashMemLoad(const char *Cmd){ uint32_t bytes_sent = 0; uint32_t bytes_remaining = datalen; - while (bytes_remaining > 0){ + while (bytes_remaining > 0) { uint32_t bytes_in_packet = MIN(FLASH_MEM_BLOCK_SIZE, bytes_remaining); UsbCommand c = {CMD_FLASHMEM_WRITE, {start_index + bytes_sent, bytes_in_packet, 0}}; @@ -279,7 +288,7 @@ int CmdFlashMemLoad(const char *Cmd){ bytes_sent += bytes_in_packet; UsbCommand resp; - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 2000) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { PrintAndLogEx(WARNING, "timeout while waiting for reply."); free(data); return 1; @@ -295,7 +304,8 @@ int CmdFlashMemLoad(const char *Cmd){ PrintAndLogEx(SUCCESS, "Wrote %u bytes to offset %u", datalen, start_index); return 0; } -int CmdFlashMemSave(const char *Cmd){ +int CmdFlashMemSave(const char *Cmd) +{ char filename[FILE_PATH_SIZE] = {0}; uint8_t cmdp = 0; @@ -304,42 +314,43 @@ int CmdFlashMemSave(const char *Cmd){ while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': return usage_flashmem_save(); - case 'l': - len = param_get32ex(Cmd, cmdp+1, FLASH_MEM_MAX_SIZE, 10); - cmdp += 2; - break; - case 'o': - start_index = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - case 'f': - //File handling - if ( param_getstr(Cmd, cmdp+1, filename, FILE_PATH_SIZE) >= FILE_PATH_SIZE ) { - PrintAndLogEx(FAILED, "Filename too long"); + case 'h': + return usage_flashmem_save(); + case 'l': + len = param_get32ex(Cmd, cmdp + 1, FLASH_MEM_MAX_SIZE, 10); + cmdp += 2; + break; + case 'o': + start_index = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + case 'f': + //File handling + if (param_getstr(Cmd, cmdp + 1, filename, FILE_PATH_SIZE) >= FILE_PATH_SIZE) { + PrintAndLogEx(FAILED, "Filename too long"); + errors = true; + break; + } + cmdp += 2; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); errors = true; break; - } - cmdp += 2; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; } } //Validations - if (errors || cmdp == 0 ) return usage_flashmem_save(); + if (errors || cmdp == 0) return usage_flashmem_save(); - uint8_t* dump = calloc(len, sizeof(uint8_t)); + uint8_t *dump = calloc(len, sizeof(uint8_t)); if (!dump) { PrintAndLogDevice(WARNING, "error, cannot allocate memory "); return 1; } PrintAndLogEx(NORMAL, "downloading %u bytes from flashmem", len); - if ( !GetFromDevice(FLASH_MEM, dump, len, start_index, NULL, -1, true) ) { + if (!GetFromDevice(FLASH_MEM, dump, len, start_index, NULL, -1, true)) { PrintAndLogEx(FAILED, "ERROR; downloading flashmem"); free(dump); return 1; @@ -350,7 +361,8 @@ int CmdFlashMemSave(const char *Cmd){ free(dump); return 0; } -int CmdFlashMemWipe(const char *Cmd){ +int CmdFlashMemWipe(const char *Cmd) +{ uint8_t cmdp = 0; bool errors = false; @@ -358,35 +370,36 @@ int CmdFlashMemWipe(const char *Cmd){ uint8_t page = 0; while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': return usage_flashmem_wipe(); - case 'p': - page = param_get8ex(Cmd, cmdp+1, 0, 10); - if ( page > 2 ) { - PrintAndLogEx(WARNING, "page must be 0, 1 or 2"); + case 'h': + return usage_flashmem_wipe(); + case 'p': + page = param_get8ex(Cmd, cmdp + 1, 0, 10); + if (page > 2) { + PrintAndLogEx(WARNING, "page must be 0, 1 or 2"); + errors = true; + break; + } + cmdp += 2; + break; + case 'i': + initalwipe = true; + cmdp++; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); errors = true; break; - } - cmdp += 2; - break; - case 'i': - initalwipe = true; - cmdp++; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; } } //Validations - if (errors || cmdp == 0 ) return usage_flashmem_wipe(); + if (errors || cmdp == 0) return usage_flashmem_wipe(); UsbCommand c = {CMD_FLASHMEM_WIPE, {page, initalwipe, 0}}; clearCommandBuffer(); SendCommand(&c); UsbCommand resp; - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 8000) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 8000)) { PrintAndLogEx(WARNING, "timeout while waiting for reply."); return 1; } @@ -398,7 +411,8 @@ int CmdFlashMemWipe(const char *Cmd){ return 0; } -int CmdFlashMemInfo(const char *Cmd){ +int CmdFlashMemInfo(const char *Cmd) +{ uint8_t sha_hash[20] = {0}; mbedtls_rsa_context rsa; @@ -407,31 +421,32 @@ int CmdFlashMemInfo(const char *Cmd){ bool errors = false, shall_write = false, shall_sign = false; while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': return usage_flashmem_info(); - case 's': { - shall_sign = true; - cmdp++; - break; - } - case 'w': - shall_write = true; - cmdp++; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; + case 'h': + return usage_flashmem_info(); + case 's': { + shall_sign = true; + cmdp++; + break; + } + case 'w': + shall_write = true; + cmdp++; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); + errors = true; + break; } } //Validations - if (errors ) return usage_flashmem_info(); + if (errors) return usage_flashmem_info(); UsbCommand c = {CMD_FLASHMEM_INFO, {0, 0, 0}}; clearCommandBuffer(); SendCommand(&c); UsbCommand resp; - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 2500) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 2500)) { PrintAndLogEx(WARNING, "timeout while waiting for reply."); return 1; } @@ -447,15 +462,15 @@ int CmdFlashMemInfo(const char *Cmd){ memcpy(&mem, (rdv40_validation_t *)resp.d.asBytes, sizeof(rdv40_validation_t)); // Flash ID hash (sha1) - mbedtls_sha1( mem.flashid, sizeof(mem.flashid), sha_hash ); + mbedtls_sha1(mem.flashid, sizeof(mem.flashid), sha_hash); // print header PrintAndLogEx(INFO, "\n--- Flash memory Information ---------"); PrintAndLogEx(INFO, "-------------------------------------------------------------"); - PrintAndLogEx(INFO, "ID | %s", sprint_hex(mem.flashid, sizeof(mem.flashid) )); + PrintAndLogEx(INFO, "ID | %s", sprint_hex(mem.flashid, sizeof(mem.flashid))); PrintAndLogEx(INFO, "SHA1 | %s", sprint_hex(sha_hash, sizeof(sha_hash))); PrintAndLogEx(INFO, "RSA SIGNATURE |"); - print_hex_break( mem.signature, sizeof(mem.signature), 32); + print_hex_break(mem.signature, sizeof(mem.signature), 32); //------------------------------------------------------------------------------- // Example RSA-1024 keypair, for test purposes (from common/polarssl/rsa.c) @@ -463,53 +478,53 @@ int CmdFlashMemInfo(const char *Cmd){ // public key modulus N #define RSA_N "9292758453063D803DD603D5E777D788" \ - "8ED1D5BF35786190FA2F23EBC0848AEA" \ - "DDA92CA6C3D80B32C4D109BE0F36D6AE" \ - "7130B9CED7ACDF54CFC7555AC14EEBAB" \ - "93A89813FBF3C4F8066D2D800F7C38A8" \ - "1AE31942917403FF4946B0A83D3D3E05" \ - "EE57C6F5F5606FB5D4BC6CD34EE0801A" \ - "5E94BB77B07507233A0BC7BAC8F90F79" + "8ED1D5BF35786190FA2F23EBC0848AEA" \ + "DDA92CA6C3D80B32C4D109BE0F36D6AE" \ + "7130B9CED7ACDF54CFC7555AC14EEBAB" \ + "93A89813FBF3C4F8066D2D800F7C38A8" \ + "1AE31942917403FF4946B0A83D3D3E05" \ + "EE57C6F5F5606FB5D4BC6CD34EE0801A" \ + "5E94BB77B07507233A0BC7BAC8F90F79" // public key Exponent E #define RSA_E "10001" // private key Exponent D #define RSA_D "24BF6185468786FDD303083D25E64EFC" \ - "66CA472BC44D253102F8B4A9D3BFA750" \ - "91386C0077937FE33FA3252D28855837" \ - "AE1B484A8A9A45F7EE8C0C634F99E8CD" \ - "DF79C5CE07EE72C7F123142198164234" \ - "CABB724CF78B8173B9F880FC86322407" \ - "AF1FEDFDDE2BEB674CA15F3E81A1521E" \ - "071513A1E85B5DFA031F21ECAE91A34D" + "66CA472BC44D253102F8B4A9D3BFA750" \ + "91386C0077937FE33FA3252D28855837" \ + "AE1B484A8A9A45F7EE8C0C634F99E8CD" \ + "DF79C5CE07EE72C7F123142198164234" \ + "CABB724CF78B8173B9F880FC86322407" \ + "AF1FEDFDDE2BEB674CA15F3E81A1521E" \ + "071513A1E85B5DFA031F21ECAE91A34D" // prime P #define RSA_P "C36D0EB7FCD285223CFB5AABA5BDA3D8" \ - "2C01CAD19EA484A87EA4377637E75500" \ - "FCB2005C5C7DD6EC4AC023CDA285D796" \ - "C3D9E75E1EFC42488BB4F1D13AC30A57" + "2C01CAD19EA484A87EA4377637E75500" \ + "FCB2005C5C7DD6EC4AC023CDA285D796" \ + "C3D9E75E1EFC42488BB4F1D13AC30A57" // prime Q #define RSA_Q "C000DF51A7C77AE8D7C7370C1FF55B69" \ - "E211C2B9E5DB1ED0BF61D0D9899620F4" \ - "910E4168387E3C30AA1E00C339A79508" \ - "8452DD96A9A5EA5D9DCA68DA636032AF" + "E211C2B9E5DB1ED0BF61D0D9899620F4" \ + "910E4168387E3C30AA1E00C339A79508" \ + "8452DD96A9A5EA5D9DCA68DA636032AF" #define RSA_DP "C1ACF567564274FB07A0BBAD5D26E298" \ - "3C94D22288ACD763FD8E5600ED4A702D" \ - "F84198A5F06C2E72236AE490C93F07F8" \ - "3CC559CD27BC2D1CA488811730BB5725" + "3C94D22288ACD763FD8E5600ED4A702D" \ + "F84198A5F06C2E72236AE490C93F07F8" \ + "3CC559CD27BC2D1CA488811730BB5725" #define RSA_DQ "4959CBF6F8FEF750AEE6977C155579C7" \ - "D8AAEA56749EA28623272E4F7D0592AF" \ - "7C1F1313CAC9471B5C523BFE592F517B" \ - "407A1BD76C164B93DA2D32A383E58357" + "D8AAEA56749EA28623272E4F7D0592AF" \ + "7C1F1313CAC9471B5C523BFE592F517B" \ + "407A1BD76C164B93DA2D32A383E58357" #define RSA_QP "9AE7FBC99546432DF71896FC239EADAE" \ - "F38D18D2B2F0E2DD275AA977E2BF4411" \ - "F5A3B2A5D33605AEBBCCBA7FEB9F2D2F" \ - "A74206CEC169D74BF5A8C50D6F48EA08" + "F38D18D2B2F0E2DD275AA977E2BF4411" \ + "F5A3B2A5D33605AEBBCCBA7FEB9F2D2F" \ + "A74206CEC169D74BF5A8C50D6F48EA08" #define KEY_LEN 128 @@ -518,18 +533,18 @@ int CmdFlashMemInfo(const char *Cmd){ rsa.len = KEY_LEN; - mbedtls_mpi_read_string( &rsa.N , 16, RSA_N ); - mbedtls_mpi_read_string( &rsa.E , 16, RSA_E ); - mbedtls_mpi_read_string( &rsa.D , 16, RSA_D ); - mbedtls_mpi_read_string( &rsa.P , 16, RSA_P ); - mbedtls_mpi_read_string( &rsa.Q , 16, RSA_Q ); - mbedtls_mpi_read_string( &rsa.DP, 16, RSA_DP ); - mbedtls_mpi_read_string( &rsa.DQ, 16, RSA_DQ ); - mbedtls_mpi_read_string( &rsa.QP, 16, RSA_QP ); + mbedtls_mpi_read_string(&rsa.N, 16, RSA_N); + mbedtls_mpi_read_string(&rsa.E, 16, RSA_E); + mbedtls_mpi_read_string(&rsa.D, 16, RSA_D); + mbedtls_mpi_read_string(&rsa.P, 16, RSA_P); + mbedtls_mpi_read_string(&rsa.Q, 16, RSA_Q); + mbedtls_mpi_read_string(&rsa.DP, 16, RSA_DP); + mbedtls_mpi_read_string(&rsa.DQ, 16, RSA_DQ); + mbedtls_mpi_read_string(&rsa.QP, 16, RSA_QP); PrintAndLogEx(INFO, "KEY length | %d", KEY_LEN); - bool is_keyok = ( mbedtls_rsa_check_pubkey( &rsa ) == 0 || mbedtls_rsa_check_privkey( &rsa ) == 0 ); + bool is_keyok = (mbedtls_rsa_check_pubkey(&rsa) == 0 || mbedtls_rsa_check_privkey(&rsa) == 0); if (is_keyok) PrintAndLogEx(SUCCESS, "RSA key validation ok"); else @@ -548,7 +563,7 @@ int CmdFlashMemInfo(const char *Cmd){ // Signing (private key) if (shall_sign) { - int is_signed = mbedtls_rsa_pkcs1_sign( &rsa, NULL, NULL, MBEDTLS_RSA_PRIVATE, MBEDTLS_MD_SHA1, 20, sha_hash, sign ); + int is_signed = mbedtls_rsa_pkcs1_sign(&rsa, NULL, NULL, MBEDTLS_RSA_PRIVATE, MBEDTLS_MD_SHA1, 20, sha_hash, sign); if (is_signed == 0) PrintAndLogEx(SUCCESS, "RSA Signing ok"); else @@ -556,11 +571,11 @@ int CmdFlashMemInfo(const char *Cmd){ if (shall_write) { // save to mem - c = (UsbCommand){CMD_FLASHMEM_WRITE, {FLASH_MEM_SIGNATURE_OFFSET, FLASH_MEM_SIGNATURE_LEN, 0}}; + c = (UsbCommand) {CMD_FLASHMEM_WRITE, {FLASH_MEM_SIGNATURE_OFFSET, FLASH_MEM_SIGNATURE_LEN, 0}}; memcpy(c.d.asBytes, sign, sizeof(sign)); clearCommandBuffer(); SendCommand(&c); - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 2000) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { PrintAndLogEx(WARNING, "timeout while waiting for reply."); } else { @@ -572,11 +587,11 @@ int CmdFlashMemInfo(const char *Cmd){ } } PrintAndLogEx(INFO, "Signed | "); - print_hex_break( sign, sizeof(sign), 32); + print_hex_break(sign, sizeof(sign), 32); } // Verify (public key) - int is_verified = mbedtls_rsa_pkcs1_verify( &rsa, NULL, NULL, MBEDTLS_RSA_PUBLIC, MBEDTLS_MD_SHA1, 20, sha_hash, from_device ); + int is_verified = mbedtls_rsa_pkcs1_verify(&rsa, NULL, NULL, MBEDTLS_RSA_PUBLIC, MBEDTLS_MD_SHA1, 20, sha_hash, from_device); if (is_verified == 0) PrintAndLogEx(SUCCESS, "RSA Verification ok"); else @@ -597,13 +612,15 @@ static command_t CommandTable[] = { {NULL, NULL, 0, NULL} }; -int CmdFlashMem(const char *Cmd) { +int CmdFlashMem(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { +int CmdHelp(const char *Cmd) +{ CmdsHelp(CommandTable); return 0; } diff --git a/client/cmdflashmem.h b/client/cmdflashmem.h index c7c6f8979..0a56ebed4 100644 --- a/client/cmdflashmem.h +++ b/client/cmdflashmem.h @@ -35,9 +35,9 @@ typedef enum { extern int CmdFlashMem(const char *Cmd); -extern int CmdFlashMemRead(const char* cmd); -extern int CmdFlashMemLoad(const char* cmd); -extern int CmdFlashMemSave(const char* cmd); +extern int CmdFlashMemRead(const char *cmd); +extern int CmdFlashMemLoad(const char *cmd); +extern int CmdFlashMemSave(const char *cmd); extern int CmdFlashMemWipe(const char *Cmd); extern int CmdFlashMemInfo(const char *Cmd); #endif diff --git a/client/cmdhf.c b/client/cmdhf.c index 475a2659d..a076d4e6d 100644 --- a/client/cmdhf.c +++ b/client/cmdhf.c @@ -12,7 +12,8 @@ static int CmdHelp(const char *Cmd); -int usage_hf_search(){ +int usage_hf_search() +{ PrintAndLogEx(NORMAL, "Usage: hf search"); PrintAndLogEx(NORMAL, "Will try to find a HF read out of the unknown tag. Stops when found."); PrintAndLogEx(NORMAL, "Options:"); @@ -20,7 +21,8 @@ int usage_hf_search(){ PrintAndLogEx(NORMAL, ""); return 0; } -int usage_hf_snoop(){ +int usage_hf_snoop() +{ PrintAndLogEx(NORMAL, "Usage: hf snoop "); PrintAndLogEx(NORMAL, "The high frequence snoop will assign all available memory on device for snooped data"); PrintAndLogEx(NORMAL, "User the 'data samples' command to download from device, and 'data plot' to look at it"); @@ -36,7 +38,8 @@ int usage_hf_snoop(){ return 0; } -int CmdHFSearch(const char *Cmd){ +int CmdHFSearch(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_hf_search(); @@ -52,7 +55,7 @@ int CmdHFSearch(const char *Cmd){ return ans; } ans = HFLegicReader("", false); - if ( ans == 0) { + if (ans == 0) { PrintAndLogEx(SUCCESS, "\nValid LEGIC Tag Found\n"); return 1; } @@ -85,7 +88,8 @@ int CmdHFSearch(const char *Cmd){ return 0; } -int CmdHFTune(const char *Cmd) { +int CmdHFTune(const char *Cmd) +{ PrintAndLogEx(SUCCESS, "Measuring HF antenna, press button to exit"); UsbCommand c = {CMD_MEASURE_ANTENNA_TUNING_HF}; clearCommandBuffer(); @@ -93,7 +97,8 @@ int CmdHFTune(const char *Cmd) { return 0; } -int CmdHFSnoop(const char *Cmd) { +int CmdHFSnoop(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_hf_snoop(); @@ -128,13 +133,15 @@ static command_t CommandTable[] = { {NULL, NULL, 0, NULL} }; -int CmdHF(const char *Cmd) { +int CmdHF(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { +int CmdHelp(const char *Cmd) +{ CmdsHelp(CommandTable); return 0; } diff --git a/client/cmdhf14a.c b/client/cmdhf14a.c index d88746e3f..009ca4f8c 100644 --- a/client/cmdhf14a.c +++ b/client/cmdhf14a.c @@ -136,24 +136,26 @@ static const manufactureName manufactureMapping[] = { // get a product description based on the UID // uid[8] tag uid // returns description of the best match -char* getTagInfo(uint8_t uid) { +char *getTagInfo(uint8_t uid) +{ int i; int len = sizeof(manufactureMapping) / sizeof(manufactureName); - for ( i = 0; i < len; ++i ) - if ( uid == manufactureMapping[i].uid) + for (i = 0; i < len; ++i) + if (uid == manufactureMapping[i].uid) return manufactureMapping[i].desc; //No match, return default - return manufactureMapping[len-1].desc; + return manufactureMapping[len - 1].desc; } // iso14a apdu input frame length static uint16_t frameLength = 0; uint16_t atsFSC[] = {16, 24, 32, 40, 48, 64, 96, 128, 256}; -int usage_hf_14a_sim(void) { +int usage_hf_14a_sim(void) +{ // PrintAndLogEx(NORMAL, "\n Emulating ISO/IEC 14443 type A tag with 4,7 or 10 byte UID\n"); PrintAndLogEx(NORMAL, "\n Emulating ISO/IEC 14443 type A tag with 4,7 byte UID\n"); PrintAndLogEx(NORMAL, "Usage: hf 14a sim [h] t u [x] [e] [v]"); @@ -180,7 +182,8 @@ int usage_hf_14a_sim(void) { // PrintAndLogEx(NORMAL, " hf 14a sim t 1 u 11223445566778899AA\n"); return 0; } -int usage_hf_14a_sniff(void) { +int usage_hf_14a_sniff(void) +{ PrintAndLogEx(NORMAL, "It get data from the field and saves it into command buffer."); PrintAndLogEx(NORMAL, "Buffer accessible from command 'hf list 14a'"); PrintAndLogEx(NORMAL, "Usage: hf 14a sniff [c][r]"); @@ -190,7 +193,8 @@ int usage_hf_14a_sniff(void) { PrintAndLogEx(NORMAL, " hf 14a sniff c r"); return 0; } -int usage_hf_14a_raw(void) { +int usage_hf_14a_raw(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 14a raw [-h] [-r] [-c] [-p] [-a] [-T] [-t] [-b] <0A 0B 0C ... hex>"); PrintAndLogEx(NORMAL, " -h this help"); PrintAndLogEx(NORMAL, " -r do not read response"); @@ -204,7 +208,8 @@ int usage_hf_14a_raw(void) { PrintAndLogEx(NORMAL, " -3 ISO14443-3 select only (skip RATS)"); return 0; } -int usage_hf_14a_reader(void) { +int usage_hf_14a_reader(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 14a reader [k|s|x] [3]"); PrintAndLogEx(NORMAL, " k keep the field active after command executed"); PrintAndLogEx(NORMAL, " s silent (no messages)"); @@ -212,7 +217,8 @@ int usage_hf_14a_reader(void) { PrintAndLogEx(NORMAL, " 3 ISO14443-3 select only (skip RATS)"); return 0; } -int usage_hf_14a_info(void){ +int usage_hf_14a_info(void) +{ PrintAndLogEx(NORMAL, "This command makes more extensive tests against a ISO14443a tag in order to collect information"); PrintAndLogEx(NORMAL, "Usage: hf 14a info [h|s]"); PrintAndLogEx(NORMAL, " s silent (no messages)"); @@ -220,34 +226,36 @@ int usage_hf_14a_info(void){ return 0; } -int CmdHF14AList(const char *Cmd) { +int CmdHF14AList(const char *Cmd) +{ //PrintAndLogEx(NORMAL, "Deprecated command, use 'hf list 14a' instead"); CmdTraceList("14a"); return 0; } -int Hf14443_4aGetCardData(iso14a_card_select_t * card) { +int Hf14443_4aGetCardData(iso14a_card_select_t *card) +{ UsbCommand c = {CMD_READER_ISO_14443a, {ISO14A_CONNECT, 0, 0}}; SendCommand(&c); UsbCommand resp; - WaitForResponse(CMD_ACK,&resp); + WaitForResponse(CMD_ACK, &resp); memcpy(card, (iso14a_card_select_t *)resp.d.asBytes, sizeof(iso14a_card_select_t)); uint64_t select_status = resp.arg[0]; // 0: couldn't read, 1: OK, with ATS, 2: OK, no ATS, 3: proprietary Anticollision - if(select_status == 0) { + if (select_status == 0) { PrintAndLog("E->iso14443a card select failed"); return 1; } - if(select_status == 2) { + if (select_status == 2) { PrintAndLog("E->Card doesn't support iso14443-4 mode"); return 1; } - if(select_status == 3) { + if (select_status == 3) { PrintAndLog("E->Card doesn't support standard iso14443-3 anticollision"); PrintAndLog("\tATQA : %02x %02x", card->atqa[1], card->atqa[0]); return 1; @@ -256,7 +264,7 @@ int Hf14443_4aGetCardData(iso14a_card_select_t * card) { PrintAndLog(" UID: %s", sprint_hex(card->uid, card->uidlen)); PrintAndLog("ATQA: %02x %02x", card->atqa[1], card->atqa[0]); PrintAndLog(" SAK: %02x [%" PRIu64 "]", card->sak, resp.arg[0]); - if(card->ats_len < 3) { // a valid ATS consists of at least the length byte (TL) and 2 CRC bytes + if (card->ats_len < 3) { // a valid ATS consists of at least the length byte (TL) and 2 CRC bytes PrintAndLog("E-> Error ATS length(%d) : %s", card->ats_len, sprint_hex(card->ats, card->ats_len)); return 1; } @@ -265,7 +273,8 @@ int Hf14443_4aGetCardData(iso14a_card_select_t * card) { return 0; } -int CmdHF14AReader(const char *Cmd) { +int CmdHF14AReader(const char *Cmd) +{ uint32_t cm = ISO14A_CONNECT; bool disconnectAfter = true, silent = false; @@ -273,23 +282,23 @@ int CmdHF14AReader(const char *Cmd) { while (param_getchar(Cmd, cmdp) != 0x00) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': - return usage_hf_14a_reader(); - case '3': - cm |= ISO14A_NO_RATS; - break; - case 'k': - disconnectAfter = false; - break; - case 's': - silent = true; - break; - case 'x': - cm &= ~ISO14A_CONNECT; - break; - default: - PrintAndLogEx(WARNING, "Unknown command."); - return 1; + case 'h': + return usage_hf_14a_reader(); + case '3': + cm |= ISO14A_NO_RATS; + break; + case 'k': + disconnectAfter = false; + break; + case 's': + silent = true; + break; + case 'x': + cm &= ~ISO14A_CONNECT; + break; + default: + PrintAndLogEx(WARNING, "Unknown command."); + return 1; } cmdp++; } @@ -337,7 +346,7 @@ int CmdHF14AReader(const char *Cmd) { PrintAndLogEx(NORMAL, "ATQA : %02x %02x", card.atqa[1], card.atqa[0]); PrintAndLogEx(NORMAL, " SAK : %02x [%" PRIu64 "]", card.sak, resp.arg[0]); - if(card.ats_len >= 3) { // a valid ATS consists of at least the length byte (TL) and 2 CRC bytes + if (card.ats_len >= 3) { // a valid ATS consists of at least the length byte (TL) and 2 CRC bytes PrintAndLogEx(NORMAL, " ATS : %s", sprint_hex(card.ats, card.ats_len)); } @@ -353,7 +362,8 @@ int CmdHF14AReader(const char *Cmd) { return 0; } -int CmdHF14AInfo(const char *Cmd) { +int CmdHF14AInfo(const char *Cmd) +{ if (Cmd[0] == 'h' || Cmd[0] == 'H') return usage_hf_14a_info(); @@ -425,30 +435,62 @@ int CmdHF14AInfo(const char *Cmd) { select_status = resp.arg[0]; // 0: couldn't read, 1: OK, with ATS, 2: OK, no ATS - if(select_status == 0) { + if (select_status == 0) { DropField(); return 0; } break; - case 0x01: PrintAndLogEx(NORMAL, "TYPE : NXP TNP3xxx Activision Game Appliance"); break; - case 0x04: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE (various !DESFire !DESFire EV1)"); isMifareClassic = false; break; - case 0x08: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE CLASSIC 1k | Plus 2k SL1 | 1k Ev1"); break; - case 0x09: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Mini 0.3k"); break; - case 0x0A: PrintAndLogEx(NORMAL, "TYPE : FM11RF005SH (Shanghai Metro)"); break; - case 0x10: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Plus 2k SL2"); break; - case 0x11: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Plus 4k SL2"); break; - case 0x18: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Classic 4k | Plus 4k SL1 | 4k Ev1"); break; - case 0x20: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE DESFire 4k | DESFire EV1 2k/4k/8k | Plus 2k/4k SL3 | JCOP 31/41"); isMifareClassic = false; break; - case 0x24: PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE DESFire | DESFire EV1"); isMifareClassic = false; break; - case 0x28: PrintAndLogEx(NORMAL, "TYPE : JCOP31 or JCOP41 v2.3.1"); break; - case 0x38: PrintAndLogEx(NORMAL, "TYPE : Nokia 6212 or 6131 MIFARE CLASSIC 4K"); break; - case 0x88: PrintAndLogEx(NORMAL, "TYPE : Infineon MIFARE CLASSIC 1K"); break; - case 0x98: PrintAndLogEx(NORMAL, "TYPE : Gemplus MPCOS"); break; - default: ; + case 0x01: + PrintAndLogEx(NORMAL, "TYPE : NXP TNP3xxx Activision Game Appliance"); + break; + case 0x04: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE (various !DESFire !DESFire EV1)"); + isMifareClassic = false; + break; + case 0x08: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE CLASSIC 1k | Plus 2k SL1 | 1k Ev1"); + break; + case 0x09: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Mini 0.3k"); + break; + case 0x0A: + PrintAndLogEx(NORMAL, "TYPE : FM11RF005SH (Shanghai Metro)"); + break; + case 0x10: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Plus 2k SL2"); + break; + case 0x11: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Plus 4k SL2"); + break; + case 0x18: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE Classic 4k | Plus 4k SL1 | 4k Ev1"); + break; + case 0x20: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE DESFire 4k | DESFire EV1 2k/4k/8k | Plus 2k/4k SL3 | JCOP 31/41"); + isMifareClassic = false; + break; + case 0x24: + PrintAndLogEx(NORMAL, "TYPE : NXP MIFARE DESFire | DESFire EV1"); + isMifareClassic = false; + break; + case 0x28: + PrintAndLogEx(NORMAL, "TYPE : JCOP31 or JCOP41 v2.3.1"); + break; + case 0x38: + PrintAndLogEx(NORMAL, "TYPE : Nokia 6212 or 6131 MIFARE CLASSIC 4K"); + break; + case 0x88: + PrintAndLogEx(NORMAL, "TYPE : Infineon MIFARE CLASSIC 1K"); + break; + case 0x98: + PrintAndLogEx(NORMAL, "TYPE : Gemplus MPCOS"); + break; + default: + ; } // Double & triple sized UID, can be mapped to a manufacturer. - if ( card.uidlen > 4 ) { + if (card.uidlen > 4) { PrintAndLogEx(NORMAL, "MANUFACTURER : %s", getTagInfo(card.uid[0])); } @@ -461,13 +503,13 @@ int CmdHF14AInfo(const char *Cmd) { memcpy(c.d.asBytes, rats, 2); clearCommandBuffer(); SendCommand(&c); - WaitForResponse(CMD_ACK,&resp); + WaitForResponse(CMD_ACK, &resp); memcpy(card.ats, resp.d.asBytes, resp.arg[0]); card.ats_len = resp.arg[0]; // note: ats_len includes CRC Bytes } - if(card.ats_len >= 3) { // a valid ATS consists of at least the length byte (TL) and 2 CRC bytes + if (card.ats_len >= 3) { // a valid ATS consists of at least the length byte (TL) and 2 CRC bytes bool ta1 = 0, tb1 = 0, tc1 = 0; int pos; @@ -487,13 +529,13 @@ int CmdHF14AInfo(const char *Cmd) { int16_t fsci = card.ats[1] & 0x0f; PrintAndLogEx(NORMAL, " - T0 : TA1 is%s present, TB1 is%s present, " - "TC1 is%s present, FSCI is %d (FSC = %ld)", - (ta1 ? "" : " NOT"), - (tb1 ? "" : " NOT"), - (tc1 ? "" : " NOT"), - fsci, - fsci < sizeof(atsFSC) ? atsFSC[fsci] : -1 - ); + "TC1 is%s present, FSCI is %d (FSC = %ld)", + (ta1 ? "" : " NOT"), + (tb1 ? "" : " NOT"), + (tc1 ? "" : " NOT"), + fsci, + fsci < sizeof(atsFSC) ? atsFSC[fsci] : -1 + ); } pos = 2; if (ta1) { @@ -508,29 +550,29 @@ int CmdHF14AInfo(const char *Cmd) { if (strlen(ds) != 0) ds[strlen(ds) - 2] = '\0'; if (strlen(dr) != 0) dr[strlen(dr) - 2] = '\0'; PrintAndLogEx(NORMAL, " - TA1 : different divisors are%s supported, " - "DR: [%s], DS: [%s]", - (card.ats[pos] & 0x80 ? " NOT" : ""), dr, ds); + "DR: [%s], DS: [%s]", + (card.ats[pos] & 0x80 ? " NOT" : ""), dr, ds); pos++; } if (tb1) { uint32_t sfgi = card.ats[pos] & 0x0F; uint32_t fwi = card.ats[pos] >> 4; PrintAndLogEx(NORMAL, " - TB1 : SFGI = %d (SFGT = %s%ld/fc), FWI = %d (FWT = %ld/fc)", - (sfgi), - sfgi ? "" : "(not needed) ", - sfgi ? (1 << 12) << sfgi : 0, - fwi, - (1 << 12) << fwi - ); + (sfgi), + sfgi ? "" : "(not needed) ", + sfgi ? (1 << 12) << sfgi : 0, + fwi, + (1 << 12) << fwi + ); pos++; } if (tc1) { PrintAndLogEx(NORMAL, " - TC1 : NAD is%s supported, CID is%s supported", - (card.ats[pos] & 0x01) ? "" : " NOT", - (card.ats[pos] & 0x02) ? "" : " NOT"); + (card.ats[pos] & 0x01) ? "" : " NOT", + (card.ats[pos] & 0x02) ? "" : " NOT"); pos++; } - if (card.ats[0] > pos && card.ats[0] < card.ats_len - 2 ) { + if (card.ats[0] > pos && card.ats[0] < card.ats_len - 2) { char *tip = ""; if (card.ats[0] - pos >= 7) { if (memcmp(card.ats + pos, "\xC1\x05\x2F\x2F\x01\xBC\xD6", 7) == 0) { @@ -544,29 +586,59 @@ int CmdHF14AInfo(const char *Cmd) { PrintAndLogEx(NORMAL, " c1 -> Mifare or (multiple) virtual cards of various type"); PrintAndLogEx(NORMAL, " %02x -> Length is %d bytes", card.ats[pos + 1], card.ats[pos + 1]); switch (card.ats[pos + 2] & 0xf0) { - case 0x10: PrintAndLogEx(NORMAL, " 1x -> MIFARE DESFire"); break; - case 0x20: PrintAndLogEx(NORMAL, " 2x -> MIFARE Plus"); break; + case 0x10: + PrintAndLogEx(NORMAL, " 1x -> MIFARE DESFire"); + break; + case 0x20: + PrintAndLogEx(NORMAL, " 2x -> MIFARE Plus"); + break; } switch (card.ats[pos + 2] & 0x0f) { - case 0x00: PrintAndLogEx(NORMAL, " x0 -> <1 kByte"); break; - case 0x01: PrintAndLogEx(NORMAL, " x1 -> 1 kByte"); break; - case 0x02: PrintAndLogEx(NORMAL, " x2 -> 2 kByte"); break; - case 0x03: PrintAndLogEx(NORMAL, " x3 -> 4 kByte"); break; - case 0x04: PrintAndLogEx(NORMAL, " x4 -> 8 kByte"); break; + case 0x00: + PrintAndLogEx(NORMAL, " x0 -> <1 kByte"); + break; + case 0x01: + PrintAndLogEx(NORMAL, " x1 -> 1 kByte"); + break; + case 0x02: + PrintAndLogEx(NORMAL, " x2 -> 2 kByte"); + break; + case 0x03: + PrintAndLogEx(NORMAL, " x3 -> 4 kByte"); + break; + case 0x04: + PrintAndLogEx(NORMAL, " x4 -> 8 kByte"); + break; } switch (card.ats[pos + 3] & 0xf0) { - case 0x00: PrintAndLogEx(NORMAL, " 0x -> Engineering sample"); break; - case 0x20: PrintAndLogEx(NORMAL, " 2x -> Released"); break; + case 0x00: + PrintAndLogEx(NORMAL, " 0x -> Engineering sample"); + break; + case 0x20: + PrintAndLogEx(NORMAL, " 2x -> Released"); + break; } switch (card.ats[pos + 3] & 0x0f) { - case 0x00: PrintAndLogEx(NORMAL, " x0 -> Generation 1"); break; - case 0x01: PrintAndLogEx(NORMAL, " x1 -> Generation 2"); break; - case 0x02: PrintAndLogEx(NORMAL, " x2 -> Generation 3"); break; + case 0x00: + PrintAndLogEx(NORMAL, " x0 -> Generation 1"); + break; + case 0x01: + PrintAndLogEx(NORMAL, " x1 -> Generation 2"); + break; + case 0x02: + PrintAndLogEx(NORMAL, " x2 -> Generation 3"); + break; } switch (card.ats[pos + 4] & 0x0f) { - case 0x00: PrintAndLogEx(NORMAL, " x0 -> Only VCSL supported"); break; - case 0x01: PrintAndLogEx(NORMAL, " x1 -> VCS, VCSL, and SVC supported"); break; - case 0x0E: PrintAndLogEx(NORMAL, " xE -> no VCS command supported"); break; + case 0x00: + PrintAndLogEx(NORMAL, " x0 -> Only VCSL supported"); + break; + case 0x01: + PrintAndLogEx(NORMAL, " x1 -> VCS, VCSL, and SVC supported"); + break; + case 0x0E: + PrintAndLogEx(NORMAL, " xE -> no VCS command supported"); + break; } } } @@ -578,14 +650,14 @@ int CmdHF14AInfo(const char *Cmd) { if (isMifareClassic) { int res = detect_classic_prng(); - if ( res == 1 ) + if (res == 1) PrintAndLogEx(SUCCESS, "Prng detection: " _GREEN_(WEAK)); - else if (res == 0 ) + else if (res == 0) PrintAndLogEx(SUCCESS, "Prng detection: " _YELLOW_(HARD)); else PrintAndLogEx(FAILED, "prng detection: " _RED_(failed)); - if ( do_nack_test ) + if (do_nack_test) detect_classic_nackbug(silent); } @@ -593,7 +665,8 @@ int CmdHF14AInfo(const char *Cmd) { } // Collect ISO14443 Type A UIDs -int CmdHF14ACUIDs(const char *Cmd) { +int CmdHF14ACUIDs(const char *Cmd) +{ // requested number of UIDs int n = atoi(Cmd); // collect at least 1 (e.g. if no parameter was given) @@ -606,7 +679,8 @@ int CmdHF14ACUIDs(const char *Cmd) { for (int i = 0; i < n; i++) { if (ukbhit()) { - int gc = getchar(); (void)gc; + int gc = getchar(); + (void)gc; PrintAndLogEx(WARNING, "\n[!] aborted via keyboard!\n"); break; } @@ -616,7 +690,7 @@ int CmdHF14ACUIDs(const char *Cmd) { SendCommand(&c); UsbCommand resp; - WaitForResponse(CMD_ACK,&resp); + WaitForResponse(CMD_ACK, &resp); iso14a_card_select_t *card = (iso14a_card_select_t *) resp.d.asBytes; @@ -626,37 +700,38 @@ int CmdHF14ACUIDs(const char *Cmd) { } else { char uid_string[20]; for (uint16_t i = 0; i < card->uidlen; i++) { - sprintf(&uid_string[2*i], "%02X", card->uid[i]); + sprintf(&uid_string[2 * i], "%02X", card->uid[i]); } PrintAndLogEx(NORMAL, "%s", uid_string); } } - PrintAndLogEx(SUCCESS, "end: %" PRIu64 " seconds", (msclock()-t1)/1000); + PrintAndLogEx(SUCCESS, "end: %" PRIu64 " seconds", (msclock() - t1) / 1000); return 1; } // ## simulate iso14443a tag -int CmdHF14ASim(const char *Cmd) { +int CmdHF14ASim(const char *Cmd) +{ bool errors = false; uint8_t flags = 0; uint8_t tagtype = 1; uint8_t cmdp = 0; - uint8_t uid[10] = {0,0,0,0,0,0,0,0,0,0}; + uint8_t uid[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int uidlen = 0; bool useUIDfromEML = true; bool setEmulatorMem = false; bool verbose = false; nonces_t data[1]; - while(param_getchar(Cmd, cmdp) != 0x00 && !errors) { - switch(param_getchar(Cmd, cmdp)) { + while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { + switch (param_getchar(Cmd, cmdp)) { case 'h': case 'H': return usage_hf_14a_sim(); case 't': case 'T': // Retrieve the tag type - tagtype = param_get8ex(Cmd, cmdp+1, 0, 10); + tagtype = param_get8ex(Cmd, cmdp + 1, 0, 10); if (tagtype == 0) errors = true; cmdp += 2; @@ -664,15 +739,21 @@ int CmdHF14ASim(const char *Cmd) { case 'u': case 'U': // Retrieve the full 4,7,10 byte long uid - param_gethex_ex(Cmd, cmdp+1, uid, &uidlen); - switch(uidlen) { + param_gethex_ex(Cmd, cmdp + 1, uid, &uidlen); + switch (uidlen) { //case 20: flags |= FLAG_10B_UID_IN_DATA; break; - case 14: flags |= FLAG_7B_UID_IN_DATA; break; - case 8: flags |= FLAG_4B_UID_IN_DATA; break; - default: errors = true; break; + case 14: + flags |= FLAG_7B_UID_IN_DATA; + break; + case 8: + flags |= FLAG_4B_UID_IN_DATA; + break; + default: + errors = true; + break; } if (!errors) { - PrintAndLogEx(SUCCESS, "Emulating ISO/IEC 14443 type A tag with %d byte UID (%s)", uidlen>>1, sprint_hex(uid, uidlen>>1)); + PrintAndLogEx(SUCCESS, "Emulating ISO/IEC 14443 type A tag with %d byte UID (%s)", uidlen >> 1, sprint_hex(uid, uidlen >> 1)); useUIDfromEML = false; } cmdp += 2; @@ -696,36 +777,37 @@ int CmdHF14ASim(const char *Cmd) { PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); errors = true; break; - } + } } //Validations if (errors || cmdp == 0) return usage_hf_14a_sim(); - if ( useUIDfromEML ) + if (useUIDfromEML) flags |= FLAG_UID_IN_EMUL; - UsbCommand c = {CMD_SIMULATE_TAG_ISO_14443a,{ tagtype, flags, 0 }}; - memcpy(c.d.asBytes, uid, uidlen>>1); + UsbCommand c = {CMD_SIMULATE_TAG_ISO_14443a, { tagtype, flags, 0 }}; + memcpy(c.d.asBytes, uid, uidlen >> 1); clearCommandBuffer(); SendCommand(&c); UsbCommand resp; PrintAndLogEx(SUCCESS, "press pm3-button to abort simulation"); - while( !ukbhit() ){ - if (!WaitForResponseTimeout(CMD_ACK, &resp, 1500) ) continue; - if ( !(flags & FLAG_NR_AR_ATTACK) ) break; - if ( (resp.arg[0] & 0xffff) != CMD_SIMULATE_MIFARE_CARD ) break; + while (!ukbhit()) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 1500)) continue; + if (!(flags & FLAG_NR_AR_ATTACK)) break; + if ((resp.arg[0] & 0xffff) != CMD_SIMULATE_MIFARE_CARD) break; - memcpy(data, resp.d.asBytes, sizeof(data) ); + memcpy(data, resp.d.asBytes, sizeof(data)); readerAttack(data[0], setEmulatorMem, verbose); } showSectorTable(); return 0; } -int CmdHF14ASniff(const char *Cmd) { +int CmdHF14ASniff(const char *Cmd) +{ int param = 0; uint8_t ctmp; for (int i = 0; i < 2; i++) { @@ -740,7 +822,8 @@ int CmdHF14ASniff(const char *Cmd) { return 0; } -int ExchangeRAW14a(uint8_t *datain, int datainlen, bool activateField, bool leaveSignalON, uint8_t *dataout, int maxdataoutlen, int *dataoutlen) { +int ExchangeRAW14a(uint8_t *datain, int datainlen, bool activateField, bool leaveSignalON, uint8_t *dataout, int maxdataoutlen, int *dataoutlen) +{ static bool responseNum = false; uint16_t cmdc = 0; *dataoutlen = 0; @@ -803,7 +886,7 @@ int ExchangeRAW14a(uint8_t *datain, int datainlen, bool activateField, bool leav recv = resp.d.asBytes; int iLen = resp.arg[0]; - if(!iLen) { + if (!iLen) { PrintAndLogEx(ERR, "No card response."); return 1; } @@ -839,7 +922,8 @@ int ExchangeRAW14a(uint8_t *datain, int datainlen, bool activateField, bool leav return 0; } -int SelectCard14443_4(bool disconnect, iso14a_card_select_t *card) { +int SelectCard14443_4(bool disconnect, iso14a_card_select_t *card) +{ UsbCommand resp; frameLength = 0; @@ -909,7 +993,8 @@ int SelectCard14443_4(bool disconnect, iso14a_card_select_t *card) { return 0; } -int CmdExchangeAPDU(bool chainingin, uint8_t *datain, int datainlen, bool activateField, uint8_t *dataout, int maxdataoutlen, int *dataoutlen, bool *chainingout) { +int CmdExchangeAPDU(bool chainingin, uint8_t *datain, int datainlen, bool activateField, uint8_t *dataout, int maxdataoutlen, int *dataoutlen, bool *chainingout) +{ *chainingout = false; if (activateField) { @@ -956,7 +1041,7 @@ int CmdExchangeAPDU(bool chainingin, uint8_t *datain, int datainlen, bool activa return 0; } - if(!iLen) { + if (!iLen) { PrintAndLogEx(ERR, "APDU: No APDU response."); return 1; } @@ -993,14 +1078,15 @@ int CmdExchangeAPDU(bool chainingin, uint8_t *datain, int datainlen, bool activa return 0; } -int ExchangeAPDU14a(uint8_t *datain, int datainlen, bool activateField, bool leaveSignalON, uint8_t *dataout, int maxdataoutlen, int *dataoutlen) { +int ExchangeAPDU14a(uint8_t *datain, int datainlen, bool activateField, bool leaveSignalON, uint8_t *dataout, int maxdataoutlen, int *dataoutlen) +{ *dataoutlen = 0; bool chaining = false; int res; // 3 byte here - 1b framing header, 2b crc16 if (APDUInFramingEnable && - ( (frameLength && (datainlen > frameLength - 3)) || (datainlen > USB_CMD_DATA_SIZE - 3)) ) { + ((frameLength && (datainlen > frameLength - 3)) || (datainlen > USB_CMD_DATA_SIZE - 3))) { int clen = 0; bool vActivateField = activateField; @@ -1063,7 +1149,8 @@ int ExchangeAPDU14a(uint8_t *datain, int datainlen, bool activateField, bool lea } // ISO14443-4. 7. Half-duplex block transmission protocol -int CmdHF14AAPDU(const char *cmd) { +int CmdHF14AAPDU(const char *cmd) +{ uint8_t data[USB_CMD_DATA_SIZE]; int datalen = 0; bool activateField = false; @@ -1071,10 +1158,10 @@ int CmdHF14AAPDU(const char *cmd) { bool decodeTLV = false; CLIParserInit("hf 14a apdu", - "Sends an ISO 7816-4 APDU via ISO 14443-4 block transmission protocol (T=CL)", - "Sample:\n\thf 14a apdu -st 00A404000E325041592E5359532E444446303100\n"); + "Sends an ISO 7816-4 APDU via ISO 14443-4 block transmission protocol (T=CL)", + "Sample:\n\thf 14a apdu -st 00A404000E325041592E5359532E444446303100\n"); - void* argtable[] = { + void *argtable[] = { arg_param_begin, arg_lit0("sS", "select", "activate field and select card"), arg_lit0("kK", "keep", "leave the signal field ON after receive response"), @@ -1087,11 +1174,11 @@ int CmdHF14AAPDU(const char *cmd) { activateField = arg_get_lit(1); leaveSignalON = arg_get_lit(2); decodeTLV = arg_get_lit(3); - // len = data + PCB(1b) + CRC(2b) + // len = data + PCB(1b) + CRC(2b) CLIGetHexBLessWithReturn(4, data, &datalen, 1 + 2); -CLIParserFree(); - PrintAndLogEx(NORMAL, ">>>>[%s%s%s] %s", activateField ? "sel ": "", leaveSignalON ? "keep ": "", decodeTLV ? "TLV": "", sprint_hex(data, datalen)); + CLIParserFree(); + PrintAndLogEx(NORMAL, ">>>>[%s%s%s] %s", activateField ? "sel " : "", leaveSignalON ? "keep " : "", decodeTLV ? "TLV" : "", sprint_hex(data, datalen)); int res = ExchangeAPDU14a(data, datalen, activateField, leaveSignalON, data, USB_CMD_DATA_SIZE, &datalen); @@ -1110,7 +1197,8 @@ CLIParserFree(); return 0; } -int CmdHF14ACmdRaw(const char *cmd) { +int CmdHF14ACmdRaw(const char *cmd) +{ UsbCommand c = {CMD_READER_ISO_14443a, {0, 0, 0}}; bool reply = 1; bool crc = false; @@ -1122,7 +1210,7 @@ int CmdHF14ACmdRaw(const char *cmd) { bool bTimeout = false; uint32_t timeout = 0; bool topazmode = false; - char buf[5]=""; + char buf[5] = ""; int i = 0; uint8_t data[USB_CMD_DATA_SIZE]; uint16_t datalen = 0; @@ -1131,12 +1219,12 @@ int CmdHF14ACmdRaw(const char *cmd) { if (strlen(cmd) < 2) return usage_hf_14a_raw(); // strip - while (*cmd==' ' || *cmd=='\t') cmd++; + while (*cmd == ' ' || *cmd == '\t') cmd++; - while (cmd[i]!='\0') { - if (cmd[i]==' ' || cmd[i]=='\t') { i++; continue; } - if (cmd[i]=='-') { - switch (cmd[i+1]) { + while (cmd[i] != '\0') { + if (cmd[i] == ' ' || cmd[i] == '\t') { i++; continue; } + if (cmd[i] == '-') { + switch (cmd[i + 1]) { case 'H': case 'h': return usage_hf_14a_raw(); @@ -1156,19 +1244,19 @@ int CmdHF14ACmdRaw(const char *cmd) { active_select = true; break; case 'b': - sscanf(cmd+i+2, "%d", &temp); + sscanf(cmd + i + 2, "%d", &temp); numbits = temp & 0xFFFF; - i+=3; - while(cmd[i]!=' ' && cmd[i]!='\0') { i++; } - i-=2; + i += 3; + while (cmd[i] != ' ' && cmd[i] != '\0') { i++; } + i -= 2; break; case 't': bTimeout = true; - sscanf(cmd+i+2, "%d", &temp); + sscanf(cmd + i + 2, "%d", &temp); timeout = temp; - i+=3; - while(cmd[i]!=' ' && cmd[i]!='\0') { i++; } - i-=2; + i += 3; + while (cmd[i] != ' ' && cmd[i] != '\0') { i++; } + i -= 2; break; case 'T': topazmode = true; @@ -1182,10 +1270,10 @@ int CmdHF14ACmdRaw(const char *cmd) { i += 2; continue; } - if ((cmd[i]>='0' && cmd[i]<='9') || - (cmd[i]>='a' && cmd[i]<='f') || - (cmd[i]>='A' && cmd[i]<='F') ) { - buf[strlen(buf)+1] = 0; + if ((cmd[i] >= '0' && cmd[i] <= '9') || + (cmd[i] >= 'a' && cmd[i] <= 'f') || + (cmd[i] >= 'A' && cmd[i] <= 'F')) { + buf[strlen(buf) + 1] = 0; buf[strlen(buf)] = cmd[i]; i++; @@ -1193,7 +1281,7 @@ int CmdHF14ACmdRaw(const char *cmd) { sscanf(buf, "%x", &temp); data[datalen] = (uint8_t)(temp & 0xff); *buf = 0; - if (++datalen >= sizeof(data)){ + if (++datalen >= sizeof(data)) { if (crc) PrintAndLogEx(NORMAL, "Buffer is full, we can't add CRC to your data"); break; @@ -1205,7 +1293,7 @@ int CmdHF14ACmdRaw(const char *cmd) { return 0; } - if (crc && datalen > 0 && datalen < sizeof(data)-2) { + if (crc && datalen > 0 && datalen < sizeof(data) - 2) { uint8_t first, second; if (topazmode) { compute_crc(CRC_14443_B, data, datalen, &first, &second); @@ -1222,14 +1310,14 @@ int CmdHF14ACmdRaw(const char *cmd) { c.arg[0] |= ISO14A_NO_SELECT; } - if (bTimeout){ - #define MAX_TIMEOUT 40542464 // = (2^32-1) * (8*16) / 13560000Hz * 1000ms/s + if (bTimeout) { +#define MAX_TIMEOUT 40542464 // = (2^32-1) * (8*16) / 13560000Hz * 1000ms/s c.arg[0] |= ISO14A_SET_TIMEOUT; - if(timeout > MAX_TIMEOUT) { + if (timeout > MAX_TIMEOUT) { timeout = MAX_TIMEOUT; PrintAndLogEx(NORMAL, "Set timeout to 40542 seconds (11.26 hours). The max we can wait for response"); } - c.arg[2] = 13560000 / 1000 / (8*16) * timeout; // timeout in ETUs (time to transfer 1 bit, approx. 9.4 us) + c.arg[2] = 13560000 / 1000 / (8 * 16) * timeout; // timeout in ETUs (time to transfer 1 bit, approx. 9.4 us) } if (power) { @@ -1266,15 +1354,16 @@ int CmdHF14ACmdRaw(const char *cmd) { return 0; } -static int waitCmd(uint8_t iSelect) { +static int waitCmd(uint8_t iSelect) +{ UsbCommand resp; uint16_t len = 0; if (WaitForResponseTimeout(CMD_ACK, &resp, 1500)) { len = (resp.arg[0] & 0xFFFF); - if (iSelect){ + if (iSelect) { len = (resp.arg[1] & 0xFFFF); - if (len){ + if (len) { PrintAndLogEx(NORMAL, "Card selected. UID[%i]:", len); } else { PrintAndLogEx(WARNING, "Can't select card."); @@ -1286,7 +1375,7 @@ static int waitCmd(uint8_t iSelect) { if (!len) return 1; - PrintAndLogEx(NORMAL, "%s", sprint_hex(resp.d.asBytes, len) ); + PrintAndLogEx(NORMAL, "%s", sprint_hex(resp.d.asBytes, len)); } else { PrintAndLogEx(WARNING, "timeout while waiting for reply."); return 3; @@ -1294,14 +1383,15 @@ static int waitCmd(uint8_t iSelect) { return 0; } -int CmdHF14AAntiFuzz(const char *cmd) { +int CmdHF14AAntiFuzz(const char *cmd) +{ CLIParserInit("hf 14a antifuzz", - "Tries to fuzz the ISO14443a anticollision phase", - "Usage:\n" - "\thf 14a antifuzz -4\n"); + "Tries to fuzz the ISO14443a anticollision phase", + "Usage:\n" + "\thf 14a antifuzz -4\n"); - void* argtable[] = { + void *argtable[] = { arg_param_begin, arg_lit0("4", NULL, "4 byte uid"), arg_lit0("7", NULL, "7 byte uid"), @@ -1323,15 +1413,16 @@ int CmdHF14AAntiFuzz(const char *cmd) { return 0; } -int CmdHF14AChaining(const char *cmd) { +int CmdHF14AChaining(const char *cmd) +{ CLIParserInit("hf 14a chaining", - "Enable/Disable ISO14443a input chaining. Maximum input length goes from ATS.", - "Usage:\n" - "\thf 14a chaining disable -> disable chaining\n" - "\thf 14a chaining -> show chaining enable/disable state\n"); + "Enable/Disable ISO14443a input chaining. Maximum input length goes from ATS.", + "Usage:\n" + "\thf 14a chaining disable -> disable chaining\n" + "\thf 14a chaining -> show chaining enable/disable state\n"); - void* argtable[] = { + void *argtable[] = { arg_param_begin, arg_str0(NULL, NULL, "", NULL), arg_param_end @@ -1369,13 +1460,15 @@ static command_t CommandTable[] = { {NULL, NULL, 0, NULL} }; -int CmdHF14A(const char *Cmd) { +int CmdHF14A(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { - CmdsHelp(CommandTable); - return 0; +int CmdHelp(const char *Cmd) +{ + CmdsHelp(CommandTable); + return 0; } diff --git a/client/cmdhf14a.h b/client/cmdhf14a.h index 73cbd069c..55b94bb08 100644 --- a/client/cmdhf14a.h +++ b/client/cmdhf14a.h @@ -36,7 +36,7 @@ // structure and database for uid -> tagtype lookups typedef struct { uint8_t uid; - char* desc; + char *desc; } manufactureName; extern int CmdHF14A(const char *Cmd); @@ -49,8 +49,8 @@ extern int CmdHF14ACmdRaw(const char *Cmd); extern int CmdHF14ACUIDs(const char *Cmd); extern int CmdHF14AAntiFuzz(const char *cmd); -extern char* getTagInfo(uint8_t uid); -extern int Hf14443_4aGetCardData(iso14a_card_select_t * card); +extern char *getTagInfo(uint8_t uid); +extern int Hf14443_4aGetCardData(iso14a_card_select_t *card); extern int ExchangeAPDU14a(uint8_t *datain, int datainlen, bool activateField, bool leaveSignalON, uint8_t *dataout, int maxdataoutlen, int *dataoutlen); extern int ExchangeRAW14a(uint8_t *datain, int datainlen, bool activateField, bool leaveSignalON, uint8_t *dataout, int maxdataoutlen, int *dataoutlen); diff --git a/client/cmdhf14b.c b/client/cmdhf14b.c index c3962a17f..0bff7a32d 100644 --- a/client/cmdhf14b.c +++ b/client/cmdhf14b.c @@ -14,7 +14,8 @@ #define TIMEOUT 2000 static int CmdHelp(const char *Cmd); -int usage_hf_14b_info(void){ +int usage_hf_14b_info(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 14b info [h] [s]"); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " h this help"); @@ -23,7 +24,8 @@ int usage_hf_14b_info(void){ PrintAndLogEx(NORMAL, " hf 14b info"); return 0; } -int usage_hf_14b_reader(void){ +int usage_hf_14b_reader(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 14b reader [h] [s]"); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " h this help"); @@ -32,7 +34,8 @@ int usage_hf_14b_reader(void){ PrintAndLogEx(NORMAL, " hf 14b reader"); return 0; } -int usage_hf_14b_raw(void){ +int usage_hf_14b_raw(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 14b raw [-h] [-r] [-c] [-p] [-s || -ss] <0A 0B 0C ... hex>"); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " -h this help"); @@ -45,7 +48,8 @@ int usage_hf_14b_raw(void){ PrintAndLogEx(NORMAL, " hf 14b raw -s -c -p 0200a40400"); return 0; } -int usage_hf_14b_sniff(void){ +int usage_hf_14b_sniff(void) +{ PrintAndLogEx(NORMAL, "It get data from the field and saves it into command buffer."); PrintAndLogEx(NORMAL, "Buffer accessible from command 'hf list 14b'"); PrintAndLogEx(NORMAL, "Usage: hf 14b sniff [h]"); @@ -55,7 +59,8 @@ int usage_hf_14b_sniff(void){ PrintAndLogEx(NORMAL, " hf 14b sniff"); return 0; } -int usage_hf_14b_sim(void){ +int usage_hf_14b_sim(void) +{ PrintAndLogEx(NORMAL, "Emulating ISO/IEC 14443 type B tag with 4 UID / PUPI"); PrintAndLogEx(NORMAL, "Usage: hf 14b sim [h] u "); PrintAndLogEx(NORMAL, "Options:"); @@ -66,7 +71,8 @@ int usage_hf_14b_sim(void){ PrintAndLogEx(NORMAL, " hf 14b sim u 11223344"); return 0; } -int usage_hf_14b_read_srx(void){ +int usage_hf_14b_read_srx(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 14b sriread [h] <1|2>"); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " h this help"); @@ -76,7 +82,8 @@ int usage_hf_14b_read_srx(void){ PrintAndLogEx(NORMAL, " hf 14b sriread 2"); return 0; } -int usage_hf_14b_write_srx(void){ +int usage_hf_14b_write_srx(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 14b [h] sriwrite <1|2> "); PrintAndLogEx(NORMAL, "Options:"); PrintAndLogEx(NORMAL, " h this help"); @@ -90,18 +97,19 @@ int usage_hf_14b_write_srx(void){ PrintAndLogEx(NORMAL, " hf 14b sriwrite 2 FF 11223344"); return 0; } -int usage_hf_14b_dump(void){ +int usage_hf_14b_dump(void) +{ PrintAndLogEx(NORMAL, "This command dumps the contents of a ISO-14443-B tag and save it to file\n" - "\n" - "Usage: hf 14b dump [h] [card memory] \n" - "Options:\n" - "\th this help\n" - "\t[card memory] 1 = SRIX4K (default), 2 = SRI512" - "\tf filename, if no UID will be used as filename\n" - "\n" - "Example:\n" - "\thf 14b dump f\n" - "\thf 14b dump 2 f mydump"); + "\n" + "Usage: hf 14b dump [h] [card memory] \n" + "Options:\n" + "\th this help\n" + "\t[card memory] 1 = SRIX4K (default), 2 = SRI512" + "\tf filename, if no UID will be used as filename\n" + "\n" + "Example:\n" + "\thf 14b dump f\n" + "\thf 14b dump 2 f mydump"); return 0; } @@ -113,19 +121,22 @@ static void switch_on_field_14b(void) { } */ -static int switch_off_field_14b(void) { +static int switch_off_field_14b(void) +{ UsbCommand c = {CMD_ISO_14443B_COMMAND, {ISO14B_DISCONNECT, 0, 0}}; clearCommandBuffer(); SendCommand(&c); return 0; } -int CmdHF14BList(const char *Cmd) { +int CmdHF14BList(const char *Cmd) +{ CmdTraceList("14b"); return 0; } -int CmdHF14BSim(const char *Cmd) { +int CmdHF14BSim(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_hf_14b_sim(); @@ -140,7 +151,8 @@ int CmdHF14BSim(const char *Cmd) { return 0; } -int CmdHF14BSniff(const char *Cmd) { +int CmdHF14BSniff(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_hf_14b_sniff(); @@ -151,7 +163,8 @@ int CmdHF14BSniff(const char *Cmd) { return 0; } -int CmdHF14BCmdRaw (const char *Cmd) { +int CmdHF14BCmdRaw(const char *Cmd) +{ bool reply = true, power = false, select = false; char buf[5] = ""; int i = 0; @@ -160,15 +173,15 @@ int CmdHF14BCmdRaw (const char *Cmd) { uint32_t flags = ISO14B_CONNECT; uint32_t temp = 0; - if ( strlen(Cmd) < 3 ) return usage_hf_14b_raw(); + if (strlen(Cmd) < 3) return usage_hf_14b_raw(); // strip - while (*Cmd==' ' || *Cmd=='\t') ++Cmd; + while (*Cmd == ' ' || *Cmd == '\t') ++Cmd; - while (Cmd[i]!='\0') { - if (Cmd[i]==' ' || Cmd[i]=='\t') { ++i; continue; } - if (Cmd[i]=='-') { - switch (tolower(Cmd[i+1])) { + while (Cmd[i] != '\0') { + if (Cmd[i] == ' ' || Cmd[i] == '\t') { ++i; continue; } + if (Cmd[i] == '-') { + switch (tolower(Cmd[i + 1])) { case 'h': return usage_hf_14b_raw(); case 'r': @@ -182,7 +195,7 @@ int CmdHF14BCmdRaw (const char *Cmd) { break; case 's': select = true; - if (tolower(Cmd[i+2]) == 's') { + if (tolower(Cmd[i + 2]) == 's') { flags |= ISO14B_SELECT_SR; ++i; } else { @@ -192,20 +205,20 @@ int CmdHF14BCmdRaw (const char *Cmd) { default: return usage_hf_14b_raw(); } - i+=2; + i += 2; continue; } - if ((Cmd[i]>='0' && Cmd[i]<='9') || - (Cmd[i]>='a' && Cmd[i]<='f') || - (Cmd[i]>='A' && Cmd[i]<='F') ) { - buf[strlen(buf)+1]=0; - buf[strlen(buf)]=Cmd[i]; + if ((Cmd[i] >= '0' && Cmd[i] <= '9') || + (Cmd[i] >= 'a' && Cmd[i] <= 'f') || + (Cmd[i] >= 'A' && Cmd[i] <= 'F')) { + buf[strlen(buf) + 1] = 0; + buf[strlen(buf)] = Cmd[i]; i++; - if (strlen(buf)>=2) { - sscanf(buf,"%x",&temp); + if (strlen(buf) >= 2) { + sscanf(buf, "%x", &temp); data[datalen++] = (uint8_t)(temp & 0xff); - *buf=0; + *buf = 0; memset(buf, 0x00, sizeof(buf)); } continue; @@ -236,12 +249,13 @@ int CmdHF14BCmdRaw (const char *Cmd) { success = waitCmd14b(false); // get back response from the raw bytes you sent. - if (success && datalen>0) waitCmd14b(true); + if (success && datalen > 0) waitCmd14b(true); return 1; } -static bool get_14b_UID(iso14b_card_select_t *card) { +static bool get_14b_UID(iso14b_card_select_t *card) +{ if (!card) return false; @@ -258,7 +272,7 @@ static bool get_14b_UID(iso14b_card_select_t *card) { if (WaitForResponseTimeout(CMD_ACK, &resp, TIMEOUT)) { uint8_t status = resp.arg[0]; - if ( status == 0) { + if (status == 0) { memcpy(card, (iso14b_card_select_t *)resp.d.asBytes, sizeof(iso14b_card_select_t)); return true; } @@ -276,14 +290,14 @@ static bool get_14b_UID(iso14b_card_select_t *card) { if (WaitForResponseTimeout(CMD_ACK, &resp, TIMEOUT)) { uint8_t status = resp.arg[0]; - if ( status == 0) { + if (status == 0) { memcpy(card, (iso14b_card_select_t *)resp.d.asBytes, sizeof(iso14b_card_select_t)); return true; } } } // retry - if ( retry <= 0 ) + if (retry <= 0) PrintAndLogEx(WARNING, "timeout while waiting for reply."); return false; @@ -295,10 +309,11 @@ static bool get_14b_UID(iso14b_card_select_t *card) { // 4 = bit rate capacity // 5 = max frame size / -4 info // 6 = FWI / Coding options -static void print_atqb_resp(uint8_t *data, uint8_t cid){ +static void print_atqb_resp(uint8_t *data, uint8_t cid) +{ //PrintAndLogEx(NORMAL, " UID: %s", sprint_hex(data+1,4)); - PrintAndLogEx(NORMAL, " App Data: %s", sprint_hex(data,4)); - PrintAndLogEx(NORMAL, " Protocol: %s", sprint_hex(data+4,3)); + PrintAndLogEx(NORMAL, " App Data: %s", sprint_hex(data, 4)); + PrintAndLogEx(NORMAL, " Protocol: %s", sprint_hex(data + 4, 3)); uint8_t BitRate = data[4]; if (!BitRate) PrintAndLogEx(NORMAL, " Bit Rate: 106 kbit/s only PICC <-> PCD"); if (BitRate & 0x10) PrintAndLogEx(NORMAL, " Bit Rate: 212 kbit/s PICC -> PCD supported"); @@ -320,10 +335,10 @@ static void print_atqb_resp(uint8_t *data, uint8_t cid){ PrintAndLogEx(NORMAL, "Max Frame Size: %u%s bytes", maxFrame, (maxFrame == 257) ? "+ RFU" : ""); uint8_t protocolT = data[5] & 0xF; - PrintAndLogEx(NORMAL, " Protocol Type: Protocol is %scompliant with ISO/IEC 14443-4",(protocolT) ? "" : "not " ); + PrintAndLogEx(NORMAL, " Protocol Type: Protocol is %scompliant with ISO/IEC 14443-4", (protocolT) ? "" : "not "); - uint8_t fwt = data[6]>>4; - if ( fwt < 16 ){ + uint8_t fwt = data[6] >> 4; + if (fwt < 16) { uint32_t etus = (32 << fwt); uint32_t fwt_time = (302 << fwt); PrintAndLogEx(NORMAL, "Frame Wait Integer: %u - %u ETUs | %u us", fwt, etus, fwt_time); @@ -331,86 +346,105 @@ static void print_atqb_resp(uint8_t *data, uint8_t cid){ PrintAndLogEx(NORMAL, "Frame Wait Integer: %u - RFU", fwt); } - PrintAndLogEx(NORMAL, " App Data Code: Application is %s",(data[6]&4) ? "Standard" : "Proprietary"); - PrintAndLogEx(NORMAL, " Frame Options: NAD is %ssupported",(data[6]&2) ? "" : "not "); - PrintAndLogEx(NORMAL, " Frame Options: CID is %ssupported",(data[6]&1) ? "" : "not "); + PrintAndLogEx(NORMAL, " App Data Code: Application is %s", (data[6] & 4) ? "Standard" : "Proprietary"); + PrintAndLogEx(NORMAL, " Frame Options: NAD is %ssupported", (data[6] & 2) ? "" : "not "); + PrintAndLogEx(NORMAL, " Frame Options: CID is %ssupported", (data[6] & 1) ? "" : "not "); PrintAndLogEx(NORMAL, "Tag :"); - PrintAndLogEx(NORMAL, " Max Buf Length: %u (MBLI) %s", cid>>4, (cid & 0xF0) ? "" : "chained frames not supported"); + PrintAndLogEx(NORMAL, " Max Buf Length: %u (MBLI) %s", cid >> 4, (cid & 0xF0) ? "" : "chained frames not supported"); PrintAndLogEx(NORMAL, " CID : %u", cid & 0x0f); return; } // get SRx chip model (from UID) // from ST Microelectronics -char *get_ST_Chip_Model(uint8_t data){ +char *get_ST_Chip_Model(uint8_t data) +{ static char model[20]; char *retStr = model; - memset(model,0, sizeof(model)); + memset(model, 0, sizeof(model)); switch (data) { - case 0x0: sprintf(retStr, "SRIX4K (Special)"); break; - case 0x2: sprintf(retStr, "SR176"); break; - case 0x3: sprintf(retStr, "SRIX4K"); break; - case 0x4: sprintf(retStr, "SRIX512"); break; - case 0x6: sprintf(retStr, "SRI512"); break; - case 0x7: sprintf(retStr, "SRI4K"); break; - case 0xC: sprintf(retStr, "SRT512"); break; - default : sprintf(retStr, "Unknown"); break; + case 0x0: + sprintf(retStr, "SRIX4K (Special)"); + break; + case 0x2: + sprintf(retStr, "SR176"); + break; + case 0x3: + sprintf(retStr, "SRIX4K"); + break; + case 0x4: + sprintf(retStr, "SRIX512"); + break; + case 0x6: + sprintf(retStr, "SRI512"); + break; + case 0x7: + sprintf(retStr, "SRI4K"); + break; + case 0xC: + sprintf(retStr, "SRT512"); + break; + default : + sprintf(retStr, "Unknown"); + break; } return retStr; } // REMAKE: -int print_ST_Lock_info(uint8_t model){ +int print_ST_Lock_info(uint8_t model) +{ // PrintAndLogEx(NORMAL, "Chip Write Protection Bits:"); // // now interpret the data // switch (model){ - // case 0x0: //fall through (SRIX4K special) - // case 0x3: //fall through (SRIx4K) - // case 0x7: // (SRI4K) - // //only need data[3] - // blk1 = 9; - // PrintAndLogEx(NORMAL, " raw: %s", sprint_bin(data+3, 1)); - // PrintAndLogEx(NORMAL, " 07/08:%slocked", (data[3] & 1) ? " not " : " " ); - // for (uint8_t i = 1; i<8; i++){ - // PrintAndLogEx(NORMAL, " %02u:%slocked", blk1, (data[3] & (1 << i)) ? " not " : " " ); - // blk1++; - // } - // break; - // case 0x4: //fall through (SRIX512) - // case 0x6: //fall through (SRI512) - // case 0xC: // (SRT512) - // //need data[2] and data[3] - // blk1 = 0; - // PrintAndLogEx(NORMAL, " raw: %s", sprint_bin(data+2, 2)); - // for (uint8_t b=2; b<4; b++){ - // for (uint8_t i=0; i<8; i++){ - // PrintAndLogEx(NORMAL, " %02u:%slocked", blk1, (data[b] & (1 << i)) ? " not " : " " ); - // blk1++; - // } - // } - // break; - // case 0x2: // (SR176) - // //need data[2] - // blk1 = 0; - // PrintAndLogEx(NORMAL, " raw: %s", sprint_bin(data+2, 1)); - // for (uint8_t i = 0; i<8; i++){ - // PrintAndLogEx(NORMAL, " %02u/%02u:%slocked", blk1, blk1+1, (data[2] & (1 << i)) ? " " : " not " ); - // blk1+=2; - // } - // break; - // default: - // return rawClose(); + // case 0x0: //fall through (SRIX4K special) + // case 0x3: //fall through (SRIx4K) + // case 0x7: // (SRI4K) + // //only need data[3] + // blk1 = 9; + // PrintAndLogEx(NORMAL, " raw: %s", sprint_bin(data+3, 1)); + // PrintAndLogEx(NORMAL, " 07/08:%slocked", (data[3] & 1) ? " not " : " " ); + // for (uint8_t i = 1; i<8; i++){ + // PrintAndLogEx(NORMAL, " %02u:%slocked", blk1, (data[3] & (1 << i)) ? " not " : " " ); + // blk1++; + // } + // break; + // case 0x4: //fall through (SRIX512) + // case 0x6: //fall through (SRI512) + // case 0xC: // (SRT512) + // //need data[2] and data[3] + // blk1 = 0; + // PrintAndLogEx(NORMAL, " raw: %s", sprint_bin(data+2, 2)); + // for (uint8_t b=2; b<4; b++){ + // for (uint8_t i=0; i<8; i++){ + // PrintAndLogEx(NORMAL, " %02u:%slocked", blk1, (data[b] & (1 << i)) ? " not " : " " ); + // blk1++; + // } + // } + // break; + // case 0x2: // (SR176) + // //need data[2] + // blk1 = 0; + // PrintAndLogEx(NORMAL, " raw: %s", sprint_bin(data+2, 1)); + // for (uint8_t i = 0; i<8; i++){ + // PrintAndLogEx(NORMAL, " %02u/%02u:%slocked", blk1, blk1+1, (data[2] & (1 << i)) ? " " : " not " ); + // blk1+=2; + // } + // break; + // default: + // return rawClose(); // } return 1; } // print UID info from SRx chips (ST Microelectronics) -static void print_st_general_info(uint8_t *data, uint8_t len){ +static void print_st_general_info(uint8_t *data, uint8_t len) +{ //uid = first 8 bytes in data - PrintAndLogEx(NORMAL, " UID: %s", sprint_hex(SwapEndian64(data,8,8), len)); + PrintAndLogEx(NORMAL, " UID: %s", sprint_hex(SwapEndian64(data, 8, 8), len)); PrintAndLogEx(NORMAL, " MFG: %02X, %s", data[6], getTagInfo(data[6])); - PrintAndLogEx(NORMAL, "Chip: %02X, %s", data[5]>>2, get_ST_Chip_Model(data[5]>>2)); + PrintAndLogEx(NORMAL, "Chip: %02X, %s", data[5] >> 2, get_ST_Chip_Model(data[5] >> 2)); return; } @@ -433,7 +467,8 @@ static void print_st_general_info(uint8_t *data, uint8_t len){ //a2 = ? (resp 02 [6a d3]) // 14b get and print Full Info (as much as we know) -bool HF14B_Std_Info(bool verbose){ +bool HF14B_Std_Info(bool verbose) +{ bool isSuccess = false; @@ -446,7 +481,7 @@ bool HF14B_Std_Info(bool verbose){ if (!WaitForResponseTimeout(CMD_ACK, &resp, TIMEOUT)) { if (verbose) PrintAndLogEx(WARNING, "command execution timeout"); switch_off_field_14b(); - return false; + return false; } iso14b_card_select_t card; @@ -454,7 +489,7 @@ bool HF14B_Std_Info(bool verbose){ uint64_t status = resp.arg[0]; - switch( status ){ + switch (status) { case 0: PrintAndLogEx(NORMAL, " UID : %s", sprint_hex(card.uid, card.uidlen)); PrintAndLogEx(NORMAL, " ATQB : %s", sprint_hex(card.atqb, sizeof(card.atqb))); @@ -477,7 +512,8 @@ bool HF14B_Std_Info(bool verbose){ } // SRx get and print full info (needs more info...) -bool HF14B_ST_Info(bool verbose){ +bool HF14B_ST_Info(bool verbose) +{ UsbCommand c = {CMD_ISO_14443B_COMMAND, {ISO14B_CONNECT | ISO14B_SELECT_SR | ISO14B_DISCONNECT, 0, 0}}; clearCommandBuffer(); @@ -493,7 +529,7 @@ bool HF14B_ST_Info(bool verbose){ memcpy(&card, (iso14b_card_select_t *)resp.d.asBytes, sizeof(iso14b_card_select_t)); uint64_t status = resp.arg[0]; - if ( status > 0 ) + if (status > 0) return false; print_st_general_info(card.uid, card.uidlen); @@ -506,16 +542,16 @@ bool HF14B_ST_Info(bool verbose){ // // if (model == 0x2) { //SR176 has special command: - // data[1] = 0xf; - // resplen = 4; + // data[1] = 0xf; + // resplen = 4; // } else { - // data[1] = 0xff; - // resplen = 6; + // data[1] = 0xff; + // resplen = 6; // } // //std read cmd // if (HF14BCmdRaw(true, true, data, &datalen, false)==0) - // return rawClose(); + // return rawClose(); // if (datalen != resplen || !crc) return rawClose(); //print_ST_Lock_info(data[5]>>2); @@ -523,7 +559,8 @@ bool HF14B_ST_Info(bool verbose){ } // get and print all info known about any known 14b tag -bool HF14BInfo(bool verbose){ +bool HF14BInfo(bool verbose) +{ // try std 14b (atqb) if (HF14B_Std_Info(verbose)) return true; @@ -538,7 +575,8 @@ bool HF14BInfo(bool verbose){ } // menu command to get and print all info known about any known 14b tag -int CmdHF14Binfo(const char *Cmd){ +int CmdHF14Binfo(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_hf_14b_info(); @@ -546,7 +584,8 @@ int CmdHF14Binfo(const char *Cmd){ return HF14BInfo(verbose); } -bool HF14B_ST_Reader(bool verbose){ +bool HF14B_ST_Reader(bool verbose) +{ bool isSuccess = false; @@ -565,7 +604,7 @@ bool HF14B_ST_Reader(bool verbose){ uint64_t status = resp.arg[0]; - switch( status ){ + switch (status) { case 0: print_st_general_info(card.uid, card.uidlen); isSuccess = true; @@ -586,7 +625,8 @@ bool HF14B_ST_Reader(bool verbose){ return isSuccess; } -bool HF14B_Std_Reader(bool verbose){ +bool HF14B_Std_Reader(bool verbose) +{ bool isSuccess = false; @@ -606,7 +646,7 @@ bool HF14B_Std_Reader(bool verbose){ uint64_t status = resp.arg[0]; - switch( status ){ + switch (status) { case 0: PrintAndLogEx(NORMAL, " UID : %s", sprint_hex(card.uid, card.uidlen)); PrintAndLogEx(NORMAL, " ATQB : %s", sprint_hex(card.atqb, sizeof(card.atqb))); @@ -628,7 +668,8 @@ bool HF14B_Std_Reader(bool verbose){ } // test for other 14b type tags (mimic another reader - don't have tags to identify) -bool HF14B_Other_Reader(){ +bool HF14B_Other_Reader() +{ // uint8_t data[] = {0x00, 0x0b, 0x3f, 0x80}; // uint8_t datalen = 4; @@ -645,11 +686,11 @@ bool HF14B_Other_Reader(){ // WaitForResponse(CMD_ACK,&resp); // if (datalen > 2 ) { - // PrintAndLogEx(NORMAL, "\n14443-3b tag found:"); - // PrintAndLogEx(NORMAL, "unknown tag type answered to a 0x000b3f80 command ans:"); - // //PrintAndLogEx(NORMAL, "%s", sprint_hex(data, datalen)); - // rawclose(); - // return true; + // PrintAndLogEx(NORMAL, "\n14443-3b tag found:"); + // PrintAndLogEx(NORMAL, "unknown tag type answered to a 0x000b3f80 command ans:"); + // //PrintAndLogEx(NORMAL, "%s", sprint_hex(data, datalen)); + // rawclose(); + // return true; // } // c.arg1 = 1; @@ -660,11 +701,11 @@ bool HF14B_Other_Reader(){ // WaitForResponse(CMD_ACK, &resp); // if (datalen > 0) { - // PrintAndLogEx(NORMAL, "\n14443-3b tag found:"); - // PrintAndLogEx(NORMAL, "Unknown tag type answered to a 0x0A command ans:"); - // // PrintAndLogEx(NORMAL, "%s", sprint_hex(data, datalen)); - // rawClose(); - // return true; + // PrintAndLogEx(NORMAL, "\n14443-3b tag found:"); + // PrintAndLogEx(NORMAL, "Unknown tag type answered to a 0x0A command ans:"); + // // PrintAndLogEx(NORMAL, "%s", sprint_hex(data, datalen)); + // rawClose(); + // return true; // } // c.arg1 = 1; @@ -675,11 +716,11 @@ bool HF14B_Other_Reader(){ // WaitForResponse(CMD_ACK, &resp); // if (datalen > 0) { - // PrintAndLogEx(NORMAL, "\n14443-3b tag found:"); - // PrintAndLogEx(NORMAL, "Unknown tag type answered to a 0x0C command ans:"); - // PrintAndLogEx(NORMAL, "%s", sprint_hex(data, datalen)); - // rawClose(); - // return true; + // PrintAndLogEx(NORMAL, "\n14443-3b tag found:"); + // PrintAndLogEx(NORMAL, "Unknown tag type answered to a 0x0C command ans:"); + // PrintAndLogEx(NORMAL, "%s", sprint_hex(data, datalen)); + // rawClose(); + // return true; // } // rawClose(); @@ -687,7 +728,8 @@ bool HF14B_Other_Reader(){ } // get and print general info about all known 14b chips -bool HF14BReader(bool verbose){ +bool HF14BReader(bool verbose) +{ // try std 14b (atqb) if (HF14B_Std_Reader(verbose)) return true; @@ -704,7 +746,8 @@ bool HF14BReader(bool verbose){ } // menu command to get and print general info about all known 14b chips -int CmdHF14BReader(const char *Cmd){ +int CmdHF14BReader(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_hf_14b_reader(); @@ -716,7 +759,8 @@ int CmdHF14BReader(const char *Cmd){ * SRI* tags are ISO14443-B modulated memory tags, * this command just dumps the contents of the memory/ */ -int CmdHF14BReadSri(const char *Cmd){ +int CmdHF14BReadSri(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) < 1 || cmdp == 'h') return usage_hf_14b_read_srx(); @@ -729,17 +773,18 @@ int CmdHF14BReadSri(const char *Cmd){ return 0; } // New command to write a SRI512/SRIX4K tag. -int CmdHF14BWriteSri(const char *Cmd){ -/* - * For SRIX4K blocks 00 - 7F - * hf 14b raw -c -p 09 $srix4kwblock $srix4kwdata - * - * For SR512 blocks 00 - 0F - * hf 14b raw -c -p 09 $sr512wblock $sr512wdata - * - * Special block FF = otp_lock_reg block. - * Data len 4 bytes- - */ +int CmdHF14BWriteSri(const char *Cmd) +{ + /* + * For SRIX4K blocks 00 - 7F + * hf 14b raw -c -p 09 $srix4kwblock $srix4kwdata + * + * For SR512 blocks 00 - 0F + * hf 14b raw -c -p 09 $sr512wblock $sr512wdata + * + * Special block FF = otp_lock_reg block. + * Data len 4 bytes- + */ char cmdp = tolower(param_getchar(Cmd, 0)); uint8_t blockno = -1; uint8_t data[4] = {0x00}; @@ -749,23 +794,23 @@ int CmdHF14BWriteSri(const char *Cmd){ if (strlen(Cmd) < 1 || cmdp == 'h') return usage_hf_14b_write_srx(); - if ( cmdp == '2' ) + if (cmdp == '2') isSrix4k = false; //blockno = param_get8(Cmd, 1); - if ( param_gethex(Cmd, 1, &blockno, 2) ) { + if (param_gethex(Cmd, 1, &blockno, 2)) { PrintAndLogEx(WARNING, "block number must include 2 HEX symbols"); return 0; } - if ( isSrix4k ){ - if ( blockno > 0x7f && blockno != 0xff ){ + if (isSrix4k) { + if (blockno > 0x7f && blockno != 0xff) { PrintAndLogEx(FAILED, "block number out of range"); return 0; } } else { - if ( blockno > 0x0f && blockno != 0xff ){ + if (blockno > 0x0f && blockno != 0xff) { PrintAndLogEx(FAILED, "block number out of range"); return 0; } @@ -776,18 +821,18 @@ int CmdHF14BWriteSri(const char *Cmd){ return 0; } - if ( blockno == 0xff) { + if (blockno == 0xff) { PrintAndLogEx(SUCCESS, "[%s] Write special block %02X [ %s ]", - (isSrix4k) ? "SRIX4K":"SRI512", - blockno, - sprint_hex(data,4) - ); + (isSrix4k) ? "SRIX4K" : "SRI512", + blockno, + sprint_hex(data, 4) + ); } else { PrintAndLogEx(SUCCESS, "[%s] Write block %02X [ %s ]", - (isSrix4k) ? "SRIX4K":"SRI512", - blockno, - sprint_hex(data,4) - ); + (isSrix4k) ? "SRIX4K" : "SRI512", + blockno, + sprint_hex(data, 4) + ); } sprintf(str, "-ss -c %02x %02x %02x %02x %02x %02x", ISO14443B_WRITE_BLK, blockno, data[0], data[1], data[2], data[3]); @@ -796,11 +841,12 @@ int CmdHF14BWriteSri(const char *Cmd){ } // need to write to file -int CmdHF14BDump(const char*Cmd) { +int CmdHF14BDump(const char *Cmd) +{ uint8_t fileNameLen = 0; char filename[FILE_PATH_SIZE] = {0}; - char * fptr = filename; + char *fptr = filename; bool errors = false; uint8_t cmdp = 0, cardtype = 1; uint16_t cardsize = 0; @@ -809,35 +855,35 @@ int CmdHF14BDump(const char*Cmd) { while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': - return usage_hf_14b_dump(); - case 'f': - fileNameLen = param_getstr(Cmd, cmdp+1, filename, FILE_PATH_SIZE); - cmdp += 2; - break; - default: - if (cmdp == 0) { - cardtype = param_get8ex(Cmd, cmdp, 1, 10); - cmdp++; - } else { - PrintAndLogEx(WARNING, "Unknown parameter '%c'\n", param_getchar(Cmd, cmdp)); - errors = true; + case 'h': + return usage_hf_14b_dump(); + case 'f': + fileNameLen = param_getstr(Cmd, cmdp + 1, filename, FILE_PATH_SIZE); + cmdp += 2; break; - } + default: + if (cmdp == 0) { + cardtype = param_get8ex(Cmd, cmdp, 1, 10); + cmdp++; + } else { + PrintAndLogEx(WARNING, "Unknown parameter '%c'\n", param_getchar(Cmd, cmdp)); + errors = true; + break; + } } } //Validations if (errors) return usage_hf_14b_dump(); - switch (cardtype){ + switch (cardtype) { case 2: - cardsize = (512/8) + 4; + cardsize = (512 / 8) + 4; blocks = 0x0F; break; case 1: default: - cardsize = (4096/8) + 4; + cardsize = (4096 / 8) + 4; blocks = 0x7F; break; } @@ -891,21 +937,21 @@ int CmdHF14BDump(const char*Cmd) { if (WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { uint8_t status = resp.arg[0] & 0xFF; - if ( status > 0 ) { + if (status > 0) { continue; } uint16_t len = (resp.arg[1] & 0xFFFF); recv = resp.d.asBytes; - if ( !check_crc(CRC_14443_B, recv, len) ) { + if (!check_crc(CRC_14443_B, recv, len)) { PrintAndLogEx(FAILED, "crc fail, retrying one more time"); continue; } memcpy(data + (blocknum * 4), resp.d.asBytes, 4); - if ( blocknum == 0xFF) { + if (blocknum == 0xFF) { //last read. break; } @@ -913,16 +959,17 @@ int CmdHF14BDump(const char*Cmd) { retry = 0; blocknum++; - if ( blocknum > blocks ) { + if (blocknum > blocks) { // read config block blocknum = 0xFF; } - printf("."); fflush(stdout); + printf("."); + fflush(stdout); } } - if ( blocknum != 0xFF) { + if (blocknum != 0xFF) { PrintAndLogEx(NORMAL, "\n Dump failed"); goto out; } @@ -933,95 +980,102 @@ int CmdHF14BDump(const char*Cmd) { for (int i = 0; i <= blocks; i++) { PrintAndLogEx(NORMAL, - "%3d/0x%02X | %s | %s", - i, - i, - sprint_hex(data + (i*4), 4 ), - sprint_ascii(data + (i*4), 4) - ); + "%3d/0x%02X | %s | %s", + i, + i, + sprint_hex(data + (i * 4), 4), + sprint_ascii(data + (i * 4), 4) + ); } PrintAndLogEx(NORMAL, "\n"); - size_t datalen = (blocks+1) * 4; + size_t datalen = (blocks + 1) * 4; saveFileEML(filename, "eml", data, datalen, 4); saveFile(filename, "bin", data, datalen); out: return switch_off_field_14b(); } -uint32_t srix4kEncode(uint32_t value) { -/* -// vv = value -// pp = position -// vv vv vv pp -4 bytes : 00 1A 20 01 -*/ +uint32_t srix4kEncode(uint32_t value) +{ + /* + // vv = value + // pp = position + // vv vv vv pp + 4 bytes : 00 1A 20 01 + */ // only the lower crumbs. uint8_t block = (value & 0xFF); uint8_t i = 0; - uint8_t valuebytes[] = {0,0,0}; + uint8_t valuebytes[] = {0, 0, 0}; num_to_bytes(value, 3, valuebytes); // Scrambled part // Crumb swapping of value. - uint8_t temp[] = {0,0}; - temp[0] = (CRUMB(value, 22) << 4 | CRUMB(value, 14 ) << 2 | CRUMB(value, 6)) << 4; - temp[0] |= CRUMB(value, 20) << 4 | CRUMB(value, 12 ) << 2 | CRUMB(value, 4); - temp[1] = (CRUMB(value, 18) << 4 | CRUMB(value, 10 ) << 2 | CRUMB(value, 2)) << 4; - temp[1] |= CRUMB(value, 16) << 4 | CRUMB(value, 8 ) << 2 | CRUMB(value, 0); + uint8_t temp[] = {0, 0}; + temp[0] = (CRUMB(value, 22) << 4 | CRUMB(value, 14) << 2 | CRUMB(value, 6)) << 4; + temp[0] |= CRUMB(value, 20) << 4 | CRUMB(value, 12) << 2 | CRUMB(value, 4); + temp[1] = (CRUMB(value, 18) << 4 | CRUMB(value, 10) << 2 | CRUMB(value, 2)) << 4; + temp[1] |= CRUMB(value, 16) << 4 | CRUMB(value, 8) << 2 | CRUMB(value, 0); // chksum part uint32_t chksum = 0xFF - block; // chksum is reduced by each nibbles of value. - for (i = 0; i < 3; ++i){ + for (i = 0; i < 3; ++i) { chksum -= NIBBLE_HIGH(valuebytes[i]); chksum -= NIBBLE_LOW(valuebytes[i]); } // base4 conversion and left shift twice i = 3; - uint8_t base4[] = {0,0,0,0}; - while( chksum !=0 ){ + uint8_t base4[] = {0, 0, 0, 0}; + while (chksum != 0) { base4[i--] = (chksum % 4 << 2); chksum /= 4; } // merge scambled and chksum parts uint32_t encvalue = - ( NIBBLE_LOW ( base4[0]) << 28 ) | - ( NIBBLE_HIGH( temp[0]) << 24 ) | + (NIBBLE_LOW(base4[0]) << 28) | + (NIBBLE_HIGH(temp[0]) << 24) | - ( NIBBLE_LOW ( base4[1]) << 20 ) | - ( NIBBLE_LOW ( temp[0]) << 16 ) | + (NIBBLE_LOW(base4[1]) << 20) | + (NIBBLE_LOW(temp[0]) << 16) | - ( NIBBLE_LOW ( base4[2]) << 12 ) | - ( NIBBLE_HIGH( temp[1]) << 8 ) | + (NIBBLE_LOW(base4[2]) << 12) | + (NIBBLE_HIGH(temp[1]) << 8) | - ( NIBBLE_LOW ( base4[3]) << 4 ) | - NIBBLE_LOW ( temp[1] ); + (NIBBLE_LOW(base4[3]) << 4) | + NIBBLE_LOW(temp[1]); PrintAndLogEx(NORMAL, "ICE encoded | %08X -> %08X", value, encvalue); return encvalue; } -uint32_t srix4kDecode(uint32_t value) { - switch(value) { - case 0xC04F42C5: return 0x003139; - case 0xC1484807: return 0x002943; - case 0xC0C60848: return 0x001A20; +uint32_t srix4kDecode(uint32_t value) +{ + switch (value) { + case 0xC04F42C5: + return 0x003139; + case 0xC1484807: + return 0x002943; + case 0xC0C60848: + return 0x001A20; } return 0; } -uint32_t srix4kDecodeCounter(uint32_t num) { +uint32_t srix4kDecodeCounter(uint32_t num) +{ uint32_t value = ~num; ++value; return value; } -uint32_t srix4kGetMagicbytes( uint64_t uid, uint32_t block6, uint32_t block18, uint32_t block19 ){ +uint32_t srix4kGetMagicbytes(uint64_t uid, uint32_t block6, uint32_t block18, uint32_t block19) +{ #define MASK 0xFFFFFFFF; uint32_t uid32 = uid & MASK; uint32_t counter = srix4kDecodeCounter(block6); @@ -1033,7 +1087,8 @@ uint32_t srix4kGetMagicbytes( uint64_t uid, uint32_t block6, uint32_t block18, u PrintAndLogEx(SUCCESS, "Magic bytes | %08X", result); return result; } -int srix4kValid(const char *Cmd){ +int srix4kValid(const char *Cmd) +{ uint64_t uid = 0xD00202501A4532F9; uint32_t block6 = 0xFFFFFFFF; @@ -1044,14 +1099,15 @@ int srix4kValid(const char *Cmd){ uint32_t test_b18 = 0x00313918; uint32_t test_b18_enc = srix4kEncode(test_b18); //uint32_t test_b18_dec = srix4kDecode(test_b18_enc); - PrintAndLogEx(SUCCESS, "ENCODE & CHECKSUM | %08X -> %08X (%s)", test_b18, test_b18_enc , ""); + PrintAndLogEx(SUCCESS, "ENCODE & CHECKSUM | %08X -> %08X (%s)", test_b18, test_b18_enc, ""); uint32_t magic = srix4kGetMagicbytes(uid, block6, block18, block19); PrintAndLogEx(SUCCESS, "BLOCK 21 | %08X -> %08X (no XOR)", block21, magic ^ block21); return 0; } -bool waitCmd14b(bool verbose) { +bool waitCmd14b(bool verbose) +{ bool crc = false; uint8_t data[USB_CMD_DATA_SIZE] = {0x00}; @@ -1062,25 +1118,25 @@ bool waitCmd14b(bool verbose) { if (WaitForResponseTimeout(CMD_ACK, &resp, TIMEOUT)) { status = (resp.arg[0] & 0xFF); - if ( status > 0 ) return false; + if (status > 0) return false; len = (resp.arg[1] & 0xFFFF); memcpy(data, resp.d.asBytes, len); if (verbose) { - if ( len >= 3 ) { + if (len >= 3) { crc = check_crc(CRC_14443_B, data, len); PrintAndLogEx(NORMAL, "[LEN %u] %s[%02X %02X] %s", - len, - sprint_hex(data, len-2), - data[len-2], - data[len-1], - (crc) ? "OK" : "FAIL" - ); + len, + sprint_hex(data, len - 2), + data[len - 2], + data[len - 1], + (crc) ? "OK" : "FAIL" + ); } else { - PrintAndLogEx(NORMAL, "[LEN %u] %s", len, sprint_hex(data, len) ); + PrintAndLogEx(NORMAL, "[LEN %u] %s", len, sprint_hex(data, len)); } } return true; @@ -1105,13 +1161,15 @@ static command_t CommandTable[] = { {NULL, NULL, 0, NULL} }; -int CmdHF14B(const char *Cmd) { +int CmdHF14B(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { +int CmdHelp(const char *Cmd) +{ CmdsHelp(CommandTable); return 0; } diff --git a/client/cmdhf14b.h b/client/cmdhf14b.h index f77658c19..56114af94 100644 --- a/client/cmdhf14b.h +++ b/client/cmdhf14b.h @@ -42,14 +42,14 @@ extern int CmdHF14BList(const char *Cmd); extern int CmdHF14BInfo(const char *Cmd); extern int CmdHF14BSim(const char *Cmd); extern int CmdHF14BSniff(const char *Cmd); -extern int CmdHF14BWrite( const char *cmd); +extern int CmdHF14BWrite(const char *cmd); extern int CmdHF14BReader(const char *Cmd); extern int CmdHF14BDump(const char *Cmd); extern bool HF14BInfo(bool verbose); extern bool HF14BReader(bool verbose); -extern int CmdHF14BCmdRaw (const char *Cmd); +extern int CmdHF14BCmdRaw(const char *Cmd); // SRi ST Microelectronics read/write extern int CmdHF14BReadSri(const char *Cmd); diff --git a/client/cmdhf15.c b/client/cmdhf15.c index 9164ac70e..3a2fe28f0 100644 --- a/client/cmdhf15.c +++ b/client/cmdhf15.c @@ -39,7 +39,7 @@ typedef struct { uint64_t uid; int mask; // how many MSB bits used - char* desc; + char *desc; } productName; const productName uidmapping[] = { @@ -183,13 +183,14 @@ const productName uidmapping[] = { { 0xE042000000000000LL, 16, "3Alogics Inc Korea" }, { 0xE043000000000000LL, 16, "Top TroniQ Asia Limited Hong Kong" }, { 0xE044000000000000LL, 16, "Gentag Inc (USA) USA" }, - { 0,0,"no tag-info available" } // must be the last entry + { 0, 0, "no tag-info available" } // must be the last entry }; // fast method to just read the UID of a tag (collission detection not supported) // *buf should be large enough to fit the 64bit uid // returns 1 if suceeded -int getUID(uint8_t *buf) { +int getUID(uint8_t *buf) +{ UsbCommand resp; UsbCommand c = {CMD_ISO_15693_COMMAND, {0, 1, 1}}; // len,speed,recv? @@ -213,13 +214,13 @@ int getUID(uint8_t *buf) { uint8_t resplen = resp.arg[0]; if (resplen >= 12 && CheckCrc(resp.d.asBytes, 12)) { - memcpy(buf, resp.d.asBytes + 2, 8); - return 1; + memcpy(buf, resp.d.asBytes + 2, 8); + return 1; } } } // retry - if ( retry >= 3 ) + if (retry >= 3) PrintAndLogEx(WARNING, "timeout while waiting for reply."); return 0; @@ -228,18 +229,19 @@ int getUID(uint8_t *buf) { // get a product description based on the UID // uid[8] tag uid // returns description of the best match -static char* getTagInfo_15(uint8_t *uid) { +static char *getTagInfo_15(uint8_t *uid) +{ uint64_t myuid, mask; int i = 0, best = -1; memcpy(&myuid, uid, sizeof(uint64_t)); while (uidmapping[i].mask > 0) { - mask = (~0LL) << (64-uidmapping[i].mask); + mask = (~0LL) << (64 - uidmapping[i].mask); if ((myuid & mask) == uidmapping[i].uid) { if (best == -1) { best = i; } else { if (uidmapping[i].mask > uidmapping[best].mask) { - best=i; + best = i; } } } @@ -252,88 +254,108 @@ static char* getTagInfo_15(uint8_t *uid) { } // return a clear-text message to an errorcode -static char* TagErrorStr(uint8_t error) { +static char *TagErrorStr(uint8_t error) +{ switch (error) { - case 0x01: return "The command is not supported"; - case 0x02: return "The command is not recognised"; - case 0x03: return "The option is not supported."; - case 0x0f: return "Unknown error."; - case 0x10: return "The specified block is not available (doesn't exist)."; - case 0x11: return "The specified block is already -locked and thus cannot be locked again"; - case 0x12: return "The specified block is locked and its content cannot be changed."; - case 0x13: return "The specified block was not successfully programmed."; - case 0x14: return "The specified block was not successfully locked."; - default: return "Reserved for Future Use or Custom command error."; + case 0x01: + return "The command is not supported"; + case 0x02: + return "The command is not recognised"; + case 0x03: + return "The option is not supported."; + case 0x0f: + return "Unknown error."; + case 0x10: + return "The specified block is not available (doesn't exist)."; + case 0x11: + return "The specified block is already -locked and thus cannot be locked again"; + case 0x12: + return "The specified block is locked and its content cannot be changed."; + case 0x13: + return "The specified block was not successfully programmed."; + case 0x14: + return "The specified block was not successfully locked."; + default: + return "Reserved for Future Use or Custom command error."; } } -int usage_15_demod(void){ +int usage_15_demod(void) +{ PrintAndLogEx(NORMAL, "Tries to demodulate / decode ISO15693, from downloaded samples.\n" - "Gather samples with 'hf 15 read' / 'hf 15 record'"); + "Gather samples with 'hf 15 read' / 'hf 15 record'"); return 0; } -int usage_15_samples(void){ +int usage_15_samples(void) +{ PrintAndLogEx(NORMAL, "Acquire samples as Reader (enables carrier, send inquiry\n" - "and download it to graphbuffer. Try 'hf 15 demod' to try to demodulate/decode signal"); + "and download it to graphbuffer. Try 'hf 15 demod' to try to demodulate/decode signal"); return 0; } -int usage_15_info(void){ +int usage_15_info(void) +{ PrintAndLogEx(NORMAL, "Uses the optional command 'get_systeminfo' 0x2B to try and extract information\n" - "command may fail, depending on tag.\n" - "defaults to '1 out of 4' mode\n" - "\n" - "Usage: hf 15 info [options] \n" - "Options:\n" - "\t-2 use slower '1 out of 256' mode\n" - "\tuid (either): \n" - "\t <8B hex> full UID eg E011223344556677\n" - "\t u unaddressed mode\n" - "\t * scan for tag\n" - "Examples:\n" - "\thf 15 info u"); + "command may fail, depending on tag.\n" + "defaults to '1 out of 4' mode\n" + "\n" + "Usage: hf 15 info [options] \n" + "Options:\n" + "\t-2 use slower '1 out of 256' mode\n" + "\tuid (either): \n" + "\t <8B hex> full UID eg E011223344556677\n" + "\t u unaddressed mode\n" + "\t * scan for tag\n" + "Examples:\n" + "\thf 15 info u"); return 0; } -int usage_15_record(void){ +int usage_15_record(void) +{ PrintAndLogEx(NORMAL, "Record activity without enableing carrier"); return 0; } -int usage_15_reader(void){ +int usage_15_reader(void) +{ PrintAndLogEx(NORMAL, "This command identifies a ISO 15693 tag\n" - "\n" - "Usage: hf 15 reader [h]\n" - "Options:\n" - "\th this help\n" - "\n" - "Example:\n" - "\thf 15 reader"); + "\n" + "Usage: hf 15 reader [h]\n" + "Options:\n" + "\th this help\n" + "\n" + "Example:\n" + "\thf 15 reader"); return 0; } -int usage_15_sim(void){ +int usage_15_sim(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 15 sim \n" - "\n" - "Example:\n" - "\thf 15 sim E016240000000000"); + "\n" + "Example:\n" + "\thf 15 sim E016240000000000"); return 0; } -int usage_15_findafi(void){ +int usage_15_findafi(void) +{ PrintAndLogEx(NORMAL, "'hf 15 finafi' This command needs a helptext. Feel free to add one!"); return 0; } -int usage_15_dump(void){ +int usage_15_dump(void) +{ PrintAndLogEx(NORMAL, "This command dumps the contents of a ISO-15693 tag and save it to file\n" - "\n" - "Usage: hf 15 dump [h] \n" - "Options:\n" - "\th this help\n" - "\tf filename, if no UID will be used as filename\n" - "\n" - "Example:\n" - "\thf 15 dump f\n" - "\thf 15 dump f mydump"); + "\n" + "Usage: hf 15 dump [h] \n" + "Options:\n" + "\th this help\n" + "\tf filename, if no UID will be used as filename\n" + "\n" + "Example:\n" + "\thf 15 dump f\n" + "\thf 15 dump f mydump"); return 0; } -int usage_15_restore(void){ - char *options[][2]={ +int usage_15_restore(void) +{ + char *options[][2] = { {"h", "this help"}, {"-2", "use slower '1 out of 256' mode"}, {"-o", "set OPTION Flag (needed for TI)"}, @@ -346,8 +368,9 @@ int usage_15_restore(void){ PrintAndLogOptions(options, 7, 3); return 0; } -int usage_15_raw(void){ - char *options[][2]={ +int usage_15_raw(void) +{ + char *options[][2] = { {"-r", "do not read response" }, {"-2", "use slower '1 out of 256' mode" }, {"-c", "calculate and append CRC" }, @@ -357,45 +380,49 @@ int usage_15_raw(void){ PrintAndLogOptions(options, 4, 3); return 0; } -int usage_15_read(void){ +int usage_15_read(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 15 read [options] \n" - "Options:\n" - "\t-2 use slower '1 out of 256' mode\n" - "\tuid (either): \n" - "\t <8B hex> full UID eg E011223344556677\n" - "\t u unaddressed mode\n" - "\t * scan for tag\n" - "\tpage#: page number 0-255"); + "Options:\n" + "\t-2 use slower '1 out of 256' mode\n" + "\tuid (either): \n" + "\t <8B hex> full UID eg E011223344556677\n" + "\t u unaddressed mode\n" + "\t * scan for tag\n" + "\tpage#: page number 0-255"); return 0; } -int usage_15_write(void){ +int usage_15_write(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 15 write [options] \n" - "Options:\n" - "\t-2 use slower '1 out of 256' mode\n" - "\t-o set OPTION Flag (needed for TI)\n" - "\tuid (either): \n" - "\t <8B hex> full UID eg E011223344556677\n" - "\t u unaddressed mode\n" - "\t * scan for tag\n" - "\tpage#: page number 0-255\n" - "\thexdata: data to be written eg AA BB CC DD"); + "Options:\n" + "\t-2 use slower '1 out of 256' mode\n" + "\t-o set OPTION Flag (needed for TI)\n" + "\tuid (either): \n" + "\t <8B hex> full UID eg E011223344556677\n" + "\t u unaddressed mode\n" + "\t * scan for tag\n" + "\tpage#: page number 0-255\n" + "\thexdata: data to be written eg AA BB CC DD"); return 0; } -int usage_15_readmulti(void){ +int usage_15_readmulti(void) +{ PrintAndLogEx(NORMAL, "Usage: hf 15 readmulti [options] \n" - "Options:\n" - "\t-2 use slower '1 out of 256' mode\n" - "\tuid (either): \n" - "\t <8B hex> full UID eg E011223344556677\n" - "\t u unaddressed mode\n" - "\t * scan for tag\n" - "\tstart#: page number to start 0-255\n" - "\tcount#: number of pages"); + "Options:\n" + "\t-2 use slower '1 out of 256' mode\n" + "\tuid (either): \n" + "\t <8B hex> full UID eg E011223344556677\n" + "\t u unaddressed mode\n" + "\t * scan for tag\n" + "\tstart#: page number to start 0-255\n" + "\tcount#: number of pages"); return 0; } // Mode 3 //helptext -int CmdHF15Demod(const char *Cmd) { +int CmdHF15Demod(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_15_demod(); @@ -475,11 +502,12 @@ int CmdHF15Demod(const char *Cmd) { // * Acquire Samples as Reader (enables carrier, sends inquiry) //helptext -int CmdHF15Samples(const char *Cmd) { +int CmdHF15Samples(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_15_samples(); - UsbCommand c = {CMD_ACQUIRE_RAW_ADC_SAMPLES_ISO_15693, {0,0,0}}; + UsbCommand c = {CMD_ACQUIRE_RAW_ADC_SAMPLES_ISO_15693, {0, 0, 0}}; clearCommandBuffer(); SendCommand(&c); @@ -491,10 +519,11 @@ int CmdHF15Samples(const char *Cmd) { * Commandline handling: HF15 CMD SYSINFO * get system information from tag/VICC */ -int CmdHF15Info(const char *Cmd) { +int CmdHF15Info(const char *Cmd) +{ char cmdp = param_getchar(Cmd, 0); - if (strlen(Cmd)<1 || cmdp == 'h' || cmdp == 'H') return usage_15_info(); + if (strlen(Cmd) < 1 || cmdp == 'h' || cmdp == 'H') return usage_15_info(); UsbCommand resp; uint8_t *recv; @@ -506,7 +535,7 @@ int CmdHF15Info(const char *Cmd) { strncpy(cmd, Cmd, 99); - if ( !prepareHF15Cmd(&cmd, &c, ISO15_CMD_SYSINFO) ) + if (!prepareHF15Cmd(&cmd, &c, ISO15_CMD_SYSINFO)) return 0; AddCrc(req, c.arg[0]); @@ -517,28 +546,28 @@ int CmdHF15Info(const char *Cmd) { clearCommandBuffer(); SendCommand(&c); - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 2000) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { PrintAndLogEx(WARNING, "iso15693 card select failed"); return 1; } uint32_t status = resp.arg[0]; - if ( status < 2 ) { + if (status < 2) { PrintAndLogEx(WARNING, "iso15693 card doesn't answer to systeminfo command"); return 1; } recv = resp.d.asBytes; - if ( recv[0] & ISO15_RES_ERROR ) { + if (recv[0] & ISO15_RES_ERROR) { PrintAndLogEx(WARNING, "iso15693 card returned error %i: %s", recv[0], TagErrorStr(recv[0])); return 3; } - PrintAndLogEx(NORMAL, " UID : %s", sprintUID(NULL, recv+2)); - PrintAndLogEx(NORMAL, " TYPE : %s", getTagInfo_15(recv+2)); - PrintAndLogEx(NORMAL, " SYSINFO : %s", sprint_hex(recv, status-2)); + PrintAndLogEx(NORMAL, " UID : %s", sprintUID(NULL, recv + 2)); + PrintAndLogEx(NORMAL, " TYPE : %s", getTagInfo_15(recv + 2)); + PrintAndLogEx(NORMAL, " SYSINFO : %s", sprint_hex(recv, status - 2)); // DSFID if (recv[1] & 0x01) @@ -561,9 +590,9 @@ int CmdHF15Info(const char *Cmd) { // memory if (recv[1] & 0x04) { PrintAndLogEx(NORMAL, " - Tag provides info on memory layout (vendor dependent)"); - uint8_t blocks = recv[12]+1; + uint8_t blocks = recv[12] + 1; uint8_t size = (recv[13] & 0x1F); - PrintAndLogEx(NORMAL, " %u (or %u) bytes/blocks x %u blocks", size+1, size, blocks ); + PrintAndLogEx(NORMAL, " %u (or %u) bytes/blocks x %u blocks", size + 1, size, blocks); } else { PrintAndLogEx(NORMAL, " - Tag does not provide information on memory layout"); } @@ -573,19 +602,21 @@ int CmdHF15Info(const char *Cmd) { // Record Activity without enabeling carrier //helptext -int CmdHF15Record(const char *Cmd) { +int CmdHF15Record(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_15_record(); - UsbCommand c = {CMD_RECORD_RAW_ADC_SAMPLES_ISO_15693, {0,0,0}}; + UsbCommand c = {CMD_RECORD_RAW_ADC_SAMPLES_ISO_15693, {0, 0, 0}}; clearCommandBuffer(); SendCommand(&c); return 0; } // used with 'hf search' -int HF15Reader(const char *Cmd, bool verbose) { - uint8_t uid[8] = {0,0,0,0,0,0,0,0}; +int HF15Reader(const char *Cmd, bool verbose) +{ + uint8_t uid[8] = {0, 0, 0, 0, 0, 0, 0, 0}; if (!getUID(uid)) { if (verbose) PrintAndLogEx(WARNING, "No tag found."); return 0; @@ -596,7 +627,8 @@ int HF15Reader(const char *Cmd, bool verbose) { return 1; } -int CmdHF15Reader(const char *Cmd) { +int CmdHF15Reader(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_15_reader(); @@ -606,17 +638,18 @@ int CmdHF15Reader(const char *Cmd) { // Simulation is still not working very good // helptext -int CmdHF15Sim(const char *Cmd) { - char cmdp =tolower(param_getchar(Cmd, 0)); +int CmdHF15Sim(const char *Cmd) +{ + char cmdp = tolower(param_getchar(Cmd, 0)); if (strlen(Cmd) < 1 || cmdp == 'h') return usage_15_sim(); - uint8_t uid[8] = {0,0,0,0,0,0,0,0}; + uint8_t uid[8] = {0, 0, 0, 0, 0, 0, 0, 0}; if (param_gethex(Cmd, 0, uid, 16)) { PrintAndLogEx(WARNING, "UID must include 16 HEX symbols"); return 0; } - PrintAndLogEx(SUCCESS, "Starting simulating UID %s", sprint_hex(uid, sizeof(uid)) ); + PrintAndLogEx(SUCCESS, "Starting simulating UID %s", sprint_hex(uid, sizeof(uid))); UsbCommand c = {CMD_SIMTAG_ISO_15693, {0, 0, 0}}; memcpy(c.d.asBytes, uid, 8); @@ -628,7 +661,8 @@ int CmdHF15Sim(const char *Cmd) { // finds the AFI (Application Family Idendifier) of a card, by trying all values // (There is no standard way of reading the AFI, allthough some tags support this) // helptext -int CmdHF15Afi(const char *Cmd) { +int CmdHF15Afi(const char *Cmd) +{ char cmdp = tolower(param_getchar(Cmd, 0)); if (cmdp == 'h') return usage_15_findafi(); @@ -647,27 +681,28 @@ typedef struct { // Reads all memory pages // need to write to file -int CmdHF15Dump(const char*Cmd) { +int CmdHF15Dump(const char *Cmd) +{ uint8_t fileNameLen = 0; char filename[FILE_PATH_SIZE] = {0}; - char * fptr = filename; + char *fptr = filename; bool errors = false; uint8_t cmdp = 0; - uint8_t uid[8] = {0,0,0,0,0,0,0,0}; + uint8_t uid[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - while(param_getchar(Cmd, cmdp) != 0x00 && !errors) { + while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { switch (tolower(param_getchar(Cmd, cmdp))) { - case 'h': - return usage_15_dump(); - case 'f': - fileNameLen = param_getstr(Cmd, cmdp+1, filename, FILE_PATH_SIZE); - cmdp += 2; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'\n", param_getchar(Cmd, cmdp)); - errors = true; - break; + case 'h': + return usage_15_dump(); + case 'f': + fileNameLen = param_getstr(Cmd, cmdp + 1, filename, FILE_PATH_SIZE); + cmdp += 2; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'\n", param_getchar(Cmd, cmdp)); + errors = true; + break; } } @@ -684,11 +719,11 @@ int CmdHF15Dump(const char*Cmd) { } fptr += sprintf(fptr, "hf-15-"); - FillFileNameByUID(fptr,uid,"-dump",sizeof(uid)); + FillFileNameByUID(fptr, uid, "-dump", sizeof(uid)); } // detect blocksize from card :) - PrintAndLogEx(NORMAL, "Reading memory from tag UID " _YELLOW_(%s), sprintUID(NULL, uid)); + PrintAndLogEx(NORMAL, "Reading memory from tag UID " _YELLOW_( % s), sprintUID(NULL, uid)); int blocknum = 0; uint8_t *recv = NULL; @@ -696,7 +731,7 @@ int CmdHF15Dump(const char*Cmd) { // memory. t15memory mem[256]; - uint8_t data[256*4] = {0}; + uint8_t data[256 * 4] = {0}; memset(data, 0, sizeof(data)); UsbCommand resp; @@ -706,7 +741,7 @@ int CmdHF15Dump(const char*Cmd) { req[1] = ISO15_CMD_READ; // copy uid to read command - memcpy(req+2, uid, sizeof(uid)); + memcpy(req + 2, uid, sizeof(uid)); for (int retry = 0; retry < 5; retry++) { @@ -720,20 +755,20 @@ int CmdHF15Dump(const char*Cmd) { if (WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { uint8_t len = resp.arg[0]; - if ( len < 2 ) { + if (len < 2) { PrintAndLogEx(FAILED, "iso15693 card select failed"); continue; } recv = resp.d.asBytes; - if ( !CheckCrc(recv, len) ) { + if (!CheckCrc(recv, len)) { PrintAndLogEx(FAILED, "crc fail"); continue; } if (recv[0] & ISO15_RES_ERROR) { - PrintAndLogEx(FAILED, "Tag returned Error %i: %s", recv[1], TagErrorStr(recv[1]) ); + PrintAndLogEx(FAILED, "Tag returned Error %i: %s", recv[1], TagErrorStr(recv[1])); break; } @@ -744,7 +779,8 @@ int CmdHF15Dump(const char*Cmd) { retry = 0; blocknum++; - printf("."); fflush(stdout); + printf("."); + fflush(stdout); } } PrintAndLogEx(NORMAL, "\n"); @@ -752,7 +788,7 @@ int CmdHF15Dump(const char*Cmd) { PrintAndLogEx(NORMAL, "block# | data |lck| ascii"); PrintAndLogEx(NORMAL, "---------+--------------+---+----------"); for (int i = 0; i < blocknum; i++) { - PrintAndLogEx(NORMAL, "%3d/0x%02X | %s | %d | %s", i, i, sprint_hex(mem[i].block, 4 ), mem[i].lock, sprint_ascii(mem[i].block, 4) ); + PrintAndLogEx(NORMAL, "%3d/0x%02X | %s | %d | %s", i, i, sprint_hex(mem[i].block, 4), mem[i].lock, sprint_ascii(mem[i].block, 4)); } PrintAndLogEx(NORMAL, "\n"); @@ -762,53 +798,53 @@ int CmdHF15Dump(const char*Cmd) { return 0; } -int CmdHF15Restore(const char*Cmd) { +int CmdHF15Restore(const char *Cmd) +{ FILE *f; - uint8_t uid[8]={0x00}; + uint8_t uid[8] = {0x00}; char filename[FILE_PATH_SIZE] = {0x00}; char buff[255] = {0x00}; - size_t blocksize=4; + size_t blocksize = 4; uint8_t cmdp = 0; char newCmdPrefix[255] = {0x00}, tmpCmd[255] = {0x00}; - char param[FILE_PATH_SIZE]=""; - char hex[255]=""; + char param[FILE_PATH_SIZE] = ""; + char hex[255] = ""; uint8_t retries = 3, tried = 0, i = 0; - int retval=0; + int retval = 0; size_t bytes_read; - while(param_getchar(Cmd, cmdp) != 0x00) { - switch(tolower(param_getchar(Cmd, cmdp))) { + while (param_getchar(Cmd, cmdp) != 0x00) { + switch (tolower(param_getchar(Cmd, cmdp))) { case '-': param_getstr(Cmd, cmdp, param, sizeof(param)); - switch(param[1]) - { + switch (param[1]) { case '2': case 'o': - strncpy(newCmdPrefix, " ", sizeof(newCmdPrefix)-1); - strncat(newCmdPrefix, param, sizeof(newCmdPrefix)-1); - break; + strncpy(newCmdPrefix, " ", sizeof(newCmdPrefix) - 1); + strncat(newCmdPrefix, param, sizeof(newCmdPrefix) - 1); + break; default: PrintAndLogEx(WARNING, "Unknown parameter '%s'", param); return usage_15_restore(); } break; case 'f': - param_getstr(Cmd, cmdp+1, filename, FILE_PATH_SIZE); + param_getstr(Cmd, cmdp + 1, filename, FILE_PATH_SIZE); cmdp++; break; case 'r': - retries=param_get8ex(Cmd, cmdp+1, 3, 10); + retries = param_get8ex(Cmd, cmdp + 1, 3, 10); cmdp++; break; case 'b': - blocksize=param_get8ex(Cmd, cmdp+1, 4, 10); + blocksize = param_get8ex(Cmd, cmdp + 1, 4, 10); cmdp++; break; case 'u': - param_getstr(Cmd, cmdp+1, buff, FILE_PATH_SIZE); + param_getstr(Cmd, cmdp + 1, buff, FILE_PATH_SIZE); cmdp++; - snprintf(filename,sizeof(filename),"hf-15-dump-%s-bin",buff); + snprintf(filename, sizeof(filename), "hf-15-dump-%s-bin", buff); break; case 'h': return usage_15_restore(); @@ -819,10 +855,10 @@ int CmdHF15Restore(const char*Cmd) { cmdp++; } - PrintAndLogEx(INFO,"Blocksize: %u",blocksize); + PrintAndLogEx(INFO, "Blocksize: %u", blocksize); - if ( !strlen(filename)) { - PrintAndLogEx(WARNING,"Please provide a filename"); + if (!strlen(filename)) { + PrintAndLogEx(WARNING, "Please provide a filename"); return usage_15_restore(); } @@ -842,32 +878,32 @@ int CmdHF15Restore(const char*Cmd) { hex[0] = 0x00; tmpCmd[0] = 0x00; - bytes_read = fread( buff, 1, blocksize, f ); - if ( bytes_read == 0) { + bytes_read = fread(buff, 1, blocksize, f); + if (bytes_read == 0) { PrintAndLogEx(SUCCESS, "File reading done `%s`", filename); fclose(f); return 0; - } else if ( bytes_read != blocksize) { + } else if (bytes_read != blocksize) { PrintAndLogEx(WARNING, "File reading error (%s), %u bytes read instead of %u bytes.", filename, bytes_read, blocksize); fclose(f); return 2; } - for(int j=0; j < blocksize; j++) - snprintf(hex+j*2, 3, "%02X", buff[j]); + for (int j = 0; j < blocksize; j++) + snprintf(hex + j * 2, 3, "%02X", buff[j]); - for(int j=0; j < sizeof(uid)/sizeof(uid[0]); j++) - snprintf(buff+j*2,3,"%02X", uid[j]); + for (int j = 0; j < sizeof(uid) / sizeof(uid[0]); j++) + snprintf(buff + j * 2, 3, "%02X", uid[j]); //TODO: Addressed mode currently not work //snprintf(tmpCmd, sizeof(tmpCmd), "%s %s %d %s", newCmdPrefix, buff, i, hex); snprintf(tmpCmd, sizeof(tmpCmd), "%s u %u %s", newCmdPrefix, i, hex); PrintAndLogEx(DEBUG, "Command to be sent| %s", tmpCmd); - for(tried=0; tried < retries; tried++) - if(!(retval = CmdHF15Write(tmpCmd))) + for (tried = 0; tried < retries; tried++) + if (!(retval = CmdHF15Write(tmpCmd))) break; - if(tried >= retries) + if (tried >= retries) return retval; i++; @@ -875,16 +911,18 @@ int CmdHF15Restore(const char*Cmd) { fclose(f); } -int CmdHF15List(const char *Cmd) { +int CmdHF15List(const char *Cmd) +{ //PrintAndLogEx(WARNING, "Deprecated command, use 'hf list 15' instead"); CmdTraceList("15"); return 0; } -int CmdHF15Raw(const char *Cmd) { +int CmdHF15Raw(const char *Cmd) +{ char cmdp = param_getchar(Cmd, 0); - if (strlen(Cmd)<3 || cmdp == 'h' || cmdp == 'H') return usage_15_raw(); + if (strlen(Cmd) < 3 || cmdp == 'h' || cmdp == 'H') return usage_15_raw(); UsbCommand resp; UsbCommand c = {CMD_ISO_15693_COMMAND, {0, 1, 1}}; // len,speed,recv? @@ -895,12 +933,12 @@ int CmdHF15Raw(const char *Cmd) { uint32_t datalen = 0, temp; // strip - while (*Cmd==' ' || *Cmd=='\t') Cmd++; + while (*Cmd == ' ' || *Cmd == '\t') Cmd++; - while (Cmd[i]!='\0') { - if (Cmd[i]==' ' || Cmd[i]=='\t') { i++; continue; } - if (Cmd[i]=='-') { - switch (Cmd[i+1]) { + while (Cmd[i] != '\0') { + if (Cmd[i] == ' ' || Cmd[i] == '\t') { i++; continue; } + if (Cmd[i] == '-') { + switch (Cmd[i + 1]) { case 'r': case 'R': reply = 0; @@ -916,13 +954,13 @@ int CmdHF15Raw(const char *Cmd) { PrintAndLogEx(WARNING, "Invalid option"); return 0; } - i+=2; + i += 2; continue; } - if ((Cmd[i]>='0' && Cmd[i]<='9') || - (Cmd[i]>='a' && Cmd[i]<='f') || - (Cmd[i]>='A' && Cmd[i]<='F') ) { - buf[strlen(buf)+1] = 0; + if ((Cmd[i] >= '0' && Cmd[i] <= '9') || + (Cmd[i] >= 'a' && Cmd[i] <= 'f') || + (Cmd[i] >= 'A' && Cmd[i] <= 'F')) { + buf[strlen(buf) + 1] = 0; buf[strlen(buf)] = Cmd[i]; i++; @@ -955,7 +993,7 @@ int CmdHF15Raw(const char *Cmd) { if (WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { uint8_t len = resp.arg[0]; PrintAndLogEx(NORMAL, "received %i octets", len); - PrintAndLogEx(NORMAL, "%s", sprint_hex(resp.d.asBytes, len) ); + PrintAndLogEx(NORMAL, "%s", sprint_hex(resp.d.asBytes, len)); } else { PrintAndLogEx(WARNING, "timeout while waiting for reply."); } @@ -968,14 +1006,15 @@ int CmdHF15Raw(const char *Cmd) { * Parameters: * **cmd command line */ -int prepareHF15Cmd(char **cmd, UsbCommand *c, uint8_t iso15cmd) { +int prepareHF15Cmd(char **cmd, UsbCommand *c, uint8_t iso15cmd) +{ int temp; uint8_t *req = c->d.asBytes; uint8_t uid[8] = {0x00}; uint32_t reqlen = 0; // strip - while (**cmd==' ' || **cmd=='\t') (*cmd)++; + while (**cmd == ' ' || **cmd == '\t')(*cmd)++; if (strstr(*cmd, "-2") == *cmd) { c->arg[1] = 0; // use 1of256 @@ -983,7 +1022,7 @@ int prepareHF15Cmd(char **cmd, UsbCommand *c, uint8_t iso15cmd) { } // strip - while (**cmd==' ' || **cmd=='\t') (*cmd)++; + while (**cmd == ' ' || **cmd == '\t')(*cmd)++; if (strstr(*cmd, "-o") == *cmd) { req[reqlen] = ISO15_REQ_OPTION; @@ -991,7 +1030,7 @@ int prepareHF15Cmd(char **cmd, UsbCommand *c, uint8_t iso15cmd) { } // strip - while (**cmd == ' ' || **cmd == '\t') (*cmd)++; + while (**cmd == ' ' || **cmd == '\t')(*cmd)++; switch (**cmd) { case 0: @@ -1022,9 +1061,9 @@ int prepareHF15Cmd(char **cmd, UsbCommand *c, uint8_t iso15cmd) { req[reqlen++] = iso15cmd; // parse UID - for (int i=0; i<8 && (*cmd)[i*2] && (*cmd)[i*2+1]; i++) { - sscanf((char[]){(*cmd)[i*2], (*cmd)[i*2+1],0}, "%X", &temp); - uid[7-i] = temp & 0xff; + for (int i = 0; i < 8 && (*cmd)[i * 2] && (*cmd)[i * 2 + 1]; i++) { + sscanf((char[]) {(*cmd)[i * 2], (*cmd)[i * 2 + 1], 0}, "%X", &temp); + uid[7 - i] = temp & 0xff; } PrintAndLogEx(NORMAL, "Using UID %s", sprintUID(NULL, uid)); @@ -1033,9 +1072,9 @@ int prepareHF15Cmd(char **cmd, UsbCommand *c, uint8_t iso15cmd) { break; } // skip to next space - while (**cmd!=' ' && **cmd!='\t') (*cmd)++; + while (**cmd != ' ' && **cmd != '\t')(*cmd)++; // skip over the space - while (**cmd==' ' || **cmd=='\t') (*cmd)++; + while (**cmd == ' ' || **cmd == '\t')(*cmd)++; c->arg[0] = reqlen; return 1; @@ -1045,10 +1084,11 @@ int prepareHF15Cmd(char **cmd, UsbCommand *c, uint8_t iso15cmd) { * Commandline handling: HF15 CMD READMULTI * Read multiple blocks at once (not all tags support this) */ -int CmdHF15Readmulti(const char *Cmd) { +int CmdHF15Readmulti(const char *Cmd) +{ char cmdp = param_getchar(Cmd, 0); - if (strlen(Cmd)<3 || cmdp == 'h' || cmdp == 'H') return usage_15_readmulti(); + if (strlen(Cmd) < 3 || cmdp == 'h' || cmdp == 'H') return usage_15_readmulti(); UsbCommand resp; uint8_t *recv; @@ -1060,7 +1100,7 @@ int CmdHF15Readmulti(const char *Cmd) { char *cmd = cmdbuf; strncpy(cmd, Cmd, 99); - if ( !prepareHF15Cmd(&cmd, &c, ISO15_CMD_READMULTI) ) + if (!prepareHF15Cmd(&cmd, &c, ISO15_CMD_READMULTI)) return 0; // add OPTION flag, in order to get lock-info @@ -1081,18 +1121,18 @@ int CmdHF15Readmulti(const char *Cmd) { req[reqlen++] = pagenum; req[reqlen++] = pagecount; AddCrc(req, reqlen); - c.arg[0] = reqlen+2; + c.arg[0] = reqlen + 2; clearCommandBuffer(); SendCommand(&c); - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 2000) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { PrintAndLogEx(FAILED, "iso15693 card select failed"); return 1; } uint32_t status = resp.arg[0]; - if ( status < 2 ) { + if (status < 2) { PrintAndLogEx(FAILED, "iso15693 card select failed"); return 1; } @@ -1104,20 +1144,20 @@ int CmdHF15Readmulti(const char *Cmd) { return 2; } - if ( recv[0] & ISO15_RES_ERROR ) { + if (recv[0] & ISO15_RES_ERROR) { PrintAndLogEx(FAILED, "iso15693 card returned error %i: %s", recv[0], TagErrorStr(recv[0])); return 3; } int start = 1; // skip status byte - int stop = (pagecount+1) * 5; + int stop = (pagecount + 1) * 5; int currblock = pagenum; // print response PrintAndLogEx(NORMAL, ""); PrintAndLogEx(NORMAL, "block# | data |lck| ascii"); PrintAndLogEx(NORMAL, "---------+--------------+---+----------"); for (int i = start; i < stop; i += 5) { - PrintAndLogEx(NORMAL, "%3d/0x%02X | %s | %d | %s", currblock, currblock, sprint_hex(recv+i+1, 4 ), recv[i], sprint_ascii(recv+i+1, 4) ); + PrintAndLogEx(NORMAL, "%3d/0x%02X | %s | %d | %s", currblock, currblock, sprint_hex(recv + i + 1, 4), recv[i], sprint_ascii(recv + i + 1, 4)); currblock++; } @@ -1128,10 +1168,11 @@ int CmdHF15Readmulti(const char *Cmd) { * Commandline handling: HF15 CMD READ * Reads a single Block */ -int CmdHF15Read(const char *Cmd) { +int CmdHF15Read(const char *Cmd) +{ char cmdp = param_getchar(Cmd, 0); - if (strlen(Cmd)<3 || cmdp == 'h' || cmdp == 'H') return usage_15_read(); + if (strlen(Cmd) < 3 || cmdp == 'h' || cmdp == 'H') return usage_15_read(); UsbCommand resp; uint8_t *recv; @@ -1147,7 +1188,7 @@ int CmdHF15Read(const char *Cmd) { char *cmd = cmdbuf; strncpy(cmd, Cmd, 99); - if ( !prepareHF15Cmd(&cmd, &c, ISO15_CMD_READ) ) + if (!prepareHF15Cmd(&cmd, &c, ISO15_CMD_READ)) return 0; // add OPTION flag, in order to get lock-info @@ -1161,39 +1202,39 @@ int CmdHF15Read(const char *Cmd) { AddCrc(req, reqlen); - c.arg[0] = reqlen+2; + c.arg[0] = reqlen + 2; clearCommandBuffer(); SendCommand(&c); - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 2000) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { PrintAndLogEx(NORMAL, "iso15693 card select failed"); return 1; } uint32_t status = resp.arg[0]; - if ( status < 2 ) { + if (status < 2) { PrintAndLogEx(NORMAL, "iso15693 card select failed"); return 1; } recv = resp.d.asBytes; - if ( !CheckCrc(recv, status) ) { + if (!CheckCrc(recv, status)) { PrintAndLogEx(NORMAL, "CRC failed"); return 2; } - if ( recv[0] & ISO15_RES_ERROR ) { + if (recv[0] & ISO15_RES_ERROR) { PrintAndLogEx(WARNING, "iso15693 card returned error %i: %s", recv[0], TagErrorStr(recv[0])); return 3; } // print response PrintAndLogEx(NORMAL, ""); - PrintAndLogEx(NORMAL, "block #%3d |lck| ascii", blocknum ); - PrintAndLogEx(NORMAL, "------------+---+------" ); - PrintAndLogEx(NORMAL, "%s| %d | %s", sprint_hex(recv+2, status-4), recv[1], sprint_ascii(recv+2, status-4) ); + PrintAndLogEx(NORMAL, "block #%3d |lck| ascii", blocknum); + PrintAndLogEx(NORMAL, "------------+---+------"); + PrintAndLogEx(NORMAL, "%s| %d | %s", sprint_hex(recv + 2, status - 4), recv[1], sprint_ascii(recv + 2, status - 4)); PrintAndLogEx(NORMAL, ""); return 0; } @@ -1202,10 +1243,11 @@ int CmdHF15Read(const char *Cmd) { * Commandline handling: HF15 CMD WRITE * Writes a single Block - might run into timeout, even when successful */ -int CmdHF15Write(const char *Cmd) { +int CmdHF15Write(const char *Cmd) +{ char cmdp = param_getchar(Cmd, 0); - if (strlen(Cmd)<3 || cmdp == 'h' || cmdp == 'H') return usage_15_write(); + if (strlen(Cmd) < 3 || cmdp == 'h' || cmdp == 'H') return usage_15_write(); UsbCommand resp; uint8_t *recv; @@ -1218,14 +1260,14 @@ int CmdHF15Write(const char *Cmd) { strncpy(cmd, Cmd, 99); - if ( !prepareHF15Cmd(&cmd, &c, ISO15_CMD_WRITE) ) + if (!prepareHF15Cmd(&cmd, &c, ISO15_CMD_WRITE)) return 0; reqlen = c.arg[0]; // *cmd -> page num ; *cmd2 -> data - cmd2=cmd; - while (*cmd2!=' ' && *cmd2!='\t' && *cmd2) cmd2++; + cmd2 = cmd; + while (*cmd2 != ' ' && *cmd2 != '\t' && *cmd2) cmd2++; *cmd2 = 0; cmd2++; @@ -1234,41 +1276,41 @@ int CmdHF15Write(const char *Cmd) { req[reqlen++] = (uint8_t)pagenum; while (cmd2[0] && cmd2[1]) { // hexdata, read by 2 hexchars - if (*cmd2==' ') { + if (*cmd2 == ' ') { cmd2++; continue; } - sscanf((char[]){cmd2[0],cmd2[1],0},"%X",&temp); - req[reqlen++]=temp & 0xff; - cmd2+=2; + sscanf((char[]) {cmd2[0], cmd2[1], 0}, "%X", &temp); + req[reqlen++] = temp & 0xff; + cmd2 += 2; } AddCrc(req, reqlen); - c.arg[0] = reqlen+2; + c.arg[0] = reqlen + 2; PrintAndLogEx(NORMAL, "iso15693 writing to page %02d (0x%02X) | data ", pagenum, pagenum); clearCommandBuffer(); SendCommand(&c); - if ( !WaitForResponseTimeout(CMD_ACK, &resp, 2000) ) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { PrintAndLogEx(FAILED, "iso15693 card timeout, data may be written anyway"); return 1; } uint32_t status = resp.arg[0]; - if ( status < 2 ) { + if (status < 2) { PrintAndLogEx(FAILED, "iso15693 card select failed"); return 1; } recv = resp.d.asBytes; - if ( !CheckCrc(recv, status) ) { + if (!CheckCrc(recv, status)) { PrintAndLogEx(FAILED, "CRC failed"); return 2; } - if ( recv[0] & ISO15_RES_ERROR ) { + if (recv[0] & ISO15_RES_ERROR) { PrintAndLogEx(NORMAL, "iso15693 card returned error %i: %s", recv[0], TagErrorStr(recv[0])); return 3; } @@ -1296,13 +1338,15 @@ static command_t CommandTable15[] = { {NULL, NULL, 0, NULL} }; -int CmdHF15(const char *Cmd) { +int CmdHF15(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable15, Cmd); return 0; } -int CmdHF15Help(const char *Cmd) { +int CmdHF15Help(const char *Cmd) +{ CmdsHelp(CommandTable15); return 0; } \ No newline at end of file diff --git a/client/cmdhf15.h b/client/cmdhf15.h index 38f4b7801..942fd2952 100644 --- a/client/cmdhf15.h +++ b/client/cmdhf15.h @@ -37,13 +37,13 @@ extern int CmdHF15Record(const char *Cmd); extern int CmdHF15Reader(const char *Cmd); extern int CmdHF15Sim(const char *Cmd); extern int CmdHF15Afi(const char *Cmd); -extern int CmdHF15Dump(const char*Cmd); +extern int CmdHF15Dump(const char *Cmd); extern int CmdHF15Raw(const char *cmd); extern int CmdHF15Readmulti(const char *Cmd); extern int CmdHF15Read(const char *Cmd); extern int CmdHF15Write(const char *Cmd); -extern int CmdHF15Help(const char*Cmd); +extern int CmdHF15Help(const char *Cmd); // usages extern int usage_15_demod(void); diff --git a/client/cmdhfepa.c b/client/cmdhfepa.c index be8771b86..3c2299d7f 100644 --- a/client/cmdhfepa.c +++ b/client/cmdhfepa.c @@ -28,7 +28,7 @@ int CmdHFEPACollectPACENonces(const char *Cmd) n = n > 0 ? n : 1; PrintAndLogEx(NORMAL, "Collecting %u %u byte nonces", n, m); - PrintAndLogEx(NORMAL, "Start: %" PRIu64, msclock()/1000); + PrintAndLogEx(NORMAL, "Start: %" PRIu64, msclock() / 1000); // repeat n times for (uint32_t i = 0; i < n; i++) { // execute PACE @@ -36,15 +36,15 @@ int CmdHFEPACollectPACENonces(const char *Cmd) clearCommandBuffer(); SendCommand(&c); UsbCommand resp; - WaitForResponse(CMD_ACK,&resp); + WaitForResponse(CMD_ACK, &resp); // check if command failed if (resp.arg[0] != 0) { - PrintAndLogEx(FAILED, "Error in step %d, Return code: %d",resp.arg[0],(int)resp.arg[1]); + PrintAndLogEx(FAILED, "Error in step %d, Return code: %d", resp.arg[0], (int)resp.arg[1]); } else { size_t nonce_length = resp.arg[1]; char *nonce = (char *) calloc(2 * nonce_length + 1, sizeof(uint8_t)); - for(int j = 0; j < nonce_length; j++) { + for (int j = 0; j < nonce_length; j++) { sprintf(nonce + (2 * j), "%02X", resp.d.asBytes[j]); } // print nonce @@ -55,7 +55,7 @@ int CmdHFEPACollectPACENonces(const char *Cmd) sleep(d); } } - PrintAndLogEx(NORMAL, "End: %" PRIu64, msclock()/1000); + PrintAndLogEx(NORMAL, "End: %" PRIu64, msclock() / 1000); return 1; } @@ -83,7 +83,7 @@ int CmdHFEPAPACEReplay(const char *Cmd) while (Cmd[skip] != ' ' && Cmd[skip] != '\0') { // convert scan_return = sscanf(Cmd + skip, "%2X%n", - (unsigned int *) (apdus[i] + apdu_lengths[i]), + (unsigned int *)(apdus[i] + apdu_lengths[i]), &skip_add); if (scan_return < 1) { PrintAndLogEx(NORMAL, (char *)usage_msg); @@ -169,12 +169,14 @@ static command_t CommandTable[] = { {NULL, NULL, 0, NULL} }; -int CmdHelp(const char *Cmd) { +int CmdHelp(const char *Cmd) +{ CmdsHelp(CommandTable); return 0; } -int CmdHFEPA(const char *Cmd) { +int CmdHFEPA(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; diff --git a/client/cmdhffelica.c b/client/cmdhffelica.c index 615f51049..06da1b8b4 100644 --- a/client/cmdhffelica.c +++ b/client/cmdhffelica.c @@ -11,7 +11,8 @@ static int CmdHelp(const char *Cmd); -int usage_hf_felica_sim(void) { +int usage_hf_felica_sim(void) +{ PrintAndLogEx(NORMAL, "\n Emulating ISO/18092 FeliCa tag \n"); PrintAndLogEx(NORMAL, "Usage: hf felica sim [h] t [v]"); PrintAndLogEx(NORMAL, "Options:"); @@ -23,7 +24,8 @@ int usage_hf_felica_sim(void) { PrintAndLogEx(NORMAL, " hf felica sim t 1 "); return 0; } -int usage_hf_felica_sniff(void){ +int usage_hf_felica_sniff(void) +{ PrintAndLogEx(NORMAL, "It get data from the field and saves it into command buffer."); PrintAndLogEx(NORMAL, "Buffer accessible from command 'hf list felica'"); PrintAndLogEx(NORMAL, "Usage: hf felica sniff "); @@ -33,7 +35,8 @@ int usage_hf_felica_sniff(void){ PrintAndLogEx(NORMAL, " hf felica sniff s 1000"); return 0; } -int usage_hf_felica_simlite(void) { +int usage_hf_felica_simlite(void) +{ PrintAndLogEx(NORMAL, "\n Emulating ISO/18092 FeliCa Lite tag \n"); PrintAndLogEx(NORMAL, "Usage: hf felica litesim [h] u "); PrintAndLogEx(NORMAL, "Options:"); @@ -43,7 +46,8 @@ int usage_hf_felica_simlite(void) { PrintAndLogEx(NORMAL, " hf felica litesim 11223344556677"); return 0; } -int usage_hf_felica_dumplite(void) { +int usage_hf_felica_dumplite(void) +{ PrintAndLogEx(NORMAL, "\n Dump ISO/18092 FeliCa Lite tag \n"); PrintAndLogEx(NORMAL, "press button to abort run, otherwise it will loop for 200sec."); PrintAndLogEx(NORMAL, "Usage: hf felica litedump [h]"); @@ -53,7 +57,8 @@ int usage_hf_felica_dumplite(void) { PrintAndLogEx(NORMAL, " hf felica litedump"); return 0; } -int usage_hf_felica_raw(void){ +int usage_hf_felica_raw(void) +{ PrintAndLogEx(NORMAL, "Usage: hf felica raw [-h] [-r] [-c] [-p] [-a] <0A 0B 0C ... hex>"); PrintAndLogEx(NORMAL, " -h this help"); PrintAndLogEx(NORMAL, " -r do not read response"); @@ -64,13 +69,15 @@ int usage_hf_felica_raw(void){ return 0; } -int CmdHFFelicaList(const char *Cmd) { +int CmdHFFelicaList(const char *Cmd) +{ //PrintAndLogEx(NORMAL, "Deprecated command, use 'hf list felica' instead"); CmdTraceList("felica"); return 0; } -int CmdHFFelicaReader(const char *Cmd) { +int CmdHFFelicaReader(const char *Cmd) +{ bool silent = (Cmd[0] == 's' || Cmd[0] == 'S'); //UsbCommand cDisconnect = {CMD_FELICA_COMMAND, {0,0,0}}; UsbCommand c = {CMD_FELICA_COMMAND, {FELICA_CONNECT, 0, 0}}; @@ -87,7 +94,7 @@ int CmdHFFelicaReader(const char *Cmd) { memcpy(&card, (felica_card_select_t *)resp.d.asBytes, sizeof(felica_card_select_t)); uint64_t status = resp.arg[0]; - switch(status) { + switch (status) { case 1: { if (!silent) PrintAndLogEx(WARNING, "card timeout"); @@ -122,12 +129,13 @@ int CmdHFFelicaReader(const char *Cmd) { } // simulate iso18092 / FeliCa tag -int CmdHFFelicaSim(const char *Cmd) { +int CmdHFFelicaSim(const char *Cmd) +{ bool errors = false; uint8_t flags = 0; uint8_t tagtype = 1; uint8_t cmdp = 0; - uint8_t uid[10] = {0,0,0,0,0,0,0,0,0,0}; + uint8_t uid[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int uidlen = 0; bool verbose = false; @@ -139,7 +147,7 @@ int CmdHFFelicaSim(const char *Cmd) { case 't': case 'T': // Retrieve the tag type - tagtype = param_get8ex(Cmd, cmdp+1, 0, 10); + tagtype = param_get8ex(Cmd, cmdp + 1, 0, 10); if (tagtype == 0) errors = true; cmdp += 2; @@ -147,9 +155,9 @@ int CmdHFFelicaSim(const char *Cmd) { case 'u': case 'U': // Retrieve the full 4,7,10 byte long uid - param_gethex_ex(Cmd, cmdp+1, uid, &uidlen); + param_gethex_ex(Cmd, cmdp + 1, uid, &uidlen); if (!errors) { - PrintAndLogEx(NORMAL, "Emulating ISO18092/FeliCa tag with %d byte UID (%s)", uidlen>>1, sprint_hex(uid, uidlen>>1)); + PrintAndLogEx(NORMAL, "Emulating ISO18092/FeliCa tag with %d byte UID (%s)", uidlen >> 1, sprint_hex(uid, uidlen >> 1)); } cmdp += 2; break; @@ -166,53 +174,54 @@ int CmdHFFelicaSim(const char *Cmd) { PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); errors = true; break; - } + } } //Validations if (errors || cmdp == 0) return usage_hf_felica_sim(); - UsbCommand c = {CMD_FELICA_SIMULATE_TAG,{ tagtype, flags, 0 }}; - memcpy(c.d.asBytes, uid, uidlen>>1); + UsbCommand c = {CMD_FELICA_SIMULATE_TAG, { tagtype, flags, 0 }}; + memcpy(c.d.asBytes, uid, uidlen >> 1); clearCommandBuffer(); SendCommand(&c); UsbCommand resp; - if ( verbose ) + if (verbose) PrintAndLogEx(NORMAL, "Press pm3-button to abort simulation"); - while( !ukbhit() ){ - if (!WaitForResponseTimeout(CMD_ACK, &resp, 1500) ) continue; + while (!ukbhit()) { + if (!WaitForResponseTimeout(CMD_ACK, &resp, 1500)) continue; } return 0; } -int CmdHFFelicaSniff(const char *Cmd) { +int CmdHFFelicaSniff(const char *Cmd) +{ uint8_t cmdp = 0; uint64_t samples2skip = 0; uint64_t triggers2skip = 0; bool errors = false; - while(param_getchar(Cmd, cmdp) != 0x00 && !errors) { - switch(param_getchar(Cmd, cmdp)) { - case 'h': - case 'H': - return usage_hf_felica_sniff(); - case 's': - case 'S': - samples2skip = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - case 't': - case 'T': - triggers2skip = param_get32ex(Cmd, cmdp+1, 0, 10); - cmdp += 2; - break; - default: - PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); - errors = true; - break; + while (param_getchar(Cmd, cmdp) != 0x00 && !errors) { + switch (param_getchar(Cmd, cmdp)) { + case 'h': + case 'H': + return usage_hf_felica_sniff(); + case 's': + case 'S': + samples2skip = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + case 't': + case 'T': + triggers2skip = param_get32ex(Cmd, cmdp + 1, 0, 10); + cmdp += 2; + break; + default: + PrintAndLogEx(WARNING, "Unknown parameter '%c'", param_getchar(Cmd, cmdp)); + errors = true; + break; } } //Validations @@ -225,7 +234,8 @@ int CmdHFFelicaSniff(const char *Cmd) { } // uid hex -int CmdHFFelicaSimLite(const char *Cmd) { +int CmdHFFelicaSimLite(const char *Cmd) +{ uint64_t uid = param_get64ex(Cmd, 0, 0, 16); @@ -238,12 +248,14 @@ int CmdHFFelicaSimLite(const char *Cmd) { return 0; } -static void printSep() { +static void printSep() +{ PrintAndLogEx(NORMAL, "------------------------------------------------------------------------------------"); } -uint16_t PrintFliteBlock(uint16_t tracepos, uint8_t *trace, uint16_t tracelen) { - if (tracepos+19 >= tracelen) +uint16_t PrintFliteBlock(uint16_t tracepos, uint8_t *trace, uint16_t tracelen) +{ + if (tracepos + 19 >= tracelen) return tracelen; trace += tracepos; @@ -253,120 +265,163 @@ uint16_t PrintFliteBlock(uint16_t tracepos, uint8_t *trace, uint16_t tracelen) { char line[110] = {0}; for (int j = 0; j < 16; j++) { - snprintf(line + (j * 4), sizeof(line) - 1 - (j*4) , "%02x ", trace[j+3]); + snprintf(line + (j * 4), sizeof(line) - 1 - (j * 4), "%02x ", trace[j + 3]); } - PrintAndLogEx(NORMAL, "block number %02x, status: %02x %02x",blocknum,status1, status2); + PrintAndLogEx(NORMAL, "block number %02x, status: %02x %02x", blocknum, status1, status2); switch (blocknum) { - case 0x00: PrintAndLogEx(NORMAL, "S_PAD0: %s",line);break; - case 0x01: PrintAndLogEx(NORMAL, "S_PAD1: %s",line);break; - case 0x02: PrintAndLogEx(NORMAL, "S_PAD2: %s",line);break; - case 0x03: PrintAndLogEx(NORMAL, "S_PAD3: %s",line);break; - case 0x04: PrintAndLogEx(NORMAL, "S_PAD4: %s",line);break; - case 0x05: PrintAndLogEx(NORMAL, "S_PAD5: %s",line);break; - case 0x06: PrintAndLogEx(NORMAL, "S_PAD6: %s",line);break; - case 0x07: PrintAndLogEx(NORMAL, "S_PAD7: %s",line);break; - case 0x08: PrintAndLogEx(NORMAL, "S_PAD8: %s",line);break; - case 0x09: PrintAndLogEx(NORMAL, "S_PAD9: %s",line);break; - case 0x0a: PrintAndLogEx(NORMAL, "S_PAD10: %s",line);break; - case 0x0b: PrintAndLogEx(NORMAL, "S_PAD11: %s",line);break; - case 0x0c: PrintAndLogEx(NORMAL, "S_PAD12: %s",line);break; - case 0x0d: PrintAndLogEx(NORMAL, "S_PAD13: %s",line);break; + case 0x00: + PrintAndLogEx(NORMAL, "S_PAD0: %s", line); + break; + case 0x01: + PrintAndLogEx(NORMAL, "S_PAD1: %s", line); + break; + case 0x02: + PrintAndLogEx(NORMAL, "S_PAD2: %s", line); + break; + case 0x03: + PrintAndLogEx(NORMAL, "S_PAD3: %s", line); + break; + case 0x04: + PrintAndLogEx(NORMAL, "S_PAD4: %s", line); + break; + case 0x05: + PrintAndLogEx(NORMAL, "S_PAD5: %s", line); + break; + case 0x06: + PrintAndLogEx(NORMAL, "S_PAD6: %s", line); + break; + case 0x07: + PrintAndLogEx(NORMAL, "S_PAD7: %s", line); + break; + case 0x08: + PrintAndLogEx(NORMAL, "S_PAD8: %s", line); + break; + case 0x09: + PrintAndLogEx(NORMAL, "S_PAD9: %s", line); + break; + case 0x0a: + PrintAndLogEx(NORMAL, "S_PAD10: %s", line); + break; + case 0x0b: + PrintAndLogEx(NORMAL, "S_PAD11: %s", line); + break; + case 0x0c: + PrintAndLogEx(NORMAL, "S_PAD12: %s", line); + break; + case 0x0d: + PrintAndLogEx(NORMAL, "S_PAD13: %s", line); + break; case 0x0E: { uint32_t regA = trace[3] | trace[4] << 8 | trace[5] << 16 | trace[ 6] << 24; uint32_t regB = trace[7] | trace[8] << 8 | trace[9] << 16 | trace[10] << 24; line[0] = 0; for (int j = 0; j < 8; j++) - snprintf(line + (j*2), sizeof(line)-1-(j*2), "%02x", trace[j+11]); + snprintf(line + (j * 2), sizeof(line) - 1 - (j * 2), "%02x", trace[j + 11]); PrintAndLogEx(NORMAL, "REG: regA: %d regB: %d regC: %s ", regA, regB, line); - } + } break; - case 0x80: PrintAndLogEx(NORMAL, "Random Challenge, WO: %s ", line); break; - case 0x81: PrintAndLogEx(NORMAL, "MAC, only set on dual read: %s ", line); break; + case 0x80: + PrintAndLogEx(NORMAL, "Random Challenge, WO: %s ", line); + break; + case 0x81: + PrintAndLogEx(NORMAL, "MAC, only set on dual read: %s ", line); + break; case 0x82: { char idd[20]; char idm[20]; for (int j = 0; j < 8; j++) - snprintf(idd + (j*2), sizeof(idd)-1-(j*2), "%02x", trace[j+3]); + snprintf(idd + (j * 2), sizeof(idd) - 1 - (j * 2), "%02x", trace[j + 3]); for (int j = 0; j < 6; j++) - snprintf(idm + (j*2), sizeof(idm)-1-(j*2), "%02x", trace[j+13]); + snprintf(idm + (j * 2), sizeof(idm) - 1 - (j * 2), "%02x", trace[j + 13]); PrintAndLogEx(NORMAL, "ID Block, IDd: 0x%s DFC: 0x%02x%02x Arb: %s ", idd, trace[11], trace [12], idm); - } + } break; case 0x83: { char idm[20]; char pmm[20]; for (int j = 0; j < 8; j++) - snprintf(idm + (j*2), sizeof(idm)-1-(j*2), "%02x", trace[j+3]); + snprintf(idm + (j * 2), sizeof(idm) - 1 - (j * 2), "%02x", trace[j + 3]); for (int j = 0; j < 8; j++) - snprintf(pmm + (j*2), sizeof(pmm)-1-(j*2), "%02x", trace[j+11]); + snprintf(pmm + (j * 2), sizeof(pmm) - 1 - (j * 2), "%02x", trace[j + 11]); PrintAndLogEx(NORMAL, "DeviceId: IDm: 0x%s PMm: 0x%s ", idm, pmm); - } + } break; - case 0x84: PrintAndLogEx(NORMAL, "SER_C: 0x%02x%02x ", trace[3], trace[4]); break; - case 0x85: PrintAndLogEx(NORMAL, "SYS_Cl 0x%02x%02x ", trace[3], trace[4]); break; - case 0x86: PrintAndLogEx(NORMAL, "CKV (key version): 0x%02x%02x ", trace[3], trace[4]); break; - case 0x87: PrintAndLogEx(NORMAL, "CK (card key), WO: %s ", line); break; + case 0x84: + PrintAndLogEx(NORMAL, "SER_C: 0x%02x%02x ", trace[3], trace[4]); + break; + case 0x85: + PrintAndLogEx(NORMAL, "SYS_Cl 0x%02x%02x ", trace[3], trace[4]); + break; + case 0x86: + PrintAndLogEx(NORMAL, "CKV (key version): 0x%02x%02x ", trace[3], trace[4]); + break; + case 0x87: + PrintAndLogEx(NORMAL, "CK (card key), WO: %s ", line); + break; case 0x88: { PrintAndLogEx(NORMAL, "Memory Configuration (MC):"); - PrintAndLogEx(NORMAL, "MAC needed to write state: %s", trace[3+12]? "on" : "off"); + PrintAndLogEx(NORMAL, "MAC needed to write state: %s", trace[3 + 12] ? "on" : "off"); //order might be off here... - PrintAndLogEx(NORMAL, "Write with MAC for S_PAD : %s ", sprint_bin(trace+3+10, 2) ); - PrintAndLogEx(NORMAL, "Write with AUTH for S_PAD : %s ", sprint_bin(trace+3+8, 2) ); - PrintAndLogEx(NORMAL, "Read after AUTH for S_PAD : %s ", sprint_bin(trace+3+6, 2) ); - PrintAndLogEx(NORMAL, "MAC needed to write CK and CKV: %s", trace[3+5] ? "on" : "off"); - PrintAndLogEx(NORMAL, "RF parameter: %02x", (trace[3+4] & 0x7) ); - PrintAndLogEx(NORMAL, "Compatible with NDEF: %s", trace[3+3] ? "yes" : "no"); - PrintAndLogEx(NORMAL, "Memory config writable : %s", (trace[3+2] == 0xff) ? "yes" : "no"); - PrintAndLogEx(NORMAL, "RW access for S_PAD : %s ", sprint_bin(trace+3, 2) ); - } + PrintAndLogEx(NORMAL, "Write with MAC for S_PAD : %s ", sprint_bin(trace + 3 + 10, 2)); + PrintAndLogEx(NORMAL, "Write with AUTH for S_PAD : %s ", sprint_bin(trace + 3 + 8, 2)); + PrintAndLogEx(NORMAL, "Read after AUTH for S_PAD : %s ", sprint_bin(trace + 3 + 6, 2)); + PrintAndLogEx(NORMAL, "MAC needed to write CK and CKV: %s", trace[3 + 5] ? "on" : "off"); + PrintAndLogEx(NORMAL, "RF parameter: %02x", (trace[3 + 4] & 0x7)); + PrintAndLogEx(NORMAL, "Compatible with NDEF: %s", trace[3 + 3] ? "yes" : "no"); + PrintAndLogEx(NORMAL, "Memory config writable : %s", (trace[3 + 2] == 0xff) ? "yes" : "no"); + PrintAndLogEx(NORMAL, "RW access for S_PAD : %s ", sprint_bin(trace + 3, 2)); + } break; case 0x90: { PrintAndLogEx(NORMAL, "Write count, RO: %02x %02x %02x ", trace[3], trace[4], trace[5]); - } + } break; case 0x91: { PrintAndLogEx(NORMAL, "MAC_A, RW (auth): %s ", line); - } + } break; case 0x92: PrintAndLogEx(NORMAL, "State:"); - PrintAndLogEx(NORMAL, "Polling disabled: %s", trace[3+8] ? "yes" : "no"); + PrintAndLogEx(NORMAL, "Polling disabled: %s", trace[3 + 8] ? "yes" : "no"); PrintAndLogEx(NORMAL, "Authenticated: %s", trace[3] ? "yes" : "no"); break; case 0xa0: - PrintAndLogEx(NORMAL, "CRC of all bloacks match : %s", (trace[3+2]==0xff) ? "no" : "yes"); + PrintAndLogEx(NORMAL, "CRC of all bloacks match : %s", (trace[3 + 2] == 0xff) ? "no" : "yes"); break; default: PrintAndLogEx(WARNING, "INVALID %d: %s", blocknum, line); - break; + break; } - return tracepos+19; + return tracepos + 19; } -int CmdHFFelicaDumpLite(const char *Cmd) { +int CmdHFFelicaDumpLite(const char *Cmd) +{ char ctmp = tolower(param_getchar(Cmd, 0)); - if ( ctmp == 'h') return usage_hf_felica_dumplite(); + if (ctmp == 'h') return usage_hf_felica_dumplite(); PrintAndLogEx(SUCCESS, "FeliCa lite - dump started"); PrintAndLogEx(SUCCESS, "press pm3-button to cancel"); - UsbCommand c = {CMD_FELICA_LITE_DUMP, {0,0,0}}; + UsbCommand c = {CMD_FELICA_LITE_DUMP, {0, 0, 0}}; clearCommandBuffer(); SendCommand(&c); UsbCommand resp; uint8_t timeout = 0; - while ( !WaitForResponseTimeout(CMD_ACK, &resp, 2000) ) { + while (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { timeout++; - printf("."); fflush(stdout); + printf("."); + fflush(stdout); if (ukbhit()) { - int gc = getchar(); (void)gc; + int gc = getchar(); + (void)gc; PrintAndLogEx(WARNING, "\n[!] aborted via keyboard!\n"); DropField(); return 1; @@ -384,15 +439,15 @@ int CmdHFFelicaDumpLite(const char *Cmd) { uint64_t tracelen = resp.arg[1]; uint8_t *trace = calloc(tracelen, sizeof(uint8_t)); - if ( trace == NULL ) { + if (trace == NULL) { PrintAndLogEx(WARNING, "Cannot allocate memory for trace"); return 1; } // only download data if there is any. - if ( tracelen > 0 ) { + if (tracelen > 0) { - if ( !GetFromDevice(BIG_BUF, trace, tracelen, 0, NULL, 2500, false) ){ + if (!GetFromDevice(BIG_BUF, trace, tracelen, 0, NULL, 2500, false)) { PrintAndLogEx(WARNING, "command execution time out"); free(trace); return 0; @@ -414,7 +469,8 @@ int CmdHFFelicaDumpLite(const char *Cmd) { return 0; } -int CmdHFFelicaCmdRaw(const char *cmd) { +int CmdHFFelicaCmdRaw(const char *cmd) +{ UsbCommand c = {CMD_FELICA_COMMAND, {0, 0, 0}}; bool reply = 1; bool crc = false; @@ -422,7 +478,7 @@ int CmdHFFelicaCmdRaw(const char *cmd) { bool active = false; bool active_select = false; uint16_t numbits = 0; - char buf[5]=""; + char buf[5] = ""; int i = 0; uint8_t data[USB_CMD_DATA_SIZE]; uint16_t datalen = 0; @@ -431,12 +487,12 @@ int CmdHFFelicaCmdRaw(const char *cmd) { if (strlen(cmd) < 2) return usage_hf_felica_raw(); // strip - while (*cmd==' ' || *cmd=='\t') cmd++; + while (*cmd == ' ' || *cmd == '\t') cmd++; - while (cmd[i]!='\0') { - if (cmd[i]==' ' || cmd[i]=='\t') { i++; continue; } - if (cmd[i]=='-') { - switch (cmd[i+1]) { + while (cmd[i] != '\0') { + if (cmd[i] == ' ' || cmd[i] == '\t') { i++; continue; } + if (cmd[i] == '-') { + switch (cmd[i + 1]) { case 'H': case 'h': return usage_hf_felica_raw(); @@ -456,11 +512,11 @@ int CmdHFFelicaCmdRaw(const char *cmd) { active_select = true; break; case 'b': - sscanf(cmd+i+2, "%d", &temp); + sscanf(cmd + i + 2, "%d", &temp); numbits = temp & 0xFFFF; - i+=3; - while(cmd[i]!=' ' && cmd[i]!='\0') { i++; } - i-=2; + i += 3; + while (cmd[i] != ' ' && cmd[i] != '\0') { i++; } + i -= 2; break; default: return usage_hf_felica_raw(); @@ -468,18 +524,18 @@ int CmdHFFelicaCmdRaw(const char *cmd) { i += 2; continue; } - if ((cmd[i]>='0' && cmd[i]<='9') || - (cmd[i]>='a' && cmd[i]<='f') || - (cmd[i]>='A' && cmd[i]<='F') ) { - buf[strlen(buf)+1]=0; - buf[strlen(buf)]=cmd[i]; + if ((cmd[i] >= '0' && cmd[i] <= '9') || + (cmd[i] >= 'a' && cmd[i] <= 'f') || + (cmd[i] >= 'A' && cmd[i] <= 'F')) { + buf[strlen(buf) + 1] = 0; + buf[strlen(buf)] = cmd[i]; i++; - if (strlen(buf)>=2) { - sscanf(buf,"%x",&temp); - data[datalen]=(uint8_t)(temp & 0xff); - *buf=0; - if (++datalen >= sizeof(data)){ + if (strlen(buf) >= 2) { + sscanf(buf, "%x", &temp); + data[datalen] = (uint8_t)(temp & 0xff); + *buf = 0; + if (++datalen >= sizeof(data)) { if (crc) PrintAndLogEx(NORMAL, "Buffer is full, we can't add CRC to your data"); break; @@ -491,7 +547,7 @@ int CmdHFFelicaCmdRaw(const char *cmd) { return 0; } - if (crc && datalen>0 && datalen < sizeof(data)-2) { + if (crc && datalen > 0 && datalen < sizeof(data) - 2) { uint8_t b1, b2; compute_crc(CRC_FELICA, data, datalen, &b1, &b2); data[datalen++] = b1; @@ -500,7 +556,7 @@ int CmdHFFelicaCmdRaw(const char *cmd) { if (active || active_select) { c.arg[0] |= FELICA_CONNECT; - if(active) + if (active) c.arg[0] |= FELICA_NO_SELECT; } @@ -530,41 +586,44 @@ int CmdHFFelicaCmdRaw(const char *cmd) { return 0; } -void waitCmdFelica(uint8_t iSelect) { +void waitCmdFelica(uint8_t iSelect) +{ UsbCommand resp; uint16_t len = 0; if (WaitForResponseTimeout(CMD_ACK, &resp, 2000)) { len = iSelect ? (resp.arg[1] & 0xffff) : (resp.arg[0] & 0xffff); PrintAndLogEx(NORMAL, "received %i octets", len); - if(!len) + if (!len) return; - PrintAndLogEx(NORMAL, "%s", sprint_hex(resp.d.asBytes, len) ); + PrintAndLogEx(NORMAL, "%s", sprint_hex(resp.d.asBytes, len)); } else { PrintAndLogEx(WARNING, "timeout while waiting for reply."); } } static command_t CommandTable[] = { - {"help", CmdHelp, 1, "This help"}, - {"list", CmdHFFelicaList, 0, "[Deprecated] List ISO 18092/FeliCa history"}, - {"reader", CmdHFFelicaReader, 0, "Act like an ISO18092/FeliCa reader"}, - {"sim", CmdHFFelicaSim, 0, " -- Simulate ISO 18092/FeliCa tag"}, - {"sniff", CmdHFFelicaSniff, 0, "sniff ISO 18092/Felica traffic"}, - {"raw", CmdHFFelicaCmdRaw, 0, "Send raw hex data to tag"}, + {"help", CmdHelp, 1, "This help"}, + {"list", CmdHFFelicaList, 0, "[Deprecated] List ISO 18092/FeliCa history"}, + {"reader", CmdHFFelicaReader, 0, "Act like an ISO18092/FeliCa reader"}, + {"sim", CmdHFFelicaSim, 0, " -- Simulate ISO 18092/FeliCa tag"}, + {"sniff", CmdHFFelicaSniff, 0, "sniff ISO 18092/Felica traffic"}, + {"raw", CmdHFFelicaCmdRaw, 0, "Send raw hex data to tag"}, - {"litesim", CmdHFFelicaSimLite, 0, " - only reply to poll request"}, - {"litedump", CmdHFFelicaDumpLite, 0, "Wait for and try dumping FelicaLite"}, - {NULL, NULL, 0, NULL} + {"litesim", CmdHFFelicaSimLite, 0, " - only reply to poll request"}, + {"litedump", CmdHFFelicaDumpLite, 0, "Wait for and try dumping FelicaLite"}, + {NULL, NULL, 0, NULL} }; -int CmdHFFelica(const char *Cmd) { +int CmdHFFelica(const char *Cmd) +{ clearCommandBuffer(); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { +int CmdHelp(const char *Cmd) +{ CmdsHelp(CommandTable); return 0; } \ No newline at end of file diff --git a/client/cmdhffido.c b/client/cmdhffido.c index 588c3b037..80d2416d7 100644 --- a/client/cmdhffido.c +++ b/client/cmdhffido.c @@ -49,7 +49,8 @@ static int CmdHelp(const char *Cmd); -int CmdHFFidoInfo(const char *cmd) { +int CmdHFFidoInfo(const char *cmd) +{ if (cmd && strlen(cmd) > 0) PrintAndLog("WARNING: command don't have any parameters.\n"); @@ -104,7 +105,7 @@ int CmdHFFidoInfo(const char *cmd) { return 0; } - if(buf[0]) { + if (buf[0]) { PrintAndLog("FIDO2 ger version error: %d - %s", buf[0], fido2GetCmdErrorDescription(buf[0])); return 0; } @@ -124,12 +125,13 @@ int CmdHFFidoInfo(const char *cmd) { return 0; } -json_t *OpenJson(int paramnum, char *fname, void* argtable[], bool *err) { +json_t *OpenJson(int paramnum, char *fname, void *argtable[], bool *err) +{ json_t *root = NULL; json_error_t error; *err = false; - uint8_t jsonname[250] ={0}; + uint8_t jsonname[250] = {0}; char *cjsonname = (char *)jsonname; int jsonnamelen = 0; @@ -168,7 +170,8 @@ json_t *OpenJson(int paramnum, char *fname, void* argtable[], bool *err) { return root; } -int CmdHFFidoRegister(const char *cmd) { +int CmdHFFidoRegister(const char *cmd) +{ uint8_t data[64] = {0}; int chlen = 0; uint8_t cdata[250] = {0}; @@ -177,12 +180,12 @@ int CmdHFFidoRegister(const char *cmd) { json_t *root = NULL; CLIParserInit("hf fido reg", - "Initiate a U2F token registration. Needs two 32-byte hash number. \nchallenge parameter (32b) and application parameter (32b).", - "Usage:\n\thf fido reg -> execute command with 2 parameters, filled 0x00\n" - "\thf fido reg 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f -> execute command with parameters" - "\thf fido reg -p s0 s1 -> execute command with plain parameters"); + "Initiate a U2F token registration. Needs two 32-byte hash number. \nchallenge parameter (32b) and application parameter (32b).", + "Usage:\n\thf fido reg -> execute command with 2 parameters, filled 0x00\n" + "\thf fido reg 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f -> execute command with parameters" + "\thf fido reg -p s0 s1 -> execute command with plain parameters"); - void* argtable[] = { + void *argtable[] = { arg_param_begin, arg_lit0("aA", "apdu", "show APDU reqests and responses"), arg_litn("vV", "verbose", 0, 2, "show technical data. vv - show full certificates data"), @@ -204,7 +207,7 @@ int CmdHFFidoRegister(const char *cmd) { char fname[250] = {0}; bool err; root = OpenJson(5, fname, argtable, &err); - if(err) + if (err) return 1; if (root) { size_t jlen; @@ -345,19 +348,19 @@ int CmdHFFidoRegister(const char *cmd) { uint8_t xbuf[4096] = {0}; size_t xbuflen = 0; res = FillBuffer(xbuf, sizeof(xbuf), &xbuflen, - "\x00", 1, - &data[32], 32, // application parameter - &data[0], 32, // challenge parameter - &buf[67], keyHandleLen, // keyHandle - &buf[1], 65, // user public key - NULL, 0); + "\x00", 1, + &data[32], 32, // application parameter + &data[0], 32, // challenge parameter + &buf[67], keyHandleLen, // keyHandle + &buf[1], 65, // user public key + NULL, 0); //PrintAndLog("--xbuf(%d)[%d]: %s", res, xbuflen, sprint_hex(xbuf, xbuflen)); res = ecdsa_signature_verify(public_key, xbuf, xbuflen, &buf[hashp], len - hashp); if (res) { if (res == -0x4e00) { PrintAndLog("Signature is NOT VALID."); } else { - PrintAndLog("Other signature check error: %x %s", (res<0)?-res:res, ecdsa_get_error(res)); + PrintAndLog("Other signature check error: %x %s", (res < 0) ? -res : res, ecdsa_get_error(res)); } } else { PrintAndLog("Signature is OK."); @@ -368,11 +371,11 @@ int CmdHFFidoRegister(const char *cmd) { } PrintAndLog("\nauth command: "); - printf("hf fido auth %s%s", paramsPlain?"-p ":"", sprint_hex_inrow(&buf[67], keyHandleLen)); - if(chlen || applen) - printf(" %s", paramsPlain?(char *)cdata:sprint_hex_inrow(cdata, 32)); - if(applen) - printf(" %s", paramsPlain?(char *)adata:sprint_hex_inrow(adata, 32)); + printf("hf fido auth %s%s", paramsPlain ? "-p " : "", sprint_hex_inrow(&buf[67], keyHandleLen)); + if (chlen || applen) + printf(" %s", paramsPlain ? (char *)cdata : sprint_hex_inrow(cdata, 32)); + if (applen) + printf(" %s", paramsPlain ? (char *)adata : sprint_hex_inrow(adata, 32)); printf("\n"); if (root) { @@ -397,7 +400,8 @@ int CmdHFFidoRegister(const char *cmd) { return 0; }; -int CmdHFFidoAuthenticate(const char *cmd) { +int CmdHFFidoAuthenticate(const char *cmd) +{ uint8_t data[512] = {0}; uint8_t hdata[250] = {0}; bool public_key_loaded = false; @@ -407,12 +411,12 @@ int CmdHFFidoAuthenticate(const char *cmd) { json_t *root = NULL; CLIParserInit("hf fido auth", - "Initiate a U2F token authentication. Needs key handle and two 32-byte hash number. \nkey handle(var 0..255), challenge parameter (32b) and application parameter (32b).", - "Usage:\n\thf fido auth 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f -> execute command with 2 parameters, filled 0x00 and key handle\n" - "\thf fido auth 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f " - "000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f -> execute command with parameters"); + "Initiate a U2F token authentication. Needs key handle and two 32-byte hash number. \nkey handle(var 0..255), challenge parameter (32b) and application parameter (32b).", + "Usage:\n\thf fido auth 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f -> execute command with 2 parameters, filled 0x00 and key handle\n" + "\thf fido auth 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f " + "000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f 000102030405060708090a0b0c0d0e0f000102030405060708090a0b0c0d0e0f -> execute command with parameters"); - void* argtable[] = { + void *argtable[] = { arg_param_begin, arg_lit0("aA", "apdu", "show APDU reqests and responses"), arg_lit0("vV", "verbose", "show technical data"), @@ -441,7 +445,7 @@ int CmdHFFidoAuthenticate(const char *cmd) { char fname[250] = {0}; bool err; root = OpenJson(7, fname, argtable, &err); - if(err) + if (err) return 1; if (root) { size_t jlen; @@ -554,8 +558,8 @@ int CmdHFFidoAuthenticate(const char *cmd) { } PrintAndLog("---------------------------------------------------------------"); - PrintAndLog("User presence: %s", (buf[0]?"verified":"not verified")); - uint32_t cntr = (uint32_t)bytes_to_num(&buf[1], 4); + PrintAndLog("User presence: %s", (buf[0] ? "verified" : "not verified")); + uint32_t cntr = (uint32_t)bytes_to_num(&buf[1], 4); PrintAndLog("Counter: %d", cntr); PrintAndLog("Hash[%d]: %s", len - 5, sprint_hex(&buf[5], len - 5)); @@ -572,18 +576,18 @@ int CmdHFFidoAuthenticate(const char *cmd) { uint8_t xbuf[4096] = {0}; size_t xbuflen = 0; res = FillBuffer(xbuf, sizeof(xbuf), &xbuflen, - &data[32], 32, // application parameter - &buf[0], 1, // user presence - &buf[1], 4, // counter - data, 32, // challenge parameter - NULL, 0); + &data[32], 32, // application parameter + &buf[0], 1, // user presence + &buf[1], 4, // counter + data, 32, // challenge parameter + NULL, 0); //PrintAndLog("--xbuf(%d)[%d]: %s", res, xbuflen, sprint_hex(xbuf, xbuflen)); res = ecdsa_signature_verify(public_key, xbuf, xbuflen, &buf[5], len - 5); if (res) { if (res == -0x4e00) { PrintAndLog("Signature is NOT VALID."); } else { - PrintAndLog("Other signature check error: %x %s", (res<0)?-res:res, ecdsa_get_error(res)); + PrintAndLog("Other signature check error: %x %s", (res < 0) ? -res : res, ecdsa_get_error(res)); } } else { PrintAndLog("Signature is OK."); @@ -615,13 +619,15 @@ int CmdHFFidoAuthenticate(const char *cmd) { return 0; }; -void CheckSlash(char *fileName) { +void CheckSlash(char *fileName) +{ if ((fileName[strlen(fileName) - 1] != '/') && (fileName[strlen(fileName) - 1] != '\\')) strcat(fileName, "/"); } -int GetExistsFileNameJson(char *prefixDir, char *reqestedFileName, char *fileName) { +int GetExistsFileNameJson(char *prefixDir, char *reqestedFileName, char *fileName) +{ fileName[0] = 0x00; strcpy(fileName, get_my_executable_directory()); CheckSlash(fileName); @@ -648,17 +654,18 @@ int GetExistsFileNameJson(char *prefixDir, char *reqestedFileName, char *fileNam return 0; } -int CmdHFFido2MakeCredential(const char *cmd) { +int CmdHFFido2MakeCredential(const char *cmd) +{ json_error_t error; json_t *root = NULL; char fname[300] = {0}; CLIParserInit("hf fido make", - "Execute a FIDO2 Make Credentional command. Needs json file with parameters. Sample file `fido2.json`. File can be placed in proxmark directory or in `proxmark/fido` directory.", - "Usage:\n\thf fido make -> execute command default parameters file `fido2.json`\n" - "\thf fido make test.json -> execute command with parameters file `text.json`"); + "Execute a FIDO2 Make Credentional command. Needs json file with parameters. Sample file `fido2.json`. File can be placed in proxmark directory or in `proxmark/fido` directory.", + "Usage:\n\thf fido make -> execute command default parameters file `fido2.json`\n" + "\thf fido make test.json -> execute command with parameters file `text.json`"); - void* argtable[] = { + void *argtable[] = { arg_param_begin, arg_lit0("aA", "apdu", "show APDU reqests and responses"), arg_litn("vV", "verbose", 0, 2, "show technical data. vv - show full certificates data"), @@ -675,7 +682,7 @@ int CmdHFFido2MakeCredential(const char *cmd) { bool showDERTLV = arg_get_lit(3); bool showCBOR = arg_get_lit(4); - uint8_t jsonname[250] ={0}; + uint8_t jsonname[250] = {0}; char *cjsonname = (char *)jsonname; int jsonnamelen = 0; CLIGetStrWithReturn(5, jsonname, &jsonnamelen); @@ -690,7 +697,7 @@ int CmdHFFido2MakeCredential(const char *cmd) { SetAPDULogging(APDULogging); int res = GetExistsFileNameJson("fido", cjsonname, fname); - if(res) { + if (res) { PrintAndLog("ERROR: Can't found the json file."); return res; } @@ -745,7 +752,7 @@ int CmdHFFido2MakeCredential(const char *cmd) { return 3; } - if(buf[0]) { + if (buf[0]) { PrintAndLog("FIDO2 make credential error: %d - %s", buf[0], fido2GetCmdErrorDescription(buf[0])); return 0; } @@ -775,17 +782,18 @@ int CmdHFFido2MakeCredential(const char *cmd) { return 0; }; -int CmdHFFido2GetAssertion(const char *cmd) { +int CmdHFFido2GetAssertion(const char *cmd) +{ json_error_t error; json_t *root = NULL; char fname[300] = {0}; CLIParserInit("hf fido assert", - "Execute a FIDO2 Get Assertion command. Needs json file with parameters. Sample file `fido2.json`. File can be placed in proxmark directory or in `proxmark/fido` directory.", - "Usage:\n\thf fido assert -> execute command default parameters file `fido2.json`\n" - "\thf fido assert test.json -l -> execute command with parameters file `text.json` and add to request CredentialId"); + "Execute a FIDO2 Get Assertion command. Needs json file with parameters. Sample file `fido2.json`. File can be placed in proxmark directory or in `proxmark/fido` directory.", + "Usage:\n\thf fido assert -> execute command default parameters file `fido2.json`\n" + "\thf fido assert test.json -l -> execute command with parameters file `text.json` and add to request CredentialId"); - void* argtable[] = { + void *argtable[] = { arg_param_begin, arg_lit0("aA", "apdu", "show APDU reqests and responses"), arg_litn("vV", "verbose", 0, 2, "show technical data. vv - show full certificates data"), @@ -802,7 +810,7 @@ int CmdHFFido2GetAssertion(const char *cmd) { bool showCBOR = arg_get_lit(3); bool createAllowList = arg_get_lit(4); - uint8_t jsonname[250] ={0}; + uint8_t jsonname[250] = {0}; char *cjsonname = (char *)jsonname; int jsonnamelen = 0; CLIGetStrWithReturn(5, jsonname, &jsonnamelen); @@ -817,7 +825,7 @@ int CmdHFFido2GetAssertion(const char *cmd) { SetAPDULogging(APDULogging); int res = GetExistsFileNameJson("fido", "fido2", fname); - if(res) { + if (res) { PrintAndLog("ERROR: Can't found the json file."); return res; } @@ -872,7 +880,7 @@ int CmdHFFido2GetAssertion(const char *cmd) { return 3; } - if(buf[0]) { + if (buf[0]) { PrintAndLog("FIDO2 get assertion error: %d - %s", buf[0], fido2GetCmdErrorDescription(buf[0])); return 0; } @@ -902,24 +910,25 @@ int CmdHFFido2GetAssertion(const char *cmd) { return 0; }; -static command_t CommandTable[] = -{ - {"help", CmdHelp, 1, "This help."}, - {"info", CmdHFFidoInfo, 0, "Info about FIDO tag."}, - {"reg", CmdHFFidoRegister, 0, "FIDO U2F Registration Message."}, - {"auth", CmdHFFidoAuthenticate, 0, "FIDO U2F Authentication Message."}, - {"make", CmdHFFido2MakeCredential, 0, "FIDO2 MakeCredential command."}, - {"assert", CmdHFFido2GetAssertion, 0, "FIDO2 GetAssertion command."}, - {NULL, NULL, 0, NULL} +static command_t CommandTable[] = { + {"help", CmdHelp, 1, "This help."}, + {"info", CmdHFFidoInfo, 0, "Info about FIDO tag."}, + {"reg", CmdHFFidoRegister, 0, "FIDO U2F Registration Message."}, + {"auth", CmdHFFidoAuthenticate, 0, "FIDO U2F Authentication Message."}, + {"make", CmdHFFido2MakeCredential, 0, "FIDO2 MakeCredential command."}, + {"assert", CmdHFFido2GetAssertion, 0, "FIDO2 GetAssertion command."}, + {NULL, NULL, 0, NULL} }; -int CmdHFFido(const char *Cmd) { +int CmdHFFido(const char *Cmd) +{ (void)WaitForResponseTimeout(CMD_ACK, NULL, 100); CmdsParse(CommandTable, Cmd); return 0; } -int CmdHelp(const char *Cmd) { - CmdsHelp(CommandTable); - return 0; +int CmdHelp(const char *Cmd) +{ + CmdsHelp(CommandTable); + return 0; } diff --git a/client/cmdhficlass.c b/client/cmdhficlass.c index 20bcfdf33..0fc95a586 100644 --- a/client/cmdhficlass.c +++ b/client/cmdhficlass.c @@ -19,17 +19,18 @@ static int CmdHelp(const char *Cmd); static uint8_t iClass_Key_Table[ICLASS_KEYS_MAX][8] = { - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }, - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }, - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }, - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }, - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }, - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }, - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }, - { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 } + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } }; -int usage_hf_iclass_sim(void) { +int usage_hf_iclass_sim(void) +{ PrintAndLogEx(NORMAL, "Usage: hf iclass sim