//----------------------------------------------------------------------------- // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details. // // This program is free software: you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // See LICENSE.txt for the text of the license. //----------------------------------------------------------------------------- // Low frequency demod/decode commands // // NOTES: // LF Demod functions are placed here to allow the flexibility to use client or // device side. Most BUT NOT ALL of these functions are currenlty safe for // device side use currently. (DetectST for example...) // // There are likely many improvements to the code that could be made, please // make suggestions... // // we tried to include author comments so any questions could be directed to // the source. // // There are 4 main sections of code below: // // Utilities Section: // for general utilities used by multiple other functions // // Clock / Bitrate Detection Section: // for clock detection functions for each modulation // // Modulation Demods &/or Decoding Section: // for main general modulation demodulating and encoding decoding code. // // Tag format detection section: // for detection of specific tag formats within demodulated data // // marshmellow //----------------------------------------------------------------------------- #include "lfdemod.h" #include // for memset, memcmp and size_t #include // qsort #include "parity.h" // for parity test #include "pm3_cmd.h" // error codes #include "commonutil.h" // Arraylen // ********************************************************************************************** // ---------------------------------Utilities Section-------------------------------------------- // ********************************************************************************************** #define LOWEST_DEFAULT_CLOCK 32 #define FSK_PSK_THRESHOLD 123 //to allow debug print calls when used not on dev #ifndef ON_DEVICE #include "ui.h" #include "util.h" # include "cmddata.h" # define prnt(args...) PrintAndLogEx(DEBUG, ## args ); #else # include "dbprint.h" uint8_t g_debugMode = 0; # define prnt Dbprintf #endif static signal_t signalprop = { 255, -255, 0, 0, true }; signal_t *getSignalProperties(void) { return &signalprop; } static void resetSignal(void) { signalprop.low = 255; signalprop.high = -255; signalprop.mean = 0; signalprop.amplitude = 0; signalprop.isnoise = true; } static void printSignal(void) { prnt("LF signal properties:"); prnt(" high..........%d", signalprop.high); prnt(" low...........%d", signalprop.low); prnt(" mean..........%d", signalprop.mean); prnt(" amplitude.....%d", signalprop.amplitude); prnt(" is Noise......%s", (signalprop.isnoise) ? _RED_("Yes") : _GREEN_("No")); prnt(" THRESHOLD noise amplitude......%d", NOISE_AMPLITUDE_THRESHOLD); } #ifndef ON_DEVICE static 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; } #endif void computeSignalProperties(const uint8_t *samples, uint32_t size) { resetSignal(); if (samples == NULL || size < SIGNAL_MIN_SAMPLES) return; uint32_t sum = 0; uint32_t offset_size = size - SIGNAL_IGNORE_FIRST_SAMPLES; #ifndef ON_DEVICE uint8_t tmp[offset_size]; memcpy(tmp, samples + SIGNAL_IGNORE_FIRST_SAMPLES, sizeof(tmp)); qsort(tmp, sizeof(tmp), sizeof(uint8_t), cmp_uint8); uint8_t low10 = 0.5 * (tmp[(int)(offset_size * 0.1)] + tmp[(int)((offset_size - 1) * 0.1)]); uint8_t hi90 = 0.5 * (tmp[(int)(offset_size * 0.9)] + tmp[(int)((offset_size - 1) * 0.9)]); uint32_t cnt = 0; for (uint32_t i = SIGNAL_IGNORE_FIRST_SAMPLES; i < size; i++) { if (samples[i] < signalprop.low) signalprop.low = samples[i]; if (samples[i] > signalprop.high) signalprop.high = samples[i]; if (samples[i] < low10 || samples[i] > hi90) continue; sum += samples[i]; cnt++; } if (cnt > 0) signalprop.mean = sum / cnt; else signalprop.mean = 0; #else for (uint32_t i = SIGNAL_IGNORE_FIRST_SAMPLES; i < size; i++) { if (samples[i] < signalprop.low) signalprop.low = samples[i]; if (samples[i] > signalprop.high) signalprop.high = samples[i]; sum += samples[i]; } signalprop.mean = sum / offset_size; #endif // measure amplitude of signal signalprop.amplitude = signalprop.high - signalprop.mean; // By measuring mean and look at amplitude of signal from HIGH / LOW, // we can detect noise signalprop.isnoise = signalprop.amplitude < NOISE_AMPLITUDE_THRESHOLD; if (g_debugMode) printSignal(); } void removeSignalOffset(uint8_t *samples, uint32_t size) { if (samples == NULL || size < SIGNAL_MIN_SAMPLES) { return; } int acc_off = 0; uint32_t offset_size = size - SIGNAL_IGNORE_FIRST_SAMPLES; #ifndef ON_DEVICE uint8_t tmp[offset_size]; memcpy(tmp, samples + SIGNAL_IGNORE_FIRST_SAMPLES, sizeof(tmp)); qsort(tmp, sizeof(tmp), sizeof(uint8_t), cmp_uint8); uint8_t low10 = 0.5 * (tmp[(int)(offset_size * 0.05)] + tmp[(int)((offset_size - 1) * 0.05)]); uint8_t hi90 = 0.5 * (tmp[(int)(offset_size * 0.95)] + tmp[(int)((offset_size - 1) * 0.95)]); int32_t cnt = 0; for (uint32_t i = SIGNAL_IGNORE_FIRST_SAMPLES; i < size; i++) { if (samples[i] < low10 || samples[i] > hi90) continue; acc_off += samples[i] - 128; cnt++; } if (cnt > 0) acc_off /= cnt; else acc_off = 0; #else for (uint32_t i = SIGNAL_IGNORE_FIRST_SAMPLES; i < size; i++) acc_off += samples[i] - 128; acc_off /= (int)offset_size; #endif // shift and saturate samples to center the mean for (uint32_t i = 0; i < size; i++) { if (acc_off > 0) { samples[i] = (samples[i] >= acc_off) ? samples[i] - acc_off : 0; } if (acc_off < 0) { samples[i] = (255 - samples[i] >= -acc_off) ? samples[i] - acc_off : 255; } } } // get high and low values of a wave with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise // void getHiLo(uint8_t *bits, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo) { void getHiLo(int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo) { // add fuzz. *high = (signalprop.high * fuzzHi) / 100; if (signalprop.low < 0) { *low = (signalprop.low * fuzzLo) / 100; } else { uint8_t range = signalprop.high - signalprop.low; *low = signalprop.low + ((range * (100 - fuzzLo)) / 100); } // if fuzzing to great and overlap if (*high <= *low) { *high = signalprop.high; *low = signalprop.low; } // prnt("getHiLo fuzzed: High %d | Low %d", *high, *low); } // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType // returns 1 if passed bool parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) { return oddparity32(bits) ^ pType; } // takes a array of binary values, start position, length of bits per parity (includes parity bit - MAX 32), // Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run) size_t removeParity(uint8_t *bits, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) { uint32_t parityWd = 0; size_t bitCnt = 0; for (int word = 0; word < (bLen); word += pLen) { for (int bit = 0; bit < pLen; bit++) { if (word + bit >= bLen) break; parityWd = (parityWd << 1) | bits[startIdx + word + bit]; bits[bitCnt++] = (bits[startIdx + word + bit]); } if (word + pLen > bLen) break; bitCnt--; // overwrite parity with next data // if parity fails then return 0 switch (pType) { case 3: if (bits[bitCnt] == 1) { return 0; } break; // should be 0 spacer bit case 2: if (bits[bitCnt] == 0) { return 0; } break; // should be 1 spacer bit default: if (parityTest(parityWd, pLen, pType) == 0) { return 0; } break; // test parity } parityWd = 0; } // if we got here then all the parities passed //return size return bitCnt; } static size_t removeEm410xParity(uint8_t *bits, size_t startIdx, size_t *size, bool *validShort, bool *validShortExtended, bool *validLong) { uint32_t parityWd = 0; size_t bitCnt = 0; bool validColParity = false; bool validRowParity = true; bool validRowParitySkipColP = true; *validShort = false; *validShortExtended = false; *validLong = false; uint8_t blen = 55; switch (*size) { case 128: blen = 110; break; case 80: blen = 70; break; default: blen = 55; break; } uint16_t parityCol[4] = { 0, 0, 0, 0 }; for (int word = 0; word < blen; word += 5) { for (int bit = 0; bit < 5; bit++) { if (word + bit >= blen) { break; } parityWd = (parityWd << 1) | bits[startIdx + word + bit]; if ((word <= 50) && (bit < 4)) { parityCol[bit] = (parityCol[bit] << 1) | bits[startIdx + word + bit]; } bits[bitCnt++] = (bits[startIdx + word + bit]); } if (word + 5 > blen) break; bitCnt--; // overwrite parity with next data validRowParity &= parityTest(parityWd, 5, 0) != 0; if (word == 50) { // column parity nibble on short EM and on Electra validColParity = parityTest(parityCol[0], 11, 0) != 0; validColParity &= parityTest(parityCol[1], 11, 0) != 0; validColParity &= parityTest(parityCol[2], 11, 0) != 0; validColParity &= parityTest(parityCol[3], 11, 0) != 0; } else { validRowParitySkipColP &= parityTest(parityWd, 5, 0) != 0; } parityWd = 0; } if ((blen != 128) && validRowParitySkipColP && validColParity) { *validShort = true; } if ((blen == 128) && validRowParity) { *validLong = true; } if ((blen == 128) && validRowParitySkipColP && validColParity) { *validShortExtended = true; } if (*validShort || *validShortExtended || *validLong) { return bitCnt; } else { return 0; } } // takes a array of binary values, length of bits per parity (includes parity bit), // Parity Type (1 for odd; 0 for even; 2 Always 1's; 3 Always 0's), and binary Length (length to run) // Make sure *dest is long enough to store original sourceLen + #_of_parities_to_be_added size_t addParity(const uint8_t *src, uint8_t *dest, uint8_t sourceLen, uint8_t pLen, uint8_t pType) { uint32_t parityWd = 0; size_t j = 0, bitCnt = 0; for (int word = 0; word < sourceLen; word += pLen - 1) { for (int bit = 0; bit < pLen - 1; bit++) { parityWd = (parityWd << 1) | src[word + bit]; dest[j++] = (src[word + bit]); } // if parity fails then return 0 switch (pType) { case 3: dest[j++] = 0; break; // marker bit which should be a 0 case 2: dest[j++] = 1; break; // marker bit which should be a 1 default: dest[j++] = parityTest(parityWd, pLen - 1, pType) ^ 1; break; } bitCnt += pLen; parityWd = 0; } // if we got here then all the parities passed //return ID start index and size return bitCnt; } // array must be size dividable with 8 int bits_to_array(const uint8_t *bits, size_t size, uint8_t *dest) { if ((size == 0) || (size % 8) != 0) return PM3_EINVARG; for (uint32_t i = 0; i < (size / 8); i++) dest[i] = bytebits_to_byte((uint8_t *) bits + (i * 8), 8); return PM3_SUCCESS; } uint32_t bytebits_to_byte(uint8_t *src, size_t numbits) { uint32_t num = 0; for (int i = 0 ; i < numbits ; i++) { num = (num << 1) | (*src); src++; } 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; } // search for given preamble in given BitStream and return success = TRUE or fail = FALSE and startIndex and length bool preambleSearch(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx) { return preambleSearchEx(bits, preamble, pLen, size, startIdx, false); } // search for given preamble in given BitStream and return success=1 or fail=0 and startIndex (where it was found) and length if not fineone // fineone does not look for a repeating preamble for em4x05/4x69 sends preamble once, so look for it once in the first pLen bits // (iceman) FINDONE, only finds start index. NOT SIZE!. I see Em410xDecode (lfdemod.c) uses SIZE to determine success bool preambleSearchEx(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx, bool findone) { // Sanity check. If preamble length is bigger than bits length. if (*size <= pLen) return false; uint8_t foundCnt = 0; for (size_t idx = 0; idx < *size - pLen; idx++) { if (memcmp(bits + idx, preamble, pLen) == 0) { //first index found foundCnt++; if (foundCnt == 1) { if (g_debugMode >= 1) prnt("DEBUG: (preambleSearchEx) preamble found at %zu", idx); *startIdx = idx; if (findone) return true; } if (foundCnt == 2) { if (g_debugMode >= 1) prnt("DEBUG: (preambleSearchEx) preamble 2 found at %zu", idx); *size = idx - *startIdx; return true; } } } return (foundCnt > 0); } // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup. static size_t findModStart(const uint8_t *src, size_t size, uint8_t expWaveSize) { size_t i = 0; size_t waveSizeCnt = 0; uint8_t thresholdCnt = 0; // FSK_PSK_THRESHOLD; bool isAboveThreshold = (src[i++] >= signalprop.mean); for (; i < size - 20; i++) { if (src[i] < signalprop.mean && isAboveThreshold) { thresholdCnt++; if (thresholdCnt > 2 && waveSizeCnt < expWaveSize + 1) { break; } isAboveThreshold = false; waveSizeCnt = 0; } else if (src[i] >= signalprop.mean && !isAboveThreshold) { thresholdCnt++; if (thresholdCnt > 2 && waveSizeCnt < expWaveSize + 1) { break; } isAboveThreshold = true; waveSizeCnt = 0; } else { waveSizeCnt++; } if (thresholdCnt > 10) { break; } } if (g_debugMode == 2) { prnt("DEBUG: threshold Count reached at index %zu, count: %u", i, thresholdCnt); } return i; } static int getClosestClock(int testclk) { const uint16_t clocks[] = {8, 16, 32, 40, 50, 64, 100, 128, 256, 272, 384}; const uint8_t limit[] = {1, 2, 4, 4, 5, 8, 8, 8, 8, 24, 24}; for (uint8_t i = 0; i < ARRAYLEN(clocks); i++) { if (testclk >= clocks[i] - limit[i] && testclk <= clocks[i] + limit[i]) return clocks[i]; } return 0; } void getNextLow(const uint8_t *samples, size_t size, int low, size_t *i) { while ((samples[*i] > low) && (*i < size)) *i += 1; } void getNextHigh(const uint8_t *samples, size_t size, int high, size_t *i) { while ((samples[*i] < high) && (*i < size)) *i += 1; } // load wave counters bool loadWaveCounters(uint8_t *samples, size_t size, int lowToLowWaveLen[], int highToLowWaveLen[], int *waveCnt, int *skip, int *minClk, int *high, int *low) { size_t i = 0; //size_t testsize = (size < 512) ? size : 512; // just noise - no super good detection. good enough if (signalprop.isnoise) { if (g_debugMode == 2) prnt("DEBUG STT: just noise detected - quitting"); return false; } getHiLo(high, low, 80, 80); // get to first full low to prime loop and skip incomplete first pulse getNextHigh(samples, size, *high, &i); getNextLow(samples, size, *low, &i); *skip = i; // populate tmpbuff buffer with pulse lengths while (i < size) { // measure from low to low size_t firstLow = i; //find first high point for this wave getNextHigh(samples, size, *high, &i); size_t firstHigh = i; getNextLow(samples, size, *low, &i); if (*waveCnt >= (size / LOWEST_DEFAULT_CLOCK)) break; highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low lowToLowWaveLen[*waveCnt] = i - firstLow; *waveCnt += 1; if (i - firstLow < *minClk && i < size) { *minClk = i - firstLow; } } return true; } size_t pskFindFirstPhaseShift(const uint8_t *samples, size_t size, uint8_t *curPhase, size_t waveStart, uint16_t fc, uint16_t *fullWaveLen) { uint16_t loopCnt = (size + 3 < 4096) ? size : 4096; //don't need to loop through entire array... uint16_t avgWaveVal = 0, lastAvgWaveVal; size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave; for (; i < loopCnt; i++) { // find peak // was "samples[i] + fc" but why? must have been used to weed out some wave error... removed.. if (samples[i] < samples[i + 1] && samples[i + 1] >= samples[i + 2]) { waveEnd = i + 1; if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %zu, waveStart: %zu", waveEnd, waveStart); waveLenCnt = waveEnd - waveStart; if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc + 8)) { //not first peak and is a large wave but not out of whack lastAvgWaveVal = avgWaveVal / (waveLenCnt); firstFullWave = waveStart; *fullWaveLen = waveLenCnt; //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting) if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1; return firstFullWave; } waveStart = i + 1; avgWaveVal = 0; } avgWaveVal += samples[i + 2]; } return 0; } // amplify based on ask edge detection - not accurate enough to use all the time void askAmp(uint8_t *bits, size_t size) { uint8_t last = 128; for (size_t i = 1; i < size; ++i) { if (bits[i] - bits[i - 1] >= 30) //large jump up last = 255; else if (bits[i - 1] - bits[i] >= 20) //large jump down last = 0; bits[i] = last; } } // iceman, simplify this uint32_t manchesterEncode2Bytes(uint16_t datain) { uint32_t output = 0; for (uint8_t i = 0; i < 16; i++) { uint8_t b = (datain >> (15 - i) & 1); output |= (1 << (((15 - i) * 2) + b)); } return output; } void manchesterEncodeUint32(uint32_t data_in, uint8_t bitlen_in, uint8_t *bits_out, uint16_t *index) { for (int i = bitlen_in - 1; i >= 0; i--) { if ((data_in >> i) & 1) { bits_out[(*index)++] = 1; bits_out[(*index)++] = 0; } else { bits_out[(*index)++] = 0; bits_out[(*index)++] = 1; } } } // encode binary data into binary manchester // NOTE: bitstream must have triple the size of "size" available in memory to do the swap int ManchesterEncode(uint8_t *bits, size_t size) { //allow up to 4096b out (means bits must be at least 2048+4096 to handle the swap) size = (size > 2048) ? 2048 : size; size_t modIdx = size; size_t i; for (size_t idx = 0; idx < size; idx++) { bits[idx + modIdx++] = bits[idx]; bits[idx + modIdx++] = bits[idx] ^ 1; } for (i = 0; i < (size * 2); i++) { bits[i] = bits[i + size]; } return i; } // to detect a wave that has heavily clipped (clean) samples // loop 1024 samples, if 250 of them is deemed maxed out, we assume the wave is clipped. bool DetectCleanAskWave(const uint8_t *dest, size_t size, uint8_t high, uint8_t low) { bool allArePeaks = true; uint16_t cntPeaks = 0; size_t loopEnd = 1024 + 160; // sanity check if (loopEnd > size) loopEnd = size; for (size_t i = 160; i < loopEnd; i++) { if (dest[i] > low && dest[i] < high) allArePeaks = false; else { cntPeaks++; //if (g_debugMode == 2) prnt("DEBUG DetectCleanAskWave: peaks (200) %u", cntPeaks); if (cntPeaks > 200) return true; } } if (allArePeaks == false) { if (g_debugMode == 2) prnt("DEBUG DetectCleanAskWave: peaks (200) %u", cntPeaks); if (cntPeaks > 200) return true; } return allArePeaks; } // ********************************************************************************************** // -------------------Clock / Bitrate Detection Section------------------------------------------ // ********************************************************************************************** // to help detect clocks on heavily clipped samples // based on count of low to low int DetectStrongAskClock(uint8_t *dest, size_t size, int high, int low, int *clock) { size_t i = 100; size_t minClk = 768; uint16_t shortestWaveIdx = 0; // get to first full low to prime loop and skip incomplete first pulse getNextHigh(dest, size, high, &i); getNextLow(dest, size, low, &i); if (i == size) return -1; if (size < 768) return -2; // clock, numoftimes, first idx uint16_t tmpclk[11][3] = { {8, 0, 0}, {16, 0, 0}, {32, 0, 0}, {40, 0, 0}, {50, 0, 0}, {64, 0, 0}, {100, 0, 0}, {128, 0, 0}, {256, 0, 0}, {272, 0, 0}, {384, 0, 0}, }; // loop through all samples (well, we don't want to go out-of-bounds) while (i < (size - 768)) { // measure from low to low size_t startwave = i; getNextHigh(dest, size, high, &i); getNextLow(dest, size, low, &i); //get minimum measured distance if (i - startwave < minClk && i < size) { minClk = i - startwave; shortestWaveIdx = startwave; } int foo = getClosestClock(minClk); if (foo > 0) { for (uint8_t j = 0; j < 11; j++) { if (tmpclk[j][0] == foo) { tmpclk[j][1]++; if (tmpclk[j][2] == 0) { tmpclk[j][2] = shortestWaveIdx; } break; } } } } // find the clock with most hits and it the first index it was encountered. int possible_clks = 0; for (uint8_t j = 0; j < 11; j++) { if (tmpclk[j][1] > 0) { possible_clks++; } } uint16_t second_shortest = 0; int second = 0; int max = 0; for (int j = 10; j > -1; j--) { if (g_debugMode == 2) { prnt("DEBUG, ASK, clocks %u | hits %u | idx %u" , tmpclk[j][0] , tmpclk[j][1] , tmpclk[j][2] ); } if (max < tmpclk[j][1]) { second = *clock; second_shortest = shortestWaveIdx; *clock = tmpclk[j][0]; shortestWaveIdx = tmpclk[j][2]; max = tmpclk[j][1]; } } // ASK clock 8 is very rare and usually gives us false positives if (possible_clks > 1 && *clock == 8) { *clock = second; shortestWaveIdx = second_shortest; } if (*clock == 0) return -1; return shortestWaveIdx; } // not perfect especially with lower clocks or VERY good antennas (heavy wave clipping) // maybe somehow adjust peak trimming value based on samples to fix? // return start index of best starting position for that clock and return clock (by reference) int DetectASKClock(uint8_t *dest, size_t size, int *clock, int maxErr) { //don't need to loop through entire array. (cotag has clock of 384) uint16_t loopCnt = 1000; // not enough samples if (size <= loopCnt + 60) { if (g_debugMode == 2) prnt("DEBUG DetectASKClock: not enough samples - aborting"); return -1; } // just noise - no super good detection. good enough if (signalprop.isnoise) { if (g_debugMode == 2) prnt("DEBUG DetectASKClock: just noise detected - aborting"); return -2; } size_t i = 1; uint8_t num_clks = 10; // first 255 value pos0 is placeholder for user inputed clock. uint16_t clk[] = {255, 8, 16, 32, 40, 50, 64, 100, 128, 255, 272}; // sometimes there is a strange end wave - filter out this size -= 60; // What is purpose? // already have a valid clock? uint8_t found_clk = 0; for (; i < num_clks; ++i) { if (clk[i] == *clock) { found_clk = i; } } // threshold 75% of high, low peak int peak_hi, peak_low; getHiLo(&peak_hi, &peak_low, 75, 75); // test for large clean, STRONG, CLIPPED peaks if (!found_clk) { if (DetectCleanAskWave(dest, size, peak_hi, peak_low)) { int idx = DetectStrongAskClock(dest, size, peak_hi, peak_low, clock); if (g_debugMode == 2) prnt("DEBUG ASK: DetectASKClock Clean ASK Wave detected: clk %i, Best Starting Position: %i", *clock, idx); // return shortest wave start position if (idx > -1) return idx; } } // test for weak peaks // test clock if given as cmd parameter if (*clock > 0) clk[0] = *clock; uint8_t clkCnt, tol; size_t j = 0; uint16_t bestErr[] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; uint8_t bestStart[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; size_t errCnt, arrLoc, loopEnd; if (found_clk) { clkCnt = found_clk; num_clks = found_clk + 1; } else { clkCnt = 1; } //test each valid clock from smallest to greatest to see which lines up for (; clkCnt < num_clks; clkCnt++) { if (clk[clkCnt] <= 32) { tol = 1; } else { tol = 0; } //if no errors allowed - keep start within the first clock if (!maxErr && size > clk[clkCnt] * 2 + tol && clk[clkCnt] < 128) loopCnt = clk[clkCnt] * 2; bestErr[clkCnt] = 1000; //try lining up the peaks by moving starting point (try first few clocks) // get to first full low to prime loop and skip incomplete first pulse getNextHigh(dest, size, peak_hi, &j); getNextLow(dest, size, peak_low, &j); for (; j < loopCnt; j++) { errCnt = 0; // now that we have the first one lined up test rest of wave array loopEnd = ((size - j - tol) / clk[clkCnt]) - 1; for (i = 0; i < loopEnd; ++i) { arrLoc = j + (i * clk[clkCnt]); if (dest[arrLoc] >= peak_hi || dest[arrLoc] <= peak_low) { } else if (dest[arrLoc - tol] >= peak_hi || dest[arrLoc - tol] <= peak_low) { } else if (dest[arrLoc + tol] >= peak_hi || dest[arrLoc + tol] <= peak_low) { } else { //error no peak detected errCnt++; } } // if we found no errors then we can stop here and a low clock (common clocks) // this is correct one - return this clock // if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d", clk[clkCnt], errCnt, j, i); if (errCnt == 0 && clkCnt < 7) { if (!found_clk) *clock = clk[clkCnt]; return j; } // if we found errors see if it is lowest so far and save it as best run if (errCnt < bestErr[clkCnt]) { bestErr[clkCnt] = errCnt; bestStart[clkCnt] = j; } } } uint8_t k, best = 0; for (k = 1; k < num_clks; ++k) { if (bestErr[k] < bestErr[best]) { if (bestErr[k] == 0) bestErr[k] = 1; // current best bit to error ratio vs new bit to error ratio if ((size / clk[best]) / bestErr[best] < (size / clk[k]) / bestErr[k]) { best = k; } } //if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d", clk[k], bestErr[k], clk[best], bestStart[best]); } bool chg = false; for (i = 0; i < ARRAYLEN(bestErr); i++) { chg = (bestErr[i] != 1000); if (chg) break; chg = (bestStart[i] != 0); if (chg) break; } // just noise - no super good detection. good enough if (chg == false) { if (g_debugMode == 2) prnt("DEBUG DetectASKClock: no good values detected - aborting"); return -2; } if (!found_clk) *clock = clk[best]; return bestStart[best]; } int DetectStrongNRZClk(const uint8_t *dest, size_t size, int peak, int low, bool *strong) { //find shortest transition from high to low *strong = false; size_t i = 0; size_t transition1 = 0; int lowestTransition = 255; bool lastWasHigh = false; size_t transitionSampleCount = 0; //find first valid beginning of a high or low wave while ((dest[i] >= peak || dest[i] <= low) && (i < size)) ++i; while ((dest[i] < peak && dest[i] > low) && (i < size)) ++i; lastWasHigh = (dest[i] >= peak); if (i == size) return 0; transition1 = i; for (; i < size; i++) { if ((dest[i] >= peak && !lastWasHigh) || (dest[i] <= low && lastWasHigh)) { lastWasHigh = (dest[i] >= peak); if (i - transition1 < lowestTransition) lowestTransition = i - transition1; transition1 = i; } else if (dest[i] < peak && dest[i] > low) { transitionSampleCount++; } } if (lowestTransition == 255) lowestTransition = 0; if (g_debugMode == 2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d", lowestTransition); // if less than 10% of the samples were not peaks (or 90% were peaks) then we have a strong wave if (transitionSampleCount / size < 10) { *strong = true; lowestTransition = getClosestClock(lowestTransition); } return lowestTransition; } // detect nrz clock by reading #peaks vs no peaks(or errors) int DetectNRZClock(uint8_t *dest, size_t size, int clock, size_t *clockStartIdx) { size_t i = 0; uint16_t clk[] = {8, 16, 32, 40, 50, 64, 100, 128, 255, 272, 384}; size_t loopCnt = 4096; //don't need to loop through entire array... //if we already have a valid clock quit for (; i < ARRAYLEN(clk); ++i) if (clk[i] == clock) return clock; if (size < 20) return 0; // size must be larger than 20 here if (size < loopCnt) loopCnt = size - 20; // just noise - no super good detection. good enough if (signalprop.isnoise) { if (g_debugMode == 2) prnt("DEBUG DetectNZRClock: just noise detected - quitting"); return 0; } //get high and low peak int peak, low; //getHiLo(dest, loopCnt, &peak, &low, 90, 90); getHiLo(&peak, &low, 90, 90); bool strong = false; int lowestTransition = DetectStrongNRZClk(dest, size - 20, peak, low, &strong); if (strong) return lowestTransition; size_t ii; uint8_t clkCnt; uint8_t tol = 0; uint16_t smplCnt = 0; int16_t peakcnt = 0; int16_t peaksdet[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint16_t minPeak = 255; bool firstpeak = true; //test for large clipped waves - ignore first peak for (i = 0; i < loopCnt; i++) { if (dest[i] >= peak || dest[i] <= low) { if (firstpeak) continue; smplCnt++; } else { firstpeak = false; if (smplCnt > 0) { if (minPeak > smplCnt && smplCnt > 7) minPeak = smplCnt; peakcnt++; if (g_debugMode == 2) prnt("DEBUG NRZ: minPeak: %d, smplCnt: %d, peakcnt: %d", minPeak, smplCnt, peakcnt); smplCnt = 0; } } } if (minPeak < 8) return 0; bool errBitHigh = 0, bitHigh = 0, lastPeakHigh = 0; uint8_t ignoreCnt = 0, ignoreWindow = 4; int lastBit = 0; size_t bestStart[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; peakcnt = 0; //test each valid clock from smallest to greatest to see which lines up for (clkCnt = 0; clkCnt < ARRAYLEN(bestStart); ++clkCnt) { //ignore clocks smaller than smallest peak if (clk[clkCnt] < minPeak - (clk[clkCnt] / 4)) continue; //try lining up the peaks by moving starting point (try first 256) for (ii = 20; ii < loopCnt; ++ii) { if ((dest[ii] >= peak) || (dest[ii] <= low)) { peakcnt = 0; bitHigh = false; ignoreCnt = 0; lastBit = ii - clk[clkCnt]; //loop through to see if this start location works for (i = ii; i < size - 20; ++i) { //if we are at a clock bit if ((i >= lastBit + clk[clkCnt] - tol) && (i <= lastBit + clk[clkCnt] + tol)) { //test high/low if (dest[i] >= peak || dest[i] <= low) { //if same peak don't count it if ((dest[i] >= peak && !lastPeakHigh) || (dest[i] <= low && lastPeakHigh)) { peakcnt++; } lastPeakHigh = (dest[i] >= peak); bitHigh = true; errBitHigh = false; ignoreCnt = ignoreWindow; lastBit += clk[clkCnt]; } else if (i == lastBit + clk[clkCnt] + tol) { lastBit += clk[clkCnt]; } //else if not a clock bit and no peaks } else if (dest[i] < peak && dest[i] > low) { if (ignoreCnt == 0) { bitHigh = false; if (errBitHigh == true) peakcnt--; errBitHigh = false; } else { ignoreCnt--; } // else if not a clock bit but we have a peak } else if ((dest[i] >= peak || dest[i] <= low) && (!bitHigh)) { //error bar found no clock... errBitHigh = true; } } if (peakcnt > peaksdet[clkCnt]) { bestStart[clkCnt] = ii; peaksdet[clkCnt] = peakcnt; } } } } uint8_t best = 0; for (int m = ARRAYLEN(peaksdet) - 1; m >= 0; m--) { if ((peaksdet[m] >= (peaksdet[best] - 1)) && (peaksdet[m] <= peaksdet[best] + 1) && lowestTransition) { if (clk[m] > (lowestTransition - (clk[m] / 8)) && clk[m] < (lowestTransition + (clk[m] / 8))) { best = m; } } else if (peaksdet[m] > peaksdet[best]) { best = m; } if (g_debugMode == 2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, minPeak: %d, bestClk: %d, lowestTrs: %d", clk[m], peaksdet[m], minPeak, clk[best], lowestTransition); } *clockStartIdx = bestStart[best]; return clk[best]; } // countFC is to detect the field clock lengths. // counts and returns the 2 most common wave lengths // mainly used for FSK field clock detection uint16_t countFC(const uint8_t *bits, size_t size, bool fskAdj) { uint8_t fcLens[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint16_t fcCnts[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t fcLensFnd = 0; uint8_t lastFCcnt = 0; uint8_t fcCounter = 0; size_t i; if (size < 180) return 0; // prime i to first up transition for (i = 160; i < size - 20; i++) if (bits[i] > bits[i - 1] && bits[i] >= bits[i + 1]) break; for (; i < size - 20; i++) { if (bits[i] > bits[i - 1] && bits[i] >= bits[i + 1]) { // new up transition fcCounter++; if (fskAdj) { //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8) if (lastFCcnt == 5 && fcCounter == 9) fcCounter--; //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5) if ((fcCounter == 9) || fcCounter == 4) fcCounter++; // save last field clock count (fc/xx) lastFCcnt = fcCounter; } // find which fcLens to save it to: for (int m = 0; m < 15; m++) { if (fcLens[m] == fcCounter) { fcCnts[m]++; fcCounter = 0; break; } } if (fcCounter > 0 && fcLensFnd < 15) { //add new fc length fcCnts[fcLensFnd]++; fcLens[fcLensFnd++] = fcCounter; } fcCounter = 0; } else { // count sample fcCounter++; } } uint8_t best1 = 14, best2 = 14, best3 = 14; uint16_t maxCnt1 = 0; // go through fclens and find which ones are bigest 2 for (i = 0; i < 15; i++) { // get the 3 best FC values if (fcCnts[i] > maxCnt1) { best3 = best2; best2 = best1; maxCnt1 = fcCnts[i]; best1 = i; } else if (fcCnts[i] > fcCnts[best2]) { best3 = best2; best2 = i; } else if (fcCnts[i] > fcCnts[best3]) { best3 = i; } if (g_debugMode == 2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u", fcLens[i], fcCnts[i], fcLens[best1], fcLens[best2]); if (fcLens[i] == 0) break; } if (fcLens[best1] == 0) return 0; uint8_t fcH = 0, fcL = 0; if (fcLens[best1] > fcLens[best2]) { fcH = fcLens[best1]; fcL = fcLens[best2]; } else { fcH = fcLens[best2]; fcL = fcLens[best1]; } /* if ((size - 180) / fcH / 3 > fcCnts[best1] + fcCnts[best2]) { if (g_debugMode == 2) prnt("DEBUG countfc: fc is too large: %zu > %u. Not psk or fsk", (size - 180) / fcH / 3, fcCnts[best1] + fcCnts[best2]); return 0; //lots of waves not psk or fsk } */ // TODO: take top 3 answers and compare to known Field clocks to get top 2 uint16_t fcs = (((uint16_t)fcH) << 8) | fcL; if (fskAdj) return fcs; return (uint16_t)fcLens[best2] << 8 | fcLens[best1]; } // detect psk clock by reading each phase shift // a phase shift is determined by measuring the sample length of each wave int DetectPSKClock(uint8_t *dest, size_t size, int clock, size_t *firstPhaseShift, uint8_t *curPhase, uint8_t *fc) { uint16_t clk[] = {255, 16, 32, 40, 50, 64, 100, 128, 256, 272, 384}; // 255 is not a valid clock uint16_t loopCnt = 4096; // don't need to loop through entire array... if (size < 160 + 20) return 0; // size must be larger than 20 here, and 160 later on. if (size < loopCnt) loopCnt = size - 20; uint16_t fcs = countFC(dest, size, 0); *fc = fcs & 0xFF; if (g_debugMode == 2) prnt("DEBUG PSK: FC: %d, FC2: %d", *fc, fcs >> 8); if ((fcs >> 8) == 10 && *fc == 8) return 0; if (*fc != 2 && *fc != 4 && *fc != 8) return 0; size_t waveEnd, firstFullWave = 0; uint8_t clkCnt; uint16_t waveLenCnt, fullWaveLen = 0; uint16_t bestErr[] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; uint16_t peaksdet[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; //find start of modulating data in trace size_t i = findModStart(dest, size, *fc); firstFullWave = pskFindFirstPhaseShift(dest, size, curPhase, i, *fc, &fullWaveLen); if (firstFullWave == 0) { // no phase shift detected - could be all 1's or 0's - doesn't matter where we start // so skip a little to ensure we are past any Start Signal firstFullWave = 160; fullWaveLen = 0; } *firstPhaseShift = firstFullWave; if (g_debugMode == 2) prnt("DEBUG PSK: firstFullWave: %zu, waveLen: %d", firstFullWave, fullWaveLen); // Avoid autodetect if user selected a clock for (uint8_t validClk = 1; validClk < 8; validClk++) { if (clock == clk[validClk]) return (clock); } //test each valid clock from greatest to smallest to see which lines up for (clkCnt = 9; clkCnt >= 1 ; clkCnt--) { uint8_t tol = *fc / 2; size_t lastClkBit = firstFullWave; //set end of wave as clock align size_t waveStart = 0; uint16_t errCnt = 0; uint16_t peakcnt = 0; if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %zu", clk[clkCnt], lastClkBit); for (i = firstFullWave + fullWaveLen - 1; i < loopCnt - 2; i++) { //top edge of wave = start of new wave if (dest[i] < dest[i + 1] && dest[i + 1] >= dest[i + 2]) { if (waveStart == 0) { waveStart = i + 1; } else { //waveEnd waveEnd = i + 1; waveLenCnt = waveEnd - waveStart; if (waveLenCnt > *fc) { //if this wave is a phase shift if (g_debugMode == 2) prnt("DEBUG PSK: phase shift at: %zu, len: %d, nextClk: %zu, i: %zu, fc: %d", waveStart, waveLenCnt, lastClkBit + clk[clkCnt] - tol, i + 1, *fc); if (i + 1 >= lastClkBit + clk[clkCnt] - tol) { //should be a clock bit peakcnt++; lastClkBit += clk[clkCnt]; } else if (i < lastClkBit + 8) { //noise after a phase shift - ignore } else { //phase shift before supposed to based on clock errCnt++; } } else if (i + 1 > lastClkBit + clk[clkCnt] + tol + *fc) { lastClkBit += clk[clkCnt]; //no phase shift but clock bit } waveStart = i + 1; } } } if (errCnt == 0) return clk[clkCnt]; if (errCnt <= bestErr[clkCnt]) bestErr[clkCnt] = errCnt; if (peakcnt > peaksdet[clkCnt]) peaksdet[clkCnt] = peakcnt; } //all tested with errors //return the highest clk with the most peaks found uint8_t best = 9; for (i = 9; i >= 1; i--) { if (peaksdet[i] > peaksdet[best]) best = i; if (g_debugMode == 2) prnt("DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d", clk[i], peaksdet[i], bestErr[i], clk[best]); } return clk[best]; } // detects the bit clock for FSK given the high and low Field Clocks uint8_t detectFSKClk(const uint8_t *bits, size_t size, uint8_t fcHigh, uint8_t fcLow, int *firstClockEdge) { if (size == 0) return 0; uint8_t clk[] = {8, 16, 32, 40, 50, 64, 100, 128, 0}; uint16_t rfLens[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t rfCnts[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t rfLensFnd = 0; uint8_t lastFCcnt = 0; uint16_t fcCounter = 0; uint16_t rfCounter = 0; uint8_t firstBitFnd = 0; size_t i; uint8_t fcTol = ((fcHigh * 100 - fcLow * 100) / 2 + 50) / 100; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2); // prime i to first peak / up transition for (i = 160; i < size - 20; i++) if (bits[i] > bits[i - 1] && bits[i] >= bits[i + 1]) break; for (; i < size - 20; i++) { fcCounter++; rfCounter++; if (bits[i] <= bits[i - 1] || bits[i] < bits[i + 1]) continue; // else new peak // if we got less than the small fc + tolerance then set it to the small fc // if it is inbetween set it to the last counter if (fcCounter < fcHigh && fcCounter > fcLow) fcCounter = lastFCcnt; else if (fcCounter < fcLow + fcTol) fcCounter = fcLow; else //set it to the large fc fcCounter = fcHigh; //look for bit clock (rf/xx) if ((fcCounter < lastFCcnt || fcCounter > lastFCcnt)) { //not the same size as the last wave - start of new bit sequence if (firstBitFnd > 1) { //skip first wave change - probably not a complete bit for (int ii = 0; ii < 15; ii++) { if (rfLens[ii] >= (rfCounter - 4) && rfLens[ii] <= (rfCounter + 4)) { rfCnts[ii]++; rfCounter = 0; break; } } if (rfCounter > 0 && rfLensFnd < 15) { //prnt("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); rfCnts[rfLensFnd]++; rfLens[rfLensFnd++] = rfCounter; } } else { *firstClockEdge = i; firstBitFnd++; } rfCounter = 0; lastFCcnt = fcCounter; } fcCounter = 0; } uint8_t rfHighest = 15, rfHighest2 = 15, rfHighest3 = 15; for (i = 0; i < 15; i++) { //get highest 2 RF values (might need to get more values to compare or compare all?) if (rfCnts[i] > rfCnts[rfHighest]) { rfHighest3 = rfHighest2; rfHighest2 = rfHighest; rfHighest = i; } else if (rfCnts[i] > rfCnts[rfHighest2]) { rfHighest3 = rfHighest2; rfHighest2 = i; } else if (rfCnts[i] > rfCnts[rfHighest3]) { rfHighest3 = i; } if (g_debugMode == 2) prnt("DEBUG FSK: RF %d, cnts %d", rfLens[i], rfCnts[i]); } // set allowed clock remainder tolerance to be 1 large field clock length+1 // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off uint8_t tol1 = fcHigh + 1; if (g_debugMode == 2) prnt("DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d", rfLens[rfHighest], rfLens[rfHighest2], rfLens[rfHighest3]); // loop to find the highest clock that has a remainder less than the tolerance // compare samples counted divided by // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less) int m = 7; for (; m >= 2; m--) { if (rfLens[rfHighest] % clk[m] < tol1 || rfLens[rfHighest] % clk[m] > clk[m] - tol1) { if (rfLens[rfHighest2] % clk[m] < tol1 || rfLens[rfHighest2] % clk[m] > clk[m] - tol1) { if (rfLens[rfHighest3] % clk[m] < tol1 || rfLens[rfHighest3] % clk[m] > clk[m] - tol1) { if (g_debugMode == 2) prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance", clk[m]); break; } } } } if (m < 2) return 0; // oops we went too far return clk[m]; } // ********************************************************************************************** // --------------------Modulation Demods &/or Decoding Section----------------------------------- // ********************************************************************************************** // look for Sequence Terminator - should be pulses of clk*(1 or 2), clk*2, clk*(1.5 or 2), by idx we mean graph position index... static bool findST(int *stStopLoc, int *stStartIdx, const int lowToLowWaveLen[], const int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) { if (buffSize < *i + 4) return false; for (; *i < buffSize - 4; *i += 1) { *stStartIdx += lowToLowWaveLen[*i]; //caution part of this wave may be data and part may be ST.... to be accounted for in main function for now... if (lowToLowWaveLen[*i] >= clk * 1 - tol && lowToLowWaveLen[*i] <= (clk * 2) + tol && highToLowWaveLen[*i] < clk + tol) { //1 to 2 clocks depending on 2 bits prior if (lowToLowWaveLen[*i + 1] >= clk * 2 - tol && lowToLowWaveLen[*i + 1] <= clk * 2 + tol && highToLowWaveLen[*i + 1] > clk * 3 / 2 - tol) { //2 clocks and wave size is 1 1/2 if (lowToLowWaveLen[*i + 2] >= (clk * 3) / 2 - tol && lowToLowWaveLen[*i + 2] <= clk * 2 + tol && highToLowWaveLen[*i + 2] > clk - tol) { //1 1/2 to 2 clocks and at least one full clock wave if (lowToLowWaveLen[*i + 3] >= clk * 1 - tol && lowToLowWaveLen[*i + 3] <= clk * 2 + tol) { //1 to 2 clocks for end of ST + first bit *stStopLoc = *i + 3; return true; } } } } } return false; } // attempt to identify a Sequence Terminator in ASK modulated raw wave bool DetectST(uint8_t *buffer, size_t *size, int *foundclock, size_t *ststart, size_t *stend) { size_t bufsize = *size; //need to loop through all samples and identify our clock, look for the ST pattern int clk = 0; int tol = 0; int j = 0, high, low, skip = 0, start = 0, end = 0, minClk = 255; size_t i = 0; //probably should calloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow] int tmpbuff[bufsize / LOWEST_DEFAULT_CLOCK]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured int waveLen[bufsize / LOWEST_DEFAULT_CLOCK]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed... //size_t testsize = (bufsize < 512) ? bufsize : 512; int phaseoff = 0; high = low = 128; memset(tmpbuff, 0, sizeof(tmpbuff)); memset(waveLen, 0, sizeof(waveLen)); if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false; // set clock - might be able to get this externally and remove this work... clk = getClosestClock(minClk); // clock not found - ERROR if (!clk) { if (g_debugMode == 2) prnt("DEBUG STT: clock not found - quitting"); return false; } *foundclock = clk; tol = clk / 8; if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) { // first ST not found - ERROR if (g_debugMode == 2) prnt("DEBUG STT: first STT not found - quitting"); return false; } else { if (g_debugMode == 2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j); } if (waveLen[i + 2] > clk * 1 + tol) phaseoff = 0; else phaseoff = clk / 2; // skip over the remainder of ST skip += clk * 7 / 2; //3.5 clocks from tmpbuff[i] = end of st - also aligns for ending point // now do it again to find the end int dummy1 = 0; end = skip; i += 3; if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) { //didn't find second ST - ERROR if (g_debugMode == 2) prnt("DEBUG STT: second STT not found - quitting"); return false; } end -= phaseoff; if (g_debugMode == 2) prnt("DEBUG STT: start of data: %d end of data: %d, datalen: %d, clk: %d, bits: %d, phaseoff: %d", skip, end, end - skip, clk, (end - skip) / clk, phaseoff); //now begin to trim out ST so we can use normal demod cmds start = skip; size_t datalen = end - start; // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock if (clk - (datalen % clk) <= clk / 8) { // padd the amount off - could be problematic... but shouldn't happen often datalen += clk - (datalen % clk); } else if ((datalen % clk) <= clk / 8) { // padd the amount off - could be problematic... but shouldn't happen often datalen -= datalen % clk; } else { if (g_debugMode == 2) prnt("DEBUG STT: datalen not divisible by clk: %zu %% %d = %zu - quitting", datalen, clk, datalen % clk); return false; } // if datalen is less than one t55xx block - ERROR if (datalen / clk < 8 * 4) { if (g_debugMode == 2) prnt("DEBUG STT: datalen is less than 1 full t55xx block - quitting"); return false; } size_t dataloc = start; if (buffer[dataloc - (clk * 4) - (clk / 4)] <= low && buffer[dataloc] <= low && buffer[dataloc - (clk * 4)] >= high) { //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start for (i = 0; i <= (clk / 4); ++i) { if (buffer[dataloc - (clk * 4) - i] <= low) { dataloc -= i; break; } } } size_t newloc = 0; i = 0; if (g_debugMode == 2) prnt("DEBUG STT: Starting STT trim - start: %zu, datalen: %zu ", dataloc, datalen); bool firstrun = true; // warning - overwriting buffer given with raw wave data with ST removed... while (dataloc < bufsize - (clk / 2)) { //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part) if (buffer[dataloc] < high && buffer[dataloc] > low && buffer[dataloc + clk / 4] < high && buffer[dataloc + clk / 4] > low) { for (i = 0; i < clk / 2 - tol; ++i) { buffer[dataloc + i] = high + 5; } } //test for small spike outlier (high between two lows) in the case of very strong waves if (buffer[dataloc] > low && buffer[dataloc + clk / 4] <= low) { for (i = 0; i < clk / 4; ++i) { buffer[dataloc + i] = buffer[dataloc + clk / 4]; } } if (firstrun) { *stend = dataloc; *ststart = dataloc - (clk * 4); firstrun = false; } for (i = 0; i < datalen; ++i) { if (i + newloc < bufsize) { if (i + newloc < dataloc) buffer[i + newloc] = buffer[dataloc]; dataloc++; } } newloc += i; //skip next ST - we just assume it will be there from now on... if (g_debugMode == 2) prnt("DEBUG STT: skipping STT at %zu to %zu", dataloc, dataloc + (clk * 4)); dataloc += clk * 4; } *size = newloc; return true; } // take 11 10 01 11 00 and make 01100 ... miller decoding // check for phase errors - should never have half a 1 or 0 by itself and should never exceed 1111 or 0000 in a row // decodes miller encoded binary // NOTE askrawdemod will NOT demod miller encoded ask unless the clock is manually set to 1/2 what it is detected as! /* static int millerRawDecode(uint8_t *bits, size_t *size, int invert) { if (*size < 16) return -1; uint16_t MaxBits = MAX_DEMODULATION_BITS, errCnt = 0; size_t i, bitCnt = 0; uint8_t alignCnt = 0, curBit = bits[0], alignedIdx = 0, halfClkErr = 0; //find alignment, needs 4 1s or 0s to properly align for (i = 1; i < *size - 1; i++) { alignCnt = (bits[i] == curBit) ? alignCnt + 1 : 0; curBit = bits[i]; if (alignCnt == 4) break; } // for now error if alignment not found. later add option to run it with multiple offsets... if (alignCnt != 4) { if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bits is not miller or your data does not have a 101 in it"); return -1; } alignedIdx = (i - 1) % 2; for (i = alignedIdx; i < *size - 3; i += 2) { halfClkErr = (uint8_t)((halfClkErr << 1 | bits[i]) & 0xFF); if ((halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) { errCnt++; bits[bitCnt++] = 7; continue; } bits[bitCnt++] = bits[i] ^ bits[i + 1] ^ invert; if (bitCnt > MaxBits) break; } *size = bitCnt; return errCnt; } */ // take 01 or 10 = 1 and 11 or 00 = 0 // check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010 // decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding int BiphaseRawDecode(uint8_t *bits, size_t *size, int *offset, int invert) { //sanity check if (*size < 51) return -1; if (*offset < 0) *offset = 0; uint16_t bitnum = 0; uint16_t errCnt = 0; size_t i = *offset; uint16_t maxbits = MAX_DEMODULATION_BITS; //check for phase change faults - skip one sample if faulty bool offsetA = true, offsetB = true; for (; i < *offset + 48; i += 2) { if (bits[i + 1] == bits[i + 2]) offsetA = false; if (bits[i + 2] == bits[i + 3]) offsetB = false; } if (!offsetA && offsetB) ++*offset; // main loop for (i = *offset; i < *size - 1; i += 2) { //check for phase error if (bits[i + 1] == bits[i + 2]) { bits[bitnum++] = 7; errCnt++; } if ((bits[i] == 1 && bits[i + 1] == 0) || (bits[i] == 0 && bits[i + 1] == 1)) { bits[bitnum++] = 1 ^ invert; } else if ((bits[i] == 0 && bits[i + 1] == 0) || (bits[i] == 1 && bits[i + 1] == 1)) { bits[bitnum++] = invert; } else { bits[bitnum++] = 7; errCnt++; } if (bitnum > maxbits) break; } *size = bitnum; return errCnt; } // take 10 and 01 and manchester decode // run through 2 times and take least errCnt // "," indicates 00 or 11 wrong bit uint16_t manrawdecode(uint8_t *bits, size_t *size, uint8_t invert, uint8_t *alignPos) { // sanity check if (*size < 16) { return 0xFFFF; } int errCnt = 0, bestErr = 1000; uint16_t bitnum = 0, maxBits = MAX_DEMODULATION_BITS, bestRun = 0; size_t i; // find correct start position [alignment] for (uint8_t k = 0; k < 2; k++) { for (i = k; i < *size - 1; i += 2) { if (bits[i] == bits[i + 1]) { errCnt++; } if (errCnt > 50) { break; } } if (bestErr > errCnt) { bestErr = errCnt; bestRun = k; if (g_debugMode == 2) prnt("DEBUG manrawdecode: bestErr %d | bestRun %u", bestErr, bestRun); } errCnt = 0; } *alignPos = bestRun; // decode for (i = bestRun; i < *size; i += 2) { if (bits[i] == 1 && (bits[i + 1] == 0)) { bits[bitnum++] = invert; } else if ((bits[i] == 0) && bits[i + 1] == 1) { bits[bitnum++] = invert ^ 1; } else { bits[bitnum++] = 7; } if (bitnum > maxBits) { break; } } *size = bitnum; return bestErr; } // demodulates strong heavily clipped samples // RETURN: num of errors. if 0, is ok. static uint16_t cleanAskRawDemod(uint8_t *bits, size_t *size, int clk, int invert, int high, int low, int *startIdx) { *startIdx = 0; size_t bitCnt = 0, smplCnt = 1, errCnt = 0, pos = 0; uint8_t cl_4 = clk / 4; uint8_t cl_2 = clk / 2; bool waveHigh = true; getNextHigh(bits, *size, high, &pos); // getNextLow(bits, *size, low, &pos); // do not skip first transition if ((pos > cl_2 - cl_4 - 1) && (pos <= clk + cl_4 + 1)) { bits[bitCnt++] = invert ^ 1; } // sample counts, like clock = 32.. it tries to find 32/4 = 8, 32/2 = 16 for (size_t i = pos; i < *size; i++) { if (bits[i] >= high && waveHigh) { smplCnt++; } else if (bits[i] <= low && !waveHigh) { smplCnt++; } else { //transition if ((bits[i] >= high && !waveHigh) || (bits[i] <= low && waveHigh)) { // 8 :: 8-2-1 = 5 8+2+1 = 11 // 16 :: 16-4-1 = 11 16+4+1 = 21 // 32 :: 32-8-1 = 23 32+8+1 = 41 // 64 :: 64-16-1 = 47 64+16+1 = 81 if (smplCnt > clk - cl_4 - 1) { //full clock if (smplCnt > clk + cl_4 + 1) { //too many samples errCnt++; if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod ASK Modulation Error FULL at: %zu [%zu > %u]", i, smplCnt, clk + cl_4 + 1); bits[bitCnt++] = 7; } else if (waveHigh) { bits[bitCnt++] = invert; bits[bitCnt++] = invert; } else { bits[bitCnt++] = invert ^ 1; bits[bitCnt++] = invert ^ 1; } if (*startIdx == 0) { *startIdx = i - clk; if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod minus clock [%d]", *startIdx); } waveHigh = !waveHigh; smplCnt = 0; // 16-8-1 = 7 } else if (smplCnt > cl_2 - cl_4 - 1) { //half clock if (smplCnt > cl_2 + cl_4 + 1) { //too many samples errCnt++; if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod ASK Modulation Error HALF at: %zu [%zu]", i, smplCnt); bits[bitCnt++] = 7; } if (waveHigh) { bits[bitCnt++] = invert; } else { bits[bitCnt++] = invert ^ 1; } if (*startIdx == 0) { *startIdx = i - cl_2; if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod minus half clock [%d]", *startIdx); } waveHigh = !waveHigh; smplCnt = 0; } else { smplCnt++; //transition bit oops } } else { //haven't hit new high or new low yet smplCnt++; } } } *size = bitCnt; if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod Startidx %d", *startIdx); return errCnt; } // attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester int askdemod_ext(uint8_t *bits, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType, int *startIdx) { if (*size == 0) return -1; if (signalprop.isnoise) { if (g_debugMode == 2) prnt("DEBUG (askdemod_ext) just noise detected - aborting"); return -2; } int start = DetectASKClock(bits, *size, clk, maxErr); if (*clk == 0 || start < 0) return -3; if (*invert != 1) *invert = 0; // amplify signal data. // ICEMAN todo, if (amp == 1) askAmp(bits, *size); if (g_debugMode == 2) prnt("DEBUG (askdemod_ext) clk %d, beststart %d, amp %d", *clk, start, amp); // Detect high and lows //25% clip in case highs and lows aren't clipped [marshmellow] int high, low; getHiLo(&high, &low, 75, 75); size_t errCnt = 0; // if clean clipped waves detected run alternate demod if (DetectCleanAskWave(bits, *size, high, low)) { //start pos from detect ask clock is 1/2 clock offset // NOTE: can be negative (demod assumes rest of wave was there) *startIdx = start - (*clk / 2); if (g_debugMode == 2) prnt("DEBUG: (askdemod_ext) Clean wave detected --- startindex %d", *startIdx); errCnt = cleanAskRawDemod(bits, size, *clk, *invert, high, low, startIdx); if (askType) { //ask/manchester uint8_t alignPos = 0; errCnt = manrawdecode(bits, size, 0, &alignPos); *startIdx += ((*clk / 2) * alignPos); if (g_debugMode == 2) prnt("DEBUG: (askdemod_ext) CLEAN: startIdx %i, alignPos %u , bestError %zu", *startIdx, alignPos, errCnt); } return errCnt; } *startIdx = start - (*clk / 2); if (g_debugMode == 2) prnt("DEBUG: (askdemod_ext) Weak wave detected: startIdx %i", *startIdx); int lastBit; // set first clock check - can go negative size_t i, bitnum = 0; // output counter uint8_t midBit = 0; uint8_t tol = 0; // clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave if (*clk <= 32) tol = 1; // clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely size_t MaxBits = 3072; // max bits to collect lastBit = start - *clk; for (i = start; i < *size; ++i) { if (i - lastBit >= *clk - tol) { if (bits[i] >= high) { bits[bitnum++] = *invert; } else if (bits[i] <= low) { bits[bitnum++] = *invert ^ 1; } else if (i - lastBit >= *clk + tol) { if (bitnum > 0) { // if (g_debugMode == 2) prnt("DEBUG: (askdemod_ext) Modulation Error at: %u", i); bits[bitnum++] = 7; errCnt++; } } else { //in tolerance - looking for peak continue; } midBit = 0; lastBit += *clk; } else if (i - lastBit >= (*clk / 2 - tol) && !midBit && !askType) { if (bits[i] >= high) { bits[bitnum++] = *invert; } else if (bits[i] <= low) { bits[bitnum++] = *invert ^ 1; } else if (i - lastBit >= *clk / 2 + tol) { if (bitnum > 0) { bits[bitnum] = bits[bitnum - 1]; bitnum++; } else { bits[bitnum] = 0; bitnum++; } } else { //in tolerance - looking for peak continue; } midBit = 1; } if (bitnum >= MaxBits) break; } *size = bitnum; return errCnt; } int askdemod(uint8_t *bits, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType) { int start = 0; return askdemod_ext(bits, size, clk, invert, maxErr, amp, askType, &start); } // demodulate NRZ wave - requires a read with strong signal // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, const int *invert, int *startIdx) { if (signalprop.isnoise) { if (g_debugMode == 2) prnt("DEBUG nrzRawDemod: just noise detected - quitting"); return -1; } size_t clkStartIdx = 0; *clk = DetectNRZClock(dest, *size, *clk, &clkStartIdx); if (*clk == 0) return -2; size_t i; int high, low; getHiLo(&high, &low, 75, 75); uint8_t bit = 0; //convert wave samples to 1's and 0's for (i = 20; i < *size - 20; i++) { if (dest[i] >= high) bit = 1; if (dest[i] <= low) bit = 0; dest[i] = bit; } //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit) size_t lastBit = 0; size_t numBits = 0; for (i = 21; i < *size - 20; i++) { //if transition detected or large number of same bits - store the passed bits if (dest[i] != dest[i - 1] || (i - lastBit) == (10 * *clk)) { memset(dest + numBits, dest[i - 1] ^ *invert, (i - lastBit + (*clk / 4)) / *clk); numBits += (i - lastBit + (*clk / 4)) / *clk; if (lastBit == 0) { *startIdx = i - (numBits * *clk); if (g_debugMode == 2) prnt("DEBUG NRZ: startIdx %i", *startIdx); } lastBit = i - 1; } } *size = numBits; return 0; } // translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq]) static size_t fsk_wave_demod(uint8_t *dest, size_t size, uint8_t fchigh, uint8_t fclow, int *startIdx) { if (size < 1024) return 0; // not enough samples if (fchigh == 0) fchigh = 10; if (fclow == 0) fclow = 8; //set the threshold close to 0 (graph) or 128 std to avoid static size_t preLastSample, LastSample = 0; size_t currSample = 0, last_transition = 0; size_t idx, numBits = 0; //find start of modulating data in trace idx = findModStart(dest, size, fchigh); // Need to threshold first sample dest[idx] = (dest[idx] < signalprop.mean) ? 0 : 1; last_transition = idx; idx++; // Definition: cycles between consecutive lo-hi transitions // Lets define some expected lengths. FSK1 is easier since it has bigger differences between. // FSK1 8/5 // 50/8 = 6 | 40/8 = 5 | 64/8 = 8 // 50/5 = 10 | 40/5 = 8 | 64/5 = 12 // FSK2 10/8 // 50/10 = 5 | 40/10 = 4 | 64/10 = 6 // 50/8 = 6 | 40/8 = 5 | 64/8 = 8 // count cycles between consecutive lo-hi transitions, // in practice due to noise etc we may end up with anywhere // To allow fuzz would mean +-1 on expected cycle width. // FSK1 8/5 // 50/8 = 6 (5-7) | 40/8 = 5 (4-6) | 64/8 = 8 (7-9) // 50/5 = 10 (9-11) | 40/5 = 8 (7-9) | 64/5 = 12 (11-13) // FSK2 10/8 // 50/10 = 5 (4-6) | 40/10 = 4 (3-5) | 64/10 = 6 (5-7) // 50/8 = 6 (5-7) | 40/8 = 5 (4-6) | 64/8 = 8 (7-9) // // It easy to see to the overgaping, but luckily we the group value also, like 1111000001111 // to separate between which bit to demodulate to. // process: // count width from 0-1 transition to 1-0. // determine the width is within FUZZ_min and FUZZ_max tolerances // width should be divided with exp_one. i:e 6+7+6+2=21, 21/5 = 4, // the 1-0 to 0-1 width should be divided with exp_zero. Ie: 3+5+6+7 = 21/6 = 3 for (; idx < size - 20; idx++) { // threshold current value dest[idx] = (dest[idx] < signalprop.mean) ? 0 : 1; // Check for 0->1 transition if (dest[idx - 1] < dest[idx]) { preLastSample = LastSample; LastSample = currSample; currSample = idx - last_transition; if (currSample < (fclow - 2)) { //0-5 = garbage noise (or 0-3) //do nothing with extra garbage } else if (currSample < (fchigh - 1)) { //6-8 = 8 sample waves (or 3-6 = 5) //correct previous 9 wave surrounded by 8 waves (or 6 surrounded by 5) if (numBits > 1 && LastSample > (fchigh - 2) && (preLastSample < (fchigh - 1))) { dest[numBits - 1] = 1; } dest[numBits++] = 1; if (numBits > 0 && *startIdx == 0) *startIdx = idx - fclow; } else if (currSample > (fchigh + 1) && numBits < 3) { //12 + and first two bit = unusable garbage //do nothing with beginning garbage and reset.. should be rare.. numBits = 0; } else if (currSample == (fclow + 1) && LastSample == (fclow - 1)) { // had a 7 then a 9 should be two 8's (or 4 then a 6 should be two 5's) dest[numBits++] = 1; if (numBits > 0 && *startIdx == 0) { *startIdx = idx - fclow; } } else { //9+ = 10 sample waves (or 6+ = 7) dest[numBits++] = 0; if (numBits > 0 && *startIdx == 0) { *startIdx = idx - fchigh; } } last_transition = idx; } } return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0 } // translate 11111100000 to 10 //rfLen = clock, fchigh = larger field clock, fclow = smaller field clock static size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t clk, uint8_t invert, uint8_t fchigh, uint8_t fclow, int *startIdx) { uint8_t lastval = dest[0]; size_t i = 0; size_t numBits = 0; uint32_t n = 1; uint8_t hclk = clk / 2; for (i = 1; i < size; i++) { n++; if (dest[i] == lastval) continue; //skip until we hit a transition //find out how many bits (n) we collected (use 1/2 clk tolerance) if (dest[i - 1] == 1) //if lastval was 1, we have a 1->0 crossing n = (n * fclow + hclk) / clk; else // 0->1 crossing n = (n * fchigh + hclk) / clk; if (n == 0) n = 1; //first transition - save startidx if (numBits == 0) { if (lastval == 1) { //high to low *startIdx += (fclow * i) - (n * clk); if (g_debugMode == 2) prnt("DEBUG (aggregate_bits) FSK startIdx %i, fclow*idx %zu, n*clk %u", *startIdx, fclow * i, n * clk); } else { *startIdx += (fchigh * i) - (n * clk); if (g_debugMode == 2) prnt("DEBUG (aggregate_bits) FSK startIdx %i, fchigh*idx %zu, n*clk %u", *startIdx, fchigh * i, n * clk); } } //add to our destination the bits we collected memset(dest + numBits, dest[i - 1] ^ invert, n); numBits += n; n = 0; lastval = dest[i]; }//end for // if valid extra bits at the end were all the same frequency - add them in if (n > clk / fchigh) { if (dest[i - 2] == 1) { n = (n * fclow + clk / 2) / clk; } else { n = (n * fchigh + clk / 2) / clk; } memset(dest + numBits, dest[i - 1] ^ invert, n); numBits += n; if (g_debugMode == 2) prnt("DEBUG (aggregate_bits) extra bits in the end"); } return numBits; } // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod) size_t fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow, int *start_idx) { if (signalprop.isnoise) return 0; // FSK demodulator size = fsk_wave_demod(dest, size, fchigh, fclow, start_idx); if (g_debugMode == 2) prnt("DEBUG (fskdemod) got %zu bits", size); size = aggregate_bits(dest, size, rfLen, invert, fchigh, fclow, start_idx); if (g_debugMode == 2) prnt("DEBUG (fskdemod) got %zu bits", size); return size; } // convert psk1 demod to psk2 demod // only transition waves are 1s // TODO: Iceman - hard coded value 7, should be #define void psk1TOpsk2(uint8_t *bits, size_t size) { uint8_t lastbit = bits[0]; for (size_t i = 1; i < size; i++) { //ignore errors if (bits[i] == 7) continue; if (lastbit != bits[i]) { lastbit = bits[i]; bits[i] = 1; } else { bits[i] = 0; } } } // convert psk2 demod to psk1 demod // from only transition waves are 1s to phase shifts change bit void psk2TOpsk1(uint8_t *bits, size_t size) { uint8_t phase = 0; for (size_t i = 0; i < size; i++) { if (bits[i] == 1) { phase ^= 1; } bits[i] = phase; } } // demodulate PSK1 wave // uses wave lengths (# Samples) // TODO: Iceman - hard coded value 7, should be #define int pskRawDemod_ext(uint8_t *dest, size_t *size, int *clock, const int *invert, int *startIdx) { // sanity check if (*size < 170) return -1; uint8_t curPhase = *invert; uint8_t fc = 0; size_t i = 0, numBits = 0, waveStart = 1, waveEnd, firstFullWave = 0, lastClkBit = 0; uint16_t fullWaveLen = 0, waveLenCnt; //uint16_t avgWaveVal = 0; uint16_t errCnt = 0, errCnt2 = 0; *clock = DetectPSKClock(dest, *size, *clock, &firstFullWave, &curPhase, &fc); if (*clock <= 0) return -1; //if clock detect found firstfullwave... uint16_t tol = fc / 2; if (firstFullWave == 0) { //find start of modulating data in trace i = findModStart(dest, *size, fc); //find first phase shift firstFullWave = pskFindFirstPhaseShift(dest, *size, &curPhase, i, fc, &fullWaveLen); if (firstFullWave == 0) { // no phase shift detected - could be all 1's or 0's - doesn't matter where we start // so skip a little to ensure we are past any Start Signal firstFullWave = 160; memset(dest, curPhase, firstFullWave / *clock); } else { memset(dest, curPhase ^ 1, firstFullWave / *clock); } } else { memset(dest, curPhase ^ 1, firstFullWave / *clock); } //advance bits numBits += (firstFullWave / *clock); *startIdx = firstFullWave - (*clock * numBits) + 2; //set start of wave as clock align lastClkBit = firstFullWave; if (g_debugMode == 2) { prnt("DEBUG PSK: firstFullWave: %zu, waveLen: %u, startIdx %i", firstFullWave, fullWaveLen, *startIdx); prnt("DEBUG PSK: clk: %d, lastClkBit: %zu, fc: %u", *clock, lastClkBit, fc); } waveStart = 0; dest[numBits++] = curPhase; //set first read bit for (i = firstFullWave + fullWaveLen - 1; i < *size - 3; i++) { //top edge of wave = start of new wave if (dest[i] + fc < dest[i + 1] && dest[i + 1] >= dest[i + 2]) { if (waveStart == 0) { waveStart = i + 1; //avgWaveVal = dest[i + 1]; } else { //waveEnd waveEnd = i + 1; waveLenCnt = waveEnd - waveStart; if (waveLenCnt > fc) { //this wave is a phase shift /* prnt("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d" , waveStart , waveLenCnt , lastClkBit + *clock - tol , i + 1 , fc); */ if (i + 1 >= lastClkBit + *clock - tol) { //should be a clock bit curPhase ^= 1; dest[numBits++] = curPhase; lastClkBit += *clock; } else if (i < lastClkBit + 10 + fc) { //noise after a phase shift - ignore } else { //phase shift before supposed to based on clock errCnt++; dest[numBits++] = 7; } } else if (i + 1 > lastClkBit + *clock + tol + fc) { lastClkBit += *clock; //no phase shift but clock bit dest[numBits++] = curPhase; } else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often) errCnt2++; if (errCnt2 > 101) return errCnt2; //avgWaveVal += dest[i + 1]; continue; } //avgWaveVal = 0; waveStart = i + 1; } } //avgWaveVal += dest[i + 1]; } *size = numBits; return errCnt; } int pskRawDemod(uint8_t *dest, size_t *size, int *clock, int *invert) { int start_idx = 0; return pskRawDemod_ext(dest, size, clock, invert, &start_idx); } // ********************************************************************************************** // -----------------Tag format detection section------------------------------------------------- // ********************************************************************************************** // FSK Demod then try to locate an AWID ID int detectAWID(uint8_t *dest, size_t *size, int *waveStartIdx) { //make sure buffer has enough data (96bits * 50clock samples) if (*size < 96 * 50) return -1; if (signalprop.isnoise) return -2; // FSK2a demodulator clock 50, invert 1, fcHigh 10, fcLow 8 *size = fskdemod(dest, *size, 50, 1, 10, 8, waveStartIdx); //awid fsk2a //did we get a good demod? if (*size < 96) return -3; size_t start_idx = 0; uint8_t preamble[] = {0, 0, 0, 0, 0, 0, 0, 1}; if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) return -4; //preamble not found // wrong size? (between to preambles) if (*size != 96) return -5; return (int)start_idx; } // takes 1s and 0s and searches for EM410x format - output EM ID int Em410xDecode(uint8_t *bits, size_t *size, size_t *start_idx, uint32_t *hi, uint64_t *lo) { // sanity check if (bits[1] > 1) return -1; if (*size < 64) return -2; *start_idx = 0; bool adjust = false; if (*size < 128) { adjust = true; } // preamble 0111111111 // include 0 in front to help get start pos uint8_t preamble[] = {0, 1, 1, 1, 1, 1, 1, 1, 1, 1}; if (!preambleSearch(bits, preamble, sizeof(preamble), size, start_idx)) return -4; bool validShort = false; bool validShortExtended = false; bool validLong = false; // detect sledge of 0x05's int fix = -1; uint8_t fives[] = {0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1}; for (size_t x = 0; x < *size - sizeof(fives); x += 1) { if (memcmp(bits + x, fives, sizeof(fives)) == 0) { // save first occasion if (fix == -1) { fix = x; break; } } } size_t sidx = *start_idx + sizeof(preamble); if (adjust) { sidx--; } // not 128 or 55.. if (fix != -1) { *size = 80; } #ifndef ON_DEVICE // prnt("fix... %d size... %zu", fix, *size); #endif // HACK *size = removeEm410xParity(bits, sidx, size, &validShort, &validShortExtended, &validLong); if (validShort) { // std em410x format *hi = 0; *lo = ((uint64_t)(bytebits_to_byte(bits, 8)) << 32) | (bytebits_to_byte(bits + 8, 32)); // 1 = Short return 1; } if (validShortExtended || validLong) { // store in long em format *hi = (bytebits_to_byte(bits, 24)); *lo = ((uint64_t)(bytebits_to_byte(bits + 24, 32)) << 32) | (bytebits_to_byte(bits + 24 + 32, 32)); // 2 = Long // 4 = ShortExtended return ((int)validShortExtended << 2) + ((int)validLong << 1); } return -6; } // loop to get raw HID waveform then FSK demodulate the TAG ID from it int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo, int *waveStartIdx) { //make sure buffer has data if (*size < 96 * 50) return -1; if (signalprop.isnoise) return -2; // FSK demodulator fsk2a so invert and fc/10/8 *size = fskdemod(dest, *size, 50, 1, 10, 8, waveStartIdx); //hid fsk2a //did we get a good demod? if (*size < 96 * 2) return -3; // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1 size_t start_idx = 0; uint8_t preamble[] = {0, 0, 0, 1, 1, 1, 0, 1}; if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) return -4; //preamble not found // wrong size? (between to preambles) //if (*size != 96) return -5; size_t num_start = start_idx + sizeof(preamble); // final loop, go over previously decoded FSK data and manchester decode into usable tag ID for (size_t idx = num_start; (idx - num_start) < *size - sizeof(preamble); idx += 2) { if (dest[idx] == dest[idx + 1]) { return -5; //not manchester data } *hi2 = (*hi2 << 1) | (*hi >> 31); *hi = (*hi << 1) | (*lo >> 31); //Then, shift in a 0 or one into low *lo <<= 1; if (dest[idx] && !dest[idx + 1]) // 1 0 *lo |= 1; else // 0 1 *lo |= 0; } return (int)start_idx; } int detectIOProx(uint8_t *dest, size_t *size, int *waveStartIdx) { //make sure buffer has data if (*size < 66 * 64) return -1; if (signalprop.isnoise) return -2; // FSK demodulator RF/64, fsk2a so invert, and fc/10/8 *size = fskdemod(dest, *size, 64, 1, 10, 8, waveStartIdx); //io fsk2a //did we get enough demod data? if (*size < 64) return -3; //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 ???????? 11 // //XSF(version)facility:codeone+codetwo size_t start_idx = 0; uint8_t preamble[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 1}; if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) return -4; //preamble not found // wrong size? (between to preambles) if (*size != 64) return -5; if (!dest[start_idx + 8] && dest[start_idx + 17] == 1 && dest[start_idx + 26] == 1 && dest[start_idx + 35] == 1 && dest[start_idx + 44] == 1 && dest[start_idx + 53] == 1) { //confirmed proper separator bits found //return start position return (int) start_idx; } return -6; }