ULS-DeCoNet CAN Transport Specification

_io_op_result sendPacket() {
    uint8_t framecntr = 0;
    uint8_t *pxPld = ifTxPacket->pld;
    
    // Check if single-frame message
    if (ifTxPacket->cmd < 16) {
        if (ifTxLen > 12) return IO_ERROR;
        
        // Single-frame transmission
        _can_mail mail;
        mail.packet.raw.data[0] = ifTxPacket->cmd;
        mail.packet.raw.data[1] = ifTxPacket->src_lid;
        memcpy(&mail.packet.raw.data[2], ifTxPacket->pld, ifTxLen - 2);
        mail.len = (ifTxLen < 4) ? 0 : ifTxLen - 4;
        
        send_can_frame(&mail, CAN_PRIORITY_HIGH);
        return IO_OK;
    }
    
    // Multi-frame transmission
    ifTxLen -= 2;  // Subtract CMD and DID
    
    while (ifTxLen > 0) {
        _can_mail mail;
        mail.packet.raw.data[0] = ifTxPacket->cmd;
        mail.packet.raw.data[1] = ifTxPacket->src_lid;
        mail.packet.raw.data[2] = framecntr++;
        
        // Copy up to 5 bytes payload
        uint32_t len = (ifTxLen > 5) ? 5 : ifTxLen;
        for (uint32_t i = 0; i < len; i++) {
            mail.packet.raw.data[3 + i] = *pxPld++;
            ifTxLen--;
        }
        
        // Mark last frame
        if (ifTxLen == 0) {
            mail.packet.raw.data[2] |= 0x80;
        }
        
        mail.len = len + 2;  // Data length code
        
        if (!send_can_frame(&mail, CAN_PRIORITY_NORMAL)) {
            return IO_BUFFER_FULL;
        }
    }
    
    return IO_OK;
}

_io_op_result receivePacket() {
    _can_mail mail;
    
    while (read_can_frame(&mail)) {
        uint8_t did = mail.packet.raw.data[1] & 0x3F;
        uint8_t cmd = mail.packet.raw.data[0];
        
        // Update device timeout
        _devs[did].timeout = CN_CAN_DEV_TIMEOUT;
        
        // Single-frame message
        if (cmd < 16) {
            ifRxPacket->cmd = cmd;
            ifRxPacket->src_lid = did;
            memcpy(ifRxPacket->pld, &mail.packet.raw.data[2], mail.len + 2);
            ifRxLen = 4 + mail.len;
            return IO_OK;
        }
        
        // Multi-frame message
        uint8_t frame_num = mail.packet.raw.data[2] & 0x7F;
        bool is_last = (mail.packet.raw.data[2] & 0x80) != 0;
        
        // Store frame in pool
        store_frame_in_pool(did, frame_num, &mail);
        
        if (is_last) {
            // Attempt to assemble message
            return assemble_message(did, frame_num);
        }
    }
    
    return IO_NO_DATA;
}

_io_op_result assemble_message(uint8_t did, uint8_t last_frame) {
    ifRxPacket->cmd = get_cmd_from_pool(did);
    ifRxPacket->src_lid = did;
    
    uint32_t expected_frames = last_frame + 1;
    uint32_t expected_len = last_frame * 5 + get_last_frame_len(did);
    
    ifRxLen = 2;  // CMD + DID
    
    // Verify all frames present
    for (uint32_t frame = 0; frame <= last_frame; frame++) {
        if (!frame_exists_in_pool(did, frame)) {
            // Missing frame
            log_missing_frames(did, expected_frames);
            clean_pool(did);
            return IO_ERROR;
        }
        
        // Copy frame payload
        copy_frame_payload(did, frame, &ifRxPacket->pld[frame * 5]);
        ifRxLen += get_frame_payload_len(did, frame);
    }
    
    // Verify length
    if (ifRxLen != expected_len + 2) {
        clean_pool(did);
        return IO_ERROR;
    }
    
    // Success - clean up pool
    clean_pool(did);
    return IO_OK;
}

#define CN_CAN_POOL_SIZE 256  // Frame storage slots
#define CN_CAN_DEV_SIZE 64    // Device tracking entries

typedef struct {
    uint32_t timeout;
    uint32_t start;
    uint32_t size;
    uint32_t len;
} _cn_can_dev;

_can_mail _pool[CN_CAN_POOL_SIZE];
_cn_can_dev _devs[CN_CAN_DEV_SIZE];

bool store_frame_in_pool(uint8_t did, uint8_t frame_num, _can_mail *mail) {
    // Find free slot
    uint32_t slot = allocate_pool_slot();
    if (slot == INVALID_SLOT) {
        // Pool full - trigger cleanup
        clean_expired_frames();
        slot = allocate_pool_slot();
        if (slot == INVALID_SLOT) {
            return false;  // Still full
        }
    }
    
    // Store frame
    memcpy(&_pool[slot], mail, sizeof(_can_mail));
    
    // Mark slot as used
    _pool[slot].packet.raw.data[0] = mail->packet.raw.data[0];
    
    return true;
}

uint32_t allocate_pool_slot() {
    for (uint32_t i = 0; i < CN_CAN_POOL_SIZE; i++) {
        _iempty++;
        if (_iempty >= CN_CAN_POOL_SIZE) {
            _iempty = 0;
        }
        
        // Check if slot is empty (marked with 0xFF)
        if (_pool[_iempty].packet.raw.data[0] == 0xFF) {
            return _iempty;
        }
    }
    return INVALID_SLOT;
}

#define CN_CAN_DEV_TIMEOUT (CN_CAN_POOL_SIZE / 2)  // 128 frame times

void task(uint32_t dtms) {
    // Decrement all device timeouts
    for (uint32_t i = 0; i < CN_CAN_DEV_SIZE; i++) {
        if (_devs[i].timeout > 0) {
            _devs[i].timeout--;
        }
    }
    
    // On device timeout, clean its frames
    for (uint32_t i = 0; i < CN_CAN_DEV_SIZE; i++) {
        if (_devs[i].timeout == 0 && _devs[i].size > 0) {
            clean_device_frames(i);
        }
    }
}

void clean_expired_frames() {
    for (uint32_t i = 0; i < CN_CAN_POOL_SIZE; i++) {
        if (_pool[i].packet.raw.data[0] == 0xFF) {
            continue;  // Already free
        }
        
        uint8_t did = _pool[i].packet.raw.data[1] & 0x3F;
        
        if (_devs[did].timeout == 0) {
            // Device timed out - free frame
            _pool[i].packet.raw.data[0] = 0xFF;
        }
    }
}

uint32_t clean_device_frames(uint8_t did) {
    uint32_t count = 0;
    
    for (uint32_t i = 0; i < CN_CAN_POOL_SIZE; i++) {
        if (_pool[i].packet.raw.data[0] == 0xFF) {
            continue;
        }
        
        uint8_t frame_did = _pool[i].packet.raw.data[1] & 0x3F;
        
        if (frame_did == did) {
            _pool[i].packet.raw.data[0] = 0xFF;
            count++;
        }
    }
    
    return count;
}

uint32_t get_pool_free_count() {
    uint32_t count = 0;
    for (uint32_t i = 0; i < CN_CAN_POOL_SIZE; i++) {
        if (_pool[i].packet.raw.data[0] == 0xFF) {
            count++;
        }
    }
    return count;
}

void check_pool_health() {
    uint32_t free = get_pool_free_count();
    
    if (free < CN_CAN_POOL_SIZE / 4) {
        // Less than 25% free - warning
        log_warning("CAN pool low: %d free", free);
    }
    
    if (free == 0) {
        // Pool exhausted - critical
        log_error("CAN pool exhausted!");
        emergency_cleanup();
    }
}

typedef enum {
    _CAN_MAIL_PRIORITY_HI = 0,      // Critical messages
    _CAN_MAIL_PRIORITY_NORMAL = 1,   // Regular data
    _CAN_MAIL_PRIORITY_LOW = 2       // Background tasks
} _can_mail_priority;

// Check for missing frames in sequence
for (uint32_t i = 0; i <= last_frame; i++) {
    if (!frame_exists(did, i)) {
        log_error("Missing frame %d from device %d", i, did);
        _missedFrames[i] = 1;
    }
}

if (allocate_pool_slot() == INVALID_SLOT) {
    log_error("CAN pool exhausted!");
    return IO_BUFFER_FULL;
}

1. Trigger emergency cleanup
2. Remove oldest/timed-out frames
3. If still full, reject new frames
4. Report error to higher layer

void handle_device_timeout(uint8_t did) {
    log_warning("Device %d timeout", did);
    
    // Clean all frames from device
    uint32_t cleaned = clean_device_frames(did);
    log_info("Cleaned %d frames from device %d", cleaned, did);
    
    // Notify higher layer
    notify_device_disconnected(did);
}

typedef struct {
    uint32_t tx_ok;
    uint32_t tx_errors;
    uint32_t rx_ok;
    uint32_t rx_errors;
    uint32_t missing_frames;
    uint32_t pool_overflows;
    uint32_t device_timeouts;
} can_stats_t;

class ULSBusConnectionCAN : public ULSBusConnection, DrvCAN {
public:
    ULSBusConnectionCAN(_drvcan_handle_td* handler, 
                       ULSDBase* dev,
                       ULSBusConnectionsList* connections, 
                       const char* name, 
                       uint8_t id, 
                       uint8_t cid);
    
    // Configuration
    bool open() override;
    void setSpeed(uint32_t speed);
    
    // Task processing
    void task(uint32_t dtms) override;
    
    // Frame I/O
    _io_op_result sendPacket() override;
    _io_op_result receivePacket() override;
    
    // Statistics
    uint32_t poolSize();
    
private:
    _can_mail _pool[CN_CAN_POOL_SIZE];
    _cn_can_dev _devs[CN_CAN_DEV_SIZE];
    uint32_t _iempty;
    uint32_t _missedFrames[128];
    
    bool allocate();
    void clean();
    uint32_t clean(uint8_t id);
    void setTimeouts();
};

typedef struct {
    union {
        struct {
            uint32_t id;           // CAN identifier
            uint8_t data[8];       // Payload
            uint8_t len;           // DLC (0-8)
        } raw;
        // Alternative access methods
    } packet;
} _can_mail;

class DrvCAN {
protected:
    _drvcan_handle_td* _handler;
    
public:
    bool open();
    void setSpeed(uint32_t speed);
    bool send(_can_mail* mail, _can_mail_priority priority);
    bool read(_can_mail* mail);
    void txTask();  // Process TX queue
};

// Initialize CAN peripheral
FDCAN_HandleTypeDef hfdcan1;
hfdcan1.Instance = FDCAN1;
hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan1.Init.AutoRetransmission = ENABLE;
hfdcan1.Init.TransmitPause = DISABLE;
hfdcan1.Init.ProtocolException = ENABLE;

// Bit timing (1 Mbps @ 80 MHz APB clock)
hfdcan1.Init.NominalPrescaler = 4;
hfdcan1.Init.NominalSyncJumpWidth = 1;
hfdcan1.Init.NominalTimeSeg1 = 15;
hfdcan1.Init.NominalTimeSeg2 = 4;

// Create ULS connection
ULSBusConnectionCAN canConnection(
    &hfdcan1,           // CAN handle
    &device,            // Device object
    &connections,       // Connection list
    "CAN1",            // Name
    static_did,        // Device ID
    0                  // Connection ID
);

// Configure and open
canConnection.setSpeed(1000000);  // 1 Mbps
canConnection.open();

// Main loop
void main_loop() {
    while (1) {
        // Process CAN I/O
        canConnection.task(dt_ms);
        
        // Send data
        if (data_ready) {
            canConnection.ifSendBLITZ(msg_id, data, size);
        }
        
        // Check pool health
        if (canConnection.poolSize() < 64) {
            log_warning("CAN pool low");
        }
        
        delay(1);  // 1ms tick
    }
}