CAN bus and automotive protocols

/* Bit timing for 500 kbps on STM32F446 with APB1 = 45 MHz
   Prescaler = 5 → TQ clock = 45MHz / 5 = 9 MHz
   Bit time = 1 / 500kbps = 2000 ns = 18 TQ at 9 MHz
   Segments: SyncSeg=1 + PropSeg+PhaseSeg1=11 + PhaseSeg2=6 = 18 TQ
   Sample point = (1 + 11) / 18 = 66.7%  (target: 75–80%)           */

CAN_InitTypeDef can_init = {
    .Prescaler        = 5,     /* APB1 / Prescaler = TQ clock */
    .Mode             = CAN_MODE_NORMAL,
    .SyncJumpWidth    = CAN_SJW_1TQ,
    .TimeSeg1         = CAN_BS1_11TQ,
    .TimeSeg2         = CAN_BS2_6TQ,
    .TimeTriggeredMode= DISABLE,
    .AutoBusOff       = ENABLE,    /* auto recover from bus-off */
    .AutoWakeUp       = DISABLE,
    .AutoRetransmission = ENABLE,
    .ReceiveFifoLocked  = DISABLE,
    .TransmitFifoPriority = DISABLE,
};

/* Verify: use an online CAN bit timing calculator to cross-check.
   bittiming.can-wiki.info is the standard tool.                       */

/* TX — request transmission of one frame */
bool CAN_Transmit(uint32_t id, uint8_t *data, uint8_t len) {
    uint32_t mailbox;
    CAN_TxHeaderTypeDef hdr = {
        .StdId = id,
        .IDE   = CAN_ID_STD,
        .RTR   = CAN_RTR_DATA,
        .DLC   = len,
        .TransmitGlobalTime = DISABLE,
    };
    HAL_StatusTypeDef s = HAL_CAN_AddTxMessage(&hcan1, &hdr, data, &mailbox);
    return (s == HAL_OK);
}

/* RX FIFO0 interrupt handler */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
    CAN_RxHeaderTypeDef hdr;
    uint8_t data[8];
    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &hdr, data) == HAL_OK) {
        /* Route to upper layer by ID */
        CAN_RouteFrame(hdr.StdId, data, hdr.DLC);
    }
}

/* Error interrupt — log and recover */
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) {
    uint32_t err = HAL_CAN_GetError(hcan);
    LOG_ERROR("CAN error: 0x%08X  TEC=%lu  REC=%lu",
              err,
              (hcan->Instance->ESR >> 16) & 0xFF,  /* TEC */
              (hcan->Instance->ESR >>  8) & 0xFF);  /* REC */
    if (err & HAL_CAN_ERROR_BOF) {
        /* Bus-off: auto-recovery re-initialises after 128×11 recessive bits */
        LOG_WARN("Bus-off detected — waiting for auto-recovery");
    }
}

/* Accept: IDs 0x100–0x1FF → FIFO0 (diagnostic frames)
           IDs 0x200–0x2FF → FIFO0 (sensor data)
           ID  0x7DF exactly → FIFO1 (OBD-II functional address)
           ID  0x7E0 exactly → FIFO1 (OBD-II physical address)        */

/* Bank 0: 32-bit mask — accept 0x100–0x1FF */
CAN_FilterTypeDef f = {
    .FilterIdHigh         = 0x100 << 5,
    .FilterIdLow          = 0x0000,
    .FilterMaskIdHigh     = 0x700 << 5,   /* top 3 bits must match */
    .FilterMaskIdLow      = 0x0000,
    .FilterFIFOAssignment = CAN_FILTER_FIFO0,
    .FilterBank           = 0,
    .FilterMode           = CAN_FILTERMODE_IDMASK,
    .FilterScale          = CAN_FILTERSCALE_32BIT,
    .FilterActivation     = ENABLE,
};
HAL_CAN_ConfigFilter(&hcan1, &f);

/* Bank 1: 32-bit mask — accept 0x200–0x2FF */
f.FilterIdHigh     = 0x200 << 5;
f.FilterMaskIdHigh = 0x700 << 5;
f.FilterBank       = 1;
HAL_CAN_ConfigFilter(&hcan1, &f);

/* Bank 2: 16-bit list — accept 0x7DF and 0x7E0 exactly → FIFO1 */
f.FilterIdHigh         = 0x7DF << 5;
f.FilterIdLow          = 0x7E0 << 5;
f.FilterMaskIdHigh     = 0x0000;
f.FilterMaskIdLow      = 0x0000;
f.FilterFIFOAssignment = CAN_FILTER_FIFO1;
f.FilterBank           = 2;
f.FilterMode           = CAN_FILTERMODE_IDLIST;
f.FilterScale          = CAN_FILTERSCALE_16BIT;
HAL_CAN_ConfigFilter(&hcan1, &f);

/* FDCAN configuration for STM32G474RE
   Nominal: 500 kbps, Data: 2 Mbps
   Clock: FDCAN kernel clock = 80 MHz (from PLL)                       */

FDCAN_HandleTypeDef hfdcan1;

void FDCAN_Init(void) {
    hfdcan1.Instance                  = FDCAN1;
    hfdcan1.Init.ClockDivider         = FDCAN_CLOCK_DIV1;
    hfdcan1.Init.FrameFormat          = FDCAN_FRAME_FD_BRS;  /* CAN-FD with BRS */
    hfdcan1.Init.Mode                 = FDCAN_MODE_NORMAL;
    hfdcan1.Init.AutoRetransmission   = ENABLE;
    hfdcan1.Init.TransmitPause        = ENABLE;
    hfdcan1.Init.ProtocolException    = ENABLE;

    /* Nominal bit timing: 500 kbps @ 80 MHz
       Prescaler=1, SyncSeg=1, PropSeg+PhaseSeg1=139, PhaseSeg2=20
       Bit time = (1+139+20)/80MHz = 160/80MHz = 2µs = 500 kbps        */
    hfdcan1.Init.NominalPrescaler     = 1;
    hfdcan1.Init.NominalSyncJumpWidth = 20;
    hfdcan1.Init.NominalTimeSeg1      = 139;
    hfdcan1.Init.NominalTimeSeg2      = 20;

    /* Data bit timing: 2 Mbps @ 80 MHz
       Prescaler=1, SyncSeg=1, PropSeg+PhaseSeg1=29, PhaseSeg2=10
       Bit time = (1+29+10)/80MHz = 40/80MHz = 500 ns = 2 Mbps         */
    hfdcan1.Init.DataPrescaler        = 1;
    hfdcan1.Init.DataSyncJumpWidth    = 4;
    hfdcan1.Init.DataTimeSeg1         = 29;
    hfdcan1.Init.DataTimeSeg2         = 10;

    /* TDC: enable for 2 Mbps data rate */
    hfdcan1.Init.TxDelayCompensation  = ENABLE;
    hfdcan1.Init.TxDelayCompensationOffset = 5;   /* transceiver-dependent */
    hfdcan1.Init.TxDelayCompensationFilter = 0;

    HAL_FDCAN_Init(&hfdcan1);
    HAL_FDCAN_Start(&hfdcan1);
}

/* Transmit a CAN-FD frame with 64-byte payload */
void FDCAN_Transmit64(uint32_t id, uint8_t *data) {
    FDCAN_TxHeaderTypeDef hdr = {
        .Identifier          = id,
        .IdType              = FDCAN_STANDARD_ID,
        .TxFrameType         = FDCAN_DATA_FRAME,
        .DataLength          = FDCAN_DLC_BYTES_64,
        .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
        .BitRateSwitch       = FDCAN_BRS_ON,   /* switch to 2 Mbps for data */
        .FDFormat            = FDCAN_FD_CAN,
        .TxEventFifoControl  = FDCAN_NO_TX_EVENTS,
        .MessageMarker       = 0,
    };
    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &hdr, data);
}

#!/usr/bin/env python3
"""Simulate a multi-node CAN bus — send heartbeats and monitor responses"""
import can, time, threading

bus = can.Bus(interface='socketcan', channel='can0', bitrate=500000)

def heartbeat_task(node_id: int, period: float):
    """Broadcast heartbeat every `period` seconds"""
    msg = can.Message(
        arbitration_id = 0x700 + node_id,
        data           = [0x05],   # operational
        is_extended_id = False
    )
    task = bus.send_periodic(msg, period)
    return task

def monitor(duration: float):
    """Log all frames for `duration` seconds"""
    deadline = time.monotonic() + duration
    while time.monotonic() < deadline:
        msg = bus.recv(timeout=0.1)
        if msg:
            node = msg.arbitration_id - 0x700 if msg.arbitration_id >= 0x700 else 0
            print(f"  [{msg.timestamp:.3f}]  ID=0x{msg.arbitration_id:03X}  "
                  f"DLC={msg.dlc}  DATA={msg.data.hex()}")

# Start heartbeats for two simulated nodes
t1 = heartbeat_task(node_id=1, period=0.1)
t2 = heartbeat_task(node_id=2, period=0.25)

print("Monitoring bus for 10 seconds...")
monitor(duration=10.0)

t1.stop()
t2.stop()
bus.shutdown()

typedef enum {
    ISOTP_TX_IDLE,
    ISOTP_TX_SEND_FF,      /* send first frame, wait for FC */
    ISOTP_TX_WAIT_FC,      /* waiting for flow control from receiver */
    ISOTP_TX_SEND_CF,      /* sending consecutive frames */
    ISOTP_TX_COMPLETE,
    ISOTP_TX_ERROR,
} ISOTP_TxState_t;

typedef struct {
    ISOTP_TxState_t state;
    const uint8_t  *payload;
    uint32_t        total_len;
    uint32_t        bytes_sent;
    uint8_t         sn;          /* sequence number 1–F */
    uint8_t         bs;          /* block size from FC */
    uint8_t         stmin_ms;    /* separation time from FC */
    uint32_t        fc_timeout;  /* N_Bs timer */
    uint8_t         frames_in_block; /* frames sent since last FC */
} ISOTP_TxCtx_t;

/* Call from CAN TX complete interrupt or application loop */
void ISOTP_TX_Tick(ISOTP_TxCtx_t *ctx) {
    switch (ctx->state) {
    case ISOTP_TX_SEND_FF: {
        uint8_t ff[8] = {0};
        ff[0] = 0x10 | ((ctx->total_len >> 8) & 0x0F);
        ff[1] = ctx->total_len & 0xFF;
        memcpy(&ff[2], ctx->payload, 6);
        CAN_Transmit(ISOTP_TX_ID, ff, 8);
        ctx->bytes_sent = 6;
        ctx->fc_timeout = HAL_GetTick() + 1000;  /* N_Bs = 1000ms */
        ctx->state = ISOTP_TX_WAIT_FC;
        break;
    }
    case ISOTP_TX_WAIT_FC:
        if (HAL_GetTick() > ctx->fc_timeout) {
            ctx->state = ISOTP_TX_ERROR;  /* N_Bs timeout */
        }
        break;
    case ISOTP_TX_SEND_CF: {
        uint8_t cf[8] = {0};
        cf[0] = 0x20 | (ctx->sn & 0x0F);
        uint32_t remaining = ctx->total_len - ctx->bytes_sent;
        uint8_t chunk = (remaining > 7) ? 7 : (uint8_t)remaining;
        memcpy(&cf[1], ctx->payload + ctx->bytes_sent, chunk);
        CAN_Transmit(ISOTP_TX_ID, cf, 8);
        ctx->bytes_sent += chunk;
        ctx->sn = (ctx->sn + 1) & 0x0F;
        ctx->frames_in_block++;
        if (ctx->bytes_sent >= ctx->total_len) {
            ctx->state = ISOTP_TX_COMPLETE;
        } else if (ctx->bs > 0 && ctx->frames_in_block >= ctx->bs) {
            ctx->frames_in_block = 0;
            ctx->fc_timeout = HAL_GetTick() + 1000;
            ctx->state = ISOTP_TX_WAIT_FC;
        }
        break;
    }
    default: break;
    }
}

/* UDS service dispatch table */
typedef struct {
    uint8_t service_id;
    void (*handler)(const uint8_t *req, uint16_t req_len,
                          uint8_t *resp, uint16_t *resp_len);
} UDS_ServiceEntry_t;

static const UDS_ServiceEntry_t s_services[] = {
    { 0x10, UDS_DiagnosticSessionControl },
    { 0x27, UDS_SecurityAccess           },
    { 0x31, UDS_RoutineControl           },
    { 0x34, UDS_RequestDownload          },
    { 0x36, UDS_TransferData             },
    { 0x37, UDS_RequestTransferExit      },
};

void UDS_ProcessRequest(const uint8_t *req, uint16_t len,
                              uint8_t *resp, uint16_t *resp_len) {
    if (len == 0) return;
    uint8_t sid = req[0];
    for (size_t i = 0; i < ARRAY_SIZE(s_services); i++) {
        if (s_services[i].service_id == sid) {
            s_services[i].handler(req, len, resp, resp_len);
            return;
        }
    }
    /* Service not supported */
    resp[0] = 0x7F;   /* negative response */
    resp[1] = sid;
    resp[2] = 0x11;   /* NRC: serviceNotSupported */
    *resp_len = 3;
}

/* Negative response helper */
void UDS_NegativeResponse(uint8_t sid, uint8_t nrc,
                                uint8_t *resp, uint16_t *resp_len) {
    resp[0] = 0x7F;
    resp[1] = sid;
    resp[2] = nrc;
    *resp_len = 3;
}

/* NRC codes used in firmware update:
   0x11 serviceNotSupported
   0x12 subFunctionNotSupported
   0x13 incorrectMessageLengthOrInvalidFormat
   0x22 conditionsNotCorrect        (e.g. not in programming session)
   0x24 requestSequenceError        (e.g. RequestDownload before erase)
   0x25 noResponseFromSubnetComponent
   0x31 requestOutOfRange
   0x33 securityAccessDenied
   0x35 invalidKey                  (wrong key in SecurityAccess)
   0x36 exceededNumberOfAttempts    (too many wrong keys)
   0x37 requiredTimeDelayNotExpired (too many wrong keys — lockout)
   0x70 uploadDownloadNotAccepted
   0x72 generalProgrammingFailure   (flash write error)
   0x73 wrongBlockSequenceCounter   */

#!/usr/bin/env python3
"""
flash_can.py — flash firmware to STM32 bootloader via CAN/ISO-TP/UDS

Usage: python flash_can.py --channel can0 --bitrate 500000 firmware.fwu
       python flash_can.py --channel can0 --bitrate 500000 --fd firmware.fwu
"""
import udsoncan, can, isotp, struct, zlib, argparse, sys, time
from udsoncan.connections import PythonIsoTpConnection
from udsoncan.services import *
from pathlib import Path

# Node addresses
ECU_TX_ID = 0x7E0   # tester → ECU (physical addressing)
ECU_RX_ID = 0x7E8   # ECU → tester

SECRET_XOR = 0xA5B6C7D8   # must match STM32 bootloader

def crc32(data: bytes) -> int:
    return zlib.crc32(data) & 0xFFFFFFFF

def derive_key(seed: int) -> int:
    return (seed ^ SECRET_XOR) & 0xFFFFFFFF

def flash(channel: str, bitrate: int, image_path: str, use_fd: bool) -> bool:
    data = Path(image_path).read_bytes()
    img_crc = crc32(data)
    total = len(data)
    block_size = 512   # adjusted after 0x34 response

    print(f"Image  : {image_path} ({total} bytes, CRC=0x{img_crc:08X})")

    # Set up CAN bus + ISO-TP
    bus = can.Bus(interface='socketcan', channel=channel,
                  bitrate=bitrate, fd=use_fd)
    tp_addr = isotp.Address(isotp.AddressingMode.Normal_11bits,
                             txid=ECU_TX_ID, rxid=ECU_RX_ID)
    tp_layer = isotp.CanStack(bus, address=tp_addr)
    conn = PythonIsoTpConnection(tp_layer)

    config = udsoncan.Config({
        'request_timeout': 5,
        'p2_timeout':      0.05,   # 50ms P2
        'p2_star_timeout': 5.0,    # 5s P2* (for slow operations)
        's3_client_timeout': 4.0,  # S3: send TesterPresent to keep session alive
        'use_server_timing': True,
    })

    with udsoncan.Client(conn, config=config) as client:

        # 1. Enter programming session
        print("[1/6] Opening programming session...")
        client.change_session(DiagnosticSessionControl.Session.programmingSession)
        print("      OK")

        # 2. Security access
        print("[2/6] Security access...")
        result = client.request_seed(0x01)
        seed = struct.unpack('>I', result.service_data.seed)[0]
        key  = derive_key(seed)
        client.send_key(0x02, struct.pack('>I', key))
        print(f"      OK  (seed=0x{seed:08X})")

        # 3. Erase flash
        print("[3/6] Erasing flash...")
        client.start_routine(0xFF00)   # erase routine
        print("      OK")

        # 4. RequestDownload
        print("[4/6] Request download...")
        app_start = 0x0800A000   # must match bootloader APP_START
        resp = client.request_download(
            udsoncan.MemoryLocation(app_start, total, memorysize_length=4, address_length=4),
            dfi=udsoncan.DataFormatIdentifier(compression=0, encryption=0)
        )
        block_size = resp.service_data.max_length
        print(f"      OK  (maxBlockLen={block_size})")

        # 5. TransferData
        print("[5/6] Transferring firmware...")
        seq = 1
        offset = 0
        while offset < total:
            chunk = data[offset : offset + block_size]
            client.transfer_data(seq, chunk)
            offset += len(chunk)
            seq = (seq + 1) & 0xFF or 1   # wraps 0xFF → 01 (not 00)
            pct = offset / total * 100
            bar = '#' * int(pct / 2) + '.' * (50 - int(pct / 2))
            print(f"\r      [{bar}] {pct:5.1f}%", end='', flush=True)
        print("\n      OK")

        # 6. RequestTransferExit
        print("[6/6] Verifying and committing...")
        client.request_transfer_exit(struct.pack('>I', img_crc))
        print("      OK  — target rebooting")

    bus.shutdown()
    return True

if __name__ == '__main__':
    p = argparse.ArgumentParser()
    p.add_argument('--channel', default='can0')
    p.add_argument('--bitrate', type=int, default=500000)
    p.add_argument('--fd', action='store_true')
    p.add_argument('image')
    args = p.parse_args()
    sys.exit(0 if flash(args.channel, args.bitrate, args.image, args.fd) else 1)