#!/usr/bin/env python3
"""Verify the IEEE 1609.2 / TS 103 097 signatures of secured GeoNetworking frames in a pcap.

Written 2026-09-23 to check, independently of the firmware, that the ESP32-C5 really signs with
the demo authorization ticket the app provisions. It shares no code with vanetza-idf: the envelope
is decoded with asn1tools from the IEEE 1609.2 ASN.1 modules, and ECDSA is checked with Python's
`cryptography` (OpenSSL).

    py -3.11 obu-firmware/test/verify_signed_pcap.py capture.pcap \\
        --bundle app/src/main/assets/demo-chain.vcr \\
        --asn1 obu-firmware/external/vanetza-idf/asn1

For every frame whose GN Basic Header says "secured" it reports: the signer (digest or full
certificate), whether that signer is the bundle's ticket, the psid and generation time, and
whether the message signature verifies with the ticket's public key. It also checks the bundle's
own chain (ticket signed by AA, AA by root, root self-signed). Frames signed by anyone else (an
RSU under the EU PKI) are counted and listed, not verified: their certificates are not known here.

Signature input, IEEE 1609.2 clause 5.3.1: ECDSA over Hash(tbsData) || Hash(signer), where the
signer part is the COER of the signing certificate (the empty string for a self-signed root).

Handles linktype 105 (bare 802.11, what the V2X2MAP bridge records) and 127 (radiotap).
"""

from __future__ import annotations

import argparse
import hashlib
import struct
import sys
from collections import Counter
from datetime import datetime, timezone
from pathlib import Path

LLC_SNAP_GN = b"\xaa\xaa\x03\x00\x00\x00\x89\x47"
ITS_EPOCH_UNIX = 1072915200


def pcap_frames(path: Path):
    data = path.read_bytes()
    magic = struct.unpack("<I", data[:4])[0]
    endian = "<" if magic in (0xA1B2C3D4, 0xA1B23C4D) else ">"
    linktype = struct.unpack(endian + "I", data[20:24])[0]
    i = 24
    while i + 16 <= len(data):
        ts_sec, ts_frac, incl, _orig = struct.unpack(endian + "IIII", data[i:i + 16])
        frame = data[i + 16:i + 16 + incl]
        i += 16 + incl
        if linktype == 127:  # radiotap: skip its own length
            frame = frame[struct.unpack("<H", frame[2:4])[0]:]
        elif linktype != 105:
            raise SystemExit("unsupported linktype %d" % linktype)
        yield ts_sec + ts_frac / 1e6, frame


def secured_payload(frame: bytes):
    """Source MAC and the bytes after the GN Basic Header, if this is a secured GN frame."""
    if len(frame) < 24:
        return None
    fc = frame[0]
    if (fc >> 2) & 0x3 != 2:  # not a data frame
        return None
    header = 26 if (fc >> 4) & 0x8 else 24  # QoS data carries 2 more octets
    at = frame.find(LLC_SNAP_GN, header, header + 16)
    if at < 0:
        return None
    gn = frame[at + 8:]
    if len(gn) < 5 or gn[0] & 0x0F != 2:  # Basic Header next header 2: secured packet
        return None
    return frame[10:16], gn[4:]


def read_bundle(path: Path):
    """The VCR1 bundle's certificates: [type 1][length 2 BE][payload] records."""
    data = path.read_bytes()
    certs = {"root": [], "authority": [], "ticket": []}
    kinds = {1: "root", 2: "authority", 3: "ticket"}
    i = 4 if data[:4] == b"VCR1" else 0
    while i + 3 <= len(data):
        kind, length = data[i], struct.unpack(">H", data[i + 1:i + 3])[0]
        if kind in kinds:
            certs[kinds[kind]].append(data[i + 3:i + 3 + length])
        i += 3 + length
    return certs


def its_station(gn_common_onward: bytes):
    """StationID of the ITS PDU inside a secured GN packet's payload.

    The signed payload is the GN packet from the Common Header on: Common Header (8), the extended
    header of the Common Header's type, BTP-B (4), then the ITS PDU, whose header is
    protocolVersion (1), messageID (1), stationID (4)."""
    if len(gn_common_onward) < 8:
        return None
    ext = {5: 28, 4: 44}.get(gn_common_onward[1] >> 4)  # HT: 5 TSB/SHB, 4 GBC
    if ext is None:
        return None
    at = 8 + ext + 4
    pdu = gn_common_onward[at:at + 6]
    return int.from_bytes(pdu[2:6], "big") if len(pdu) == 6 else None


def hashed_id8(octets: bytes) -> bytes:
    return hashlib.sha256(octets).digest()[-8:]


class Verifier:
    def __init__(self, asn1_dir: Path):
        import asn1tools
        spec = asn1tools.compile_files([str(asn1_dir / "IEEE1609dot2.asn"),
                                        str(asn1_dir / "IEEE1609dot2BaseTypes.asn")], "oer")
        self.m = spec.modules["IEEE1609dot2"]

    def public_key(self, cert_octets: bytes):
        from cryptography.hazmat.primitives.asymmetric import ec
        cert = self.m["Certificate"].decode(cert_octets)
        kind, key = cert["toBeSigned"]["verifyKeyIndicator"]
        if kind != "verificationKey" or key[0] != "ecdsaNistP256":
            raise ValueError("not an ECDSA P-256 verification key: %r" % (key[0],))
        form, point = key[1]
        encoded = {"compressed-y-0": b"\x02" + point, "compressed-y-1": b"\x03" + point,
                   "uncompressedP256": b"\x04" + point.get("x", b"") + point.get("y", b"")
                   if isinstance(point, dict) else None}[form]
        return ec.EllipticCurvePublicKey.from_encoded_point(ec.SECP256R1(), encoded)

    @staticmethod
    def _ecdsa_ok(public_key, message: bytes, signature) -> bool:
        from cryptography.exceptions import InvalidSignature
        from cryptography.hazmat.primitives import hashes
        from cryptography.hazmat.primitives.asymmetric import ec
        from cryptography.hazmat.primitives.asymmetric.utils import encode_dss_signature
        kind, sig = signature
        if kind != "ecdsaNistP256Signature":
            raise ValueError("unsupported signature %s" % kind)
        r_kind, r = sig["rSig"]
        r_x = r if isinstance(r, (bytes, bytearray)) else r["x"]  # x-only / compressed: r is x
        der = encode_dss_signature(int.from_bytes(r_x, "big"), int.from_bytes(sig["sSig"], "big"))
        try:
            public_key.verify(der, message, ec.ECDSA(hashes.SHA256()))
            return True
        except InvalidSignature:
            return False

    def certificate_signed_by(self, cert_octets: bytes, issuer_octets: bytes | None) -> bool:
        cert = self.m["Certificate"].decode(cert_octets)
        tbs = self.m["ToBeSignedCertificate"].encode(cert["toBeSigned"])
        signer_input = hashlib.sha256(issuer_octets if issuer_octets is not None else b"").digest()
        key = self.public_key(issuer_octets if issuer_octets is not None else cert_octets)
        return self._ecdsa_ok(key, hashlib.sha256(tbs).digest() + signer_input, cert["signature"])

    def message(self, octets: bytes, known: dict[bytes, bytes]):
        """Decodes one Ieee1609Dot2Data; returns a result dict."""
        data = self.m["Ieee1609Dot2Data"].decode(octets)
        encoded = self.m["Ieee1609Dot2Data"].encode(data)
        kind, signed = data["content"]
        if kind != "signedData":
            return {"kind": kind}
        tbs = self.m["ToBeSignedData"].encode(signed["tbsData"])
        header = signed["tbsData"]["headerInfo"]
        signer_kind, signer = signed["signer"]
        if signer_kind == "digest":
            digest, cert_octets = bytes(signer), known.get(bytes(signer))
        elif signer_kind == "certificate":
            cert_octets = self.m["Certificate"].encode(signer[0])
            digest = hashed_id8(cert_octets)
        else:
            return {"kind": "signedData", "signer": signer_kind}
        result = {
            "kind": "signedData",
            "signer": signer_kind,
            "digest": digest.hex().upper(),
            "psid": header["psid"],
            "generation_time_us": header.get("generationTime"),
            # COER is canonical, so a re-encoding identical to the wire bytes means the slices
            # hashed below are exactly what the sender signed.
            "canonical": octets.startswith(encoded) and tbs in octets,
        }
        if cert_octets is None:
            result["verified"] = None  # unknown signer
            return result
        message = hashlib.sha256(tbs).digest() + hashlib.sha256(cert_octets).digest()
        result["verified"] = self._ecdsa_ok(self.public_key(cert_octets), message, signed["signature"])
        inner = signed["tbsData"]["payload"].get("data")
        if inner and inner["content"][0] == "unsecuredData":
            payload = bytes(inner["content"][1])
            result["payload"] = payload
        return result


def main() -> int:
    p = argparse.ArgumentParser(description=__doc__.split("\n\n")[0])
    p.add_argument("pcap", type=Path)
    p.add_argument("--bundle", type=Path, required=True, help="VCR1 credential bundle (demo-chain.vcr)")
    p.add_argument("--asn1", type=Path, required=True, help="directory with IEEE1609dot2*.asn")
    args = p.parse_args()

    v = Verifier(args.asn1)
    certs = read_bundle(args.bundle)
    root, aa, at = certs["root"][0], certs["authority"][0], certs["ticket"][0]
    print("bundle: root %s, AA %s, AT %s" % (hashed_id8(root).hex().upper(), hashed_id8(aa).hex().upper(),
                                             hashed_id8(at).hex().upper()))
    print("chain:  root self-signed %s, AA by root %s, AT by AA %s" % (
        v.certificate_signed_by(root, None), v.certificate_signed_by(aa, root), v.certificate_signed_by(at, aa)))
    known = {hashed_id8(at): at}

    tally = Counter()
    ours = []
    for ts, frame in pcap_frames(args.pcap):
        found = secured_payload(frame)
        if not found:
            continue
        mac, octets = found
        try:
            r = v.message(octets, known)
        except Exception as e:  # noqa: BLE001 - a malformed frame is a finding, not a crash
            tally["undecodable"] += 1
            continue
        if r.get("verified") is None:
            tally["signed by an unknown signer %s (psid %s)" % (r.get("digest"), r.get("psid"))] += 1
            continue
        tally["demo AT, signature %s" % ("VALID" if r["verified"] else "INVALID")] += 1
        ours.append((ts, mac, r))

    for line, n in sorted(tally.items()):
        print("%5d  %s" % (n, line))
    # The V2X2MAP bridge stamps records with board uptime, not wall-clock time, so the signature's
    # generationTime is compared with the file's modification time (end of the recording) instead.
    recorded_until = args.pcap.stat().st_mtime
    for ts, mac, r in ours[:5]:
        gen = r["generation_time_us"] / 1e6 + ITS_EPOCH_UNIX if r["generation_time_us"] else None
        print("  %s from %s: signer %s %s, psid %d, ITS PDU station %s, generationTime %s UTC "
              "(%+.0f s before the recording ended), canonical %s, signature %s" % (
                  f"{ts:.3f}", mac.hex(":"), r["signer"], r["digest"], r["psid"], its_station(r.get("payload", b"")),
                  datetime.fromtimestamp(gen, timezone.utc).strftime("%Y-%m-%d %H:%M:%S.%f")[:-3] if gen else "-",
                  (recorded_until - gen) if gen else float("nan"), r["canonical"],
                  "VALID" if r["verified"] else "INVALID"))
    return 0 if ours and all(r["verified"] for _, _, r in ours) else 1


if __name__ == "__main__":
    sys.exit(main())