#!/usr/bin/env python3
"""
VanMoof S3 — custom-firmware-upload over BLE (zelfde pad als MoofMate, eigen implementatie).

Werkt firmware-geverifieerd tegen de dump (ble.dis, alle adressen in de bron):
  OAD-service 0x5510:
    char 0x5511 (param_id 0 → 0xC420): START.  16 bytes, userkey-ECB versleuteld over
        [nonce(2)][filetype(1)][size u32 BE(4)][crc u32 BE(4)][pad 0(5)]  — v5: BIG-endian!
        filetype 0 → externe SPI-flash vanaf 0x80000 = de PACK-regio (max 0x180000).
        De CRC in dit bericht is de standaard zlib.crc32 van het bestand (ZONDER complement).
        Vóór v5 werd hier LE + ^0xFFFFFFFF gestuurd → fiets antwoordde met directe disconnect
        binnen ~62 ms (size 195860 als LE = 0x14FD0200 > limiet 0x180000). Bewijs: fw
        START-handler intern 0xA420 leest size als b[1]<<24|b[2]<<16|b[3]<<8|b[4]; crc op 5..8.
        Referentie: chwdt/vanmoof-tools update.py + pymoof (MoofMate-pad), live bevestigd 27-9.
    char 0x5512 (param_id 1 → 0xC7A4): DATA-chunks.  PLAINTEXT over BLE maar per 16-byte
        blok MKEY-ECB versleuteld (0xC7D0 → 0x2BA00 → slot 126).  Grootte: veelvoud van 16,
        aanbevolen ≤ 0xF0; MTU≥len+3 vereist (elif-ATU-exchange gebeurt automatisch in bleak).
        4K-sectoren worden bij overschrijding automatisch geërase (0xC83A-pad).
    Einde (received==size): CRC-check; OK → event 0x13, mismatch → event 0x16; beide als
        1-byte notify op char 0x5513. [bevestigd via vanmoof-s3-decomp oad.c]
    Event 0x13 → SM-handler (thunk 0x2D5D4) → pack_ingest 0x142BC: pakfs_open(0x80000),
        magic "PACK", dir_off/dir_end, dan SSP 0x10E status 1 naar mainware → manager
        start en staged/flasht de leden. Dus: NA een geslaagde upload is GEEN extra
        trigger nodig — de pack-verwerking start automatisch bij CRC-OK.
  Backoffice-service 0x5500:
    char 0x5505 (param_id 4 → 0x44A0): [nonce(2)][versie=1(1)][off=0(1)][MKEY-ECB × n(16n)].
        Inner: [sid4 (BE, ge-echo'd)][type u16 LE][len u8][payload][crc16 LE over 0..7+len].
        CRC16 = Modbus-stijl (poly 0xA001 reflected, init 0xFFFF, GEEN xorout) — 0x2DFE0.
        GEEN userkey-auth nodig (perms 0x08 = alleen nonce-check); authenticatie = MKEY zelf.
        Respons: notification op 0x5505: [sid4][tijd4][status1] met status 0=OK, 1=fout.
        Veilige probe: type 4 = no-op (0x2F9E4 returnt 1 → status 0).

Gebruik:
  # Stap 1 — veilige probe (geen flash-aanraking): verifieert MKEY + berichtformaat:
  ~/vmble/bin/python -u vanmoof_customfw.py --bike-json FIETS.json --name ES3-XXXX --probe

  # Stap 2 — PACK-upload naar ext-flash 0x80000 (herschrijft de PACK-regio!):
  ~/vmble/bin/python -u vanmoof_customfw.py --bike-json FIETS.json --name ES3-XXXX \
      --upload image.pack

MKEY: standaard slot 126 uit de dump; overschrijven met --mkey <hex>.
Vereist: pip3 install bleak cryptography
"""
import argparse
import asyncio
import struct
import sys
import time
import zlib

try:
    from cryptography.hazmat.primitives.ciphers import Cipher, algorithms, modes
except ImportError:
    sys.exit("cryptography ontbreekt.  Doe eerst:  pip3 install cryptography")

try:
    from bleak import BleakClient, BleakScanner
except ImportError:
    sys.exit("bleak ontbreekt.  Doe eerst:  pip3 install bleak cryptography")

VM = "e631-4069-944d-b8ca7598ad50"
CH = lambda c: f"6acc{c}-{VM}"
CHALLENGE, KEY_INDEX = CH("5501"), CH("5502")
BACKOFFICE = CH("5505")       # param_id 4 → 0x44A0
OAD_START, OAD_DATA = CH("5511"), CH("5512")
LOG_SIZE, LOG_BLOCK = CH("55c2"), CH("55c3")   # fiets-log ringbuffer (zelfde leesprotocol als vanmoof_ble_cells.py)

# Slot 126 (0x5AFE0) uit de SPI-dump van deze fiets; magic 'MKEY', CRC geldig.
DEFAULT_MKEY = "c9d9217a5ec6094d5489fa2ac2390e35"


def load_creds(a):
    import json, os
    key_hex = None
    if a.bike_json:
        with open(a.bike_json) as fh:
            d = json.load(fh)
        try:
            key_hex, ukid = d["key"]["encryptionKey"], int(d["key"]["userKeyId"])
        except (KeyError, ValueError, TypeError) as e:
            sys.exit(f"bike-json mist key.encryptionKey / key.userKeyId ({e})")
    else:
        key_hex, ukid = a.key, a.user_key_id
    if not key_hex or ukid is None:
        sys.exit("Geef --bike-json, OF (--key/VANMOOF_KEY én --user-key-id).")
    try:
        key = bytes.fromhex(key_hex)
    except ValueError:
        sys.exit("--key/VANMOOF_KEY is geen geldige hex.")
    if len(key) != 16:
        sys.exit("userkey moet 16 bytes (32 hex tekens) zijn.")
    return key, ukid


class Crypto:
    """Userkey-AES-ECB (auth + versleutelde reads/writes)."""

    def __init__(self, key):
        self._c = Cipher(algorithms.AES(key), modes.ECB())

    def enc(self, b):
        e = self._c.encryptor(); return e.update(b) + e.finalize()

    def dec(self, b):
        if not b or len(b) % 16:
            return None
        d = self._c.decryptor(); return d.update(b) + d.finalize()


def crc16_modbus(data):
    """0x2DFE0: poly 0xA001 (reflected CRC-16/CCITT-FALSE-variant), init 0xFFFF, geen xorout."""
    crc = 0xFFFF
    for b in data:
        crc ^= b
        for _ in range(8):
            crc = (crc >> 1) ^ 0xA001 if crc & 1 else crc >> 1
    return crc


def raw_crc32(data):
    """Transport-CRC (geverifieerd): standaard CRC-32 = zlib.crc32, ZONDER extra complement.
    Referentie: chwdt/vanmoof-tools update.py (werkt op echte fietsen)."""
    return zlib.crc32(data) & 0xFFFFFFFF


class Backoffice:
    """Backoffice-protocol (char 5505 → 0x44A0): MKEY-ECB binnenlaag."""

    def __init__(self, mkey):
        self._c = Cipher(algorithms.AES(mkey), modes.ECB())
        self._sid = 0
        self.responses = asyncio.Queue()

    def _enc(self, b):
        e = self._c.encryptor(); return e.update(b) + e.finalize()

    def _dec(self, b):
        d = self._c.decryptor(); return d.update(b) + d.finalize()

    def build(self, msg_type, payload=b"", nonce=b""):
        """Inner: [sid4 BE][type u16 LE][len u8][payload][crc16 LE over 0..7+len];
        ATT: [nonce2][01][00] + MKEY-ECB(inner gepadded naar 16n).  nonce = 2 bytes uit char 5501."""
        self._sid = (self._sid + 1) & 0xFFFFFFFF
        inner = bytearray(7 + len(payload) + 2)
        inner[0:4] = struct.pack(">I", self._sid)
        inner[4:6] = struct.pack("<H", msg_type)
        inner[6] = len(payload)
        inner[7:7 + len(payload)] = payload
        crc = crc16_modbus(bytes(inner[:7 + len(payload)]))
        inner[7 + len(payload):] = struct.pack("<H", crc)
        pad = (-len(inner)) % 16
        blocks = self._enc(bytes(inner) + bytes(pad))
        # 0x44A4: (len-2)-2 moet veelvoud van 16 zijn (16n blokken);
        # 0x44F6: off + (len-2) <= 0x90 → met off=0: max 9 blokken (144 bytes).
        if len(blocks) > 9 * 16:
            raise ValueError(f"backoffice-payload te groot: {len(blocks)} ECB-bytes > 144")
        return bytes(nonce) + b"\x01\x00" + blocks

    def parse_response(self, data):
        """Notify op 5505: [sid4 BE][tijd4 BE][status1]; 0=OK, 1=fout (0x4832-pad)."""
        if len(data) < 9:
            return None
        sid = struct.unpack_from(">I", data, 0)[0]
        ts = struct.unpack_from(">I", data, 4)[0]
        return {"sid": sid, "ts": ts, "status": data[8]}


async def find(name):
    print("Scannen (10 s)… MoofMate/VanMoof-app afsluiten.")
    devs = await BleakScanner.discover(timeout=10.0)
    if not devs:
        sys.exit("Geen BLE-apparaten gevonden.")
    if not name:
        vm = [x for x in devs if x.name and x.name.startswith("ES3-")]
        if len(vm) != 1:
            sys.exit(f"Geen unieke ES3- gevonden ({len(vm)}): geef --name.")
        return vm[0]
    for dev in devs:
        if dev.name and dev.name.upper() == name.upper():
            return dev
    sys.exit(f"'{name}' niet gevonden.")


async def get_nonce(client):
    """Lees 5501: de nonce die 0x29438 verwacht in chars met de nonce-check-bit."""
    return (await client.read_gatt_char(CHALLENGE))[0:2]


async def authenticate(client, crypto, ukid):
    nonce = await client.read_gatt_char(CHALLENGE)
    block = bytearray(16); block[0:2] = nonce[0:2]
    payload = bytearray(crypto.enc(bytes(block))) + bytes([0, 0, 0, ukid])
    await client.write_gatt_char(KEY_INDEX, bytes(payload), response=True)


async def verify_key(client, crypto):
    try:
        d = crypto.dec(await client.read_gatt_char(CH("554a")))
    except Exception:
        return None
    if d is None:
        return False
    s = d.split(b"\0")[0]
    if len(s) > 0 and all(32 <= b < 127 for b in s):
        log("auth", f"fiets fw app/main (0x554a): {s.decode()!r}")
        return True
    return False


def log(kind, msg):
    print(f"[{kind}] {msg}")

async def fetch_bikelog(client, crypto):
    """Fiets-log (ringbuffer) lezen via 55c2/55c3 — identiek protocol als vanmoof_ble_cells.py.
    Wordt DIRECT na de upload over dezelfde verbinding gedraaid, zodat de manager-regels
    niet door de ~1 min ring-churn van de wake-cycli zijn weggeschreven."""
    size_raw = crypto.dec(await client.read_gatt_char(LOG_SIZE))
    if not size_raw:
        raise RuntimeError("55c2 (log-grootte) onleesbaar")
    size = int.from_bytes(size_raw[0:4], "big")
    if size == 0 or size > 0x100000:
        raise RuntimeError(f"rare log-grootte {size}")
    buf = []

    def on_block(_sender, data):
        d = crypto.dec(bytes(data))
        buf.append(d if d is not None else bytes(data))

    try:
        await client.stop_notify(LOG_BLOCK)      # diagnostiek-abonnement (raw-print) even vervangen
    except Exception:
        pass
    await client.start_notify(LOG_BLOCK, on_block)
    n_reqs = (size + 15 * 16 - 1) // (15 * 16)
    for i in range(n_reqs + 1):
        nonce = (await client.read_gatt_char(CHALLENGE))[0:2]
        data = struct.pack(">I", i * 15) + b"\xff"          # [offset in 16B-blokken][count=0xff]
        payload = bytearray(16); payload[0:2] = nonce; payload[2:2 + len(data)] = data
        await client.write_gatt_char(LOG_BLOCK, bytes(crypto.enc(bytes(payload))), response=True)
        await asyncio.sleep(0.3)
    try:
        await client.stop_notify(LOG_BLOCK)
    except Exception:
        pass
    return b"".join(buf)


LOG_KEYWORDS = ("Ask Header", "Calculated CRC", "invalid pack", "Disable Advertise", "Motorpcb",
                "No need", "Start motor", "F2806", "Update Ok", "Fail", "Wrong", "SSP MOTOR",
                "update", "PACK")


def bikelog_report(bikelog, tail=40):
    """Toon manager-/update-regels + de nieuwste tail-regels uit het fiets-log."""
    lines = bikelog.decode("latin-1").replace("\r\n", "\n").split("\n")
    hits = [ln for ln in lines if any(k in ln for k in LOG_KEYWORDS)]
    print(f"[log] {len(bikelog)} B, {len(lines)} regels; {len(hits)} manager-/update-regels:")
    for ln in hits:
        print(f"  {ln.strip()}")
    print(f"[log] laatste {tail} regels van de ring (nieuwste onderaan):")
    for ln in [l for l in lines if l.strip()][-tail:]:
        print(f"  {ln.strip()}")


async def do_probe(client, mkey):
    """Veilige backoffice-probe: type 4 = no-op (0x2F9E4 → altijd OK). Verifieert MKEY + formaat
    zonder iets te schrijven; een verkeerde MKEY valt af op de CRC16-check (0x456C → geen dispatch)."""
    bo = Backoffice(mkey)
    seen = []

    def on_notify(_h, data):
        r = bo.parse_response(data)
        seen.append(r if r else {"sid": None, "ts": None, "status": None, "raw": data})
        log("resp", f"raw {data.hex()}" + (f"  ->  sid=0x{r['sid']:08X} status={r['status']}"
                                           if r else "  (formaat != [sid4][ts4][status1])"))

    await client.start_notify(BACKOFFICE, on_notify)
    await asyncio.sleep(0.3)
    nonce = await get_nonce(client)
    msg = bo.build(4, b"", nonce)
    log("tx", f"backoffice type 4 (no-op probe): {len(msg)} bytes")
    await client.write_gatt_char(BACKOFFICE, msg, response=True)
    await asyncio.sleep(2.0)
    await client.stop_notify(BACKOFFICE)
    if not seen:
        print("\nGeen respons op 0x5505 ontvangen. Mogelijke oorzaken: MKEY onjuist (CRC16-fout "
              "→ stille afwijzing), geen notification-abonnement mogelijk, of fiets in slaap.")
        return False
    ok = any(r["status"] == 0 for r in seen)
    echoed = any(r.get("sid") == bo._sid for r in seen)
    print(f"\n{'MKEY en backoffice-formaat VERIFIED' if ok else 'Backoffice wees het bericht AF'} "
          f"({len(seen)} responsen).")
    print(f"  verzonden sid=0x{bo._sid:08X}; respons {'echoot onze sid' if echoed else 'echoot een ANDERE sid (nieuwere fw of ander formaat)'}")
    return ok


async def do_upload(client, crypto, mkey, packfile):
    data = open(packfile, "rb").read()
    if len(data) > 0x180000:
        sys.exit(f"Bestand te groot: {len(data)} > 0x180000 (max ext-flash regio).")
    crc = raw_crc32(data)
    size = len(data)
    pad = (-size) % 16                # fiets knipt af op `size` (0xA814: r5=min(chunklen,rest))
    log("pack", f"{packfile}: {size} bytes (0x{size:X}), CRC (raw) 0x{crc:08X}, pad {pad}")
    print(f"DOORGAAN HERSCHRIJFT DE PACK-REGIO (ext-flash 0x80000) VAN JE FIETS.")
    print("Bij stroomverlies/afbreken halverwege is de oude PACK weg (herstel = opnieuw uploaden of SPI-clip).")
    if input("Typ 'upload' om door te gaan: ").strip() != "upload":
        sys.exit("Afgebroken.")

    tx = data + b"\xff" * pad      # ECB vereist volle blokken (referentie padt met 0xff); fiets schrijft er precies `size`

    # --- Diagnostiek: MTU (bleak 3: mtu_size), listeners op ALLE notify-chars, timing ---
    t0 = time.monotonic()              # diagnostisch nulpunt (vóór abonnementen; START logt eigen marker)
    events = []

    def mk_on(c):
        def on(_h, d):
            off = (time.monotonic() - t0) * 1000
            print(f"[notify:{c}] +{off:8.0f} ms  raw {bytes(d).hex()}")
            events.append((off, c, bytes(d)))
        return on

    subs = []
    for svc in client.services:
        for ch in svc.characteristics:
            props = {str(p) for p in ch.properties}
            if props & {"notify", "indicate"}:
                c = ch.uuid[4:8] if len(ch.uuid) >= 8 else ch.uuid
                try:
                    await client.start_notify(ch, mk_on(c))
                    subs.append(c)
                except Exception as e:
                    log("diag", f"abonnement op {ch.uuid} mislukt: {type(e).__name__}: {e}")
    log("diag", f"listeners actief op: {', '.join(subs) if subs else 'GEEN'}")

    mtu = None
    for attr in ("mtu_size", "mtu"):    # bleak 3: mtu_size; oudere bleak: mtu
        try:
            mtu = getattr(client, attr)
            break
        except Exception:
            pass
    if mtu:
        log("diag", f"onderhandelde ATT-MTU: {mtu}")
    else:
        log("diag", "MTU niet opvraagbaar — conservatieve chunkgrootte")

    aes_mkey = Cipher(algorithms.AES(mkey), modes.ECB()).encryptor()

    # --- Start (5511): userkey-ECB [nonce2][filetype=0][size u32 BE][crc u32 BE][pad 0]
    # Geverifieerd: fw START-handler (intern 0xA420) leest size als b[1]<<24|b[2]<<16|b[3]<<8|b[4]
    # (BIG-endian, msg-offset 1) en crc idem op offset 5; limiet 0x180000, daarboven: log + DISCONNECT.
    # Referentie-implementatie: chwdt/vanmoof-tools update.py + pymoof (werkend op echte fietsen).
    nonce = await get_nonce(client)
    pl = bytearray(16)
    pl[0:2] = nonce
    struct.pack_into(">I", pl, 3, size)
    struct.pack_into(">I", pl, 7, crc)
    await client.write_gatt_char(OAD_START, crypto.enc(bytes(pl)), response=True)
    log("oad", f"start verstuurd (+{(time.monotonic() - t0) * 1000:.0f} ms, klok {time.monotonic():.3f}, filetype 0, size {size}, crc 0x{crc:08X})")
    await asyncio.sleep(2.0)           # wachtpunt: valt de link al spontaan na START?
    if not client.is_connected:
        await asyncio.sleep(1.0)       # late afwijzings-notifications nog meevangen
        sys.exit("Fiets verbrak binnen 2 s na START, vóór enige datawrite.\n"
                 f"=> start-bericht afgewezen; notifications totaal: {len(events)}\n"
                 "Ext-flash-regio mogelijk half-geformatteerd — opnieuw proberen vóór reboot.")
    log("diag", f"2 s na START: verbinding staat nog ({len(events)} notifications)")

    # --- Data (5512): MKEY-ECB chunks, plaintext over BLE ---
    # MTU onbekend of te klein -> 176 past altijd in MTU>=179 (veilige keuze na de eerdere disconnect).
    if mtu is not None and mtu >= 243:
        CHUNK = 0xF0
    else:
        CHUNK = ((mtu - 3) // 16) * 16 if mtu is not None else 176
        if CHUNK < 16:
            sys.exit(f"MTU {mtu} is te klein voor 16-byte ECB-blokken.")
    log("diag", f"chunkgrootte {CHUNK} (MTU {mtu if mtu else '?'})")
    enc_all = aes_mkey.update(tx)     # hele (gepadde) blob ECB -> identiek aan per-chunk encrypten
    sent = 0
    total = len(tx)
    try:
        while sent < total:
            n = min(CHUNK, total - sent)
            await client.write_gatt_char(OAD_DATA, enc_all[sent:sent + n], response=True)
            sent += n
            if sent == n:
                log("diag", f"eerste datawrite ({n} bytes) geaccepteerd")
            if sent % (CHUNK * 64) == 0 or sent == total:
                log("oad", f"{sent}/{total} bytes ({100 * sent // total}%)")
    except Exception as e:
        sys.exit(f"Upload afgebroken bij {sent}/{size} ({(time.monotonic() - t0) * 1000:.0f} ms na START): {e}\n"
                 f"notifications onderweg ontvangen: {len(events)}\n"
                 "De ext-flash-regio is nu mogelijk half-geformatteerd — upload opnieuw vóór reboot.")

    print(f"\nUpload klaar: {sent} bytes. De fiets controleert de transport-CRC intern.")
    print("Op bleware 2.04.01 is nog nooit een 5513-notify gezien; het fiets-log is de betrouwbare bron.")

    print("15 s wachten zodat CRC-check, pack-verwerking en manager-start kunnen draaien…")
    await asyncio.sleep(15)

    print("\n=== Fiets-log nu ophalen (zelfde verbinding, vóór de ring-churn) ===")
    try:
        bikelog = await fetch_bikelog(client, crypto)
        open("vanmoof_bmslog.bin", "wb").write(bikelog)
        print(f"Ruw log opgeslagen: vanmoof_bmslog.bin ({len(bikelog)} B)")
        bikelog_report(bikelog)
    except Exception as e:
        print(f"[log] ophalen mislukt ({type(e).__name__}: {e}); ga door met hold.")

    print("\nVerbinding nog 45 s openhouden zodat de motor-flash (indien gestart) kan doorlopen…")
    await asyncio.sleep(45)

    print("\n=== Fiets-log nogmaals (na het flash-venster) ===")
    try:
        bikelog2 = await fetch_bikelog(client, crypto)
        open("vanmoof_bmslog2.bin", "wb").write(bikelog2)
        bikelog_report(bikelog2)
    except Exception as e:
        print(f"[log] tweede ophalen mislukt ({type(e).__name__}: {e}).")
    st = {}
    for ev in events:
        if str(ev[1]).endswith("5513") and ev[2]:
            st[ev[2][0]] = ev[0]
    if 0x13 in st:
        print(f"[5513] 0x13 CRC-OK gezien (+{st[0x13]:.0f} ms) -> pack_ingest draait; mainware-manager hoort te starten.")
    if 0x16 in st:
        print(f"[5513] 0x16 CRC-MISMATCH (+{st[0x16]:.0f} ms) -> pack is NIET verwerkt; CRC-berekening klopt niet.")
    if not st:
        print("[5513] geen status-notify ontvangen (normaal op deze bleware-build).")
    print("Packs zonder bleware-lid passeren zonder versie-gate; mét bleware-lid geldt: bleware-versie > 2.04.00")


async def dump_gatt(client):
    """Volledige GATT-tabel + Device Information Service (fw-versie van de fiets lezen)."""
    print("--- GATT-tabel ---")
    for svc in client.services:
        if not svc.characteristics:
            continue
        print(f"service {svc.uuid}")
        for ch in svc.characteristics:
            props = ",".join(sorted(str(p) for p in ch.properties))
            print(f"  char {ch.uuid}  [{props}]")
    print("--- Device Information Service ---")
    names = {"00002a24-0000-1000-8000-00805f9b34fb": "Model",
             "00002a26-0000-1000-8000-00805f9b34fb": "Firmware-versie",
             "00002a27-0000-1000-8000-00805f9b34fb": "Hardware-revisie",
             "00002a28-0000-1000-8000-00805f9b34fb": "Software-versie",
             "00002a29-0000-1000-8000-00805f9b34fb": "Fabrikant",
             "00002a50-0000-1000-8000-00805f9b34fb": "PnP-ID"}
    for uuid, nm in names.items():
        try:
            d = await client.read_gatt_char(uuid)
            print(f"  {nm} (0x{uuid[4:8]}): {bytes(d)!r}")
        except Exception as e:
            print(f"  {nm} (0x{uuid[4:8]}): niet leesbaar ({type(e).__name__})")


async def do_diag(client, crypto, mkey):
    """Read-only diagnose: GEEN writes, opent geen flash-sessie. Leest 5513 (OAD-status),
    5551 (versietabel), 5562 (module-state), 554a/554b (fw-versie), 5549 en de tien
    6aa5xxxx-info-chars van de nieuwere fw; probeert userkey/MKEY-ontcijfering.
    Post-reboot-verificatie: 5562 = 0105 (idle), 5551/554b mainware intern f4 03 09 01."""
    await dump_gatt(client)
    print("--- Read-only diagnose ---")
    targets = [CH("5513"), CH("5549"), CH("5551"), CH("5562"), CH("554a"), CH("554b"),
               CH("5541"), CH("5542"), CH("5543"), CH("5544"), CH("5546"), CH("5547"),
               CH("5548"), CH("554c"), CH("554d"), CH("554e"), CH("554f"), CH("5550"),
               CH("5552"), CH("5561"), CH("5563"), CH("5564"), CH("5565"), CH("5567"),
               CH("5568"), CH("5569"), CH("5581"), CH("5582"), CH("5583"), CH("5584"),
               CH("55c1"), CH("55c2"), CH("55c3")]
    for svc in client.services:
        for ch in svc.characteristics:
            if ch.uuid.startswith("6aa5") and "read" in {str(p) for p in ch.properties}:
                targets.append(ch.uuid)
    for uuid in targets:
        label = f"0x{uuid[4:8]}" if uuid.startswith("6acc") else uuid[:13]
        try:
            d = bytes(await client.read_gatt_char(uuid))
        except Exception as e:
            print(f"  {label}: niet leesbaar ({type(e).__name__}: {e})")
            continue
        line = f"  {label}: {d.hex()}"
        printable = d.split(b"\0")[0]
        if printable and all(32 <= b < 127 for b in printable):
            line += f"   ascii={printable!r}"
        if len(d) % 16 == 0 and len(d) > 0:
            for naam, dec in (("userkey", crypto.dec(d)), ("mkey", _ecb_dec(mkey, d))):
                if dec:
                    txt = dec.split(b"\0")[0]
                    if txt and all(32 <= b < 127 for b in txt):
                        line += f"   {naam}-dec={txt!r}"
                    else:
                        line += f"   {naam}-dec={dec.hex()}"
            if len(d) > 16:
                blokken = []
                for i in range(0, len(d), 16):
                    b = crypto.dec(d[i:i+16]).split(b"\0")[0]
                    blokken.append(b.decode(errors="replace")
                                   if all(32 <= x < 127 for x in b) else b.hex())
                line += f"   userkey-blokken={blokken}"
        print(line)
    print("Klaar — niets geschreven.")


def _ecb_dec(key, data):
    d = Cipher(algorithms.AES(key), modes.ECB()).decryptor()
    return d.update(data) + d.finalize()


async def main():
    ap = argparse.ArgumentParser(description="VanMoof S3 custom-firmware-upload (OAD 5510/5511/5512 + backoffice 5505)")
    ap.add_argument("--bike-json"); ap.add_argument("--key"); ap.add_argument("--user-key-id", type=int)
    ap.add_argument("--name", help="naam van de fiets (bv. ES3-200B16CB3EBF) — aanbevolen!")
    ap.add_argument("--mkey", default=DEFAULT_MKEY,
                    help=f"MKEY hex (standaard: slot 126 uit de dump = {DEFAULT_MKEY})")
    ap.add_argument("--probe", action="store_true", help="veilige backoffice-probe (type 4 no-op; schrijft niets)")
    ap.add_argument("--diag", action="store_true", help="read-only diagnose (GATT + 5513 + 6aa5xxxx-chars; schrijft niets)")
    ap.add_argument("--upload", metavar="PACK", help="PACK-bestand uploaden naar ext-flash 0x80000")
    a = ap.parse_args()

    if not a.probe and not a.upload and not a.diag:
        sys.exit("Geef --probe, --diag en/of --upload <bestand>.")
    try:
        mkey = bytes.fromhex(a.mkey)
    except ValueError:
        sys.exit("--mkey is geen geldige hex.")
    if len(mkey) != 16:
        sys.exit("MKEY moet 16 bytes (32 hex tekens) zijn.")
    key, ukid = load_creds(a)
    crypto = Crypto(key)
    dev = await find(a.name)

    def on_disc(_c):
        print(f"\n[diag] LINK VERBROKEN (callback op klok {time.monotonic():.3f})")
    async with BleakClient(dev, disconnected_callback=on_disc) as client:
        # Start vereist userkey-auth (char 5511 perms 0x0D bevat de auth-bit);
        # de backoffice zelf niet, maar auth geeft ook toegang tot diagnose/lees-chars.
        await authenticate(client, crypto, ukid)
        ok = await verify_key(client, crypto)
        if ok is False:
            sys.exit("Userkey lijkt onjuist (0x554a ontsleutelt niet naar tekst). Niets geschreven.")
        if a.probe:
            if not await do_probe(client, mkey):
                sys.exit(1)
        if a.diag:
            await do_diag(client, crypto, mkey)
        if a.upload:
            await do_upload(client, crypto, mkey, a.upload)


if __name__ == "__main__":
    try:
        asyncio.run(main())
    except KeyboardInterrupt:
        print("\nGestopt.")
    except SystemExit:
        raise
    except Exception as e:
        sys.exit(f"BLE-fout ({type(e).__name__}): {e}")