From fc089d4863dbd7021056e3228ae934d21c261898 Mon Sep 17 00:00:00 2001 From: divanchykhin Date: Tue, 31 Mar 2015 15:10:26 +0300 Subject: [PATCH] Emulator actual embodiment: memory handle-based mem management and parsing in protocol handlers and around --- tests/emulator/sa-commlayer.cpp | 38 ++ tests/emulator/sa-commlayer.h | 6 + tests/emulator/sa_server.cpp | 419 ++++++++++++++- tests/emulator/sagdp_protocol.cpp | 808 +++++++++++++++++++++++++++- tests/emulator/sagdp_protocol.h | 6 +- tests/emulator/sasp_protocol.cpp | 176 +++++- tests/emulator/sasp_protocol.h | 7 +- tests/emulator/test-generator.cpp | 113 +++- tests/emulator/test-generator.h | 10 + tests/emulator/yoctovm_protocol.cpp | 283 ++++++++++ tests/emulator/yoctovm_protocol.h | 8 + tests/emulator/zepto-mem-mngmt.cpp | 48 +- tests/emulator/zepto-mem-mngmt.h | 16 + 13 files changed, 1908 insertions(+), 30 deletions(-) diff --git a/tests/emulator/sa-commlayer.cpp b/tests/emulator/sa-commlayer.cpp index bfa0049..3cff7c9 100644 --- a/tests/emulator/sa-commlayer.cpp +++ b/tests/emulator/sa-commlayer.cpp @@ -124,6 +124,20 @@ void communicationTerminate() hPipe = INVALID_HANDLE_VALUE; } +uint8_t sendMessage( MEMORY_HANDLE mem_h ) +{ + uint8_t buff[512]; + uint16_t msgSize; + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + msgSize = zepto_parsing_remaining_bytes( &po ); + assert( msgSize <= 512 ); + zepto_parse_read_block( &po, buff, msgSize ); + printf( "message sent; size = %d\n", msgSize ); + return sendMessage(&msgSize, buff); +} + uint8_t sendMessage(uint16_t* msgSize, const uint8_t * buff) { BOOL fSuccess = FALSE; @@ -161,6 +175,15 @@ uint8_t sendMessage(uint16_t* msgSize, const uint8_t * buff) return COMMLAYER_RET_OK; } +uint8_t getMessage( MEMORY_HANDLE mem_h ) // returns when a packet received +{ + uint8_t buff[512]; + uint16_t msgSize; + uint8_t ret = getMessage( &msgSize, buff, 512 ); + zepto_write_block( mem_h, buff, msgSize ); + return ret; +} + uint8_t getMessage(uint16_t* msgSize, uint8_t * buff, int maxSize) // returns when a packet received { BOOL fSuccess = FALSE; @@ -312,6 +335,21 @@ uint8_t finalizeReading(uint16_t* msgSize, uint8_t * buff, int maxSize) } + +uint8_t tryGetMessage( MEMORY_HANDLE mem_h ) // returns immediately, but a packet reception is not guaranteed +{ + uint8_t buff[512]; + uint16_t msgSize; + uint8_t ret = tryGetMessage( &msgSize, buff, 512 ); + if ( ret == COMMLAYER_RET_OK ) + { + printf( "message received; size = %d\n", msgSize ); + zepto_write_block( mem_h, buff, msgSize ); + } + return ret; +} + + uint8_t tryGetMessage(uint16_t* msgSize, uint8_t * buff, int maxSize) // returns immediately, but a packet reception is not guaranteed { uint8_t ret_code; diff --git a/tests/emulator/sa-commlayer.h b/tests/emulator/sa-commlayer.h index f70cbc1..718e27e 100644 --- a/tests/emulator/sa-commlayer.h +++ b/tests/emulator/sa-commlayer.h @@ -18,6 +18,8 @@ Copyright (C) 2015 OLogN Technologies AG #if !defined __SA_COMMLAYER_H__ #define __SA_COMMLAYER_H__ +#include "zepto-mem-mngmt.h" + // RET codes #define COMMLAYER_RET_FAILED 0 // not authenticated, etc @@ -32,4 +34,8 @@ uint8_t sendMessage( uint16_t* msgSize, const uint8_t * buff ); uint8_t getMessage( uint16_t* msgSize, uint8_t * buff, int maxSize ); // returns when a packet received uint8_t tryGetMessage(uint16_t* msgSize, uint8_t * buff, int maxSize); // returns immediately, but a packet reception is not guaranteed +uint8_t sendMessage( MEMORY_HANDLE mem_h ); +uint8_t getMessage( MEMORY_HANDLE mem_h ); // returns when a packet received +uint8_t tryGetMessage( MEMORY_HANDLE mem_h ); // returns immediately, but a packet reception is not guaranteed + #endif // __SA_COMMLAYER_H__ \ No newline at end of file diff --git a/tests/emulator/sa_server.cpp b/tests/emulator/sa_server.cpp index 5e3f433..249ac9e 100644 --- a/tests/emulator/sa_server.cpp +++ b/tests/emulator/sa_server.cpp @@ -27,7 +27,6 @@ Copyright (C) 2015 OLogN Technologies AG #define BUF_SIZE 512 -uint8_t rwBuff[BUF_SIZE]; uint8_t data_buff[BUF_SIZE]; uint8_t msgLastSent[BUF_SIZE]; uint8_t pid[ SASP_NONCE_SIZE ]; @@ -35,8 +34,8 @@ uint8_t nonce[ SASP_NONCE_SIZE ]; - -int main(int argc, char *argv[]) +#if 0 +int main_loop() { #ifdef ENABLE_COUNTER_SYSTEM INIT_COUNTER_SYSTEM @@ -52,6 +51,7 @@ int main(int argc, char *argv[]) // in this preliminary implementation all memory segments are kept separately // All memory objects are listed below // TODO: revise memory management + uint8_t rwBuff[BUF_SIZE]; uint16_t* sizeInOut = (uint16_t*)(rwBuff + 3 * BUF_SIZE / 4); uint8_t* stack = (uint8_t*)sizeInOut + 2; // first two bytes are used for sizeInOut int stackSize = BUF_SIZE / 4 - 2; @@ -95,7 +95,7 @@ int main(int argc, char *argv[]) } if ( ret_code == SAGDP_RET_NEED_NONCE ) { - ret_code = handlerSASP_get_nonce( sizeInOut, nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); assert( ret_code == SASP_RET_NONCE ); ret_code = handlerSAGDP_receiveRequestResendLSP( &timer_val, nonce, sizeInOut, rwBuff, rwBuff + BUF_SIZE / 4, BUF_SIZE / 4, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); assert( ret_code != SAGDP_RET_NEED_NONCE && ret_code != SAGDP_RET_TO_LOWER_NONE ); @@ -167,7 +167,7 @@ int main(int argc, char *argv[]) } if ( ret_code == SAGDP_RET_NEED_NONCE ) { - ret_code = handlerSASP_get_nonce( sizeInOut, nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); assert( ret_code == SASP_RET_NONCE ); ret_code = handlerSAGDP_receiveRequestResendLSP( &timer_val, nonce, sizeInOut, rwBuff, rwBuff + BUF_SIZE / 4, BUF_SIZE / 4, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); assert( ret_code != SAGDP_RET_NEED_NONCE && ret_code != SAGDP_RET_TO_LOWER_NONE ); @@ -196,7 +196,7 @@ int main(int argc, char *argv[]) } else if ( ret_code == SAGDP_RET_NEED_NONCE ) { - ret_code = handlerSASP_get_nonce( sizeInOut, nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); assert( ret_code == SASP_RET_NONCE ); ret_code = handlerSAGDP_receiveUP( &timer_val, nonce, pid, sizeInOut, rwBuff, rwBuff + BUF_SIZE / 4, BUF_SIZE / 4, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); assert( ret_code != SAGDP_RET_NEED_NONCE ); @@ -330,7 +330,7 @@ int main(int argc, char *argv[]) ret_code = handlerSAGDP_receiveHLP( &timer_val, NULL, sizeInOut, rwBuff, rwBuff + BUF_SIZE / 4, BUF_SIZE / 4, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); if ( ret_code == SAGDP_RET_NEED_NONCE ) { - ret_code = handlerSASP_get_nonce( sizeInOut, nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); assert( ret_code == SASP_RET_NONCE ); ret_code = handlerSAGDP_receiveHLP( &timer_val, nonce, sizeInOut, rwBuff, rwBuff + BUF_SIZE / 4, BUF_SIZE / 4, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); assert( ret_code != SAGDP_RET_NEED_NONCE ); @@ -412,3 +412,408 @@ memcpy( rwBuff, rwBuff + BUF_SIZE / 4, *sizeInOut ); return 0; } + +#endif // 0 + + + + + + + + + +int main_loop2( REQUEST_REPLY_HANDLE mem_h ) +{ +#ifdef ENABLE_COUNTER_SYSTEM + INIT_COUNTER_SYSTEM +#endif // ENABLE_COUNTER_SYSTEM + + + printf("STARTING SERVER...\n"); + printf("==================\n\n"); + + initTestSystem(); + + + // in this preliminary implementation all memory segments are kept separately + // All memory objects are listed below + // TODO: revise memory management + uint8_t miscBuff[BUF_SIZE]; + uint8_t* stack = miscBuff; // first two bytes are used for sizeInOut + int stackSize = BUF_SIZE / 4 - 2; + uint8_t timer_val; + uint16_t wake_time; + // TODO: revise time/timer management + + uint8_t ret_code; + + // test setup values + bool wait_for_incoming_chain_with_timer; + uint16_t wake_time_to_start_new_chain; + + // do necessary initialization + sagdp_init( data_buff + DADA_OFFSET_SAGDP ); + + + printf("\nAwaiting client connection... \n" ); + if (!communicationInitializeAsServer()) + return -1; + + printf("Client connected.\n"); + + // MAIN LOOP + for (;;) + { +getmsg: + // 1. Get message from comm peer + ret_code = tryGetMessage( mem_h ); + zepto_response_to_request( mem_h ); + INCREMENT_COUNTER_IF( 91, "MAIN LOOP, packet received [1]", ret_code == COMMLAYER_RET_OK ); + while ( ret_code == COMMLAYER_RET_PENDING ) + { + waitForTimeQuantum(); + if ( timer_val && getTime() >= wake_time ) + { + printf( "no reply received; the last message (if any) will be resent by timer\n" ); + ret_code = handlerSAGDP_receiveRequestResendLSP( &timer_val, NULL, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + if ( ret_code == SAGDP_RET_TO_LOWER_NONE ) + { + zepto_response_to_request( mem_h ); + continue; + } + if ( ret_code == SAGDP_RET_NEED_NONCE ) + { + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + assert( ret_code == SASP_RET_NONCE ); + ret_code = handlerSAGDP_receiveRequestResendLSP( &timer_val, nonce, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + assert( ret_code != SAGDP_RET_NEED_NONCE && ret_code != SAGDP_RET_TO_LOWER_NONE ); + } + zepto_response_to_request( mem_h ); + goto saspsend; + break; + } + else if ( wait_for_incoming_chain_with_timer && getTime() >= wake_time_to_start_new_chain ) + { + wait_for_incoming_chain_with_timer = false; + ret_code = master_start( mem_h ); + zepto_response_to_request( mem_h ); + goto alt_entry; + break; + } + ret_code = tryGetMessage( mem_h ); + zepto_response_to_request( mem_h ); + INCREMENT_COUNTER_IF( 92, "MAIN LOOP, packet received [2]", ret_code == COMMLAYER_RET_OK ); + } + if ( ret_code != COMMLAYER_RET_OK ) + { + printf("\n\nWAITING FOR ESTABLISHING COMMUNICATION WITH SERVER...\n\n"); + if (!communicationInitializeAsClient()) // regardles of errors... quick and dirty solution so far + return -1; + goto getmsg; + } + printf("Message from client received\n"); + printf( "ret: %d; rq_size: %d, rsp_size: %d\n", ret_code, ugly_hook_get_request_size( mem_h ), ugly_hook_get_response_size( mem_h ) ); + +rectosasp: + // 2. Pass to SASP + ret_code = handlerSASP_receive( pid, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + zepto_response_to_request( mem_h ); + printf( "SASP1: ret: %d; rq_size: %d, rsp_size: %d\n", ret_code, ugly_hook_get_request_size( mem_h ), ugly_hook_get_response_size( mem_h ) ); + + switch ( ret_code ) + { + case SASP_RET_IGNORE: + { + printf( "BAD MESSAGE_RECEIVED\n" ); + goto getmsg; + break; + } + case SASP_RET_TO_LOWER_ERROR: + { + goto sendmsg; + break; + } + case SASP_RET_TO_HIGHER_NEW: + { + // regular processing will be done below in the next block + break; + } +/* case SASP_RET_TO_HIGHER_REPEATED: + { + printf( "NONCE_LAST_SENT has been reset; the last message (if any) will be resent\n" ); + ret_code = handlerSAGDP_receiveRepeatedUP( &timer_val, sizeInOut, rwBuff, rwBuff + BUF_SIZE / 4, BUF_SIZE / 4, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + zepto_response_to_request( mem_h ); + goto saspsend; + break; + }*/ + case SASP_RET_TO_HIGHER_LAST_SEND_FAILED: + { + printf( "NONCE_LAST_SENT has been reset; the last message (if any) will be resent\n" ); + ret_code = handlerSAGDP_receiveRequestResendLSP( &timer_val, NULL, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + if ( ret_code == SAGDP_RET_TO_LOWER_NONE ) + { + zepto_response_to_request( mem_h ); + continue; + } + if ( ret_code == SAGDP_RET_NEED_NONCE ) + { + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + assert( ret_code == SASP_RET_NONCE ); + ret_code = handlerSAGDP_receiveRequestResendLSP( &timer_val, nonce, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + assert( ret_code != SAGDP_RET_NEED_NONCE && ret_code != SAGDP_RET_TO_LOWER_NONE ); + } + zepto_response_to_request( mem_h ); + goto saspsend; + break; + } + default: + { + // unexpected ret_code + printf( "Unexpected ret_code %d\n", ret_code ); + assert( 0 ); + break; + } + } + + // 3. pass to SAGDP a new packet + ret_code = handlerSAGDP_receiveUP( &timer_val, NULL, pid, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + if ( ret_code == SAGDP_RET_START_OVER_FIRST_RECEIVED ) + { + sagdp_init( data_buff + DADA_OFFSET_SAGDP ); + // TODO: do remaining reinitialization + ret_code = handlerSAGDP_receiveUP( &timer_val, NULL, pid, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + assert( ret_code != SAGDP_RET_START_OVER_FIRST_RECEIVED ); + } + else if ( ret_code == SAGDP_RET_NEED_NONCE ) + { + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + assert( ret_code == SASP_RET_NONCE ); + ret_code = handlerSAGDP_receiveUP( &timer_val, nonce, pid, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + assert( ret_code != SAGDP_RET_NEED_NONCE ); + } + zepto_response_to_request( mem_h ); + printf( "SAGDP1: ret: %d; rq_size: %d, rsp_size: %d\n", ret_code, ugly_hook_get_request_size( mem_h ), ugly_hook_get_response_size( mem_h ) ); + + switch ( ret_code ) + { +#ifdef USED_AS_MASTER + case SAGDP_RET_OK: + { + printf( "master received unexpected packet. ignored\n" ); + goto getmsg; + break; + } +#else + case SAGDP_RET_SYS_CORRUPTED: + { + // TODO: reinitialize all + sagdp_init( data_buff + DADA_OFFSET_SAGDP ); +// zepto_response_to_request( mem_h ); + goto saspsend; + break; + } +#endif + case SAGDP_RET_TO_HIGHER: + { + // regular processing will be done below in the next block + break; + } + case SAGDP_RET_TO_LOWER_REPEATED: + { + goto saspsend; + } + case SAGDP_RET_OK: + { + goto getmsg; + } + default: + { + // unexpected ret_code + printf( "Unexpected ret_code %d\n", ret_code ); + assert( 0 ); + break; + } + } + +processcmd: + // 4. Process received command (yoctovm) + ret_code = slave_process( mem_h/*, BUF_SIZE / 4, stack, stackSize*/ ); +/* if ( ret_code == YOCTOVM_RESET_STACK ) + { + sagdp_init( data_buff + DADA_OFFSET_SAGDP ); + printf( "slave_process(): ret_code = YOCTOVM_RESET_STACK\n" ); + // TODO: reinit the rest of stack (where applicable) + ret_code = master_start( sizeInOut, rwBuff, rwBuff + BUF_SIZE / 4 ); + }*/ + zepto_response_to_request( mem_h ); + printf( "YOCTO: ret: %d; rq_size: %d, rsp_size: %d\n", ret_code, ugly_hook_get_request_size( mem_h ), ugly_hook_get_response_size( mem_h ) ); + + switch ( ret_code ) + { + case YOCTOVM_PASS_LOWER: + { + // test generation: sometimes slave can start a new chain at not in-chain reason (although in this case it won't be accepted by Master) +// bool restart_chain = get_rand_val() % 8 == 0; + bool restart_chain = false; + if ( restart_chain ) + { + sagdp_init( data_buff + DADA_OFFSET_SAGDP ); + ret_code = master_start( mem_h/*, BUF_SIZE / 4, stack, stackSize*/ ); + zepto_response_to_request( mem_h ); + assert( ret_code == YOCTOVM_PASS_LOWER ); + } + // regular processing will be done below in the next block + break; + } + case YOCTOVM_PASS_LOWER_THEN_IDLE: + { +// bool start_now = get_rand_val() % 3; + bool start_now = true; + wake_time_to_start_new_chain = start_now ? getTime() : getTime() + get_rand_val() % 8; + wait_for_incoming_chain_with_timer = true; + break; + } + case YOCTOVM_FAILED: + sagdp_init( data_buff + DADA_OFFSET_SAGDP ); + // NOTE: no 'break' is here as the rest is the same as for YOCTOVM_OK + case YOCTOVM_OK: + { + // here, in general, two main options are present: + // (1) to start a new chain immediately, or + // (2) to wait, during certain period of time, for an incoming chain, and then, if no packet is received, to start a new chain +// bool start_now = get_rand_val() % 3; + bool start_now = true; + if ( start_now ) + { + PRINTF( " === YOCTOVM_OK, forced chain restart ===\n" ); + ret_code = master_start( mem_h/*, BUF_SIZE / 4, stack, stackSize*/ ); + zepto_response_to_request( mem_h ); + assert( ret_code == YOCTOVM_PASS_LOWER ); + // one more trick: wait for some time to ensure that master will start its own chain, and then send "our own" chain start +/* bool mutual = get_rand_val() % 5 == 0; + if ( mutual ) + justWait( 4 );*/ + } + else + { + PRINTF( " === YOCTOVM_OK, delayed chain restart ===\n" ); + wake_time_to_start_new_chain = getTime() + get_rand_val() % 8; + wait_for_incoming_chain_with_timer = true; + goto getmsg; + } + break; + } + default: + { + // unexpected ret_code + printf( "Unexpected ret_code %d\n", ret_code ); + assert( 0 ); + break; + } + } + + + + // 5. SAGDP +alt_entry: + uint8_t timer_val; + ret_code = handlerSAGDP_receiveHLP( &timer_val, NULL, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + if ( ret_code == SAGDP_RET_NEED_NONCE ) + { + ret_code = handlerSASP_get_nonce( nonce, SASP_NONCE_SIZE, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + assert( ret_code == SASP_RET_NONCE ); + ret_code = handlerSAGDP_receiveHLP( &timer_val, nonce, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SAGDP, msgLastSent ); + assert( ret_code != SAGDP_RET_NEED_NONCE ); + } + zepto_response_to_request( mem_h ); + printf( "SAGDP2: ret: %d; rq_size: %d, rsp_size: %d\n", ret_code, ugly_hook_get_request_size( mem_h ), ugly_hook_get_response_size( mem_h ) ); + + switch ( ret_code ) + { + case SAGDP_RET_TO_LOWER_NEW: + { + // regular processing will be done below in the next block + // set timer + break; + } + case SAGDP_RET_OK: // TODO: is it possible here? + { + goto getmsg; + break; + } + case SAGDP_RET_TO_LOWER_REPEATED: // TODO: is it possible here? + { + goto getmsg; + break; + } + case SAGDP_RET_SYS_CORRUPTED: // TODO: is it possible here? + { + // TODO: process reset + sagdp_init( data_buff + DADA_OFFSET_SAGDP ); +// bool start_now = get_rand_val() % 3; + bool start_now = true; + wake_time_to_start_new_chain = start_now ? getTime() : getTime() + get_rand_val() % 8; + wait_for_incoming_chain_with_timer = true; + zepto_response_to_request( mem_h ); + goto saspsend; + break; + } + default: + { + // unexpected ret_code + printf( "Unexpected ret_code %d\n", ret_code ); + assert( 0 ); + break; + } + } + + // SASP +saspsend: + ret_code = handlerSASP_send( nonce, mem_h, stack, stackSize, data_buff + DADA_OFFSET_SASP ); + zepto_response_to_request( mem_h ); + printf( "SASP2: ret: %d; rq_size: %d, rsp_size: %d\n", ret_code, ugly_hook_get_request_size( mem_h ), ugly_hook_get_response_size( mem_h ) ); + + switch ( ret_code ) + { + case SASP_RET_TO_LOWER_REGULAR: + { + // regular processing will be done below in the next block + break; + } + default: + { + // unexpected ret_code + printf( "Unexpected ret_code %d\n", ret_code ); + assert( 0 ); + break; + } + } + +sendmsg: + ret_code = sendMessage( mem_h ); + if (ret_code != COMMLAYER_RET_OK ) + { + return -1; + } + zepto_response_to_request( mem_h ); + INCREMENT_COUNTER( 90, "MAIN LOOP, packet sent" ); + printf("\nMessage replied to client\n"); + + } + + return 0; +} + +int main(int argc, char *argv[]) +{ +// return main_loop(); + REQUEST_REPLY_HANDLE mem_h = 0; + uint8_t main_buff_pad[0x20000]; + uint8_t* main_buff = main_buff_pad + 0x10000; + memory_objects[ mem_h ].ptr = main_buff; + memory_objects[ mem_h ].rq_size = 0; + memory_objects[ mem_h ].rsp_size = 0; + + return main_loop2( mem_h ); +} diff --git a/tests/emulator/sagdp_protocol.cpp b/tests/emulator/sagdp_protocol.cpp index 2fc2213..35707f9 100644 --- a/tests/emulator/sagdp_protocol.cpp +++ b/tests/emulator/sagdp_protocol.cpp @@ -88,6 +88,27 @@ void sagdp_init( uint8_t* data ) // memset( data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET, 0xff, SAGDP_PACKETID_SIZE ); } +uint8_t handlerSAGDP_timer( uint8_t* timeout, uint8_t* nonce, REQUEST_REPLY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ) +{ + uint8_t state = *( data + DATA_SAGDP_STATE_OFFSET ); + if ( state == SAGDP_STATE_WAIT_REMOTE ) + { + INCREMENT_COUNTER( 20, "handlerSAGDP_timer(), packet resent" ); + assert( nonce != NULL ); + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + cappedExponentiateLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_REMOTE; // note that PID can be changed! + return SAGDP_RET_TO_LOWER_REPEATED; + } + else // other states: ignore + { + return SAGDP_RET_OK; + } +} + uint8_t handlerSAGDP_timer( uint8_t* timeout, uint8_t* nonce, uint16_t* sizeInOut, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ) { uint8_t state = *( data + DATA_SAGDP_STATE_OFFSET ); @@ -103,9 +124,578 @@ uint8_t handlerSAGDP_timer( uint8_t* timeout, uint8_t* nonce, uint16_t* sizeInOu *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_REMOTE; // note that PID can be changed! return SAGDP_RET_TO_LOWER_REPEATED; } - else // other states: ignore + else // other states: ignore + { + return SAGDP_RET_OK; + } +} + +uint8_t handlerSAGDP_receiveUP( uint8_t* timeout, uint8_t* nonce, uint8_t* pid, REQUEST_REPLY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ) +{ + PRINTF( "handlerSAGDP_receiveNewUP(): pid: %x%x%x%x%x%x\n", pid[0], pid[1], pid[2], pid[3], pid[4], pid[5] ); + // sizeInOut represents a size of UP packet + // A new packet can come either in idle (beginning of a chain), or in wait-remote (continuation of a chain) state. + // As a result SAGDP changes its state to wait-local, or (in case of errors) to not-initialized state + + INCREMENT_COUNTER( 21, "handlerSAGDP_receiveUP()" ); + + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + + uint8_t state = *( data + DATA_SAGDP_STATE_OFFSET ); + uint8_t packet_status = zepto_parse_uint8( &po ); + packet_status &= SAGDP_P_STATUS_FULL_MASK; // TODO: use bit-field processing instead + PRINTF( "handlerSAGDP_receiveUP(): state: %d, packet_status: %d\n", state, packet_status ); + + if ( state == SAGDP_STATE_IDLE ) + { +#ifdef USED_AS_MASTER + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_ERROR_MSG ) + { + INCREMENT_COUNTER( 22, "handlerSAGDP_receiveUP(), idle, error message" ); + // TODO: process + cancelLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + + if ( zepto_parse_skip_block( &po, SAGDP_LRECEIVED_PID_SIZE ) ) + { + parser_obj po1; + zepto_parser_init( &po1, &po ); + uint16_t body_size = zepto_parsing_remaining_bytes( &po ); + zepto_parse_skip_block( &po1, body_size ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); + zepto_write_prepend_byte( mem_h, packet_status & SAGDP_P_STATUS_MASK ); + } + else + { + zepto_write_uint8( mem_h, packet_status & SAGDP_P_STATUS_MASK ); + } + + +/* buffOut[0] = ( packet_status & SAGDP_P_STATUS_MASK ); + if ( *sizeInOut > 1 + SAGDP_LRECEIVED_PID_SIZE ) + { + *sizeInOut -= SAGDP_LRECEIVED_PID_SIZE; + memcpy( buffOut + 1, buffIn + 1 + SAGDP_LRECEIVED_PID_SIZE, *sizeInOut-1 ); + } + else + *sizeInOut = 1;*/ + + + + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_IDLE; + return SAGDP_RET_TO_HIGHER; + } + else if ( ( packet_status & SAGDP_P_STATUS_MASK ) != SAGDP_P_STATUS_FIRST ) + { + uint8_t* buffIn = stack; + zepto_parse_read_block( &po, buffIn, SAGDP_LSENT_PID_SIZE ); + uint8_t* pidlsent_first = data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET; + uint8_t* pidlsent_last = data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET; + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent first : %x%x%x%x%x%x\n", pidlsent_first[0], pidlsent_first[1], pidlsent_first[2], pidlsent_first[3], pidlsent_first[4], pidlsent_first[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID reply-to in packet: %x%x%x%x%x%x\n", buffIn[0], buffIn[1], buffIn[2], buffIn[3], buffIn[4], buffIn[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent last : %x%x%x%x%x%x\n", pidlsent_last[0], pidlsent_last[1], pidlsent_last[2], pidlsent_last[3], pidlsent_last[4], pidlsent_last[5] ); + bool isold = pid_compare( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET ) < 0; + if ( isold ) + { + // TODO: check against previous range + INCREMENT_COUNTER( 23, "handlerSAGDP_receiveUP(), idle, is-old" ); + PRINTF( "SAGDP OK: state = %d, packet_status = %d; isold\n", state, packet_status ); + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 24, "handlerSAGDP_receiveUP(), idle, is-old intermediate" ); + + PRINTF( "handlerSAGDP_receiveUP(): nonce: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + // re-send LSP + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + // SAGDP status remains the same + return SAGDP_RET_TO_LOWER_REPEATED; + } + else + { + INCREMENT_COUNTER( 25, "handlerSAGDP_receiveUP(), idle, is-old terminating" ); + assert( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_TERMINATING ); + return SAGDP_RET_OK; // ignored + } + } + bool isreply = is_pid_in_range( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET ); + if ( !isreply ) // above the range; silently ignore + { + INCREMENT_COUNTER( 26, "handlerSAGDP_receiveUP(), idle, too old, ignored" ); + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d, !isreply\n", state, packet_status ); + return SAGDP_RET_OK; + } + INCREMENT_COUNTER( 27, "handlerSAGDP_receiveUP(), idle, other" ); + // for non-terminating, save packet ID + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ) + { + return SAGDP_RET_OK; // ignored + } + else + { + assert( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_TERMINATING ); + return SAGDP_RET_OK; // ignored + } + } +#else // USED_AS_MASTER not ndefined + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_ERROR_MSG ) // unexpected at slave's side + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 22, "handlerSAGDP_receiveUP(), idle, error message" ); + // send an error message to a communication partner and reinitialize + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_SYS_CORRUPTED; + } + if ( ( packet_status & SAGDP_P_STATUS_MASK ) != SAGDP_P_STATUS_FIRST ) + { +// *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; +// PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); +// return SAGDP_RET_SYS_CORRUPTED; + uint8_t* buffIn = stack; + zepto_parse_read_block( &po, buffIn, SAGDP_LSENT_PID_SIZE ); + uint8_t* pidprevlsent_first = data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET; + uint8_t* pidlsent_first = data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET; + uint8_t* pidlsent_last = data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET; + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent first : %x%x%x%x%x%x\n", pidprevlsent_first[0], pidprevlsent_first[1], pidprevlsent_first[2], pidprevlsent_first[3], pidprevlsent_first[4], pidprevlsent_first[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent first : %x%x%x%x%x%x\n", pidlsent_first[0], pidlsent_first[1], pidlsent_first[2], pidlsent_first[3], pidlsent_first[4], pidlsent_first[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID reply-to in packet: %x%x%x%x%x%x\n", buffIn[0], buffIn[1], buffIn[2], buffIn[3], buffIn[4], buffIn[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent last : %x%x%x%x%x%x\n", pidlsent_last[0], pidlsent_last[1], pidlsent_last[2], pidlsent_last[3], pidlsent_last[4], pidlsent_last[5] ); + bool isold = (!is_pid_zero(data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET)) && pid_compare( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET ) < 0 && pid_compare( buffIn, data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET ) >= 0; + if ( isold ) + { + INCREMENT_COUNTER( 23, "handlerSAGDP_receiveUP(), idle, is-old" ); + PRINTF( "SAGDP OK: state = %d, packet_status = %d; isold\n", state, packet_status ); + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ) + { + // re-send LSP + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 24, "handlerSAGDP_receiveUP(), idle, is-old intermediate" ); + + PRINTF( "handlerSAGDP_receiveUP(): nonce: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + return SAGDP_RET_TO_LOWER_REPEATED; + } + else + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 25, "handlerSAGDP_receiveUP(), idle, is-old terminating" ); + assert( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_TERMINATING ); + // send an error message to a communication partner and reinitialize + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_SYS_CORRUPTED; + } +// return handlerSAGDP_receiveRepeatedUP( timeout, sizeInOut, buffIn, buffOut, buffOutSize, stack, stackSize, data, lsm ); + } + bool isreply = is_pid_in_range( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET ); + if ( !isreply ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 26, "handlerSAGDP_receiveUP(), idle, too old, sys corrupted" ); + // send an error message to a communication partner and reinitialize + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_SYS_CORRUPTED; + } + // for non-terminating, save packet ID + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 27, "handlerSAGDP_receiveUP(), idle, intermediate, sys corrupted" ); + // send an error message to a communication partner and reinitialize + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_SYS_CORRUPTED; + } + else + { + INCREMENT_COUNTER( 28, "handlerSAGDP_receiveUP(), idle, terminating, ignored" ); + assert( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_TERMINATING ); + return SAGDP_RET_OK; // ignored + } + } +#endif + else // allowed combination: packet_status == SAGDP_P_STATUS_FIRST in SAGDP_STATE_IDLE + { + INCREMENT_COUNTER( 29, "handlerSAGDP_receiveUP(), idle, first" ); + assert( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_FIRST && state == SAGDP_STATE_IDLE ); + // note: this "first" packet can be start of a new chain, or a re-sent of the beginning of the previous chain (if that previous chain had a length of 2) + zepto_parse_read_block( &po, stack, SAGDP_LSENT_PID_SIZE ); + const uint8_t* this_chain_id = stack; + const uint8_t* prev_chain_id = data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET; + PRINTF( "handlerSAGDP_receiveNewUP(): this_chain_id: %x%x%x%x%x%x\n", this_chain_id[0], this_chain_id[1], this_chain_id[2], this_chain_id[3], this_chain_id[4], this_chain_id[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): prev_chain_id: %x%x%x%x%x%x\n", prev_chain_id[0], prev_chain_id[1], prev_chain_id[2], prev_chain_id[3], prev_chain_id[4], prev_chain_id[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): pid: %x%x%x%x%x%x\n", pid[0], pid[1], pid[2], pid[3], pid[4], pid[5] ); + bool is_resent = pid_compare( this_chain_id, prev_chain_id ) == 0; + if ( is_resent ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 30, "handlerSAGDP_receiveUP(), idle, first, resent" ); + + PRINTF( "handlerSAGDP_receiveUP(): nonce: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + // re-send LSP + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + // SAGDP status remains the same + return SAGDP_RET_TO_LOWER_REPEATED; + } + else + { + INCREMENT_COUNTER( 31, "handlerSAGDP_receiveUP(), idle, first, new" ); + cancelLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + memcpy( data + DATA_SAGDP_LRECEIVED_PID_OFFSET, pid, SAGDP_LRECEIVED_PID_SIZE ); + memcpy( data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET, this_chain_id, SAGDP_LRECEIVED_PID_SIZE ); + +/* *sizeInOut -= SAGDP_LRECEIVED_PID_SIZE; + buffOut[0] = ( packet_status & SAGDP_P_STATUS_MASK ); + memcpy( buffOut + 1, buffIn + 1 + SAGDP_LRECEIVED_PID_SIZE, *sizeInOut-1 );*/ + parser_obj po1; + zepto_parser_init( &po1, &po ); + uint16_t body_size = zepto_parsing_remaining_bytes( &po ); + zepto_parse_skip_block( &po1, body_size ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); + zepto_write_prepend_byte( mem_h, packet_status & SAGDP_P_STATUS_MASK ); + + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_LOCAL; + return SAGDP_RET_TO_HIGHER; + } + } + } + + else if ( state == SAGDP_STATE_WAIT_REMOTE ) + { +#ifdef USED_AS_MASTER + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_ERROR_MSG ) + { + INCREMENT_COUNTER( 40, "handlerSAGDP_receiveUP(), wait-remote, error" ); + cancelLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); +/* buffOut[0] = ( packet_status & SAGDP_P_STATUS_MASK ); + if ( *sizeInOut > 1 + SAGDP_LRECEIVED_PID_SIZE ) + { + *sizeInOut -= SAGDP_LRECEIVED_PID_SIZE; + memcpy( buffOut + 1, buffIn + 1 + SAGDP_LRECEIVED_PID_SIZE, *sizeInOut-1 ); + } + else + *sizeInOut = 1;*/ + if ( zepto_parse_skip_block( &po, SAGDP_LRECEIVED_PID_SIZE ) ) + { + parser_obj po1; + zepto_parser_init( &po1, &po ); + uint16_t body_size = zepto_parsing_remaining_bytes( &po ); + zepto_parse_skip_block( &po1, body_size ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); + zepto_write_prepend_byte( mem_h, packet_status & SAGDP_P_STATUS_MASK ); + } + else + { + zepto_write_uint8( mem_h, packet_status & SAGDP_P_STATUS_MASK ); + } + + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_IDLE; + return SAGDP_RET_TO_HIGHER; + } + else if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_FIRST ) + { + INCREMENT_COUNTER( 41, "handlerSAGDP_receiveUP(), wait-remote, first" ); + // note: this "first" packet can be start of a new chain, or a re-sent of the beginning of the previous chain (if that previous chain had a length of 2) + zepto_parse_read_block( &po, stack, SAGDP_LSENT_PID_SIZE ); + const uint8_t* this_chain_id = stack; + const uint8_t* prev_chain_id = data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET; + bool is_resent = pid_compare( this_chain_id, prev_chain_id ) == 0; + if ( is_resent ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + + INCREMENT_COUNTER( 42, "handlerSAGDP_receiveUP(), wait-remote, first, resent" ); + PRINTF( "handlerSAGDP_receiveUP(): nonce: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + // re-send LSP + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + // SAGDP status remains the same + return SAGDP_RET_TO_LOWER_REPEATED; + } + else + { + INCREMENT_COUNTER( 43, "handlerSAGDP_receiveUP(), wait-remote, first, new (ignored)" ); + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_OK; // just ignore + // TODO: form a packet +// return SAGDP_RET_TO_HIGHER_ERROR; + } + } + else + { + assert( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE || ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_TERMINATING ); + uint8_t* pidlsent_first = data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET; + uint8_t* pidlsent_last = data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET; + zepto_parse_read_block( &po, stack, SAGDP_LSENT_PID_SIZE ); + uint8_t* buffIn = stack; + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent first : %x%x%x%x%x%x\n", pidlsent_first[0], pidlsent_first[1], pidlsent_first[2], pidlsent_first[3], pidlsent_first[4], pidlsent_first[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID reply-to in packet: %x%x%x%x%x%x\n", buffIn[0], buffIn[1], buffIn[2], buffIn[3], buffIn[4], buffIn[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent last : %x%x%x%x%x%x\n", pidlsent_last[0], pidlsent_last[1], pidlsent_last[2], pidlsent_last[3], pidlsent_last[4], pidlsent_last[5] ); +// bool isreply = memcmp( buffIn + 1, data + DATA_SAGDP_LSENT_PID_OFFSET, SAGDP_LSENT_PID_SIZE ) == 0; + bool isold = pid_compare( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET ) < 0; + if ( isold ) + { + // TODO: check against too-old status (previous last sent first) +// return handlerSAGDP_receiveRepeatedUP( timeout, sizeInOut, buffIn, buffOut, buffOutSize, stack, stackSize, data, lsm ); + parser_obj po1; + zepto_parser_init( &po1, mem_h ); + if ( ( zepto_parse_uint8( &po1 ) & SAGDP_P_STATUS_NO_RESEND ) == 0 ) // TODO: use bit-field processing instead + { + PRINTF( "SAGDP: state = %d, packet_status = %d; isold\n", state, packet_status ); + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 44, "handlerSAGDP_receiveUP(), wait-remote, is-old, resend requested" ); + + PRINTF( "handlerSAGDP_receiveUP(): nonce: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + +// *(data+DATA_SAGDP_ALREADY_REPLIED_OFFSET) = 1; + cappedExponentiateLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + + +/* *sizeInOut = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + memcpy( buffOut, lsm, *sizeInOut ); + buffOut[0] |= SAGDP_P_STATUS_NO_RESEND;*/ + // TODO: construction below represents the logic above but is a little bit strange... think about proper processing + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm+1, packet_sz-1 ); + zepto_write_prepend_byte( mem_h, lsm[0] | SAGDP_P_STATUS_NO_RESEND ); + + + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_REMOTE; // note that PID can be changed! + return SAGDP_RET_TO_LOWER_REPEATED; + } + else + { + INCREMENT_COUNTER( 45, "handlerSAGDP_receiveUP(), wait-remote, is-old, resend NOT requested" ); + PRINTF( "SAGDP OK: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_OK; + } + } + bool isreply = is_pid_in_range( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET ); + if ( !isreply ) + { + INCREMENT_COUNTER( 46, "handlerSAGDP_receiveUP(), wait-remote, !is-reply, ignored" ); + PRINTF( "SAGDP OK: CORRRUPTED: state = %d, packet_status = %d, !isreply\n", state, packet_status ); + return SAGDP_RET_OK; // silently ignore + // TODO: form a packet +// return SAGDP_RET_TO_HIGHER_ERROR; + } + INCREMENT_COUNTER( 47, "handlerSAGDP_receiveUP(), wait-remote, other" ); + // for non-terminating, save packet ID + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ) + { + INCREMENT_COUNTER( 48, "handlerSAGDP_receiveUP(), wait-remote, other, intermediate" ); + PRINTF( "handlerSAGDP_receiveHLP(): PID of packet (LRECEIVED): %x%x%x%x%x%x\n", pid[0], pid[1], pid[2], pid[3], pid[4], pid[5] ); + memcpy( data + DATA_SAGDP_LRECEIVED_PID_OFFSET, pid, SAGDP_LRECEIVED_PID_SIZE ); + } + // form a packet for higher level +/* *sizeInOut -= SAGDP_LRECEIVED_PID_SIZE; + buffOut[0] = ( packet_status & SAGDP_P_STATUS_MASK ); + memcpy( buffOut + 1, buffIn + 1 + SAGDP_LRECEIVED_PID_SIZE, *sizeInOut - 1 );*/ + parser_obj po1; + zepto_parser_init( &po1, &po ); + uint16_t body_size = zepto_parsing_remaining_bytes( &po ); + zepto_parse_skip_block( &po1, body_size ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); + zepto_write_prepend_byte( mem_h, packet_status & SAGDP_P_STATUS_MASK ); + + cancelLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + *( data + DATA_SAGDP_STATE_OFFSET ) = ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ? SAGDP_STATE_WAIT_LOCAL : SAGDP_STATE_IDLE; + return SAGDP_RET_TO_HIGHER; + } +#else // USED_AS_MASTER not defined + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_ERROR_MSG ) + { + // send an error message to a communication partner and reinitialize + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 40, "handlerSAGDP_receiveUP(), wait-remote, error" ); + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_SYS_CORRUPTED; + } + else if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_FIRST ) + { + INCREMENT_COUNTER( 41, "handlerSAGDP_receiveUP(), wait-remote, first" ); + // main question: is it a re-sent or a start of an actually new chain + zepto_parse_read_block( &po, stack, SAGDP_LSENT_PID_SIZE ); + uint8_t* buffIn = stack; + bool current = pid_compare( buffIn, data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET ) == 0; + if ( current ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 42, "handlerSAGDP_receiveUP(), wait-remote, first, resent" ); + + PRINTF( "handlerSAGDP_receiveUP(): nonce: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + return SAGDP_RET_TO_LOWER_REPEATED; + } + else + { + INCREMENT_COUNTER( 43, "handlerSAGDP_receiveUP(), wait-remote, first, new (applied)" ); + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_START_OVER_FIRST_RECEIVED; + } + } + else // intermediate or terminating + { + assert( ( packet_status & SAGDP_P_STATUS_MASK ) != SAGDP_P_STATUS_FIRST ); + + zepto_parse_read_block( &po, stack, SAGDP_LSENT_PID_SIZE ); + uint8_t* buffIn = stack; + uint8_t* pidprevlsent_first = data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET; + uint8_t* pidlsent_first = data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET; + uint8_t* pidlsent_last = data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET; + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent first : %x%x%x%x%x%x\n", pidprevlsent_first[0], pidprevlsent_first[1], pidprevlsent_first[2], pidprevlsent_first[3], pidprevlsent_first[4], pidprevlsent_first[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent first : %x%x%x%x%x%x\n", pidlsent_first[0], pidlsent_first[1], pidlsent_first[2], pidlsent_first[3], pidlsent_first[4], pidlsent_first[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID reply-to in packet: %x%x%x%x%x%x\n", buffIn[0], buffIn[1], buffIn[2], buffIn[3], buffIn[4], buffIn[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): PID last sent last : %x%x%x%x%x%x\n", pidlsent_last[0], pidlsent_last[1], pidlsent_last[2], pidlsent_last[3], pidlsent_last[4], pidlsent_last[5] ); + bool isold = (!is_pid_zero(data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET)) && pid_compare( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET ) < 0 && pid_compare( buffIn, data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET ) >= 0; + if ( isold ) + { + parser_obj po1; + zepto_parser_init( &po1, mem_h ); + if ( ( zepto_parse_uint8( &po1 ) & SAGDP_P_STATUS_NO_RESEND ) == 0 ) // TODO: use bit-field processing instead + { + PRINTF( "SAGDP: state = %d, packet_status = %d; isold\n", state, packet_status ); + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 44, "handlerSAGDP_receiveUP(), wait-remote, is-old, resend requested" ); + + PRINTF( "handlerSAGDP_receiveUP(): nonce: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + +// *(data+DATA_SAGDP_ALREADY_REPLIED_OFFSET) = 1; + cappedExponentiateLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + /*sizeInOut = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + memcpy( buffOut, lsm, *sizeInOut ); + buffOut[0] |= SAGDP_P_STATUS_NO_RESEND;*/ + // TODO: construction below represents the logic above but is a little bit strange... think about proper processing + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + zepto_write_block( mem_h, lsm+1, packet_sz-1 ); + zepto_write_prepend_byte( mem_h, lsm[0] | SAGDP_P_STATUS_NO_RESEND ); + + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_REMOTE; // note that PID can be changed! + return SAGDP_RET_TO_LOWER_REPEATED; + } + else + { + INCREMENT_COUNTER( 45, "handlerSAGDP_receiveUP(), wait-remote, is-old, resend NOT requested" ); + PRINTF( "SAGDP OK: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_OK; + } + } + bool isreply = is_pid_in_range( buffIn, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET ); + if ( !isreply ) // silently ignore + { + // send an error message to a communication partner and reinitialize + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 46, "handlerSAGDP_receiveUP(), wait-remote, !is-reply, ignored" ); + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d, !isreply\n", state, packet_status ); + return SAGDP_RET_SYS_CORRUPTED; + } + INCREMENT_COUNTER( 47, "handlerSAGDP_receiveUP(), wait-remote, other" ); + // for non-terminating, save packet ID + if ( ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ) + { + INCREMENT_COUNTER( 48, "handlerSAGDP_receiveUP(), wait-remote, other, intermediate" ); + PRINTF( "handlerSAGDP_receiveHLP(): PID of packet (LRECEIVED): %x%x%x%x%x%x\n", pid[0], pid[1], pid[2], pid[3], pid[4], pid[5] ); + memcpy( data + DATA_SAGDP_LRECEIVED_PID_OFFSET, pid, SAGDP_LRECEIVED_PID_SIZE ); + } + // form a packet for higher level +/* *sizeInOut -= SAGDP_LRECEIVED_PID_SIZE; + buffOut[0] = ( packet_status & SAGDP_P_STATUS_MASK ); + memcpy( buffOut + 1, buffIn + 1 + SAGDP_LRECEIVED_PID_SIZE, *sizeInOut - 1 );*/ + parser_obj po1; + zepto_parser_init( &po1, &po ); + uint16_t body_size = zepto_parsing_remaining_bytes( &po ); + zepto_parse_skip_block( &po1, body_size ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); + zepto_write_prepend_byte( mem_h, packet_status & SAGDP_P_STATUS_MASK ); + + cancelLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + *( data + DATA_SAGDP_STATE_OFFSET ) = ( packet_status & SAGDP_P_STATUS_MASK ) == SAGDP_P_STATUS_INTERMEDIATE ? SAGDP_STATE_WAIT_LOCAL : SAGDP_STATE_IDLE; + return SAGDP_RET_TO_HIGHER; + } +#endif + } + + else // invalid states { - return SAGDP_RET_OK; + INCREMENT_COUNTER( 50, "handlerSAGDP_receiveUP(), invalid state" ); +#if !defined USED_AS_MASTER + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + // send an error message to a communication partner and reinitialize + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); +#endif + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + PRINTF( "SAGDP: CORRRUPTED: state = %d, packet_status = %d\n", state, packet_status ); + return SAGDP_RET_SYS_CORRUPTED; } } @@ -306,6 +896,7 @@ uint8_t handlerSAGDP_receiveUP( uint8_t* timeout, uint8_t* nonce, uint8_t* pid, const uint8_t* prev_chain_id = data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET; PRINTF( "handlerSAGDP_receiveNewUP(): this_chain_id: %x%x%x%x%x%x\n", this_chain_id[0], this_chain_id[1], this_chain_id[2], this_chain_id[3], this_chain_id[4], this_chain_id[5] ); PRINTF( "handlerSAGDP_receiveNewUP(): prev_chain_id: %x%x%x%x%x%x\n", prev_chain_id[0], prev_chain_id[1], prev_chain_id[2], prev_chain_id[3], prev_chain_id[4], prev_chain_id[5] ); + PRINTF( "handlerSAGDP_receiveNewUP(): pid: %x%x%x%x%x%x\n", pid[0], pid[1], pid[2], pid[3], pid[4], pid[5] ); bool is_resent = pid_compare( this_chain_id, prev_chain_id ) == 0; if ( is_resent ) { @@ -602,6 +1193,64 @@ uint8_t handlerSAGDP_receiveUP( uint8_t* timeout, uint8_t* nonce, uint8_t* pid, } } +uint8_t handlerSAGDP_receiveRequestResendLSP( uint8_t* timeout, uint8_t* nonce, MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ) +{ + // SAGDP can legitimately receive a repeated packet in wait-remote state (the other side sounds like "we have not received anything from you; please resend, only then we will probably send you something new") + // LSP must be resent + + uint16_t packet_sz = *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET); + if ( packet_sz == 0 ) + { + INCREMENT_COUNTER( 63, "handlerSAGDP_receiveRequestResendLSP(), no lsm" ); + return SAGDP_RET_TO_LOWER_NONE; + } + + uint8_t state = *( data + DATA_SAGDP_STATE_OFFSET ); + if ( state == SAGDP_STATE_WAIT_REMOTE ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 60, "handlerSAGDP_receiveRequestResendLSP(), wait-remote" ); + + PRINTF( "handlerSAGDP_receivePID(): PID: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + cappedExponentiateLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_REMOTE; // note that PID can be changed! + return SAGDP_RET_TO_LOWER_REPEATED; + } + if ( state == SAGDP_STATE_IDLE ) + { + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + INCREMENT_COUNTER( 61, "handlerSAGDP_receiveRequestResendLSP(), idle" ); + + PRINTF( "handlerSAGDP_receivePID(): PID: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + cappedExponentiateLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + zepto_write_block( mem_h, lsm, packet_sz ); + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_IDLE; // note that PID can be changed! + return SAGDP_RET_TO_LOWER_REPEATED; + } + else // invalid states + { + INCREMENT_COUNTER( 62, "handlerSAGDP_receiveRequestResendLSP(), invalid state" ); + // send an error message to a communication partner and reinitialize + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + return SAGDP_RET_SYS_CORRUPTED; + } +} + uint8_t handlerSAGDP_receiveRequestResendLSP( uint8_t* timeout, uint8_t* nonce, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ) { // SAGDP can legitimately receive a repeated packet in wait-remote state (the other side sounds like "we have not received anything from you; please resend, only then we will probably send you something new") @@ -661,6 +1310,161 @@ uint8_t handlerSAGDP_receiveRequestResendLSP( uint8_t* timeout, uint8_t* nonce, } } +uint8_t handlerSAGDP_receiveHLP( uint8_t* timeout, uint8_t* nonce, MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ) +{ + // Important: sizeInOut is a size of the message; returned size: sizeInOut += SAGDP_LRECEIVED_PID_SIZE + // + // there are two states when SAGDP can legitimately receive a packet from a higher level: idle (packet is first in the chain), and wait-local (any subsequent packet) + // the packet is processed and passed for further sending; SAGDP waits for its PID and thus transits to SAGDP_STATE_WAIT_PID + // + // It is a responsibility of a higher level to report the status of a packet. + // + + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + + INCREMENT_COUNTER( 70, "handlerSAGDP_receiveHLP()" ); + + uint8_t state = *( data + DATA_SAGDP_STATE_OFFSET ); + uint8_t packet_status = zepto_parse_uint8( &po ); + PRINTF( "handlerSAGDP_receiveHLP(): state = %d, packet_status = %d\n", state, packet_status ); + + if ( state == SAGDP_STATE_IDLE ) + { + if ( ( packet_status & SAGDP_P_STATUS_FIRST ) == 0 || ( packet_status & SAGDP_P_STATUS_TERMINATING ) ) + { +#ifdef USED_AS_MASTER + // TODO: should we do anything else but error reporting? +#else + // send an error message to a communication partner and reinitialize + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; +#endif // USED_AS_MASTER + INCREMENT_COUNTER( 71, "handlerSAGDP_receiveHLP(), idle, state/packet mismatch" ); + return SAGDP_RET_SYS_CORRUPTED; + } + assert( ( packet_status & ( SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING ) ) == SAGDP_P_STATUS_FIRST ); // in idle state we can expect only "first" packet + assert( packet_status == SAGDP_P_STATUS_FIRST ); + + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + + PRINTF( "handlerSAGDP_receivePID(): PID: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + memcpy( data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, SAGDP_LSENT_PID_SIZE ); + memcpy( data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + // "chain id" is shared between devices and therefore, should be unique for both sides, that is, shoud have master/slave distinguishing bit + memcpy( data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + *(data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET + SAGDP_PACKETID_SIZE - 1) |= ( MASTER_SLAVE_BIT << 7 ); + + // form a UP packet + assert( ( packet_status & ( ~( SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING ) ) ) == 0 ); // TODO: can we rely on sanity of the caller? + parser_obj po1; + zepto_parser_init( &po1, &po ); + uint16_t body_size = zepto_parsing_remaining_bytes( &po ); + zepto_parse_skip_block( &po1, body_size ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); + zepto_write_prepend_block( mem_h, data + DATA_SAGDP_LRECEIVED_CHAIN_ID_OFFSET, SAGDP_LRECEIVED_PID_SIZE ); + zepto_write_prepend_byte( mem_h, packet_status & ( SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING ) ); + + // save a copy + *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET) = zepto_writer_get_response_size( mem_h ); + zepto_writer_get_copy_of_response( mem_h, lsm ); + + // request set timer + setIniLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + +// *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_PID; +// *( data + DATA_SAGDP_STATE_OFFSET ) = packet_status == SAGDP_P_STATUS_TERMINATING ? SAGDP_STATE_WAIT_FIRST_PID_THEN_IDLE : SAGDP_STATE_WAIT_FIRST_PID_THEN_WR; + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_REMOTE; +// *(data+DATA_SAGDP_ALREADY_REPLIED_OFFSET) = 0; + INCREMENT_COUNTER( 72, "handlerSAGDP_receiveHLP(), idle, PACKET=FIRST" ); + return SAGDP_RET_TO_LOWER_NEW; + } + else if ( state == SAGDP_STATE_WAIT_LOCAL ) + { + if ( packet_status & SAGDP_P_STATUS_FIRST ) + { +#ifdef USED_AS_MASTER + // TODO: should we do anything else but error reporting? +#else + // send an error message to a communication partner and reinitialize + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; +#endif + INCREMENT_COUNTER( 73, "handlerSAGDP_receiveHLP(), wait-remote, state/packet mismatch" ); + return SAGDP_RET_SYS_CORRUPTED; + } + + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + + PRINTF( "handlerSAGDP_receivePID(): PID: %x%x%x%x%x%x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + + // apply nonce + bool is_prev = pid_compare( data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET ) != 0; + if ( is_prev ) + memcpy( data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET, data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, SAGDP_LSENT_PID_SIZE ); + else + memcpy( data + DATA_SAGDP_PREV_FIRST_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + memcpy( data + DATA_SAGDP_FIRST_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + memcpy( data + DATA_SAGDP_NEXT_LSENT_PID_OFFSET, nonce, SAGDP_LSENT_PID_SIZE ); + + // form a UP packet + assert( ( packet_status & ( ~( SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING ) ) ) == 0 ); // TODO: can we rely on sanity of the caller? + parser_obj po1; + zepto_parser_init( &po1, &po ); + uint16_t body_size = zepto_parsing_remaining_bytes( &po ); + zepto_parse_skip_block( &po1, body_size ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); + zepto_write_prepend_block( mem_h, data + DATA_SAGDP_LRECEIVED_PID_OFFSET, SAGDP_LRECEIVED_PID_SIZE ); + zepto_write_prepend_byte( mem_h, packet_status & ( SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING ) ); + + // save a copy + *(uint16_t*)(data+DATA_SAGDP_LSM_SIZE_OFFSET) = zepto_writer_get_response_size( mem_h ); + zepto_writer_get_copy_of_response( mem_h, lsm ); + + // request set timer + setIniLTO( data + DATA_SAGDP_LTO_OFFSET ); + *timeout = *(data + DATA_SAGDP_LTO_OFFSET); + +// *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_WAIT_PID; +// *( data + DATA_SAGDP_STATE_OFFSET ) = packet_status == SAGDP_P_STATUS_TERMINATING ? SAGDP_STATE_WAIT_FIRST_PID_THEN_IDLE : SAGDP_STATE_WAIT_FIRST_PID_THEN_WR; + *( data + DATA_SAGDP_STATE_OFFSET ) = packet_status == SAGDP_P_STATUS_TERMINATING ? SAGDP_STATE_IDLE : SAGDP_STATE_WAIT_REMOTE; +// *(data+DATA_SAGDP_ALREADY_REPLIED_OFFSET) = 0; + INCREMENT_COUNTER( 74, "handlerSAGDP_receiveHLP(), wait-remote, intermediate/terminating" ); + INCREMENT_COUNTER_IF( 75, "handlerSAGDP_receiveHLP(), wait-remote, terminating", (packet_status >> 1) ); + return SAGDP_RET_TO_LOWER_NEW; + } + else // invalid states + { +#ifdef USED_AS_MASTER + // TODO: should we do anything else but error reporting? +#else + // send an error message to a communication partner and reinitialize + if ( nonce == NULL ) + return SAGDP_RET_NEED_NONCE; + // send an error message to a communication partner and reinitialize + zepto_write_uint8( mem_h, SAGDP_P_STATUS_ERROR_MSG ); + // TODO: add other relevant data, if any, and update sizeInOut +#endif + *( data + DATA_SAGDP_STATE_OFFSET ) = SAGDP_STATE_NOT_INITIALIZED; + INCREMENT_COUNTER( 76, "handlerSAGDP_receiveHLP(), invalid state" ); + return SAGDP_RET_SYS_CORRUPTED; + } +} + uint8_t handlerSAGDP_receiveHLP( uint8_t* timeout, uint8_t* nonce, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ) { // Important: sizeInOut is a size of the message; returned size: sizeInOut += SAGDP_LRECEIVED_PID_SIZE diff --git a/tests/emulator/sagdp_protocol.h b/tests/emulator/sagdp_protocol.h index 9ea9eff..dd39f4b 100644 --- a/tests/emulator/sagdp_protocol.h +++ b/tests/emulator/sagdp_protocol.h @@ -21,6 +21,7 @@ Copyright (C) 2015 OLogN Technologies AG #include "sa-common.h" //#include "sa-eeprom.h" +#include "zepto-mem-mngmt.h" // RET codes @@ -77,7 +78,10 @@ uint8_t handlerSAGDP_timer( uint8_t* timeout, uint8_t* nonce, uint16_t* sizeInOu uint8_t handlerSAGDP_receiveUP( uint8_t* timeout, uint8_t* nonce, uint8_t* pid, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ); uint8_t handlerSAGDP_receiveRequestResendLSP( uint8_t* timeout, uint8_t* nonce, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ); uint8_t handlerSAGDP_receiveHLP( uint8_t* timeout, uint8_t* nonce, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ); -//uint8_t handlerSAGDP_receivePID( uint8_t* PID, uint8_t* data ); +uint8_t handlerSAGDP_timer( uint8_t* timeout, uint8_t* nonce, REQUEST_REPLY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ); +uint8_t handlerSAGDP_receiveUP( uint8_t* timeout, uint8_t* nonce, uint8_t* pid, REQUEST_REPLY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ); +uint8_t handlerSAGDP_receiveRequestResendLSP( uint8_t* timeout, uint8_t* nonce, MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ); +uint8_t handlerSAGDP_receiveHLP( uint8_t* timeout, uint8_t* nonce, MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data, uint8_t* lsm ); #endif // __SAGDP_PROTOCOL_H__ \ No newline at end of file diff --git a/tests/emulator/sasp_protocol.cpp b/tests/emulator/sasp_protocol.cpp index 990acbd..28cf451 100644 --- a/tests/emulator/sasp_protocol.cpp +++ b/tests/emulator/sasp_protocol.cpp @@ -19,7 +19,7 @@ Copyright (C) 2015 OLogN Technologies AG #include "sasp_protocol.h" #include "zepto-mem-mngmt.h" -#define NEW_APPROACH +//#define NEW_APPROACH @@ -28,7 +28,7 @@ void SASP_initAtLifeStart( uint8_t* dataBuff ) memset( dataBuff + DATA_SASP_NONCE_LW_OFFSET, 0, SASP_NONCE_SIZE ); memset( dataBuff + DATA_SASP_NONCE_LS_OFFSET, 0, SASP_NONCE_SIZE ); *( dataBuff + DATA_SASP_NONCE_LS_OFFSET + SASP_NONCE_SIZE - 1 ) = 1; - memset( dataBuff + DATA_SASP_LRPS_OFFSET, 0, SASP_TAG_SIZE ); +// memset( dataBuff + DATA_SASP_LRPS_OFFSET, 0, SASP_TAG_SIZE ); eeprom_write( DATA_SASP_NONCE_LW_ID, dataBuff + DATA_SASP_NONCE_LW_OFFSET, SASP_NONCE_SIZE ); eeprom_write( DATA_SASP_NONCE_LW_ID, dataBuff + DATA_SASP_NONCE_LW_OFFSET, SASP_NONCE_SIZE ); @@ -36,7 +36,7 @@ void SASP_initAtLifeStart( uint8_t* dataBuff ) void SASP_restoreFromBackup( uint8_t* dataBuff ) { - memset( dataBuff + DATA_SASP_LRPS_OFFSET, 0, SASP_TAG_SIZE ); +// memset( dataBuff + DATA_SASP_LRPS_OFFSET, 0, SASP_TAG_SIZE ); uint8_t size; @@ -620,6 +620,66 @@ bool SASP_IntraPacketAuthenticateAndDecrypt( uint8_t* pid, uint16_t* sizeInOut, return tagOK; } +void DEBUG_SASP_EncryptAndAddAuthenticationDataChecked( MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, const uint8_t* nonce ) +{ + parser_obj po, po1; + zepto_parser_init( &po, mem_h ); + uint16_t inisz = zepto_parsing_remaining_bytes( &po ); + + PRINTF( "handlerSASP_send(): nonce used: %02x %02x %02x %02x %02x %02x\n", nonce[0], nonce[1], nonce[2], nonce[3], nonce[4], nonce[5] ); + PRINTF( "handlerSASP_send(): size: %d\n", inisz ); +/* if ( inisz == 0 ) + { + inisz = 0; + }*/ + + uint16_t encr_sz, decr_sz; + uint8_t inimsg[1024]; zepto_parse_read_block( &po, inimsg, inisz ); + uint8_t checkedMsg[1024]; + uint8_t dbg_stack[1024]; + uint8_t dbg_nonce[6]; + + + // do required job + SASP_EncryptAndAddAuthenticationData( mem_h, stack, stackSize, nonce ); + + // CHECK RESULTS + + // init parser object + zepto_parser_init( &po, mem_h ); + zepto_parser_init( &po1, mem_h ); + + + // TODO: if this debug code remains in use, lines below must be replaced by getting a new handle + REQUEST_REPLY_HANDLE mem_h2 = 110; + uint8_t x_buff1_x[1024], x_buff1_x2[1024]; + uint8_t* x_buff1 = x_buff1_x + 512; + memory_objects[ mem_h2 ].ptr = x_buff1; + memory_objects[ mem_h2 ].rq_size = 0; + memory_objects[ mem_h2 ].rsp_size = 0; + // copy output to input of a new handle and restore + zepto_response_to_request( mem_h ); + encr_sz = zepto_parsing_remaining_bytes( &po1 ); + decr_sz = encr_sz; + zepto_parse_read_block( &po1, x_buff1_x2, encr_sz ); + zepto_convert_part_of_request_to_response( mem_h, &po, &po1 ); // restore the picture + zepto_write_block( mem_h2, x_buff1_x2, encr_sz ); + zepto_response_to_request( mem_h2 ); + + bool ipaad = SASP_IntraPacketAuthenticateAndDecrypt( dbg_nonce, mem_h2, dbg_stack, 512 ); + memcpy( checkedMsg, memory_objects[ mem_h2 ].ptr + memory_objects[ mem_h2 ].rq_size, memory_objects[ mem_h2 ].rsp_size ); + decr_sz = memory_objects[ mem_h2 ].rsp_size; + + PRINTF( "handlerSASP_send(): dbg_nonce: %02x %02x %02x %02x %02x %02x\n", dbg_nonce[0], dbg_nonce[1], dbg_nonce[2], dbg_nonce[3], dbg_nonce[4], dbg_nonce[5] ); + assert( ipaad ); + assert( decr_sz == inisz ); + assert( memcmp( nonce, dbg_nonce, 6 ) == 0 ); + checkedMsg[0] &= 0x7F; // get rid of SASP bit + for ( int k=0; k= 0 ); + +// SASP_EncryptAndAddAuthenticationData( mem_h, stack, stackSize, nonce ); + DEBUG_SASP_EncryptAndAddAuthenticationDataChecked( mem_h, stack, stackSize, nonce ); + + INCREMENT_COUNTER( 10, "handlerSASP_send()" ); + return SASP_RET_TO_LOWER_REGULAR; +} + uint8_t handlerSASP_send( const uint8_t* nonce, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ) { // SASP_NonceLS_increment( data + DATA_SASP_NONCE_LS_OFFSET ); @@ -686,6 +757,101 @@ uint8_t handlerSASP_send( const uint8_t* nonce, uint16_t* sizeInOut, const uint8 return SASP_RET_TO_LOWER_REGULAR; } +uint8_t handlerSASP_receive( uint8_t* pid, MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data ) +{ + INCREMENT_COUNTER( 11, "handlerSASP_receive()" ); + // 1. Perform intra-packet authentication + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + zepto_parse_read_block( &po, stack, SASP_NONCE_SIZE ); + bool for_sasp = SASP_NonceIsIntendedForSasp( stack ); + bool ipaad = SASP_IntraPacketAuthenticateAndDecrypt( pid, mem_h, stack + SASP_NONCE_SIZE, stackSize ); + + PRINTF( "handlerSASP_receive(): PID: %x%x%x%x%x%x\n", pid[0], pid[1], pid[2], pid[3], pid[4], pid[5] ); + if ( !ipaad ) + { + INCREMENT_COUNTER( 12, "handlerSASP_receive(), garbage in" ); + PRINTF( "handlerSASP_receive(): BAD PACKET RECEIVED\n" ); + return SASP_RET_IGNORE; + } + + // 2. Is it an Error Old Nonce Message + if ( for_sasp ) + { + INCREMENT_COUNTER( 13, "handlerSASP_receive(), intended for SASP" ); + uint8_t* nls = data+DATA_SASP_NONCE_LS_OFFSET; + // now we need to extract a proposed value of nonce from the output + // we do some strange manipulations: make output input, parse proper block, make input output + // TODO: think whether it is possible to do in more sane way... somehow as it was before + // uint8_t* new_nls = buffOut+2; + parser_obj po1, po2; + zepto_parser_init( &po1, mem_h ); + zepto_response_to_request( mem_h ); + zepto_parse_skip_block( &po1, 2 ); + zepto_parse_read_block( &po1, stack + SASP_NONCE_SIZE, SASP_NONCE_SIZE ); + zepto_parser_init( &po1, mem_h ); + zepto_parser_init( &po2, mem_h ); + uint16_t packet_size = zepto_parsing_remaining_bytes( &po2 ); + zepto_parse_skip_block( &po2, packet_size ); + zepto_convert_part_of_request_to_response( mem_h, &po1, &po2 ); // finally, put where taken... + + uint8_t* new_nls = stack + SASP_NONCE_SIZE; + + PRINTF( "handlerSASP_receive(): packet for SASP received...\n" ); + PRINTF( " current NLS: %02x %02x %02x %02x %02x %02x\n", nls[0], nls[1], nls[2], nls[3], nls[4], nls[5] ); + PRINTF( " proposed NLS: %02x %02x %02x %02x %02x %02x\n", new_nls[0], new_nls[1], new_nls[2], new_nls[3], new_nls[4], new_nls[5] ); + // Recommended value of NLS starts from the second byte of the message; we should update NLS, if the recommended value is greater + int8_t nonceCmp = SASP_NonceCompare( nls, new_nls ); // skipping First Byte and reserved byte + if ( nonceCmp < 0 ) + { + INCREMENT_COUNTER( 14, "handlerSASP_receive(), nonce las updated" ); + SASP_NonceClearForSaspFlag( new_nls ); + memcpy( nls, new_nls, SASP_NONCE_SIZE ); + SASP_NonceLS_increment( nls ); + // TODO: shuold we do anything else? + return SASP_RET_TO_HIGHER_LAST_SEND_FAILED; + } + return SASP_RET_IGNORE; + } + + // 3. Compare nonces... + uint8_t* nlw = data+DATA_SASP_NONCE_LW_OFFSET; + int8_t nonceCmp = SASP_NonceCompare( stack, nlw ); + if ( nonceCmp <= 0 ) // error message must be prepared + { + INCREMENT_COUNTER( 15, "handlerSASP_receive(), error old nonce" ); + PRINTF( "handlerSASP_receive(): old nonce; packet for SASP is being prepared...\n" ); + PRINTF( " nonce received: %02x %02x %02x %02x %02x %02x\n", stack[0], stack[1], stack[2], stack[3], stack[4], stack[5] ); + PRINTF( " current NLW: %02x %02x %02x %02x %02x %02x\n", nlw[0], nlw[1], nlw[2], nlw[3], nlw[4], nlw[5] ); + + // we are to create a brand new output + // TODO: think about faster and more straightforward approaches +/* uint8_t* ins_ptr = stack; + *(ins_ptr++) = 0; + *(ins_ptr++) = 0; // Byte + memcpy( ins_ptr, nlw, SASP_NONCE_SIZE ); + ins_ptr += SASP_NONCE_SIZE; + *sizeInOut = SASP_NONCE_SIZE+2;*/ + zepto_write_uint8( mem_h, 0 ); // First Byte + zepto_write_uint8( mem_h, 0 ); // Reserved Byte + zepto_write_block( mem_h, nlw, SASP_NONCE_SIZE ); + zepto_response_to_request( mem_h ); + + memcpy( stack, pid, SASP_NONCE_SIZE ); + SASP_NonceSetIntendedForSaspFlag( stack ); + SASP_EncryptAndAddAuthenticationData( mem_h, stack + SASP_NONCE_SIZE, stackSize - SASP_NONCE_SIZE, stack ); + return SASP_RET_TO_LOWER_ERROR; + } + + // 4. Finally, we have a brand new packet + memcpy( data+DATA_SASP_NONCE_LW_OFFSET, stack, SASP_NONCE_SIZE ); // update Nonce Low Watermark +// memcpy( data+DATA_SASP_LRPS_OFFSET, buffIn+SASP_HEADER_SIZE, SASP_TAG_SIZE ); // save packet signature + pid[ SASP_NONCE_SIZE - 1 ] &= (uint8_t)(0x7F); + INCREMENT_COUNTER( 16, "handlerSASP_receive(), passed up" ); + return SASP_RET_TO_HIGHER_NEW; +} + uint8_t handlerSASP_receive( uint8_t* pid, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ) { INCREMENT_COUNTER( 11, "handlerSASP_receive()" ); @@ -758,13 +924,13 @@ uint8_t handlerSASP_receive( uint8_t* pid, uint16_t* sizeInOut, const uint8_t* b // 4. Finally, we have a brand new packet memcpy( data+DATA_SASP_NONCE_LW_OFFSET, buffIn, SASP_NONCE_SIZE ); // update Nonce Low Watermark - memcpy( data+DATA_SASP_LRPS_OFFSET, buffIn+SASP_HEADER_SIZE, SASP_TAG_SIZE ); // save packet signature +// memcpy( data+DATA_SASP_LRPS_OFFSET, buffIn+SASP_HEADER_SIZE, SASP_TAG_SIZE ); // save packet signature pid[ SASP_NONCE_SIZE - 1 ] &= (uint8_t)(0x7F); INCREMENT_COUNTER( 16, "handlerSASP_receive(), passed up" ); return SASP_RET_TO_HIGHER_NEW; } -uint8_t handlerSASP_get_nonce( uint16_t* sizeInOut, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ) +uint8_t handlerSASP_get_nonce( uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ) { SASP_NonceLS_increment( data + DATA_SASP_NONCE_LS_OFFSET ); memcpy( buffOut, data + DATA_SASP_NONCE_LS_OFFSET, SASP_NONCE_SIZE ); diff --git a/tests/emulator/sasp_protocol.h b/tests/emulator/sasp_protocol.h index d1975e4..1daca6c 100644 --- a/tests/emulator/sasp_protocol.h +++ b/tests/emulator/sasp_protocol.h @@ -20,6 +20,7 @@ Copyright (C) 2015 OLogN Technologies AG #include "sa-common.h" #include "sa-eeprom.h" +#include "zepto-mem-mngmt.h" // RET codes #define SASP_RET_IGNORE 0 // not authenticated, etc @@ -47,6 +48,10 @@ Copyright (C) 2015 OLogN Technologies AG // handlers uint8_t handlerSASP_send( const uint8_t* nonce, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ); uint8_t handlerSASP_receive( uint8_t* pid, uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ); -uint8_t handlerSASP_get_nonce( uint16_t* sizeInOut, uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ); +uint8_t handlerSASP_get_nonce( uint8_t* buffOut, int buffOutSize, uint8_t* stack, int stackSize, uint8_t* data ); + +uint8_t handlerSASP_receive( uint8_t* pid, MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data ); +uint8_t handlerSASP_send( const uint8_t* nonce, MEMORY_HANDLE mem_h, uint8_t* stack, int stackSize, uint8_t* data ); + #endif // __SASP_PROTOCOL_H__ \ No newline at end of file diff --git a/tests/emulator/test-generator.cpp b/tests/emulator/test-generator.cpp index 9c11636..b4697f4 100644 --- a/tests/emulator/test-generator.cpp +++ b/tests/emulator/test-generator.cpp @@ -30,6 +30,106 @@ uint8_t packetOnHold[ PACKET_MAX_SIZE ]; bool isPacketOnHold = false; bool holdRequested = false; +void registerIncomingPacket( REQUEST_REPLY_HANDLE mem_h ) +{ + return; + uint8_t buff[ PACKET_MAX_SIZE ]; + uint16_t packet_size; + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + packet_size = zepto_parsing_remaining_bytes( &po ); + assert( packet_size <= PACKET_MAX_SIZE ); + zepto_parse_read_block( &po, buff, packet_size ); + registerIncomingPacket( buff, packet_size ); +} + +void registerOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ) +{ + return; + uint8_t buff[ PACKET_MAX_SIZE ]; + uint16_t packet_size; + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + packet_size = zepto_parsing_remaining_bytes( &po ); + assert( packet_size <= PACKET_MAX_SIZE ); + zepto_parse_read_block( &po, buff, packet_size ); + registerOutgoingPacket( buff, packet_size ); +} + +bool shouldInsertIncomingPacket( REQUEST_REPLY_HANDLE mem_h ) +{ + return false; + uint8_t buff[ PACKET_MAX_SIZE ]; + uint16_t packet_size; + bool ret = shouldInsertIncomingPacket( buff, &packet_size ); + if ( !ret ) return false; + zepto_write_block( mem_h, buff, packet_size ); + return true; +} + +bool shouldInsertOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ) +{ + return false; + uint8_t buff[ PACKET_MAX_SIZE ]; + uint16_t packet_size; + bool ret = shouldInsertOutgoingPacket( buff, &packet_size ); + if ( !ret ) return false; + zepto_write_block( mem_h, buff, packet_size ); + return true; +} + + + +bool holdOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ) +{ + return false; + uint8_t buff[ PACKET_MAX_SIZE ]; + uint16_t packet_size; + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + packet_size = zepto_parsing_remaining_bytes( &po ); + assert( packet_size <= PACKET_MAX_SIZE ); + zepto_parse_read_block( &po, buff, packet_size ); + return holdOutgoingPacket( buff, &packet_size ); +} + +bool releaseOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ) +{ + return false; + uint8_t buff[ PACKET_MAX_SIZE ]; + uint16_t packet_size; + bool ret = releaseOutgoingPacket( buff, &packet_size ); + if ( !ret ) return false; + zepto_write_block( mem_h, buff, packet_size ); + return true; +} + +bool holdPacketOnRequest( REQUEST_REPLY_HANDLE mem_h ) +{ + return false; + uint8_t buff[ PACKET_MAX_SIZE ]; + uint16_t packet_size; + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + packet_size = zepto_parsing_remaining_bytes( &po ); + assert( packet_size <= PACKET_MAX_SIZE ); + zepto_parse_read_block( &po, buff, packet_size ); + return holdOutgoingPacket( buff, &packet_size ); +} + + + + + + + + + + bool holdOutgoingPacket( const uint8_t* packet, const uint16_t* size ) { if ( isPacketOnHold ) @@ -42,6 +142,7 @@ bool holdOutgoingPacket( const uint8_t* packet, const uint16_t* size ) bool isOutgoingPacketOnHold() { + return false; return isPacketOnHold; } @@ -100,21 +201,21 @@ void registerOutgoingPacket( const uint8_t* packet, uint16_t size ) bool shouldDropIncomingPacket() { -// return false; - return get_rand_val() % 8 == 0; // rate selection + return false; +// return get_rand_val() % 8 == 0; // rate selection } bool shouldDropOutgoingPacket() { -// return false; - return get_rand_val() % 8 == 0; // rate selection + return false; +// return get_rand_val() % 8 == 0; // rate selection } bool shouldInsertIncomingPacket( uint8_t* packet, uint16_t* size ) { - if ( get_rand_val() % 8 != 0 ) // rate selection +// if ( get_rand_val() % 8 != 0 ) // rate selection return false; // select one of saved incoming packets @@ -126,7 +227,7 @@ bool shouldInsertIncomingPacket( uint8_t* packet, uint16_t* size ) bool shouldInsertOutgoingPacket( uint8_t* packet, uint16_t* size ) { - if ( get_rand_val() % 8 != 0 ) // rate selection +// if ( get_rand_val() % 8 != 0 ) // rate selection return false; // select one of saved incoming packets diff --git a/tests/emulator/test-generator.h b/tests/emulator/test-generator.h index d617ddd..82ae041 100644 --- a/tests/emulator/test-generator.h +++ b/tests/emulator/test-generator.h @@ -32,6 +32,12 @@ bool shouldDropIncomingPacket(); bool shouldDropOutgoingPacket(); bool shouldInsertIncomingPacket( uint8_t* packet, uint16_t* size ); bool shouldInsertOutgoingPacket( uint8_t* packet, uint16_t* size ); + +void registerIncomingPacket( REQUEST_REPLY_HANDLE mem_h ); +void registerOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ); +bool shouldInsertIncomingPacket( REQUEST_REPLY_HANDLE mem_h ); +bool shouldInsertOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ); + /* void insertIncomingPacket(); void insertOutgoingPacket(); @@ -43,6 +49,10 @@ bool releaseOutgoingPacket( uint8_t* packet, uint16_t* size ); void requestHoldingPacket(); bool holdPacketOnRequest( const uint8_t* packet, const uint16_t* size ); +bool holdOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ); +bool releaseOutgoingPacket( REQUEST_REPLY_HANDLE mem_h ); +bool holdPacketOnRequest( REQUEST_REPLY_HANDLE mem_h ); + // sync hooks /*void requestSyncExec(); void allowSyncExec(); diff --git a/tests/emulator/yoctovm_protocol.cpp b/tests/emulator/yoctovm_protocol.cpp index 7b21d08..aec773d 100644 --- a/tests/emulator/yoctovm_protocol.cpp +++ b/tests/emulator/yoctovm_protocol.cpp @@ -41,6 +41,112 @@ uint16_t currChainIdBase[2] = { 0, ( MASTER_SLAVE_BIT << 15 ) }; +uint8_t slave_process( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) +{ + // buffer assumes to contain an input message (including First Byte) + // FAKE structure of the buffer: First_Byte | fake_body (9-16 bytes) + // fake body is used for external controlling of data integrity and is organized as follows: + // chain_id (2 bytes) | chain_ini_size (2 bytes) | replying_to (2 bytes) | self_id == packet_ordinal (2 bytes) | rand_part (1-8 bytes) + // self_id is a randomly generated 2-byte integer (for simplicity we assume that in the test environment comm peers have the same endiannes) + // replying_to is a copy of self_id of the received packet (or 0 for the first packet ion the chain) + // rand_part is filled with some number of '-' ended by '>' + + INCREMENT_COUNTER( 0, "slave_process(), packet received" ); + + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + + uint16_t msg_size = zepto_parsing_remaining_bytes( &po ); // all these bytes + (potentially) {padding_size + padding} will be written + uint8_t first_byte = zepto_parse_uint8( &po ); + if ( ( first_byte & ( SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING ) ) == SAGDP_P_STATUS_ERROR_MSG ) + { + PRINTF( "slave_process(): ERROR MESSAGE RECEIVED IN YOCTO\n" ); + assert(0); + } + + uint16_t chain_id[2]; + chain_id[0] = zepto_parse_encoded_uint16( &po ); + chain_id[1] = zepto_parse_encoded_uint16( &po ); + uint16_t chain_ini_size = zepto_parse_encoded_uint16( &po ); + uint16_t reply_to_id = zepto_parse_encoded_uint16( &po ); + uint16_t self_id = zepto_parse_encoded_uint16( &po ); + char tail[256]; + uint16_t tail_sz = zepto_parsing_remaining_bytes( &po ); + zepto_parse_read_block( &po, (uint8_t*)tail, tail_sz ); + tail[ tail_sz ] = 0; + + // print packet + PRINTF( "Yocto: Packet received: [%d bytes] [%d][0x%04x][0x%04x][0x%04x][0x%04x][0x%04x]%s\n", msg_size, first_byte, chain_id[0], chain_id[1], chain_ini_size, reply_to_id, self_id, tail ); + + // test and analyze + + // size + if ( !( msg_size >= 7 && msg_size <= 22 ) ) + printf( "ZEPTO: BAD PACKET RECEIVED\n", msg_size ); + assert( msg_size >= 7 && msg_size <= 22 ); + + // flags + assert( (first_byte & 4 ) == 0 ); + first_byte &= SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING; // to use only respective bits + if ( first_byte == SAGDP_P_STATUS_FIRST ) + { + assert( 0 == reply_to_id ); + assert( chain_id[0] != currChainID[0] || chain_id[1] != currChainID[1] ); + currChainID[0] = chain_id[0]; + currChainID[1] = chain_id[1]; + } + else + { + assert( last_sent_id == reply_to_id ); + assert( chain_id[0] == currChainID[0] && chain_id[1] == currChainID[1] ); + } + + if ( first_byte == SAGDP_P_STATUS_TERMINATING ) + { +// chainContinued = false; + currChainIdBase[0] ++; + return YOCTOVM_OK; + } + + // fake implementation: should this packet be terminal? + if ( chain_ini_size == self_id + 1 ) + first_byte = SAGDP_P_STATUS_TERMINATING; + else + first_byte = SAGDP_P_STATUS_INTERMEDIATE; + + // prepare outgoing packet + reply_to_id = self_id; + self_id++; + last_sent_id = self_id; + + zepto_write_uint8( mem_h, first_byte ); + zepto_write_encoded_uint16( mem_h, chain_id[0] ); + zepto_write_encoded_uint16( mem_h, chain_id[1] ); + zepto_write_encoded_uint16( mem_h, chain_ini_size ); + zepto_write_encoded_uint16( mem_h, reply_to_id ); + zepto_write_encoded_uint16( mem_h, self_id ); + + uint16_t varln = 6 - self_id % 7; // 0:6 + for ( uint8_t i=0;i= 7 && msg_size <= 22 ); + + INCREMENT_COUNTER( 1, "slave_process(), packet sent" ); + + // return status +// chainContinued = true; + return YOCTOVM_PASS_LOWER; +} + uint8_t slave_process( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) { // buffer assumes to contain an input message (including First Byte) @@ -141,12 +247,83 @@ uint8_t slave_process( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buff } +uint8_t master_error( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) +{ + INCREMENT_COUNTER( 2, "master_error()" ); + + return master_start( mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); +} + +#if 0 uint8_t master_error( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) { INCREMENT_COUNTER( 2, "master_error()" ); return master_start( sizeInOut, buffIn, buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); } +#endif // 0 +uint8_t master_start( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) +{ + // Forms a first packet in the chain + // for structure of the packet see comments to yocto_process() + // Initial number of packets in the chain is currently entered manually by a tester + PRINT_COUNTERS(); + + uint8_t first_byte = SAGDP_P_STATUS_FIRST; + +#ifdef MANUAL_TEST_DATA_ENTERING + + uint8_t chain_ini_size = 0; + while ( chain_ini_size == 0 || chain_ini_size == '\n' ) + chain_ini_size = getchar(); + if ( chain_ini_size == 'x' ) + return YOCTOVM_FAILED; + chain_ini_size -= '0'; + chainContinued = true; + +#else // MANUAL_TEST_DATA_ENTERING + + uint16_t chain_ini_size = get_rand_val() % ( CHAIN_MAX_SIZE - 2 ) + 2; + +#endif // MANUAL_TEST_DATA_ENTERING + + currChainID[0] = ++( currChainIdBase[0] ); + currChainID[1] = currChainIdBase[1]; + uint16_t chain_id[2]; + chain_id[0] = currChainID[0]; + chain_id[1] = currChainID[1]; + uint16_t reply_to_id = 0; + uint16_t self_id = 1; + last_sent_id = self_id; + + // prepare outgoing packet + zepto_write_uint8( mem_h, first_byte ); + zepto_write_encoded_uint16( mem_h, chain_id[0] ); + zepto_write_encoded_uint16( mem_h, chain_id[1] ); + zepto_write_encoded_uint16( mem_h, chain_ini_size ); + zepto_write_encoded_uint16( mem_h, reply_to_id ); + zepto_write_encoded_uint16( mem_h, self_id ); + + char tail[256]; + uint16_t varln = 6 - self_id % 7; // 0:6 + for ( uint8_t i=0;i= 7 && msg_size <= 22 ); + + INCREMENT_COUNTER( 3, "master_start(), chain started" ); + + + // return status + return YOCTOVM_PASS_LOWER; +} uint8_t master_start( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) { @@ -210,6 +387,112 @@ uint8_t master_start( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffO return YOCTOVM_PASS_LOWER; } +uint8_t master_continue( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) +{ + // by now master_continue() does the same as yocto_process + + INCREMENT_COUNTER( 4, "master_continue(), packet received" ); + + // init parser object + parser_obj po; + zepto_parser_init( &po, mem_h ); + + uint16_t msg_size = zepto_parsing_remaining_bytes( &po ); // all these bytes + (potentially) {padding_size + padding} will be written + uint8_t first_byte = zepto_parse_uint8( &po ); + if ( ( first_byte & ( SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING ) ) == SAGDP_P_STATUS_ERROR_MSG ) + { + PRINTF( "master_continue(): ERROR MESSAGE RECEIVED IN YOCTO\n" ); + (currChainIdBase[0]) ++; + INCREMENT_COUNTER( 9, "master_continue(), error message received" ); + return YOCTOVM_OK; + assert(0); + } + + uint16_t chain_id[2]; + chain_id[0] = zepto_parse_encoded_uint16( &po ); + chain_id[1] = zepto_parse_encoded_uint16( &po ); + uint16_t chain_ini_size = zepto_parse_encoded_uint16( &po ); + uint16_t reply_to_id = zepto_parse_encoded_uint16( &po ); + uint16_t self_id = zepto_parse_encoded_uint16( &po ); + char tail[256]; + uint16_t tail_sz = zepto_parsing_remaining_bytes( &po ); + zepto_parse_read_block( &po, (uint8_t*)tail, tail_sz ); + tail[ tail_sz ] = 0; + + // print packet + PRINTF( "Yocto: Packet received: [%d bytes] [%d][0x%04x][0x%04x][0x%04x][0x%04x][0x%04x]%s\n", msg_size, first_byte, chain_id[0], chain_id[1], chain_ini_size, reply_to_id, self_id, tail ); + + // test and analyze + + // size + if ( !( msg_size >= 7 && msg_size <= 22 ) ) + printf( "ZEPTO: BAD PACKET RECEIVED\n", msg_size ); + assert( msg_size >= 7 && msg_size <= 22 ); + + // flags + assert( (first_byte & 4 ) == 0 ); + first_byte &= SAGDP_P_STATUS_FIRST | SAGDP_P_STATUS_TERMINATING; // to use only respective bits + + if ( first_byte == SAGDP_P_STATUS_FIRST ) + { + assert( 0 == reply_to_id ); + assert( chain_id[0] != currChainID[0] || chain_id[1] != currChainID[1] ); + currChainID[0] = chain_id[0]; + currChainID[1] = chain_id[1]; + } + else + { + assert( last_sent_id == reply_to_id ); + assert( chain_id[0] == currChainID[0] && chain_id[1] == currChainID[1] ); + } + + + if ( first_byte == SAGDP_P_STATUS_TERMINATING ) + { +// chainContinued = false; + (currChainIdBase[0]) ++; + INCREMENT_COUNTER( 6, "master_continue(), packet terminating received" ); + return YOCTOVM_OK; + } + + // fake implementation: should this packet be terminal? + if ( chain_ini_size == self_id + 1 ) + first_byte = SAGDP_P_STATUS_TERMINATING; + else + first_byte = SAGDP_P_STATUS_INTERMEDIATE; + + // prepare outgoing packet + reply_to_id = self_id; + self_id++; + last_sent_id = self_id; + + zepto_write_uint8( mem_h, first_byte ); + zepto_write_encoded_uint16( mem_h, chain_id[0] ); + zepto_write_encoded_uint16( mem_h, chain_id[1] ); + zepto_write_encoded_uint16( mem_h, chain_ini_size ); + zepto_write_encoded_uint16( mem_h, reply_to_id ); + zepto_write_encoded_uint16( mem_h, self_id ); + + uint16_t varln = 6 - self_id % 7; // 0:6 + for ( uint8_t i=0;i= 7 && msg_size <= 22 ); + + // return status + INCREMENT_COUNTER( 7, "master_continue(), packet sent" ); + INCREMENT_COUNTER_IF( 8, "master_continue(), last packet sent", (first_byte >> 1) ); + + return first_byte == SAGDP_P_STATUS_TERMINATING ? YOCTOVM_PASS_LOWER_THEN_IDLE : YOCTOVM_PASS_LOWER; +} + uint8_t master_continue( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ) { // by now master_continue() does the same as yocto_process diff --git a/tests/emulator/yoctovm_protocol.h b/tests/emulator/yoctovm_protocol.h index a7d34da..9e40572 100644 --- a/tests/emulator/yoctovm_protocol.h +++ b/tests/emulator/yoctovm_protocol.h @@ -28,6 +28,7 @@ Copyright (C) 2015 OLogN Technologies AG #include "sa-common.h" #include "sa-eeprom.h" +#include "zepto-mem-mngmt.h" // RET codes #define YOCTOVM_FAILED 0 // sunject for system reset @@ -36,10 +37,17 @@ Copyright (C) 2015 OLogN Technologies AG #define YOCTOVM_PASS_LOWER_THEN_IDLE 3 // ret code for testing; same as YOCTOVM_PASS_LOWER but notifies main loop that the chain is over //#define YOCTOVM_RESET_STACK 4 +#if 0 uint8_t slave_process( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); uint8_t master_start( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); uint8_t master_continue( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); uint8_t master_error( uint16_t* sizeInOut, const uint8_t* buffIn, uint8_t* buffOut/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); +#endif // 0 + +uint8_t slave_process( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); +uint8_t master_start( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); +uint8_t master_continue( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); +uint8_t master_error( REQUEST_REPLY_HANDLE mem_h/*, int buffOutSize, uint8_t* stack, int stackSize*/ ); // Pure Testing Block bool isChainContinued(); diff --git a/tests/emulator/zepto-mem-mngmt.cpp b/tests/emulator/zepto-mem-mngmt.cpp index 89840a8..76b9214 100644 --- a/tests/emulator/zepto-mem-mngmt.cpp +++ b/tests/emulator/zepto-mem-mngmt.cpp @@ -45,8 +45,15 @@ uint8_t* memory_object_get_response_ptr( REQUEST_REPLY_HANDLE mem_h ) return memory_objects[ mem_h ].ptr + memory_objects[ mem_h ].rq_size; } +uint16_t memory_object_get_response_size( REQUEST_REPLY_HANDLE mem_h ) +{ + assert( mem_h < REQUEST_REPLY_OBJ_MAX_CNT ); + return memory_objects[ mem_h ].rsp_size; +} + uint8_t* memory_object_append( REQUEST_REPLY_HANDLE mem_h, uint16_t size ) { +printf( "memory_object_append( %d )\n", size ); // TODO: reallocation logic uint8_t* ret = memory_objects[ mem_h ].ptr + memory_objects[ mem_h ].rq_size + memory_objects[ mem_h ].rsp_size; memory_objects[ mem_h ].rsp_size += size; @@ -123,6 +130,7 @@ uint8_t zepto_parse_uint8( parser_obj* po ) uint16_t zepto_parse_encoded_uint16( parser_obj* po ) { +printf( "zepto_write_encoded_uint16(): ini offset = %d...", po->offset ); assert( po->mem_handle != MEMORY_HANDLE_INVALID ); uint8_t* buff = memory_object_get_request_ptr( po->mem_handle ) + po->offset; assert( buff != NULL ); @@ -131,22 +139,26 @@ uint16_t zepto_parse_encoded_uint16( parser_obj* po ) { ret = (uint16_t)(buff[ 0 ]); (po->offset)++; - return ret; } else if ( ( buff[ 1 ] & 128 ) == 0 ) { - ret = 128 + ( (uint16_t)(buff[0] & 0x7F) | ( (uint16_t)(buff[1]) << 7) ); + ret = 128 + ( (uint16_t)(buff[0] & 0x7F) | ( ((uint16_t)(buff[1])) << 7) ); po->offset += 2; - return ret; } +/* else if (buff[0] == 0x80 && buff[1] == 0xff && buff[2] == 1 ) + { + ret = 0x8000; + po->offset += 3; + }*/ else { assert( (buff[2] & 0x80) == 0 ); assert( buff[2] < 4 ); // to stay within uint16_max - ret = 16512 + ( (uint16_t)(buff[0] & 0x7F) | ( (uint16_t)(buff[1]) << 7) ) | ( (uint16_t)(buff[2]) << 14); + ret = 16512 + ( (uint16_t)(buff[0] & 0x7F) | ( (uint16_t)(buff[1]) << 7) ) | ( ((uint16_t)(buff[2])) << 14); po->offset += 3; - return ret; } + printf( "new offset = %d, num = %x\n", po->offset, ret ); + return ret; } bool zepto_parse_read_block( parser_obj* po, uint8_t* block, uint16_t size ) @@ -195,6 +207,11 @@ void zepto_write_uint8( REQUEST_REPLY_HANDLE mem_h, uint8_t val ) void zepto_write_encoded_uint16( REQUEST_REPLY_HANDLE mem_h, uint16_t num ) { +printf( "zepto_write_encoded_uint16( %x )\n", num ); +if ( num == 0x8000 ) +{ + num = 0x8000; +} assert( mem_h != MEMORY_HANDLE_INVALID ); uint8_t* buff; if ( num < 128 ) @@ -215,8 +232,9 @@ void zepto_write_encoded_uint16( REQUEST_REPLY_HANDLE mem_h, uint16_t num ) buff = memory_object_append( mem_h, 3 ); assert( buff != NULL ); buff[0] = ((uint8_t)num & 0x7F) + 128; - buff[1] = ( (num - 128) >> 7 ) + 128; - buff[2] = (num - 128) >> 14; + buff[1] = (( (num - 128) >> 7 ) & 0x7F ) + 128; + buff[2] = (num - 16512) >> 14; + printf( "0x%x = %x, %x, %x\n", num, buff[0], buff[1], buff[2] ); } } @@ -268,4 +286,18 @@ void zepto_write_prepend_block( MEMORY_HANDLE mem_h, const uint8_t* block, uint1 { assert( mem_h != MEMORY_HANDLE_INVALID ); memory_object_prepend( mem_h, block, size ); -} \ No newline at end of file +} + + + +// inspired by SAGDP: creating a copy of the packet +uint16_t zepto_writer_get_response_size( MEMORY_HANDLE mem_h ) +{ + return memory_object_get_response_size( mem_h ); +} + +void zepto_writer_get_copy_of_response( MEMORY_HANDLE mem_h, uint8_t* buff ) +{ + memcpy( buff, memory_object_get_response_ptr( mem_h ), memory_object_get_response_size( mem_h ) ); +} + diff --git a/tests/emulator/zepto-mem-mngmt.h b/tests/emulator/zepto-mem-mngmt.h index b0cafb3..f23a1d1 100644 --- a/tests/emulator/zepto-mem-mngmt.h +++ b/tests/emulator/zepto-mem-mngmt.h @@ -39,6 +39,18 @@ struct request_reply_mem_obj }; extern request_reply_mem_obj memory_objects[ 128 ]; +inline +uint16_t ugly_hook_get_request_size( REQUEST_REPLY_HANDLE mem_h ) +{ + return memory_objects[ mem_h ].rq_size; +} +inline +uint16_t ugly_hook_get_response_size( REQUEST_REPLY_HANDLE mem_h ) +{ + return memory_objects[ mem_h ].rsp_size; +} + +// end of UGLY HOOK FOR BY-PARTS (INITIAL PHASE OF) DEVELOPMENT // parsing functions @@ -63,4 +75,8 @@ void zepto_convert_part_of_request_to_response( MEMORY_HANDLE mem_h, parser_obj* void zepto_write_prepend_byte( MEMORY_HANDLE mem_h, uint8_t bt ); void zepto_write_prepend_block( MEMORY_HANDLE mem_h, const uint8_t* block, uint16_t size ); +// inspired by SAGDP: creating a copy of the packet +uint16_t zepto_writer_get_response_size( MEMORY_HANDLE mem_h ); +void zepto_writer_get_copy_of_response( MEMORY_HANDLE mem_h, uint8_t* buff ); + #endif // __ZEPTO_MEM_MNGMT_H__ \ No newline at end of file