|
|
|
@ -1,16 +1,16 @@ |
|
|
|
#include "pkt_normalizer.h" |
|
|
|
#include "pkt_normalizer.h" |
|
|
|
#include "../u_async/u_async.h" |
|
|
|
#include "../lib/u_async.h" |
|
|
|
#include <stdlib.h> |
|
|
|
#include <stdlib.h> |
|
|
|
#include <string.h> |
|
|
|
#include <string.h> |
|
|
|
#include <stdint.h> |
|
|
|
#include <stdint.h> |
|
|
|
#include <stdio.h> |
|
|
|
#include <stdio.h> |
|
|
|
static void packer_handler(ll_queue_t* q, ll_entry_t* unused, void* arg); |
|
|
|
static void packer_handler(struct ll_queue* q, struct ll_entry* unused, void* arg); |
|
|
|
static void unpacker_handler(ll_queue_t* q, ll_entry_t* unused, void* arg); |
|
|
|
static void unpacker_handler(struct ll_queue* q, struct ll_entry* unused, void* arg); |
|
|
|
static void send_buf(pn_struct* pn); |
|
|
|
static void send_buf(struct pn_struct* pn); |
|
|
|
static int get_header(uint8_t* header, size_t L); |
|
|
|
static int get_header(uint8_t* header, size_t L); |
|
|
|
/* Calculate fragment size from mtu */ |
|
|
|
/* Calculate fragment size from mtu */ |
|
|
|
pn_struct* pkt_normalizer_init(uasync_t* ua, int is_packer, int mtu) { |
|
|
|
struct pn_struct* pkt_normalizer_init(uasync_t* ua, int is_packer, int mtu) { |
|
|
|
pn_struct* pn = malloc(sizeof(pn_struct)); |
|
|
|
struct pn_struct* pn = malloc(sizeof(struct pn_struct)); |
|
|
|
if (!pn) return NULL; |
|
|
|
if (!pn) return NULL; |
|
|
|
pn->ua = ua; |
|
|
|
pn->ua = ua; |
|
|
|
pn->input = queue_new(ua); |
|
|
|
pn->input = queue_new(ua); |
|
|
|
@ -51,7 +51,7 @@ pn_struct* pkt_normalizer_init(uasync_t* ua, int is_packer, int mtu) { |
|
|
|
return pn; |
|
|
|
return pn; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void pkt_normalizer_deinit(pn_struct* pn) { |
|
|
|
void pkt_normalizer_deinit(struct pn_struct* pn) { |
|
|
|
if (!pn) return; |
|
|
|
if (!pn) return; |
|
|
|
queue_free(pn->input); |
|
|
|
queue_free(pn->input); |
|
|
|
queue_free(pn->output); |
|
|
|
queue_free(pn->output); |
|
|
|
@ -63,8 +63,8 @@ void pkt_normalizer_deinit(pn_struct* pn) { |
|
|
|
} |
|
|
|
} |
|
|
|
free(pn); |
|
|
|
free(pn); |
|
|
|
} |
|
|
|
} |
|
|
|
pkt_normalizer_pair* pkt_normalizer_pair_init(uasync_t* ua, int mtu) { |
|
|
|
struct pkt_normalizer_pair* pkt_normalizer_pair_init(uasync_t* ua, int mtu) { |
|
|
|
pkt_normalizer_pair* pair = malloc(sizeof(pkt_normalizer_pair)); |
|
|
|
struct pkt_normalizer_pair* pair = malloc(sizeof(struct pkt_normalizer_pair)); |
|
|
|
if (!pair) return NULL; |
|
|
|
if (!pair) return NULL; |
|
|
|
pair->packer = pkt_normalizer_init(ua, 1, mtu); |
|
|
|
pair->packer = pkt_normalizer_init(ua, 1, mtu); |
|
|
|
if (!pair->packer) { |
|
|
|
if (!pair->packer) { |
|
|
|
@ -79,7 +79,7 @@ pkt_normalizer_pair* pkt_normalizer_pair_init(uasync_t* ua, int mtu) { |
|
|
|
} |
|
|
|
} |
|
|
|
return pair; |
|
|
|
return pair; |
|
|
|
} |
|
|
|
} |
|
|
|
void pkt_normalizer_pair_deinit(pkt_normalizer_pair* pair) { |
|
|
|
void pkt_normalizer_pair_deinit(struct pkt_normalizer_pair* pair) { |
|
|
|
if (!pair) return; |
|
|
|
if (!pair) return; |
|
|
|
pkt_normalizer_deinit(pair->packer); |
|
|
|
pkt_normalizer_deinit(pair->packer); |
|
|
|
pkt_normalizer_deinit(pair->unpacker); |
|
|
|
pkt_normalizer_deinit(pair->unpacker); |
|
|
|
@ -99,7 +99,7 @@ static int get_header(uint8_t* header, size_t L) { |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
/* Сбросить состояние сборки фрагментов */ |
|
|
|
/* Сбросить состояние сборки фрагментов */ |
|
|
|
static void reset_fragment_state(pn_struct* pn) { |
|
|
|
static void reset_fragment_state(struct pn_struct* pn) { |
|
|
|
if (!pn->is_packer) { |
|
|
|
if (!pn->is_packer) { |
|
|
|
pn->u.unpacker.len = 0; |
|
|
|
pn->u.unpacker.len = 0; |
|
|
|
pn->u.unpacker.total_len = 0; |
|
|
|
pn->u.unpacker.total_len = 0; |
|
|
|
@ -107,10 +107,10 @@ static void reset_fragment_state(pn_struct* pn) { |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
/* Таймаут для сборки фрагментов */ |
|
|
|
/* Таймаут для сборки фрагментов */ |
|
|
|
static void send_buf(pn_struct* pn) { |
|
|
|
static void send_buf(struct pn_struct* pn) { |
|
|
|
if (pn->u.packer.len == 0) return; |
|
|
|
if (pn->u.packer.len == 0) return; |
|
|
|
size_t payload_len = pn->u.packer.len; |
|
|
|
size_t payload_len = pn->u.packer.len; |
|
|
|
ll_entry_t* out = queue_entry_new(2 + payload_len); |
|
|
|
struct ll_entry* out = queue_entry_new(2 + payload_len); |
|
|
|
if (!out) return; |
|
|
|
if (!out) return; |
|
|
|
uint8_t* d = ll_entry_data(out); |
|
|
|
uint8_t* d = ll_entry_data(out); |
|
|
|
*(uint16_t*)d = (uint16_t)payload_len; |
|
|
|
*(uint16_t*)d = (uint16_t)payload_len; |
|
|
|
@ -118,11 +118,11 @@ static void send_buf(pn_struct* pn) { |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
pn->u.packer.len = 0; |
|
|
|
pn->u.packer.len = 0; |
|
|
|
} |
|
|
|
} |
|
|
|
static void packer_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
static void packer_handler(struct ll_queue* q, struct ll_entry* unused, void* arg) { |
|
|
|
(void)unused; |
|
|
|
(void)unused; |
|
|
|
pn_struct* pn = arg; |
|
|
|
struct pn_struct* pn = arg; |
|
|
|
size_t max = (size_t)1400; |
|
|
|
size_t max = (size_t)1400; |
|
|
|
ll_entry_t* entry = queue_entry_get(q); |
|
|
|
struct ll_entry* entry = queue_entry_get(q); |
|
|
|
if (!entry) { |
|
|
|
if (!entry) { |
|
|
|
queue_resume_callback(q); |
|
|
|
queue_resume_callback(q); |
|
|
|
return; |
|
|
|
return; |
|
|
|
@ -143,7 +143,7 @@ static void packer_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
while (remaining > 0) { |
|
|
|
while (remaining > 0) { |
|
|
|
size_t chunk; |
|
|
|
size_t chunk; |
|
|
|
size_t payload_len; |
|
|
|
size_t payload_len; |
|
|
|
ll_entry_t* fout; |
|
|
|
struct ll_entry* fout; |
|
|
|
uint8_t* fd; |
|
|
|
uint8_t* fd; |
|
|
|
uint8_t frag_header[2]; |
|
|
|
uint8_t frag_header[2]; |
|
|
|
int frag_hsize; |
|
|
|
int frag_hsize; |
|
|
|
@ -270,12 +270,12 @@ static void packer_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
} |
|
|
|
} |
|
|
|
queue_resume_callback(q); |
|
|
|
queue_resume_callback(q); |
|
|
|
} |
|
|
|
} |
|
|
|
void pkt_normalizer_set_service_callback(pn_struct* pn, pkt_normalizer_service_callback_t callback, void* user_data) { |
|
|
|
void pkt_normalizer_set_service_callback(struct pn_struct* pn, pkt_normalizer_service_callback_t callback, void* user_data) { |
|
|
|
if (!pn) return; |
|
|
|
if (!pn) return; |
|
|
|
pn->service_callback = callback; |
|
|
|
pn->service_callback = callback; |
|
|
|
pn->service_callback_user_data = user_data; |
|
|
|
pn->service_callback_user_data = user_data; |
|
|
|
} |
|
|
|
} |
|
|
|
void pkt_normalizer_reset_service_state(pn_struct* pn) { |
|
|
|
void pkt_normalizer_reset_service_state(struct pn_struct* pn) { |
|
|
|
if (!pn || pn->is_packer) return; |
|
|
|
if (!pn || pn->is_packer) return; |
|
|
|
if (pn->u.unpacker.in_service) { |
|
|
|
if (pn->u.unpacker.in_service) { |
|
|
|
// Deliver pending service packet
|
|
|
|
// Deliver pending service packet
|
|
|
|
@ -292,7 +292,7 @@ void pkt_normalizer_reset_service_state(pn_struct* pn) { |
|
|
|
pn->u.unpacker.in_service = 0; |
|
|
|
pn->u.unpacker.in_service = 0; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
void pkt_normalizer_reset_state(pn_struct* pn) { |
|
|
|
void pkt_normalizer_reset_state(struct pn_struct* pn) { |
|
|
|
if (!pn) return; |
|
|
|
if (!pn) return; |
|
|
|
if (pn->is_packer) { |
|
|
|
if (pn->is_packer) { |
|
|
|
// Flush packer buffer
|
|
|
|
// Flush packer buffer
|
|
|
|
@ -306,7 +306,7 @@ void pkt_normalizer_reset_state(pn_struct* pn) { |
|
|
|
pkt_normalizer_reset_service_state(pn); |
|
|
|
pkt_normalizer_reset_service_state(pn); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
int pkt_normalizer_send_service(pn_struct* pn, uint8_t type, const void* data, size_t len) { |
|
|
|
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; |
|
|
|
if (!pn || !pn->is_packer) return -1; |
|
|
|
// Service packet ограничен 256 байтами всего
|
|
|
|
// Service packet ограничен 256 байтами всего
|
|
|
|
if (len > 256 - 2) return -1; // 2 байта на заголовок (0xFC + тип)
|
|
|
|
if (len > 256 - 2) return -1; // 2 байта на заголовок (0xFC + тип)
|
|
|
|
@ -338,7 +338,7 @@ int pkt_normalizer_send_service(pn_struct* pn, uint8_t type, const void* data, s |
|
|
|
} |
|
|
|
} |
|
|
|
if (chunk == 0) break; |
|
|
|
if (chunk == 0) break; |
|
|
|
size_t payload_len = 1 + chunk + (pos == 0 ? 1 : 0); // +1 байт типа для первого пакета
|
|
|
|
size_t payload_len = 1 + chunk + (pos == 0 ? 1 : 0); // +1 байт типа для первого пакета
|
|
|
|
ll_entry_t* entry = queue_entry_new(2 + payload_len); |
|
|
|
struct ll_entry* entry = queue_entry_new(2 + payload_len); |
|
|
|
if (!entry) return -1; |
|
|
|
if (!entry) return -1; |
|
|
|
uint8_t* d = ll_entry_data(entry); |
|
|
|
uint8_t* d = ll_entry_data(entry); |
|
|
|
*(uint16_t*)d = (uint16_t)payload_len; |
|
|
|
*(uint16_t*)d = (uint16_t)payload_len; |
|
|
|
@ -354,14 +354,14 @@ int pkt_normalizer_send_service(pn_struct* pn, uint8_t type, const void* data, s |
|
|
|
} |
|
|
|
} |
|
|
|
return 0; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
} |
|
|
|
int pkt_normalizer_get_error_count(const pn_struct* pn) { |
|
|
|
int pkt_normalizer_get_error_count(const struct pn_struct* pn) { |
|
|
|
if (!pn) return 0; |
|
|
|
if (!pn) return 0; |
|
|
|
if (pn->is_packer) { |
|
|
|
if (pn->is_packer) { |
|
|
|
return pn->u.packer.error_count; |
|
|
|
return pn->u.packer.error_count; |
|
|
|
} |
|
|
|
} |
|
|
|
return pn->u.unpacker.error_count; |
|
|
|
return pn->u.unpacker.error_count; |
|
|
|
} |
|
|
|
} |
|
|
|
void pkt_normalizer_reset_error_count(pn_struct* pn) { |
|
|
|
void pkt_normalizer_reset_error_count(struct pn_struct* pn) { |
|
|
|
if (!pn) return; |
|
|
|
if (!pn) return; |
|
|
|
if (pn->is_packer) { |
|
|
|
if (pn->is_packer) { |
|
|
|
pn->u.packer.error_count = 0; |
|
|
|
pn->u.packer.error_count = 0; |
|
|
|
@ -369,17 +369,17 @@ void pkt_normalizer_reset_error_count(pn_struct* pn) { |
|
|
|
pn->u.unpacker.error_count = 0; |
|
|
|
pn->u.unpacker.error_count = 0; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
void pkt_normalizer_flush(pn_struct* pn) { |
|
|
|
void pkt_normalizer_flush(struct pn_struct* pn) { |
|
|
|
if (!pn || !pn->is_packer) return; |
|
|
|
if (!pn || !pn->is_packer) return; |
|
|
|
if (pn->u.packer.len > 0) { |
|
|
|
if (pn->u.packer.len > 0) { |
|
|
|
send_buf(pn); |
|
|
|
send_buf(pn); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
static void unpacker_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
static void unpacker_handler(struct ll_queue* q, struct ll_entry* unused, void* arg) { |
|
|
|
(void)unused; |
|
|
|
(void)unused; |
|
|
|
pn_struct* pn = arg; |
|
|
|
struct pn_struct* pn = arg; |
|
|
|
while (queue_entry_count(q) > 0) { |
|
|
|
while (queue_entry_count(q) > 0) { |
|
|
|
ll_entry_t* entry = queue_entry_get(q); |
|
|
|
struct ll_entry* entry = queue_entry_get(q); |
|
|
|
uint8_t* data = ll_entry_data(entry); |
|
|
|
uint8_t* data = ll_entry_data(entry); |
|
|
|
size_t total = ll_entry_size(entry); |
|
|
|
size_t total = ll_entry_size(entry); |
|
|
|
uint16_t payload_len = *(uint16_t*)data; |
|
|
|
uint16_t payload_len = *(uint16_t*)data; |
|
|
|
@ -424,7 +424,7 @@ static void unpacker_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
/* Проверить, не собрали ли уже весь пакет */ |
|
|
|
/* Проверить, не собрали ли уже весь пакет */ |
|
|
|
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
|
|
|
ll_entry_t* out = queue_entry_new(pn->u.unpacker.total_len); |
|
|
|
struct ll_entry* out = queue_entry_new(pn->u.unpacker.total_len); |
|
|
|
if (out) { |
|
|
|
if (out) { |
|
|
|
memcpy(ll_entry_data(out), pn->u.unpacker.buf, pn->u.unpacker.total_len); |
|
|
|
memcpy(ll_entry_data(out), pn->u.unpacker.buf, pn->u.unpacker.total_len); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
@ -458,7 +458,7 @@ static void unpacker_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
/* Проверить, не собрали ли уже весь пакет */ |
|
|
|
/* Проверить, не собрали ли уже весь пакет */ |
|
|
|
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
|
|
|
ll_entry_t* out = queue_entry_new(pn->u.unpacker.total_len); |
|
|
|
struct ll_entry* out = queue_entry_new(pn->u.unpacker.total_len); |
|
|
|
if (out) { |
|
|
|
if (out) { |
|
|
|
memcpy(ll_entry_data(out), pn->u.unpacker.buf, pn->u.unpacker.total_len); |
|
|
|
memcpy(ll_entry_data(out), pn->u.unpacker.buf, pn->u.unpacker.total_len); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
@ -576,7 +576,7 @@ static void unpacker_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
/* Проверить, собрали ли весь пакет */ |
|
|
|
/* Проверить, собрали ли весь пакет */ |
|
|
|
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len >= pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
|
|
|
if (pn->u.unpacker.len == pn->u.unpacker.total_len) { |
|
|
|
ll_entry_t* out = queue_entry_new(pn->u.unpacker.total_len); |
|
|
|
struct ll_entry* out = queue_entry_new(pn->u.unpacker.total_len); |
|
|
|
if (out) { |
|
|
|
if (out) { |
|
|
|
memcpy(ll_entry_data(out), pn->u.unpacker.buf, pn->u.unpacker.total_len); |
|
|
|
memcpy(ll_entry_data(out), pn->u.unpacker.buf, pn->u.unpacker.total_len); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
@ -589,7 +589,7 @@ static void unpacker_handler(ll_queue_t* q, ll_entry_t* unused, void* arg) { |
|
|
|
} |
|
|
|
} |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
/* Обычная запись (не часть фрагмента) */ |
|
|
|
/* Обычная запись (не часть фрагмента) */ |
|
|
|
ll_entry_t* out = queue_entry_new(L); |
|
|
|
struct ll_entry* out = queue_entry_new(L); |
|
|
|
if (out) { |
|
|
|
if (out) { |
|
|
|
memcpy(ll_entry_data(out), cg + cg_pos, L); |
|
|
|
memcpy(ll_entry_data(out), cg + cg_pos, L); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
queue_entry_put(pn->output, out); |
|
|
|
|
Evgeny
3 months ago