/*
 * FreeRTOS Kernel V10.3.0
 * Copyright (C) 2020 Cambridge Consultants Ltd.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * http://www.FreeRTOS.org
 * http://aws.amazon.com/freertos
 *
 */

/*-----------------------------------------------------------
* Implementation of functions defined in portable.h for the Posix port.
*
* Each task has a pthread which eases use of standard debuggers
* (allowing backtraces of tasks etc). Threads for tasks that are not
* running are blocked in sigwait().
*
* Task switch is done by resuming the thread for the next task by
* sending it the resume signal (SIGUSR1) and then suspending the
* current thread.
*
* The timer interrupt uses SIGALRM and care is taken to ensure that
* the signal handler runs only on the thread for the current task.
*
* Use of part of the standard C library requires care as some
* functions can take pthread mutexes internally which can result in
* deadlocks as the FreeRTOS kernel can switch tasks while they're
* holding a pthread mutex.
*
* Replacement malloc(), free(), calloc(), and realloc() are provided
* for glibc (see below for more information).
*
* stdio (printf() and friends) should be called from a single task
* only or serialized with a FreeRTOS primitive such as a binary
* semaphore or mutex.
*----------------------------------------------------------*/

#include <errno.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <sys/times.h>
#include <time.h>

/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
/*-----------------------------------------------------------*/

#define SIG_RESUME    SIGUSR1

typedef struct THREAD
{
    pthread_t pthread;
    pdTASK_CODE pxCode;
    void * pvParams;
    BaseType_t xDying;
} Thread_t;

/*
 * The additional per-thread data is stored at the beginning of the
 * task's stack.
 */
static inline Thread_t * prvGetThreadFromTask( TaskHandle_t xTask )
{
    StackType_t * pxTopOfStack = *( StackType_t ** ) xTask;

    return ( Thread_t * ) ( pxTopOfStack + 1 );
}

/*-----------------------------------------------------------*/

static pthread_once_t         hSigSetupThread = PTHREAD_ONCE_INIT;
static sigset_t               xResumeSignals;
static sigset_t               xAllSignals;
static sigset_t               xSchedulerOriginalSignalMask;
static pthread_t              hMainThread     = ( pthread_t ) NULL;
static volatile portBASE_TYPE uxCriticalNesting;
/*-----------------------------------------------------------*/

static portBASE_TYPE          xSchedulerEnd   = pdFALSE;
/*-----------------------------------------------------------*/

static void prvSetupSignalsAndSchedulerPolicy( void );
static void prvSetupTimerInterrupt( void );
static void * prvWaitForStart( void * pvParams );
static void prvSwitchThread( Thread_t * xThreadToResume,
                             Thread_t * xThreadToSuspend );
static void prvSuspendSelf( void );
static void prvResumeThread( pthread_t xThreadId );
static void vPortSystemTickHandler( int sig );
static void vPortStartFirstTask( void );
/*-----------------------------------------------------------*/

/*
 * The standard glibc malloc(), free() etc. take an internal lock so
 * it is not safe to switch tasks while calling them.
 *
 * Requiring the application use the safe xPortMalloc() and
 * vPortFree() is not sufficient as malloc() is used internally by
 * glibc (e.g., by strdup() and the pthread library.)
 *
 * To further complicate things malloc() and free() may be called
 * outside of task context during pthread destruction so using
 * vTaskSuspend() and xTaskResumeAll() cannot be used.
 * vPortEnterCritical() and vPortExitCritical() cannot be used either
 * as they use global state for the critical section nesting (this
 * cannot be fixed by using TLS as pthread destruction needs to free
 * the TLS).
 *
 * Explicitly save/disable and restore the signal mask to block the
 * timer (SIGALRM) and other signals.
 */

extern void * __libc_malloc( size_t );
extern void __libc_free( void * );
extern void * __libc_calloc( size_t,
                             size_t );
extern void * __libc_realloc( void * ptr,
                              size_t );

void * malloc( size_t size )
{
    sigset_t xSavedSignals;
    void *   ptr;

    pthread_sigmask( SIG_BLOCK, &xAllSignals, &xSavedSignals );
    ptr = __libc_malloc( size );
    pthread_sigmask( SIG_SETMASK, &xSavedSignals, NULL );

    return ptr;
}

void free( void * ptr )
{
    sigset_t xSavedSignals;

    pthread_sigmask( SIG_BLOCK, &xAllSignals, &xSavedSignals );
    __libc_free( ptr );
    pthread_sigmask( SIG_SETMASK, &xSavedSignals, NULL );
}

void * calloc( size_t nmemb,
               size_t size )
{
    sigset_t xSavedSignals;
    void *   ptr;

    pthread_sigmask( SIG_BLOCK, &xAllSignals, &xSavedSignals );
    ptr = __libc_calloc( nmemb, size );
    pthread_sigmask( SIG_SETMASK, &xSavedSignals, NULL );

    return ptr;
}

void * realloc( void * ptr,
                size_t size )
{
    sigset_t xSavedSignals;

    pthread_sigmask( SIG_BLOCK, &xAllSignals, &xSavedSignals );
    ptr = __libc_realloc( ptr, size );
    pthread_sigmask( SIG_SETMASK, &xSavedSignals, NULL );

    return ptr;
}

static void prvFatalError( const char * pcCall,
                           int iErrno )
{
    fprintf( stderr, "%s: %s\n", pcCall, strerror( iErrno ) );
    abort();
}

/*
 * See header file for description.
 */
portSTACK_TYPE * pxPortInitialiseStack( portSTACK_TYPE * pxTopOfStack,
                                        portSTACK_TYPE * pxEndOfStack,
                                        pdTASK_CODE pxCode,
                                        void * pvParameters )
{
    Thread_t *     thread;
    pthread_attr_t xThreadAttributes;
    size_t         ulStackSize;
    int            iRet;

    ( void ) pthread_once( &hSigSetupThread, prvSetupSignalsAndSchedulerPolicy );

    /*
     * Store the additional thread data at the start of the stack.
     */
    thread           = ( Thread_t * ) ( pxTopOfStack + 1 ) - 1;
    pxTopOfStack     = ( portSTACK_TYPE * ) thread - 1;
    ulStackSize      = ( pxTopOfStack + 1 - pxEndOfStack ) * sizeof( *pxTopOfStack );

    thread->pxCode   = pxCode;
    thread->pvParams = pvParameters;
    thread->xDying   = pdFALSE;

    pthread_attr_init( &xThreadAttributes );
    pthread_attr_setstack( &xThreadAttributes, pxEndOfStack, ulStackSize );

    vPortEnterCritical();

    iRet             = pthread_create( &thread->pthread, &xThreadAttributes,
                                       prvWaitForStart, thread );

    if( iRet )
    {
        prvFatalError( "pthread_create", iRet );
    }

    vPortExitCritical();

    return pxTopOfStack;
}
/*-----------------------------------------------------------*/

void vPortStartFirstTask( void )
{
    Thread_t * pxFirstThread = prvGetThreadFromTask( xTaskGetCurrentTaskHandle() );

    /* Start the first task. */
    prvResumeThread( pxFirstThread->pthread );
}
/*-----------------------------------------------------------*/

/*
 * See header file for description.
 */
portBASE_TYPE xPortStartScheduler( void )
{
    int      iSignal;
    sigset_t xSignals;

    hMainThread = pthread_self();

    /* Start the timer that generates the tick ISR.  Interrupts are disabled
     * here already. */
    prvSetupTimerInterrupt();

    /* Start the first task. */
    vPortStartFirstTask();

    /* Wait until signaled by vPortEndScheduler(). */
    sigemptyset( &xSignals );
    sigaddset( &xSignals, SIG_RESUME );

    while( !xSchedulerEnd )
    {
        sigwait( &xSignals, &iSignal );
    }

    /* Restore original signal mask. */
    ( void ) pthread_sigmask( SIG_SETMASK, &xSchedulerOriginalSignalMask, NULL );

    return 0;
}
/*-----------------------------------------------------------*/

void vPortEndScheduler( void )
{
    struct itimerval itimer;
    struct sigaction sigtick;

    /* Stop the timer and ignore any pending SIGALRMs that would end
     * up running on the main thread when it is resumed. */
    itimer.it_value.tv_sec     = 0;
    itimer.it_value.tv_usec    = 0;

    itimer.it_interval.tv_sec  = 0;
    itimer.it_interval.tv_usec = 0;
    ( void ) setitimer( ITIMER_REAL, &itimer, NULL );

    sigtick.sa_flags           = 0;
    sigtick.sa_handler         = SIG_IGN;
    sigemptyset( &sigtick.sa_mask );
    sigaction( SIGALRM, &sigtick, NULL );

    /* Signal the scheduler to exit its loop. */
    xSchedulerEnd              = pdTRUE;
    ( void ) pthread_kill( hMainThread, SIG_RESUME );

    prvSuspendSelf();
}
/*-----------------------------------------------------------*/

void vPortEnterCritical( void )
{
    if( uxCriticalNesting == 0 )
    {
        vPortDisableInterrupts();
    }

    uxCriticalNesting++;
}
/*-----------------------------------------------------------*/

void vPortExitCritical( void )
{
    uxCriticalNesting--;

    /* If we have reached 0 then re-enable the interrupts. */
    if( uxCriticalNesting == 0 )
    {
        vPortEnableInterrupts();
    }
}
/*-----------------------------------------------------------*/

void vPortYieldFromISR( void )
{
    Thread_t * xThreadToSuspend;
    Thread_t * xThreadToResume;

    xThreadToSuspend = prvGetThreadFromTask( xTaskGetCurrentTaskHandle() );

    vTaskSwitchContext();

    xThreadToResume  = prvGetThreadFromTask( xTaskGetCurrentTaskHandle() );

    prvSwitchThread( xThreadToResume, xThreadToSuspend );
}
/*-----------------------------------------------------------*/

void vPortYield( void )
{
    vPortEnterCritical();

    vPortYieldFromISR();

    vPortExitCritical();
}
/*-----------------------------------------------------------*/

void vPortDisableInterrupts( void )
{
    pthread_sigmask( SIG_BLOCK, &xAllSignals, NULL );
}
/*-----------------------------------------------------------*/

void vPortEnableInterrupts( void )
{
    pthread_sigmask( SIG_UNBLOCK, &xAllSignals, NULL );
}
/*-----------------------------------------------------------*/

portBASE_TYPE xPortSetInterruptMask( void )
{
    /* Interrupts are always disabled inside ISRs (signals
     * handlers). */
    return pdTRUE;
}
/*-----------------------------------------------------------*/

void vPortClearInterruptMask( portBASE_TYPE xMask )
{
}
/*-----------------------------------------------------------*/

static uint64_t prvGetTimeNs( void )
{
    struct timespec t;

    clock_gettime( CLOCK_MONOTONIC, &t );

    return t.tv_sec * 1000000000ull + t.tv_nsec;
}

static uint64_t prvStartTimeNs;
static uint64_t prvTickCount;

/*
 * Setup the systick timer to generate the tick interrupts at the required
 * frequency.
 */
void prvSetupTimerInterrupt( void )
{
    struct itimerval itimer;
    int              iRet;

    /* Initialise the structure with the current timer information. */
    iRet                       = getitimer( ITIMER_REAL, &itimer );

    if( iRet )
    {
        prvFatalError( "getitimer", errno );
    }

    /* Set the interval between timer events. */
    itimer.it_interval.tv_sec  = 0;
    itimer.it_interval.tv_usec = portTICK_RATE_MICROSECONDS;

    /* Set the current count-down. */
    itimer.it_value.tv_sec     = 0;
    itimer.it_value.tv_usec    = portTICK_RATE_MICROSECONDS;

    /* Set-up the timer interrupt. */
    iRet                       = setitimer( ITIMER_REAL, &itimer, NULL );

    if( iRet )
    {
        prvFatalError( "setitimer", errno );
    }

    prvStartTimeNs             = prvGetTimeNs();
}
/*-----------------------------------------------------------*/

static void vPortSystemTickHandler( int sig )
{
    Thread_t * pxThreadToSuspend;
    Thread_t * pxThreadToResume;
    uint64_t   xExpectedTicks;

    uxCriticalNesting++; /* Signals are blocked in this signal handler. */

    pxThreadToSuspend    = prvGetThreadFromTask( xTaskGetCurrentTaskHandle() );

    /* Tick Increment, accounting for any lost signals or drift in
     * the timer. */
    xExpectedTicks       = ( prvGetTimeNs() - prvStartTimeNs )
                           / ( portTICK_RATE_MICROSECONDS * 1000 );

    do
    {
        xTaskIncrementTick();
        prvTickCount++;
    } while( prvTickCount < xExpectedTicks );

    #if ( configUSE_PREEMPTION == 1 )
        /* Select Next Task. */
        vTaskSwitchContext();

        pxThreadToResume = prvGetThreadFromTask( xTaskGetCurrentTaskHandle() );

        prvSwitchThread( pxThreadToResume, pxThreadToSuspend );
    #endif

    uxCriticalNesting--;
}
/*-----------------------------------------------------------*/

void vPortThreadDying( void * pxTaskToDelete,
                       volatile BaseType_t * pxPendYield )
{
    Thread_t * pxThread = prvGetThreadFromTask( pxTaskToDelete );

    pxThread->xDying = pdTRUE;
}

void vPortCancelThread( void * pxTaskToDelete )
{
    Thread_t * pxThreadToCancel = prvGetThreadFromTask( pxTaskToDelete );

    /*
     * The thread has already been suspended so it can be safely
     * cancelled.
     */
    pthread_cancel( pxThreadToCancel->pthread );
    pthread_join( pxThreadToCancel->pthread, NULL );
}
/*-----------------------------------------------------------*/

static void * prvWaitForStart( void * pvParams )
{
    Thread_t * pxThread = pvParams;

    prvSuspendSelf();

    /* Resumed for the first time, unblocks all signals. */
    uxCriticalNesting = 0;
    vPortEnableInterrupts();

    /* Call the task's entry point. */
    pxThread->pxCode( pxThread->pvParams );

    return NULL;
}
/*-----------------------------------------------------------*/

static void prvSwitchThread( Thread_t * pxThreadToResume,
                             Thread_t * pxThreadToSuspend )
{
    BaseType_t uxSavedCriticalNesting;

    if( pxThreadToSuspend != pxThreadToResume )
    {
        /*
         * Switch tasks.
         *
         * The critical section nesting is per-task, so save it on the
         * stack of the current (suspending thread), restoring it when
         * we switch back to this task.
         */
        uxSavedCriticalNesting = uxCriticalNesting;

        prvResumeThread( pxThreadToResume->pthread );

        if( pxThreadToSuspend->xDying )
        {
            pthread_exit( NULL );
        }

        prvSuspendSelf();

        uxCriticalNesting      = uxSavedCriticalNesting;
    }
}
/*-----------------------------------------------------------*/

static void prvSuspendSelf( void )
{
    int iSig;

    /*
     * Suspend this thread by waiting for a SIG_RESUME signal.
     *
     * A suspended thread must not handle signals (interrupts) so
     * all signals must be blocked by calling this from:
     *
     * - Inside a critical section (vPortEnterCritical() /
     *   vPortExitCritical()).
     *
     * - From a signal handler that has all signals masked.
     *
     * - A thread with all signals blocked with pthread_sigmask().
     */
    sigwait( &xResumeSignals, &iSig );
}

/*-----------------------------------------------------------*/

static void prvResumeThread( pthread_t xThreadId )
{
    if( pthread_self() != xThreadId )
    {
        pthread_kill( xThreadId, SIG_RESUME );
    }
}
/*-----------------------------------------------------------*/

static void prvSetupSignalsAndSchedulerPolicy( void )
{
    struct sigaction sigresume, sigtick;
    int              iRet;

    hMainThread          = pthread_self();

    /* Initialise common signal masks. */
    sigemptyset( &xResumeSignals );
    sigaddset( &xResumeSignals, SIG_RESUME );
    sigfillset( &xAllSignals );

    /* Don't block SIGINT so this can be used to break into GDB while
     * in a critical section. */
    sigdelset( &xAllSignals, SIGINT );

    /*
     * Block all signals in this thread so all new threads
     * inherits this mask.
     *
     * When a thread is resumed for the first time, all signals
     * will be unblocked.
     */
    ( void ) pthread_sigmask( SIG_SETMASK, &xAllSignals,
                              &xSchedulerOriginalSignalMask );

    /* SIG_RESUME is only used with sigwait() so doesn't need a
     * handler. */
    sigresume.sa_flags   = 0;
    sigresume.sa_handler = SIG_IGN;
    sigfillset( &sigresume.sa_mask );

    sigtick.sa_flags     = 0;
    sigtick.sa_handler   = vPortSystemTickHandler;
    sigfillset( &sigtick.sa_mask );

    iRet                 = sigaction( SIG_RESUME, &sigresume, NULL );

    if( iRet )
    {
        prvFatalError( "sigaction", errno );
    }

    iRet                 = sigaction( SIGALRM, &sigtick, NULL );

    if( iRet )
    {
        prvFatalError( "sigaction", errno );
    }
}
/*-----------------------------------------------------------*/

unsigned long ulPortGetRunTime( void )
{
    struct tms xTimes;

    times( &xTimes );

    return ( unsigned long ) xTimes.tms_utime;
}
/*-----------------------------------------------------------*/