Browse Source
- ll_queue: Add dgram, dgram_free_fn, dgram_pool, len fields to ll_entry - ll_queue: Remove ref_count, simplify memory management - etcp: Add normalizer pointer to ETCP_CONN struct - pkt_normalizer: Major refactoring for queue integration - tests: Update for new ll_entry structure - cleanup: Remove backup filesnodeinfo-routing-update
Evgeny
8 months ago
11 changed files with 1477 additions and 2318 deletions
@ -1,606 +1,355 @@
|
||||
// pkt_normalizer.c - Implementation of packet normalizer for ETCP
|
||||
#include "pkt_normalizer.h" |
||||
#include "../lib/u_async.h" |
||||
#include "etcp.h" // For ETCP_CONN and related structures |
||||
#include "ll_queue.h" // For queue operations |
||||
#include "u_async.h" // For UASYNC |
||||
#include <stdlib.h> |
||||
#include <string.h> |
||||
#include <stdint.h> |
||||
#include <stdio.h> |
||||
static void packer_handler(struct ll_queue* q, void* arg); |
||||
static void unpacker_handler(struct ll_queue* q, void* arg); |
||||
static void send_buf(struct pn_struct* pn); |
||||
static int get_header(uint8_t* header, size_t L); |
||||
/* Calculate fragment size from mtu */ |
||||
struct pn_struct* pkt_normalizer_init(uasync_t* ua, int is_packer, int mtu) { |
||||
struct pn_struct* pn = malloc(sizeof(struct pn_struct)); |
||||
#include <stdio.h> // For debugging (can be removed if not needed) |
||||
|
||||
// Internal helper to convert void* data to struct ll_entry*
|
||||
static inline struct ll_entry* data_to_entry(void* data) { |
||||
if (!data) return NULL; |
||||
return (struct ll_entry*)data; |
||||
} |
||||
|
||||
// Forward declarations
|
||||
static void packer_cb(struct ll_queue* q, void* arg); |
||||
static void flush_cb(void* arg); |
||||
static void input_ready_cb(struct ll_queue* q, void* arg); |
||||
|
||||
// Initialization
|
||||
struct PKTNORM* pn_init(struct ETCP_CONN* etcp) { |
||||
if (!etcp) return NULL; |
||||
|
||||
struct PKTNORM* pn = calloc(1, sizeof(struct PKTNORM)); |
||||
if (!pn) return NULL; |
||||
pn->ua = ua; |
||||
pn->input = queue_new(ua, 0); // No memory pool for now, no hash table
|
||||
if (!pn->input) { |
||||
free(pn); |
||||
return NULL; |
||||
} |
||||
pn->output = queue_new(ua, 0); // No memory pool for now, no hash table
|
||||
if (!pn->output) { |
||||
queue_free(pn->input); |
||||
free(pn); |
||||
|
||||
pn->etcp = etcp; |
||||
pn->ua = etcp->instance->ua; |
||||
pn->pkt_size = etcp->mtu; // Use MTU as fixed packet size (adjust if headers need subtraction)
|
||||
|
||||
pn->input = queue_new(pn->ua, 0); // No hash needed
|
||||
pn->output = queue_new(pn->ua, 0); // No hash needed
|
||||
pn->pending = queue_new(pn->ua, 0); |
||||
pn->send_pending = queue_new(pn->ua, 0); |
||||
|
||||
if (!pn->input || !pn->output || !pn->pending || !pn->send_pending) { |
||||
pn_pair_deinit(pn); |
||||
return NULL; |
||||
} |
||||
pn->is_packer = is_packer; |
||||
if (is_packer) { |
||||
// Calculate fragment size from mtu: fragment = mtu - 100
|
||||
int fragment_size = mtu - ETCP_OVERHEAD; |
||||
if (fragment_size < 256) fragment_size = 256; // Minimum sane value
|
||||
pn->u.packer.cap = fragment_size; |
||||
pn->u.packer.buf = malloc(pn->u.packer.cap); |
||||
if (!pn->u.packer.buf) { |
||||
queue_free(pn->input); |
||||
queue_free(pn->output); |
||||
free(pn); |
||||
return NULL; |
||||
} |
||||
pn->u.packer.len = 0; |
||||
pn->u.packer.error_count = 0; |
||||
queue_set_callback(pn->input, packer_handler, pn); |
||||
} else { |
||||
pn->u.unpacker.buf = NULL; |
||||
pn->u.unpacker.len = 0; |
||||
pn->u.unpacker.total_len = 0; |
||||
pn->u.unpacker.cap = 0; |
||||
pn->u.unpacker.error_count = 0; |
||||
queue_set_callback(pn->input, unpacker_handler, pn); |
||||
} |
||||
|
||||
pn->in_buf = calloc(1, pn->pkt_size); |
||||
pn->cap = pn->pkt_size; // For packer buffer (fixed size)
|
||||
pn->len = 0; // Current length in packer buffer
|
||||
|
||||
pn->out_buf = calloc(1, pn->pkt_size * 10); // Arbitrary large buffer for assembly
|
||||
pn->cap = pn->pkt_size * 10; // Initial capacity for unpacker (can reallocate if needed)
|
||||
pn->len = 0; |
||||
pn->total_len = 0; |
||||
pn->in_fragment = 0; |
||||
|
||||
// Set callback for automatic processing when items are added to input
|
||||
queue_set_callback(pn->input, packer_cb, pn); |
||||
|
||||
pn->flush_timer = NULL; |
||||
|
||||
return pn; |
||||
} |
||||
|
||||
void pkt_normalizer_deinit(struct pn_struct* pn) { |
||||
// Deinitialization
|
||||
void pn_pair_deinit(struct PKTNORM* pn) { |
||||
if (!pn) return; |
||||
queue_free(pn->input); |
||||
queue_free(pn->output); |
||||
if (pn->is_packer) { |
||||
free(pn->u.packer.buf); |
||||
} else { |
||||
free(pn->u.unpacker.buf); |
||||
free(pn->u.unpacker.service_buf); |
||||
|
||||
// Drain and free queues
|
||||
if (pn->input) { |
||||
void* data; |
||||
while ((data = queue_data_get(pn->input)) != NULL) { |
||||
queue_data_free(data); |
||||
} |
||||
queue_free(pn->input); |
||||
} |
||||
free(pn); |
||||
} |
||||
struct pkt_normalizer_pair* pkt_normalizer_pair_init(uasync_t* ua, int mtu) { |
||||
struct pkt_normalizer_pair* pair = malloc(sizeof(struct pkt_normalizer_pair)); |
||||
if (!pair) return NULL; |
||||
pair->packer = pkt_normalizer_init(ua, 1, mtu); |
||||
if (!pair->packer) { |
||||
free(pair); |
||||
return NULL; |
||||
if (pn->output) { |
||||
void* data; |
||||
while ((data = queue_data_get(pn->output)) != NULL) { |
||||
queue_data_free(data); |
||||
} |
||||
queue_free(pn->output); |
||||
} |
||||
pair->unpacker = pkt_normalizer_init(ua, 0, mtu); |
||||
if (!pair->unpacker) { |
||||
pkt_normalizer_deinit(pair->packer); |
||||
free(pair); |
||||
return NULL; |
||||
if (pn->pending) { |
||||
void* data; |
||||
while ((data = queue_data_get(pn->pending)) != NULL) { |
||||
queue_data_free(data); |
||||
} |
||||
queue_free(pn->pending); |
||||
} |
||||
return pair; |
||||
} |
||||
void pkt_normalizer_pair_deinit(struct pkt_normalizer_pair* pair) { |
||||
if (!pair) return; |
||||
pkt_normalizer_deinit(pair->packer); |
||||
pkt_normalizer_deinit(pair->unpacker); |
||||
free(pair); |
||||
} |
||||
static int get_header(uint8_t* header, size_t L) { |
||||
if (L > 1535) return -1; |
||||
if (L <= 239) { |
||||
header[0] = (uint8_t)L; |
||||
return 1; |
||||
} else { |
||||
uint8_t high = (uint8_t)(L >> 8); |
||||
if (high > 5) return -1; |
||||
header[0] = 0xF0 + high; |
||||
header[1] = (uint8_t)(L & 0xFF); |
||||
return 2; |
||||
if (pn->send_pending) { |
||||
void* data; |
||||
while ((data = queue_data_get(pn->send_pending)) != NULL) { |
||||
struct ETCP_FRAGMENT* frag = data_to_entry(data); |
||||
if (frag->ll.dgram) memory_pool_free(pn->etcp->instance->data_pool, frag->ll.dgram); |
||||
memory_pool_free(pn->etcp->rx_pool, frag); |
||||
} |
||||
queue_free(pn->send_pending); |
||||
} |
||||
} |
||||
/* Сбросить состояние сборки фрагментов */ |
||||
static void reset_fragment_state(struct pn_struct* pn) { |
||||
if (!pn->is_packer) { |
||||
pn->u.unpacker.len = 0; |
||||
pn->u.unpacker.total_len = 0; |
||||
pn->u.unpacker.in_fragment = 0; |
||||
|
||||
if (pn->flush_timer) { |
||||
uasync_cancel_timeout(pn->ua, pn->flush_timer); |
||||
} |
||||
|
||||
free(pn->in_buf); |
||||
free(pn->out_buf); |
||||
free(pn); |
||||
} |
||||
/* Таймаут для сборки фрагментов */ |
||||
static void send_buf(struct pn_struct* pn) { |
||||
if (pn->u.packer.len == 0) return; |
||||
size_t payload_len = pn->u.packer.len; |
||||
uint8_t* out = queue_data_new(2 + payload_len); // Новый API
|
||||
if (!out) return; |
||||
*(uint16_t*)out = (uint16_t)payload_len; |
||||
memcpy(out + 2, pn->u.packer.buf, payload_len); |
||||
queue_data_put(pn->output, out, 0); // Новый API, ID=0 для внутренних пакетов
|
||||
pn->u.packer.len = 0; |
||||
|
||||
// Reset unpacker state
|
||||
void pn_unpacker_reset_state(struct PKTNORM* pn) { |
||||
if (!pn) return; |
||||
pn->len = 0; |
||||
pn->total_len = 0; |
||||
pn->in_fragment = 0; |
||||
} |
||||
static void packer_handler(struct ll_queue* q, void* arg) { |
||||
struct pn_struct* pn = arg; |
||||
size_t max = (size_t)1400; |
||||
|
||||
// Получить данные из очереди
|
||||
void* pkt_data = queue_data_get(q); |
||||
if (!pkt_data) { |
||||
queue_resume_callback(q); |
||||
return; |
||||
} |
||||
|
||||
// Для определения размера нужно знать структуру, но в новом API размер в самих данных
|
||||
// В pkt_normalizer все данные имеют 2-байтовый заголовок с размером
|
||||
uint16_t* size_ptr = (uint16_t*)pkt_data; |
||||
size_t L = *size_ptr; |
||||
uint8_t* pkt_payload = (uint8_t*)pkt_data + 2; |
||||
uint8_t header[2]; |
||||
int hsize = get_header(header, L); |
||||
size_t needed = (size_t)hsize + L; |
||||
if (hsize < 0 || needed > max) { |
||||
// Fragment
|
||||
if (pn->u.packer.len > 0) { |
||||
send_buf(pn); |
||||
} |
||||
size_t remaining = L; |
||||
size_t pos = 0; |
||||
int fragment_count = 0; |
||||
while (remaining > 0) { |
||||
size_t chunk; |
||||
size_t payload_len; |
||||
uint8_t* fout; |
||||
uint8_t* fd; |
||||
uint8_t frag_header[2]; |
||||
int frag_hsize; |
||||
if (fragment_count == 0) { |
||||
// Первый фрагмент: FF + общая длина (2 байта)
|
||||
chunk = remaining > (max - 5) ? (max - 5) : remaining; // 2+1+2+chunk <= max
|
||||
payload_len = 1 + 2 + chunk; // FF + total_len + data
|
||||
fout = queue_data_new(2 + payload_len); |
||||
if (!fout) { |
||||
break; |
||||
} |
||||
fd = fout; |
||||
*(uint16_t*)fd = (uint16_t)payload_len; |
||||
fd += 2; |
||||
*fd++ = 0xFF; |
||||
*fd++ = (uint8_t)(L >> 8); // старший байт общей длины
|
||||
*fd++ = (uint8_t)(L & 0xFF); // младший байт общей длины
|
||||
} else { |
||||
// Не первый фрагмент
|
||||
if (remaining <= max - 3) { |
||||
// Это последний возможный фрагмент (помещается в один пакет с префиксом FE)
|
||||
// Пытаемся отправить как обычный блок
|
||||
frag_hsize = get_header(frag_header, remaining); |
||||
if (frag_hsize > 0 && (size_t)frag_hsize + remaining + 2 <= max) { |
||||
// Успешно: обычный блок
|
||||
payload_len = frag_hsize + remaining; |
||||
chunk = remaining; |
||||
fout = queue_data_new(2 + payload_len); |
||||
if (!fout) { |
||||
break; |
||||
} |
||||
fd = fout; |
||||
*(uint16_t*)fd = (uint16_t)payload_len; |
||||
fd += 2; |
||||
memcpy(fd, frag_header, frag_hsize); |
||||
fd += frag_hsize; |
||||
} else { |
||||
// Не удалось отправить как обычный блок - разбиваем на 2 фрагмента
|
||||
// 1. FE фрагмент с частью данных
|
||||
// 2. Обычный блок с оставшимися данными
|
||||
// Находим максимальный размер для FE фрагмента
|
||||
size_t max_fe_data = max - 3; // 2 байта длины + 0xFE
|
||||
if (max_fe_data > remaining) { |
||||
max_fe_data = remaining; |
||||
} |
||||
// Пробуем различные размеры, начиная с максимального
|
||||
size_t fe_data_size = 0; |
||||
for (size_t try_fe = max_fe_data; try_fe > 0; try_fe--) { |
||||
size_t try_regular = remaining - try_fe; |
||||
if (try_regular == 0) continue; // Нужно отправить что-то как обычный блок
|
||||
uint8_t test_header[2]; |
||||
int hsize = get_header(test_header, try_regular); |
||||
if (hsize <= 0) continue; |
||||
if ((size_t)hsize + try_regular + 2 <= max) { |
||||
fe_data_size = try_fe; |
||||
break; |
||||
} |
||||
} |
||||
if (fe_data_size == 0) { |
||||
// Не удалось найти разбиение - ошибка
|
||||
pn->u.packer.error_count++; |
||||
// Отправляем как FE (нарушение спецификации, но это крайний случай)
|
||||
chunk = remaining > (max - 3) ? (max - 3) : remaining; |
||||
payload_len = 1 + chunk; // FE + data
|
||||
fout = queue_data_new(2 + payload_len); |
||||
if (!fout) break; |
||||
fd = (fout); |
||||
*(uint16_t*)fd = (uint16_t)payload_len; |
||||
fd += 2; |
||||
*fd++ = 0xFE; |
||||
} else { |
||||
// Отправляем FE фрагмент
|
||||
chunk = fe_data_size; |
||||
payload_len = 1 + chunk; // FE + data
|
||||
fout = queue_data_new(2 + payload_len); |
||||
if (!fout) break; |
||||
fd = (fout); |
||||
*(uint16_t*)fd = (uint16_t)payload_len; |
||||
fd += 2; |
||||
*fd++ = 0xFE; |
||||
memcpy(fd, pkt_data + pos, chunk); |
||||
queue_data_put(pn->output, fout, 0); |
||||
pos += chunk; |
||||
remaining -= chunk; |
||||
fragment_count++; |
||||
// Обновляем оставшиеся данные для обычного блока
|
||||
// (цикл продолжит обработку на следующей итерации)
|
||||
continue; |
||||
} |
||||
} |
||||
} else { |
||||
// Промежуточный фрагмент, отправляем как FE
|
||||
chunk = remaining > (max - 3) ? (max - 3) : remaining; |
||||
payload_len = 1 + chunk; // FE + data
|
||||
fout = queue_data_new(2 + payload_len); |
||||
if (!fout) { |
||||
break; |
||||
} |
||||
fd = fout; |
||||
*(uint16_t*)fd = (uint16_t)payload_len; |
||||
fd += 2; |
||||
*fd++ = 0xFE; |
||||
} |
||||
} |
||||
memcpy(fd, pkt_data + pos, chunk); |
||||
queue_data_put(pn->output, fout, 0); |
||||
pos += chunk; |
||||
remaining -= chunk; |
||||
fragment_count++; |
||||
} |
||||
|
||||
// Send data to packer (copies and adds to input queue or pending, triggering callback)
|
||||
void pn_packer_send(struct PKTNORM* pn, uint8_t* data, uint16_t len) { |
||||
if (!pn || !data || len == 0) return; |
||||
|
||||
void* entry_data = queue_data_new(len); |
||||
if (!entry_data) return; |
||||
|
||||
struct ll_entry* entry = data_to_entry(entry_data); |
||||
memcpy(entry->data, data, len); |
||||
entry->len = len; |
||||
entry->size = len; |
||||
|
||||
// To minimize delay, add to input only if empty, else to pending and set waiter if not set
|
||||
if (queue_entry_count(pn->input) == 0 && pn->input->waiter.callback == NULL) { |
||||
queue_data_put(pn->input, entry_data, 0); |
||||
} else { |
||||
if (pn->u.packer.len + needed > max) { |
||||
send_buf(pn); |
||||
queue_data_put(pn->pending, entry_data, 0); |
||||
if (pn->input->waiter.callback == NULL) { |
||||
queue_wait_threshold(pn->input, 0, 0, input_ready_cb, pn); |
||||
} |
||||
// Add to buffer
|
||||
uint8_t* p = pn->u.packer.buf + pn->u.packer.len; |
||||
memcpy(p, header, (size_t)hsize); |
||||
memcpy(p + hsize, pkt_data, L); |
||||
pn->u.packer.len += needed; |
||||
} |
||||
queue_data_free(pkt_data); |
||||
if (pn->u.packer.len > 0) { |
||||
send_buf(pn); |
||||
} |
||||
queue_resume_callback(q); |
||||
} |
||||
void pkt_normalizer_set_service_callback(struct pn_struct* pn, pkt_normalizer_service_callback_t callback, void* user_data) { |
||||
if (!pn) return; |
||||
pn->service_callback = callback; |
||||
pn->service_callback_user_data = user_data; |
||||
} |
||||
void pkt_normalizer_reset_service_state(struct pn_struct* pn) { |
||||
if (!pn || pn->is_packer) return; |
||||
if (pn->u.unpacker.in_service) { |
||||
// Deliver pending service packet
|
||||
if (pn->service_callback) { |
||||
pn->service_callback(pn->service_callback_user_data, |
||||
pn->u.unpacker.service_type, |
||||
pn->u.unpacker.service_buf, |
||||
pn->u.unpacker.service_len); |
||||
|
||||
// Internal: Callback when input queue becomes empty, move next from pending
|
||||
static void input_ready_cb(struct ll_queue* q, void* arg) { |
||||
struct PKTNORM* pn = (struct PKTNORM*)arg; |
||||
|
||||
void* data = queue_data_get(pn->pending); |
||||
if (data) { |
||||
queue_data_put(q, data, 0); // This will trigger packer_cb async if needed
|
||||
if (queue_entry_count(pn->pending) > 0) { |
||||
queue_wait_threshold(q, 0, 0, input_ready_cb, pn); |
||||
} |
||||
free(pn->u.unpacker.service_buf); |
||||
pn->u.unpacker.service_buf = NULL; |
||||
pn->u.unpacker.service_len = 0; |
||||
pn->u.unpacker.service_cap = 0; |
||||
pn->u.unpacker.in_service = 0; |
||||
} |
||||
} |
||||
void pkt_normalizer_reset_state(struct pn_struct* pn) { |
||||
if (!pn) return; |
||||
if (pn->is_packer) { |
||||
// Flush packer buffer
|
||||
if (pn->u.packer.len > 0) { |
||||
send_buf(pn); |
||||
|
||||
static void etcp_input_ready_cb(struct ll_queue* q, void* arg) { |
||||
struct PKTNORM* pn = (struct PKTNORM*)arg; |
||||
|
||||
void* data = queue_data_get(pn->send_pending); |
||||
if (data) { |
||||
queue_data_put(q, data, 0); |
||||
if (queue_entry_count(pn->send_pending) > 0) { |
||||
queue_wait_threshold(q, 0, 0, etcp_input_ready_cb, pn); |
||||
} |
||||
} |
||||
} |
||||
|
||||
// Internal: Send a fixed-size chunk to etcp->input_queue (or send_pending)
|
||||
static void send_chunk(struct PKTNORM* pn, uint8_t* chunk_data, uint16_t chunk_len) { |
||||
// Alloc ETCP_FRAGMENT for the chunk (as payload for etcp)
|
||||
struct ETCP_FRAGMENT* frag = memory_pool_alloc(pn->etcp->rx_pool); |
||||
if (!frag) return; |
||||
|
||||
frag->ll.dgram = memory_pool_alloc(pn->etcp->instance->data_pool); |
||||
if (!frag->ll.dgram) { |
||||
memory_pool_free(pn->etcp->rx_pool, frag); |
||||
return; |
||||
} |
||||
|
||||
memcpy(frag->ll.dgram, chunk_data, chunk_len); |
||||
frag->ll.size = chunk_len; |
||||
frag->ll.len = chunk_len; |
||||
frag->seq = 0; // Not used for input
|
||||
frag->timestamp = 0; |
||||
|
||||
struct ll_queue* eq = pn->etcp->input_queue; |
||||
|
||||
// Add to etcp input_queue only if empty, else to send_pending and set waiter
|
||||
if (queue_entry_count(eq) == 0 && eq->waiter.callback == NULL) { |
||||
queue_data_put(eq, frag, 0); |
||||
} else { |
||||
// Reset unpacker fragment state
|
||||
reset_fragment_state(pn); |
||||
// Reset service state
|
||||
pkt_normalizer_reset_service_state(pn); |
||||
queue_data_put(pn->send_pending, frag, 0); |
||||
if (eq->waiter.callback == NULL) { |
||||
queue_wait_threshold(eq, 0, 0, etcp_input_ready_cb, pn); |
||||
} |
||||
} |
||||
} |
||||
int pkt_normalizer_send_service(struct pn_struct* pn, uint8_t type, const void* data, size_t len) { |
||||
if (!pn || !pn->is_packer) return -1; |
||||
// Service packet ограничен 256 байтами всего
|
||||
if (len > 256 - 2) return -1; // 2 байта на заголовок (0xFC + тип)
|
||||
size_t max = (size_t)1400; |
||||
if (max < 3) return -1; |
||||
// Размер сервисного пакета: 2 байта длины + 1 байт 0xFC + 1 байт тип + данные
|
||||
// Если не помещается в один фрагмент - используем продолжение 0xFD
|
||||
size_t total_service_len = 1 + 1 + len; // 0xFC + type + data
|
||||
size_t pos = 0; |
||||
while (total_service_len > 0) { |
||||
// Определяем размер куска для этого пакета
|
||||
size_t chunk; |
||||
uint8_t service_header; |
||||
if (pos == 0) { |
||||
// Первый пакет: 0xFC + тип + часть данных
|
||||
// Максимум данных в первом пакете: max - 2 (длина) - 2 (0xFC+тип)
|
||||
size_t max_first_data = max - 4; |
||||
if (max_first_data > len) max_first_data = len; |
||||
chunk = max_first_data; |
||||
service_header = 0xFC; |
||||
|
||||
// Internal: Packer callback (aggregates small, fragments large, sends chunks)
|
||||
static void packer_cb(struct ll_queue* q, void* arg) { |
||||
struct PKTNORM* pn = (struct PKTNORM*)arg; |
||||
if (!pn) return; |
||||
|
||||
pn->len = 0; // Start with empty buffer for this batch
|
||||
|
||||
while (q->head) { |
||||
void* data = queue_data_get(q); |
||||
struct ll_entry* entry = data_to_entry(data); |
||||
uint16_t item_len = entry->len; |
||||
|
||||
if (item_len + 2 > pn->pkt_size) { |
||||
// Large item: fragment (must be alone in buffer)
|
||||
if (pn->len > 0) { |
||||
// Flush current aggregated small items first
|
||||
memset(pn->in_buf + pn->len, 0, pn->pkt_size - pn->len); // Pad
|
||||
send_chunk(pn, pn->in_buf, pn->pkt_size); |
||||
pn->len = 0; |
||||
} |
||||
|
||||
// Fragment the large item
|
||||
uint8_t* ptr = entry->data; |
||||
uint32_t remaining = item_len; |
||||
|
||||
// First fragment
|
||||
*(uint16_t*)(pn->in_buf) = 0xffff; |
||||
*(uint32_t*)(pn->in_buf + 2) = remaining; |
||||
uint16_t chunk = pn->pkt_size - 6; |
||||
memcpy(pn->in_buf + 6, ptr, chunk); |
||||
send_chunk(pn, pn->in_buf, pn->pkt_size); |
||||
ptr += chunk; |
||||
remaining -= chunk; |
||||
|
||||
// Continuation fragments
|
||||
while (remaining > 0) { |
||||
chunk = (remaining < pn->pkt_size - 2) ? remaining : pn->pkt_size - 2; |
||||
*(uint16_t*)(pn->in_buf) = 0x0000; // Continuation marker
|
||||
memcpy(pn->in_buf + 2, ptr, chunk); |
||||
memset(pn->in_buf + 2 + chunk, 0, pn->pkt_size - 2 - chunk); // Pad
|
||||
send_chunk(pn, pn->in_buf, pn->pkt_size); |
||||
ptr += chunk; |
||||
remaining -= chunk; |
||||
} |
||||
} else { |
||||
// Продолжение: 0xFD + данные
|
||||
// Максимум данных: max - 2 (длина) - 1 (0xFD)
|
||||
size_t max_cont_data = max - 3; |
||||
size_t remaining = len - pos; |
||||
if (max_cont_data > remaining) max_cont_data = remaining; |
||||
chunk = max_cont_data; |
||||
service_header = 0xFD; |
||||
} |
||||
if (chunk == 0) break; |
||||
size_t payload_len = 1 + chunk + (pos == 0 ? 1 : 0); // +1 байт типа для первого пакета
|
||||
struct ll_entry* entry = queue_data_new(2 + payload_len); |
||||
if (!entry) return -1; |
||||
uint8_t* d = (entry); |
||||
*(uint16_t*)d = (uint16_t)payload_len; |
||||
d += 2; |
||||
*d++ = service_header; |
||||
if (pos == 0) { |
||||
*d++ = type; |
||||
// Small item: try to aggregate
|
||||
if (pn->len + item_len + 2 > pn->pkt_size) { |
||||
// Flush current buffer
|
||||
memset(pn->in_buf + pn->len, 0, pn->pkt_size - pn->len); // Pad
|
||||
send_chunk(pn, pn->in_buf, pn->pkt_size); |
||||
pn->len = 0; |
||||
} |
||||
|
||||
// Add to buffer
|
||||
*(uint16_t*)(pn->in_buf + pn->len) = item_len; |
||||
memcpy(pn->in_buf + pn->len + 2, entry->data, item_len); |
||||
pn->len += item_len + 2; |
||||
} |
||||
memcpy(d, (const uint8_t*)data + pos, chunk); |
||||
queue_data_put(pn->output, entry, 0); |
||||
pos += chunk; |
||||
total_service_len -= chunk + (pos == chunk ? 2 : 1); // корректно вычитаем заголовки
|
||||
|
||||
queue_data_free(data); // Free the entry
|
||||
} |
||||
return 0; |
||||
} |
||||
int pkt_normalizer_get_error_count(const struct pn_struct* pn) { |
||||
if (!pn) return 0; |
||||
if (pn->is_packer) { |
||||
return pn->u.packer.error_count; |
||||
|
||||
// If remaining in buffer, set timeout to flush if queues empty
|
||||
if (pn->len > 0) { |
||||
if (pn->flush_timer) { |
||||
uasync_cancel_timeout(pn->ua, pn->flush_timer); |
||||
} |
||||
pn->flush_timer = uasync_set_timeout(pn->ua, 10, pn, flush_cb); // 1ms = 10 * 0.1ms
|
||||
} |
||||
return pn->u.unpacker.error_count; |
||||
|
||||
queue_resume_callback(q); |
||||
} |
||||
void pkt_normalizer_reset_error_count(struct pn_struct* pn) { |
||||
|
||||
// Internal: Flush callback on timeout
|
||||
static void flush_cb(void* arg) { |
||||
struct PKTNORM* pn = (struct PKTNORM*)arg; |
||||
if (!pn) return; |
||||
if (pn->is_packer) { |
||||
pn->u.packer.error_count = 0; |
||||
} else { |
||||
pn->u.unpacker.error_count = 0; |
||||
|
||||
pn->flush_timer = NULL; |
||||
|
||||
if (pn->len > 0 && queue_entry_count(pn->input) == 0 && queue_entry_count(pn->pending) == 0) { |
||||
memset(pn->in_buf + pn->len, 0, pn->pkt_size - pn->len); // Pad
|
||||
send_chunk(pn, pn->in_buf, pn->pkt_size); |
||||
pn->len = 0; |
||||
} |
||||
} |
||||
void pkt_normalizer_flush(struct pn_struct* pn) { |
||||
if (!pn || !pn->is_packer) return; |
||||
if (pn->u.packer.len > 0) { |
||||
send_buf(pn); |
||||
|
||||
// Internal: Add assembled data to etcp->output_queue as ETCP_FRAGMENT
|
||||
static void add_to_output(struct PKTNORM* pn, uint8_t* app_data, uint16_t app_len) { |
||||
struct ETCP_FRAGMENT* frag = memory_pool_alloc(pn->etcp->rx_pool); |
||||
if (!frag) return; |
||||
|
||||
frag->ll.dgram = memory_pool_alloc(pn->etcp->instance->data_pool); |
||||
if (!frag->ll.dgram) { |
||||
memory_pool_free(pn->etcp->rx_pool, frag); |
||||
return; |
||||
} |
||||
|
||||
memcpy(frag->ll.dgram, app_data, app_len); |
||||
frag->ll.size = app_len; |
||||
frag->ll.len = app_len; |
||||
frag->seq = 0; |
||||
frag->timestamp = 0; |
||||
|
||||
queue_data_put(pn->etcp->output_queue, frag, 0); |
||||
} |
||||
static void unpacker_handler(struct ll_queue* q, void* arg) { |
||||
struct pn_struct* pn = arg; |
||||
while (queue_entry_count(q) > 0) { |
||||
uint8_t* entry = queue_data_get(q); |
||||
uint8_t* data = entry; |
||||
uint16_t payload_len = *(uint16_t*)data; |
||||
if (entry != NULL && *(uint16_t*)entry != 2 + (size_t)payload_len) { |
||||
queue_data_free(entry); |
||||
continue; |
||||
|
||||
// Internal: Process incoming fixed-size payload chunk from etcp
|
||||
void pn_unpacker_input(struct PKTNORM* pn, uint8_t* data, uint16_t len) { |
||||
if (!pn || len != pn->pkt_size) return; |
||||
|
||||
uint16_t header = *(uint16_t*)data; |
||||
|
||||
if (pn->in_fragment) { |
||||
// Expect continuation
|
||||
if (header != 0x0000) { |
||||
// Error: invalid continuation
|
||||
pn_unpacker_reset_state(pn); |
||||
return; |
||||
} |
||||
uint8_t* cg = data + 2; |
||||
size_t cg_pos = 0; |
||||
size_t cg_len = (size_t)payload_len; |
||||
while (cg_pos < cg_len) { |
||||
uint8_t byte = cg[cg_pos++]; |
||||
if (byte == 0xFF) { |
||||
/* Начало нового фрагментированного пакета */ |
||||
if (pn->u.unpacker.in_fragment) { |
||||
/* Не завершен предыдущий фрагмент - ошибка */ |
||||
pn->u.unpacker.error_count++; |
||||
reset_fragment_state(pn); |
||||
} |
||||
/* Проверить, что есть 2 байта для общей длины */ |
||||
if (cg_pos + 2 > cg_len) { |
||||
pn->u.unpacker.error_count++; |
||||
goto err; |
||||
} |
||||
/* Прочитать общую длину */ |
||||
uint16_t total_len = ((uint16_t)cg[cg_pos] << 8) | cg[cg_pos + 1]; |
||||
cg_pos += 2; |
||||
size_t chunk_len = cg_len - cg_pos; |
||||
/* Выделить буфер при необходимости */ |
||||
size_t new_len = chunk_len; |
||||
if (new_len > pn->u.unpacker.cap) { |
||||
size_t new_cap = pn->u.unpacker.cap ? pn->u.unpacker.cap * 2 : 4096; |
||||
if (new_cap < new_len) new_cap = new_len; |
||||
pn->u.unpacker.buf = realloc(pn->u.unpacker.buf, new_cap); |
||||
pn->u.unpacker.cap = new_cap; |
||||
} |
||||
memcpy(pn->u.unpacker.buf, cg + cg_pos, chunk_len); |
||||
pn->u.unpacker.len = chunk_len; |
||||
pn->u.unpacker.total_len = total_len; |
||||
pn->u.unpacker.in_fragment = 1; |
||||
cg_pos += chunk_len; |
||||
/* Проверить, не собрали ли уже весь пакет */ |
||||
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
||||
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
||||
uint8_t* out = queue_data_new(pn->u.unpacker.total_len); |
||||
if (out) { |
||||
memcpy(out, pn->u.unpacker.buf, pn->u.unpacker.total_len); |
||||
queue_data_put(pn->output, out, 0); |
||||
} |
||||
} else { |
||||
/* Слишком много данных - ошибка */ |
||||
pn->u.unpacker.error_count++; |
||||
} |
||||
reset_fragment_state(pn); |
||||
} |
||||
continue; |
||||
} |
||||
if (byte == 0xFE) { |
||||
/* Продолжение фрагментированного пакета */ |
||||
if (!pn->u.unpacker.in_fragment) { |
||||
/* Не было начала фрагмента - ошибка */ |
||||
pn->u.unpacker.error_count++; |
||||
goto err; |
||||
} |
||||
size_t chunk_len = cg_len - cg_pos; |
||||
size_t new_len = pn->u.unpacker.len + chunk_len; |
||||
if (new_len > pn->u.unpacker.cap) { |
||||
size_t new_cap = pn->u.unpacker.cap ? pn->u.unpacker.cap * 2 : 4096; |
||||
if (new_cap < new_len) new_cap = new_len; |
||||
pn->u.unpacker.buf = realloc(pn->u.unpacker.buf, new_cap); |
||||
pn->u.unpacker.cap = new_cap; |
||||
} |
||||
memcpy(pn->u.unpacker.buf + pn->u.unpacker.len, cg + cg_pos, chunk_len); |
||||
pn->u.unpacker.len = new_len; |
||||
cg_pos += chunk_len; |
||||
/* Проверить, не собрали ли уже весь пакет */ |
||||
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
||||
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
||||
uint8_t* out = queue_data_new(pn->u.unpacker.total_len); |
||||
if (out) { |
||||
memcpy(out, pn->u.unpacker.buf, pn->u.unpacker.total_len); |
||||
queue_data_put(pn->output, out, 0); |
||||
} |
||||
} else { |
||||
/* Слишком много данных - ошибка */ |
||||
pn->u.unpacker.error_count++; |
||||
} |
||||
reset_fragment_state(pn); |
||||
} |
||||
continue; |
||||
} |
||||
if (byte == 0xFC || byte == 0xFD) { |
||||
/* Service packet */ |
||||
if (byte == 0xFC) { |
||||
/* Start of service packet */ |
||||
if (pn->u.unpacker.in_service) { |
||||
/* Previous service packet finished - deliver it */ |
||||
if (pn->service_callback) { |
||||
pn->service_callback(pn->service_callback_user_data, |
||||
pn->u.unpacker.service_type, |
||||
pn->u.unpacker.service_buf, |
||||
pn->u.unpacker.service_len); |
||||
} |
||||
free(pn->u.unpacker.service_buf); |
||||
pn->u.unpacker.service_buf = NULL; |
||||
pn->u.unpacker.service_len = 0; |
||||
pn->u.unpacker.service_cap = 0; |
||||
pn->u.unpacker.in_service = 0; |
||||
} |
||||
/* Read service type */ |
||||
if (cg_pos >= cg_len) goto err; |
||||
uint8_t service_type = cg[cg_pos++]; |
||||
pn->u.unpacker.service_type = service_type; |
||||
pn->u.unpacker.in_service = 1; |
||||
pn->u.unpacker.service_len = 0; |
||||
} else { |
||||
/* 0xFD - continuation */ |
||||
if (!pn->u.unpacker.in_service) { |
||||
/* No service packet started - error */ |
||||
pn->u.unpacker.error_count++; |
||||
goto err; |
||||
} |
||||
} |
||||
/* Read data */ |
||||
size_t data_len = cg_len - cg_pos; |
||||
if (data_len > 0) { |
||||
size_t new_len = pn->u.unpacker.service_len + data_len; |
||||
if (new_len > 256) { |
||||
/* Service packet too long - error */ |
||||
pn->u.unpacker.error_count++; |
||||
free(pn->u.unpacker.service_buf); |
||||
pn->u.unpacker.service_buf = NULL; |
||||
pn->u.unpacker.service_len = 0; |
||||
pn->u.unpacker.service_cap = 0; |
||||
pn->u.unpacker.in_service = 0; |
||||
goto err; |
||||
} |
||||
if (new_len > pn->u.unpacker.service_cap) { |
||||
size_t new_cap = pn->u.unpacker.service_cap ? pn->u.unpacker.service_cap * 2 : 256; |
||||
if (new_cap < new_len) new_cap = new_len; |
||||
if (new_cap > 256) new_cap = 256; |
||||
uint8_t* new_buf = realloc(pn->u.unpacker.service_buf, new_cap); |
||||
if (!new_buf) { |
||||
pn->u.unpacker.error_count++; |
||||
goto err; |
||||
} |
||||
pn->u.unpacker.service_buf = new_buf; |
||||
pn->u.unpacker.service_cap = new_cap; |
||||
} |
||||
memcpy(pn->u.unpacker.service_buf + pn->u.unpacker.service_len, cg + cg_pos, data_len); |
||||
pn->u.unpacker.service_len = new_len; |
||||
cg_pos += data_len; |
||||
} |
||||
/* Check if this is the end of service packet (end of payload) */ |
||||
if (cg_pos >= cg_len) { |
||||
/* End of current payload, but service packet may continue in next transport packet */ |
||||
continue; |
||||
} else { |
||||
/* There is more data in this payload after service packet - error */ |
||||
pn->u.unpacker.error_count++; |
||||
free(pn->u.unpacker.service_buf); |
||||
pn->u.unpacker.service_buf = NULL; |
||||
pn->u.unpacker.service_len = 0; |
||||
pn->u.unpacker.service_cap = 0; |
||||
pn->u.unpacker.in_service = 0; |
||||
goto err; |
||||
} |
||||
} |
||||
/* Обычная запись (не фрагмент) */ |
||||
size_t L; |
||||
if (byte <= 0xEF) { |
||||
L = byte; |
||||
} else if (byte >= 0xF0 && byte <= 0xF5) { |
||||
if (cg_pos >= cg_len) goto err; |
||||
uint8_t ext = cg[cg_pos++]; |
||||
L = ((size_t)(byte - 0xF0) << 8) | ext; |
||||
} else { |
||||
/* Недопустимый байт */ |
||||
goto err; |
||||
|
||||
uint16_t chunk = pn->pkt_size - 2; |
||||
if (pn->len + chunk > pn->cap) { |
||||
pn->cap *= 2; |
||||
pn->out_buf = realloc(pn->out_buf, pn->cap); |
||||
} |
||||
memcpy(pn->out_buf + pn->len, data + 2, chunk); |
||||
pn->len += chunk; |
||||
|
||||
if (pn->len >= pn->total_len) { |
||||
// Assembly complete: add to output_queue
|
||||
add_to_output(pn, pn->out_buf, pn->total_len); |
||||
pn_unpacker_reset_state(pn); |
||||
} |
||||
} else { |
||||
if (header == 0xffff) { |
||||
// Start of fragment
|
||||
pn->total_len = *(uint32_t*)(data + 2); |
||||
uint16_t chunk = pn->pkt_size - 6; |
||||
if (pn->total_len <= chunk) { |
||||
// Invalid or degenerate case
|
||||
pn_unpacker_reset_state(pn); |
||||
return; |
||||
} |
||||
if (cg_pos + L > cg_len) goto err; |
||||
if (pn->u.unpacker.in_fragment) { |
||||
/* Это последний фрагмент в виде обычной записи */ |
||||
size_t new_len = pn->u.unpacker.len + L; |
||||
if (new_len > pn->u.unpacker.cap) { |
||||
size_t new_cap = pn->u.unpacker.cap ? pn->u.unpacker.cap * 2 : 4096; |
||||
if (new_cap < new_len) new_cap = new_len; |
||||
pn->u.unpacker.buf = realloc(pn->u.unpacker.buf, new_cap); |
||||
pn->u.unpacker.cap = new_cap; |
||||
} |
||||
memcpy(pn->u.unpacker.buf + pn->u.unpacker.len, cg + cg_pos, L); |
||||
pn->u.unpacker.len = new_len; |
||||
cg_pos += L; |
||||
/* Проверить, собрали ли весь пакет */ |
||||
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
||||
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
||||
uint8_t* out = queue_data_new(pn->u.unpacker.total_len); |
||||
if (out) { |
||||
memcpy(out, pn->u.unpacker.buf, pn->u.unpacker.total_len); |
||||
queue_data_put(pn->output, out, 0); |
||||
} |
||||
} else { |
||||
/* Слишком много данных - ошибка */ |
||||
pn->u.unpacker.error_count++; |
||||
} |
||||
reset_fragment_state(pn); |
||||
} |
||||
} else { |
||||
/* Обычная запись (не часть фрагмента) */ |
||||
uint8_t* out = queue_data_new(L); |
||||
if (out) { |
||||
memcpy(out, cg + cg_pos, L); |
||||
queue_data_put(pn->output, out, 0); |
||||
} |
||||
cg_pos += L; |
||||
memcpy(pn->out_buf, data + 6, chunk); |
||||
pn->len = chunk; |
||||
pn->in_fragment = 1; |
||||
} else { |
||||
// Normal aggregated packet: process multiple items (ignore pad)
|
||||
uint8_t* ptr = data; |
||||
size_t processed = 0; |
||||
while (processed < pn->pkt_size) { |
||||
header = *(uint16_t*)ptr; |
||||
if (header == 0 || header == 0xffff || header == 0x0000) break; // End (pad) or error
|
||||
if (processed + 2 + header > pn->pkt_size) break; |
||||
|
||||
add_to_output(pn, ptr + 2, header); |
||||
|
||||
ptr += 2 + header; |
||||
processed += 2 + header; |
||||
} |
||||
} |
||||
err: |
||||
queue_data_free(entry); |
||||
} |
||||
queue_resume_callback(q); |
||||
} |
||||
|
||||
@ -1,75 +1,59 @@
|
||||
// pkt_normalizer.h
|
||||
// pkt_normalizer.h (упрощенная версия)
|
||||
#ifndef PKT_NORMALIZER_H |
||||
#define PKT_NORMALIZER_H |
||||
|
||||
#include "ll_queue.h" |
||||
#include "../lib/ll_queue.h" |
||||
#include "../lib/u_async.h" |
||||
#include <stdint.h> |
||||
|
||||
/* Default fragment reassembly timeout in uasync timebase units (0.1 ms) */ |
||||
#ifndef PKT_NORMALIZER_FRAGMENT_TIMEOUT |
||||
#define PKT_NORMALIZER_FRAGMENT_TIMEOUT 5000 /* 500 ms */ |
||||
#endif |
||||
// Структура для packer
|
||||
struct PKTNORM { |
||||
// public:
|
||||
struct ll_queue* input; // Входная очередь в packer (через нее отправляем пакеты)
|
||||
struct ll_queue* output; // Выходная очередь из unpacker (через нее принимаем пакеты)
|
||||
|
||||
// private:
|
||||
struct ETCP_CONN* etcp; |
||||
uint16_t pkt_size; |
||||
|
||||
|
||||
// packer:
|
||||
uasync_t* ua; // uasync instance
|
||||
uint8_t* in_buf; // Буфер для упаковки
|
||||
size_t len; // Текущая длина в буфере
|
||||
size_t cap; // Емкость буфера (MAX_PACKET_SIZE)
|
||||
void* flush_timer; // For timeout flush
|
||||
struct ll_queue* pending; // For not filling input
|
||||
|
||||
// unpacker:
|
||||
uint8_t* out_buf; // Буфер для сборки фрагментов
|
||||
// size_t len; // Текущая накопленная длина
|
||||
size_t total_len; // Ожидаемая общая длина (для фрагментов)
|
||||
// size_t cap; // Емкость буфера
|
||||
int in_fragment; // Флаг: идет сборка фрагмента (1) или нет (0)
|
||||
struct ll_queue* send_pending; // For not filling etcp->input_queue
|
||||
|
||||
/* ETCP overhead for calculating fragment size from MTU */ |
||||
#define ETCP_OVERHEAD 100 // Reserve 100 bytes for headers, crypto, etc.*/
|
||||
|
||||
/* Service packet callback type */ |
||||
typedef void (*pkt_normalizer_service_callback_t)(void* user_data, uint8_t type, const uint8_t* data, size_t len); |
||||
|
||||
struct pn_struct { |
||||
struct ll_queue* input; |
||||
struct ll_queue* output; |
||||
uasync_t* ua; |
||||
int is_packer; |
||||
union { |
||||
struct { |
||||
uint8_t* buf; |
||||
size_t len; |
||||
size_t cap; |
||||
int error_count; |
||||
} packer; |
||||
struct { |
||||
uint8_t* buf; /* буфер для сборки фрагментов */ |
||||
size_t len; /* текущая накопленная длина */ |
||||
size_t total_len; /* ожидаемая общая длина из первого фрагмента */ |
||||
size_t cap; /* ёмкость буфера */ |
||||
int error_count; /* счетчик ошибок сборки */ |
||||
int in_fragment; /* флаг: идет сборка фрагментов (1) или нет (0) */ |
||||
/* Service packet reassembly */ |
||||
uint8_t* service_buf; /* буфер для сборки сервисных пакетов */ |
||||
size_t service_len; /* текущая накопленная длина сервисного пакета */ |
||||
size_t service_cap; /* ёмкость буфера сервисного пакета */ |
||||
uint8_t service_type; /* тип сервисного пакета */ |
||||
int in_service; /* флаг: идет сборка сервисного пакета (1) или нет (0) */ |
||||
} unpacker; |
||||
} u; |
||||
/* Service packet callback */ |
||||
pkt_normalizer_service_callback_t service_callback; |
||||
void* service_callback_user_data; |
||||
}; |
||||
|
||||
struct pkt_normalizer_pair { |
||||
struct pn_struct* packer; |
||||
struct pn_struct* unpacker; |
||||
}; |
||||
// Инициализация пары
|
||||
struct PKTNORM* pn_init(struct ETCP_CONN* etcp);// все что нужно (в т.ч. mtu и ua) берет из etcp
|
||||
|
||||
// Деинициализация пары
|
||||
void pn_pair_deinit(struct PKTNORM* pn); |
||||
|
||||
struct pn_struct* pkt_normalizer_init(uasync_t* ua, int is_packer, int mtu); // 1 for packer, 0 for unpacker, mtu for fragment size calculation
|
||||
void pkt_normalizer_deinit(struct pn_struct* pn); |
||||
// Сброс состояния (для unpacker)
|
||||
void pn_unpacker_reset_state(struct PKTNORM* pn); |
||||
|
||||
struct pkt_normalizer_pair* pkt_normalizer_pair_init(uasync_t* ua, int mtu); |
||||
void pkt_normalizer_pair_deinit(struct pkt_normalizer_pair* pair); |
||||
// создаёт malloc data, копирует, помещает в input.
|
||||
void pn_packer_send(struct PKTNORM* pn, uint8_t* data, uint16_t len); |
||||
|
||||
/* Error handling */ |
||||
int pkt_normalizer_get_error_count(const struct pn_struct* pn); |
||||
void pkt_normalizer_reset_error_count(struct pn_struct* pn); |
||||
/* Как работает:
|
||||
Формат отправки в etcp: 2 байта размер, далее данные (порезанные на куски и отправленные через etcp) |
||||
собирает по возможности полные пакеты с размером pkt_size. |
||||
+ timeout: неполный пакет отправляется по таймауту 1ms (если очереди пустые) |
||||
+ входящая очередь etcp не должне наполняться для минимизации задержки - новый пакет отправляем только когда очередь пустая |
||||
|
||||
/* Flush internal buffer (packer only) */ |
||||
void pkt_normalizer_flush(struct pn_struct* pn); |
||||
*/ |
||||
|
||||
int pkt_normalizer_send_service(struct pn_struct* pn, uint8_t type, const void* data, size_t len); |
||||
void pkt_normalizer_set_service_callback(struct pn_struct* pn, pkt_normalizer_service_callback_t callback, void* user_data); |
||||
void pkt_normalizer_reset_service_state(struct pn_struct* pn); |
||||
void pkt_normalizer_reset_state(struct pn_struct* pn); |
||||
|
||||
#endif // PKT_NORMALIZER_H
|
||||
#endif // PKT_NORMALIZER_H
|
||||
@ -1,369 +0,0 @@
|
||||
/* sc_lib.c - Secure Channel library implementation using TinyCrypt */ |
||||
|
||||
#include "secure_channel.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ecc.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ecc_dh.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/aes.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ccm_mode.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/constants.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ecc_platform_specific.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/sha256.h" |
||||
#include <string.h> |
||||
#include <stddef.h> |
||||
#include <sys/types.h> |
||||
#include <unistd.h> |
||||
#include <sys/time.h> |
||||
#include <stdio.h> |
||||
|
||||
// Simple debug macros
|
||||
#define DEBUG_CATEGORY_CRYPTO 1 |
||||
#define DEBUG_ERROR(category, fmt, ...) fprintf(stderr, "ERROR: " fmt "\n", ##__VA_ARGS__) |
||||
#define DEBUG_INFO(category, fmt, ...) fprintf(stdout, "INFO: " fmt "\n", ##__VA_ARGS__) |
||||
#include <stdio.h> |
||||
#include <fcntl.h> |
||||
#include "crc32.h" |
||||
|
||||
static const struct uECC_Curve_t *curve = NULL; |
||||
static uint8_t sc_urandom_seed[8] = {0}; |
||||
static int sc_urandom_initialized = 0; |
||||
|
||||
static void sc_init_random_seed(void) |
||||
{ |
||||
int fd = open("/dev/urandom", O_RDONLY); |
||||
if (fd >= 0) { |
||||
ssize_t ret = read(fd, sc_urandom_seed, 8); |
||||
close(fd); |
||||
if (ret == 8) { |
||||
sc_urandom_initialized = 1; |
||||
} |
||||
} |
||||
} |
||||
|
||||
|
||||
static int sc_rng(uint8_t *dest, unsigned size) |
||||
{ |
||||
int fd = open("/dev/urandom", O_RDONLY); |
||||
if (fd < 0) { |
||||
return 0; |
||||
} |
||||
|
||||
ssize_t ret = read(fd, dest, size); |
||||
close(fd); |
||||
if (ret != size) { |
||||
return 0; |
||||
} |
||||
|
||||
/* Mix in PID and microtime for additional entropy */ |
||||
pid_t pid = getpid(); |
||||
struct timeval tv; |
||||
gettimeofday(&tv, NULL); |
||||
|
||||
for (unsigned i = 0; i < size; i++) { |
||||
dest[i] ^= ((pid >> (i % (sizeof(pid) * 8))) & 0xFF); |
||||
dest[i] ^= ((tv.tv_sec >> (i % (sizeof(tv.tv_sec) * 8))) & 0xFF); |
||||
dest[i] ^= ((tv.tv_usec >> (i % (sizeof(tv.tv_usec) * 8))) & 0xFF); |
||||
} |
||||
|
||||
return 1; |
||||
} |
||||
|
||||
static int sc_validate_key(const uint8_t *public_key) |
||||
{ |
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
int result = uECC_valid_public_key(public_key, curve); |
||||
DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "sc_validate_key: uECC_valid_public_key returned %d", result); |
||||
return result; |
||||
} |
||||
|
||||
sc_status_t sc_generate_keypair(struct SC_MYKEYS *pk) |
||||
{ |
||||
if (!pk) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
/* Set custom RNG function */ |
||||
uECC_set_rng(sc_rng); |
||||
|
||||
if (!uECC_make_key(pk->public_key, pk->private_key, curve)) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
return SC_OK; |
||||
} |
||||
|
||||
// Конвертация hex строки в бинарный формат
|
||||
static int hex_to_binary(const char *hex_str, uint8_t *binary, size_t binary_len) { |
||||
if (!hex_str || !binary || strlen(hex_str) != binary_len * 2) return -1; |
||||
|
||||
for (size_t i = 0; i < binary_len; i++) { |
||||
unsigned int byte; |
||||
if (sscanf(hex_str + i * 2, "%2x", &byte) != 1) return -1; |
||||
binary[i] = (uint8_t)byte; |
||||
} |
||||
return 0; |
||||
} |
||||
|
||||
sc_status_t sc_init_local_keys(struct SC_MYKEYS *mykeys, const char *public_key, const char *private_key) { |
||||
if (!mykeys || !public_key || !private_key) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: invalid arguments"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: public_key len=%zu, private_key len=%zu",
|
||||
strlen(public_key), strlen(private_key)); |
||||
|
||||
/* Convert hex to binary first */ |
||||
if (hex_to_binary(public_key, mykeys->public_key, SC_PUBKEY_SIZE)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: failed to convert public key from hex"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
if (hex_to_binary(private_key, mykeys->private_key, SC_PRIVKEY_SIZE)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: failed to convert private key from hex"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Validate the converted binary public key */ |
||||
if (sc_validate_key(mykeys->public_key) != 0) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: public key validation failed"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: keys initialized successfully"); |
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_init_ctx(sc_context_t *ctx, struct SC_MYKEYS *mykeys) { |
||||
|
||||
ctx->pk=mykeys; |
||||
ctx->initialized = 1; |
||||
ctx->peer_key_set = 0; |
||||
ctx->session_ready = 0; |
||||
ctx->tx_counter = 0; |
||||
ctx->rx_counter = 0; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_set_peer_public_key(sc_context_t *ctx, const char *peer_public_key_h, int mode) { |
||||
uint8_t shared_secret[SC_SHARED_SECRET_SIZE]; |
||||
uint8_t peer_public_key[SC_PUBKEY_SIZE]; |
||||
|
||||
if (mode) { |
||||
if (hex_to_binary(peer_public_key_h, peer_public_key, SC_PUBKEY_SIZE)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: invalid hex key format"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
} |
||||
else memcpy(peer_public_key, peer_public_key_h, SC_PUBKEY_SIZE); |
||||
|
||||
if (!ctx) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: invalid ctx"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!ctx->initialized) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: ctx not initialized"); |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
/* Validate peer public key */ |
||||
if (sc_validate_key(peer_public_key) != 0) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: invalid key"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Compute shared secret using ECDH */ |
||||
if (!ctx->pk) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: no private key"); |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
if (!uECC_shared_secret(peer_public_key, ctx->pk->private_key, |
||||
shared_secret, curve)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: shared secret error"); |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Derive session key from shared secret (simple copy for demo) */ |
||||
memcpy(ctx->session_key, shared_secret, SC_SESSION_KEY_SIZE); |
||||
|
||||
/* Store peer public key */ |
||||
memcpy(ctx->peer_public_key, peer_public_key, SC_PUBKEY_SIZE); |
||||
ctx->peer_key_set = 1; |
||||
|
||||
ctx->session_ready = 1; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
// Новая функция для генерации nonce с микросекундами
|
||||
static void generate_nonce_with_timer(uint8_t *nonce) { |
||||
struct timeval tv; |
||||
gettimeofday(&tv, NULL); |
||||
uint32_t usec = (uint32_t)tv.tv_usec; |
||||
|
||||
// Поместить usec в первые 4 байта (little-endian)
|
||||
nonce[0] = usec & 0xFF; |
||||
nonce[1] = (usec >> 8) & 0xFF; |
||||
nonce[2] = (usec >> 16) & 0xFF; |
||||
nonce[3] = (usec >> 24) & 0xFF; |
||||
|
||||
// Заполнить оставшиеся 9 байт случайными данными
|
||||
sc_rng(nonce + 4, SC_NONCE_SIZE - 4); |
||||
} |
||||
|
||||
sc_status_t sc_encrypt(sc_context_t *ctx, const uint8_t *plaintext, size_t plaintext_len, uint8_t *ciphertext, size_t *ciphertext_len) { |
||||
uint8_t nonce[SC_NONCE_SIZE]; |
||||
uint8_t plaintext_with_crc[plaintext_len + SC_CRC32_SIZE]; |
||||
size_t total_plaintext_len = plaintext_len + SC_CRC32_SIZE; |
||||
uint8_t combined_output[total_plaintext_len + SC_TAG_SIZE]; |
||||
struct tc_aes_key_sched_struct sched; |
||||
struct tc_ccm_mode_struct ccm_state; |
||||
|
||||
if (!ctx || !plaintext || !ciphertext || !ciphertext_len) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!ctx->session_ready) { |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
|
||||
if (plaintext_len == 0) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Добавляем CRC32 к данным */ |
||||
memcpy(plaintext_with_crc, plaintext, plaintext_len); |
||||
uint32_t crc = crc32_calc(plaintext, plaintext_len); |
||||
plaintext_with_crc[plaintext_len] = (crc >> 0) & 0xFF; |
||||
plaintext_with_crc[plaintext_len + 1] = (crc >> 8) & 0xFF; |
||||
plaintext_with_crc[plaintext_len + 2] = (crc >> 16) & 0xFF; |
||||
plaintext_with_crc[plaintext_len + 3] = (crc >> 24) & 0xFF; |
||||
|
||||
/* Генерируем nonce с таймером */ |
||||
generate_nonce_with_timer(nonce); |
||||
|
||||
/* Initialize AES key schedule */ |
||||
if (tc_aes128_set_encrypt_key(&sched, ctx->session_key) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Configure CCM mode */ |
||||
if (tc_ccm_config(&ccm_state, &sched, nonce, SC_NONCE_SIZE, SC_TAG_SIZE) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Encrypt and generate tag */ |
||||
if (tc_ccm_generation_encryption(combined_output, sizeof(combined_output), |
||||
NULL, 0, /* no associated data */ |
||||
plaintext_with_crc, total_plaintext_len, |
||||
&ccm_state) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Copy nonce + ciphertext + tag to output buffer */ |
||||
memcpy(ciphertext, nonce, SC_NONCE_SIZE); |
||||
memcpy(ciphertext + SC_NONCE_SIZE, combined_output, total_plaintext_len + SC_TAG_SIZE); |
||||
*ciphertext_len = SC_NONCE_SIZE + total_plaintext_len + SC_TAG_SIZE; |
||||
|
||||
ctx->tx_counter++; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_decrypt(sc_context_t *ctx, |
||||
const uint8_t *ciphertext, |
||||
size_t ciphertext_len, |
||||
uint8_t *plaintext, |
||||
size_t *plaintext_len) |
||||
{ |
||||
uint8_t nonce[SC_NONCE_SIZE]; |
||||
struct tc_aes_key_sched_struct sched; |
||||
struct tc_ccm_mode_struct ccm_state; |
||||
size_t total_plaintext_len = ciphertext_len - SC_NONCE_SIZE - SC_TAG_SIZE; |
||||
uint8_t plaintext_with_crc[total_plaintext_len]; |
||||
|
||||
if (!ctx || !ciphertext || !plaintext || !plaintext_len) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!ctx->session_ready) { |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
|
||||
if (ciphertext_len < SC_NONCE_SIZE + SC_TAG_SIZE + SC_CRC32_SIZE) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Извлекаем nonce из начала ciphertext */ |
||||
memcpy(nonce, ciphertext, SC_NONCE_SIZE); |
||||
|
||||
/* Ciphertext для расшифровки начинается после nonce */ |
||||
const uint8_t *encrypted_data = ciphertext + SC_NONCE_SIZE; |
||||
size_t encrypted_len = ciphertext_len - SC_NONCE_SIZE; |
||||
|
||||
/* Initialize AES key schedule */ |
||||
if (tc_aes128_set_encrypt_key(&sched, ctx->session_key) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Configure CCM mode с извлечённым nonce */ |
||||
if (tc_ccm_config(&ccm_state, &sched, nonce, SC_NONCE_SIZE, SC_TAG_SIZE) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Decrypt and verify tag */ |
||||
if (tc_ccm_decryption_verification(plaintext_with_crc, total_plaintext_len, |
||||
NULL, 0, /* no associated data */ |
||||
encrypted_data, encrypted_len, |
||||
&ccm_state) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_AUTH_FAILED; |
||||
} |
||||
|
||||
/* Проверяем CRC32 */ |
||||
size_t data_len = total_plaintext_len - SC_CRC32_SIZE; |
||||
uint32_t expected_crc = crc32_calc(plaintext_with_crc, data_len); |
||||
uint32_t received_crc = (plaintext_with_crc[data_len] << 0) | |
||||
(plaintext_with_crc[data_len + 1] << 8) | |
||||
(plaintext_with_crc[data_len + 2] << 16) | |
||||
(plaintext_with_crc[data_len + 3] << 24); |
||||
|
||||
if (expected_crc != received_crc) { |
||||
return SC_ERR_CRC_FAILED; |
||||
} |
||||
|
||||
/* Копируем данные без CRC32 */ |
||||
memcpy(plaintext, plaintext_with_crc, data_len); |
||||
*plaintext_len = data_len; |
||||
|
||||
ctx->rx_counter++; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_compute_public_key_from_private(const uint8_t *private_key, uint8_t *public_key) { |
||||
if (!private_key || !public_key) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
if (!uECC_compute_public_key(private_key, public_key, curve)) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
return SC_OK; |
||||
} |
||||
@ -1,388 +0,0 @@
|
||||
/* sc_lib.c - Secure Channel library implementation using TinyCrypt */ |
||||
|
||||
#include "secure_channel.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ecc.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ecc_dh.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/aes.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ccm_mode.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/constants.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/ecc_platform_specific.h" |
||||
#include "../tinycrypt/lib/include/tinycrypt/sha256.h" |
||||
#include <string.h> |
||||
#include <stddef.h> |
||||
#include <sys/types.h> |
||||
#include <unistd.h> |
||||
#include <sys/time.h> |
||||
#include <stdio.h> |
||||
|
||||
// Simple debug macros |
||||
#define DEBUG_CATEGORY_CRYPTO 1 |
||||
#define DEBUG_ERROR(category, fmt, ...) fprintf(stderr, "ERROR: " fmt "\n", ##__VA_ARGS__) |
||||
#define DEBUG_INFO(category, fmt, ...) fprintf(stdout, "INFO: " fmt "\n", ##__VA_ARGS__) |
||||
#include <stdio.h> |
||||
#include <fcntl.h> |
||||
#include "crc32.h" |
||||
|
||||
static const struct uECC_Curve_t *curve = NULL; |
||||
static uint8_t sc_urandom_seed[8] = {0}; |
||||
static int sc_urandom_initialized = 0; |
||||
|
||||
static void sc_init_random_seed(void) |
||||
{ |
||||
int fd = open("/dev/urandom", O_RDONLY); |
||||
if (fd >= 0) { |
||||
ssize_t ret = read(fd, sc_urandom_seed, 8); |
||||
close(fd); |
||||
if (ret == 8) { |
||||
sc_urandom_initialized = 1; |
||||
} |
||||
} |
||||
} |
||||
|
||||
|
||||
static int sc_rng(uint8_t *dest, unsigned size) |
||||
{ |
||||
int fd = open("/dev/urandom", O_RDONLY); |
||||
if (fd < 0) { |
||||
return 0; |
||||
} |
||||
|
||||
ssize_t ret = read(fd, dest, size); |
||||
close(fd); |
||||
if (ret != size) { |
||||
return 0; |
||||
} |
||||
|
||||
/* Mix in PID and microtime for additional entropy */ |
||||
pid_t pid = getpid(); |
||||
struct timeval tv; |
||||
gettimeofday(&tv, NULL); |
||||
|
||||
for (unsigned i = 0; i < size; i++) { |
||||
dest[i] ^= ((pid >> (i % (sizeof(pid) * 8))) & 0xFF); |
||||
dest[i] ^= ((tv.tv_sec >> (i % (sizeof(tv.tv_sec) * 8))) & 0xFF); |
||||
dest[i] ^= ((tv.tv_usec >> (i % (sizeof(tv.tv_usec) * 8))) & 0xFF); |
||||
} |
||||
|
||||
return 1; |
||||
} |
||||
|
||||
static int sc_validate_key(const uint8_t *public_key) |
||||
{ |
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
int result = uECC_valid_public_key(public_key, curve); |
||||
DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "sc_validate_key: uECC_valid_public_key returned %d", result); |
||||
return result; |
||||
} |
||||
|
||||
sc_status_t sc_generate_keypair(struct SC_MYKEYS *pk) |
||||
{ |
||||
if (!pk) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
/* Set custom RNG function */ |
||||
uECC_set_rng(sc_rng); |
||||
|
||||
if (!uECC_make_key(pk->public_key, pk->private_key, curve)) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
return SC_OK; |
||||
} |
||||
|
||||
// Конвертация hex строки в бинарный формат |
||||
static int hex_to_binary(const char *hex_str, uint8_t *binary, size_t binary_len) { |
||||
if (!hex_str || !binary || strlen(hex_str) != binary_len * 2) return -1; |
||||
|
||||
for (size_t i = 0; i < binary_len; i++) { |
||||
unsigned int byte; |
||||
if (sscanf(hex_str + i * 2, "%2x", &byte) != 1) return -1; |
||||
binary[i] = (uint8_t)byte; |
||||
} |
||||
return 0; |
||||
} |
||||
|
||||
sc_status_t sc_init_local_keys(struct SC_MYKEYS *mykeys, const char *public_key, const char *private_key) { |
||||
if (!mykeys || !public_key || !private_key) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: invalid arguments"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: public_key len=%zu, private_key len=%zu", |
||||
strlen(public_key), strlen(private_key)); |
||||
|
||||
/* Convert hex to binary first */ |
||||
if (hex_to_binary(public_key, mykeys->public_key, SC_PUBKEY_SIZE)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: failed to convert public key from hex"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
if (hex_to_binary(private_key, mykeys->private_key, SC_PRIVKEY_SIZE)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: failed to convert private key from hex"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Validate the converted binary public key */ |
||||
if (sc_validate_key(mykeys->public_key) != 0) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: public key validation failed"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
DEBUG_INFO(DEBUG_CATEGORY_CRYPTO, "sc_init_local_keys: keys initialized successfully"); |
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_init_ctx(sc_context_t *ctx, struct SC_MYKEYS *mykeys) { |
||||
|
||||
ctx->pk=mykeys; |
||||
ctx->initialized = 1; |
||||
ctx->peer_key_set = 0; |
||||
ctx->session_ready = 0; |
||||
ctx->tx_counter = 0; |
||||
ctx->rx_counter = 0; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_set_peer_public_key(sc_context_t *ctx, const char *peer_public_key_h, int mode) { |
||||
uint8_t shared_secret[SC_SHARED_SECRET_SIZE]; |
||||
uint8_t peer_public_key[SC_PUBKEY_SIZE]; |
||||
|
||||
if (mode) { |
||||
if (hex_to_binary(peer_public_key_h, peer_public_key, SC_PUBKEY_SIZE)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: invalid hex key format"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
} |
||||
else memcpy(peer_public_key, peer_public_key_h, SC_PUBKEY_SIZE); |
||||
|
||||
if (!ctx) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: invalid ctx"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!ctx->initialized) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: ctx not initialized"); |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
/* Validate peer public key */ |
||||
if (sc_validate_key(peer_public_key) != 0) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: invalid key"); |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Compute shared secret using ECDH */ |
||||
if (!ctx->pk) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: no private key"); |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
if (!uECC_shared_secret(peer_public_key, ctx->pk->private_key, |
||||
shared_secret, curve)) { |
||||
DEBUG_ERROR(DEBUG_CATEGORY_CRYPTO, "sc_set_peer_public_key: shared secret error"); |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Derive session key from shared secret (simple copy for demo) */ |
||||
memcpy(ctx->session_key, shared_secret, SC_SESSION_KEY_SIZE); |
||||
|
||||
/* Store peer public key */ |
||||
memcpy(ctx->peer_public_key, peer_public_key, SC_PUBKEY_SIZE); |
||||
ctx->peer_key_set = 1; |
||||
|
||||
ctx->session_ready = 1; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
static void sc_build_nonce(uint64_t counter, uint8_t *nonce_out) |
||||
{ |
||||
struct tc_sha256_state_struct sha_ctx; |
||||
uint8_t hash[32]; |
||||
struct timeval tv; |
||||
uint8_t data[8 + 8 + 4]; |
||||
|
||||
if (!sc_urandom_initialized) { |
||||
sc_init_random_seed(); |
||||
} |
||||
|
||||
gettimeofday(&tv, NULL); |
||||
|
||||
memcpy(data, sc_urandom_seed, 8); |
||||
data[8] = (counter >> 0) & 0xFF; |
||||
data[9] = (counter >> 8) & 0xFF; |
||||
data[10] = (counter >> 16) & 0xFF; |
||||
data[11] = (counter >> 24) & 0xFF; |
||||
data[12] = (counter >> 32) & 0xFF; |
||||
data[13] = (counter >> 40) & 0xFF; |
||||
data[14] = (counter >> 48) & 0xFF; |
||||
data[15] = (counter >> 56) & 0xFF; |
||||
data[16] = (tv.tv_sec >> 0) & 0xFF; |
||||
data[17] = (tv.tv_sec >> 8) & 0xFF; |
||||
data[18] = (tv.tv_sec >> 16) & 0xFF; |
||||
data[19] = (tv.tv_sec >> 24) & 0xFF; |
||||
|
||||
tc_sha256_init(&sha_ctx); |
||||
tc_sha256_update(&sha_ctx, data, 20); |
||||
tc_sha256_final(hash, &sha_ctx); |
||||
|
||||
memcpy(nonce_out, hash, SC_NONCE_SIZE); |
||||
} |
||||
|
||||
sc_status_t sc_encrypt(sc_context_t *ctx, |
||||
const uint8_t *plaintext, |
||||
size_t plaintext_len, |
||||
uint8_t *ciphertext, |
||||
size_t *ciphertext_len) |
||||
{ |
||||
uint8_t nonce[SC_NONCE_SIZE]; |
||||
struct tc_aes_key_sched_struct sched; |
||||
struct tc_ccm_mode_struct ccm_state; |
||||
size_t total_plaintext_len = plaintext_len + SC_CRC32_SIZE; |
||||
uint8_t plaintext_with_crc[total_plaintext_len]; |
||||
uint8_t combined_output[total_plaintext_len + SC_TAG_SIZE]; |
||||
|
||||
if (!ctx || !plaintext || !ciphertext || !ciphertext_len) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!ctx->session_ready) { |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
|
||||
if (plaintext_len == 0) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Добавляем CRC32 к данным */ |
||||
memcpy(plaintext_with_crc, plaintext, plaintext_len); |
||||
uint32_t crc = crc32_calc(plaintext, plaintext_len); |
||||
plaintext_with_crc[plaintext_len] = (crc >> 0) & 0xFF; |
||||
plaintext_with_crc[plaintext_len + 1] = (crc >> 8) & 0xFF; |
||||
plaintext_with_crc[plaintext_len + 2] = (crc >> 16) & 0xFF; |
||||
plaintext_with_crc[plaintext_len + 3] = (crc >> 24) & 0xFF; |
||||
|
||||
/* Initialize AES key schedule */ |
||||
if (tc_aes128_set_encrypt_key(&sched, ctx->session_key) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Build nonce from counter */ |
||||
sc_build_nonce(ctx->tx_counter, nonce); |
||||
|
||||
/* Configure CCM mode */ |
||||
if (tc_ccm_config(&ccm_state, &sched, nonce, SC_NONCE_SIZE, SC_TAG_SIZE) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Encrypt and generate tag */ |
||||
if (tc_ccm_generation_encryption(combined_output, sizeof(combined_output), |
||||
NULL, 0, /* no associated data */ |
||||
plaintext_with_crc, total_plaintext_len, |
||||
&ccm_state) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Copy ciphertext + tag to output buffer */ |
||||
memcpy(ciphertext, combined_output, total_plaintext_len + SC_TAG_SIZE); |
||||
*ciphertext_len = total_plaintext_len + SC_TAG_SIZE; |
||||
|
||||
ctx->tx_counter++; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_decrypt(sc_context_t *ctx, |
||||
const uint8_t *ciphertext, |
||||
size_t ciphertext_len, |
||||
uint8_t *plaintext, |
||||
size_t *plaintext_len) |
||||
{ |
||||
uint8_t nonce[SC_NONCE_SIZE]; |
||||
struct tc_aes_key_sched_struct sched; |
||||
struct tc_ccm_mode_struct ccm_state; |
||||
TCCcmMode_t c = &ccm_state; |
||||
size_t total_plaintext_len = ciphertext_len - SC_TAG_SIZE; |
||||
uint8_t plaintext_with_crc[total_plaintext_len]; |
||||
|
||||
if (!ctx || !ciphertext || !plaintext || !plaintext_len) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!ctx->session_ready) { |
||||
return SC_ERR_NOT_INITIALIZED; |
||||
} |
||||
|
||||
if (ciphertext_len < SC_TAG_SIZE + SC_CRC32_SIZE) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
/* Initialize AES key schedule */ |
||||
if (tc_aes128_set_encrypt_key(&sched, ctx->session_key) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Build nonce from counter */ |
||||
sc_build_nonce(ctx->rx_counter, nonce); |
||||
|
||||
/* Configure CCM mode */ |
||||
if (tc_ccm_config(c, &sched, nonce, SC_NONCE_SIZE, SC_TAG_SIZE) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
|
||||
/* Decrypt and verify tag */ |
||||
if (tc_ccm_decryption_verification(plaintext_with_crc, total_plaintext_len, |
||||
NULL, 0, /* no associated data */ |
||||
ciphertext, ciphertext_len, |
||||
c) != TC_CRYPTO_SUCCESS) { |
||||
return SC_ERR_AUTH_FAILED; |
||||
} |
||||
|
||||
/* Проверяем CRC32 */ |
||||
size_t data_len = total_plaintext_len - SC_CRC32_SIZE; |
||||
uint32_t expected_crc = crc32_calc(plaintext_with_crc, data_len); |
||||
uint32_t received_crc = (plaintext_with_crc[data_len] << 0) | |
||||
(plaintext_with_crc[data_len + 1] << 8) | |
||||
(plaintext_with_crc[data_len + 2] << 16) | |
||||
(plaintext_with_crc[data_len + 3] << 24); |
||||
|
||||
if (expected_crc != received_crc) { |
||||
return SC_ERR_CRC_FAILED; |
||||
} |
||||
|
||||
/* Копируем данные без CRC32 */ |
||||
memcpy(plaintext, plaintext_with_crc, data_len); |
||||
*plaintext_len = data_len; |
||||
|
||||
ctx->rx_counter++; |
||||
|
||||
return SC_OK; |
||||
} |
||||
|
||||
sc_status_t sc_compute_public_key_from_private(const uint8_t *private_key, uint8_t *public_key) { |
||||
if (!private_key || !public_key) { |
||||
return SC_ERR_INVALID_ARG; |
||||
} |
||||
|
||||
if (!curve) { |
||||
curve = uECC_secp256r1(); |
||||
} |
||||
|
||||
if (!uECC_compute_public_key(private_key, public_key, curve)) { |
||||
return SC_ERR_CRYPTO; |
||||
} |
||||
return SC_OK; |
||||
} |
||||
@ -1,68 +0,0 @@
|
||||
// secure_channel.h |
||||
#ifndef SECURE_CHANNEL_H |
||||
#define SECURE_CHANNEL_H |
||||
|
||||
#include <stdint.h> |
||||
#include <stddef.h> |
||||
|
||||
// Размеры ключей |
||||
#define SC_PRIVKEY_SIZE 32 |
||||
#define SC_PUBKEY_SIZE 64 |
||||
#define SC_HASH_SIZE 32 |
||||
#define SC_NONCE_SIZE 13 // CCM requires exactly 13 bytes |
||||
#define SC_SHARED_SECRET_SIZE SC_HASH_SIZE |
||||
#define SC_SESSION_KEY_SIZE 16 |
||||
#define SC_TAG_SIZE 8 |
||||
#define SC_CRC32_SIZE 4 |
||||
|
||||
// Коды возврата |
||||
#define SC_OK 0 |
||||
#define SC_ERR_INVALID_ARG -1 |
||||
#define SC_ERR_CRYPTO -2 |
||||
#define SC_ERR_NOT_INITIALIZED -3 |
||||
#define SC_ERR_AUTH_FAILED -4 |
||||
#define SC_ERR_CRC_FAILED -5 |
||||
|
||||
#define SC_PEER_PUBKEY_BIN 0 |
||||
#define SC_PEER_PUBKEY_HEX 1 |
||||
|
||||
// Типы |
||||
typedef int sc_status_t; |
||||
typedef struct secure_channel sc_context_t; |
||||
|
||||
struct SC_MYKEYS { |
||||
/* Локальные ключи */ |
||||
uint8_t private_key[SC_PRIVKEY_SIZE]; |
||||
uint8_t public_key[SC_PUBKEY_SIZE]; |
||||
}; |
||||
|
||||
// Контекст защищенного канала |
||||
struct secure_channel { |
||||
struct SC_MYKEYS* pk; |
||||
/* Ключи пира (после key exchange) */ |
||||
uint8_t peer_public_key[SC_PUBKEY_SIZE]; |
||||
uint8_t session_key[SC_SESSION_KEY_SIZE]; /* Derived session key */ |
||||
|
||||
/* Nonces для отправки и приема */ |
||||
uint8_t send_nonce[SC_NONCE_SIZE]; |
||||
uint8_t recv_nonce[SC_NONCE_SIZE]; |
||||
|
||||
uint8_t initialized; |
||||
uint8_t peer_key_set; |
||||
uint8_t session_ready; |
||||
uint64_t tx_counter; |
||||
uint64_t rx_counter; |
||||
}; |
||||
|
||||
// Функции инициализации |
||||
sc_status_t sc_init_ctx(sc_context_t *ctx, struct SC_MYKEYS *mykeys); |
||||
sc_status_t sc_generate_keypair(struct SC_MYKEYS *keys); |
||||
sc_status_t sc_init_local_keys(struct SC_MYKEYS *mykeys, const char *public_key, const char *private_key); |
||||
sc_status_t sc_set_peer_public_key(sc_context_t *ctx, const char *peer_public_key, int mode);// mode: 0-bin 1-hex key format |
||||
sc_status_t sc_compute_public_key_from_private(const uint8_t *private_key, uint8_t *public_key); |
||||
|
||||
// Криптографические операции |
||||
sc_status_t sc_encrypt(sc_context_t *ctx, const uint8_t *plaintext, size_t plaintext_len, uint8_t *ciphertext, size_t *ciphertext_len); |
||||
sc_status_t sc_decrypt(sc_context_t *ctx, const uint8_t *ciphertext, size_t ciphertext_len, uint8_t *plaintext, size_t *plaintext_len); |
||||
|
||||
#endif // SECURE_CHANNEL_H |
||||
Loading…
Reference in new issue