made code more robust by adding a dedicated printf wrapper to make synchronised serial prints easier accessable - hardened parse_command by validating <id> and removed strndup and strncpy

main.cpp

@@ -1,5 +1,6 @@
 #include <stdio.h>
 #include <stdlib.h>
+#include <stdarg.h>
 #include <string.h>
 
 #include "pico/stdlib.h"
@@ -14,6 +15,7 @@
 #define MAX_COMMAND_LENGTH 64
 
 #define distilleCore(core) (1 << (core))
+#define is_digit_char(num) (num >= '0' && num <= '9')
 
 QueueHandle_t serial_queue;
 QueueHandle_t command_handler_task_queue;
@@ -26,13 +28,22 @@ typedef struct {
 
 static SemaphoreHandle_t serial_out_mutex;
 
+static void printf_sync(const char *fmt, ...){
+    va_list args;
+    va_start(args, fmt);
+
+    if(xSemaphoreTake(serial_out_mutex, portMAX_DELAY) == pdTRUE){
+        vprintf(fmt, args);
+        xSemaphoreGive(serial_out_mutex);
+    }
+
+    va_end(args);
+}
+
 static void serial_command_handler(CommandTask *cmd_task){
     const char *cmd_str = (const char *)cmd_task->command;
     if(strstr(cmd_str, PING_COMMAND) == cmd_str){
-        if(xSemaphoreTake(serial_out_mutex, portMAX_DELAY) == pdTRUE){
+        printf_sync("%d:%s\n", cmd_task->id, PING_RESPONSE);
-            printf("%d:%s\n", cmd_task->id, PING_RESPONSE);
-            xSemaphoreGive(serial_out_mutex);
-        }
     }
 }
 
@@ -58,7 +69,7 @@ static void read_serial_line_task(void *params){
             buf[it++] = (char)c;
         }
         else{
-            printf("Command too long, discarding\n");
+            printf_sync("Command too long, discarding\n");
             it = 0;
         }
         
@@ -77,19 +88,23 @@ bool is_valid_command(const char *command){
 
 void parse_command(CommandTask *dest, const char *command){
     char *delim = strchr(command, ':');
-    if(delim == nullptr){
+    if(delim == nullptr || delim == command){
         dest->id = -1;
         dest->command[0] = '\0';
         return;
     }
 
-    char *id_str = strndup(command, delim - command);
+    for(char* p = (char*)command; p < delim; p++){
+        if(!is_digit_char(*p)){
+            dest->id = -1;
+            dest->command[0] = '\0';
+            return;
+        }
+    }
     
-    dest->id = (int)strtol(id_str, nullptr, 10);
+    dest->id = (int)strtol(command, nullptr, 10);
-    free(id_str);
+    snprintf(dest->command, MAX_COMMAND_LENGTH, "%s", delim + 1);
 
-    strncpy(dest->command, delim + 1, MAX_COMMAND_LENGTH - 1);
-    dest->command[MAX_COMMAND_LENGTH - 1] = '\0';
     return;
 }
 
@@ -120,7 +135,7 @@ static void serial_processing_task(void *params){
             CommandTask* cmd_task_p = nullptr;
             xQueueReceive(command_handler_task_queue, &cmd_task_p, portMAX_DELAY);
             if(cmd_task_p == nullptr){
-                printf("Failed to receive command task from queue\n");
+                printf_sync("Failed to receive command task from queue\n");
                 continue;
             }
 
@@ -129,7 +144,8 @@ static void serial_processing_task(void *params){
             if(cmd_task_p->id == -1  
                || cmd_task_p->command[0] == '\0'
                || !is_valid_command(cmd_task_p->command)){
-                printf("Invalid command format: %s\n", buf);
+                
+                printf_sync("Invalid command format: %s\n", buf);
                 clear_command_task(cmd_task_p);
                 xQueueSendToBack(command_handler_task_queue, &cmd_task_p, portMAX_DELAY);
                 continue;