#include "wmbus.h" uint16_t wmbus_crc16(const uint8_t *data, size_t len) { // EN 13757-4: poly 0x3D65, init 0x0000, MSB first, final complement uint16_t crc = 0; for (size_t i = 0; i < len; i++) { crc ^= (uint16_t)data[i] << 8; for (int b = 0; b < 8; b++) { crc = (crc & 0x8000) ? (crc << 1) ^ 0x3D65 : (crc << 1); } } return ~crc; } wmbus_frame_fmt_t wmbus_frame_format(uint8_t b0, uint8_t b1) { if (b0 != WMBUS_MODE_BYTE_A_HI) return WMBUS_FRAME_UNKNOWN; if (b1 == WMBUS_MODE_BYTE_A_LO) return WMBUS_FRAME_A; if (b1 == WMBUS_MODE_BYTE_B_LO) return WMBUS_FRAME_B; return WMBUS_FRAME_UNKNOWN; } int wmbus_frame_total_len(wmbus_frame_fmt_t fmt, uint8_t lfield) { int total; switch (fmt) { case WMBUS_FRAME_A: // L counts C,M,A,CI,data and excludes every CRC byte. Block 1 // carries the L-field plus C,M,A (10 bytes) with its own CRC, so 9 // L-counted bytes. The remaining (L - 9) bytes are split into // blocks of at most 16, each followed by 2 CRC bytes. // // rtl-wmbus uses `2 * ((L - 10 + 15)/16 + 1)`, i.e. floor((L+5)/16)+1 // CRC pairs. That differs from the decomposition above by one pair // whenever L = 10 (mod 16), where it would have to place 17 bytes in // a 16-byte block. wmbusmeters' trimCRCsFrameFormatA walks greedy // 16-byte blocks and accepts a final block of 1..16 bytes, which // matches the form used here. // // Both forms agree on every VALID length observed at this site // (L = 54, 57, 62, 67, 87, 95, 98, 102). No valid L = 10 (mod 16) // frame was ever captured, so this is an internal-consistency fix // found by firmware/test/test_wmbus.c, not a fix for an observed // interoperability failure: with the old formula such a frame would // have been computed 2 bytes short and always failed CRC. if (lfield < 9) return -1; total = 1 + lfield + 2 * (1 + ((lfield - 9 + 15) / 16)); break; case WMBUS_FRAME_B: // L counts everything after itself including CRC bytes. if (lfield < 11) return -1; total = 1 + lfield; break; default: return -1; } if (total > 253) return -1; // must fit SX1262 payload with 2 mode bytes return total; } bool wmbus_check_crc(wmbus_frame_fmt_t fmt, const uint8_t *frame, int total_len) { if (total_len < 12) return false; if (fmt == WMBUS_FRAME_A) { // Block 1: 10 bytes + CRC if (wmbus_crc16(frame, 10) != (((uint16_t)frame[10] << 8) | frame[11])) return false; int pos = 12; while (pos < total_len) { int n = total_len - pos - 2; if (n > 16) n = 16; if (n <= 0) return false; if (wmbus_crc16(frame + pos, n) != (((uint16_t)frame[pos + n] << 8) | frame[pos + n + 1])) return false; pos += n + 2; } return pos == total_len; } if (fmt == WMBUS_FRAME_B) { // Blocks 1+2 share one CRC over their concatenation. Frames longer // than 128 bytes carry further 128-byte chunks, each CRCed over its // first 126 bytes (mirrors rtl-wmbus check_calc_crc_wmbus_b_frame_type). const uint8_t *p = frame; int rem = total_len; while (rem) { if (rem >= 128) { if (wmbus_crc16(p, 126) != (((uint16_t)p[126] << 8) | p[127])) return false; p += 128; rem -= 128; } else { if (rem < 3) return false; if (wmbus_crc16(p, rem - 2) != (((uint16_t)p[rem - 2] << 8) | p[rem - 1])) return false; rem = 0; } } return true; } return false; } uint32_t wmbus_hash(const uint8_t *data, size_t len) { uint32_t h = 2166136261u; for (size_t i = 0; i < len; i++) { h ^= data[i]; h *= 16777619u; } return h; } int wmbus_build_test_frame(uint8_t *out, size_t maxlen, uint8_t access_nr, uint8_t lfield) { // Format A frame: L C M(2) A(6) | CRC | CI ...data... | CRC per block // Manufacturer "SLN" = ((19)<<10)|(12<<5)|(14) = 0x4D8E (Solenos test) // ID 0x00000001 (BCD 00000001), version 0x01, device type 0x07 (water) uint8_t body[256]; if (lfield < 19) lfield = 19; if (lfield > 200) lfield = 200; int L = lfield; body[0] = (uint8_t)L; body[1] = 0x44; // SND_NR body[2] = 0x8E; body[3] = 0x4D; // manufacturer, little endian body[4] = 0x01; body[5] = 0x00; body[6] = 0x00; body[7] = 0x00; // ID BCD LE body[8] = 0x01; // version body[9] = 0x07; // device type body[10] = 0x78; // CI: response, no header // data: marker + access counter + PRBS-ish padding body[11] = 0x53; body[12] = 0x4F; body[13] = 0x4C; body[14] = 0x54; // "SOLT" body[15] = 0x58; // "X" body[16] = access_nr; uint8_t x = 0xA5 ^ access_nr; for (int i = 17; i <= L; i++) { x = (uint8_t)(x * 197 + 31); body[i] = x; } int total = wmbus_frame_total_len(WMBUS_FRAME_A, (uint8_t)L); if (total < 0 || (size_t)total > maxlen) return -1; // block 1: bytes 0..9 + crc for (int i = 0; i < 10; i++) out[i] = body[i]; uint16_t c1 = wmbus_crc16(out, 10); out[10] = c1 >> 8; out[11] = c1 & 0xFF; // blocks 2..n: 16 data bytes + crc each (last block short) int src = 10; // next body byte (CI onward), body holds L+1 bytes int dst = 12; while (src <= L) { int n = L + 1 - src; if (n > 16) n = 16; for (int i = 0; i < n; i++) out[dst + i] = body[src + i]; uint16_t c = wmbus_crc16(out + dst, n); out[dst + n] = c >> 8; out[dst + n + 1] = c & 0xFF; src += n; dst += n + 2; } return total; }