+ // remove marker bits (1's every 9th digit after preamble) (pType = 2)
+ size = removeParity(BitStream, preambleIndex + 11, 9, 2, 117);
+ if ( size != 104 ) {
+ 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)
+{
+ int invert=0;
+ int clk=0;
+ int maxErr=100;
+ sscanf(Cmd, "%i %i %i", &clk, &invert, &maxErr);
+ if (clk==1){
+ invert=1;
+ clk=0;
+ }
+ if (invert != 0 && invert != 1) {
+ if (g_debugMode || verbose) PrintAndLog("Invalid argument: %s", Cmd);
+ return 0;
+ }
+ uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0};
+ size_t BitLen = getFromGraphBuf(BitStream);
+ if (BitLen==0) return 0;
+ uint8_t carrier=countFC(BitStream, BitLen, 0);
+ if (carrier!=2 && carrier!=4 && carrier!=8){
+ //invalid carrier
+ return 0;
+ }
+ if (g_debugMode){
+ PrintAndLog("Carrier: rf/%d",carrier);
+ }
+ int errCnt=0;
+ errCnt = pskRawDemod(BitStream, &BitLen, &clk, &invert);
+ if (errCnt > maxErr){
+ if (g_debugMode || verbose) PrintAndLog("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 (g_debugMode || verbose) PrintAndLog("no data found, clk: %d, invert: %d, numbits: %d, errCnt: %d",clk,invert,BitLen,errCnt);
+ return 0;
+ }
+ if (verbose || g_debugMode){
+ PrintAndLog("\nUsing Clock:%d, invert:%d, Bits Found:%d",clk,invert,BitLen);
+ if (errCnt>0){
+ PrintAndLog("# Errors during Demoding (shown as 7 in bit stream): %d",errCnt);
+ }
+ }
+ //prime demod buffer for output
+ setDemodBuf(BitStream,BitLen,0);
+ return 1;
+}
+
+// Indala 26 bit decode
+// by marshmellow
+// optional arguments - same as CmdpskNRZrawDemod (clock & invert)
+int CmdIndalaDecode(const char *Cmd)
+{
+ int ans;
+ if (strlen(Cmd)>0){
+ ans = PSKDemod(Cmd, 0);
+ } else{ //default to RF/32
+ ans = PSKDemod("32", 0);
+ }
+
+ if (!ans){
+ if (g_debugMode)
+ PrintAndLog("Error1: %d",ans);
+ return 0;
+ }
+ uint8_t invert=0;
+ size_t size = DemodBufferLen;
+ int startIdx = indala26decode(DemodBuffer, &size, &invert);
+ if (startIdx < 0 || size > 224) {
+ if (g_debugMode)
+ PrintAndLog("Error2: %d",ans);
+ return -1;
+ }
+ setDemodBuf(DemodBuffer, size, (size_t)startIdx);
+ if (invert)
+ if (g_debugMode)
+ PrintAndLog("Had to invert bits");
+
+ PrintAndLog("BitLen: %d",DemodBufferLen);
+ //convert UID to HEX
+ uint32_t uid1, uid2, uid3, uid4, uid5, uid6, uid7;
+ uid1=bytebits_to_byte(DemodBuffer,32);
+ uid2=bytebits_to_byte(DemodBuffer+32,32);
+ if (DemodBufferLen==64){
+ PrintAndLog("Indala UID=%s (%x%08x)", sprint_bin_break(DemodBuffer,DemodBufferLen,16), uid1, uid2);
+ } else {
+ uid3=bytebits_to_byte(DemodBuffer+64,32);
+ uid4=bytebits_to_byte(DemodBuffer+96,32);
+ uid5=bytebits_to_byte(DemodBuffer+128,32);
+ uid6=bytebits_to_byte(DemodBuffer+160,32);
+ uid7=bytebits_to_byte(DemodBuffer+192,32);
+ PrintAndLog("Indala UID=%s (%x%08x%08x%08x%08x%08x%08x)",
+ sprint_bin_break(DemodBuffer,DemodBufferLen,16), uid1, uid2, uid3, uid4, uid5, uid6, uid7);
+ }
+ if (g_debugMode){
+ PrintAndLog("DEBUG: printing demodbuffer:");
+ printDemodBuff();
+ }
+ 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
+int NRZrawDemod(const char *Cmd, bool verbose)
+{
+ int invert=0;
+ int clk=0;
+ int maxErr=100;
+ sscanf(Cmd, "%i %i %i", &clk, &invert, &maxErr);
+ if (clk==1){
+ invert=1;
+ clk=0;
+ }
+ if (invert != 0 && invert != 1) {
+ PrintAndLog("Invalid argument: %s", Cmd);
+ return 0;
+ }
+ uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0};
+ size_t BitLen = getFromGraphBuf(BitStream);
+ if (BitLen==0) return 0;
+ int errCnt=0;
+ errCnt = nrzRawDemod(BitStream, &BitLen, &clk, &invert);
+ if (errCnt > maxErr){
+ if (g_debugMode) PrintAndLog("Too many errors found, clk: %d, invert: %d, numbits: %d, errCnt: %d",clk,invert,BitLen,errCnt);
+ return 0;
+ }
+ if (errCnt<0 || BitLen<16){ //throw away static - allow 1 and -1 (in case of threshold command first)
+ if (g_debugMode) PrintAndLog("no data found, clk: %d, invert: %d, numbits: %d, errCnt: %d",clk,invert,BitLen,errCnt);
+ return 0;
+ }
+ if (verbose || g_debugMode) PrintAndLog("Tried NRZ Demod using Clock: %d - invert: %d - Bits Found: %d",clk,invert,BitLen);
+ //prime demod buffer for output
+ setDemodBuf(BitStream,BitLen,0);
+
+ if (errCnt>0 && (verbose || g_debugMode)) PrintAndLog("# Errors during Demoding (shown as 7 in bit stream): %d",errCnt);
+ if (verbose || g_debugMode) {
+ PrintAndLog("NRZ demoded bitstream:");
+ // Now output the bitstream to the scrollback by line of 16 bits
+ printDemodBuff();
+ }
+ return 1;
+}
+
+int CmdNRZrawDemod(const char *Cmd)
+{
+ char cmdp = param_getchar(Cmd, 0);
+ if (strlen(Cmd) > 10 || cmdp == 'h' || cmdp == 'H') {
+ PrintAndLog("Usage: data rawdemod nr [clock] <0|1> [maxError]");
+ PrintAndLog(" [set clock as integer] optional, if not set, autodetect.");
+ PrintAndLog(" <invert>, 1 for invert output");
+ PrintAndLog(" [set maximum allowed errors], default = 100.");
+ PrintAndLog("");
+ PrintAndLog(" sample: data rawdemod nr = demod a nrz/direct tag from GraphBuffer");
+ PrintAndLog(" : data rawdemod nr 32 = demod a nrz/direct tag from GraphBuffer using a clock of RF/32");
+ PrintAndLog(" : data rawdemod nr 32 1 = demod a nrz/direct tag from GraphBuffer using a clock of RF/32 and inverting data");
+ PrintAndLog(" : data rawdemod nr 1 = demod a nrz/direct tag from GraphBuffer while inverting data");
+ PrintAndLog(" : 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;
+ }
+ return NRZrawDemod(Cmd, TRUE);
+}
+
+// by marshmellow
+// 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 ans;
+ char cmdp = param_getchar(Cmd, 0);
+ if (strlen(Cmd) > 10 || cmdp == 'h' || cmdp == 'H') {
+ PrintAndLog("Usage: data rawdemod p1 [clock] <0|1> [maxError]");
+ PrintAndLog(" [set clock as integer] optional, if not set, autodetect.");
+ PrintAndLog(" <invert>, 1 for invert output");
+ PrintAndLog(" [set maximum allowed errors], default = 100.");
+ PrintAndLog("");
+ PrintAndLog(" sample: data rawdemod p1 = demod a psk1 tag from GraphBuffer");
+ PrintAndLog(" : data rawdemod p1 32 = demod a psk1 tag from GraphBuffer using a clock of RF/32");
+ PrintAndLog(" : data rawdemod p1 32 1 = demod a psk1 tag from GraphBuffer using a clock of RF/32 and inverting data");
+ PrintAndLog(" : data rawdemod p1 1 = demod a psk1 tag from GraphBuffer while inverting data");
+ PrintAndLog(" : 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;
+ }
+ ans = PSKDemod(Cmd, TRUE);
+ //output
+ if (!ans){
+ if (g_debugMode) PrintAndLog("Error demoding: %d",ans);
+ return 0;
+ }
+
+ PrintAndLog("PSK1 demoded bitstream:");
+ // Now output the bitstream to the scrollback by line of 16 bits
+ printDemodBuff();
+ return 1;
+}
+
+// by marshmellow
+// takes same args as cmdpsk1rawdemod
+int CmdPSK2rawDemod(const char *Cmd)
+{
+ int ans=0;
+ char cmdp = param_getchar(Cmd, 0);
+ if (strlen(Cmd) > 10 || cmdp == 'h' || cmdp == 'H') {
+ PrintAndLog("Usage: data rawdemod p2 [clock] <0|1> [maxError]");
+ PrintAndLog(" [set clock as integer] optional, if not set, autodetect.");
+ PrintAndLog(" <invert>, 1 for invert output");
+ PrintAndLog(" [set maximum allowed errors], default = 100.");
+ PrintAndLog("");
+ PrintAndLog(" sample: data rawdemod p2 = demod a psk2 tag from GraphBuffer, autodetect clock");
+ PrintAndLog(" : data rawdemod p2 32 = demod a psk2 tag from GraphBuffer using a clock of RF/32");
+ PrintAndLog(" : data rawdemod p2 32 1 = demod a psk2 tag from GraphBuffer using a clock of RF/32 and inverting output");
+ PrintAndLog(" : data rawdemod p2 1 = demod a psk2 tag from GraphBuffer, autodetect clock and invert output");
+ PrintAndLog(" : 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;
+ }
+ ans=PSKDemod(Cmd, TRUE);
+ if (!ans){
+ if (g_debugMode) PrintAndLog("Error demoding: %d",ans);
+ return 0;
+ }
+ psk1TOpsk2(DemodBuffer, DemodBufferLen);
+ PrintAndLog("PSK2 demoded bitstream:");
+ // Now output the bitstream to the scrollback by line of 16 bits
+ printDemodBuff();
+ return 1;
+}
+
+// by marshmellow - combines all raw demod functions into one menu command
+int CmdRawDemod(const char *Cmd)
+{
+ char cmdp = Cmd[0]; //param_getchar(Cmd, 0);
+
+ if (strlen(Cmd) > 20 || cmdp == 'h' || cmdp == 'H' || strlen(Cmd)<2) {
+ PrintAndLog("Usage: data rawdemod [modulation] <help>|<options>");
+ PrintAndLog(" [modulation] as 2 char, 'ab' for ask/biphase, 'am' for ask/manchester, 'ar' for ask/raw, 'fs' for fsk, ...");
+ PrintAndLog(" 'nr' for nrz/direct, 'p1' for psk1, 'p2' for psk2");
+ PrintAndLog(" <help> as 'h', prints the help for the specific modulation");
+ PrintAndLog(" <options> see specific modulation help for optional parameters");
+ PrintAndLog("");
+ PrintAndLog(" sample: data rawdemod fs h = print help specific to fsk demod");
+ PrintAndLog(" : data rawdemod fs = demod GraphBuffer using: fsk - autodetect");
+ PrintAndLog(" : data rawdemod ab = demod GraphBuffer using: ask/biphase - autodetect");
+ PrintAndLog(" : data rawdemod am = demod GraphBuffer using: ask/manchester - autodetect");
+ PrintAndLog(" : data rawdemod ar = demod GraphBuffer using: ask/raw - autodetect");
+ PrintAndLog(" : data rawdemod nr = demod GraphBuffer using: nrz/direct - autodetect");
+ PrintAndLog(" : data rawdemod p1 = demod GraphBuffer using: psk1 - autodetect");
+ PrintAndLog(" : data rawdemod p2 = demod GraphBuffer using: psk2 - autodetect");
+ return 0;
+ }
+ char cmdp2 = Cmd[1];
+ int ans = 0;
+ if (cmdp == 'f' && cmdp2 == 's'){
+ ans = CmdFSKrawdemod(Cmd+2);
+ } else if(cmdp == 'a' && cmdp2 == 'b'){
+ ans = Cmdaskbiphdemod(Cmd+2);
+ } else if(cmdp == 'a' && cmdp2 == 'm'){
+ ans = Cmdaskmandemod(Cmd+2);
+ } else if(cmdp == 'a' && cmdp2 == 'r'){
+ ans = Cmdaskrawdemod(Cmd+2);
+ } else if(cmdp == 'n' && cmdp2 == 'r'){
+ ans = CmdNRZrawDemod(Cmd+2);
+ } else if(cmdp == 'p' && cmdp2 == '1'){
+ ans = CmdPSK1rawDemod(Cmd+2);
+ } else if(cmdp == 'p' && cmdp2 == '2'){
+ ans = CmdPSK2rawDemod(Cmd+2);
+ } else {
+ PrintAndLog("unknown modulation entered - see help ('h') for parameter structure");
+ }
+ return ans;
projects / proxmark3-svn / blobdiff