From 1f2648087f2e03791b168f9aa175555b1e9dc73f Mon Sep 17 00:00:00 2001 From: iceman1001 Date: Thu, 30 Jul 2026 16:40:14 +0200 Subject: [PATCH] new low frequency DSP.... --- client/src/pm3_dsp.c | 1812 ++++++++++++++++++++++++++++++++++++++++++ client/src/pm3_dsp.h | 200 +++++ client/src/pm3_fit.c | 1066 +++++++++++++++++++++++++ client/src/pm3_fit.h | 119 +++ 4 files changed, 3197 insertions(+) create mode 100644 client/src/pm3_dsp.c create mode 100644 client/src/pm3_dsp.h create mode 100644 client/src/pm3_fit.c create mode 100644 client/src/pm3_fit.h diff --git a/client/src/pm3_dsp.c b/client/src/pm3_dsp.c new file mode 100644 index 000000000..36b3a3aff --- /dev/null +++ b/client/src/pm3_dsp.c @@ -0,0 +1,1812 @@ +//----------------------------------------------------------------------------- +// 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. +//----------------------------------------------------------------------------- +// Host side DSP primitives - FFT, windows, power spectrum, STFT +//----------------------------------------------------------------------------- + +#include "pm3_dsp.h" + +#include +#include +#include + +#include "pm3_cmd.h" +#include "commonutil.h" // ARRAYLEN + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +// refuse to allocate a spectrogram larger than this, in doubles +#define PM3_DSP_MAX_STFT_CELLS (8 * 1024 * 1024) + +// peaks closer together than this many bins are treated as the same peak +#define PM3_DSP_PEAK_SEP 3 +// half width of the neighbourhood used to estimate the local noise floor +#define PM3_DSP_FLOOR_SPAN 48 +// a peak has to be at least this prominent to take part in comb finding +#define COMB_MIN_PROMINENCE 8.0 +// autocorrelation lag window when hunting for the message repeat period +#define PM3_DSP_ACORR_MIN_LAG 256 +#define PM3_DSP_ACORR_MAX_LAG 32768 +#define PM3_DSP_ACORR_MIN_STRENGTH 0.25 +// Schmitt trigger level for run length extraction, in standard deviations +#define PM3_DSP_RUN_HYST 0.35 +// longest run over chip, above which the code is carrying raw levels +#define PM3_DSP_RUN_TRANSITION 2.6 +// a run this many chips long or less counts as short +#define PM3_DSP_RUN_SHORT 2.5 +// Transition codes put essentially every run under that limit, raw levels +// leave a quarter of them above it. Measured over the T5577 corpus the two +// populations sit at 0.95..0.99 and 0.61..0.81, so the boundary goes in the +// middle of that gap rather than at the edge of either. +#define PM3_DSP_RUN_SHORT_FRAC 0.88 +// harmonics past this are not worth chasing +#define COMB_MAX_HARMONIC 12 +// symbol clocks outside this range are not something any LF tag uses +#define COMB_MIN_CLK 4.0 +#define COMB_MAX_CLK 600.0 + +size_t pm3_next_pow2(size_t v) { + size_t p = 1; + while (p < v && p < PM3_DSP_MAX_FFT) { + p <<= 1; + } + return p; +} + +static bool is_pow2(size_t v) { + return (v != 0) && ((v & (v - 1)) == 0); +} + +pm3_fft_plan_t *pm3_fft_plan_create(size_t n) { + + if (is_pow2(n) == false || n < 2 || n > PM3_DSP_MAX_FFT) { + return NULL; + } + + pm3_fft_plan_t *plan = calloc(1, sizeof(pm3_fft_plan_t)); + if (plan == NULL) { + return NULL; + } + + plan->n = n; + plan->twiddle = calloc(n / 2, sizeof(pm3_cplx_t)); + plan->rev = calloc(n, sizeof(uint32_t)); + if (plan->twiddle == NULL || plan->rev == NULL) { + pm3_fft_plan_destroy(plan); + return NULL; + } + + for (size_t k = 0; k < n / 2; k++) { + double ang = -2.0 * M_PI * (double)k / (double)n; + plan->twiddle[k].re = cos(ang); + plan->twiddle[k].im = sin(ang); + } + + // bit reversal permutation, built incrementally so we never need to know the width + size_t shift = 0; + while ((((size_t)1) << shift) < n) { + shift++; + } + + for (size_t i = 0; i < n; i++) { + + size_t r = 0; + for (size_t b = 0; b < shift; b++) { + + if (i & (((size_t)1) << b)) { + r |= ((size_t)1) << (shift - 1 - b); + } + } + plan->rev[i] = (uint32_t)r; + } + + return plan; +} + +void pm3_fft_plan_destroy(pm3_fft_plan_t *plan) { + if (plan == NULL) { + return; + } + free(plan->twiddle); + free(plan->rev); + free(plan); +} + +void pm3_fft(const pm3_fft_plan_t *plan, pm3_cplx_t *data, bool inverse) { + + if (plan == NULL || data == NULL) { + return; + } + + const size_t n = plan->n; + + // the inverse transform is the forward transform of the conjugate, + // conjugated again and scaled. Keeps a single twiddle table. + if (inverse) { + for (size_t i = 0; i < n; i++) { + data[i].im = -data[i].im; + } + } + + for (size_t i = 0; i < n; i++) { + size_t j = plan->rev[i]; + if (j > i) { + pm3_cplx_t tmp = data[i]; + data[i] = data[j]; + data[j] = tmp; + } + } + + for (size_t span = 2; span <= n; span <<= 1) { + + const size_t half = span / 2; + const size_t step = n / span; + + for (size_t base = 0; base < n; base += span) { + for (size_t k = 0; k < half; k++) { + + const pm3_cplx_t w = plan->twiddle[k * step]; + pm3_cplx_t *lo = &data[base + k]; + pm3_cplx_t *hi = &data[base + k + half]; + + const double tr = hi->re * w.re - hi->im * w.im; + const double ti = hi->re * w.im + hi->im * w.re; + + hi->re = lo->re - tr; + hi->im = lo->im - ti; + lo->re += tr; + lo->im += ti; + } + } + } + + if (inverse) { + const double scale = 1.0 / (double)n; + for (size_t i = 0; i < n; i++) { + data[i].re *= scale; + data[i].im *= -scale; + } + } +} + +bool pm3_window_from_str(const char *str, pm3_window_t *out) { + + if (str == NULL || out == NULL) { + return false; + } + + if (strcmp(str, "hann") == 0) { + *out = PM3_WIN_HANN; + } else if (strcmp(str, "hamming") == 0) { + *out = PM3_WIN_HAMMING; + } else if (strcmp(str, "blackman") == 0) { + *out = PM3_WIN_BLACKMAN; + } else if (strcmp(str, "rect") == 0) { + *out = PM3_WIN_RECT; + } else { + return false; + } + return true; +} + +const char *pm3_window_name(pm3_window_t win) { + switch (win) { + case PM3_WIN_HANN: + return "hann"; + case PM3_WIN_HAMMING: + return "hamming"; + case PM3_WIN_BLACKMAN: + return "blackman"; + case PM3_WIN_RECT: + return "rect"; + } + return "unknown"; +} + +void pm3_apply_window(double *data, size_t len, pm3_window_t win) { + + if (data == NULL || len == 0 || win == PM3_WIN_RECT) { + return; + } + + if (len == 1) { + data[0] = 0; + return; + } + + const double denom = (double)(len - 1); + + for (size_t i = 0; i < len; i++) { + + const double phase = 2.0 * M_PI * (double)i / denom; + double w = 1.0; + + switch (win) { + case PM3_WIN_HANN: { + w = 0.5 * (1.0 - cos(phase)); + break; + } + case PM3_WIN_HAMMING: { + w = 0.54 - 0.46 * cos(phase); + break; + } + case PM3_WIN_BLACKMAN:{ + w = 0.42 - 0.5 * cos(phase) + 0.08 * cos(2.0 * phase); + break; + } + case PM3_WIN_RECT: { + break; + } + } + data[i] *= w; + } +} + +void pm3_remove_mean(double *data, size_t len) { + + if (data == NULL || len == 0) { + return; + } + + double sum = 0.0; + for (size_t i = 0; i < len; i++) { + sum += data[i]; + } + + const double mean = sum / (double)len; + for (size_t i = 0; i < len; i++) { + data[i] -= mean; + } +} + +double *pm3_extract(const int32_t *src, size_t len, size_t start, size_t count) { + + if (src == NULL || count == 0 || start >= len || (len - start) < count) { + return NULL; + } + + double *out = calloc(count, sizeof(double)); + if (out == NULL) { + return NULL; + } + + for (size_t i = 0; i < count; i++) { + out[i] = (double)src[start + i]; + } + + // LF captures carry a large DC offset. Left in place it dominates bin 0 + // and its leakage skirt swamps everything we care about. + pm3_remove_mean(out, count); + + double energy = 0.0; + for (size_t i = 0; i < count; i++) { + energy += out[i] * out[i]; + } + + // a constant input leaves nothing behind, hand back the zeros and let the + // caller decide what to say about it + if (energy > 0.0) { + const double scale = 1.0 / sqrt(energy / (double)count); + for (size_t i = 0; i < count; i++) { + out[i] *= scale; + } + } + + return out; +} + +void pm3_delay_multiply(const double *in, size_t len, size_t lag, double *out) { + + if (in == NULL || out == NULL || len == 0) { + return; + } + + for (size_t i = 0; i < len; i++) { + out[i] = (i < lag) ? 0.0 : in[i] * in[i - lag]; + } + + pm3_remove_mean(out, len); +} + +int pm3_spectrum(const double *sig, size_t len, size_t n, pm3_window_t win, pm3_spectrum_t *out) { + + if (sig == NULL || out == NULL || len == 0 || is_pow2(n) == false || n < 2) { + return PM3_EINVARG; + } + + memset(out, 0, sizeof(pm3_spectrum_t)); + + pm3_fft_plan_t *plan = pm3_fft_plan_create(n); + if (plan == NULL) { + return PM3_EMALLOC; + } + + pm3_cplx_t *buf = calloc(n, sizeof(pm3_cplx_t)); + double *win_buf = calloc(n, sizeof(double)); + if (buf == NULL || win_buf == NULL) { + free(buf); + free(win_buf); + pm3_fft_plan_destroy(plan); + return PM3_EMALLOC; + } + + const size_t take = (len < n) ? len : n; + memcpy(win_buf, sig, take * sizeof(double)); + pm3_apply_window(win_buf, n, win); + + for (size_t i = 0; i < n; i++) { + buf[i].re = win_buf[i]; + buf[i].im = 0.0; + } + free(win_buf); + + pm3_fft(plan, buf, false); + pm3_fft_plan_destroy(plan); + + out->n = n; + out->nbins = (n / 2) + 1; + out->mag = calloc(out->nbins, sizeof(double)); + if (out->mag == NULL) { + free(buf); + memset(out, 0, sizeof(pm3_spectrum_t)); + return PM3_EMALLOC; + } + + for (size_t i = 0; i < out->nbins; i++) { + out->mag[i] = sqrt(buf[i].re * buf[i].re + buf[i].im * buf[i].im); + } + free(buf); + + out->peak = 0.0; + out->peak_bin = 0; + for (size_t i = PM3_DSP_DC_GUARD; i < out->nbins; i++) { + if (out->mag[i] > out->peak) { + out->peak = out->mag[i]; + out->peak_bin = i; + } + } + + return PM3_SUCCESS; +} + +void pm3_spectrum_free(pm3_spectrum_t *spec) { + if (spec == NULL) { + return; + } + free(spec->mag); + memset(spec, 0, sizeof(pm3_spectrum_t)); +} + +// noise floor around `bin`, ignoring the peak itself and its immediate skirt +static double local_floor(const pm3_spectrum_t *spec, size_t bin) { + + double sum = 0.0; + size_t cnt = 0; + + const size_t lo = (bin > PM3_DSP_FLOOR_SPAN) ? bin - PM3_DSP_FLOOR_SPAN : PM3_DSP_DC_GUARD; + size_t hi = bin + PM3_DSP_FLOOR_SPAN; + if (hi >= spec->nbins) { + hi = spec->nbins - 1; + } + + for (size_t i = lo; i <= hi; i++) { + if (i + PM3_DSP_PEAK_SEP >= bin && i <= bin + PM3_DSP_PEAK_SEP) { + continue; + } + sum += spec->mag[i]; + cnt++; + } + + if (cnt == 0) { + return 0.0; + } + return sum / (double)cnt; +} + +size_t pm3_find_peaks(const pm3_spectrum_t *spec, pm3_peak_t *peaks, size_t max_peaks) { + + if (spec == NULL || spec->mag == NULL || peaks == NULL || max_peaks == 0) { + return 0; + } + if (spec->nbins < PM3_DSP_DC_GUARD + 3) { + return 0; + } + + size_t found = 0; + + // greedy: repeatedly take the strongest local maximum that is not already + // covered by something we picked + for (size_t pick = 0; pick < max_peaks; pick++) { + + double best = 0.0; + size_t best_bin = 0; + bool have = false; + + for (size_t i = PM3_DSP_DC_GUARD + 1; i + 1 < spec->nbins; i++) { + + if (spec->mag[i] < spec->mag[i - 1] || spec->mag[i] < spec->mag[i + 1]) { + continue; + } + + bool taken = false; + for (size_t k = 0; k < found; k++) { + const size_t d = (peaks[k].bin > i) ? peaks[k].bin - i : i - peaks[k].bin; + if (d <= PM3_DSP_PEAK_SEP) { + taken = true; + break; + } + } + if (taken) { + continue; + } + + if (have == false || spec->mag[i] > best) { + best = spec->mag[i]; + best_bin = i; + have = true; + } + } + + if (have == false || best <= 0.0) { + break; + } + + // parabolic interpolation over the three bins around the peak, in dB. + // The fractional part is the whole point of this table. + const double a = 20.0 * log10(spec->mag[best_bin - 1] + 1e-30); + const double b = 20.0 * log10(spec->mag[best_bin] + 1e-30); + const double c = 20.0 * log10(spec->mag[best_bin + 1] + 1e-30); + const double denom = a - (2.0 * b) + c; + + double delta = 0.0; + if (fabs(denom) > 1e-12) { + delta = 0.5 * (a - c) / denom; + } + if (delta > 0.5) { + delta = 0.5; + } + if (delta < -0.5) { + delta = -0.5; + } + + peaks[found].bin = best_bin; + peaks[found].freq = ((double)best_bin + delta) / (double)spec->n; + peaks[found].clk = (peaks[found].freq > 0.0) ? 1.0 / peaks[found].freq : 0.0; + peaks[found].mag_db = 20.0 * log10((best / (spec->peak + 1e-30)) + 1e-30); + + const double floor_mag = local_floor(spec, best_bin); + peaks[found].prominence = 20.0 * log10((best / (floor_mag + 1e-30)) + 1e-30); + + found++; + } + + return found; +} + +int pm3_stft(const double *sig, size_t len, size_t n, size_t hop, pm3_window_t win, double anchor, pm3_stft_t *out) { + + if (sig == NULL || out == NULL || hop == 0 || is_pow2(n) == false || n < 2) { + return PM3_EINVARG; + } + if (len < n) { + return PM3_EINVARG; + } + + memset(out, 0, sizeof(pm3_stft_t)); + + const size_t nframes = ((len - n) / hop) + 1; + const size_t nbins = (n / 2) + 1; + + if (nframes == 0 || (nframes * nbins) > PM3_DSP_MAX_STFT_CELLS) { + return PM3_EINVARG; + } + + out->n = n; + out->hop = hop; + out->nframes = nframes; + out->nbins = nbins; + out->frame_start = calloc(nframes, sizeof(size_t)); + out->peak_freq = calloc(nframes, sizeof(double)); + out->peak_mag = calloc(nframes, sizeof(double)); + out->frame_energy = calloc(nframes, sizeof(double)); + out->ridge = calloc(nframes * nbins, sizeof(double)); + + if (out->frame_start == NULL || out->peak_freq == NULL || out->peak_mag == NULL || out->frame_energy == NULL || out->ridge == NULL) { + pm3_stft_free(out); + return PM3_EMALLOC; + } + + pm3_fft_plan_t *plan = pm3_fft_plan_create(n); + pm3_cplx_t *buf = calloc(n, sizeof(pm3_cplx_t)); + double *frame = calloc(n, sizeof(double)); + + if (plan == NULL || buf == NULL || frame == NULL) { + pm3_fft_plan_destroy(plan); + free(buf); + free(frame); + pm3_stft_free(out); + return PM3_EMALLOC; + } + + for (size_t f = 0; f < nframes; f++) { + + const size_t off = f * hop; + out->frame_start[f] = off; + + memcpy(frame, sig + off, n * sizeof(double)); + pm3_remove_mean(frame, n); + pm3_apply_window(frame, n, win); + + for (size_t i = 0; i < n; i++) { + buf[i].re = frame[i]; + buf[i].im = 0.0; + } + + pm3_fft(plan, buf, false); + + double best = 0.0; + size_t best_bin = 0; + double energy = 0.0; + + size_t search_lo = PM3_DSP_DC_GUARD; + size_t search_hi = nbins; + + if (anchor > 0.0) { + + const double lo = (anchor / 1.2) * (double)n; + const double hi = (anchor * 1.2) * (double)n; + + if (lo > (double)search_lo) { + search_lo = (size_t)lo; + } + + if (hi < (double)search_hi) { + search_hi = (size_t)hi + 1; + } + + if (search_hi <= search_lo + 2) { + search_lo = PM3_DSP_DC_GUARD; + search_hi = nbins; + } + } + + for (size_t i = 0; i < nbins; i++) { + + const double m = sqrt(buf[i].re * buf[i].re + buf[i].im * buf[i].im); + + out->ridge[(f * nbins) + i] = m; + + if (i >= PM3_DSP_DC_GUARD) { + energy += m * m; + } + + if (i >= search_lo && i < search_hi && m > best) { + best = m; + best_bin = i; + } + } + + // sub bin refine the dominant line so drift shows up as a smooth curve + // rather than a staircase of whole bins + double delta = 0.0; + if (best_bin > 0 && best_bin + 1 < nbins) { + + const double a = 20.0 * log10(out->ridge[(f * nbins) + best_bin - 1] + 1e-30); + const double b = 20.0 * log10(out->ridge[(f * nbins) + best_bin] + 1e-30); + const double c = 20.0 * log10(out->ridge[(f * nbins) + best_bin + 1] + 1e-30); + const double denom = a - (2.0 * b) + c; + + if (fabs(denom) > 1e-12) { + delta = 0.5 * (a - c) / denom; + } + + if (delta > 0.5) { + delta = 0.5; + } + + if (delta < -0.5) { + delta = -0.5; + } + } + + out->peak_mag[f] = best; + out->peak_freq[f] = ((double)best_bin + delta) / (double)n; + out->frame_energy[f] = sqrt(energy); + } + + pm3_fft_plan_destroy(plan); + free(buf); + free(frame); + return PM3_SUCCESS; +} + +void pm3_stft_free(pm3_stft_t *st) { + if (st == NULL) { + return; + } + free(st->frame_start); + free(st->peak_freq); + free(st->peak_mag); + free(st->frame_energy); + free(st->ridge); + memset(st, 0, sizeof(pm3_stft_t)); +} + +const char *pm3_family_name(pm3_family_t fam) { + switch (fam) { + case PM3_FAM_UNKNOWN: + return "unknown"; + case PM3_FAM_ASK: + return "ASK"; + case PM3_FAM_FSK: + return "FSK"; + case PM3_FAM_PSK: + return "PSK"; + case PM3_FAM_NRZ: + return "NRZ"; + case PM3_FAM_MANCHESTER: + return "MANCHESTER"; + } + return "unknown"; +} + +const char *pm3_confidence_name(pm3_confidence_t conf) { + switch (conf) { + case PM3_CONF_NONE: + return "none"; + case PM3_CONF_LOW: + return "low"; + case PM3_CONF_MEDIUM: + return "medium"; + case PM3_CONF_HIGH: + return "high"; + } + return "none"; +} + +// prominence of the plain spectrum at an arbitrary frequency, used to ask +// "is there a line here as well, or only after the non linearity?" +static double prominence_at(const pm3_spectrum_t *spec, double freq) { + + if (freq <= 0.0) { + return 0.0; + } + + size_t bin = (size_t)((freq * (double)spec->n) + 0.5); + if (bin < PM3_DSP_DC_GUARD || bin + 1 >= spec->nbins) { + return 0.0; + } + + // take the strongest of the three bins around the target, a fractional + // clock puts the line between bins + double best = spec->mag[bin]; + if (spec->mag[bin - 1] > best) { + best = spec->mag[bin - 1]; + } + + if (spec->mag[bin + 1] > best) { + best = spec->mag[bin + 1]; + } + + return 20.0 * log10((best / (local_floor(spec, bin) + 1e-30)) + 1e-30); +} + +// strongest peak whose implied clock falls inside [lo, hi] samples/symbol +static const pm3_peak_t *peak_in_range(const pm3_peak_t *peaks, size_t n, double lo, double hi) { + + const pm3_peak_t *best = NULL; + + for (size_t i = 0; i < n; i++) { + if (peaks[i].clk < lo || peaks[i].clk > hi) { + continue; + } + + if (best == NULL || peaks[i].prominence > best->prominence) { + best = &peaks[i]; + } + } + return best; +} + +static bool near(double got, double want, double tol) { + return fabs(got - want) <= (want * tol); +} + +// Look for the two field clock lines the existing FSK demodulators expect. +static bool find_fsk_pair(const pm3_peak_t *peaks, size_t n, double *lo, double *hi, double *ratio) { + + static const double pairs[][2] = { {8.0, 10.0}, {5.0, 8.0} }; + + for (size_t p = 0; p < ARRAYLEN(pairs); p++) { + + const pm3_peak_t *a = NULL; + const pm3_peak_t *b = NULL; + + for (size_t i = 0; i < n; i++) { + if (near(peaks[i].clk, pairs[p][0], 0.12) && (a == NULL || peaks[i].prominence > a->prominence)) { + a = &peaks[i]; + } + + if (near(peaks[i].clk, pairs[p][1], 0.12) && (b == NULL || peaks[i].prominence > b->prominence)) { + b = &peaks[i]; + } + } + + // Both tones have to be genuinely strong, and comparable to each + // other: an FSK stream spends time on each, so neither line dominates. + // A single PSK subcarrier plus one spurious neighbour would otherwise pass for a pair. + if (a != NULL && b != NULL + && a->prominence > 10.0 && b->prominence > 10.0 + && fabs(a->mag_db - b->mag_db) < 10.0) { + + *hi = a->clk; + *lo = b->clk; + *ratio = b->clk / a->clk; + return true; + } + } + return false; +} + +// Find the spacing of the harmonic comb that best explains a set of peaks. +// +// Squaring a symbol stream produces energy at every integer multiple of the symbol rate. +// Picking the tallest line therefore lands on a harmonic as often as not. +// Try each peak and each peak divided by a small integer as a candidate spacing +// and keep whichever one places the most peaks on a multiple of itself +static double comb_fundamental(const pm3_peak_t *peaks, size_t npeaks, size_t n, double min_clk) { + + double best = 0.0; + size_t best_count = 0; + + for (size_t i = 0; i < npeaks; i++) { + + if (peaks[i].prominence < COMB_MIN_PROMINENCE) { + continue; + } + + for (size_t div = 1; div <= 4; div++) { + + const double cand = peaks[i].freq / (double)div; + if (cand <= 0.0) { + continue; + } + + size_t count = 0; + for (size_t j = 0; j < npeaks; j++) { + + if (peaks[j].prominence < COMB_MIN_PROMINENCE) { + continue; + } + + const double k = floor((peaks[j].freq / cand) + 0.5); + if (k < 1.0 || k > (double)COMB_MAX_HARMONIC) { + continue; + } + + // a line may sit about a bin away from the ideal multiple. + // The tolerance is tied to the peak's own frequency, not to the candidate spacing + const double tol = (1.5 / (double)n) + (0.015 * peaks[j].freq); + if (fabs(peaks[j].freq - (k * cand)) <= tol) { + count++; + } + } + + if (count > best_count || (count == best_count && cand > best)) { + best_count = count; + best = cand; + } + } + } + + // a single line is not a comb + if (best_count < 2) { + return 0.0; + } + // and no LF tag clocks itself outside this range + if (best < (1.0 / COMB_MAX_CLK) || best > (1.0 / min_clk)) { + return 0.0; + } + return best; +} + +static pm3_confidence_t confidence_from_db(double db) { + if (db >= 12.0) { + return PM3_CONF_HIGH; + } + + if (db >= 6.0) { + return PM3_CONF_MEDIUM; + } + + if (db >= 2.0) { + return PM3_CONF_LOW; + } + + return PM3_CONF_NONE; +} + +static int cmp_double(const void *a, const void *b); + +// Rectified, low passed copy of `sig` with its own DC left in place. +// The window is centred so a switching edge keeps its position +static double *envelope_raw(const double *sig, size_t len, size_t smooth) { + + if (sig == NULL || len == 0) { + return NULL; + } + + if (smooth == 0) { + smooth = PM3_DSP_ENVELOPE_SMOOTH; + } + if (smooth > len) { + smooth = len; + } + + double *out = calloc(len, sizeof(double)); + double *ps = calloc(len + 1, sizeof(double)); + if (out == NULL || ps == NULL) { + free(out); + free(ps); + return NULL; + } + + double mean = 0.0; + for (size_t i = 0; i < len; i++) { + mean += sig[i]; + } + mean /= (double)len; + + for (size_t i = 0; i < len; i++) { + ps[i + 1] = ps[i] + fabs(sig[i] - mean); + } + + const size_t half = smooth / 2; + + for (size_t i = 0; i < len; i++) { + const size_t lo = (i > half) ? (i - half) : 0; + size_t hi = i + smooth - half; + if (hi > len) { + hi = len; + } + out[i] = (ps[hi] - ps[lo]) / (double)(hi - lo); + } + + free(ps); + return out; +} + +double *pm3_envelope(const double *sig, size_t len, size_t smooth) { + + double *out = envelope_raw(sig, len, smooth); + if (out == NULL) { + return NULL; + } + + pm3_remove_mean(out, len); + + double energy = 0.0; + for (size_t i = 0; i < len; i++) { + energy += out[i] * out[i]; + } + + if (energy > 0.0) { + const double scale = 1.0 / sqrt(energy / (double)len); + for (size_t i = 0; i < len; i++) { + out[i] *= scale; + } + } + + return out; +} + +double pm3_envelope_depth(const double *sig, size_t len) { + + if (sig == NULL || len < 32) { + return 0.0; + } + + double *env = envelope_raw(sig, len, 0); + if (env == NULL) { + return 0.0; + } + + // percentiles rather than min and max. + // one clipped sample or one dead spot in the capture does not impact + const size_t want = 8192; + const size_t stride = (len > want) ? (len / want) : 1; + + double *s = calloc((len / stride) + 1, sizeof(double)); + if (s == NULL) { + free(env); + return 0.0; + } + + size_t cnt = 0; + for (size_t i = 0; i < len; i += stride) { + s[cnt++] = env[i]; + } + free(env); + + if (cnt < 16) { + free(s); + return 0.0; + } + + qsort(s, cnt, sizeof(double), cmp_double); + + const double hi = s[(size_t)((double)cnt * 0.9)]; + const double lo = s[(size_t)((double)cnt * 0.1)]; + free(s); + + return ((hi + lo) > 0.0) ? ((hi - lo) / (hi + lo)) : 0.0; +} + +void pm3_slice_free(pm3_slice_t *out) { + + if (out == NULL) { + return; + } + free(out->bits); + out->bits = NULL; + out->nbits = 0; +} + +int pm3_ask_slice(const double *sig, size_t len, double clk, pm3_slice_t *out) { + + if (sig == NULL || out == NULL || clk < 2.0 || len < (size_t)(clk * 4.0)) { + return PM3_EINVARG; + } + + memset(out, 0, sizeof(pm3_slice_t)); + + // Prefix sums, so the average over a chip window costs two lookups no matter how wide the chip is. + double *ps = calloc(len + 1, sizeof(double)); + if (ps == NULL) { + return PM3_EMALLOC; + } + for (size_t i = 0; i < len; i++) { + ps[i + 1] = ps[i] + sig[i]; + } + + // Robust levels rather than min and max: one clipped sample does not set the threshold for the capture. + const size_t want = 4096; + const size_t stride = (len > want) ? (len / want) : 1; + + double *s = calloc((len / stride) + 1, sizeof(double)); + if (s == NULL) { + free(ps); + return PM3_EMALLOC; + } + + size_t cnt = 0; + for (size_t i = 0; i < len; i += stride) { + s[cnt++] = sig[i]; + } + qsort(s, cnt, sizeof(double), cmp_double); + + const double lo = s[(size_t)((double)cnt * 0.1)]; + const double hi = s[(size_t)((double)cnt * 0.9)]; + free(s); + + const double thr = (hi + lo) * 0.5; + const double span = (hi - lo) * 0.5; + + if (span <= 0.0) { + free(ps); + return PM3_ESOFT; + } + + const size_t nsym = (size_t)(((double)len - clk) / clk); + if (nsym < 4) { + free(ps); + return PM3_ESOFT; + } + + // The middle half of the chip. Wide enough to average the noise down, + // narrow enough that a late edge on either side never reaches it. + const size_t half = (size_t)(clk * 0.25); + const size_t nphase = (size_t)clk; + + double best_score = -1.0; + size_t best_phase = 0; + + for (size_t p = 0; p < nphase; p++) { + + double score = 0.0; + + for (size_t k = 0; k < nsym; k++) { + + const double centre = (double)p + ((double)k + 0.5) * clk; + const size_t a = (size_t)(centre - (double)half); + const size_t b = (size_t)(centre + (double)half); + + if (b >= len || b <= a) { + break; + } + + const double mean = (ps[b] - ps[a]) / (double)(b - a); + score += fabs(mean - thr); + } + + if (score > best_score) { + best_score = score; + best_phase = p; + } + } + + out->bits = calloc(nsym, sizeof(uint8_t)); + if (out->bits == NULL) { + free(ps); + return PM3_EMALLOC; + } + + const double dead = span * 0.15; + double eye = 0.0; + + for (size_t k = 0; k < nsym; k++) { + + const double centre = (double)best_phase + ((double)k + 0.5) * clk; + const size_t a = (size_t)(centre - (double)half); + const size_t b = (size_t)(centre + (double)half); + + if (b >= len || b <= a) { + break; + } + + const double mean = (ps[b] - ps[a]) / (double)(b - a); + const double dist = fabs(mean - thr); + + if (dist < dead) { + out->bits[out->nbits] = 7; + out->nerrors++; + } else { + out->bits[out->nbits] = (mean > thr) ? 1 : 0; + } + + eye += dist / span; + out->nbits++; + } + + free(ps); + + out->phase = best_phase; + out->eye = (out->nbits) ? (eye / (double)out->nbits) : 0.0; + + return (out->nbits > 0) ? PM3_SUCCESS : PM3_ESOFT; +} + +bool pm3_is_switched_carrier(const double *sig, size_t len) { + + if (sig == NULL || len < 256) { + return false; + } + + double mean = 0.0; + for (size_t i = 0; i < len; i++) { + mean += sig[i]; + } + mean /= (double)len; + + size_t flips = 0, pairs = 0; + int prev = 0; + + for (size_t i = 0; i < len; i++) { + + const double d = sig[i] - mean; + const int s = (d > 0.0) ? 1 : ((d < 0.0) ? -1 : 0); + + if (s == 0) { + continue; + } + + if (prev != 0) { + pairs++; + if (s != prev) { + flips++; + } + } + prev = s; + } + + if (pairs == 0) { + return false; + } + + if (((double)flips / (double)pairs) < PM3_DSP_FLIP_FRAC) { + return false; + } + + return (pm3_envelope_depth(sig, len) > PM3_DSP_ENVELOPE_DEPTH); +} + +static int analyse_once(const double *sig, size_t len, size_t n, pm3_window_t win, pm3_spec_analysis_t *out) { + + if (sig == NULL || out == NULL || len == 0) { + return PM3_EINVARG; + } + + memset(out, 0, sizeof(pm3_spec_analysis_t)); + + const size_t top = PM3_DSP_MAX_PEAKS; + + pm3_spectrum_t plain; + int res = pm3_spectrum(sig, len, n, win, &plain); + if (res != PM3_SUCCESS) { + return res; + } + + out->npeaks = pm3_find_peaks(&plain, out->peaks, top); + + double *work = calloc(len, sizeof(double)); + if (work == NULL) { + pm3_spectrum_free(&plain); + return PM3_EMALLOC; + } + + pm3_spectrum_t sq; + memset(&sq, 0, sizeof(sq)); + + pm3_delay_multiply(sig, len, 0, work); + if (pm3_spectrum(work, len, n, win, &sq) == PM3_SUCCESS) { + out->have_sq = true; + out->nsq_peaks = pm3_find_peaks(&sq, out->sq_peaks, top); + } + + static const size_t lags[PM3_DSP_NDELAYS] = { 1, 2, 4, 8 }; + out->ndelay = PM3_DSP_NDELAYS; + + for (size_t i = 0; i < PM3_DSP_NDELAYS; i++) { + + out->delay[i] = lags[i]; + pm3_delay_multiply(sig, len, lags[i], work); + + pm3_spectrum_t ds; + if (pm3_spectrum(work, len, n, win, &ds) != PM3_SUCCESS) { + continue; + } + + pm3_peak_t got[1]; + if (pm3_find_peaks(&ds, got, 1) == 1) { + out->delay_peak[i] = got[0]; + out->delay_valid[i] = true; + } + pm3_spectrum_free(&ds); + } + free(work); + + // low frequency energy relative to the strongest line. + // NRZ piles up at DC, + // Manchester and biphase have null. + double dc_sum = 0.0; + size_t dc_cnt = 0; + const size_t dc_hi = (plain.nbins < 32) ? plain.nbins : 32; + for (size_t i = PM3_DSP_DC_GUARD; i < dc_hi; i++) { + dc_sum += plain.mag[i]; + dc_cnt++; + } + out->dc_ratio = (dc_cnt && plain.peak > 0.0) ? (dc_sum / (double)dc_cnt) / plain.peak : 0.0; + + const pm3_peak_t *carrier = peak_in_range(out->peaks, out->npeaks, 1.8, 12.0); + if (carrier != NULL && carrier->prominence > 4.0) { + out->have_carrier = true; + out->carrier_clk = carrier->clk; + } + + if (out->have_carrier && out->carrier_clk <= PM3_DSP_NYQUIST_CLK) { + out->carrier_at_nyquist = true; + out->envelope_depth = pm3_envelope_depth(sig, len); + out->needs_envelope = (out->envelope_depth > PM3_DSP_ENVELOPE_DEPTH); + } + + const bool is_fsk = find_fsk_pair(out->peaks, out->npeaks, &out->fsk_clk_lo, &out->fsk_clk_hi, &out->fsk_ratio); + + const double min_clk = is_fsk ? (out->fsk_clk_lo * 2.0) : COMB_MIN_CLK; + + double fundamental = 0.0; + if (out->have_sq) { + fundamental = comb_fundamental(out->sq_peaks, out->nsq_peaks, sq.n, min_clk); + } + + if (fundamental > 0.0) { + + out->symbol_clk = 1.0 / fundamental; + out->have_symbol_clk = true; + out->sq_prominence = prominence_at(&sq, fundamental); + + const double second = prominence_at(&sq, fundamental * 2.0); + out->transition_coded = (second - out->sq_prominence) > 6.0; + + } else { + const pm3_peak_t *sym_plain = peak_in_range(out->peaks, out->npeaks, 12.0, 600.0); + if (sym_plain != NULL) { + out->symbol_clk = sym_plain->clk; + out->have_symbol_clk = true; + } + } + + if (out->have_symbol_clk) { + out->sym_prominence = prominence_at(&plain, 1.0 / out->symbol_clk); + } + + if (is_fsk) { + + out->family = PM3_FAM_FSK; + out->confidence = confidence_from_db( + 0.5 * (prominence_at(&plain, 1.0 / out->fsk_clk_lo) + prominence_at(&plain, 1.0 / out->fsk_clk_hi)) + ); + + } else if (out->have_carrier) { + + out->family = PM3_FAM_PSK; + out->confidence = confidence_from_db(prominence_at(&plain, 1.0 / out->carrier_clk)); + + } else if (out->transition_coded) { + + out->family = PM3_FAM_MANCHESTER; + out->confidence = confidence_from_db(out->sq_prominence); + + } else if (out->dc_ratio > 0.4) { + + out->family = PM3_FAM_NRZ; + out->confidence = confidence_from_db(out->sq_prominence); + + } else if (out->have_symbol_clk && out->sq_prominence > 6.0) { + + out->family = PM3_FAM_ASK; + out->confidence = confidence_from_db(out->sq_prominence); + + } else { + out->family = PM3_FAM_UNKNOWN; + out->confidence = PM3_CONF_NONE; + } + + pm3_spectrum_free(&plain); + if (out->have_sq) { + pm3_spectrum_free(&sq); + } + return PM3_SUCCESS; +} + +int pm3_analyse(const double *sig, size_t len, size_t n, pm3_window_t win, pm3_spec_analysis_t *out) { + + int res = analyse_once(sig, len, n, win, out); + if (res != PM3_SUCCESS || out->needs_envelope == false) { + return res; + } + + const double carrier_clk = out->carrier_clk; + const double depth = out->envelope_depth; + + double *env = pm3_envelope(sig, len, 0); + if (env == NULL) { + return res; + } + + pm3_spec_analysis_t inner; + const int inner_res = analyse_once(env, len, n, win, &inner); + free(env); + + if (inner_res != PM3_SUCCESS) { + return res; + } + + *out = inner; + out->carrier_at_nyquist = true; + out->needs_envelope = true; + out->envelope_depth = depth; + out->carrier_clk = carrier_clk; + out->have_carrier = true; + + return PM3_SUCCESS; +} + +static int cmp_double(const void *a, const void *b) { + + const double x = *(const double *)a; + const double y = *(const double *)b; + + if (x < y) { + return -1; + } + + if (x > y) { + return 1; + } + return 0; +} + +int pm3_signal_stats(const double *sig, size_t len, pm3_sigstat_t *out) { + + if (sig == NULL || out == NULL || len < 256) { + return PM3_EINVARG; + } + + memset(out, 0, sizeof(pm3_sigstat_t)); + + double sumsq = 0.0; + for (size_t i = 0; i < len; i++) { + sumsq += sig[i] * sig[i]; + } + + const double sd = sqrt(sumsq / (double)len); + if (sd <= 0.0) { + return PM3_ESOFT; + } + + const double hi_th = sd * PM3_DSP_RUN_HYST; + const double lo_th = -hi_th; + + double *runs = calloc(len + 1, sizeof(double)); + if (runs == NULL) { + return PM3_EMALLOC; + } + + size_t nruns = 0; + int state = 0; + size_t last_edge = 0; + + for (size_t i = 0; i < len; i++) { + + int next = state; + if (sig[i] > hi_th) { + next = 1; + } else if (sig[i] < lo_th) { + next = -1; + } + + if (state == 0) { + state = next; + last_edge = i; + continue; + } + + if (next != state) { + if (i > last_edge) { + runs[nruns++] = (double)(i - last_edge); + } + last_edge = i; + state = next; + } + } + + if (nruns < 16) { + free(runs); + return PM3_ESOFT; + } + + qsort(runs, nruns, sizeof(double), cmp_double); + + // The chip is the shortest run that actually repeats. Taking the outright + // minimum would pick up a glitch, so use a low percentile. + out->chip = runs[nruns / 10]; + out->long_run = runs[(nruns * 95) / 100]; + + if (out->chip <= 0.0) { + free(runs); + return PM3_ESOFT; + } + + out->run_ratio = out->long_run / out->chip; + + size_t shortish = 0; + const double limit = out->chip * PM3_DSP_RUN_SHORT; + for (size_t i = 0; i < nruns; i++) { + if (runs[i] <= limit) { + shortish++; + } + } + out->short_fraction = (double)shortish / (double)nruns; + out->transition_coded = (out->short_fraction > PM3_DSP_RUN_SHORT_FRAC); + out->valid = true; + + free(runs); + + out->period = pm3_autocorr_period(sig, len, &out->period_strength); + return PM3_SUCCESS; +} + +double pm3_autocorr_period(const double *sig, size_t len, double *strength) { + + if (strength != NULL) { + *strength = 0.0; + } + + if (sig == NULL || len < (PM3_DSP_ACORR_MIN_LAG * 4)) { + return 0.0; + } + + size_t n = pm3_next_pow2(len * 2); + if (n > PM3_DSP_MAX_FFT) { + n = PM3_DSP_MAX_FFT; + } + + pm3_fft_plan_t *plan = pm3_fft_plan_create(n); + pm3_cplx_t *buf = calloc(n, sizeof(pm3_cplx_t)); + if (plan == NULL || buf == NULL) { + pm3_fft_plan_destroy(plan); + free(buf); + return 0.0; + } + + const size_t take = (len < n / 2) ? len : n / 2; + for (size_t i = 0; i < take; i++) { + buf[i].re = sig[i]; + } + + pm3_fft(plan, buf, false); + + // Wiener-Khinchin: the autocorrelation is the inverse transform of the power spectrum + for (size_t i = 0; i < n; i++) { + const double p = (buf[i].re * buf[i].re) + (buf[i].im * buf[i].im); + buf[i].re = p; + buf[i].im = 0.0; + } + + pm3_fft(plan, buf, true); + pm3_fft_plan_destroy(plan); + + const double energy = buf[0].re; + if (energy <= 0.0) { + free(buf); + return 0.0; + } + + size_t hi = take / 2; + if (hi > PM3_DSP_ACORR_MAX_LAG) { + hi = PM3_DSP_ACORR_MAX_LAG; + } + + double best = 0.0; + size_t best_lag = 0; + + for (size_t lag = PM3_DSP_ACORR_MIN_LAG; lag < hi; lag++) { + + const double r = buf[lag].re / energy; + if (r <= best) { + continue; + } + // only interested in genuine local maxima + if (buf[lag].re < buf[lag - 1].re || buf[lag].re < buf[lag + 1].re) { + continue; + } + best = r; + best_lag = lag; + } + + double period = 0.0; + + if (best_lag > 0 && best >= PM3_DSP_ACORR_MIN_STRENGTH) { + + // sub sample refine + const double a = buf[best_lag - 1].re; + const double b = buf[best_lag].re; + const double c = buf[best_lag + 1].re; + const double denom = a - (2.0 * b) + c; + + double delta = 0.0; + if (fabs(denom) > 1e-30) { + delta = 0.5 * (a - c) / denom; + } + + if (delta > 0.5) { + delta = 0.5; + } + + if (delta < -0.5) { + delta = -0.5; + } + + period = (double)best_lag + delta; + if (strength != NULL) { + *strength = best; + } + } + + free(buf); + return period; +} + +double *pm3_resample(const double *in, size_t len, double ratio, size_t *out_len) { + + if (in == NULL || out_len == NULL || len < 2 || ratio <= 0.0) { + return NULL; + } + + const double want = (double)len * ratio; + if (want < 2.0 || want > (double)PM3_DSP_MAX_RESAMPLE) { + return NULL; + } + + const size_t n = (size_t)want; + double *out = calloc(n, sizeof(double)); + if (out == NULL) { + return NULL; + } + + for (size_t i = 0; i < n; i++) { + + const double pos = (double)i / ratio; + size_t idx = (size_t)pos; + + if (idx + 1 >= len) { + out[i] = in[len - 1]; + continue; + } + + const double frac = pos - (double)idx; + out[i] = (in[idx] * (1.0 - frac)) + (in[idx + 1] * frac); + } + + *out_len = n; + return out; +} + +// Coherent PSK receiver. +int pm3_psk_demod(const double *sig, size_t len, int fc, double clk, + uint8_t *bits, size_t *nbits, double *clk_out, int *phase_out, + double *score_out, uint8_t *abs_bits) { + + if (sig == NULL || bits == NULL || nbits == NULL) { + return PM3_EINVARG; + } + + const size_t w = (size_t)(clk + 0.5); + if (fc < 2 || clk < 4.0 || w < 4 || len < w * 8) { + return PM3_EINVARG; + } + + const size_t zlen = len - w + 1; + + double *zr = calloc(zlen, sizeof(double)); + double *zi = calloc(zlen, sizeof(double)); + if (zr == NULL || zi == NULL) { + free(zr); + free(zi); + return PM3_EMALLOC; + } + + // Mix down by the subcarrier and integrate over one symbol + { + double sr = 0.0, si = 0.0; + for (size_t i = 0; i < w; i++) { + const double a = -2.0 * M_PI * (double)i / (double)fc; + sr += sig[i] * cos(a); + si += sig[i] * sin(a); + } + zr[0] = sr; + zi[0] = si; + + for (size_t i = 1; i < zlen; i++) { + const double aout = -2.0 * M_PI * (double)(i - 1) / (double)fc; + const double ain = -2.0 * M_PI * (double)(i + w - 1) / (double)fc; + sr += (sig[i + w - 1] * cos(ain)) - (sig[i - 1] * cos(aout)); + si += (sig[i + w - 1] * sin(ain)) - (sig[i - 1] * sin(aout)); + zr[i] = sr; + zi[i] = si; + } + } + + // Symbol timing. + double best_clk = clk, best_score = -1.0; + int best_phase = 0; + + for (int step = -20; step <= 20; step++) { + + const double c = clk + ((double)step * 0.1); + if (c < 4.0) { + continue; + } + + for (size_t p = 0; p < w; p++) { + + double sum = 0.0; + size_t cnt = 0; + + for (double pos = (double)p; pos < (double)zlen; pos += c) { + const size_t k = (size_t)(pos + 0.5); + if (k >= zlen) { + break; + } + sum += sqrt((zr[k] * zr[k]) + (zi[k] * zi[k])); + cnt++; + } + + if (cnt < 8) { + continue; + } + + const double score = sum / (double)cnt; + if (score > best_score) { + best_score = score; + best_clk = c; + best_phase = (int)p; + } + } + } + + if (best_score < 0.0) { + free(zr); + free(zi); + return PM3_ESOFT; + } + + // Absolute phase reference, when the caller wants one. + double ref_r = 1.0, ref_i = 0.0; + + if (abs_bits != NULL) { + + double sr = 0.0, si = 0.0; + for (double pos = (double)best_phase; pos < (double)zlen; pos += best_clk) { + const size_t k = (size_t)(pos + 0.5); + if (k >= zlen) { + break; + } + sr += (zr[k] * zr[k]) - (zi[k] * zi[k]); + si += 2.0 * zr[k] * zi[k]; + } + + const double mag = sqrt((sr * sr) + (si * si)); + if (mag > 1e-12) { + const double half = 0.5 * atan2(si, sr); + ref_r = cos(half); + ref_i = sin(half); + } + } + + // Differential detection at the recovered timing. + size_t n = 0; + double prev_r = 0.0, prev_i = 0.0; + uint8_t phase = 0; + bool first = true; + + for (double pos = (double)best_phase; pos < (double)zlen && n < *nbits; pos += best_clk) { + + const size_t k = (size_t)(pos + 0.5); + if (k >= zlen) { + break; + } + + if (first == false) { + // Re( s * conj(s_prev) ) - negative means the phase reversed + if (((zr[k] * prev_r) + (zi[k] * prev_i)) < 0.0) { + phase ^= 1; + } + } + + prev_r = zr[k]; + prev_i = zi[k]; + first = false; + + if (abs_bits != NULL) { + // Re( z * conj(ref) ) - which side of the axis this symbol sits on + abs_bits[n] = (((zr[k] * ref_r) + (zi[k] * ref_i)) < 0.0) ? 1 : 0; + } + + bits[n++] = phase; + } + + free(zr); + free(zi); + + *nbits = n; + if (clk_out != NULL) { + *clk_out = best_clk; + } + if (phase_out != NULL) { + *phase_out = best_phase; + } + if (score_out != NULL) { + *score_out = best_score; + } + return (n >= 32) ? PM3_SUCCESS : PM3_ESOFT; +} + +// Coherent chip slicer for amplitude keyed traces. +int pm3_ask_chips(const double *sig, size_t len, double chip, + uint8_t *chips, size_t *nchips, double *chip_out, int *phase_out) { + + if (sig == NULL || chips == NULL || nchips == NULL) { + return PM3_EINVARG; + } + + const size_t w = (size_t)(chip + 0.5); + if (chip < 2.0 || w < 2 || len < w * 16) { + return PM3_EINVARG; + } + + const size_t clen = len - w + 1; + + double *c = calloc(clen, sizeof(double)); + if (c == NULL) { + return PM3_EMALLOC; + } + + double run = 0.0; + for (size_t i = 0; i < w; i++) { + run += sig[i]; + } + c[0] = run; + for (size_t i = 1; i < clen; i++) { + run += sig[i + w - 1] - sig[i - 1]; + c[i] = run; + } + + double best_chip = chip, best_score = -1.0; + int best_phase = 0; + + for (int step = -20; step <= 20; step++) { + + const double cc = chip + ((double)step * 0.05); + if (cc < 2.0) { + continue; + } + + for (size_t p = 0; p < w; p++) { + + double sum = 0.0; + size_t cnt = 0; + + for (double pos = (double)p; pos < (double)clen; pos += cc) { + const size_t k = (size_t)(pos + 0.5); + if (k >= clen) { + break; + } + sum += fabs(c[k]); + cnt++; + } + + if (cnt < 64) { + continue; + } + + const double score = sum / (double)cnt; + if (score > best_score) { + best_score = score; + best_chip = cc; + best_phase = (int)p; + } + } + } + + if (best_score < 0.0) { + free(c); + return PM3_ESOFT; + } + + size_t n = 0; + for (double pos = (double)best_phase; pos < (double)clen && n < *nchips; pos += best_chip) { + const size_t k = (size_t)(pos + 0.5); + if (k >= clen) { + break; + } + chips[n++] = (c[k] > 0.0) ? 1 : 0; + } + + free(c); + + *nchips = n; + if (chip_out != NULL) { + *chip_out = best_chip; + } + + if (phase_out != NULL) { + *phase_out = best_phase; + } + + return (n >= 64) ? PM3_SUCCESS : PM3_ESOFT; +} diff --git a/client/src/pm3_dsp.h b/client/src/pm3_dsp.h new file mode 100644 index 000000000..1920ac026 --- /dev/null +++ b/client/src/pm3_dsp.h @@ -0,0 +1,200 @@ +//----------------------------------------------------------------------------- +// 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. +//----------------------------------------------------------------------------- +// Host side DSP primitives; FFT, windows, power spectrum, STFT +//----------------------------------------------------------------------------- + +#ifndef PM3_DSP_H__ +#define PM3_DSP_H__ + +#include +#include "common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define PM3_DSP_MAX_FFT 262144 +#define PM3_DSP_DC_GUARD 2 +#define PM3_DSP_MAX_RESAMPLE (40000 * 32) +#define PM3_DSP_NYQUIST_CLK 2.5 +#define PM3_DSP_ENVELOPE_SMOOTH 8 +#define PM3_DSP_ENVELOPE_DEPTH 0.6 + +#define PM3_DSP_FLIP_FRAC 0.7 + +typedef enum { + PM3_WIN_HANN = 0, + PM3_WIN_HAMMING, + PM3_WIN_BLACKMAN, + PM3_WIN_RECT, +} pm3_window_t; + +typedef struct { + double re; + double im; +} pm3_cplx_t; + +typedef struct { + size_t n; + pm3_cplx_t *twiddle; // n / 2 entries, exp(-2*pi*i*k/n) + uint32_t *rev; // n entries, bit reversal permutation +} pm3_fft_plan_t; + +typedef struct { + size_t n; // transform length + size_t nbins; // n / 2 + 1, input is real + double *mag; // linear magnitude per bin + double peak; // largest magnitude found + size_t peak_bin; // ... and where +} pm3_spectrum_t; + +typedef struct { + size_t bin; // integer bin the peak was found in + double freq; // cycles/sample, parabolic sub bin estimate + double clk; // samples/symbol, 1.0 / freq + double mag_db; // dB relative to the strongest bin in the spectrum + double prominence; // dB above the local noise floor +} pm3_peak_t; + +typedef struct { + size_t nframes; + size_t nbins; + size_t n; // window length + size_t hop; + size_t *frame_start; // nframes entries, offset of each frame + double *peak_freq; // nframes entries, cycles/sample of dominant bin + double *peak_mag; // nframes entries, magnitude of dominant bin + double *frame_energy; // nframes entries, total in band energy + double *ridge; // nframes * nbins, magnitude, row major +} pm3_stft_t; + + +#define PM3_DSP_MAX_PEAKS 16 +#define PM3_DSP_NDELAYS 4 + +typedef enum { + PM3_FAM_UNKNOWN = 0, + PM3_FAM_ASK, + PM3_FAM_FSK, + PM3_FAM_PSK, + PM3_FAM_NRZ, + PM3_FAM_MANCHESTER, +} pm3_family_t; + +typedef enum { + PM3_CONF_NONE = 0, + PM3_CONF_LOW, + PM3_CONF_MEDIUM, + PM3_CONF_HIGH, +} pm3_confidence_t; + +typedef struct { + + size_t npeaks; + pm3_peak_t peaks[PM3_DSP_MAX_PEAKS]; + bool have_sq; + size_t nsq_peaks; + pm3_peak_t sq_peaks[PM3_DSP_MAX_PEAKS]; + size_t ndelay; + size_t delay[PM3_DSP_NDELAYS]; + pm3_peak_t delay_peak[PM3_DSP_NDELAYS]; + bool delay_valid[PM3_DSP_NDELAYS]; + + pm3_family_t family; + pm3_confidence_t confidence; + + double symbol_clk; // best symbol rate estimate, samples/symbol + bool have_symbol_clk; + + double carrier_clk; // subcarrier / field clock, when one is present + bool have_carrier; + + double fsk_clk_hi; // the shorter of the two FSK field clocks + double fsk_clk_lo; + double fsk_ratio; + + bool transition_coded; + double dc_ratio; // low frequency energy relative to the peak + double sym_prominence; // prominence of the plain spectrum at symbol_clk + double sq_prominence; // prominence of the squaring line + + bool carrier_at_nyquist; + double envelope_depth; // 0.0 constant amplitude, 1.0 switched fully off + bool needs_envelope; +} pm3_spec_analysis_t; + +const char *pm3_family_name(pm3_family_t fam); +const char *pm3_confidence_name(pm3_confidence_t conf); + +int pm3_analyse(const double *sig, size_t len, size_t n, pm3_window_t win, pm3_spec_analysis_t *out); + +pm3_fft_plan_t *pm3_fft_plan_create(size_t n); +void pm3_fft_plan_destroy(pm3_fft_plan_t *plan); +void pm3_fft(const pm3_fft_plan_t *plan, pm3_cplx_t *data, bool inverse); + +size_t pm3_next_pow2(size_t v); +bool pm3_window_from_str(const char *str, pm3_window_t *out); +const char *pm3_window_name(pm3_window_t win); +void pm3_apply_window(double *data, size_t len, pm3_window_t win); + +double *pm3_extract(const int32_t *src, size_t len, size_t start, size_t count); +void pm3_remove_mean(double *data, size_t len); +double *pm3_envelope(const double *sig, size_t len, size_t smooth); + +typedef struct { + uint8_t *bits; + size_t nbits; + size_t nerrors; + size_t phase; // sample offset of the first chip centre + double eye; // mean distance from the threshold, 1.0 = fully open +} pm3_slice_t; + +int pm3_ask_slice(const double *sig, size_t len, double clk, pm3_slice_t *out); +void pm3_slice_free(pm3_slice_t *out); +bool pm3_is_switched_carrier(const double *sig, size_t len); +double pm3_envelope_depth(const double *sig, size_t len); +void pm3_delay_multiply(const double *in, size_t len, size_t lag, double *out); +int pm3_spectrum(const double *sig, size_t len, size_t n, pm3_window_t win, pm3_spectrum_t *out); +void pm3_spectrum_free(pm3_spectrum_t *spec); +size_t pm3_find_peaks(const pm3_spectrum_t *spec, pm3_peak_t *peaks, size_t max_peaks); +int pm3_stft(const double *sig, size_t len, size_t n, size_t hop, pm3_window_t win, double anchor, pm3_stft_t *out); +void pm3_stft_free(pm3_stft_t *st); + +typedef struct { + bool valid; + double chip; // shortest repeated run length, samples + double long_run; // 95th percentile run length, samples + double run_ratio; // long_run / chip + double short_fraction; // runs no longer than 2.5 chips, as a fraction + bool transition_coded; // runs never much exceed two chips + double period; // autocorrelation repeat period, 0 if none + double period_strength; // normalised autocorrelation at that lag +} pm3_sigstat_t; + + +int pm3_signal_stats(const double *sig, size_t len, pm3_sigstat_t *out); +double pm3_autocorr_period(const double *sig, size_t len, double *strength); +int pm3_psk_demod(const double *sig, size_t len, int fc, double clk, + uint8_t *bits, size_t *nbits, double *clk_out, int *phase_out, + double *score_out, uint8_t *abs_bits); + +int pm3_ask_chips(const double *sig, size_t len, double chip, uint8_t *chips, size_t *nchips, double *chip_out, int *phase_out); +double *pm3_resample(const double *in, size_t len, double ratio, size_t *out_len); + +#ifdef __cplusplus +} +#endif +#endif // PM3_DSP_H__ diff --git a/client/src/pm3_fit.c b/client/src/pm3_fit.c new file mode 100644 index 000000000..ea060f040 --- /dev/null +++ b/client/src/pm3_fit.c @@ -0,0 +1,1066 @@ +//----------------------------------------------------------------------------- +// 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. +//----------------------------------------------------------------------------- +// Symbol template generation and matched filter hypothesis scoring +//----------------------------------------------------------------------------- + +#include "pm3_fit.h" + +#include +#include +#include + +#include "pm3_cmd.h" +#include "commonutil.h" // ARRAYLEN +#include "pm3_dsp.h" + +#define PM3_FIT_VAR_FLOOR 0.01 +#define PM3_FIT_MIN_CONTRAST 0.15 +#define PM3_FIT_OCTAVE_TOL 6.0 +#define PM3_FIT_MAX_MID 0.85 +#define PM3_FIT_MAX_MID_FSK 0.78 +#define PM3_FIT_MIN_TONE_SPLIT 0.30 +#define PM3_FIT_MIN_SURVIVAL 0.01 +#define PM3_FIT_MIN_TONE_BALANCE 0.15 +#define PM3_FIT_MIN_TONE_ENERGY 0.30 +#define PM3_FIT_FSK_MIN_CYCLES 4 +#define PM3_FIT_AGC_SYMBOLS 8 +#define PM3_FIT_CHIP_PAIR 0.90 +#define PM3_FIT_STRUCT_MIN_CLK 16.0 +#define PM3_FIT_STRUCT_TOL 0.25 + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +// Clock candidates. +static const int g_fit_clocks[] = { 8, 16, 32, 40, 50, 64, 100, 128, 256, 272, 384 }; + +// FSK field clock pairs, as fchigh / fclow. +static const int g_fsk_pairs[][2] = { {10, 8}, {8, 5} }; + +// PSK subcarrier periods. +static const int g_psk_fc[] = { 2, 4, 8 }; + +static const int g_fit_q[] = { 2, 8, 20, 50 }; + +const char *pm3_mod_name(pm3_mod_t mod) { + switch (mod) { + case PM3_MOD_ASK: + return "ASK"; + case PM3_MOD_FSK: + return "FSK"; + case PM3_MOD_PSK: + return "PSK"; + case PM3_MOD_NRZ: + return "NRZ"; + } + return "?"; +} + +const char *pm3_enc_name(pm3_enc_t enc) { + switch (enc) { + case PM3_ENC_RAW: + return "raw"; + case PM3_ENC_MANCHESTER: + return "manchester"; + case PM3_ENC_BIPHASE: + return "biphase"; + } + return "?"; +} + +size_t pm3_fit_clocks(const int **clocks) { + if (clocks != NULL) { + *clocks = g_fit_clocks; + } + return ARRAYLEN(g_fit_clocks); +} + +// Apply a single pole antenna response to a template, in place. +static void shape_for_q(double *tpl, size_t len, int q) { + + const double tau = (double)q / M_PI; + if (tau < 1.0) { + return; + } + + const double a = 1.0 / tau; + double y = 0.0; + + for (size_t i = 0; i < len; i++) { + y += a * (tpl[i] - y); + tpl[i] = y; + } +} + +static bool normalise_energy(double *tpl, size_t len) { + + double e = 0.0; + for (size_t i = 0; i < len; i++) { + e += tpl[i] * tpl[i]; + } + + if (e < (1e-6 * (double)len)) { + return false; + } + + const double scale = 1.0 / sqrt(e); + for (size_t i = 0; i < len; i++) { + tpl[i] *= scale; + } + return true; +} + +static bool build_template(const pm3_hyp_t *hyp, double *tpl, size_t len, int variant) { + + memset(tpl, 0, len * sizeof(double)); + + switch (hyp->tpl) { + + case PM3_TPL_FLAT: + for (size_t i = 0; i < len; i++) { + tpl[i] = 1.0; + } + break; + + case PM3_TPL_HALF: + for (size_t i = 0; i < len; i++) { + tpl[i] = (i < (len / 2)) ? 1.0 : -1.0; + } + break; + + case PM3_TPL_PSK: + for (size_t i = 0; i < len; i++) { + tpl[i] = ((i % (size_t)hyp->fc) < ((size_t)hyp->fc / 2)) ? 1.0 : -1.0; + } + break; + + case PM3_TPL_FSK: { + const size_t fc = (size_t)((variant == 0) ? hyp->fc_hi : hyp->fc_lo); + for (size_t i = 0; i < len; i++) { + tpl[i] = ((i % fc) < (fc / 2)) ? 1.0 : -1.0; + } + break; + } + } + + double raw_energy = 0.0; + for (size_t i = 0; i < len; i++) { + raw_energy += tpl[i] * tpl[i]; + } + + shape_for_q(tpl, len, hyp->q); + + double shaped_energy = 0.0; + for (size_t i = 0; i < len; i++) { + shaped_energy += tpl[i] * tpl[i]; + } + + if (raw_energy <= 0.0 || (shaped_energy / raw_energy) < PM3_FIT_MIN_SURVIVAL) { + return false; + } + + return normalise_energy(tpl, len); +} + +// Correlate the whole trace against a single symbol. +static int correlate(const pm3_fft_plan_t *plan, const pm3_cplx_t *sig_fft, + const double *tpl, size_t tpl_len, pm3_cplx_t *scratch, double *out) { + + const size_t n = plan->n; + + memset(scratch, 0, n * sizeof(pm3_cplx_t)); + for (size_t i = 0; i < tpl_len; i++) { + scratch[i].re = tpl[i]; + } + + pm3_fft(plan, scratch, false); + + for (size_t i = 0; i < n; i++) { + const double xr = sig_fft[i].re, xi = sig_fft[i].im; + const double tr = scratch[i].re, ti = -scratch[i].im; // conjugate + scratch[i].re = (xr * tr) - (xi * ti); + scratch[i].im = (xr * ti) + (xi * tr); + } + + pm3_fft(plan, scratch, true); + + for (size_t i = 0; i < n; i++) { + out[i] = scratch[i].re; + } + return PM3_SUCCESS; +} + +static double eye_mean(const double *work, size_t limit, double clk, double phase, size_t *count) { + + const double half = clk / 2.0; + double sum = 0.0; + size_t cnt = 0; + + for (double pos = phase; (pos + half) < (double)limit; pos += clk) { + sum += work[(size_t)(pos + 0.5)] - work[(size_t)(pos + half + 0.5)]; + cnt++; + } + + if (count != NULL) { + *count = cnt; + } + if (cnt == 0) { + return -1e30; + } + return sum / (double)cnt; +} + +static bool score_hypothesis(pm3_hyp_t *hyp, const double *stat_in, const double *aux, const double *sgn, + size_t valid, double *work, double *agc) { + + const double *stat = stat_in; + + const size_t clk = (size_t)hyp->clk; + if (clk < 2 || valid < clk * PM3_FIT_MIN_SYMBOLS) { + return false; + } + + // Flatten slow amplitude variation, for timing recovery only. + { + size_t win = clk * PM3_FIT_AGC_SYMBOLS; + if (win < 8) { + win = 8; + } + if (win > valid) { + win = valid; + } + + double run = 0.0; + size_t lo = 0, hi = 0; + while (hi < win) { + run += stat[hi++]; + } + + for (size_t i = 0; i < valid; i++) { + + const size_t want_lo = (i > win / 2) ? i - (win / 2) : 0; + size_t want_hi = want_lo + win; + if (want_hi > valid) { + want_hi = valid; + } + + while (hi < want_hi) { + run += stat[hi++]; + } + while (lo < want_lo) { + run -= stat[lo++]; + } + + const size_t cnt = hi - lo; + const double mean = (cnt > 0) ? (run / (double)cnt) : 0.0; + agc[i] = (mean > 1e-12) ? (stat[i] / mean) : 0.0; + } + } + + // standardise the flattened copy, this is what the timing search sees + { + double sum = 0.0, sumsq = 0.0; + for (size_t i = 0; i < valid; i++) { + sum += agc[i]; + sumsq += agc[i] * agc[i]; + } + const double mean = sum / (double)valid; + double var = (sumsq / (double)valid) - (mean * mean); + if (var < 1e-30) { + return false; + } + const double sd = sqrt(var); + for (size_t i = 0; i < valid; i++) { + agc[i] = (agc[i] - mean) / sd; + } + } + + // standardise the untouched statistic + double sum = 0.0, sumsq = 0.0; + for (size_t i = 0; i < valid; i++) { + sum += stat[i]; + sumsq += stat[i] * stat[i]; + } + + const double gmean = sum / (double)valid; + double gvar = (sumsq / (double)valid) - (gmean * gmean); + if (gvar < 1e-30) { + return false; + } + const double gsd = sqrt(gvar); + + for (size_t i = 0; i < valid; i++) { + work[i] = (stat[i] - gmean) / gsd; + } + + // Stage 1 - find the phase over a short prefix. + size_t prefix = clk * 32; + if (prefix > valid) { + prefix = valid; + } + + double best_phase = 0.0; + double best_mean = -1e30; + + for (size_t phase = 0; phase < clk; phase++) { + const double m = eye_mean(agc, prefix, (double)clk, (double)phase, NULL); + if (m > best_mean) { + best_mean = m; + best_phase = (double)phase; + } + } + + // Stage 2 - refine the clock as a fraction, over the full length. + double best_clk = (double)clk; + double full_best = eye_mean(agc, valid, best_clk, best_phase, NULL); + + for (int step = -30; step <= 30; step++) { + + const double c = (double)clk + ((double)step * 0.02); + if (c < 2.0) { + continue; + } + const double m = eye_mean(agc, valid, c, best_phase, NULL); + if (m > full_best) { + full_best = m; + best_clk = c; + } + } + + // Stage 3 - re-fit the phase now that the clock is right + best_mean = -1e30; + for (size_t phase = 0; phase < clk; phase++) { + const double m = eye_mean(agc, valid, best_clk, (double)phase, NULL); + if (m > best_mean) { + best_mean = m; + best_phase = (double)phase; + } + } + + if (best_mean < PM3_FIT_MIN_CONTRAST) { + return false; + } + + // Timing is settled; score the decision instants themselves. + const double half = best_clk / 2.0; + + // See the chip rate test after this loop. + size_t pair_diff[2] = { 0, 0 }; + size_t pair_tot[2] = { 0, 0 }; + bool prev_bit = false; + + double zsum = 0.0, zsumsq = 0.0; + double rsum = 0.0, rlow = 0.0, mid_sum = 0.0; + size_t cnt = 0; + + for (double pos = best_phase; (pos + half) < (double)valid; pos += best_clk) { + + const size_t idx = (size_t)(pos + 0.5); + const size_t mid = (size_t)(pos + half + 0.5); + + const double z = work[idx]; + zsum += z; + zsumsq += z * z; + + rsum += stat[idx]; + mid_sum += stat[mid]; + if (cnt == 0 || stat[idx] < rlow) { + rlow = stat[idx]; + } + + // running tally for the chip rate test below + const bool bit = (sgn[idx] >= 0.0); + if (cnt > 0) { + const size_t align = (cnt - 1) & 1; + pair_tot[align]++; + if (bit != prev_bit) { + pair_diff[align]++; + } + } + prev_bit = bit; + + cnt++; + } + + if (cnt < PM3_FIT_MIN_SYMBOLS) { + return false; + } + + const double zmean = zsum / (double)cnt; + double zvar = (zsumsq / (double)cnt) - (zmean * zmean); + + // Floor the variance. + if (zvar < PM3_FIT_VAR_FLOOR) { + zvar = PM3_FIT_VAR_FLOOR; + } + + const double rmean = rsum / (double)cnt; + + // Chip rate test. + hyp->chip_rate = false; + for (size_t align = 0; align < 2; align++) { + if (pair_tot[align] < 16) { + continue; + } + const double frac = (double)pair_diff[align] / (double)pair_tot[align]; + if (frac > PM3_FIT_CHIP_PAIR) { + hyp->chip_rate = true; + } + } + + hyp->phase = (int)best_phase; + hyp->clk_fine = best_clk; + hyp->nsym = cnt; + hyp->snr_dd = 10.0 * log10((zmean * zmean) / zvar); + hyp->eye = (rmean > 0.0) ? (rlow / rmean) : 0.0; + hyp->mid = mid_sum / (double)cnt; + hyp->mid_ratio = (rmean > 0.0) ? (hyp->mid / rmean) : 0.0; + hyp->margin = 0.0; + + const double mid_limit = (hyp->tpl == PM3_TPL_FSK) ? PM3_FIT_MAX_MID_FSK : PM3_FIT_MAX_MID; + if (hyp->mid_ratio > mid_limit) { + return false; + } + + // PSK only: the subcarrier has to actually be in the trace. + if (hyp->tpl == PM3_TPL_PSK && rmean < (PM3_FIT_MIN_TONE_ENERGY * sqrt(best_clk))) { + return false; + } + + // FSK only: the two tone correlations have to actually separate. + if (aux != NULL) { + + double asum = 0.0; + size_t acnt = 0; + + for (double pos = best_phase; (pos + half) < (double)valid; pos += best_clk) { + asum += aux[(size_t)(pos + 0.5)]; + acnt++; + } + + if (acnt == 0) { + return false; + } + const double amean = asum / (double)acnt; + if (amean <= 0.0 || (rmean / amean) < PM3_FIT_MIN_TONE_SPLIT) { + return false; + } + + if (amean < (PM3_FIT_MIN_TONE_ENERGY * sqrt(best_clk))) { + return false; + } + + // The winning tone has to actually change. + size_t hi_cnt = 0, lo_cnt = 0; + + for (double pos = best_phase; (pos + half) < (double)valid; pos += best_clk) { + if (sgn[(size_t)(pos + 0.5)] >= 0.0) { + hi_cnt++; + } else { + lo_cnt++; + } + } + + const size_t total = hi_cnt + lo_cnt; + const size_t rarer = (hi_cnt < lo_cnt) ? hi_cnt : lo_cnt; + if (total == 0 || ((double)rarer / (double)total) < PM3_FIT_MIN_TONE_BALANCE) { + return false; + } + } + + return true; +} + +static bool same_decode(const pm3_hyp_t *a, const pm3_hyp_t *b) { + + if (a->clk != b->clk || a->tpl != b->tpl) { + return false; + } + if (a->tpl == PM3_TPL_PSK && a->fc != b->fc) { + return false; + } + if (a->tpl == PM3_TPL_FSK && (a->fc_hi != b->fc_hi || a->fc_lo != b->fc_lo)) { + return false; + } + return true; +} + +static int cmp_hyp(const void *pa, const void *pb) { + + const pm3_hyp_t *a = (const pm3_hyp_t *)pa; + const pm3_hyp_t *b = (const pm3_hyp_t *)pb; + + if (a->snr_dd > b->snr_dd) { + return -1; + } + if (a->snr_dd < b->snr_dd) { + return 1; + } + + if (a->mod != b->mod) { + return (a->mod < b->mod) ? -1 : 1; + } + if (a->enc != b->enc) { + return (a->enc < b->enc) ? -1 : 1; + } + if (a->clk != b->clk) { + return (a->clk < b->clk) ? -1 : 1; + } + if (a->q != b->q) { + return (a->q < b->q) ? -1 : 1; + } + return 0; +} + +static size_t enumerate(const pm3_fit_opts_t *opts, pm3_hyp_t *list) { + + size_t n = 0; + + for (size_t ci = 0; ci < ARRAYLEN(g_fit_clocks); ci++) { + + const int clk = g_fit_clocks[ci]; + if (opts->clk_only && clk != opts->clk_only) { + continue; + } + + for (size_t qi = 0; qi < ARRAYLEN(g_fit_q); qi++) { + + const int q = g_fit_q[qi]; + + pm3_hyp_t h; + + if (opts->mod_mask == 0 || (opts->mod_mask & (1 << PM3_MOD_ASK))) { + for (int enc = PM3_ENC_RAW; enc <= PM3_ENC_BIPHASE; enc++) { + memset(&h, 0, sizeof(h)); + h.mod = PM3_MOD_ASK; + h.enc = (pm3_enc_t)enc; + h.tpl = (enc == PM3_ENC_RAW) ? PM3_TPL_FLAT : PM3_TPL_HALF; + h.clk = clk; + h.q = q; + if (list) { + list[n] = h; + } + n++; + } + } + + if (opts->mod_mask == 0 || (opts->mod_mask & (1 << PM3_MOD_NRZ))) { + memset(&h, 0, sizeof(h)); + h.mod = PM3_MOD_NRZ; + h.enc = PM3_ENC_RAW; + h.tpl = PM3_TPL_FLAT; + h.clk = clk; + h.q = q; + if (list) { + list[n] = h; + } + n++; + } + + if (opts->mod_mask == 0 || (opts->mod_mask & (1 << PM3_MOD_PSK))) { + for (size_t fi = 0; fi < ARRAYLEN(g_psk_fc); fi++) { + if (clk < g_psk_fc[fi] * 4) { + continue; + } + memset(&h, 0, sizeof(h)); + h.mod = PM3_MOD_PSK; + h.enc = PM3_ENC_RAW; + h.tpl = PM3_TPL_PSK; + h.clk = clk; + h.q = q; + h.fc = g_psk_fc[fi]; + if (list) { + list[n] = h; + } + n++; + } + } + + if (opts->mod_mask == 0 || (opts->mod_mask & (1 << PM3_MOD_FSK))) { + for (size_t fi = 0; fi < ARRAYLEN(g_fsk_pairs); fi++) { + + if (clk < g_fsk_pairs[fi][0] * PM3_FIT_FSK_MIN_CYCLES) { + continue; + } + + memset(&h, 0, sizeof(h)); + h.mod = PM3_MOD_FSK; + h.enc = PM3_ENC_RAW; + h.tpl = PM3_TPL_FSK; + h.clk = clk; + h.q = q; + h.fc_hi = g_fsk_pairs[fi][0]; + h.fc_lo = g_fsk_pairs[fi][1]; + if (list) { + list[n] = h; + } + n++; + } + } + } + } + + return n; +} + +int pm3_fit_run(const double *sig, size_t len, const pm3_fit_opts_t *opts, pm3_fit_t *out) { + + if (sig == NULL || opts == NULL || out == NULL || len < 64) { + return PM3_EINVARG; + } + + memset(out, 0, sizeof(pm3_fit_t)); + + const size_t nhyp = enumerate(opts, NULL); + if (nhyp == 0) { + return PM3_EINVARG; + } + + const size_t longest = (size_t)g_fit_clocks[ARRAYLEN(g_fit_clocks) - 1]; + const size_t n = pm3_next_pow2(len + longest); + if (n > PM3_DSP_MAX_FFT) { + return PM3_EINVARG; + } + + pm3_fft_plan_t *plan = pm3_fft_plan_create(n); + pm3_cplx_t *sig_fft = calloc(n, sizeof(pm3_cplx_t)); + pm3_cplx_t *scratch = calloc(n, sizeof(pm3_cplx_t)); + double *corr = calloc(n, sizeof(double)); + double *corr_b = calloc(n, sizeof(double)); + double *stat = calloc(n, sizeof(double)); + double *zwork = calloc(n, sizeof(double)); + double *aux = calloc(n, sizeof(double)); + double *sgn = calloc(n, sizeof(double)); + double *agcbuf = calloc(n, sizeof(double)); + double *tpl = calloc(longest + 1, sizeof(double)); + pm3_hyp_t *list = calloc(nhyp, sizeof(pm3_hyp_t)); + + if (plan == NULL || sig_fft == NULL || scratch == NULL || corr == NULL + || corr_b == NULL || stat == NULL || zwork == NULL || aux == NULL || sgn == NULL + || agcbuf == NULL || tpl == NULL || list == NULL) { + pm3_fft_plan_destroy(plan); + free(sig_fft); + free(scratch); + free(corr); + free(corr_b); + free(stat); + free(zwork); + free(aux); + free(sgn); + free(agcbuf); + free(tpl); + free(list); + return PM3_EMALLOC; + } + + pm3_signal_stats(sig, len, &out->stats); + + enumerate(opts, list); + + // FFT of the trace + for (size_t i = 0; i < len; i++) { + sig_fft[i].re = sig[i]; + } + pm3_fft(plan, sig_fft, false); + + size_t kept = 0; + + for (size_t i = 0; i < nhyp; i++) { + + pm3_hyp_t *h = &list[i]; + const size_t tlen = (size_t)h->clk; + const size_t valid = (len > tlen) ? (len - tlen) : 0; + + if (valid < tlen * PM3_FIT_MIN_SYMBOLS) { + continue; + } + + if (h->tpl == PM3_TPL_FSK) { + + if (build_template(h, tpl, tlen, 0) == false) { + continue; + } + correlate(plan, sig_fft, tpl, tlen, scratch, corr); + + if (build_template(h, tpl, tlen, 1) == false) { + continue; + } + correlate(plan, sig_fft, tpl, tlen, scratch, corr_b); + + for (size_t k = 0; k < valid; k++) { + const double d = fabs(corr[k]) - fabs(corr_b[k]); + stat[k] = fabs(d); + aux[k] = fabs(corr[k]) + fabs(corr_b[k]); + sgn[k] = d; + } + + } else { + + if (build_template(h, tpl, tlen, 0) == false) { + continue; + } + correlate(plan, sig_fft, tpl, tlen, scratch, corr); + + for (size_t k = 0; k < valid; k++) { + stat[k] = fabs(corr[k]); + sgn[k] = corr[k]; + } + } + + const bool is_fsk = (h->tpl == PM3_TPL_FSK); + if (score_hypothesis(h, stat, is_fsk ? aux : NULL, sgn, valid, zwork, agcbuf) == false) { + continue; + } + + list[kept++] = *h; + } + + if (kept == 0) { + pm3_fft_plan_destroy(plan); + free(sig_fft); + free(scratch); + free(corr); + free(corr_b); + free(stat); + free(zwork); + free(aux); + free(sgn); + free(agcbuf); + free(tpl); + free(list); + return PM3_ESOFT; + } + + qsort(list, kept, sizeof(pm3_hyp_t), cmp_hyp); + + // Octave correction. + for (bool moved = true; moved;) { + + moved = false; + + for (size_t j = 1; j < kept; j++) { + + bool candidate = (list[j].tpl == list[0].tpl + && list[j].mod == list[0].mod + && list[j].enc == list[0].enc); + + if (candidate == false + && list[0].tpl == PM3_TPL_FLAT + && list[j].tpl == PM3_TPL_HALF + && (list[0].mod == PM3_MOD_ASK || list[0].mod == PM3_MOD_NRZ) + && list[j].mod == PM3_MOD_ASK) { + candidate = true; + } + + if (candidate == false) { + continue; + } + + if (list[0].tpl != PM3_TPL_FLAT && list[0].tpl != PM3_TPL_HALF) { + break; + } + + if (list[0].chip_rate == false) { + break; + } + + const double ratio = (double)list[j].clk / (double)list[0].clk; + if (fabs(ratio - 2.0) > 0.01) { + continue; + } + + const pm3_hyp_t winner = list[j]; + memmove(&list[1], &list[0], j * sizeof(pm3_hyp_t)); + list[0] = winner; + out->promoted = true; + break; + } + } + + // Structural override. + if (out->stats.valid) { + + const double want_clk = out->stats.transition_coded + ? (2.0 * out->stats.chip) + : out->stats.chip; + + if (want_clk >= PM3_FIT_STRUCT_MIN_CLK) { + + for (size_t j = 0; j < kept; j++) { + + if (list[j].tpl != PM3_TPL_FLAT && list[j].tpl != PM3_TPL_HALF) { + continue; + } + + const bool tpl_ok = out->stats.transition_coded + ? (list[j].tpl == PM3_TPL_HALF) + : (list[j].tpl == PM3_TPL_FLAT); + if (tpl_ok == false) { + continue; + } + + if (fabs((double)list[j].clk - want_clk) > (PM3_FIT_STRUCT_TOL * want_clk)) { + continue; + } + + if (j > 0) { + const pm3_hyp_t winner = list[j]; + memmove(&list[1], &list[0], j * sizeof(pm3_hyp_t)); + list[0] = winner; + out->promoted = true; + } + break; + } + } + } + + for (size_t i = 0; i < kept; i++) { + list[i].margin = 0.0; + for (size_t j = i + 1; j < kept; j++) { + if (same_decode(&list[i], &list[j]) == false) { + list[i].margin = list[i].snr_dd - list[j].snr_dd; + break; + } + } + } + + if (opts->keep_corr) { + + const pm3_hyp_t *best = &list[0]; + const size_t tlen = (size_t)best->clk; + const size_t valid = len - tlen; + + if (best->tpl == PM3_TPL_FSK) { + build_template(best, tpl, tlen, 0); + correlate(plan, sig_fft, tpl, tlen, scratch, corr); + build_template(best, tpl, tlen, 1); + correlate(plan, sig_fft, tpl, tlen, scratch, corr_b); + for (size_t k = 0; k < valid; k++) { + stat[k] = fabs(fabs(corr[k]) - fabs(corr_b[k])); + } + } else { + build_template(best, tpl, tlen, 0); + correlate(plan, sig_fft, tpl, tlen, scratch, corr); + for (size_t k = 0; k < valid; k++) { + stat[k] = fabs(corr[k]); + sgn[k] = corr[k]; + } + } + + out->corr = calloc(valid, sizeof(double)); + if (out->corr != NULL) { + memcpy(out->corr, stat, valid * sizeof(double)); + out->corr_len = valid; + } + } + + out->items = list; + out->count = kept; + + pm3_fft_plan_destroy(plan); + free(sig_fft); + free(scratch); + free(corr); + free(corr_b); + free(stat); + free(zwork); + free(aux); + free(sgn); + free(agcbuf); + free(tpl); + return PM3_SUCCESS; +} + +void pm3_fit_free(pm3_fit_t *fit) { + if (fit == NULL) { + return; + } + free(fit->items); + free(fit->corr); + memset(fit, 0, sizeof(pm3_fit_t)); +} + +//----------------------------------------------------------------------------- +// Synthetic signal generator +//----------------------------------------------------------------------------- +static uint32_t rng_next(uint32_t *state) { + uint32_t x = *state; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + *state = x; + return x; +} + +static double rng_uniform(uint32_t *state) { + const uint32_t v = rng_next(state); + return (double)v / 4294967296.0; +} + +static double rng_gauss(uint32_t *state) { + double u1 = rng_uniform(state); + const double u2 = rng_uniform(state); + if (u1 < 1e-12) { + u1 = 1e-12; + } + return sqrt(-2.0 * log(u1)) * cos(2.0 * M_PI * u2); +} + +int pm3_fit_generate(const pm3_gen_opts_t *opts, double *out, size_t len) { + + if (opts == NULL || out == NULL || len < 64 || opts->clk < 2.0) { + return PM3_EINVARG; + } + + uint32_t state = opts->seed ? opts->seed : 1; + + const size_t nbits = (size_t)((double)len / opts->clk) + 2; + uint8_t *bits = calloc(nbits, sizeof(uint8_t)); + if (bits == NULL) { + return PM3_EMALLOC; + } + for (size_t i = 0; i < nbits; i++) { + bits[i] = rng_next(&state) & 1; + } + + if (opts->repeat > 0) { + for (size_t i = (size_t)opts->repeat; i < nbits; i++) { + bits[i] = bits[i % (size_t)opts->repeat]; + } + } + + double *edge = calloc(nbits + 1, sizeof(double)); + if (edge == NULL) { + free(bits); + return PM3_EMALLOC; + } + for (size_t i = 0; i <= nbits; i++) { + edge[i] = ((double)i * opts->clk) + (opts->jitter ? (rng_gauss(&state) * opts->jitter) : 0.0); + } + + int8_t *first = calloc(nbits, sizeof(int8_t)); + int8_t *second = calloc(nbits, sizeof(int8_t)); + if (first == NULL || second == NULL) { + free(bits); + free(edge); + free(first); + free(second); + return PM3_EMALLOC; + } + + { + int8_t level = 1; + for (size_t i = 0; i < nbits; i++) { + level = (int8_t)(-level); // boundary transition, always + first[i] = level; + if (bits[i]) { + level = (int8_t)(-level); // mid symbol transition, on a one + } + second[i] = level; + } + } + + for (size_t i = 0; i < len; i++) { + + size_t sym = (size_t)((double)i / opts->clk); + if (sym >= nbits) { + sym = nbits - 1; + } + while (sym + 1 < nbits && (double)i >= edge[sym + 1]) { + sym++; + } + while (sym > 0 && (double)i < edge[sym]) { + sym--; + } + + const double span = edge[sym + 1] - edge[sym]; + const double frac = (span > 0.0) ? (((double)i - edge[sym]) / span) : 0.0; + const int bit = bits[sym]; + + double sym_level; + + switch (opts->enc) { + case PM3_ENC_MANCHESTER: { + sym_level = ((frac < 0.5) == (bit != 0)) ? 1.0 : -1.0; + break; + } + case PM3_ENC_BIPHASE: { + sym_level = (frac < 0.5) ? (double)first[sym] : (double)second[sym]; + break; + } + case PM3_ENC_RAW: + default: { + sym_level = bit ? 1.0 : -1.0; + break; + } + } + + double v; + + switch (opts->mod) { + case PM3_MOD_FSK: { + const int fc = (sym_level > 0.0) ? opts->fc_hi : opts->fc_lo; + v = sin(2.0 * M_PI * (double)i / (double)(fc ? fc : 8)); + break; + } + case PM3_MOD_PSK: { + const int fc = opts->fc ? opts->fc : 4; + v = sym_level * sin(2.0 * M_PI * (double)i / (double)fc); + break; + } + case PM3_MOD_ASK: + case PM3_MOD_NRZ: + default: { + v = sym_level; + break; + } + } + + if (opts->envelope > 0.0) { + v *= 1.0 + (opts->envelope * ((double)i / (double)len - 0.5)); + } + + if (opts->noise > 0.0) { + v += rng_gauss(&state) * opts->noise; + } + + if (opts->drift != 0.0) { + v += opts->drift * ((double)i / (double)len); + } + + if (opts->clip > 0.0) { + if (v > opts->clip) { + v = opts->clip; + } + + if (v < -opts->clip) { + v = -opts->clip; + } + } + + out[i] = v; + } + + free(bits); + free(edge); + free(first); + free(second); + + shape_for_q(out, len, opts->q ? opts->q : 8); + pm3_remove_mean(out, len); + return PM3_SUCCESS; +} diff --git a/client/src/pm3_fit.h b/client/src/pm3_fit.h new file mode 100644 index 000000000..228642355 --- /dev/null +++ b/client/src/pm3_fit.h @@ -0,0 +1,119 @@ +//----------------------------------------------------------------------------- +// 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. +//----------------------------------------------------------------------------- +// Symbol template generation and matched filter hypothesis scoring +//----------------------------------------------------------------------------- + +#ifndef PM3_FIT_H__ +#define PM3_FIT_H__ + +#include "common.h" +#include "pm3_dsp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define PM3_FIT_MIN_SYMBOLS 64 + +typedef enum { + PM3_MOD_ASK = 0, + PM3_MOD_FSK, + PM3_MOD_PSK, + PM3_MOD_NRZ, +} pm3_mod_t; + +typedef enum { + PM3_ENC_RAW = 0, + PM3_ENC_MANCHESTER, + PM3_ENC_BIPHASE, +} pm3_enc_t; + +typedef enum { + PM3_TPL_FLAT = 0, // one level held for the whole symbol + PM3_TPL_HALF, // half high, half low - a mid symbol transition + PM3_TPL_PSK, // subcarrier burst + PM3_TPL_FSK, // one of two tones, scored non coherently +} pm3_tpl_t; + +typedef struct { + + pm3_mod_t mod; + pm3_enc_t enc; + pm3_tpl_t tpl; + + int clk; // samples per symbol + int q; // assumed antenna Q used to shape the template + int fc_hi; // FSK, the longer field clock ( pm3 `fchigh` ) + int fc_lo; // FSK, the shorter field clock ( pm3 `fclow` ) + int fc; // PSK subcarrier period + + // results + int phase; // best symbol phase, 0 .. clk-1 + double clk_fine; // refined clock, fractional - this is the useful one + size_t nsym; // decision instants the score was built from + double snr_dd; // decision directed SNR, dB + double eye; // min / mean at the decision instants, 0 .. 1 + double mid; // mean at the mid symbol instants + double mid_ratio; // mid / mean at the decision instants + bool chip_rate; // decoded bits pair up 01/10 almost always, so + double margin; // dB to the next best genuinely different decode +} pm3_hyp_t; + +typedef struct { + int mod_mask; // bitmask of (1 << pm3_mod_t), 0 means all + int clk_only; // restrict to this clock, 0 means all candidates + bool keep_corr; // hand back the rank 1 correlator output +} pm3_fit_opts_t; + +typedef struct { + pm3_hyp_t *items; // ranked, best first + size_t count; + pm3_sigstat_t stats; // structural facts measured from the waveform + bool promoted; // rank 1 was chosen on structural evidence rather + double *corr; // rank 1 decision statistic, when requested + size_t corr_len; +} pm3_fit_t; + +const char *pm3_mod_name(pm3_mod_t mod); +const char *pm3_enc_name(pm3_enc_t enc); + +size_t pm3_fit_clocks(const int **clocks); +int pm3_fit_run(const double *sig, size_t len, const pm3_fit_opts_t *opts, pm3_fit_t *out); +void pm3_fit_free(pm3_fit_t *fit); + +typedef struct { + pm3_mod_t mod; + pm3_enc_t enc; + double clk; // may be fractional + int fc_hi; + int fc_lo; + int fc; + double noise; + double drift; + double envelope; // amplitude swing across the capture, 0 = none + double jitter; + double clip; // 0 = no clipping + int q; // antenna Q to shape with, 0 for the default + int repeat; // message length in bits, 0 for endless random data. + uint32_t seed; +} pm3_gen_opts_t; + +int pm3_fit_generate(const pm3_gen_opts_t *opts, double *out, size_t len); + +#ifdef __cplusplus +} +#endif +#endif // PM3_FIT_H__