CANEmu/Core/CANEmu/canform.c

#include "canform.h"

uint8_t raw_bits[MAX_BITS];     // До стаффинга
uint32_t raw_len = 0;

uint8_t can_bits[MAX_BITS]; // После стаффинга
uint32_t can_bits_len = 0;

#define MAX_ERR_BITS 32
uint32_t err_indices[MAX_ERR_BITS];
uint8_t err_indices_count = 0;

void append_bit(uint8_t *buf, uint32_t *len, uint8_t bit) {
    if (*len < MAX_BITS) {
        buf[(*len)++] = bit;
    }
}

void append_bits(uint8_t *buf, uint32_t *len, uint32_t value, uint8_t count) {
    for (int i = count - 1; i >= 0; i--) {
        append_bit(buf, len, (value >> i) & 1);
    }
}

uint16_t compute_crc15(const uint8_t *bits, uint32_t len) {
    uint16_t crc = 0;
    for (uint32_t i = 0; i < len; i++) {
        uint8_t bit = bits[i];
        uint8_t crc_msb = (crc >> 14) & 1;
        crc <<= 1;
        if (bit ^ crc_msb) {
            crc ^= 0x4599;
        }
        crc &= 0x7FFF; // Обрезаем до 15 бит
    }
    return crc;
}
void apply_bit_stuffing_with_error(const uint8_t *src, uint32_t src_len, uint8_t *dst, uint32_t *dst_len, uint8_t skip_stuff) {
    *dst_len = 0;
    uint8_t last_bit = src[0];
    uint8_t count = 1;
    append_bit(dst, dst_len, last_bit);

    for (uint32_t i = 1; i < src_len; i++) {
        uint8_t bit = src[i];
        append_bit(dst, dst_len, bit);

        if (bit == last_bit) {
            count++;
            if (count == 5) {
                if (!skip_stuff) {
                    append_bit(dst, dst_len, !bit); // вставляем противоположный бит
                }
                count = 0;
            }
        } else {
            count = 1;
            last_bit = bit;
        }
    }
}

// Вспомогательная функция для инверсии бита в raw_bits
void flip_bit(uint32_t bit_index) {
    if (bit_index < raw_len) {
        raw_bits[bit_index] = !raw_bits[bit_index];
    }
}

void form_CAN_bitstream_full(const CAN_TxHeaderTypeDef *header, const uint8_t *data, CAN_ErrorFlags_t errors) {
    raw_len = 0;
    err_indices_count = 0;

    // 1. SOF (start of frame)
    append_bit(raw_bits, &raw_len, 0); // dominant

    // Запоминаем индексы для ошибок
    uint32_t idx_start = raw_len; // начало Arbitration + Control

    if (header->IDE == 0) {
        // --- Standard Frame (11-bit ID) ---
        // ID[10:0]
        append_bits(raw_bits, &raw_len, header->StdId & 0x7FF, 11);
        uint32_t id_start = raw_len - 11; // начало ID

        // RTR
        append_bit(raw_bits, &raw_len, header->RTR);
        uint32_t rtr_idx = raw_len - 1;

        // IDE = 0
        append_bit(raw_bits, &raw_len, 0);
        uint32_t ide_idx = raw_len - 1;

        // r0
        append_bit(raw_bits, &raw_len, 0);
        uint32_t r0_idx = raw_len - 1;

        // Ошибки FF_RTR, FF_IDE, FF_R0
        if (errors.FF_RTR) flip_bit(rtr_idx);
        if (errors.FF_IDE) flip_bit(ide_idx);
        if (errors.FF_R0) flip_bit(r0_idx);

        // MSGID_ERR — переворачиваем 1-й бит ID (MSB ID[10]) для примера
        if (errors.MSGID) flip_bit(id_start); 

    } else {
        // --- Extended Frame (29-bit ID) ---
        uint32_t ext_id = header->ExtId & 0x1FFFFFFF;
        append_bits(raw_bits, &raw_len, (ext_id >> 18) & 0x7FF, 11);   // ID[28:18]
        uint32_t id1_start = raw_len - 11;

        // SRR
        append_bit(raw_bits, &raw_len, 1);
        uint32_t srr_idx = raw_len - 1;

        // IDE = 1
        append_bit(raw_bits, &raw_len, 1);
        uint32_t ide_idx = raw_len - 1;

        append_bits(raw_bits, &raw_len, (ext_id >> 0) & 0x3FFFF, 18);  // ID[17:0]
        uint32_t id2_start = raw_len - 18;

        // RTR
        append_bit(raw_bits, &raw_len, header->RTR);
        uint32_t rtr_idx = raw_len - 1;

        // r1
        append_bit(raw_bits, &raw_len, 0);
        uint32_t r1_idx = raw_len - 1;

        // r0
        append_bit(raw_bits, &raw_len, 0);
        uint32_t r0_idx = raw_len - 1;

        // Ошибки FF_SRS_ERR — это SRR бит в extended frame
        if (errors.FF_SRS) flip_bit(srr_idx);
        if (errors.FF_IDE) flip_bit(ide_idx);
        if (errors.FF_RTR) flip_bit(rtr_idx);
        if (errors.FF_R1) flip_bit(r1_idx);
        if (errors.FF_R0) flip_bit(r0_idx);

        // MSGID_ERR — переворачиваем 1-й бит ID (для примера бит ID[28])
        if (errors.MSGID) flip_bit(id1_start);

    }

    // 3. DLC
    append_bits(raw_bits, &raw_len, header->DLC & 0xF, 4);

    // 4. Data field (если не RTR)
    if (!header->RTR) {
        uint8_t dlc = header->DLC & 0xF;
        for (uint8_t i = 0; i < dlc && i < 8; i++) {
            uint32_t byte_start = raw_len;
            uint8_t byte_val = data[i];
            // Time-stamp override on last 2 bytes
            if (header->TransmitGlobalTime == ENABLE && i >= 6) {
                append_bits(raw_bits, &raw_len, 0x00, 8); // Will be replaced by timestamp elsewhere
            } else {
              append_bits(raw_bits, &raw_len, byte_val, 8);
            }

            // Если ошибка DATA_ERR, инвертируем 1-й бит первого байта данных для примера
            if ((errors.DATA) && i == 0) {
                flip_bit(byte_start);
            }
        }
    }

    // 5. CRC
    uint16_t crc = compute_crc15(raw_bits, raw_len);
    uint32_t crc_start = raw_len;
    append_bits(raw_bits, &raw_len, crc, 15);

    if (errors.CRC_ERR) {
        flip_bit(crc_start); // инвертируем 1-й бит CRC для примера
    }

    // 6. CRC Delimiter (recessive)
    append_bit(raw_bits, &raw_len, 1);

    // 10. Apply bit stuffing, пропускаем вставку stuff bits если ошибка ERR_STUFF_BITS
    apply_bit_stuffing_with_error(raw_bits, raw_len, can_bits, &can_bits_len, errors.STUFF_BITS);
    
    // 7. ACK Slot (dominant)
    append_bit(raw_bits, &raw_len, 1);

    // 8. ACK Delimiter (recessive)
    append_bit(raw_bits, &raw_len, 1);

    // 9. End of Frame (7 recessive bits)
    for (int i = 0; i < 7; i++) {
        append_bit(raw_bits, &raw_len, 1);
    }

}