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;
}