Momentum-Apps/nrf24scan/nrf24_packet_decoder.py

#
# NRF24L01+ Enhanced ShockBurst packets decoder
#
payload_len_default = 4
packets = (
    "10101010 11101110 00000011 00001000 00001011 01000111 000100 10 0 10101010 10101010 10101010 10101010 00011101",
    "10101010 11001000 11001000 11000011 110011 10 0 00001011 00000011 00000101 00000000 0010001100100000",
    "10101010 11001000 11001000 11000100 000100 11 1 00001011 00000011 00000101 00000000 0010010011100010",
    "10101010 11001000 11001000 11000100  00001011 00000011 00000101 00000010 1000010101000010",
    "10101010 11001000 11001000 11000000 110011 10 0 11110101 00000010 00000011 00000000 0000111001000000",
    "01010101 01000000 01101000 00010101 000000 00 0 0100100000100000",
    #  '01010101 01000010 11100100 10100110 01010101 01000100 110011 00 0 10010101 10110011 01100100 10101100 10101011 01010010 01111100 01001010 1100110100110001',
)


def bin2hex(x):
    def bin2hex_helper(r):
        while r:
            yield r[0:2].upper()
            r = r[2:]

    if len(x) == 0:
        return
    fmt = "{0:0" + str(int(len(x) / 8 * 2)) + "X}"
    hex_data = fmt.format(int(x, 2))
    return list(bin2hex_helper(hex_data))


def bin2hexlong(b):
    b = b.replace(" ", "")
    out = ""
    n = 8
    for i in range(0, len(b), n):
        b2 = b[i : i + n]
        out = out + "{0:02X}".format(int(b2.ljust(8, "0"), 2))
    return out


def split_packet(packet, parts):
    """Split a string of 1s and 0s into multiple substrings as specified by parts.
    Example: "111000011000", (3, 4, 2) -> ["111", "0000", "11", "000"]
    :param packet: String of 1s and 0s
    :param parts: Tuple of length of substrings
    :return: list of substrings
    """
    out = []
    packet = packet.replace(" ", "")
    for part_length in parts:
        out.append(packet[0:part_length])
        packet = packet[part_length:]
    out.append(packet)
    return out


def parse_packet(packet, address_length, ESB):
    """Split a packet into its fields and return them as tuple."""
    if ESB:
        preamble, address, payload_length, pid, no_ack, rest = split_packet(
            packet=packet, parts=(8, 8 * address_length, 6, 2, 1)
        )
        payload, crc = split_packet(
            packet=rest,
            parts=(
                (
                    payload_len_default
                    if int(payload_length, 2) > 32
                    else int(payload_length, 2)
                )
                * 8,
            ),
        )
    else:
        preamble, address, rest = split_packet(
            packet=packet, parts=(8, 8 * address_length)
        )
        crc = packet.rsplit(" ", 1)[1]
        payload = rest[0 : len(rest) - len(crc)]
        payload_length = pid = no_ack = ""

    assert preamble in ("10101010", "01010101")
    assert len(crc) in (8, 16)

    return preamble, address, payload_length, pid, no_ack, payload, crc


def crc(bits, size=8):
    """Calculate the crc value for the polynomial initialized with 0xFF/0xFFFF)
    :param size: 8 or 16 bit crc
    :param bits: String of 1s and 0s
    :return:
    :polynomial: 1 byte CRC - standard is 0x107 = 0b100000111 = x^8+x^2+x^1+1, result the same for 0x07
    :polynomial: 2 byte CRC - standard is 0x11021 = X^16+X^12+X^5+1, result the same for 0x1021
    """
    if size == 8:
        polynomial = 0x107
        crc = 0xFF
    else:
        polynomial = 0x11021
        crc = 0xFFFF
    max_crc_value = (1 << size) - 1  # e.g. 0xFF for mode 8bit-crc
    for bit in bits:
        bit = int(bit, 2)
        crc <<= 1
        if (crc >> size) ^ bit:  # top most lfsr bit xor current data bit
            crc ^= polynomial
        crc &= max_crc_value  # trim the crc to reject carry over bits
    #        print('{:X}'.format(crc))
    return crc


if __name__ == "__main__":
    for packet in packets:
        fld = packet.split(" ")
        address_length = -1
        ESB = True
        for f in fld:
            if len(f) == 6:
                break
            if len(f) == 0:
                ESB = False
                break
            address_length += 1
        (
            preamble,
            address,
            payload_length,
            pid,
            no_ack,
            payload,
            crc_received,
        ) = parse_packet(packet=packet, address_length=address_length, ESB=ESB)
        crc_size = len(crc_received)
        crc_received = "0x" + "{:X}".format(int(crc_received, 2))
        print(f"Packet: {packet}")
        print(
            "\n".join(
                (
                    f"Hex: {bin2hexlong(packet)}",
                    "Preamble: 0x%X" % int(preamble, 2),
                    f"Address: {address_length} bytes - {bin2hex(address)}",
                )
            )
        )
        if ESB:
            print(
                "\n".join(
                    (
                        f"Payload length in packet: {int(payload_length, 2)}, used: {(payload_len_default if int(payload_length, 2) > 32 else int(payload_length, 2))}",
                        f"Payload: {bin2hex(payload)}",
                        f"Pid: {int(pid, 2)}",
                        f"No_ack: {int(no_ack, 2) == 1}",
                    )
                )
            )
        else:
            print(
                f"Not Enhanced ShockBurst packet, payload length: {int(len(payload) / 8)}"
            )
            print(f"Payload: {bin2hex(payload)}")
        print(f"CRC{crc_size}: {crc_received}")
        crc_calculated = "0x" + "{:X}".format(
            crc(address + payload_length + pid + no_ack + payload, size=crc_size)
        )
        if crc_received == crc_calculated:
            print("CRC is valid!")
        else:
            print(f"CRC mismatch! Calculated CRC is {crc_calculated}.")
        print("-------------")