Simon
4 months ago
parent
426feb0db0
commit
31fb56d3be
@ -0,0 +1,648 @@
|
||||
#!/usr/bin/env python
|
||||
#
|
||||
# Embedded LC codec helpers for FreeDMR.
|
||||
#
|
||||
# The embedded LC BPTC layout follows MMDVMHost CDMREmbeddedData,
|
||||
# CHamming::encode16114/decode16114, and CCRC::encodeFiveBit.
|
||||
# MMDVMHost is Copyright (C) Jonathan Naylor G4KLX and licensed GPLv2+
|
||||
# (or later). FreeDMR is GPLv3+, so this port is used under GPLv3+.
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from bitarray import bitarray
|
||||
|
||||
|
||||
FULL_LC_BITS = 196
|
||||
FULL_LC_BYTES = 9
|
||||
FULL_LC_PARITY_BYTES = 3
|
||||
FULL_LC_CODE_BYTES = 12
|
||||
FULL_LC_INFO_BITS = 196
|
||||
EMBEDDED_LC_FRAGMENT_BITS = 32
|
||||
EMBEDDED_LC_RAW_BITS = 128
|
||||
EMBEDDED_LC_PAYLOAD_BITS = 72
|
||||
EMBEDDED_LC_PAYLOAD_BYTES = 9
|
||||
SLOT_TYPE_BITS = 20
|
||||
SLOT_TYPE_DATA_BITS = 8
|
||||
LC_FLCO_GROUP_VOICE = 0x00
|
||||
LC_FLCO_UNIT_VOICE = 0x03
|
||||
LC_FID_ETSI = 0x00
|
||||
LC_SERVICE_OPTIONS_NORMAL = 0x00
|
||||
LC_SERVICE_OPTIONS_OVCM = 0x04
|
||||
LC_SERVICE_OPTIONS_HBLINK_LEGACY = 0x20
|
||||
GROUP_VOICE_LC_OPT = b'\x00\x00\x00'
|
||||
UNIT_VOICE_LC_OPT = b'\x03\x00\x00'
|
||||
BS_VOICE_SYNC = bitarray('011101010101111111010111110111110111010111110111')
|
||||
BS_DATA_SYNC = bitarray('110111111111010101111101011101011101111101011101')
|
||||
EMB = {
|
||||
'BURST_B': bitarray('0001001110010001'),
|
||||
'BURST_C': bitarray('0001011101110100'),
|
||||
'BURST_D': bitarray('0001011101110100'),
|
||||
'BURST_E': bitarray('0001010100000111'),
|
||||
'BURST_F': bitarray('0001000111100010'),
|
||||
}
|
||||
SLOT_TYPE = {
|
||||
'PI_HEAD': bitarray('00010000001101100111'),
|
||||
'VOICE_LC_HEAD': bitarray('00010001101110001100'),
|
||||
'VOICE_LC_TERM': bitarray('00010010101001011001'),
|
||||
'CSBK': bitarray('00010011001010110010'),
|
||||
'MBC_HEAD': bitarray('00010100100111110000'),
|
||||
'MBC_CONT': bitarray('00010101000100011011'),
|
||||
'DATA_HEAD': bitarray('00010110000011001110'),
|
||||
'1/2_DATA': bitarray('00010111100000100101'),
|
||||
'3/4_DATA': bitarray('00011000111010100001'),
|
||||
'IDLE': bitarray('00011001011001001010'),
|
||||
'1/1_DATA': bitarray('00011010011110011111'),
|
||||
'RES_1': bitarray('00011011111101110100'),
|
||||
'RES_2': bitarray('00011100010000110110'),
|
||||
'RES_3': bitarray('00011101110011011101'),
|
||||
'RES_4': bitarray('00011110110100001000'),
|
||||
'RES_5': bitarray('00011111010111100011'),
|
||||
}
|
||||
|
||||
FULL_LC_INTERLEAVE_19696 = (
|
||||
0, 181, 166, 151, 136, 121, 106, 91, 76, 61, 46, 31, 16, 1, 182, 167, 152, 137,
|
||||
122, 107, 92, 77, 62, 47, 32, 17, 2, 183, 168, 153, 138, 123, 108, 93, 78, 63,
|
||||
48, 33, 18, 3, 184, 169, 154, 139, 124, 109, 94, 79, 64, 49, 34, 19, 4, 185, 170,
|
||||
155, 140, 125, 110, 95, 80, 65, 50, 35, 20, 5, 186, 171, 156, 141, 126, 111, 96,
|
||||
81, 66, 51, 36, 21, 6, 187, 172, 157, 142, 127, 112, 97, 82, 67, 52, 37, 22, 7,
|
||||
188, 173, 158, 143, 128, 113, 98, 83, 68, 53, 38, 23, 8, 189, 174, 159, 144, 129,
|
||||
114, 99, 84, 69, 54, 39, 24, 9, 190, 175, 160, 145, 130, 115, 100, 85, 70, 55, 40,
|
||||
25, 10, 191, 176, 161, 146, 131, 116, 101, 86, 71, 56, 41, 26, 11, 192, 177, 162,
|
||||
147, 132, 117, 102, 87, 72, 57, 42, 27, 12, 193, 178, 163, 148, 133, 118, 103, 88,
|
||||
73, 58, 43, 28, 13, 194, 179, 164, 149, 134, 119, 104, 89, 74, 59, 44, 29, 14,
|
||||
195, 180, 165, 150, 135, 120, 105, 90, 75, 60, 45, 30, 15,
|
||||
)
|
||||
|
||||
FULL_LC_PAYLOAD_INDEXES = (
|
||||
136, 121, 106, 91, 76, 61, 46, 31,
|
||||
152, 137, 122, 107, 92, 77, 62, 47, 32, 17, 2,
|
||||
123, 108, 93, 78, 63, 48, 33, 18, 3, 184, 169,
|
||||
94, 79, 64, 49, 34, 19, 4, 185, 170, 155, 140,
|
||||
65, 50, 35, 20, 5, 186, 171, 156, 141, 126, 111,
|
||||
36, 21, 6, 187, 172, 157, 142, 127, 112, 97, 82,
|
||||
7, 188, 173, 158, 143, 128, 113, 98, 83,
|
||||
)
|
||||
|
||||
RS129_HEADER_MASK = (0x96, 0x96, 0x96)
|
||||
RS129_TERMINATOR_MASK = (0x99, 0x99, 0x99)
|
||||
RS129_POLY = (64, 56, 14, 1)
|
||||
|
||||
GOLAY_2087_PARITY = (
|
||||
0x0000, 0xB08E, 0xE093, 0x501D, 0x70A9, 0xC027, 0x903A, 0x20B4,
|
||||
0x60DC, 0xD052, 0x804F, 0x30C1, 0x1075, 0xA0FB, 0xF0E6, 0x4068,
|
||||
0x7036, 0xC0B8, 0x90A5, 0x202B, 0x009F, 0xB011, 0xE00C, 0x5082,
|
||||
0x10EA, 0xA064, 0xF079, 0x40F7, 0x6043, 0xD0CD, 0x80D0, 0x305E,
|
||||
0xD06C, 0x60E2, 0x30FF, 0x8071, 0xA0C5, 0x104B, 0x4056, 0xF0D8,
|
||||
0xB0B0, 0x003E, 0x5023, 0xE0AD, 0xC019, 0x7097, 0x208A, 0x9004,
|
||||
0xA05A, 0x10D4, 0x40C9, 0xF047, 0xD0F3, 0x607D, 0x3060, 0x80EE,
|
||||
0xC086, 0x7008, 0x2015, 0x909B, 0xB02F, 0x00A1, 0x50BC, 0xE032,
|
||||
0x90D9, 0x2057, 0x704A, 0xC0C4, 0xE070, 0x50FE, 0x00E3, 0xB06D,
|
||||
0xF005, 0x408B, 0x1096, 0xA018, 0x80AC, 0x3022, 0x603F, 0xD0B1,
|
||||
0xE0EF, 0x5061, 0x007C, 0xB0F2, 0x9046, 0x20C8, 0x70D5, 0xC05B,
|
||||
0x8033, 0x30BD, 0x60A0, 0xD02E, 0xF09A, 0x4014, 0x1009, 0xA087,
|
||||
0x40B5, 0xF03B, 0xA026, 0x10A8, 0x301C, 0x8092, 0xD08F, 0x6001,
|
||||
0x2069, 0x90E7, 0xC0FA, 0x7074, 0x50C0, 0xE04E, 0xB053, 0x00DD,
|
||||
0x3083, 0x800D, 0xD010, 0x609E, 0x402A, 0xF0A4, 0xA0B9, 0x1037,
|
||||
0x505F, 0xE0D1, 0xB0CC, 0x0042, 0x20F6, 0x9078, 0xC065, 0x70EB,
|
||||
0xA03D, 0x10B3, 0x40AE, 0xF020, 0xD094, 0x601A, 0x3007, 0x8089,
|
||||
0xC0E1, 0x706F, 0x2072, 0x90FC, 0xB048, 0x00C6, 0x50DB, 0xE055,
|
||||
0xD00B, 0x6085, 0x3098, 0x8016, 0xA0A2, 0x102C, 0x4031, 0xF0BF,
|
||||
0xB0D7, 0x0059, 0x5044, 0xE0CA, 0xC07E, 0x70F0, 0x20ED, 0x9063,
|
||||
0x7051, 0xC0DF, 0x90C2, 0x204C, 0x00F8, 0xB076, 0xE06B, 0x50E5,
|
||||
0x108D, 0xA003, 0xF01E, 0x4090, 0x6024, 0xD0AA, 0x80B7, 0x3039,
|
||||
0x0067, 0xB0E9, 0xE0F4, 0x507A, 0x70CE, 0xC040, 0x905D, 0x20D3,
|
||||
0x60BB, 0xD035, 0x8028, 0x30A6, 0x1012, 0xA09C, 0xF081, 0x400F,
|
||||
0x30E4, 0x806A, 0xD077, 0x60F9, 0x404D, 0xF0C3, 0xA0DE, 0x1050,
|
||||
0x5038, 0xE0B6, 0xB0AB, 0x0025, 0x2091, 0x901F, 0xC002, 0x708C,
|
||||
0x40D2, 0xF05C, 0xA041, 0x10CF, 0x307B, 0x80F5, 0xD0E8, 0x6066,
|
||||
0x200E, 0x9080, 0xC09D, 0x7013, 0x50A7, 0xE029, 0xB034, 0x00BA,
|
||||
0xE088, 0x5006, 0x001B, 0xB095, 0x9021, 0x20AF, 0x70B2, 0xC03C,
|
||||
0x8054, 0x30DA, 0x60C7, 0xD049, 0xF0FD, 0x4073, 0x106E, 0xA0E0,
|
||||
0x90BE, 0x2030, 0x702D, 0xC0A3, 0xE017, 0x5099, 0x0084, 0xB00A,
|
||||
0xF062, 0x40EC, 0x10F1, 0xA07F, 0x80CB, 0x3045, 0x6058, 0xD0D6,
|
||||
)
|
||||
|
||||
SLOT_TYPE_NAMES = {
|
||||
0x0: "PI_HEAD",
|
||||
0x1: "VOICE_LC_HEAD",
|
||||
0x2: "VOICE_LC_TERM",
|
||||
0x3: "CSBK",
|
||||
0x4: "MBC_HEAD",
|
||||
0x5: "MBC_CONT",
|
||||
0x6: "DATA_HEAD",
|
||||
0x7: "1/2_RATE",
|
||||
0x8: "3/4_RATE",
|
||||
0x9: "IDLE",
|
||||
0xA: "1/1_RATE",
|
||||
0xB: "RES_1",
|
||||
0xC: "RES_2",
|
||||
0xD: "RES_3",
|
||||
0xE: "RES_4",
|
||||
0xF: "RES_5",
|
||||
}
|
||||
EMBEDDED_LC_PAYLOAD_RANGES = (
|
||||
(0, 11),
|
||||
(16, 27),
|
||||
(32, 42),
|
||||
(48, 58),
|
||||
(64, 74),
|
||||
(80, 90),
|
||||
(96, 106),
|
||||
)
|
||||
EMBEDDED_LC_CRC_POSITIONS = (42, 58, 74, 90, 106)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EmbeddedLC:
|
||||
data: bytes
|
||||
flco: int
|
||||
raw: bitarray
|
||||
corrected: int = 0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FullLC:
|
||||
data: bytes
|
||||
flco: int
|
||||
source_id: int
|
||||
target_id: int
|
||||
service_options: int
|
||||
is_group_call: bool
|
||||
is_unit_call: bool
|
||||
|
||||
|
||||
def build_group_voice_lc(
|
||||
dst_id: bytes,
|
||||
src_id: bytes,
|
||||
service_options: int = LC_SERVICE_OPTIONS_NORMAL,
|
||||
) -> bytes:
|
||||
if len(dst_id) != 3 or len(src_id) != 3:
|
||||
raise FullLCError("DMR LC target and source IDs must be three bytes")
|
||||
if service_options < 0 or service_options > 0xFF:
|
||||
raise FullLCError("DMR LC service options must fit in one byte")
|
||||
|
||||
return bytes([LC_FLCO_GROUP_VOICE, LC_FID_ETSI, service_options]) + dst_id + src_id
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SlotType:
|
||||
color_code: int
|
||||
data_type: int
|
||||
name: str
|
||||
corrected: int = 0
|
||||
|
||||
|
||||
class EmbeddedLCError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
class FullLCError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
class SlotTypeError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
def bytes_to_bits(data: bytes) -> bitarray:
|
||||
bits = bitarray(endian="big")
|
||||
bits.frombytes(data)
|
||||
return bits
|
||||
|
||||
|
||||
def bits_to_int(bits: bitarray) -> int:
|
||||
value = 0
|
||||
for bit in bits:
|
||||
value = (value << 1) | int(bit)
|
||||
return value
|
||||
|
||||
|
||||
def _bits_from_int(value: int, length: int) -> bitarray:
|
||||
return bitarray((bool(value & (1 << bit)) for bit in range(length - 1, -1, -1)), endian="big")
|
||||
|
||||
|
||||
def _hamming_distance(a: int, b: int) -> int:
|
||||
return (a ^ b).bit_count()
|
||||
|
||||
|
||||
def _gf256_mul(left: int, right: int) -> int:
|
||||
result = 0
|
||||
while right:
|
||||
if right & 0x01:
|
||||
result ^= left
|
||||
right >>= 1
|
||||
left <<= 1
|
||||
if left & 0x100:
|
||||
left ^= 0x11D
|
||||
return result & 0xFF
|
||||
|
||||
|
||||
def encode_hamming_15113(data: bitarray) -> bitarray:
|
||||
if len(data) != 11:
|
||||
raise FullLCError("Hamming(15,11,3) input must be 11 bits")
|
||||
return bitarray((
|
||||
data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[5] ^ data[7] ^ data[8],
|
||||
data[1] ^ data[2] ^ data[3] ^ data[4] ^ data[6] ^ data[8] ^ data[9],
|
||||
data[2] ^ data[3] ^ data[4] ^ data[5] ^ data[7] ^ data[9] ^ data[10],
|
||||
data[0] ^ data[1] ^ data[2] ^ data[4] ^ data[6] ^ data[7] ^ data[10],
|
||||
), endian="big")
|
||||
|
||||
|
||||
def encode_hamming_1393(data: bitarray) -> bitarray:
|
||||
if len(data) != 9:
|
||||
raise FullLCError("Hamming(13,9,3) input must be 9 bits")
|
||||
return bitarray((
|
||||
data[0] ^ data[1] ^ data[3] ^ data[5] ^ data[6],
|
||||
data[0] ^ data[1] ^ data[2] ^ data[4] ^ data[6] ^ data[7],
|
||||
data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[5] ^ data[7] ^ data[8],
|
||||
data[0] ^ data[2] ^ data[4] ^ data[5] ^ data[8],
|
||||
), endian="big")
|
||||
|
||||
|
||||
def rs129_parity(data: bytes) -> bytes:
|
||||
if len(data) != FULL_LC_BYTES:
|
||||
raise FullLCError("RS(12,9) LC payload must be 9 bytes")
|
||||
|
||||
parity = [0x00, 0x00, 0x00]
|
||||
for byte in data:
|
||||
dbyte = byte ^ parity[2]
|
||||
parity[2] = parity[1] ^ _gf256_mul(RS129_POLY[2], dbyte)
|
||||
parity[1] = parity[0] ^ _gf256_mul(RS129_POLY[1], dbyte)
|
||||
parity[0] = _gf256_mul(RS129_POLY[0], dbyte)
|
||||
return bytes((parity[2], parity[1], parity[0]))
|
||||
|
||||
|
||||
def encode_full_lc_parity(data: bytes, terminator: bool = False) -> bytes:
|
||||
mask = RS129_TERMINATOR_MASK if terminator else RS129_HEADER_MASK
|
||||
return bytes(value ^ mask[index] for index, value in enumerate(rs129_parity(data)))
|
||||
|
||||
|
||||
def _encode_19696(data: bytes) -> bitarray:
|
||||
if len(data) != FULL_LC_CODE_BYTES:
|
||||
raise FullLCError("BPTC(196,96) input must be 12 bytes")
|
||||
|
||||
bits = bytes_to_bits(data)
|
||||
for _index in range(4):
|
||||
bits.insert(0, 0)
|
||||
|
||||
for index in range(9):
|
||||
start = (index * 15) + 1
|
||||
end = start + 11
|
||||
parity = encode_hamming_15113(bits[start:end])
|
||||
for pbit in range(4):
|
||||
bits.insert(end + pbit, parity[pbit])
|
||||
|
||||
for _index in range(60):
|
||||
bits.append(0)
|
||||
|
||||
column = bitarray(9, endian="big")
|
||||
for col in range(15):
|
||||
start = col + 1
|
||||
for index in range(9):
|
||||
column[index] = bits[start]
|
||||
start += 15
|
||||
parity = encode_hamming_1393(column)
|
||||
|
||||
target = 136 + col
|
||||
for pbit in range(4):
|
||||
bits[target] = parity[pbit]
|
||||
target += 15
|
||||
|
||||
return bits
|
||||
|
||||
|
||||
def interleave_19696(data: bitarray) -> bitarray:
|
||||
if len(data) != FULL_LC_BITS:
|
||||
raise FullLCError("BPTC(196,96) data must be 196 bits")
|
||||
interleaved = bitarray(FULL_LC_BITS, endian="big")
|
||||
for index in range(FULL_LC_BITS):
|
||||
interleaved[FULL_LC_INTERLEAVE_19696[index]] = data[index]
|
||||
return interleaved
|
||||
|
||||
|
||||
def encode_full_lc(data: bytes, terminator: bool = False) -> bitarray:
|
||||
if len(data) != FULL_LC_BYTES:
|
||||
raise FullLCError("full LC payload must be 9 bytes")
|
||||
code = data + encode_full_lc_parity(data, terminator=terminator)
|
||||
return interleave_19696(_encode_19696(code))
|
||||
|
||||
|
||||
def encode_header_lc(data: bytes) -> bitarray:
|
||||
return encode_full_lc(data, terminator=False)
|
||||
|
||||
|
||||
def encode_terminator_lc(data: bytes) -> bitarray:
|
||||
return encode_full_lc(data, terminator=True)
|
||||
|
||||
|
||||
def decode_full_lc(bits: bitarray) -> FullLC:
|
||||
if len(bits) != FULL_LC_INFO_BITS:
|
||||
raise FullLCError("full LC data must be 196 bits")
|
||||
payload = bitarray((bits[index] for index in FULL_LC_PAYLOAD_INDEXES), endian="big")
|
||||
data = payload.tobytes()
|
||||
flco = data[0] & 0x3F
|
||||
return FullLC(
|
||||
data=data,
|
||||
flco=flco,
|
||||
source_id=int.from_bytes(data[6:9], "big"),
|
||||
target_id=int.from_bytes(data[3:6], "big"),
|
||||
service_options=data[2],
|
||||
is_group_call=flco == LC_FLCO_GROUP_VOICE,
|
||||
is_unit_call=flco == LC_FLCO_UNIT_VOICE,
|
||||
)
|
||||
|
||||
|
||||
def _padded_bits_to_bytes(bits: bitarray) -> bytes:
|
||||
padded = bits.copy()
|
||||
add_bits = 8 - (len(padded) % 8)
|
||||
if add_bits < 8:
|
||||
for _bit in range(add_bits):
|
||||
padded.insert(0, 0)
|
||||
return padded.tobytes()
|
||||
|
||||
|
||||
def voice_head_term(payload: bytes) -> dict[str, object]:
|
||||
bits = bytes_to_bits(payload)
|
||||
info = bits[0:98] + bits[166:264]
|
||||
slot_type = bits[98:108] + bits[156:166]
|
||||
sync = bits[108:156]
|
||||
lc = decode_full_lc(info).data
|
||||
return {
|
||||
"LC": lc,
|
||||
"CC": _padded_bits_to_bytes(slot_type[0:4]),
|
||||
"DTYPE": _padded_bits_to_bytes(slot_type[4:8]),
|
||||
"SYNC": sync,
|
||||
}
|
||||
|
||||
|
||||
def voice_sync(payload: bytes) -> dict[str, object]:
|
||||
bits = bytes_to_bits(payload)
|
||||
return {
|
||||
"AMBE": [
|
||||
bits[0:72],
|
||||
bits[72:108] + bits[156:192],
|
||||
bits[192:264],
|
||||
],
|
||||
"SYNC": bits[108:156],
|
||||
}
|
||||
|
||||
|
||||
def voice(payload: bytes) -> dict[str, object]:
|
||||
bits = bytes_to_bits(payload)
|
||||
emb = bits[108:116] + bits[148:156]
|
||||
return {
|
||||
"AMBE": [
|
||||
bits[0:72],
|
||||
bits[72:108] + bits[156:192],
|
||||
bits[192:264],
|
||||
],
|
||||
"CC": _padded_bits_to_bytes(emb[0:4]),
|
||||
"LCSS": _padded_bits_to_bytes(emb[5:7]),
|
||||
"EMBED": bits[116:148],
|
||||
}
|
||||
|
||||
|
||||
def encode_slot_type(color_code: int, data_type: int) -> bitarray:
|
||||
if not 0 <= color_code <= 0x0F:
|
||||
raise SlotTypeError("slot color code must fit in four bits")
|
||||
if not 0 <= data_type <= 0x0F:
|
||||
raise SlotTypeError("slot data type must fit in four bits")
|
||||
|
||||
value = (color_code << 4) | data_type
|
||||
checksum = GOLAY_2087_PARITY[value]
|
||||
code = (value << 12) | ((checksum & 0xFF) << 4) | (checksum >> 12)
|
||||
return _bits_from_int(code, SLOT_TYPE_BITS)
|
||||
|
||||
|
||||
def decode_slot_type(bits: bitarray) -> SlotType:
|
||||
if len(bits) != SLOT_TYPE_BITS:
|
||||
raise SlotTypeError("slot type must be 20 bits")
|
||||
|
||||
observed = bits_to_int(bits)
|
||||
candidates = []
|
||||
for value in range(256):
|
||||
encoded = bits_to_int(encode_slot_type(value >> 4, value & 0x0F))
|
||||
candidates.append((_hamming_distance(observed, encoded), value))
|
||||
candidates.sort()
|
||||
|
||||
distance, value = candidates[0]
|
||||
if len(candidates) > 1 and candidates[1][0] == distance:
|
||||
raise SlotTypeError("ambiguous Golay(20,8,7) slot type")
|
||||
if distance > 3:
|
||||
raise SlotTypeError("slot type Golay(20,8,7) check failed")
|
||||
|
||||
data_type = value & 0x0F
|
||||
return SlotType(
|
||||
color_code=value >> 4,
|
||||
data_type=data_type,
|
||||
name=SLOT_TYPE_NAMES[data_type],
|
||||
corrected=distance,
|
||||
)
|
||||
|
||||
|
||||
def crc5(data: bytes | bitarray) -> int:
|
||||
bits = bytes_to_bits(data) if isinstance(data, bytes) else data
|
||||
if len(bits) != EMBEDDED_LC_PAYLOAD_BITS:
|
||||
raise EmbeddedLCError("embedded LC payload must be 72 bits")
|
||||
|
||||
total = 0
|
||||
for offset in range(0, EMBEDDED_LC_PAYLOAD_BITS, 8):
|
||||
total += bits_to_int(bits[offset:offset + 8])
|
||||
return total % 31
|
||||
|
||||
|
||||
def encode_hamming_16114(row: bitarray) -> None:
|
||||
if len(row) != 16:
|
||||
raise EmbeddedLCError("Hamming row must be 16 bits")
|
||||
|
||||
row[11] = row[0] ^ row[1] ^ row[2] ^ row[3] ^ row[5] ^ row[7] ^ row[8]
|
||||
row[12] = row[1] ^ row[2] ^ row[3] ^ row[4] ^ row[6] ^ row[8] ^ row[9]
|
||||
row[13] = row[2] ^ row[3] ^ row[4] ^ row[5] ^ row[7] ^ row[9] ^ row[10]
|
||||
row[14] = row[0] ^ row[1] ^ row[2] ^ row[4] ^ row[6] ^ row[7] ^ row[10]
|
||||
row[15] = row[0] ^ row[2] ^ row[5] ^ row[6] ^ row[8] ^ row[9] ^ row[10]
|
||||
|
||||
|
||||
def decode_hamming_16114(row: bitarray) -> bool:
|
||||
if len(row) != 16:
|
||||
raise EmbeddedLCError("Hamming row must be 16 bits")
|
||||
|
||||
c0 = row[0] ^ row[1] ^ row[2] ^ row[3] ^ row[5] ^ row[7] ^ row[8]
|
||||
c1 = row[1] ^ row[2] ^ row[3] ^ row[4] ^ row[6] ^ row[8] ^ row[9]
|
||||
c2 = row[2] ^ row[3] ^ row[4] ^ row[5] ^ row[7] ^ row[9] ^ row[10]
|
||||
c3 = row[0] ^ row[1] ^ row[2] ^ row[4] ^ row[6] ^ row[7] ^ row[10]
|
||||
c4 = row[0] ^ row[2] ^ row[5] ^ row[6] ^ row[8] ^ row[9] ^ row[10]
|
||||
|
||||
syndrome = 0
|
||||
syndrome |= 0x01 if c0 != row[11] else 0
|
||||
syndrome |= 0x02 if c1 != row[12] else 0
|
||||
syndrome |= 0x04 if c2 != row[13] else 0
|
||||
syndrome |= 0x08 if c3 != row[14] else 0
|
||||
syndrome |= 0x10 if c4 != row[15] else 0
|
||||
|
||||
corrections = {
|
||||
0x01: 11,
|
||||
0x02: 12,
|
||||
0x04: 13,
|
||||
0x08: 14,
|
||||
0x10: 15,
|
||||
0x19: 0,
|
||||
0x0B: 1,
|
||||
0x1F: 2,
|
||||
0x07: 3,
|
||||
0x0E: 4,
|
||||
0x15: 5,
|
||||
0x1A: 6,
|
||||
0x0D: 7,
|
||||
0x13: 8,
|
||||
0x16: 9,
|
||||
0x1C: 10,
|
||||
}
|
||||
|
||||
if syndrome == 0:
|
||||
return True
|
||||
if syndrome not in corrections:
|
||||
return False
|
||||
row[corrections[syndrome]] = not row[corrections[syndrome]]
|
||||
return True
|
||||
|
||||
|
||||
def encode_embedded_lc(lc: bytes | bitarray) -> tuple[bitarray, bitarray, bitarray, bitarray]:
|
||||
payload = bytes_to_bits(lc) if isinstance(lc, bytes) else lc.copy()
|
||||
if len(payload) != EMBEDDED_LC_PAYLOAD_BITS:
|
||||
raise EmbeddedLCError("embedded LC must be 9 bytes / 72 bits")
|
||||
|
||||
data = bitarray([0] * EMBEDDED_LC_RAW_BITS, endian="big")
|
||||
checksum = crc5(payload)
|
||||
data[106] = bool(checksum & 0x01)
|
||||
data[90] = bool(checksum & 0x02)
|
||||
data[74] = bool(checksum & 0x04)
|
||||
data[58] = bool(checksum & 0x08)
|
||||
data[42] = bool(checksum & 0x10)
|
||||
|
||||
position = 0
|
||||
for start, end in EMBEDDED_LC_PAYLOAD_RANGES:
|
||||
data[start:end] = payload[position:position + (end - start)]
|
||||
position += end - start
|
||||
|
||||
for row_start in range(0, 112, 16):
|
||||
row = data[row_start:row_start + 16]
|
||||
encode_hamming_16114(row)
|
||||
data[row_start:row_start + 16] = row
|
||||
|
||||
for column in range(16):
|
||||
data[column + 112] = (
|
||||
data[column + 0]
|
||||
^ data[column + 16]
|
||||
^ data[column + 32]
|
||||
^ data[column + 48]
|
||||
^ data[column + 64]
|
||||
^ data[column + 80]
|
||||
^ data[column + 96]
|
||||
)
|
||||
|
||||
raw = bitarray([0] * EMBEDDED_LC_RAW_BITS, endian="big")
|
||||
position = 0
|
||||
for index in range(EMBEDDED_LC_RAW_BITS):
|
||||
raw[index] = data[position]
|
||||
position += 16
|
||||
if position > 127:
|
||||
position -= 127
|
||||
|
||||
return tuple(
|
||||
raw[index:index + EMBEDDED_LC_FRAGMENT_BITS]
|
||||
for index in range(0, EMBEDDED_LC_RAW_BITS, EMBEDDED_LC_FRAGMENT_BITS)
|
||||
)
|
||||
|
||||
|
||||
def encode_emblc(lc: bytes | bitarray) -> dict[int, bitarray]:
|
||||
fragments = encode_embedded_lc(lc)
|
||||
return {
|
||||
1: fragments[0],
|
||||
2: fragments[1],
|
||||
3: fragments[2],
|
||||
4: fragments[3],
|
||||
}
|
||||
|
||||
|
||||
def decode_embedded_lc(fragments: tuple[bitarray, bitarray, bitarray, bitarray] | list[bitarray]) -> EmbeddedLC:
|
||||
if len(fragments) != 4:
|
||||
raise EmbeddedLCError("embedded LC requires four fragments")
|
||||
|
||||
raw = bitarray(endian="big")
|
||||
for fragment in fragments:
|
||||
if len(fragment) != EMBEDDED_LC_FRAGMENT_BITS:
|
||||
raise EmbeddedLCError("embedded LC fragments must be 32 bits")
|
||||
raw += fragment
|
||||
|
||||
data = bitarray([0] * EMBEDDED_LC_RAW_BITS, endian="big")
|
||||
position = 0
|
||||
for index in range(EMBEDDED_LC_RAW_BITS):
|
||||
data[position] = raw[index]
|
||||
position += 16
|
||||
if position > 127:
|
||||
position -= 127
|
||||
|
||||
corrected = 0
|
||||
for row_start in range(0, 112, 16):
|
||||
row = data[row_start:row_start + 16]
|
||||
before = row.copy()
|
||||
if not decode_hamming_16114(row):
|
||||
raise EmbeddedLCError("embedded LC Hamming check failed")
|
||||
if row != before:
|
||||
corrected += 1
|
||||
data[row_start:row_start + 16] = row
|
||||
|
||||
for column in range(16):
|
||||
parity = (
|
||||
data[column + 0]
|
||||
^ data[column + 16]
|
||||
^ data[column + 32]
|
||||
^ data[column + 48]
|
||||
^ data[column + 64]
|
||||
^ data[column + 80]
|
||||
^ data[column + 96]
|
||||
^ data[column + 112]
|
||||
)
|
||||
if parity:
|
||||
raise EmbeddedLCError("embedded LC column parity check failed")
|
||||
|
||||
payload = bitarray(endian="big")
|
||||
for start, end in EMBEDDED_LC_PAYLOAD_RANGES:
|
||||
payload += data[start:end]
|
||||
|
||||
checksum = 0
|
||||
if data[42]:
|
||||
checksum += 16
|
||||
if data[58]:
|
||||
checksum += 8
|
||||
if data[74]:
|
||||
checksum += 4
|
||||
if data[90]:
|
||||
checksum += 2
|
||||
if data[106]:
|
||||
checksum += 1
|
||||
|
||||
if crc5(payload) != checksum:
|
||||
raise EmbeddedLCError("embedded LC 5-bit checksum failed")
|
||||
|
||||
decoded = payload.tobytes()
|
||||
return EmbeddedLC(data=decoded, flco=decoded[0] & 0x3F, raw=raw, corrected=corrected)
|
||||
|
||||
|
||||
def embedded_lc_fragment_from_payload(payload: bytes) -> bitarray:
|
||||
if len(payload) != 33:
|
||||
raise EmbeddedLCError("DMR payload must be 33 bytes")
|
||||
bits = bytes_to_bits(payload)
|
||||
return bits[116:148]
|
||||
|
||||
|
||||
def payload_with_embedded_lc_fragment(payload: bytes, fragment: bitarray) -> bytes:
|
||||
if len(payload) != 33:
|
||||
raise EmbeddedLCError("DMR payload must be 33 bytes")
|
||||
if len(fragment) != EMBEDDED_LC_FRAGMENT_BITS:
|
||||
raise EmbeddedLCError("embedded LC fragment must be 32 bits")
|
||||
bits = bytes_to_bits(payload)
|
||||
bits = bits[:116] + fragment + bits[148:264]
|
||||
return bits.tobytes()
|
||||
@ -0,0 +1,265 @@
|
||||
import unittest
|
||||
|
||||
from freedmr_dmr_codec import (
|
||||
EmbeddedLCError,
|
||||
FullLCError,
|
||||
LC_SERVICE_OPTIONS_HBLINK_LEGACY,
|
||||
LC_SERVICE_OPTIONS_NORMAL,
|
||||
build_group_voice_lc,
|
||||
SlotTypeError,
|
||||
decode_embedded_lc,
|
||||
decode_full_lc,
|
||||
decode_slot_type,
|
||||
embedded_lc_fragment_from_payload,
|
||||
encode_embedded_lc,
|
||||
encode_full_lc,
|
||||
encode_full_lc_parity,
|
||||
encode_slot_type,
|
||||
payload_with_embedded_lc_fragment,
|
||||
rs129_parity,
|
||||
voice,
|
||||
voice_head_term,
|
||||
)
|
||||
|
||||
|
||||
GROUP_VOICE_LC = bytes.fromhex("000000000c302f9be5")
|
||||
HEADER_LC_BITS = (
|
||||
"000010111100001000000100110101100001111000001100001010111001100000001000"
|
||||
"010100000111010001100001000000000000001011110100100001110110011100000000"
|
||||
"0000000000000010111000000000111111001000010110100111"
|
||||
)
|
||||
TERMINATOR_LC_BITS = (
|
||||
"000010111010110100000100000000100001111010111100001010111110000000001000"
|
||||
"001000000111010011000001000100010100001011000111000001110001000100000000"
|
||||
"1100010000000010011000000000111001011000010110010100"
|
||||
)
|
||||
EMBEDDED_LC_BITS = (
|
||||
"00001111000011110000011000000110",
|
||||
"00010111000100010000000000000110",
|
||||
"00001100000000110011010100011011",
|
||||
"00010111001101100010001000100010",
|
||||
)
|
||||
VOICE_HEAD_TERM_PAYLOAD = bytes.fromhex(
|
||||
"2b6004101f842dd00df07d41046dff57d75df5de30152e2070b20f803f88c695e2"
|
||||
)
|
||||
VOICE_BURST_PAYLOAD = bytes.fromhex(
|
||||
"b9e881526173002a6bb9e881526134e0f060691173002a6bb9e881526173002a6a"
|
||||
)
|
||||
|
||||
|
||||
class FreeDmrDmrCodecTest(unittest.TestCase):
|
||||
def test_full_lc_header_encode_matches_current_codec_fixture(self):
|
||||
lc = GROUP_VOICE_LC
|
||||
|
||||
encoded = encode_full_lc(lc)
|
||||
|
||||
self.assertEqual(encoded.to01(), HEADER_LC_BITS)
|
||||
|
||||
def test_full_lc_terminator_encode_matches_current_codec_fixture(self):
|
||||
lc = GROUP_VOICE_LC
|
||||
|
||||
encoded = encode_full_lc(lc, terminator=True)
|
||||
|
||||
self.assertEqual(encoded.to01(), TERMINATOR_LC_BITS)
|
||||
|
||||
def test_embedded_lc_group_voice_encode_matches_current_codec(self):
|
||||
lc = GROUP_VOICE_LC
|
||||
|
||||
encoded = encode_embedded_lc(lc)
|
||||
|
||||
self.assertEqual(tuple(fragment.to01() for fragment in encoded), EMBEDDED_LC_BITS)
|
||||
|
||||
def test_current_runtime_compatibility_function_names(self):
|
||||
import freedmr_dmr_codec as dmr_codec
|
||||
|
||||
lc = GROUP_VOICE_LC
|
||||
|
||||
self.assertEqual(dmr_codec.encode_header_lc(lc).to01(), HEADER_LC_BITS)
|
||||
self.assertEqual(dmr_codec.encode_terminator_lc(lc).to01(), TERMINATOR_LC_BITS)
|
||||
self.assertEqual(
|
||||
tuple(dmr_codec.encode_emblc(lc)[index].to01() for index in (1, 2, 3, 4)),
|
||||
EMBEDDED_LC_BITS,
|
||||
)
|
||||
|
||||
def test_voice_head_term_decode_matches_current_codec(self):
|
||||
decoded = voice_head_term(VOICE_HEAD_TERM_PAYLOAD)
|
||||
|
||||
self.assertEqual(decoded["LC"], bytes.fromhex("001020000c302f9be5"))
|
||||
self.assertEqual(decoded["CC"], b"\x01")
|
||||
self.assertEqual(decoded["DTYPE"], b"\x01")
|
||||
self.assertEqual(decoded["SYNC"].to01(), "110111111111010101111101011101011101111101011101")
|
||||
|
||||
def test_voice_burst_decode_matches_current_codec(self):
|
||||
decoded = voice(VOICE_BURST_PAYLOAD)
|
||||
|
||||
self.assertEqual(
|
||||
[ambe.to01() for ambe in decoded["AMBE"]],
|
||||
[
|
||||
"101110011110100010000001010100100110000101110011000000000010101001101011",
|
||||
"101110011110100010000001010100100110000101110011000000000010101001101011",
|
||||
"101110011110100010000001010100100110000101110011000000000010101001101010",
|
||||
],
|
||||
)
|
||||
self.assertEqual(decoded["CC"], b"\x01")
|
||||
self.assertEqual(decoded["LCSS"], b"\x01")
|
||||
self.assertEqual(decoded["EMBED"].to01(), "01001110000011110000011000000110")
|
||||
|
||||
def test_full_lc_decode_classifies_group_voice(self):
|
||||
lc = GROUP_VOICE_LC
|
||||
|
||||
decoded = decode_full_lc(encode_full_lc(lc))
|
||||
|
||||
self.assertEqual(decoded.data, lc)
|
||||
self.assertEqual(decoded.flco, 0x00)
|
||||
self.assertTrue(decoded.is_group_call)
|
||||
self.assertFalse(decoded.is_unit_call)
|
||||
self.assertEqual(decoded.target_id, 3120)
|
||||
self.assertEqual(decoded.source_id, 3120101)
|
||||
|
||||
def test_build_group_voice_lc_defaults_to_normal_service_options(self):
|
||||
lc = build_group_voice_lc(bytes.fromhex("00005b"), bytes.fromhex("2f9be5"))
|
||||
|
||||
self.assertEqual(lc, bytes.fromhex("00000000005b2f9be5"))
|
||||
self.assertEqual(decode_full_lc(encode_full_lc(lc)).service_options, LC_SERVICE_OPTIONS_NORMAL)
|
||||
|
||||
def test_build_group_voice_lc_can_represent_legacy_hblink_options_explicitly(self):
|
||||
lc = build_group_voice_lc(
|
||||
bytes.fromhex("00005b"),
|
||||
bytes.fromhex("2f9be5"),
|
||||
service_options=LC_SERVICE_OPTIONS_HBLINK_LEGACY,
|
||||
)
|
||||
|
||||
self.assertEqual(lc, bytes.fromhex("00002000005b2f9be5"))
|
||||
self.assertEqual(decode_full_lc(encode_full_lc(lc)).service_options, LC_SERVICE_OPTIONS_HBLINK_LEGACY)
|
||||
|
||||
def test_full_lc_decode_classifies_unit_voice(self):
|
||||
lc = bytes.fromhex("030000000c302f9be5")
|
||||
|
||||
decoded = decode_full_lc(encode_full_lc(lc))
|
||||
|
||||
self.assertEqual(decoded.flco, 0x03)
|
||||
self.assertFalse(decoded.is_group_call)
|
||||
self.assertTrue(decoded.is_unit_call)
|
||||
|
||||
def test_full_lc_rs129_parity_matches_header_and_terminator_masks(self):
|
||||
lc = bytes.fromhex("001020000c302f9be5")
|
||||
|
||||
self.assertEqual(rs129_parity(lc), bytes.fromhex("4c42cc"))
|
||||
self.assertEqual(encode_full_lc_parity(lc), bytes.fromhex("dad45a"))
|
||||
self.assertEqual(encode_full_lc_parity(lc, terminator=True), bytes.fromhex("d5db55"))
|
||||
|
||||
def test_full_lc_requires_nine_byte_lc(self):
|
||||
with self.assertRaises(FullLCError):
|
||||
encode_full_lc(b"\x00" * 8)
|
||||
|
||||
def test_slot_type_encode_matches_current_codec_fixtures(self):
|
||||
self.assertEqual(encode_slot_type(1, 0x1).to01(), "00010001101110001100")
|
||||
self.assertEqual(encode_slot_type(1, 0x2).to01(), "00010010101001011001")
|
||||
self.assertEqual(encode_slot_type(1, 0x3).to01(), "00010011001010110010")
|
||||
self.assertEqual(encode_slot_type(1, 0x6).to01(), "00010110000011001110")
|
||||
|
||||
def test_slot_type_decode_corrects_three_bits(self):
|
||||
bits = encode_slot_type(1, 0x2)
|
||||
bits[0] = not bits[0]
|
||||
bits[7] = not bits[7]
|
||||
bits[19] = not bits[19]
|
||||
|
||||
decoded = decode_slot_type(bits)
|
||||
|
||||
self.assertEqual(decoded.color_code, 1)
|
||||
self.assertEqual(decoded.data_type, 0x2)
|
||||
self.assertEqual(decoded.name, "VOICE_LC_TERM")
|
||||
self.assertEqual(decoded.corrected, 3)
|
||||
|
||||
def test_slot_type_decode_rejects_uncorrectable_error(self):
|
||||
bits = encode_slot_type(1, 0x2)
|
||||
for index in (0, 1, 2, 3):
|
||||
bits[index] = not bits[index]
|
||||
|
||||
with self.assertRaises(SlotTypeError):
|
||||
decode_slot_type(bits)
|
||||
|
||||
def test_slot_type_rejects_values_outside_four_bits(self):
|
||||
with self.assertRaises(SlotTypeError):
|
||||
encode_slot_type(16, 0x1)
|
||||
with self.assertRaises(SlotTypeError):
|
||||
encode_slot_type(1, 16)
|
||||
|
||||
def test_embedded_lc_round_trips_talker_alias_header(self):
|
||||
lc = bytes.fromhex("04004c43414c4c3132")
|
||||
|
||||
encoded = encode_embedded_lc(lc)
|
||||
decoded = decode_embedded_lc(encoded)
|
||||
|
||||
self.assertEqual(decoded.data, lc)
|
||||
self.assertEqual(decoded.flco, 0x04)
|
||||
self.assertEqual(decoded.corrected, 0)
|
||||
|
||||
def test_embedded_lc_round_trips_gps_info(self):
|
||||
lc = bytes.fromhex("080007fcfae048b57b")
|
||||
|
||||
encoded = encode_embedded_lc(lc)
|
||||
decoded = decode_embedded_lc(encoded)
|
||||
|
||||
self.assertEqual(decoded.data, lc)
|
||||
self.assertEqual(decoded.flco, 0x08)
|
||||
|
||||
def test_encoded_talker_alias_fragments_match_mmdvmhost_layout_fixture(self):
|
||||
lc = bytes.fromhex("04004c43414c4c3132")
|
||||
payload = b"\x55" * 33
|
||||
|
||||
encoded_payloads = [
|
||||
payload_with_embedded_lc_fragment(payload, fragment).hex()
|
||||
for fragment in encode_embedded_lc(lc)
|
||||
]
|
||||
|
||||
self.assertEqual(
|
||||
encoded_payloads,
|
||||
[
|
||||
"555555555555555555555555555550517092855555555555555555555555555555",
|
||||
"555555555555555555555555555550382441d55555555555555555555555555555",
|
||||
"5555555555555555555555555555522717b8e55555555555555555555555555555",
|
||||
"5555555555555555555555555555522b73ae255555555555555555555555555555",
|
||||
],
|
||||
)
|
||||
|
||||
def test_payload_fragment_helpers_extract_and_apply_embedded_lc(self):
|
||||
lc = bytes.fromhex("080007fcfae048b57b")
|
||||
fragment = encode_embedded_lc(lc)[0]
|
||||
original = b"\x55" * 33
|
||||
|
||||
updated = payload_with_embedded_lc_fragment(original, fragment)
|
||||
extracted = embedded_lc_fragment_from_payload(updated)
|
||||
|
||||
self.assertEqual(extracted, fragment)
|
||||
self.assertEqual(updated[:14], original[:14])
|
||||
self.assertEqual(updated[19:], original[19:])
|
||||
|
||||
def test_embedded_lc_decode_corrects_single_bit_error(self):
|
||||
lc = bytes.fromhex("04004c43414c4c3132")
|
||||
fragments = list(encode_embedded_lc(lc))
|
||||
fragments[0] = fragments[0].copy()
|
||||
fragments[0][0] = not fragments[0][0]
|
||||
|
||||
decoded = decode_embedded_lc(fragments)
|
||||
|
||||
self.assertEqual(decoded.data, lc)
|
||||
self.assertEqual(decoded.corrected, 1)
|
||||
|
||||
def test_embedded_lc_decode_rejects_uncorrectable_error(self):
|
||||
lc = bytes.fromhex("04004c43414c4c3132")
|
||||
fragments = list(encode_embedded_lc(lc))
|
||||
fragments[0] = fragments[0].copy()
|
||||
fragments[0][0] = not fragments[0][0]
|
||||
fragments[0][8] = not fragments[0][8]
|
||||
|
||||
with self.assertRaises(EmbeddedLCError):
|
||||
decode_embedded_lc(fragments)
|
||||
|
||||
def test_embedded_lc_requires_nine_byte_lc(self):
|
||||
with self.assertRaises(EmbeddedLCError):
|
||||
encode_embedded_lc(b"\x00" * 8)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@ -0,0 +1,24 @@
|
||||
import unittest
|
||||
|
||||
from utils import bytes_3, bytes_4, get_alias, int_id
|
||||
|
||||
|
||||
class FreeDmrUtilsTest(unittest.TestCase):
|
||||
def test_integer_byte_helpers_are_big_endian(self):
|
||||
self.assertEqual(bytes_3(0x010203), b"\x01\x02\x03")
|
||||
self.assertEqual(bytes_4(0x01020304), b"\x01\x02\x03\x04")
|
||||
self.assertEqual(int_id(b"\x01\x02\x03\x04"), 0x01020304)
|
||||
|
||||
def test_get_alias_returns_matching_record_or_original_id(self):
|
||||
aliases = {
|
||||
3120001: {"callsign": "M0ABC", "name": "Test"},
|
||||
235: "TG235",
|
||||
}
|
||||
|
||||
self.assertEqual(get_alias(bytes_3(3120001), aliases, "callsign"), ["M0ABC"])
|
||||
self.assertEqual(get_alias(235, aliases), "TG235")
|
||||
self.assertEqual(get_alias(999, aliases), 999)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in new issue