wip: Add unit tests for communication.c packet receiving

quad/library.mk

@@ -52,4 +52,4 @@ $(LIBDIR):
 	mkdir $(LIBDIR)
 
 $(TESTBIN): $(TESTOBJECTS) $(OBJECTS) | default
-	$(GCC) -o $(TESTBIN) $^ -I$(INCDIR) -L$(LIBDIR) $(REQLIBS)
+	$(GCC) -g -o $(TESTBIN) $^ -I$(INCDIR) -L$(LIBDIR) $(REQLIBS)

quad/src/quad_app/.gitignore

 obj-zybo/
-obj/
+obj/
\ No newline at end of file
+run_tests
\ No newline at end of file

quad/src/quad_app/Makefile

 TOP=../..
 
 NAME = quad_app
-REQLIBS = -ltest -lcomputation_graph
+REQLIBS = -ltest -lcomputation_graph -lm -lqueue
 
 include $(TOP)/library.mk

quad/src/quad_app/communication.c

@@ -10,7 +10,6 @@
 // plus the maximum packet size that might be carried over from the last call,
 // plus 128 for a little buffer
 // (bit/s) * (seconds) / (10 bits / byte for UART)
-#define MAX_PACKET_SIZE 256
 #define PACKET_HEADER_SIZE 7
 #define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)
 
@@ -19,35 +18,62 @@
 // Declaration of interrupt handler
 void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
 
-static u8 packet[MAX_PACKET_SIZE];
-static int bytes_recv = 0;
+u8 packet[MAX_PACKET_SIZE];
+int bytes_recv = 0;
 
 int try_receive_packet(struct UARTDriver *uart) {
 
-  // Find start of packet
   int attempts = 0;
-  while (bytes_recv == 0) {
-    if (attempts > MAX_PACKET_SIZE) return -1; // keep trying later
-    if (uart->read(uart, &packet[bytes_recv], 1)) return -1; // ran out
-    if (packet[bytes_recv] == 0xBE) bytes_recv += 1;
-    attempts += 1;
+  while (attempts++ < MAX_PACKET_SIZE) {
+    // Find start of packet
+    if (bytes_recv == 0) {
+      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
+      if (packet[bytes_recv] != 0xBE) {
+	continue; // keep looking
+      }
+      bytes_recv = 1;
+    }
+
+    // receive length bytes
+    while (bytes_recv < PACKET_HEADER_SIZE) {
+      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
+      bytes_recv += 1;
+    }
+
+    // Check if incoming packet is nonsensically large
+    unsigned short length = (packet[6] << 8 | packet[5]) + PACKET_HEADER_SIZE;
+    if (length >= MAX_PACKET_SIZE) {
+      bytes_recv = 0;
+      continue; // abort packet and start over
+    }
+
+    // Receive rest of packet
+    while (bytes_recv < length) {
+      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
+      bytes_recv += 1;
+    }
+
+    // Receive checksum and compare
+    if (bytes_recv < length + 1) {
+      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
+      unsigned char calculated_checksum = 0;
+      int i;
+      for(i = 0; i < length; i++) {
+	calculated_checksum ^= packet[i];
+      }
+      if (calculated_checksum != packet[bytes_recv]) {
+	bytes_recv = 0;
+	continue; // abort packet and start over
+      }
+      bytes_recv += 1;
+    }
+
+    // Packet ready
+    return 0;
   }
 
-  // Find length bytes in packet
-  while (bytes_recv < PACKET_HEADER_SIZE) {
-    if (uart->read(uart, &packet[bytes_recv++], 1)) return -1; // ran out
-    bytes_recv += 1;
-  }
-
-  // Wait for rest of data plus checksum (+1)
-  int length = (packet[6] << 8 | packet[5]) + PACKET_HEADER_SIZE + 1;
-  while (bytes_recv < length) {
-    if (uart->read(uart, &packet[bytes_recv++], 1)) return -1; // ran out
-    bytes_recv += 1;
-  }
-
-  // Fully recieved a packet
-  return 0;
+  // Try again later
+  return -1;
 }
 
 int process_packet(modular_structs_t *structs) {
@@ -59,21 +85,10 @@ int process_packet(modular_structs_t *structs) {
   meta.data_len = packet[6] << 8 | packet[5];
   unsigned char packet_checksum = packet[7+meta.data_len];
 
-  // Compute checksum
-  int i;
-  unsigned char calculated_checksum = 0;
-  for(i = 0; i < meta.data_len + 7; i++){
-    calculated_checksum ^= packet[i];
-  }
-  // Discard if checksum didn't match
-  if(packet_checksum != calculated_checksum) {
-    return -1;
-  }
-
   // Call appropriate function for packet
-  (* (MessageTypes[meta.msg_type].functionPtr))(structs,&meta,packet[PACKET_HEADER_SIZE], meta.data_len);
+  (* (MessageTypes[meta.msg_type].functionPtr))(structs,&meta,&packet[PACKET_HEADER_SIZE], meta.data_len);
 
-  // Done processing packets, reset
+  // Done processing packets, reset state
   bytes_recv = 0;
 
   return 0;
@@ -82,7 +97,7 @@ int process_packet(modular_structs_t *structs) {
 void process_received(modular_structs_t *structs) {
   // Parse as many packets as possible
   struct UARTDriver *uart = &structs->hardware_struct.uart;
-  while (try_receive_packet(uart)) {
+  while (!try_receive_packet(uart)) {
     process_packet(structs);
   }
 }
@@ -115,15 +130,14 @@ int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size
 	int i;
 	for(i = 0; i < PACKET_HEADER_SIZE; i++) {
 		packet_checksum ^= formattedHeader[i];
+		uart->write(uart, formattedHeader[i]);
 	}
 	for (i = 0; i < size; i++) {
 		packet_checksum ^= data[i];
+		uart->write(uart, (unsigned char) data[i]);
 	}
 
-	// Send header, data, and checksum
-	uart->write(uart, formattedHeader, PACKET_HEADER_SIZE);
-	uart->write(uart, (unsigned char *) data, size);
-	uart->write(uart, &packet_checksum, 1);
+	uart->write(uart, packet_checksum);
 
 	return 0;
 }

quad/src/quad_app/communication.h

@@ -6,6 +6,8 @@
 #include "commands.h"
 #include "hw_iface.h"
 
+#define MAX_PACKET_SIZE 256
+
 int initUartComms();
 void process_received(modular_structs_t *structs);
 int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size_t size);

quad/src/quad_app/hw_iface.h

@@ -29,8 +29,8 @@ struct PWMInputDriver {
 struct UARTDriver {
   void *state;
   int (*reset)(struct UARTDriver *self);
-  int (*write)(struct UARTDriver *self, unsigned char *data, unsigned int length);
-  int (*read)(struct UARTDriver *self, unsigned char *buff, unsigned int length);
+  int (*write)(struct UARTDriver *self, unsigned char c);
+  int (*read)(struct UARTDriver *self, unsigned char *c);
 };
 
 struct TimerDriver {

quad/src/quad_app/initialize_components.c

@@ -50,9 +50,6 @@ int init_structs(modular_structs_t *structs) {
   // Initialize the controller
   control_algorithm_init(&structs->parameter_struct);
 
-  // Xilinx given initialization
-  init_platform();
-
   // Initialize loop timers
   struct TimerDriver *global_timer = &structs->hardware_struct.global_timer;
   struct TimerDriver *axi_timer = &structs->hardware_struct.axi_timer;

quad/src/quad_app/test/test_quad_app.c

+#include "communication.h"
+#include <string.h>
+#include "queue.h"
+#include "test.h"
+
+struct Queue *queue;
+struct UARTDriver *uart;
+
+int mock_uart_read(struct UARTDriver *self, unsigned char *c) {
+  return queue_remove(queue, c);
+}
+
+struct UARTDriver * mock_uart_malloc() {
+  struct UARTDriver *uart = malloc(sizeof(struct UARTDriver));
+  uart->read = mock_uart_read;
+  return uart;
+}
+
+void queue_add_corruption(struct Queue *q, int num) {
+  int val = 0xAA;
+  int i;
+  for (i = 0; i < num; i += 1) {
+    val = (val << 1) ^ i;
+    if (val == 0xBE) queue_add(q, 0);
+    else queue_add(q, val);
+  }
+}
+
+void queue_add_short(struct Queue *q, short val) {
+  queue_add(q, val & 0x00FF);
+  queue_add(q, val >> 8);
+}
+
+// Return checksum
+unsigned char queue_add_packet(struct Queue *q,
+		unsigned short type,
+		unsigned short id,
+		unsigned short length,
+		unsigned char *data) {
+  queue_add(q, 0xBE);
+  queue_add(q, type & 0x00FF);
+  queue_add(q, type >> 8);
+  queue_add(q, id & 0x00FF);
+  queue_add(q, id >> 8);
+  queue_add(q, length & 0x00FF);
+  queue_add(q, length >> 8);
+
+  unsigned char checksum = 0;
+  checksum ^= 0xBE;
+  checksum ^= type & 0x00FF;
+  checksum ^= type >> 8;
+  checksum ^= id & 0x00FF;
+  checksum ^= id >> 8;
+  checksum ^= length & 0x00FF;
+  checksum ^= length >> 8;
+  int i;
+  for (i = 0; i < length; i += 1) {
+    queue_add(q, data[i]);
+    checksum ^= data[i];
+  }
+  queue_add(q, checksum);
+  return checksum;
+}
+
+// Spying into communication.c's global variables
+extern u8 packet[MAX_PACKET_SIZE];
+extern int bytes_recv;
+extern unsigned char packet_checksum;
+
+// Test fails when no BE and run out
+int test_try_receive_packet_fails_when_no_BE_and_run_out() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  queue = queue_malloc(5);
+  queue_add(queue, 0);
+  queue_add(queue, 0);
+  queue_add(queue, 1);
+
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 0);
+
+  // Try again to verify that we actually ran out
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 0);
+
+  return 0;
+}
+
+// Test fails when no BE and too much (check resume)
+int test_try_receive_packet_fails_when_no_BE_and_too_much() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  int size = 255;
+  queue = queue_malloc(size);
+  queue_add_corruption(queue, size);
+
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 0);
+
+  // Ensure that we quit trying
+  test_assert(size - queue_size(queue) <= MAX_PACKET_SIZE);
+
+  return 0;
+}
+
+// Test fails when BE and run out (check resume)
+int test_try_receive_packet_fails_when_BE_and_run_out() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  queue = queue_malloc(100);
+  queue_add_corruption(queue, 10);
+  queue_add(queue, 0xBE);
+
+  queue_add(queue, 1);
+  queue_add(queue, 2);
+  queue_add(queue, 3);
+
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 4);
+
+  test_assert(packet[0] == 0xBE);
+  test_assert(packet[1] == 1);
+  test_assert(packet[2] == 2);
+  test_assert(packet[3] == 3);
+
+  // Try again to verify that we actually ran out
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 4);
+
+  return 0;
+}
+
+// Test fails when BE and nonsensical length (check resume)
+int test_try_receive_packet_fails_when_BE_and_big_length() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  queue = queue_malloc(500);
+  int i;
+  queue_add_corruption(queue, 10);
+  queue_add(queue, 0xBE);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 500);
+  for (i = 0; i < 10; i += 1) queue_add_corruption(queue, i);
+
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 0);
+
+  // Try again to verify that we actually ran out
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 0);
+
+  return 0;
+}
+
+// Test fails when BE and length and run out (check resume)
+int test_try_receive_packet_fails_when_BE_length_and_run_out() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  queue = queue_malloc(500);
+  int i;
+  queue_add_corruption(queue, 20);
+  queue_add(queue, 0xBE);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 4);
+  unsigned char data[4] = {1, 2, 3, 4};
+  for (i = 0; i < 2; i += 1) queue_add(queue, data[i]);
+
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 9);
+  test_assert(packet[0] == 0xBE);
+  test_assert(packet[1] == 0);
+  test_assert(packet[2] == 0);
+  test_assert(packet[3] == 0);
+  test_assert(packet[4] == 0);
+  test_assert(packet[5] == 4);
+  test_assert(packet[6] == 0);
+  test_assert(packet[7] == 1);
+  test_assert(packet[8] == 2);
+
+  // Try again to verify that we actually ran out
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 9);
+
+  return 0;
+}
+
+// Test fails when BE and length and data and run out (check resume)
+int test_try_receive_packet_fails_when_BE_length_data_and_run_out() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  queue = queue_malloc(500);
+  int i;
+  queue_add_corruption(queue, 10);
+  queue_add(queue, 0xBE);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 4);
+  unsigned char data[4] = {1, 2, 3, 4};
+  for (i = 0; i < 4; i += 1) queue_add(queue, data[i]);
+
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 11);
+  test_assert(packet[0] == 0xBE);
+  test_assert(packet[1] == 0);
+  test_assert(packet[2] == 0);
+  test_assert(packet[3] == 0);
+  test_assert(packet[4] == 0);
+  test_assert(packet[5] == 4);
+  test_assert(packet[6] == 0);
+  test_assert(packet[7] == 1);
+  test_assert(packet[8] == 2);
+  test_assert(packet[9] == 3);
+  test_assert(packet[10] == 4);
+
+  // Try again to verify that we actually ran out
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 11);
+
+  return 0;
+}
+
+// Test fails when BE, length, data, and checksum fails
+int test_try_receive_packet_fails_when_BE_length_data_and_bad_checksum() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  queue = queue_malloc(500);
+  int i;
+  queue_add_corruption(queue, 10);
+  queue_add(queue, 0xBE);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 0);
+  queue_add_short(queue, 4);
+  unsigned char data[4] = {1, 2, 3, 4};
+  for (i = 0; i < 4; i += 1) queue_add(queue, data[i]);
+  queue_add(queue, 0xFF); // bad checksum
+  queue_add(queue, 0xBE); // next start
+
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 1);
+  test_assert(packet[0] == 0xBE);
+
+  // Try again to verify that we actually ran out
+  test_assert(try_receive_packet(uart));
+  test_assert(bytes_recv == 1);
+
+  return 0;
+}
+
+// Test succeeds when BE, length, data, checksum
+int test_try_receive_packet_succeeds() {
+  bytes_recv = 0;
+  uart = mock_uart_malloc();
+  queue = queue_malloc(500);
+  int i;
+  queue_add_corruption(queue, 10);;
+  unsigned char data[4] = {1, 2, 3, 4};
+  unsigned char checksum = queue_add_packet(queue, 0, 0, 4, data);
+
+  int failure = try_receive_packet(uart);
+  test_assert(!failure);
+  test_assert(bytes_recv == 12);
+  test_assert(packet[0] == 0xBE);
+  test_assert(packet[1] == 0);
+  test_assert(packet[2] == 0);
+  test_assert(packet[3] == 0);
+  test_assert(packet[4] == 0);
+  test_assert(packet[5] == 4);
+  test_assert(packet[6] == 0);
+  test_assert(packet[7] == 1);
+  test_assert(packet[8] == 2);
+  test_assert(packet[9] == 3);
+  test_assert(packet[10] == 4);
+  test_assert(packet[11] == checksum);
+
+  // Try again to verify that we don't lose the ready packet
+  test_assert(!try_receive_packet(uart));
+  test_assert(bytes_recv == 12);
+
+  return 0;
+}
+
+int main() {
+  TEST(test_try_receive_packet_fails_when_no_BE_and_run_out);
+  TEST(test_try_receive_packet_fails_when_no_BE_and_too_much);
+  TEST(test_try_receive_packet_fails_when_BE_and_run_out);
+  TEST(test_try_receive_packet_fails_when_BE_and_big_length);
+  TEST(test_try_receive_packet_fails_when_BE_length_and_run_out);
+  TEST(test_try_receive_packet_fails_when_BE_length_data_and_run_out);
+  TEST(test_try_receive_packet_fails_when_BE_length_data_and_bad_checksum);
+  TEST(test_try_receive_packet_succeeds);
+  return test_summary();
+}

quad/src/test/test.c

 #include "test.h"
 
 static int num_tests = 0;
-static struct Test tests[128];
+static struct Test tests[255];
 static int longest_test_name = 0;
 static int num_assertions = 0;
 

quad/src/test/test.h

@@ -7,9 +7,11 @@
 #include <sys/types.h>
 #include <sys/wait.h>
 
+#define TEST(name) test(name, #name)
+
 struct Test {
-  char name[64];
-  char result_msg[32];
+  char name[255];
+  char result_msg[255];
   unsigned char failed;
 };
 

quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.h

@@ -19,8 +19,8 @@
 #include "sleep.h"
 
 int zybo_uart_reset(struct UARTDriver *self);
-int zybo_uart_write(struct UARTDriver *self, unsigned char *data, unsigned int length);
-int zybo_uart_read(struct UARTDriver *self, unsigned char *buff, unsigned int length);
+int zybo_uart_write(struct UARTDriver *self, unsigned char c);
+int zybo_uart_read(struct UARTDriver *self, unsigned char *c);
 
 int zybo_pwm_output_reset(struct PWMOutputDriver *self);
 int zybo_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);

quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_uart.c

@@ -68,20 +68,14 @@ int zybo_uart_reset(struct UARTDriver *self) {
   return 0;
 }
 
-int zybo_uart_write(struct UARTDriver *self, unsigned char *data, unsigned int length) {
-  XUartPs *inst = self->state;
-  int i;
-  for (i = 0; i < length; i += 1) {
-    XUartPs_SendByte(inst->Config.BaseAddress, data[i]);
-  }
-
+int zybo_uart_write(struct UARTDriver *self, unsigned char c) {
+  XUartPs_SendByte(inst->Config.BaseAddress, data[i]);
   return 0;
 }
 
-int zybo_uart_read(struct UARTDriver *self, unsigned char *buff, unsigned int length) {
-  int i = 0;
-  while (queue_remove(&queue, &buff[i]) == 0 && i < length) i += 1;
-  return i;
+int zybo_uart_read(struct UARTDriver *self, unsigned char *c) {
+  if (!queue_remove(&queue, c)) return -1;
+  else return 0;
 }
 
 //This is copied from xuart driver