From afc08704b28384162eb890b553ba529f9403e7e9 Mon Sep 17 00:00:00 2001
From: Szymon Zurawski <szymon.zurawski03@gmail.com>
Date: Mon, 21 Aug 2023 12:06:12 +0100
Subject: [PATCH] Command queue API

Write C code in HAL:
 - handling the allocation and deallocation of nodes memory
 - add and pop command API

ref #10
---
 obc-firmware/interfaceview.ui.xml             |  6 +-
 .../work/hal/implem/msp430/C/src/hal.c        | 97 +++++++++++++++++++
 2 files changed, 100 insertions(+), 3 deletions(-)

diff --git a/obc-firmware/interfaceview.ui.xml b/obc-firmware/interfaceview.ui.xml
index 55d4d0f..d1b417b 100644
--- a/obc-firmware/interfaceview.ui.xml
+++ b/obc-firmware/interfaceview.ui.xml
@@ -1,4 +1,6 @@
-<UI version="1.0" ><Entity id="{4c76f858-03de-4264-8ff7-1b3a466f1eb8}"><Taste RootCoordinates="-64100 -31300 141600 72600" coordinates="6000 7000 111400 47800">
+<UI version="1.0" ><Entity id="{79126f59-8bdb-46a9-87c3-e562c108e91d}"><Taste coordinates="5400 -11300 46700 4300">
+</Taste>
+</Entity><Entity id="{4c76f858-03de-4264-8ff7-1b3a466f1eb8}"><Taste RootCoordinates="-64100 -31300 141600 72600" coordinates="6000 7000 111400 47800">
 </Taste>
 </Entity><Entity id="{972e8e24-275f-4a0e-b469-39f22bf2547f}"><Taste RootCoordinates="-62600 -26600 62600 26600" coordinates="-50800 52600 131900 67400">
 </Taste>
@@ -6,7 +8,5 @@
 </Taste>
 </Entity><Entity id="{51d88f2f-533a-4b8b-bf54-9b00b639836e}"><Taste coordinates="-46500 52600">
 </Taste>
-</Entity><Entity id="{79126f59-8bdb-46a9-87c3-e562c108e91d}"><Taste coordinates="5400 -11300 46700 4300">
-</Taste>
 </Entity>
 </UI>
\ No newline at end of file
diff --git a/obc-firmware/work/hal/implem/msp430/C/src/hal.c b/obc-firmware/work/hal/implem/msp430/C/src/hal.c
index 811fca2..0916a6a 100644
--- a/obc-firmware/work/hal/implem/msp430/C/src/hal.c
+++ b/obc-firmware/work/hal/implem/msp430/C/src/hal.c
@@ -14,10 +14,34 @@
 #include <FreeRTOS.h>
 #include <semphr.h>
 
+#include <stdlib.h>
+#include <string.h>
+
 #include "msp430_uart.h"
 
 bool P4_6_LED_ON = false;
 
+typedef struct cmdQueueNode
+{
+    struct cmdQueueNode* next;
+    unsigned char cmd[2];
+    unsigned char* param;
+} cmdQueueNode;
+
+typedef struct cmdQueue
+{
+    cmdQueueNode* head;
+    cmdQueueNode* tail;
+    uint8_t size;
+} cmdQueue;
+
+void cmdQueueNodeFree( cmdQueueNode* node );
+void cmdQueueInit( cmdQueue* queue );
+void cmdQueueEnqueue( cmdQueue* queue, cmdQueueNode* node );
+cmdQueueNode* cmdQueueDequeue( cmdQueue* queue );
+void cmdQueueAddCmd( cmdQueue* queue, unsigned char cmd[], unsigned char param[], uint8_t paramSize );
+void cmdQueuePopCmd( cmdQueue* queue );
+
 void hal_startup( void )
 {
    // Write your initialisation code, but DO NOT CALL REQUIRED INTERFACES
@@ -50,3 +74,76 @@ void hal_PI_set_led( const asn1SccT_Boolean *IN_val )
     }
     P4_6_LED_ON = *IN_val;
 }
+
+void cmdQueueNodeFree( cmdQueueNode* node )
+{
+    free(node->param);
+    free(node);
+}
+
+void cmdQueueInit( cmdQueue* q )
+{
+    q->head = NULL;
+    q->tail = NULL;
+    q->size = 0;
+}
+
+void cmdQueueEnqueue( cmdQueue* queue, cmdQueueNode* node )
+{
+    node->next = NULL;
+    if (queue->tail == NULL)
+    {
+        queue->head = node;
+    }
+    else
+    {
+        queue->tail->next = node;
+    }
+    queue->tail = node;
+    queue->size++;
+}
+
+cmdQueueNode* cmdQueueDequeue( cmdQueue* queue )
+{
+    if (queue->head == NULL) // underflow
+    {
+        return NULL;
+    }
+    cmdQueueNode* node = queue->head;
+    queue->head = queue->head->next;
+    if (queue->head == NULL)
+    {
+        queue->tail = NULL;
+    }
+    queue->size--;
+
+    return node;
+}
+
+void cmdQueueAddCmd( cmdQueue* queue, unsigned char cmd[], unsigned char param[], uint8_t paramSize )
+{
+    cmdQueueNode* newNode = (cmdQueueNode*)malloc(sizeof(cmdQueueNode));
+    if (newNode != NULL)
+    {
+        unsigned char* paramField = (unsigned char*)malloc(sizeof(unsigned char*)*paramSize);
+        if (paramField == NULL) // not enought memory for newNode
+        {
+            free(newNode);
+        }
+        else
+        {
+            memcpy(newNode->cmd, cmd, 2);
+            memcpy(newNode->param, param, paramSize);
+            cmdQueueEnqueue(queue, newNode);
+        }
+    }
+}
+
+void cmdQueuePopCmd( cmdQueue* queue )
+{
+    cmdQueueNode* popedNode = cmdQueueDequeue(queue);
+    if (popedNode != NULL)
+    {
+        cmdQueueNodeFree(popedNode);
+    }
+}