Adding CRC 16 support

pstop_c/examples/client/client_app.c

@@ -13,6 +13,7 @@
 #include "pstop/device_id.h"
 #include "pstop/os.h"
 #include "pstop/protocol_data.h"
+#include "pstop/checksum.h"
 
 udp_transport_data_t udp_transport;
 
@@ -49,6 +50,15 @@ read_msg(udp_transport_data_t *transport, pstop_os_env *env, pstop_msg_t *resp,
     return 0;
 }
 
+void
+dump_bytes(const uint8_t *b)
+{
+    for(int i = 0U; i < PSTOP_MESSAGE_SIZE; ++i) {
+        fprintf(stderr, "%X ", (int)b[i]);
+    }
+    fprintf(stderr, "\n");
+}
+
 int
 send_msg(udp_transport_data_t *transport, pstop_os_env *env, protocol_data_t *machine, const device_id_t *uuid, uint8_t msg)
 {
@@ -57,6 +67,9 @@ send_msg(udp_transport_data_t *transport, pstop_os_env *env, protocol_data_t *ma
     pstop_msg_t req_msg;
     pstop_msg_t resp_msg;
 
+    pstop_message_init(&req_msg);
+    pstop_message_init(&resp_msg);
+
     uint64_t now = env->get_time_cb();
 
     req_msg.message = msg;
@@ -66,9 +79,11 @@ send_msg(udp_transport_data_t *transport, pstop_os_env *env, protocol_data_t *ma
     req_msg.received_counter = machine->last_counter;
     req_msg.received_stamp = machine->last_timestamp;
     req_msg.stamp = now;
+    req_msg.checksum = 0x00U;
 
     machine->msg_counter++;
     pstop_message_encode(&req_msg, reqbytes);
+    //dump_bytes(reqbytes);
 
     transport_udp_write(transport, reqbytes, PSTOP_MESSAGE_SIZE, NULL);
 

pstop_c/examples/machine/machine_app.c

@@ -43,6 +43,15 @@ robot_status(pstop_status_message_t status)
     return 0;
 }
 
+void
+dump_bytes(const uint8_t *b)
+{
+    for(int i = 0U; i < PSTOP_MESSAGE_SIZE; ++i) {
+        fprintf(stderr, "%X ", (int)b[i]);
+    }
+    fprintf(stderr, "\n");
+}
+
 int
 main(int argc, char *argv[])
 {
@@ -86,6 +95,7 @@ main(int argc, char *argv[])
         result = transport_udp_read(&udp_transport, reqbytes, PSTOP_MESSAGE_SIZE, &client);
 
         if(result == PSTOP_MESSAGE_SIZE) {
+            //dump_bytes(reqbytes);
             pstop_message_decode(&req_msg, reqbytes);
 
             fprintf(stderr, "Got message: %d from %d\n", req_msg.message, req_msg.id.data[15]);

pstop_c/pstop/include/pstop/pstop_msg.h

@@ -77,12 +77,12 @@ typedef struct {
      */
     uint16_t checksum;
 
+    uint16_t calculated_checksum;
+
 } pstop_msg_t;
 
 void pstop_message_init(pstop_msg_t *msg);
 
-uint16_t pstop_calculate_checksum(const pstop_msg_t *msg);
-
 pstop_error_t pstop_is_message_valid(const pstop_msg_t *msg);
 
 /**

pstop_c/pstop/src/pstop/checksum.c

@@ -6,8 +6,22 @@
 
 #include "pstop/checksum.h"
 
+static const uint16_t POLY = 0x1021U;
+
 uint16_t
 checksum_crc16(const uint8_t *data, size_t data_length)
 {
-    return 0U;
+    uint16_t crc = 0xFFFF;
+
+    for(uint16_t i = 0U; i < data_length; ++i) {
+        crc ^= (uint16_t)data[i] << 8U;
+        for(uint16_t j = 0U; j < 8U; ++j) {
+            if(crc & 0x8000U) {
+                crc = (crc << 1U) ^ POLY;
+            } else {
+                crc <<= 1U;
+            }
+        }
+    }
+    return crc;
 }

pstop_c/pstop/src/pstop/protocol.c

@@ -11,7 +11,7 @@ static
 int
 is_checksum_valid(const pstop_msg_t *req)
 {
-    return 1;
+    return req->checksum == req->calculated_checksum;
 }
 
 pstop_error_t

pstop_c/pstop/src/pstop/pstop_msg.c

@@ -21,17 +21,7 @@ pstop_message_init(pstop_msg_t *msg)
     msg->received_counter = 0U;
     msg->message = PSTOP_MESSAGE_UNKNOWN;
     msg->checksum = 0U;
-}
-
-// this check belongs in the black channel protocol code
-uint16_t
-pstop_calculate_checksum(const pstop_msg_t *msg)
-{
-    // calculate checksum
-    const uint8_t *start = (const uint8_t *)msg;
-    const uint8_t *end = start + (((const uint8_t *)&(msg->checksum)) - start);
-
-    return checksum_crc16(start, (end - start));
+    msg->calculated_checksum = 0U;
 }
 
 static
@@ -96,11 +86,10 @@ static
 void
 write_uint16(uint16_t value, uint8_t *data, size_t *pos)
 {
-    uint8_t *bytes = data + *pos;
-    *pos = *pos + 2U;
 #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
-    bytes[0] = (uint8_t)(value & 0xFFU);
-    bytes[1] = (uint8_t)((value >> 8U)& 0xFFU);
+    uint16_t *bytes = (uint16_t *)(data + *pos);
+    *pos = *pos + 2U;
+    *bytes = value;
 #else
     bytes[1] = (uint8_t)(value & 0xFFU);
     bytes[0] = (uint8_t)((value >> 8U)& 0xFFU);
@@ -206,9 +195,8 @@ void
 pstop_message_decode(pstop_msg_t *msg, const uint8_t *data)
 {
     size_t pos = 0U;
-    uint8_t b = read_uint8(data, &pos);
-    msg->version = (b & 0xF0) >> 4U;
-    msg->message = (b & 0x0F);
+    msg->version = read_uint8(data, &pos);
+    msg->message = read_uint8(data, &pos);
     msg->stamp = read_uint64(data, &pos);
     msg->received_stamp = read_uint64(data, &pos);
     read_device_uuid(&msg->id, data, &pos);
@@ -217,20 +205,22 @@ pstop_message_decode(pstop_msg_t *msg, const uint8_t *data)
     msg->counter = read_uint32(data, &pos);
     msg->received_counter = read_uint32(data, &pos);
     msg->checksum = read_uint16(data, &pos);
+    msg->calculated_checksum = checksum_crc16(data, PSTOP_MESSAGE_SIZE - 2U);
 }
 
 void
 pstop_message_encode(const pstop_msg_t *msg, uint8_t *data)
 {
     size_t pos = 0U;
-    uint8_t b = (msg->version << 4) | msg->message;
-    write_uint8(b, data, &pos);
+    write_uint8(msg->version, data, &pos);
+    write_uint8(msg->message, data, &pos);
     write_uint64(msg->stamp, data, &pos);
     write_uint64(msg->received_stamp, data, &pos);
     write_device_uuid(&msg->id, data, &pos);
     write_device_uuid(&msg->receiver_id, data, &pos);
     write_uint32(msg->heartbeat_timeout, data, &pos);
     write_uint32(msg->counter, data, &pos);
     write_uint32(msg->received_counter, data, &pos);
-    write_uint16(msg->checksum, data, &pos);
+    uint16_t checksum = checksum_crc16(data, PSTOP_MESSAGE_SIZE - 2U);
+    write_uint16(checksum, data, &pos);
 }

pstop_c/pstop/test/src/pstop/protocol_test.c

@@ -5,6 +5,8 @@
 
 #include <unity/unity.h>
 
+#include "pstop/checksum.h"
+
 #include "pstop/test_utils.h"
 
 static uint64_t current_time;
@@ -71,8 +73,11 @@ test_protocol_invalid_receiver_id(void)
 
     pstop_msg_t req;
     pstop_msg_t resp;
+    pstop_message_init(&req);
+    pstop_message_init(&resp);
     pstop_msg_t *handle = &resp;
     device_id_set_str(&req.receiver_id, "incorrect");
+
     TEST_ASSERT_EQUAL(PSTOP_ERROR_INVALID_ID, machine.handle_protocol_message_cb(&machine, &req, &handle));
 }
 
@@ -87,8 +92,11 @@ test_protocol_operator_not_allowed(void)
 
     pstop_msg_t req;
     pstop_msg_t resp;
+    pstop_message_init(&req);
+    pstop_message_init(&resp);
     pstop_msg_t *handle = &resp;
     device_id_set_str(&req.receiver_id, "testing");
+
     TEST_ASSERT_EQUAL(PSTOP_OPERATOR_NOT_ALLOWED, machine.handle_protocol_message_cb(&machine, &req, &handle));
 }
 
@@ -112,6 +120,7 @@ test_protocol_bond_request(void)
     pstop_message_init(&resp);
     pstop_msg_t *handle = &resp;
     device_id_set_str(&req.receiver_id, "testing");
+
     TEST_ASSERT_EQUAL(PSTOP_OK, machine.handle_protocol_message_cb(&machine, &req, &handle));
     TEST_ASSERT_EQUAL(PSTOP_MESSAGE_BOND, resp.message);
     TEST_ASSERT_EQUAL(1U, resp.counter);

pstop_c/pstop/test/src/pstop/pstop_msg_test.c

@@ -9,7 +9,7 @@
 static
 uint8_t
 PSTOP_MSG_BYTES[] = {
-    0x02U, // version/message
+    0x00U, 0x02U, // version/message
     0x01U, 0x02U, 0x03U, 0x04U, 0x05U, 0x06U, 0x07U, 0x08U, // timestamp
     0x11U, 0x12U, 0x13U, 0x14U, 0x15U, 0x16U, 0x17U, 0x18U, // received timestamp