@ -20,7 +20,10 @@ struct PKTNORM* pn_init(struct ETCP_CONN* etcp) {
if ( ! etcp ) return NULL ;
struct PKTNORM * pn = calloc ( 1 , sizeof ( struct PKTNORM ) ) ;
if ( ! pn ) return NULL ;
if ( ! pn ) {
DEBUG_ERROR ( DEBUG_CATEGORY_NORMALIZER , " pn_init: calloc failed " ) ;
return NULL ;
}
pn - > etcp = etcp ;
pn - > ua = etcp - > instance - > ua ;
@ -28,9 +31,15 @@ struct PKTNORM* pn_init(struct ETCP_CONN* etcp) {
pn - > tx_wait_time = 10 ;
pn - > input = queue_new ( pn - > ua , 0 ) ; // No hash needed
pn - > output = queue_new ( pn - > ua , 0 ) ; // No hash needed
if ( ! pn - > input ) {
DEBUG_ERROR ( DEBUG_CATEGORY_NORMALIZER , " pn_init: queue_new(input) failed " ) ;
pn_pair_deinit ( pn ) ;
return NULL ;
}
if ( ! pn - > input | | ! pn - > output ) {
pn - > output = queue_new ( pn - > ua , 0 ) ; // No hash needed
if ( ! pn - > output ) {
DEBUG_ERROR ( DEBUG_CATEGORY_NORMALIZER , " pn_init: queue_new(output) failed " ) ;
pn_pair_deinit ( pn ) ;
return NULL ;
}
@ -100,10 +109,11 @@ void pn_unpacker_reset_state(struct PKTNORM* pn) {
void pn_packer_send ( struct PKTNORM * pn , uint8_t * data , uint16_t len ) {
if ( ! pn | | ! data | | len = = 0 ) return ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_packer_send: pn=%p, len=%d " , pn , len ) ;
struct ll_entry * entry = ll_alloc_lldgram ( len ) ;
if ( ! entry ) return ;
if ( ! entry ) {
DEBUG_ERROR ( DEBUG_CATEGORY_NORMALIZER , " pn_packer_send: ll_alloc_lldgram failed " ) ;
return ;
}
memcpy ( entry - > dgram , data , len ) ;
entry - > len = len ;
entry - > dgram_pool = NULL ;
@ -115,14 +125,14 @@ void pn_packer_send(struct PKTNORM* pn, uint8_t* data, uint16_t len) {
}
int ret = queue_data_put ( pn - > input , entry , 0 ) ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_packer_send: queue_data_put returned %d, input count=%d " , ret , queue_entry_count ( pn - > input ) ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, " pn_packer_send: queue_data_put returned %d, input count=%d", ret, queue_entry_count(pn ->input));
}
// Internal: Packer callback
static void packer_cb ( struct ll_queue * q , void * arg ) {
struct PKTNORM * pn = ( struct PKTNORM * ) arg ;
if ( ! pn ) return ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_packer: packer_cb " ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: packer_cb");
queue_wait_threshold ( pn - > etcp - > input_queue , 0 , 0 , etcp_input_ready_cb , pn ) ;
}
@ -131,7 +141,7 @@ static void packer_cb(struct ll_queue* q, void* arg) {
static void pn_send_to_etcp ( struct PKTNORM * pn ) {
if ( ! pn | | ! pn - > data | | pn - > data_ptr = = 0 ) return ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_packer: pn_send_to_etcp " ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: pn_send_to_etcp");
// Allocate ETCP_FRAGMENT from io_pool
struct ETCP_FRAGMENT * frag = ( struct ETCP_FRAGMENT * ) queue_entry_new_from_pool ( pn - > etcp - > io_pool ) ;
if ( ! frag ) { // drop data
@ -160,11 +170,15 @@ static void pn_buf_renew(struct PKTNORM* pn) {
}
if ( ! pn - > data ) {
pn - > data = memory_pool_alloc ( pn - > etcp - > instance - > data_pool ) ;
if ( ! pn - > data ) {
DEBUG_ERROR ( DEBUG_CATEGORY_NORMALIZER , " pn_buf_renew: memory_pool_alloc failed " ) ;
return ;
}
int size = pn - > etcp - > instance - > data_pool - > object_size ;
if ( size > pn - > frag_size ) size = pn - > frag_size ;
pn - > data_size = size ;
pn - > data_ptr = 0 ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_packer: new bufer size=%d bytes " , size ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: new bufer size=%d bytes",size);
}
}
@ -179,7 +193,7 @@ static void etcp_input_ready_cb(struct ll_queue* q, void* arg) {
if ( ! in_dgram ) { queue_resume_callback ( pn - > input ) ; return ; }
pn_buf_renew ( pn ) ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_packer: new pkt hdrpos=%d " , pn - > data_ptr ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: new pkt hdrpos=%d",pn->data_ptr);
if ( ! pn - > data ) goto exit ; // Allocation failed
pn - > data [ pn - > data_ptr + + ] = in_dgram - > len & 0xFF ;
pn - > data [ pn - > data_ptr + + ] = ( in_dgram - > len > > 8 ) & 0xFF ;
@ -189,7 +203,7 @@ static void etcp_input_ready_cb(struct ll_queue* q, void* arg) {
int remain = pn - > data_size - pn - > data_ptr ;
int avail = in_dgram - > len - in_ptr ;
if ( avail < remain ) remain = avail ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_packer: copy %d bytes (in_ptr=%d, out_ptr=%d) " , remain , in_ptr , pn - > data_ptr ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: copy %d bytes (in_ptr=%d, out_ptr=%d)",remain, in_ptr, pn->data_ptr);
memcpy ( pn - > data + pn - > data_ptr , in_dgram - > dgram + in_ptr , remain ) ;
pn - > data_ptr + = remain ;
in_ptr + = remain ;
@ -237,7 +251,7 @@ static void pn_unpacker_cb(struct ll_queue* q, void* arg) {
uint8_t * payload = frag - > ll . dgram ;
uint16_t len = frag - > ll . len ;
uint16_t ptr = 0 ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_unpacker: unpacking fragment len=%d " , len ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_unpacker: unpacking fragment len=%d", len);
while ( ptr < len ) {
if ( ! pn - > recvpart ) {
@ -248,11 +262,12 @@ static void pn_unpacker_cb(struct ll_queue* q, void* arg) {
break ;
}
uint16_t part_size = payload [ ptr ] | ( payload [ ptr + 1 ] < < 8 ) ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_unpacker: new fragment pkt_len=%d (at %d) " , part_size , ptr ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_unpacker: new fragment pkt_len=%d (at %d)", part_size, ptr);
ptr + = 2 ;
pn - > recvpart = ll_alloc_lldgram ( part_size ) ;
if ( ! pn - > recvpart ) {
DEBUG_ERROR ( DEBUG_CATEGORY_NORMALIZER , " pn_unpacker_cb: ll_alloc_lldgram failed " ) ;
break ;
}
pn - > recvpart - > len = 0 ;
@ -261,7 +276,7 @@ static void pn_unpacker_cb(struct ll_queue* q, void* arg) {
uint16_t rem = pn - > recvpart - > memlen - pn - > recvpart - > len ; // осталось собрать байт
uint16_t avail = len - ptr ; // доступно байт сейчас
uint16_t cp = ( rem < avail ) ? rem : avail ;
DEBUG_INFO ( DEBUG_CATEGORY_NORMALIZER , " pn_unpacker: copy: remain=%d avail=%d in_ptr=%d out_ptr=%d " , rem , avail , ptr , pn - > recvpart - > len ) ;
// DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, " pn_unpacker: copy: remain=%d avail=%d in_ptr=%d out_ptr=%d", rem, avail, ptr, pn->recvpart->len);
memcpy ( pn - > recvpart - > dgram + pn - > recvpart - > len , payload + ptr , cp ) ;
pn - > recvpart - > len + = cp ;
ptr + = cp ;