jeka/utun2
1
0 Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Refactor: Add dgram fields to ll_queue, remove ref_count, integrate pkt_normalizer

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 files
This commit is contained in:
Evgeny committed 2026-01-31 23:02:35 +03:00
1 parent fbc8e3d9e4
commit dd4644776d
11 files changed
+1488 -2329
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

src/etcp.c
+751 -745
View File
File diff suppressed because it is too large. Load diff
src/etcp.h
+8 -4
View File
@@ -11,6 +11,11 @@
extern "C" {
#endif
#include "pkt_normalizer.h"
// In struct ETCP_CONN, add:
//struct pn_pair* normalizer;
//!!!!!!!!!!! надо переделать ll_queue чтобы возвращала не дата а свою структуру
// Forward declarations
@@ -34,7 +39,7 @@ uint64_t get_current_time_units(void);
// в этот список пакет добавляется когда перемещается из input_queue в input_send_q, при этом к пакету добавляется struct INFLIGHT_PACKET из inflight_pool.
// пакет полностью удаляется когда приходит ACK (либо conn_reset/close)
struct INFLIGHT_PACKET {
struct INFLIGHT_PACKET {// выделяется из etcp->inflight_pool
struct ll_entry ll;
struct ETCP_LINK* last_link; // Last sent link
uint64_t last_timestamp; // Last send timestamp
@@ -42,15 +47,13 @@ struct INFLIGHT_PACKET {
uint8_t send_count; // Number of sends
uint8_t retrans_req_count; // Number of retrans requests
uint8_t state; // WAIT_ACK or WAIT_SEND
uint8_t* pkt_data;
};
// Список пакетов для сборки. собирается в ll_queue (используем быстрый поиск с хешем)
struct ETCP_FRAGMENT {
struct ETCP_FRAGMENT {// выделяется из пула etcp->rx_pool
struct ll_entry ll;
uint32_t seq;
uint16_t timestamp;
uint8_t* pkt_data;
};
struct ACK_PACKET {
@@ -72,6 +75,7 @@ struct ETCP_CONN {
// Crypto and state
struct secure_channel crypto_ctx;
struct pn_pair* normalizer;
// Peer info
uint64_t peer_node_id; // Peer node ID
src/pkt_normalizer.c
+324 -575
View File
@@ -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);
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->output = queue_new(ua, 0); // No memory pool for now, no hash table
if (!pn->output) {
queue_free(pn->input);
free(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);
}
if (pn->output) {
void* data;
while ((data = queue_data_get(pn->output)) != NULL) {
queue_data_free(data);
}
queue_free(pn->output);
}
if (pn->pending) {
void* data;
while ((data = queue_data_get(pn->pending)) != NULL) {
queue_data_free(data);
}
queue_free(pn->pending);
}
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);
}
if (pn->flush_timer) {
uasync_cancel_timeout(pn->ua, pn->flush_timer);
}
free(pn->in_buf);
free(pn->out_buf);
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;
}
pair->unpacker = pkt_normalizer_init(ua, 0, mtu);
if (!pair->unpacker) {
pkt_normalizer_deinit(pair->packer);
free(pair);
return NULL;
}
return pair;
// 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;
}
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;
// 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 {
uint8_t high = (uint8_t)(L >> 8);
if (high > 5) return -1;
header[0] = 0xF0 + high;
header[1] = (uint8_t)(L & 0xFF);
return 2;
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);
}
}
}
/* Сбросить состояние сборки фрагментов */
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;
// 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);
}
}
}
/* Таймаут для сборки фрагментов */
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;
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);
}
}
}
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);
// 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;
}
// Для определения размера нужно знать структуру, но в новом 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);
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 {
queue_data_put(pn->send_pending, frag, 0);
if (eq->waiter.callback == NULL) {
queue_wait_threshold(eq, 0, 0, etcp_input_ready_cb, 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;
}
}
}
// 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;
}
memcpy(fd, pkt_data + pos, chunk);
queue_data_put(pn->output, fout, 0);
pos += chunk;
// 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;
fragment_count++;
}
} else {
if (pn->u.packer.len + needed > max) {
send_buf(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);
}
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);
}
} else {
// Reset unpacker fragment state
reset_fragment_state(pn);
// Reset service state
pkt_normalizer_reset_service_state(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;
// 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;
// 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;
}
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;
queue_data_free(data); // Free the entry
}
// 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);
}
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); // корректно вычитаем заголовки
}
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;
}
return pn->u.unpacker.error_count;
}
void pkt_normalizer_reset_error_count(struct pn_struct* pn) {
if (!pn) return;
if (pn->is_packer) {
pn->u.packer.error_count = 0;
} else {
pn->u.unpacker.error_count = 0;
}
}
void pkt_normalizer_flush(struct pn_struct* pn) {
if (!pn || !pn->is_packer) return;
if (pn->u.packer.len > 0) {
send_buf(pn);
}
}
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;
}
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;
}
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;
}
}
err:
queue_data_free(entry);
pn->flush_timer = uasync_set_timeout(pn->ua, 10, pn, flush_cb); // 1ms = 10 * 0.1ms
}
queue_resume_callback(q);
}
// Internal: Flush callback on timeout
static void flush_cb(void* arg) {
struct PKTNORM* pn = (struct PKTNORM*)arg;
if (!pn) return;
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;
}
}
// 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);
}
// 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;
}
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;
}
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;
}
}
}
}
src/pkt_normalizer.h
+43 -59
View File
@@ -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 (через нее принимаем пакеты)
/* 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);
// 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
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
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);
// Деинициализация пары
void pn_pair_deinit(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);
// Сброс состояния (для unpacker)
void pn_unpacker_reset_state(struct PKTNORM* pn);
/* Error handling */
int pkt_normalizer_get_error_count(const struct pn_struct* pn);
void pkt_normalizer_reset_error_count(struct pn_struct* pn);
// создаёт malloc data, копирует, помещает в input.
void pn_packer_send(struct PKTNORM* pn, uint8_t* data, uint16_t len);
/* Flush internal buffer (packer only) */
void pkt_normalizer_flush(struct pn_struct* pn);
/* Как работает:
Формат отправки в etcp: 2 байта размер, далее данные (порезанные на куски и отправленные через etcp)
собирает по возможности полные пакеты с размером pkt_size.
+ timeout: неполный пакет отправляется по таймауту 1ms (если очереди пустые)
+ входящая очередь etcp не должне наполняться для минимизации задержки - новый пакет отправляем только когда очередь пустая
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
src/secure_channel.c.bak
-369
View File
@@ -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;
}
src/secure_channel.c1
-388
View File
@@ -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;
}
src/secure_channel.h1
-68
View File
@@ -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