Diff: SNIC/SNIC_UartMsgUtil.cpp

Revision:

12:0254eaccfda2

Child:

14:54378c96d285

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SNIC/SNIC_UartMsgUtil.cpp	Tue Mar 25 01:42:25 2014 +0000
@@ -0,0 +1,129 @@
+#include "SNIC_UartMsgUtil.h"
+
+using namespace murata_wifi;
+
+C_SNIC_UartMsgUtil::C_SNIC_UartMsgUtil()
+{
+}
+
+unsigned short C_SNIC_UartMsgUtil::makePayload( unsigned int cmd_len, unsigned char *cmd_p, unsigned char *payload_p )
+{
+    unsigned short payload_len = 0;
+    int i;
+    
+    for( i = 0; i < cmd_len; i++, payload_p++, payload_len++ )
+    {
+        /* check Escape code */
+        if( ( cmd_p[i] == UART_CMD_SOM ) || ( cmd_p[i] == UART_CMD_EOM ) || ( cmd_p[i] == UART_CMD_ESC ) )
+        {
+            /* Add ESC */
+            *payload_p = UART_CMD_ESC;
+            payload_len++;
+            
+            payload_p++;
+            *payload_p = (0x80 | cmd_p[i]);
+        }
+        else
+        {
+            *payload_p = cmd_p[i];
+        }
+    }
+    
+    return payload_len;
+}
+
+unsigned int C_SNIC_UartMsgUtil::makeRequest( unsigned char cmd_id,unsigned char *payload_p
+                                , unsigned short payload_len, unsigned char *uart_command_p )
+{
+    unsigned char check_sum = 0;    // Check Sum
+    unsigned int  uart_cmd_len = 0;
+    int i;
+    
+    // set SOM
+    *uart_command_p = UART_CMD_SOM;
+    uart_command_p++;
+    uart_cmd_len++;
+    
+    // set payload length L0
+    *uart_command_p = (0x80 | (payload_len & 0x007f));
+    check_sum += *uart_command_p;
+    uart_command_p++;
+    uart_cmd_len++;
+
+    // set payload length L1
+    *uart_command_p = (0x80 | ( (payload_len >> 7) & 0x003f));
+    check_sum += *uart_command_p;
+    uart_command_p++;
+    uart_cmd_len++;
+    
+    // set Command ID
+    *uart_command_p = (0x80 | cmd_id);
+    check_sum += *uart_command_p;
+    uart_command_p++;
+    uart_cmd_len++;
+
+    // set Payload
+    for( i = 0; i < payload_len; i++, uart_command_p++, uart_cmd_len++ )
+    {
+        *uart_command_p = payload_p[i];
+        check_sum += *uart_command_p;
+    }
+
+    // set Check sum
+    *uart_command_p = (0x80 | check_sum);
+    uart_command_p++;
+    uart_cmd_len++;
+    
+    // set EOM
+    *uart_command_p = UART_CMD_EOM;
+    uart_cmd_len++;
+    
+    return uart_cmd_len;
+}
+
+unsigned int C_SNIC_UartMsgUtil::getResponsePayload( unsigned int recvdata_len, unsigned char *recvdata_p
+                                            , unsigned char *command_id_p,  unsigned char *payload_p )
+{
+    unsigned short payload_len  = 0;
+    unsigned int   response_len = 0;
+    unsigned char *buf = NULL;
+    bool isESC = false;
+    int i;
+    
+    // get payload length
+    payload_len = ( ( (recvdata_p[1] & ~0x80) & 0xff) | ( ( (recvdata_p[2] & ~0xC0) << 7) & 0xff80) );
+
+    // get Command ID
+    *command_id_p = (recvdata_p[3] & ~0x80);
+
+    buf = &recvdata_p[4];
+
+    // get payload data
+    for( i = 0; i < payload_len; i++, buf++ )
+    {
+        if( isESC )
+        {
+            *payload_p = (*buf & ~0x80);
+            payload_p++;
+            response_len++;
+            isESC = false;
+        }
+        else
+        {
+            // Check Escape code
+            if( *buf == UART_CMD_ESC )
+            {
+                isESC = true;
+                continue;
+            }
+            else
+            {
+                *payload_p = *buf;
+                payload_p++;
+                response_len++;
+            }
+        }
+    }
+
+    return response_len;
+}
\ No newline at end of file