minimac.cpp

이 파일의 문서화 페이지로 가기

 
#include "minimac.h"

static const int    SIG_ADDR   = 0;
static const uint32_t SIGVAL   = 0xAA55AA55;
static const int    DATA_ADDR  = SIG_ADDR + sizeof(SIGVAL);

static uint16_t    mm_id;                        
static uint8_t     mm_key[MINIMAC_KEY_LEN];      
static uint64_t    mm_counter;                   
static MiniMacHist mm_hist[MINIMAC_HIST_LEN];    
static uint8_t     mm_hist_cnt;                  

static void debug_print_hex(const uint8_t *buf, uint16_t len)
{
    for (uint16_t i = 0; i < len; i++) {
        if (buf[i] < 0x10) Serial.print('0');
        Serial.print(buf[i], HEX);
        Serial.print(' ');
    }
    Serial.println();
}

static void print_u64(uint64_t v)
{
    if (v == 0) {
        Serial.print('0');
        return;
    }
    char buf[21];      // 최대 20자리 숫자 + 널 종료
    buf[20] = '\0';
    int pos = 19;
    while (v > 0 && pos >= 0) {
        buf[pos--] = '0' + (v % 10);
        v /= 10;
    }
    Serial.print(&buf[pos + 1]);
}

static void compute_digest(const uint8_t *data, uint8_t len, unsigned char digest[16])
{
    /* (1) 버퍼 크기 계산:
     *     - 메시지 카운터(mm_counter, 8바이트)
     *     - CAN ID(mm_id, 2바이트)
     *     - 과거 메시지 히스토리(mm_hist_cnt개, 각 항목 len 바이트)
     *     - 현재 페이로드(data, len 바이트)
     *   위 항목의 총합(buf_len)을 계산한 뒤, 이를 저장할 버퍼(buf)를 동적 할당.
     *   off 변수는 buf 내 현재 쓰기 위치를 나타냄.
     */
    uint16_t buf_len = 8 + 2;
    for (uint8_t i = 0; i < mm_hist_cnt; i++)
        buf_len += mm_hist[i].len;
    buf_len += len;
 
    uint8_t *buf = (uint8_t *)malloc(buf_len);
    uint16_t off = 0;
 
    /* (2) 카운터 삽입 (big-endian):
     *   - 64비트 카운터를 빅엔디안 순서로 buf[off..off+7]에 저장
     *   - Serial.print로 현재 카운터 값을 10진수 문자열로 출력
     */
    Serial.print("[DBG] counter = ");
    print_u64(mm_counter);
    Serial.println();
 
    uint64_t tmp = mm_counter;
    for (int i = 7; i >= 0; i--) {
        buf[off + i] = tmp & 0xFF;
        tmp >>= 8;
    }
    off += 8;
 
    /* (3) CAN ID 삽입:
     *   - mm_id 상위 바이트(buf[off])와 하위 바이트(buf[off+1])를 저장
     *   - Serial.print로 16진수 형태의 CAN ID 출력
     */
    buf[off++] = mm_id >> 8;
    buf[off++] = mm_id & 0xFF;
    Serial.print("[DBG] CAN ID = 0x");
    Serial.println(mm_id, HEX);
 
    /* (4) 메시지 히스토리 삽입:
     *   - 저장된 히스토리 개수(mm_hist_cnt)만큼 반복
     *   - 각 항목(mm_hist[i].data, length mm_hist[i].len)을 buf에 복사
     *   - debug_print_hex로 각 히스토리 데이터 덤프
     */
    Serial.print("[DBG] history_count = ");
    Serial.println(mm_hist_cnt);
 
    for (uint8_t i = 0; i < mm_hist_cnt; i++) {
        Serial.print("[DBG] hist[");
        Serial.print(i);
        Serial.print("] = ");
        debug_print_hex(mm_hist[i].data, mm_hist[i].len);
 
        memcpy(buf + off, mm_hist[i].data, mm_hist[i].len);
        off += mm_hist[i].len;
    }
 
    /* (5) 현재 페이로드 삽입:
     *   - data[0..len-1]를 buf에 연속 복사
     *   - debug_print_hex로 페이로드 덤프
     */
    Serial.print("[DBG] current_data = ");
    debug_print_hex(data, len);
 
    memcpy(buf + off, data, len);
    off += len;
 
    /* (6) HMAC-MD5 계산:
     *   - MD5.hmac_md5(buf, off, mm_key, MINIMAC_KEY_LEN, digest)를 호출하여
     *     전체 입력(buf, 길이 off)에 대한 HMAC-MD5 다이제스트 생성
     *   - debug_print_hex로 16바이트 raw MD5 덤프
     *   - 동적 할당된 buf 메모리 해제
     */
    MD5 hasher;
    hasher.hmac_md5(buf, off, (void *)mm_key, MINIMAC_KEY_LEN, digest);
 
    Serial.print("[DBG] raw MD5 = ");
    debug_print_hex(digest, 16);
 
    free(buf);
}

static bool load_state(void)
{
    uint32_t sig;
 
    /* (1) 시그니처 확인 */
    EEPROM.get(SIG_ADDR, sig);
    if (sig != SIGVAL)
        return false;
 
    /* (2) 카운터 및 히스토리 개수 복원 */
    EEPROM.get(DATA_ADDR,                     mm_counter);
    EEPROM.get(DATA_ADDR + sizeof(mm_counter), mm_hist_cnt);
 
    /* (3) 히스토리 항목 복원 */
    int addr = DATA_ADDR + sizeof(mm_counter)
                      + sizeof(mm_hist_cnt);
    for (uint8_t i = 0; i < mm_hist_cnt; i++) {
        /* (3a) 각 히스토리 길이 로드 */
        EEPROM.get(addr, mm_hist[i].len);
        addr += sizeof(mm_hist[i].len);
 
        /* (3b) 고정 크기 버퍼에 과거 페이로드 데이터 로드 */
        EEPROM.get(addr, mm_hist[i].data);
        addr += MINIMAC_MAX_DATA;
    }
 
    /* (4) 디버그 출력으로 복원된 상태 확인 */
    Serial.println("[DBG] load_state: loaded from EEPROM");
    Serial.print("  counter = ");
    print_u64(mm_counter);
    Serial.println();
    Serial.print("  history_count = ");
    Serial.println(mm_hist_cnt);
 
    return true;
}

static void save_state(void)
{
    /* (1) 시그니처 기록 */
    EEPROM.put(SIG_ADDR, SIGVAL);
 
    /* (2) 카운터 및 히스토리 개수 기록 */
    EEPROM.put(DATA_ADDR,                       mm_counter);
    EEPROM.put(DATA_ADDR + sizeof(mm_counter),  mm_hist_cnt);
 
    /* (3) 히스토리 항목 기록 */
    int addr = DATA_ADDR + sizeof(mm_counter)
                      + sizeof(mm_hist_cnt);
    for (uint8_t i = 0; i < mm_hist_cnt; i++) {
        /* (3a) 각 히스토리 길이 저장 */
        EEPROM.put(addr, mm_hist[i].len);
        addr += sizeof(mm_hist[i].len);
 
        /* (3b) 고정 크기 버퍼에 과거 페이로드 데이터 저장 */
        EEPROM.put(addr, mm_hist[i].data);
        addr += MINIMAC_MAX_DATA;
    }
 
    /* (4) 디버그 출력으로 저장된 상태 확인 */
    Serial.println("[DBG] save_state: saved to EEPROM");
    Serial.print("  counter = ");
    print_u64(mm_counter);
    Serial.println();
    Serial.print("  history_count = ");
    Serial.println(mm_hist_cnt);
}

void minimac_init(uint16_t can_id, const uint8_t *key)
{
    /* Serial 초기화: 디버그 출력용 */
    Serial.begin(115200);
    while (!Serial)
        /* 시리얼 포트가 준비될 때까지 대기 */;
    Serial.println("[DBG] minimac_init()");
 
    /* (1) CAN ID 설정: 보호할 그룹 식별자 */
    mm_id = can_id;
 
    /* (2) 그룹 키 복사: 16바이트 비밀키 */
    memcpy(mm_key, key, MINIMAC_KEY_LEN);
 
    /* (3) EEPROM에서 이전 상태 불러오기 */
    if (!load_state()) {
        /* EEPROM에 유효한 시그니처 없음: fresh 초기화 */
        Serial.println("[DBG] minimac_init: no EEPROM state, initialize fresh");
 
        /* (3a) 카운터 초기화 */
        mm_counter   = 0;
 
        /* (3b) 히스토리 개수 초기화 */
        mm_hist_cnt  = 0;
 
        /* (3c) 초기 상태 EEPROM에 저장 */
        save_state();
    }
}

uint8_t minimac_sign(uint8_t *data, uint8_t payload_len)
{
    /* 디버그: 함수 진입 */
    Serial.println("[DBG] minimac_sign()");
 
    /* (1) HMAC 입력 구성 및 다이제스트 계산 */
    unsigned char digest[16];
    compute_digest(data, payload_len, digest);
 
    /* (2) 디버그: 생성된 다이제스트의 태그 부분 출력 */
    Serial.print("[DBG] sign: tag = ");
    debug_print_hex(digest, MINIMAC_TAG_LEN);
 
    /* (3) 태그(4바이트) 붙이기 */
    memcpy(data + payload_len, digest, MINIMAC_TAG_LEN);
    uint8_t total = payload_len + MINIMAC_TAG_LEN;
 
    /* (4) 메시지 히스토리 순환 버퍼 관리 */
    if (mm_hist_cnt == MINIMAC_HIST_LEN) {
        Serial.println("[DBG] sign: history full, dropping oldest");
        /* 가장 오래된 히스토리 항목 삭제 */
        for (uint8_t i = 1; i < mm_hist_cnt; i++)
            mm_hist[i - 1] = mm_hist[i];
        mm_hist_cnt--;
    }
    /* 새로운 페이로드를 히스토리에 추가 */
    mm_hist[mm_hist_cnt].len = payload_len;
    memcpy(mm_hist[mm_hist_cnt].data, data, payload_len);
    mm_hist_cnt++;
    Serial.print("[DBG] sign: new history_count = ");
    Serial.println(mm_hist_cnt);
 
    /* (5) 카운터 증가 및 디버그 출력 */
    mm_counter++;
    Serial.print("[DBG] sign: new counter = ");
    print_u64(mm_counter);
    Serial.println();
 
    /* (6) EEPROM에 상태 저장 */
    save_state();
 
    return total;
}

bool minimac_verify(const uint8_t *data, uint8_t payload_len, const uint8_t *tag)
{
    /* 디버그: 함수 진입 */
    Serial.println("[DBG] minimac_verify()");
 
    /* (1) HMAC 입력 구성 및 다이제스트 재계산 */
    unsigned char digest[16];
    compute_digest(data, payload_len, digest);
 
    /* (2) 디버그: 기대 태그(expected) 및 수신 태그(received) 출력 */
    Serial.print("[DBG] verify: expected tag = ");
    debug_print_hex(digest, MINIMAC_TAG_LEN);
    Serial.print("[DBG] verify: recv    tag = ");
    debug_print_hex(tag, MINIMAC_TAG_LEN);
 
    /* (3) 태그 비교: 불일치 시 실패 처리 */
    if (memcmp(digest, tag, MINIMAC_TAG_LEN) != 0) {
        Serial.println("[DBG] verify: FAILED");
        return false;
    }
 
    /* (4) 히스토리 순환 버퍼 관리 (가득 찼다면 가장 오래된 항목 삭제) */
    if (mm_hist_cnt == MINIMAC_HIST_LEN) {
        Serial.println("[DBG] verify: history full, dropping oldest");
        for (uint8_t i = 1; i < mm_hist_cnt; i++)
            mm_hist[i - 1] = mm_hist[i];
        mm_hist_cnt--;
    }
 
    /* (5) 성공 페이로드를 히스토리에 추가 */
    mm_hist[mm_hist_cnt].len = payload_len;
    memcpy(mm_hist[mm_hist_cnt].data, data, payload_len);
    mm_hist_cnt++;
    Serial.print("[DBG] verify: new history_count = ");
    Serial.println(mm_hist_cnt);
 
    /* (6) 카운터 증가 및 디버그 출력 */
    mm_counter++;
    Serial.print("[DBG] verify: new counter = ");
    print_u64(mm_counter);
    Serial.println();
 
    /* (7) EEPROM에 상태 저장 */
    save_state();
 
    Serial.println("[DBG] verify: SUCCESS");
    return true;
}