linux_serial_minimal driver - Support for multiple sporadic interfaces

src/drivers/linux_serial_minimal.c

@@ -195,12 +195,17 @@ static uint32_t encode_packet(uint8_t* data, const uint32_t data_length, const u
         return 0;
     }
 
-    // first four bytes of msg_content - port number
-    uint8_t* data_ptr = (uint8_t*) (msg_content + 4); // skip port number to access data
-    // last four bytes of msg_content - actual msg length
+    // structure of the msg_content:
+    // - first four bytes of msg_content - port number
+    // - next - actual data
+    // - double word before dummy bytes  - length of data
+    // - last double word - dummy bytes
+
     uint32_t* length_ptr = (uint32_t*)(msg_content + (msg_length - 4));
 
-    if(data_length < *length_ptr * 2 + 2)
+    uint32_t bytes_to_copy = (*length_ptr) + sizeof(uint32_t);
+
+    if(data_length < (*length_ptr + sizeof(uint32_t)) * 2 + 2)
     {
         __PO_HI_DEBUG_CRITICAL ("Encoding packet - not enough data_length, at least %d bytes is required, got %d\n", *length_ptr, data_length);
         return 0;
@@ -211,9 +216,9 @@ static uint32_t encode_packet(uint8_t* data, const uint32_t data_length, const u
     data[length++] = PACKET_START_BYTE;
 
     uint32_t iter;
-    for(iter = 0; iter < *length_ptr; ++iter)
+    for(iter = 0; iter < bytes_to_copy; ++iter)
     {
-        uint8_t msg_byte = data_ptr[iter];
+        uint8_t msg_byte = msg_content[iter];
 
         if(msg_byte == PACKET_START_BYTE || msg_byte == PACKET_STOP_BYTE || msg_byte == PACKET_ESCAPE_BYTE)
         {
@@ -362,13 +367,11 @@ static enum PACKET_DECODER_STATE packet_decoder_state;
 
 static void process_packet(uint8_t* data, int size)
 {
-    uint32_t port = 7;
     uint32_t length = size;
 
     __PO_HI_DEBUG_DEBUG ("[LINUX SERIAL] Processing packet\n");
     __po_hi_msg_reallocate(&__po_hi_c_driver_serial_linux_poller_msg);
-    memcpy(__po_hi_c_driver_serial_linux_poller_msg.content, &port, 4);
-    memcpy(__po_hi_c_driver_serial_linux_poller_msg.content + 4, data, size);
+    memcpy(__po_hi_c_driver_serial_linux_poller_msg.content, data, size);
     memcpy(__po_hi_c_driver_serial_linux_poller_msg.content + (__PO_HI_MESSAGES_MAX_SIZE - 8), &length, 4);
     __po_hi_c_driver_serial_linux_poller_msg.length = __PO_HI_MESSAGES_MAX_SIZE;