From ad6219fc9167b2414299b54df03297db27e97e22 Mon Sep 17 00:00:00 2001 From: iceman1001 Date: Thu, 4 Jun 2015 10:33:55 +0200 Subject: [PATCH] ADD: @marshmellow42 's fixed version of the ISO11784 FDX-B ADD: crc16_ccitt_rev (reverse version of crc16_ccitt 0x0000) --- client/cmddata.c | 99 +++++++++++++++++++++++++++------------------ client/cmddata.h | 2 +- client/cmdlf.c | 6 +++ client/cmdlft55xx.c | 3 +- client/util.c | 15 +++++++ client/util.h | 2 +- common/crc16.c | 4 ++ common/crc16.h | 1 + common/lfdemod.c | 18 ++++++++- common/lfdemod.h | 4 +- 10 files changed, 109 insertions(+), 45 deletions(-) diff --git a/client/cmddata.c b/client/cmddata.c index 3b9c3a66..0f336f10 100644 --- a/client/cmddata.c +++ b/client/cmddata.c @@ -23,6 +23,8 @@ #include "lfdemod.h" #include "usb_cmd.h" #include "crc.h" +#include "crc16.h" + uint8_t DemodBuffer[MAX_DEMOD_BUF_LEN]; uint8_t g_debugMode; @@ -1457,11 +1459,11 @@ int CmdFSKdemodPyramid(const char *Cmd) return 1; } -// ISO11784/85 demod (aka animal tag) BIPHASE, inverted, rf/32, with preamble of 00000000001 (128bits) +// FDX-B ISO11784/85 demod (aka animal tag) BIPHASE, inverted, rf/32, with preamble of 00000000001 (128bits) // 8 databits + 1 parity (1) // CIITT 16 chksum // NATIONAL CODE, ICAR database -// COUNTRY CODE (ISO3166) +// COUNTRY CODE (ISO3166) or http://cms.abvma.ca/uploads/ManufacturersISOsandCountryCodes.pdf // FLAG (animal/non-animal) /* 38 IDbits @@ -1472,62 +1474,81 @@ int CmdFSKdemodPyramid(const char *Cmd) 16 ccitt CRC chksum over 64bit ID CODE. 24 appli bits. --- sample: 985121004515220 - -Now is nibble shifting, byte shifting. +-- sample: 985121004515220 [ 37FF65B88EF94 ] */ -int CmdIso11784demodBI(const char *Cmd){ +int CmdFDXBdemodBI(const char *Cmd){ int invert = 1; int clk = 32; int errCnt = 0; + int maxErr = 0; uint8_t BitStream[MAX_DEMOD_BUF_LEN]; size_t size = getFromGraphBuf(BitStream); - errCnt = askdemod(BitStream, &size, &clk, &invert, 0, 0, 0); - if ( errCnt<0 ) { - if (g_debugMode) PrintAndLog("DEBUG: no data found %d, clock: 32", errCnt); + errCnt = askdemod(BitStream, &size, &clk, &invert, maxErr, 0, 0); + if ( errCnt < 0 || errCnt > maxErr ) { + if (g_debugMode) PrintAndLog("DEBUG: no data or error found %d, clock: 32", errCnt); return 0; } - errCnt = BiphaseRawDecode(BitStream, &size, 0, 1); - if (errCnt < 0){ + errCnt = BiphaseRawDecode(BitStream, &size, maxErr, 1); + if (errCnt < 0 || errCnt > maxErr ) { if (g_debugMode) PrintAndLog("Error BiphaseRawDecode: %d", errCnt); return 0; } - int preambleIndex = ISO11784demodBI(BitStream, &size); + int preambleIndex = FDXBdemodBI(BitStream, &size); if (preambleIndex < 0){ - if (g_debugMode) PrintAndLog("Error ISO11784Demod , no startmarker found :: %d",preambleIndex); + if (g_debugMode) PrintAndLog("Error FDXBDemod , no startmarker found :: %d",preambleIndex); return 0; } - PrintAndLog("startmarker %d; Size %d", preambleIndex, size); + setDemodBuf(BitStream, 128, preambleIndex); - //got a good demod - uint8_t ByteStream[16] = {0x00}; - uint8_t bitCnt = 0; - uint8_t ByteCnt = 0; - size_t startIdx = preambleIndex + 11; //start after preamble - for (size_t idx = 0; idx < size; idx++){ - - if ( bitCnt == 9 ){ - bitCnt = 0; - continue; - } - //lsb first - ByteStream[ByteCnt] |= ( BitStream[startIdx+idx] << bitCnt ); - bitCnt++; - if (bitCnt % 8 == 0){ - if (g_debugMode) PrintAndLog("byte %d: %02x", ByteCnt, ByteStream[ByteCnt]); - bitCnt = 9; - ByteCnt++; - } + size = removeParity(BitStream, preambleIndex + 11, 9, 2, 128-11); + if ( size <= 103 ) { + if (g_debugMode) PrintAndLog("Error removeParity:: %d", size); + return 0; } - PrintAndLog("DATA: %s", sprint_hex(ByteStream, 14)); - //now ByteStream contains 16 bytes of decrypted raw tag data - setDemodBuf(BitStream+preambleIndex, 128, 0); - printDemodBuff(); + if (g_debugMode) { + char *bin = sprint_bin_break(BitStream,size,16); + PrintAndLog("DEBUG BinStream:\n%s",bin); + } + PrintAndLog("\nFDX-B / ISO 11784/5 Animal Tag ID Found:"); + if (g_debugMode) PrintAndLog("startmarker %d; Size %d", preambleIndex, size); + + //got a good demod + + //marshmellows + uint64_t NationalCode = ((uint64_t)(bytebits_to_byteLSBF(BitStream+32,6)) << 32) | bytebits_to_byteLSBF(BitStream,32); + uint32_t countryCode = bytebits_to_byteLSBF(BitStream+38,10); + uint8_t dataBlockBit = BitStream[48]; + uint32_t reservedCode = bytebits_to_byteLSBF(BitStream+49,14); + uint8_t animalBit = BitStream[63]; + + uint16_t crc16 = bytebits_to_byteLSBF(BitStream+64,16); + uint32_t extended = bytebits_to_byteLSBF(BitStream+80,24); + + uint64_t rawid = ((uint64_t)bytebits_to_byte(BitStream+32,32) << 32) | bytebits_to_byte(BitStream,32); + uint8_t raw[8]; + num_to_bytes(rawid, 8, raw); + PrintAndLog("%s", sprint_hex(raw,8)); + uint16_t crcCalc = crc16_ccitt_rev( raw ,8); + + PrintAndLog("Animal ID: %u-%012llu", countryCode, NationalCode); + PrintAndLog("National Code: %012llu", NationalCode); + PrintAndLog("CountryCode: %u", countryCode); + PrintAndLog("Extended Data: %s", dataBlockBit ? "True" : "False"); + PrintAndLog("reserved Code: %u", reservedCode); + PrintAndLog("Animal Tag: %s", animalBit ? "True" : "False"); + PrintAndLog("CRC: 0x%02X", crc16); + PrintAndLog("CRC : %X == %X %s", crc16, crcCalc, ( crcCalc == crc16 )?"ok":"!" ); + PrintAndLog("Extended: 0x%X", extended); + +/* + //uint16_t crcCalc = crc16_ccitt( ByteStream, 8); + PrintAndLog("Application ID: %04X", applicationid); + */ return 1; } @@ -1947,7 +1968,7 @@ typedef struct { uint8_t * buffer; uint32_t numbits; uint32_t position; -}BitstreamOut; +} BitstreamOut; bool _headBit( BitstreamOut *stream) { @@ -2277,6 +2298,7 @@ static command_t CommandTable[] = {"buffclear", CmdBuffClear, 1, "Clear sample buffer and graph window"}, {"dec", CmdDec, 1, "Decimate samples"}, {"detectclock", CmdDetectClockRate, 1, "[modulation] Detect clock rate of wave in GraphBuffer (options: 'a','f','n','p' for ask, fsk, nrz, psk respectively)"}, + {"fdxbdemod", CmdFDXBdemodBI , 1, "Demodulate a FDX-B ISO11784/85 Biphase tag from GraphBuffer"}, {"fskawiddemod", CmdFSKdemodAWID, 1, "Demodulate an AWID FSK tag from GraphBuffer"}, //{"fskfcdetect", CmdFSKfcDetect, 1, "Try to detect the Field Clock of an FSK wave"}, {"fskhiddemod", CmdFSKdemodHID, 1, "Demodulate a HID FSK tag from GraphBuffer"}, @@ -2288,7 +2310,6 @@ static command_t CommandTable[] = {"hexsamples", CmdHexsamples, 0, " [] -- Dump big buffer as hex bytes"}, {"hide", CmdHide, 1, "Hide graph window"}, {"hpf", CmdHpf, 1, "Remove DC offset from trace"}, - {"iso11784demod", CmdIso11784demodBI, 1, "Demodulate a ISO11784/85 Biphase tag from GraphBuffer"}, {"load", CmdLoad, 1, " -- Load trace (to graph window"}, {"ltrim", CmdLtrim, 1, " -- Trim samples from left of trace"}, {"rtrim", CmdRtrim, 1, " -- Trim samples from right of trace"}, diff --git a/client/cmddata.h b/client/cmddata.h index 4ad5fbd0..fcc51a6b 100644 --- a/client/cmddata.h +++ b/client/cmddata.h @@ -27,13 +27,13 @@ int CmdBitsamples(const char *Cmd); int CmdBuffClear(const char *Cmd); int CmdDec(const char *Cmd); int CmdDetectClockRate(const char *Cmd); +int CmdFDXBdemodBI(const char *Cmd); int CmdFSKdemodAWID(const char *Cmd); int CmdFSKdemodHID(const char *Cmd); int CmdFSKdemodIO(const char *Cmd); int CmdFSKdemodParadox(const char *Cmd); int CmdFSKdemodPyramid(const char *Cmd); int CmdFSKrawdemod(const char *Cmd); -int CmdIso11784demodBI(const char *Cmd); int CmdPSK1rawDemod(const char *Cmd); int CmdPSK2rawDemod(const char *Cmd); int CmdPSKNexWatch(const char *Cmd); diff --git a/client/cmdlf.c b/client/cmdlf.c index 7c5d273f..aa7f63a9 100644 --- a/client/cmdlf.c +++ b/client/cmdlf.c @@ -1077,6 +1077,12 @@ int CmdLFfind(const char *Cmd) return 1; } + ans=CmdFDXBdemodBI(""); + if (ans>0) { + PrintAndLog("\nValid FDX-B ID Found!"); + return 1; + } + ans=EM4x50Read("", false); if (ans>0) { PrintAndLog("\nValid EM4x50 ID Found!"); diff --git a/client/cmdlft55xx.c b/client/cmdlft55xx.c index cf19c0ec..2953e780 100644 --- a/client/cmdlft55xx.c +++ b/client/cmdlft55xx.c @@ -1050,6 +1050,7 @@ char * GetSelectedModulationStr( uint8_t id){ return buf; } +/* uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits){ int i = start; @@ -1063,7 +1064,7 @@ uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits){ return tmp; } - +*/ static command_t CommandTable[] = { {"help", CmdHelp, 1, "This help"}, diff --git a/client/util.c b/client/util.c index 30547080..66e0a697 100644 --- a/client/util.c +++ b/client/util.c @@ -453,3 +453,18 @@ void xor(unsigned char * dst, unsigned char * src, size_t len) { int32_t le24toh (uint8_t data[3]) { return (data[2] << 16) | (data[1] << 8) | data[0]; } + + +uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits) { + + int i = start; + int j = len-1; + + if (len > 32) return 0; + + uint32_t tmp = 0; + for (; j >= 0; --j, ++i) + tmp |= bits[i] << j; + + return tmp; +} diff --git a/client/util.h b/client/util.h index 4c7e93a7..a57daf82 100644 --- a/client/util.h +++ b/client/util.h @@ -65,4 +65,4 @@ void wiegand_add_parity(char *target, char *source, char length); void xor(unsigned char * dst, unsigned char * src, size_t len); int32_t le24toh (uint8_t data[3]); - +uint32_t PackBits(uint8_t start, uint8_t len, uint8_t* bits); diff --git a/common/crc16.c b/common/crc16.c index 973cd103..a04c4012 100644 --- a/common/crc16.c +++ b/common/crc16.c @@ -43,3 +43,7 @@ uint16_t crc16(uint8_t const *message, int length, uint16_t remainder, uint16_t uint16_t crc16_ccitt(uint8_t const *message, int length) { return crc16(message, length, 0xffff, 0x1021); } + +uint16_t crc16_ccitt_rev(uint8_t const *message, int length) { + return crc16(message, length, 0x0000, 0x1021); +} \ No newline at end of file diff --git a/common/crc16.h b/common/crc16.h index d16d83b5..254e7fdf 100644 --- a/common/crc16.h +++ b/common/crc16.h @@ -12,4 +12,5 @@ unsigned short update_crc16(unsigned short crc, unsigned char c); uint16_t crc16(uint8_t const *message, int length, uint16_t remainder, uint16_t polynomial); uint16_t crc16_ccitt(uint8_t const *message, int length); +uint16_t crc16_ccitt_rev(uint8_t const *message, int length); #endif diff --git a/common/lfdemod.c b/common/lfdemod.c index 62f52a2f..94e08ffa 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -537,6 +537,17 @@ uint32_t bytebits_to_byte(uint8_t* src, size_t numbits) return num; } +//least significant bit first +uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits) +{ + uint32_t num = 0; + for(int i = 0 ; i < numbits ; i++) + { + num = (num << 1) | *(src + (numbits-(i+1))); + } + return num; +} + int IOdemodFSK(uint8_t *dest, size_t size) { if (justNoise(dest, size)) return -1; @@ -569,7 +580,7 @@ int IOdemodFSK(uint8_t *dest, size_t size) // by marshmellow // takes a array of binary values, start position, length of bits per parity (includes parity bit), -// Parity Type (1 for odd 0 for even), and binary Length (length to run) +// Parity Type (1 for odd; 0 for even; 2 for just drop it), and binary Length (length to run) size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) { uint32_t parityWd = 0; @@ -581,7 +592,9 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p } j--; // if parity fails then return 0 + if (pType != 2) { if (parityTest(parityWd, pLen, pType) == 0) return -1; + } bitCnt+=(pLen-1); parityWd = 0; } @@ -589,9 +602,10 @@ size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t p //return ID start index and size return bitCnt; } + // Ask/Biphase Demod then try to locate an ISO 11784/85 ID // BitStream must contain previously askrawdemod and biphasedemoded data -int ISO11784demodBI(uint8_t *dest, size_t *size) +int FDXBdemodBI(uint8_t *dest, size_t *size) { //make sure buffer has enough data if (*size < 128) return -1; diff --git a/common/lfdemod.h b/common/lfdemod.h index f2cee27f..d16aab9e 100644 --- a/common/lfdemod.h +++ b/common/lfdemod.h @@ -19,6 +19,7 @@ int askdemod(uint8_t *BinStream, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType); int BiphaseRawDecode(uint8_t * BitStream, size_t *size, int offset, int invert); uint32_t bytebits_to_byte(uint8_t* src, size_t numbits); +uint32_t bytebits_to_byteLSBF(uint8_t* src, size_t numbits); uint16_t countFC(uint8_t *BitStream, size_t size, uint8_t fskAdj); int DetectASKClock(uint8_t dest[], size_t size, int *clock, int maxErr); uint8_t DetectCleanAskWave(uint8_t dest[], size_t size, uint8_t high, uint8_t low); @@ -40,6 +41,7 @@ void psk1TOpsk2(uint8_t *BitStream, size_t size); size_t removeParity(uint8_t *BitStream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen); //tag specific +int FDXBdemodBI(uint8_t *dest, size_t *size); int AWIDdemodFSK(uint8_t *dest, size_t *size); int gProxII_Demod(uint8_t BitStream[], size_t *size); int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo); @@ -47,5 +49,5 @@ int IOdemodFSK(uint8_t *dest, size_t size); int indala26decode(uint8_t *bitStream, size_t *size, uint8_t *invert); int PyramiddemodFSK(uint8_t *dest, size_t *size); int ParadoxdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo); -int ISO11784demodBI(uint8_t *dest, size_t *size); + #endif -- 2.39.5