/* LoRa demodulator, release 2022-04-28 * Copyright (C) 2022 Daniel Beer * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above * copyright notice and this permission notice appear in all copies. * * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. * * Compile with: * * gcc -O1 -Wall -std=gnu99 -o lora lora.c -lm * * Example usage (SF 7, 125 kHz bandwidth): * * rtl_sdr -g 30 -f 915000000 -s 1000000 | ./lora -b 8 -s 7 */ #include #include #include #include #include #include #include #include #include #include #ifndef ARRAY_SIZE #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) #endif #ifdef DEBUG_DEMOD #define DBG(...) printf(__VA_ARGS__) #else #define DBG(...) #endif struct header { unsigned int cr; unsigned int pktlen; unsigned int crc; }; /* Command-line parameters */ static struct header implicit_header = { .cr = 4 }; static unsigned int DECIMATION = 1; static unsigned int SF = 7; static unsigned int LDRO; static unsigned int IMPLICIT; /************************************************************************ * FFT and related utilities */ static unsigned int N; static float complex roots[12]; static float complex *dft_core(float complex *in, float complex *out) { /* Initial state: we have N DFTs, each of length 1 */ unsigned int count = N; unsigned int length = 1; for (int i = 0; i < SF; i++) { const unsigned int next_count = count >> 1; const unsigned int next_length = length << 1; const float complex factor = conjf(roots[i]); for (unsigned int i = 0; i < next_count; i++) { float complex *out_low = out + i * next_length; float complex *out_high = out_low + length; const float complex *in_even = in + i * length; const float complex *in_odd = in_even + (N >> 1); float complex fj = 1.0f; for (unsigned int j = 0; j < length; j++) { const float complex e = in_even[j]; const float complex o = in_odd[j]; out_low[j] = e + fj * o; out_high[j] = e - fj * o; fj *= factor; } } float complex *tmp = in; in = out; out = tmp; count = next_count; length = next_length; } return in; } static float complex frac_dz(const float complex *s, int k) { /* Cedron Dawg, 13 April 2017, Three Bin Exact Frequency * Formulas for a Pure Complex Tone in a DFT * https://www.dsprelated.com/showarticle/1043.php */ const float complex invr = roots[SF - 1]; const float complex r = conjf(invr); const float complex z0 = s[(k + N - 1) % N]; const float complex z1 = s[k]; const float complex z2 = s[(k + 1) % N]; const float complex g0 = z0 + z0 * invr; const float complex g1 = z1; const float complex g2 = z2 + z2 * r; const float complex gmean = (g0 + g1 + g2) / 3; const float complex k0 = g0 - gmean; const float complex k1 = g1 - gmean; const float complex k2 = g2 - gmean; return (k0*z0 + k1*z1 + k2*z2) / (k0*z0*invr + k1*z1 + k2*z2*r); } /************************************************************************ * Frame decoder */ /* SNR estimate */ static float snr_signal; static float snr_noise; /* Sync symbols */ static uint8_t sync_word; /* Symbol buffer */ static unsigned int sym_count; static unsigned int sym_data[1024]; /* Precomputed bit errors for a (7, 4) Hamming code */ static const uint8_t hamming[] = { 0x00, 0x01, 0x02, 0x20, 0x04, 0x08, 0x40, 0x10, 0x08, 0x04, 0x10, 0x40, 0x01, 0x00, 0x20, 0x02, 0x10, 0x40, 0x08, 0x04, 0x20, 0x02, 0x01, 0x00, 0x02, 0x20, 0x00, 0x01, 0x40, 0x10, 0x04, 0x08, 0x20, 0x02, 0x01, 0x00, 0x10, 0x40, 0x08, 0x04, 0x40, 0x10, 0x04, 0x08, 0x02, 0x20, 0x00, 0x01, 0x04, 0x08, 0x40, 0x10, 0x00, 0x01, 0x02, 0x20, 0x01, 0x00, 0x20, 0x02, 0x08, 0x04, 0x10, 0x40, 0x40, 0x10, 0x04, 0x08, 0x02, 0x20, 0x00, 0x01, 0x20, 0x02, 0x01, 0x00, 0x10, 0x40, 0x08, 0x04, 0x01, 0x00, 0x20, 0x02, 0x08, 0x04, 0x10, 0x40, 0x04, 0x08, 0x40, 0x10, 0x00, 0x01, 0x02, 0x20, 0x08, 0x04, 0x10, 0x40, 0x01, 0x00, 0x20, 0x02, 0x00, 0x01, 0x02, 0x20, 0x04, 0x08, 0x40, 0x10, 0x02, 0x20, 0x00, 0x01, 0x40, 0x10, 0x04, 0x08, 0x10, 0x40, 0x08, 0x04, 0x20, 0x02, 0x01, 0x00, }; /* Parity of an 8-bit value */ static const uint8_t parity[] = { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, }; static void symbol(unsigned int s) { DBG("[SYMBOL %d] ", s); if (sym_count + 1 < sizeof(sym_data)) sym_data[sym_count++] = s; } struct blkstat { unsigned int bits; unsigned int errors; }; static unsigned int process_block(const unsigned int *syms, unsigned int len, unsigned int ldro, uint8_t *bytes, struct blkstat *st) { unsigned int cooked[8]; unsigned int bps = SF; /* Un-grey symbols */ memcpy(cooked, syms, len * sizeof(cooked[0])); if (ldro) { bps -= 2; for (unsigned int i = 0; i < len; i++) cooked[i] = ((cooked[i] + 2) >> 2) & ((N >> 2) - 1); } for (unsigned int i = 0; i < len; i++) cooked[i] ^= cooked[i] >> 1; /* Deinterleave */ memset(bytes, 0, bps); for (int i = len - 1; i >= 0; i--) { unsigned int rot = cooked[i]; rot |= rot << bps; rot >>= (bps - i % bps); for (unsigned int j = 0; j < bps; j++) { bytes[j] <<= 1; bytes[j] |= rot & 1; rot >>= 1; } } /* Decode */ for (unsigned int i = 0; i < bps; i++) { uint8_t x = bytes[i]; switch (len) { case 8: x ^= hamming[x & 0x7f]; if (x != bytes[i]) st->errors++; if (parity[x]) st->errors++; break; case 7: if (hamming[x]) st->errors++; bytes[i] ^= hamming[x]; break; case 6: st->errors += parity[x & 0x17] | parity[x & 0x2e]; break; case 5: st->errors += parity[x]; break; } } #ifdef DEBUG_CODE printf("Block: len=%d ldro=%d:", len, ldro); for (unsigned int i = 0; i < len; i++) printf(" %03x", cooked[i]); printf(" =>"); for (unsigned int i = 0; i < bps; i++) printf(" %02x", bytes[i]); printf("\n"); #endif st->bits += len * bps; return bps; } static uint16_t crc16(uint16_t c, uint8_t x) { c ^= ((uint16_t)x) << 8; for (int i = 0; i < 8; i++) { if (c & 0x8000) c = (c << 1) ^ 0x1021; else c <<= 1; } return c; } static void flush(void) { if (!sym_count) return; printf("Sync: %02x\n", sync_word); printf("SNR: %4.01f dB\n", logf(snr_signal * N / snr_noise) / M_LN10 * 10.0f); #ifdef DEBUG_CODE printf("Symbols (%d):", sym_count); for (unsigned int i = 0; i < sym_count; i++) printf(" %03x", sym_data[i]); printf("\n"); #endif if (sym_count < 8) { printf("Truncated\n"); goto done; } /* Worst case for space is that we get 12 bits per symbol at a * coding rate of 4/5. * * The first block is special and is always processed with a 4/8 * coding rate and LDRO. */ struct blkstat st = {0}; uint8_t bytes[(ARRAY_SIZE(sym_data) + 4) * 48 / 5]; unsigned int n = process_block(sym_data, 8, 1, bytes, &st); struct header h; if (IMPLICIT) { memcpy(&h, &implicit_header, sizeof(h)); } else { static const uint32_t pchk[] = { 0xf0010, 0x8e108, 0x49a04, 0x25702, 0x12f01 }; uint32_t hdr = 0; for (int i = 0; i < 5; i++) hdr = (hdr << 4) | (bytes[i] & 0xf); for (int i = 0; i < ARRAY_SIZE(pchk); i++) { const uint32_t p = hdr & pchk[i]; if (parity[(p ^ (p >> 8) ^ (p >> 16)) & 0xff]) { printf("Bad header checksum\n"); goto done; } } h.pktlen = hdr >> 12; h.crc = (hdr >> 8) & 1; h.cr = ((hdr >> 9) & 7) + 4; if (h.cr > 8) { printf("Invalid coding rate\n"); goto done; } memmove(bytes, bytes + 5, n - 5); n -= 5; } for (unsigned int i = 8; i + h.cr <= sym_count; i += h.cr) n += process_block(sym_data + i, h.cr, LDRO, bytes + n, &st); printf("BER: %d/%d\n", st.errors, st.bits); /* Collect nibbles */ for (unsigned int i = 0; i + 1 < n; i += 2) bytes[i >> 1] = (bytes[i + 1] << 4) | (bytes[i] & 0xf); n >>= 1; /* Chop to correct length */ const unsigned int expectlen = h.pktlen + (h.crc ? 2 : 0); if (n < expectlen) { printf("Truncated\n"); goto done; } /* Dewhiten payload */ uint8_t lfsr = 0xff; for (unsigned int i = 0; i < h.pktlen; i++) { bytes[i] ^= lfsr; lfsr = (lfsr << 1) | parity[lfsr & 0xb8]; } /* Verify CRC if present */ if (h.crc) { uint16_t crc = 0; for (unsigned int i = 0; i + 2 < h.pktlen; i++) crc = crc16(crc, bytes[i]); crc ^= ((uint16_t)(bytes[h.pktlen + 1]) << 8); crc ^= bytes[h.pktlen]; if (h.pktlen) crc ^= bytes[h.pktlen - 1]; if (h.pktlen > 1) crc ^= ((uint16_t)bytes[h.pktlen - 2]) << 8; printf("CRC: %s\n", crc ? "BAD" : "ok"); } printf("Bytes (%d):", h.pktlen); for (unsigned int i = 0; i < h.pktlen; i++) printf(" %02x", bytes[i]); printf("\n"); done: printf("\n"); sym_count = 0; } /************************************************************************ * Demodulator */ typedef enum { STATE_IDLE = 0, STATE_PREAMBLE, STATE_LOCKED, STATE_PAYLOAD } state_t; static float complex *downchirp; static state_t state; static int timeout_counter; static uint8_t *rx_iq; static float complex *rx_z; static unsigned int rx_len; static float complex *work; static float complex fine_adjust = 1.0f; static int last_up[4]; static int symbol_offset; /* SNR estimate */ static float snr_frame_signal; static float snr_frame_noise; static int argmax_f(int adjust) { /* Apply fine frequency adjustment */ float complex z = 1.0f; for (int i = 0; i < N; i++) { work[i] *= z; z *= fine_adjust; } /* DFT and find peak bin */ const float complex *s = dft_core(work, work + N); int k = 0; float kmag = crealf(s[0] * conjf(s[0])); float sum = kmag; for (int i = 1; i < N; i++) { const float mag = crealf(s[i] * conjf(s[i])); sum += mag; if (mag > kmag) { k = i; kmag = mag; } } snr_frame_signal = kmag; snr_frame_noise = sum; /* Peak insufficiently high? Maybe just noise */ if (kmag * N <= sum * 20.0f) { DBG("[no peak] "); return -1; } DBG("[peak %4d, mag %7.02f, phase %7.02f", k, kmag * N / sum, cimagf(clogf(s[k])) / 2 / M_PI); /* Compensate for fractional carrier frequency offset, which * spreads peaks among bins. * * We need to be careful to only apply this once per frame. */ if (adjust) { const float complex a = frac_dz(s, k); DBG(", adjust %7.03f", cimagf(clogf(a)) * N / 2 / M_PI); /* Adjust the frequency in the direction of the * fractional deviation, but not too fast (to avoid * oscillation on noisy signals). This calculation is an * approximation of: * * fine_adjust *= conj(a)**0.1 * * The gain was chosen by experimentation with captured * data. * * fine_adjust is then used on the next frame as a * frequency adjustment (sample i is multiplied by * fine_adjust**i). Despite the name, the "fine" * adjustment can actually accumulate beyond a whole * frequency bin -- this allows us to track and * compensate for timing errors (SFO offset) and/or * carrier frequency drift. */ fine_adjust *= 1.0f + cimagf(a) * -0.1f * I; fine_adjust /= sqrtf(fine_adjust * conjf(fine_adjust)); } DBG("] "); return k; } static void rx(const uint8_t *data, unsigned int len) { while (len) { unsigned int r = N * 2 * DECIMATION - rx_len; if (r > len) r = len; memcpy(rx_iq + rx_len, data, r); data += r; len -= r; rx_len += r; if (rx_len < N * 2 * DECIMATION) continue; rx_len = 0; /* Convert samples */ for (unsigned int i = 0; i < N; i++) { const unsigned int j = i * DECIMATION * 2; rx_z[i] = (rx_iq[j] - 127) + (rx_iq[j + 1] - 127) * I; } /* Dechirp and identify peaks */ for (unsigned int i = 0; i < N; i++) work[i] = rx_z[i] * downchirp[i]; const int upk = argmax_f(1); /* Process based on state */ if (upk >= 0) { timeout_counter = 0; memmove(last_up + 1, last_up, sizeof(last_up) - sizeof(last_up[0])); last_up[0] = upk; switch (state) { case STATE_IDLE: for (int i = 1; i < ARRAY_SIZE(last_up); i++) last_up[i] = upk; state = STATE_PREAMBLE; snr_signal = snr_noise = 0.0f; DBG("[preamble detected] "); break; case STATE_LOCKED: state = STATE_PAYLOAD; DBG("[payload] "); case STATE_PAYLOAD: symbol((upk + symbol_offset) % N); case STATE_PREAMBLE: break; } goto done; } /* No upchirp? Time out if missing for too long */ if (state == STATE_PAYLOAD) DBG("[SYMBOL ?] "); if (++timeout_counter >= 4) { if (state != STATE_IDLE) DBG("[timeout] "); state = STATE_IDLE; fine_adjust = 1.0f; goto done; } /* Preamble only: look for a downchirp to compute coarse * CFO and STO. * * The method of estimating integer CFO and STO is * described in: * * Bernier, C., Dehmas, F., & Deparis, N. (2020). Low * complexity LoRa frame synchronization for ultra-low * power software-defined radios. IEEE Transactions on * Communications, 68(5), 3140-3152. */ if (state != STATE_PREAMBLE) goto done; for (unsigned int i = 0; i < N; i++) work[i] = rx_z[i] * conjf(downchirp[i]); const int downk = argmax_f(upk < 0); /* Process down-chirp */ if (state == STATE_PREAMBLE && downk >= 0) { const int lupk = last_up[ARRAY_SIZE(last_up) - 1]; /* We define CFO as the frequency (in bins) by * which the signal was shifted before * transmission, and STO as the time (in * samples) by which the signal was delayed * before transmission. * * downk = CFO + STO * lupk = CFO - STO * * We can solve for both CFO and STO, but there * is an ambiguity if we allow both values to * take on the full [0, N) range. Instead, we * allow STO to take on the full range, and * assume CFO is limited to [-N/4, N/4). * * The calculation adds downk and lupk to cancel * the STO term, then resolves the ambiguity by * limiting to [-N/4, N/4) (mod N). */ int cfo = (downk + lupk) >> 1; /* [0, N) */ cfo += N / 4; cfo %= N / 2; cfo += N * 3 / 4; cfo %= N; /* Having solved for CFO, subject to our * assumptions, we can now solve for STO. */ const int sto = (downk + N - cfo) % N; for (int i = 0; i < 2; i++) last_up[i] = (((last_up[i] + N * 2 - cfo + sto + 4) % N) >> 3) & 0xf; sync_word = (last_up[1] << 4) | last_up[0]; DBG("[locked cfo=%d sto=%d] ", cfo, sto); state = STATE_LOCKED; symbol_offset = (N - cfo) % N; rx_len = ((N * 7 / 4 - sto) % N) * 2 * DECIMATION; memmove(rx_iq, rx_iq + N * 2 * DECIMATION - rx_len, rx_len); } done: snr_signal += snr_frame_signal; snr_noise += snr_frame_noise; DBG("\n"); if (state == STATE_IDLE) flush(); } } /************************************************************************ * Command-line interface */ static void usage(const char *progname) { printf("Usage: %s [options]\n" "\n" "General options:\n" " --help Show this help text\n" " --version Display program version and exit\n" "\n" "Modulation options:\n" " -b n Ratio of sample rate to chirp bandwidth (integer)\n" " -s factor Spreading factor (7 to 12)\n" " -l Enable LDRO (low-data-rate optimization)\n" "\n" "Implicit header options:\n" " -h len Use an implicit header with fixed-size packets\n" " -r rate Set coding rate for implicit header (4 to 8)\n" " -c Enable CRC for implicit header\n" "\n" "8-bit unsigned I/Q samples will be read from stdin. Demodulated and\n" "decoded packets will be printed on stdout.\n", progname); } static void version(void) { printf( "LoRa demodulator, release 2022-04-28\n" "Copyright (C) 2022 Daniel Beer \n" "\n" "Permission to use, copy, modify, and/or distribute this software for any\n" "purpose with or without fee is hereby granted, provided that the above\n" "copyright notice and this permission notice appear in all copies.\n" "\n" "THE SOFTWARE IS PROVIDED \"AS IS\" AND THE AUTHOR DISCLAIMS ALL WARRANTIES\n" "WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF\n" "MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR\n" "ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES\n" "WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN\n" "ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF\n" "OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.\n"); } static void *xcalloc(size_t nmemb, size_t size) { void *r = calloc(nmemb, size); if (!r) { perror("calloc"); exit(-1); } return r; } static void die(const char *msg) { fprintf(stderr, "%s\n", msg); exit(-1); } int main(int argc, char **argv) { const static struct option longopts[] = { {"help", 0, 0, 'H'}, {"version", 0, 0, 'V'}, {NULL, 0, 0, 0} }; int opt; while ((opt = getopt_long(argc, argv, "b:s:lh:r:c", longopts, NULL)) >= 0) switch (opt) { case 'b': DECIMATION = atoi(optarg); if (DECIMATION < 1) die("Invalid bandwidth ratio"); break; case 's': SF = atoi(optarg); if (SF < 7 || SF > ARRAY_SIZE(roots)) die("Invalid spreading factor"); break; case 'h': IMPLICIT = 1; implicit_header.pktlen = atoi(optarg); if (implicit_header.pktlen > 255) die("Invalid packet length"); break; case 'r': implicit_header.cr = atoi(optarg); if (implicit_header.cr < 4 || implicit_header.cr > 8) die("Invalid coding rate"); break; case 'c': implicit_header.crc = 1; break; case 'l': LDRO = 1; break; case 'H': usage(argv[0]); exit(0); case 'V': version(); exit(0); case '?': die("Try --help for usage information."); return -1; } /* Set up buffers for demodulator */ N = 1 << SF; downchirp = xcalloc(N, sizeof(downchirp[0])); rx_iq = xcalloc(N * 2 * DECIMATION, sizeof(rx_iq[0])); rx_z = xcalloc(N, sizeof(rx_z[0])); work = xcalloc(N * 2, sizeof(work[0])); /* Set up FFT coefficients */ for (int i = 0; i < ARRAY_SIZE(roots); i++) roots[i] = cexpf(M_PI * I / (1 << i)); for (int i = 0; i < N; i++) downchirp[i] = cexpf((i - i*i / (float)N) * M_PI * I); /* Read and demodulate */ setlinebuf(stdout); for (;;) { uint8_t buf[65536]; const int r = read(0, buf, sizeof(buf)); if (r < 0) { if (errno == EAGAIN || errno == EINTR) continue; perror("read"); exit(-1); } if (!r) break; rx(buf, r); } flush(); /* Clean up */ free(downchirp); free(rx_iq); free(rx_z); free(work); return 0; }