fix: nice output..

[proxmark3-svn] / client / cmddata.c

diff --git a/client/cmddata.c b/client/cmddata.c
index a8c809cfb3df091186ba4c6b5cbcfed60054eeac..f09a5e3dbd75dc9cd8a105fe0e3abf8384ca9dff 100644 (file)
--- a/client/cmddata.c
+++ b/client/cmddata.c
@@ -23,10 +23,11 @@
 #include "lfdemod.h"
 #include "usb_cmd.h"
 #include "crc.h"
 #include "lfdemod.h"
 #include "usb_cmd.h"
 #include "crc.h"

+#include "crc16.h"

 
 uint8_t DemodBuffer[MAX_DEMOD_BUF_LEN];
 uint8_t g_debugMode;
 
 uint8_t DemodBuffer[MAX_DEMOD_BUF_LEN];
 uint8_t g_debugMode;

-int DemodBufferLen;
+size_t DemodBufferLen;

 static int CmdHelp(const char *Cmd);
 
 //set the demod buffer with given array of binary (one bit per byte)
 static int CmdHelp(const char *Cmd);
 
 //set the demod buffer with given array of binary (one bit per byte)


@@ -55,6 +56,15 @@ int CmdSetDebugMode(const char *Cmd)
        return 1;
 }
 
        return 1;
 }
 

+int usage_data_printdemodbuf(){
+               PrintAndLog("Usage: data printdemodbuffer x o <offset>");
+               PrintAndLog("Options:        ");
+               PrintAndLog("       h          This help");
+               PrintAndLog("       x          output in hex (omit for binary output)");
+               PrintAndLog("       o <offset> enter offset in # of bits");
+               return 0;       
+}
+

 //by marshmellow
 void printDemodBuff(void)
 {
 //by marshmellow
 void printDemodBuff(void)
 {


@@ -73,23 +83,50 @@ void printDemodBuff(void)
 
 int CmdPrintDemodBuff(const char *Cmd)
 {
 
 int CmdPrintDemodBuff(const char *Cmd)
 {

-       char hex;
-       char printBuff[512]={0x00};
-       uint8_t numBits = DemodBufferLen & 0xFFFC;
-       sscanf(Cmd, "%c", &hex);
-       if (hex == 'h'){
-               PrintAndLog("Usage: data printdemodbuffer [x]");
-               PrintAndLog("Options:        ");
-               PrintAndLog("       h       This help");
-               PrintAndLog("       x       output in hex (omit for binary output)");
-               return 0;
+       char hex[512]={0x00};
+       bool hexMode = false;
+       bool errors = false;
+       uint8_t offset = 0;
+       char cmdp = 0;
+       while(param_getchar(Cmd, cmdp) != 0x00)
+       {
+               switch(param_getchar(Cmd, cmdp))
+               {
+               case 'h':
+               case 'H':
+                       return usage_data_printdemodbuf();
+               case 'x':
+               case 'X':
+                       hexMode = true;
+                       cmdp++;
+                       break;
+               case 'o':
+               case 'O':
+                       offset = param_get8(Cmd, cmdp+1);
+                       if (!offset) errors = true;
+                       cmdp += 2;
+                       break;
+               default:
+                       PrintAndLog("Unknown parameter '%c'", param_getchar(Cmd, cmdp));
+                       errors = true;
+                       break;
+               }
+               if(errors) break;

        }
        }

-       if (hex == 'x'){
-               numBits = binarraytohex(printBuff, (char *)DemodBuffer, numBits);
+       //Validations
+       if(errors) return usage_data_printdemodbuf();
+
+       int numBits = (DemodBufferLen-offset) & 0x7FC; //make sure we don't exceed our string
+
+       if (hexMode){
+               char *buf = (char *) (DemodBuffer + offset);
+               numBits = binarraytohex(hex, buf, numBits);

                if (numBits==0) return 0;
                if (numBits==0) return 0;

-               PrintAndLog("DemodBuffer: %s",printBuff);
+               PrintAndLog("DemodBuffer: %s",hex);             

        } else {
        } else {

-               printDemodBuff();
+               //setDemodBuf(DemodBuffer, DemodBufferLen-offset, offset);
+               char *bin = sprint_bin_break(DemodBuffer+offset,numBits,16);
+               PrintAndLog("DemodBuffer:\n%s",bin);

        }
        return 1;
 }
        }
        return 1;
 }


@@ -462,9 +499,9 @@ int ASKbiphaseDemod(const char *Cmd, bool verbose)
 {
        //ask raw demod GraphBuffer first
        int offset=0, clk=0, invert=0, maxErr=0, ans=0;
 {
        //ask raw demod GraphBuffer first
        int offset=0, clk=0, invert=0, maxErr=0, ans=0;

-       ans = sscanf(Cmd, "%i %i %i %i", &offset, &clk, &invert, &maxErr);
+       ans = sscanf(Cmd, "%i %i 0 %i", &offset, &clk, &maxErr);

        if (ans>0)
        if (ans>0)

-               ans = ASKDemod(Cmd+1, FALSE, FALSE, 0);
+               ans = ASKDemod(Cmd+2, FALSE, FALSE, 0);

        else
                ans = ASKDemod(Cmd, FALSE, FALSE, 0);
        if (!ans) {
        else
                ans = ASKDemod(Cmd, FALSE, FALSE, 0);
        if (!ans) {


@@ -476,7 +513,7 @@ int ASKbiphaseDemod(const char *Cmd, bool verbose)
        size_t size = DemodBufferLen;
        uint8_t BitStream[MAX_DEMOD_BUF_LEN];
        memcpy(BitStream, DemodBuffer, DemodBufferLen); 
        size_t size = DemodBufferLen;
        uint8_t BitStream[MAX_DEMOD_BUF_LEN];
        memcpy(BitStream, DemodBuffer, DemodBufferLen); 

-       int errCnt = BiphaseRawDecode(BitStream, &size, offset, 0);
+       int errCnt = BiphaseRawDecode(BitStream, &size, offset, invert);

        if (errCnt < 0){
                if (g_debugMode || verbose) PrintAndLog("Error BiphaseRawDecode: %d", errCnt);
                return 0;
        if (errCnt < 0){
                if (g_debugMode || verbose) PrintAndLog("Error BiphaseRawDecode: %d", errCnt);
                return 0;


@@ -1421,6 +1458,94 @@ int CmdFSKdemodPyramid(const char *Cmd)
        return 1;
 }
 
        return 1;
 }
 

+// 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) or http://cms.abvma.ca/uploads/ManufacturersISOsandCountryCodes.pdf
+// FLAG (animal/non-animal)
+/*
+38 IDbits   
+10 country code 
+1 extra app bit
+14 reserved bits
+1 animal bit
+16 ccitt CRC chksum over 64bit ID CODE.
+24 appli bits.
+
+-- sample: 985121004515220  [ 37FF65B88EF94 ]
+*/
+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, maxErr, 0, 0);
+       if ( errCnt < 0 || errCnt > maxErr ) { 
+               if (g_debugMode) PrintAndLog("DEBUG: no data or error found %d, clock: %d", errCnt, clk);
+               return 0;
+       }
+
+       errCnt = BiphaseRawDecode(BitStream, &size, maxErr, 1);
+       if (errCnt < 0 || errCnt > maxErr ) {
+               if (g_debugMode) PrintAndLog("Error BiphaseRawDecode: %d", errCnt);
+               return 0;
+       } 
+       
+       int preambleIndex = FDXBdemodBI(BitStream, &size);
+       if (preambleIndex < 0){
+               if (g_debugMode) PrintAndLog("Error FDXBDemod , no startmarker found :: %d",preambleIndex);
+               return 0;
+       }
+       
+       setDemodBuf(BitStream, 128, preambleIndex);
+
+       // remove but don't verify parity. (pType = 2)
+       size = removeParity(BitStream, preambleIndex + 11, 9, 2, 117);
+       if ( size <= 103 ) {
+               if (g_debugMode) PrintAndLog("Error removeParity:: %d", size);
+               return 0;
+       }
+       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("Start marker %d;   Size %d", preambleIndex, size);
+
+       //got a good demod
+       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];
+       uint32_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) | bytebits_to_byte(BitStream+32,32);
+       uint8_t raw[8];
+       num_to_bytes(rawid, 8, raw);
+
+       if (g_debugMode) PrintAndLog("Raw ID Hex: %s", sprint_hex(raw,8));
+
+       uint16_t calcCrc = crc16_ccitt_kermit(raw, 8);
+       PrintAndLog("Animal ID:     %04u-%012llu", countryCode, NationalCode);
+       PrintAndLog("National Code: %012llu", NationalCode);
+       PrintAndLog("CountryCode:   %04u", countryCode);
+       PrintAndLog("Extended Data: %s", dataBlockBit ? "True" : "False");
+       PrintAndLog("reserved Code: %u", reservedCode);
+       PrintAndLog("Animal Tag:    %s", animalBit ? "True" : "False");
+       PrintAndLog("CRC:           0x%04X - [%04X] - %s", crc16, calcCrc, (calcCrc == crc16) ? "Passed" : "Failed");
+       PrintAndLog("Extended:      0x%X\n", extended);
+       
+       return 1;
+}
+
+

 //by marshmellow
 //attempt to psk1 demod graph buffer
 int PSKDemod(const char *Cmd, bool verbose)
 //by marshmellow
 //attempt to psk1 demod graph buffer
 int PSKDemod(const char *Cmd, bool verbose)


@@ -1484,7 +1609,7 @@ int CmdIndalaDecode(const char *Cmd)
                return 0;
        }
        uint8_t invert=0;
                return 0;
        }
        uint8_t invert=0;

-       ans = indala26decode(DemodBuffer,(size_t *) &DemodBufferLen, &invert);
+       ans = indala26decode(DemodBuffer, &DemodBufferLen, &invert);

        if (ans < 1) {
                if (g_debugMode==1)
                        PrintAndLog("Error2: %d",ans);
        if (ans < 1) {
                if (g_debugMode==1)
                        PrintAndLog("Error2: %d",ans);


@@ -1547,11 +1672,49 @@ int CmdIndalaDecode(const char *Cmd)
        return 1;
 }
 
        return 1;
 }
 

+int CmdPSKNexWatch(const char *Cmd)
+{
+       if (!PSKDemod("", false)) return 0;
+       uint8_t preamble[28] = {0,0,0,0,0,1,0,1,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+       size_t startIdx = 0, size = DemodBufferLen; 
+       bool invert = false;
+       if (!preambleSearch(DemodBuffer, preamble, sizeof(preamble), &size, &startIdx)){
+               // if didn't find preamble try again inverting
+               if (!PSKDemod("1", false)) return 0; 
+               size = DemodBufferLen;
+               if (!preambleSearch(DemodBuffer, preamble, sizeof(preamble), &size, &startIdx)) return 0;
+               invert = true;
+       } 
+       if (size != 128) return 0;
+       setDemodBuf(DemodBuffer, size, startIdx+4);
+       startIdx = 8+32; //4 = extra i added, 8 = preamble, 32 = reserved bits (always 0)
+       //get ID
+       uint32_t ID = 0;
+       for (uint8_t wordIdx=0; wordIdx<4; wordIdx++){
+               for (uint8_t idx=0; idx<8; idx++){
+                       ID = (ID << 1) | DemodBuffer[startIdx+wordIdx+(idx*4)];
+               }       
+       }
+       //parity check (TBD)
+
+       //checksum check (TBD)
+
+       //output
+       PrintAndLog("NexWatch ID: %d", ID);
+       if (invert){
+               PrintAndLog("Had to Invert - probably NexKey");
+               for (uint8_t idx=0; idx<size; idx++)
+                       DemodBuffer[idx] ^= 1;
+       } 
+
+       CmdPrintDemodBuff("x");
+       return 1;
+}
+

 // by marshmellow
 // takes 3 arguments - clock, invert, maxErr as integers
 // attempts to demodulate nrz only
 // prints binary found and saves in demodbuffer for further commands
 // by marshmellow
 // 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 invert=0;
 int NRZrawDemod(const char *Cmd, bool verbose)
 {
        int invert=0;


@@ -1797,7 +1960,7 @@ typedef struct {
        uint8_t * buffer;
        uint32_t numbits;
        uint32_t position;
        uint8_t * buffer;
        uint32_t numbits;
        uint32_t position;

-}BitstreamOut;
+} BitstreamOut;

 
 bool _headBit( BitstreamOut *stream)
 {
 
 bool _headBit( BitstreamOut *stream)
 {


@@ -1854,7 +2017,7 @@ int getSamples(const char *Cmd, bool silent)
                PrintAndLog("Unpacking...");
                BitstreamOut bout = { got, bits_per_sample * n,  0};
                int j =0;
                PrintAndLog("Unpacking...");
                BitstreamOut bout = { got, bits_per_sample * n,  0};
                int j =0;

-               for (j = 0; j * bits_per_sample < n * 8 && j < sizeof(GraphBuffer); j++) {
+               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;
                }
                        uint8_t sample = getByte(bits_per_sample, &bout);
                        GraphBuffer[j] = ((int) sample )- 128;
                }


@@ -2127,6 +2290,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)"},
        {"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"},
        {"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"},


@@ -2144,8 +2308,9 @@ 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)"},
        {"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] -- print the data in the DemodBuffer - 'x' for hex output"},
+       {"printdemodbuffer",CmdPrintDemodBuff,  1, "[x] [o] <offset> -- print the data in the DemodBuffer - 'x' for hex output"},

        {"pskindalademod",  CmdIndalaDecode,    1, "[clock] [invert<0|1>] -- Demodulate an indala tag (PSK1) from GraphBuffer (args optional)"},
        {"pskindalademod",  CmdIndalaDecode,    1, "[clock] [invert<0|1>] -- Demodulate an indala tag (PSK1) from GraphBuffer (args optional)"},

+       {"psknexwatchdemod",CmdPSKNexWatch,     1, "Demodulate a NexWatch tag (nexkey, quadrakey) (PSK1) from GraphBuffer"},

        {"rawdemod",        CmdRawDemod,        1, "[modulation] ... <options> -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, "<filename> -- Save trace (from graph window)"},
        {"rawdemod",        CmdRawDemod,        1, "[modulation] ... <options> -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, "<filename> -- Save trace (from graph window)"},