/* EP06 - FreeRTOS minimal system on CVE2 (CV32E20), ALINX PI-AR035-F1-1. */
#include <stdarg.h>
#include <stdint.h>
#include <stddef.h>

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

#define UART_BASE       0x10000000UL
#define UART_TX         ( *( volatile unsigned char * )( UART_BASE + 0x00 ) )
#define UART_STAT       ( *( volatile uint32_t * )( UART_BASE + 0x04 ) )
#define GPIO_BASE       0x40000000UL
#define GPIO_OUT        ( *( volatile uint32_t * )( GPIO_BASE + 0x00 ) )
#define GPIO_SET        ( *( volatile uint32_t * )( GPIO_BASE + 0x0C ) )
#define GPIO_CLR        ( *( volatile uint32_t * )( GPIO_BASE + 0x10 ) )

static QueueHandle_t     g_queue;
static SemaphoreHandle_t g_uart_mtx;
static SemaphoreHandle_t g_shared_mtx;

static TaskHandle_t h_pulse, h_producer, h_consumer, h_monitor;

static volatile uint32_t g_pulse, g_heartbeat, g_sent, g_recv;
static volatile uint32_t g_qerr, g_shared, g_expected, g_idle_loops;
static volatile uint32_t g_reported;

static void uart_putc( char c )
{
    while( UART_STAT & 1U ) { }
    UART_TX = ( unsigned char ) c;
}

static void uart_puts( const char * s )
{
    while( *s ) uart_putc( *s++ );
}

static void uart_putu( uint32_t v )
{
    char b[12];
    int i = 0;
    if( v == 0 ) { uart_putc( '0' ); return; }
    while( v ) { b[i++] = ( char )( '0' + ( v % 10U ) ); v /= 10U; }
    while( i ) uart_putc( b[--i] );
}

static void log_printf( const char * fmt, ... )
{
    va_list ap;
    va_start( ap, fmt );
    xSemaphoreTake( g_uart_mtx, portMAX_DELAY );
    while( *fmt )
    {
        if( *fmt == '%' )
        {
            fmt++;
            switch( *fmt )
            {
                case 's': uart_puts( va_arg( ap, const char * ) ); break;
                case 'c': uart_putc( ( char ) va_arg( ap, int ) ); break;
                case 'd': uart_putu( ( uint32_t ) va_arg( ap, int ) ); break;
                case 'u': uart_putu( va_arg( ap, unsigned int ) ); break;
                default:  uart_putc( *fmt ); break;
            }
        }
        else
        {
            uart_putc( *fmt );
        }
        fmt++;
    }
    uart_putc( '\r' );
    uart_putc( '\n' );
    xSemaphoreGive( g_uart_mtx );
    va_end( ap );
}

static void bump_shared( void )
{
    xSemaphoreTake( g_shared_mtx, portMAX_DELAY );
    g_shared++;
    g_expected++;
    xSemaphoreGive( g_shared_mtx );
}

static void vPulseTask( void * p )
{
    TickType_t last = xTaskGetTickCount();
    ( void ) p;
    for( ;; )
    {
        g_pulse++;
        GPIO_SET = ( 1U << 0 );
        bump_shared();
        GPIO_CLR = ( 1U << 0 );          /* short visible pulse */
        if( ( g_pulse % 20U ) == 0U )
            log_printf( "[pulse] n=%u tick=%u", g_pulse, ( unsigned ) xTaskGetTickCount() );
        vTaskDelayUntil( &last, pdMS_TO_TICKS( 50 ) );
    }
}

static void vProducerTask( void * p )
{
    TickType_t last = xTaskGetTickCount();
    uint32_t seq = 0;
    ( void ) p;
    for( ;; )
    {
        seq = g_sent + 1U;
        if( xQueueSend( g_queue, &seq, 0 ) != pdPASS )
        {
            g_qerr++;
        }
        else
        {
            g_sent++;
            for( uint32_t i = 0; i < 10U; i++ )
                bump_shared();
            if( ( g_sent % 20U ) == 0U )
                log_printf( "[prod] sent=%u tick=%u", g_sent, ( unsigned ) xTaskGetTickCount() );
        }
        vTaskDelayUntil( &last, pdMS_TO_TICKS( 100 ) );
    }
}

static void vConsumerTask( void * p )
{
    uint32_t v;
    ( void ) p;
    for( ;; )
    {
        if( xQueueReceive( g_queue, &v, portMAX_DELAY ) == pdPASS )
        {
            if( v != ( g_recv + 1U ) )
            {
                g_qerr++;
                log_printf( "[cons] BAD seq got=%u want=%u", v, g_recv + 1U );
            }
            g_recv++;
            if( ( g_recv % 20U ) == 0U )
                log_printf( "[cons] recv=%u tick=%u", g_recv, ( unsigned ) xTaskGetTickCount() );
        }
    }
}

static void vMonitorTask( void * p )
{
    TickType_t last = xTaskGetTickCount();
    uint32_t tick, pass;
    ( void ) p;
    for( ;; )
    {
        tick = ( uint32_t ) xTaskGetTickCount();
        log_printf( "[mon] tick=%u heap=%u idle=%u pulse=%u hb=%u q=%u/%u sh=%u/%u",
                    tick, ( unsigned ) xPortGetFreeHeapSize(), g_idle_loops,
                    g_pulse, g_heartbeat, g_recv, g_sent, g_shared, g_expected );
        if( ( tick >= 8000U ) && ( g_reported == 0U ) )
        {
            pass = 1U;
            if( g_sent < 60U ) pass = 0U;
            if( g_recv != g_sent ) pass = 0U;
            if( g_qerr != 0U ) pass = 0U;
            if( g_shared != g_expected ) pass = 0U;
            if( g_pulse < 120U ) pass = 0U;
            if( g_heartbeat < 10U ) pass = 0U;
            if( g_idle_loops < 1000U ) pass = 0U;
            if( xPortGetFreeHeapSize() < 2048U ) pass = 0U;
            log_printf( "[mon] tick=%u heap=%u idle=%u pulse=%u hb=%u q=%u/%u sh=%u/%u",
                        tick, ( unsigned ) xPortGetFreeHeapSize(), g_idle_loops,
                        g_pulse, g_heartbeat, g_recv, g_sent, g_shared, g_expected );
            log_printf( "EP06 SUMMARY tick=%u heap_free=%u idle=%u pulse=%u heartbeat=%u sent=%u recv=%u qerr=%u shared=%u expected=%u",
                        tick, ( unsigned ) xPortGetFreeHeapSize(), g_idle_loops,
                        g_pulse, g_heartbeat, g_sent, g_recv, g_qerr, g_shared, g_expected );
            log_printf( "EP06 STACK pulse=%u producer=%u consumer=%u monitor=%u words",
                        ( unsigned ) uxTaskGetStackHighWaterMark( h_pulse ),
                        ( unsigned ) uxTaskGetStackHighWaterMark( h_producer ),
                        ( unsigned ) uxTaskGetStackHighWaterMark( h_consumer ),
                        ( unsigned ) uxTaskGetStackHighWaterMark( h_monitor ) );
            if( pass )
                log_printf( "EP06 RESULT: PASS" );
            else
                log_printf( "EP06 RESULT: FAIL" );
            g_reported = 1U;
        }
        vTaskDelayUntil( &last, pdMS_TO_TICKS( 1000 ) );
    }
}

static void vHeartbeatTask( void * p )
{
    TickType_t last = xTaskGetTickCount();
    ( void ) p;
    for( ;; )
    {
        g_heartbeat++;
        GPIO_OUT ^= ( 1U << 1 );
        if( ( g_heartbeat % 4U ) == 0U )
            log_printf( "[hb] n=%u tick=%u", g_heartbeat, ( unsigned ) xTaskGetTickCount() );
        vTaskDelayUntil( &last, pdMS_TO_TICKS( 500 ) );
    }
}

void vApplicationIdleHook( void )
{
    g_idle_loops++;
}

void vApplicationStackOverflowHook( TaskHandle_t xTask, char * pcTaskName )
{
    ( void ) xTask;
    uart_puts( "\r\nSTACK OVERFLOW: " );
    uart_puts( pcTaskName ? pcTaskName : "?" );
    uart_puts( "\r\n" );
    taskDISABLE_INTERRUPTS();
    for( ;; ) { }
}

void vApplicationMallocFailedHook( void )
{
    uart_puts( "\r\nMALLOC FAILED\r\n" );
    taskDISABLE_INTERRUPTS();
    for( ;; ) { }
}

void vAssertCalled( const char * file, unsigned long line )
{
    uart_puts( "\r\nASSERT " );
    uart_puts( file );
    uart_putc( ':' );
    uart_putu( ( uint32_t ) line );
    uart_puts( "\r\n" );
    taskDISABLE_INTERRUPTS();
    for( ;; ) { }
}

int main( void )
{
    uart_puts( "\r\nEP06 FreeRTOS minimal system on CVE2\r\n" );
    uart_puts( "configCPU_CLOCK_HZ=50000000 configTICK_RATE_HZ=1000\r\n" );

    g_queue      = xQueueCreate( 8, sizeof( uint32_t ) );
    g_uart_mtx   = xSemaphoreCreateMutex();
    g_shared_mtx = xSemaphoreCreateMutex();
    if( ( g_queue == NULL ) || ( g_uart_mtx == NULL ) || ( g_shared_mtx == NULL ) )
    {
        uart_puts( "CREATE FAILED\r\n" );
        for( ;; ) { }
    }

    xTaskCreate( vPulseTask,    "pulse",    256, NULL, 4, &h_pulse );
    xTaskCreate( vProducerTask, "producer", 384, NULL, 3, &h_producer );
    xTaskCreate( vConsumerTask, "consumer", 384, NULL, 2, &h_consumer );
    xTaskCreate( vMonitorTask,  "monitor",  512, NULL, 1, &h_monitor );
    xTaskCreate( vHeartbeatTask,"heartbeat",256, NULL, 1, NULL );

    uart_puts( "tasks created, starting scheduler\r\n" );
    vTaskStartScheduler();

    uart_puts( "SCHEDULER RETURNED\r\n" );
    for( ;; ) { }
}
