PicoSDKandFreeRtos/main.cpp

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>

#include "pico/stdlib.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

#define PING_COMMAND "ping"
#define PING_RESPONSE "PONG!"

#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;

typedef struct {
    int id;
    char command[MAX_COMMAND_LENGTH];
    TaskHandle_t task_handle;
} CommandTask;

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){
        printf_sync("%d:%s\n", cmd_task->id, PING_RESPONSE);
    }
}

static void read_serial_line_task(void *params){
    (void)params;

    char buf[MAX_COMMAND_LENGTH];
    size_t it = 0;

    while(true){
        int c = getchar_timeout_us(0);

        if(c == PICO_ERROR_TIMEOUT){
            vTaskDelay(1);
            continue;
        }

        if(c == '\r'){
            continue;
        }

        if(it < MAX_COMMAND_LENGTH - 1){
            buf[it++] = (char)c;
        }
        else{
            printf_sync("Command too long, discarding\n");
            it = 0;
        }
        
        if(c == '\n'){
            buf[it] = '\0';
            it = 0;

            xQueueSendToBack(serial_queue, buf, portMAX_DELAY);
        }
    }
}

bool is_valid_command(const char *command){
    return strstr(command, PING_COMMAND) == command;
}

void parse_command(CommandTask *dest, const char *command){
    char *delim = strchr(command, ':');
    if(delim == nullptr || delim == command){
        dest->id = -1;
        dest->command[0] = '\0';
        return;
    }

    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(command, nullptr, 10);
    snprintf(dest->command, MAX_COMMAND_LENGTH, "%s", delim + 1);

    return;
}

void clear_command_task(CommandTask *cmd_task){
    cmd_task->id = -1;
    cmd_task->command[0] = '\0';
}

void command_handler_task(void *params){
    CommandTask *cmd_task = (CommandTask*)params;

    while(true){
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        serial_command_handler(cmd_task);

        clear_command_task(cmd_task);
        xQueueSendToBack(command_handler_task_queue, &cmd_task, portMAX_DELAY);
    }
}

static void serial_processing_task(void *params){
    (void)params;

    char buf[MAX_COMMAND_LENGTH];
    while(true){
        if(xQueueReceive(serial_queue, buf, portMAX_DELAY)){
            CommandTask* cmd_task_p = nullptr;
            xQueueReceive(command_handler_task_queue, &cmd_task_p, portMAX_DELAY);
            if(cmd_task_p == nullptr){
                printf_sync("Failed to receive command task from queue\n");
                continue;
            }

            parse_command(cmd_task_p, buf);
            
            if(cmd_task_p->id == -1  
               || cmd_task_p->command[0] == '\0'
               || !is_valid_command(cmd_task_p->command)){
                
                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;
            }
            
            xTaskNotifyGive(cmd_task_p->task_handle);
        }
    }
}

static void led_task(void *params){
    (void)params;

    int led_pin = 25;

    gpio_init(led_pin);
    gpio_set_dir(led_pin, GPIO_OUT);

    while(true){
        gpio_put(led_pin, 1);
        vTaskDelay(pdMS_TO_TICKS(500));
        gpio_put(led_pin, 0);
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

// needs to be defined to avoid linker errors, see FreeRTOSConfig.h configCHECK_FOR_STACK_OVERFLOW
extern "C" void vApplicationStackOverflowHook(TaskHandle_t task, char *task_name){
    (void)task;
    (void)task_name;

    taskDISABLE_INTERRUPTS();
    while(true){
        tight_loop_contents();
    }
}

void init_serial_queue(){
    serial_queue = xQueueCreate(10, sizeof(char[64]));
    if (serial_queue == NULL) {
        printf("Failed to create serial queue\n");
        while(true){
            tight_loop_contents();
        }
    }
}

void init_command_handler(){
    command_handler_task_queue = xQueueCreate(10, sizeof(CommandTask*));
    if (command_handler_task_queue == NULL) {
        printf("Failed to create command handler task queue\n");
        while(true){
            tight_loop_contents();
        }
    }

    for(int i = 0; i < 10; i++){
        CommandTask *cmd_task = (CommandTask*)malloc(sizeof(CommandTask));
        if(cmd_task == nullptr){
            printf("Failed to allocate memory for command task\n");
            while(true){
                tight_loop_contents();
            }
        }

        if(xTaskCreateAffinitySet(command_handler_task, "CmdHandler", 512, cmd_task, 2, distilleCore(0), &cmd_task->task_handle) != pdPASS){
            printf("Failed to create command handler task\n");
            while(true){
                tight_loop_contents();
            }
        }

        clear_command_task(cmd_task);
        xQueueSendToBack(command_handler_task_queue, &cmd_task, portMAX_DELAY);
    }
}

void init_serial_out_mutex(){
    serial_out_mutex = xSemaphoreCreateMutex();
    if (serial_out_mutex == NULL) {
        printf("Failed to create serial output mutex\n");
        while(true){
            tight_loop_contents();
        }
    }
}

int main(){
    stdio_init_all();

    init_serial_queue();
    init_command_handler();
    init_serial_out_mutex();

    xTaskCreateAffinitySet(led_task, "LED", 512, nullptr, 1, distilleCore(0), nullptr);
    xTaskCreateAffinitySet(serial_processing_task, "Serial Processing", 512, nullptr, 1, distilleCore(0), nullptr);
    xTaskCreateAffinitySet(read_serial_line_task, "Serial Receiver", 512, nullptr, 1, distilleCore(0), nullptr);

    vTaskStartScheduler();

    while (true) {
        tight_loop_contents();
    }
}

/*
Serial Command Layout: "<id>:<command>\n"
*/