linuxinitialize.cpp

/* Copyright 2013-2016 The MathWorks, Inc. */


/* ---------------------------- */
/* RTOS-specific headers        */
/* Note: must be included first */
/* ---------------------------- */
#include "linuxinitialize.h"

/* ---------------------------- */
/* Required Coder Target header */
/* ---------------------------- */
#include "MW_custom_RTOS_header.h"

/* ---------------------------- */
/* RTOS-specific declarations   */
/* ---------------------------- */
typedef struct {
    double period;
} baseRateInfo_t;

pthread_attr_t attr;
baseRateInfo_t info;
struct sched_param sp;

/* MW_NUM_SUBRATES is set to 0 if we are in single-tasking mode or number of subrates are 0 */
#define MW_SP_SCHED_FIFO   ((MW_NUMBER_SUBRATES > 0) || !defined(MW_SCHED_OTHER))
#ifdef MW_RTOS_DEBUG
    #define MW_DEBUG_LOG(str)  printf(str); fflush(stdout)
#else
    #define MW_DEBUG_LOG(str)
#endif

#ifdef MW_HAS_TARGET_SERVICES
extern int makeCSTaskIdle();
#endif

/* ---------------------------- */
/* Internally visible functions */
/* ---------------------------- */
static int createTimer(double periodInSeconds)
{
    int status;
    int fd;
    struct itimerspec its;

    /* Create the timer */
    fd = timerfd_create(CLOCK_MONOTONIC, 0);
    if (fd == -1) {
        fprintf(stderr, "Call to timerfd_create failed.\n"); 
        perror("timerfd_create");
        fflush(stderr); 
        exit(EXIT_FAILURE);
    }

    /* Make the timer periodic */
    its.it_value.tv_sec = (time_t)periodInSeconds;
    its.it_value.tv_nsec = (periodInSeconds - (time_t)periodInSeconds) * 1000000000;
    its.it_interval.tv_sec = its.it_value.tv_sec;
    its.it_interval.tv_nsec = its.it_value.tv_nsec;
    status = timerfd_settime(fd, 0, &its, NULL);
    CHECK_STATUS(status, 0, "timer_settime");
    
    return fd;
}

static void waitForTimerEvent(int fd)
{
    unsigned long long missed;
    int status;

    /* Wait for the next timer event. If we have missed any the
       number is written to "missed" */
    while ((status = read(fd, &missed, sizeof(missed)) == -1) && (errno == EINTR)) {
        /* Restart if interrupted by a signal */
        continue;
    }
    if (status == -1) {
        perror("read(timerfd)");
    }
}

void *schedulerTask(void* arg)
{
    int fd;
    baseRateInfo_t info = *((baseRateInfo_t *)arg);

    MW_DEBUG_LOG("schedulerTask entered\n");
    fd = createTimer(info.period);
    while(1) {
        waitForTimerEvent(fd);
#ifdef DETECT_OVERRUNS          
        testForRateOverrun(0);
#endif
        sem_post(&baserateTaskSem);    
    }
}

/* Should use this fcn, but currently are not using it */
/* Why: it is safe ??? from interruption */
void my_sem_wait(sem_t *sem)
{
    int status;
    while (((status = sem_wait(sem)) == -1) && (errno == EINTR)) {
        /* Restart if interrupted by a signal */
        continue;
    }
    CHECK_STATUS(status, 0, "my_sem_wait");
}

static void setThreadPriority(const int priority, pthread_attr_t *attr, struct sched_param *sp)
{
#if MW_SP_SCHED_FIFO
    int status;
    
    sp->sched_priority = priority;
    status = pthread_attr_setschedparam(attr, sp);
    CHECK_STATUS(status, 0, "pthread_attr_setschedparam");
#endif
}

/* ---------------------------- */
/* Externally visible functions */
/* ---------------------------- */
void myAddBlockForThisEvent(int sigNo)
{
    int status;
    sigset_t sigMask;

    sigemptyset(&sigMask);
    sigaddset(&sigMask, sigNo);
    status = pthread_sigmask(SIG_BLOCK, &sigMask, NULL);
    CHECK_STATUS(status, 0, "pthread_sigmask");
}

void myAddHandlerForThisEvent(int sigNo, int sigToBlock[], int numSigToBlock, void (*sigHandler)(int))
{
    int idx;
    int status;
    struct sigaction sa;

    sa.sa_handler = (__sighandler_t) sigHandler;
    sigemptyset(&sa.sa_mask);
    for (idx=0; idx<numSigToBlock; idx++) {
        sigaddset(&sa.sa_mask, sigToBlock[idx]);
    }
    sa.sa_flags = SA_RESTART; /* Restart functions if interrupted by handler */
    status = sigaction(sigNo, &sa, NULL);
    CHECK_STATUS_NOT(status, -1, "sigaction to register a signal handler");
}

void myRestoreDefaultHandlerForThisEvent(int sigNo)
{
    int status;
    struct sigaction sa;

    sa.sa_handler = SIG_DFL;
    sigemptyset(&sa.sa_mask);
    sa.sa_flags = SA_RESTART; /* Restart functions if interrupted by handler */
    status = sigaction(sigNo, &sa, NULL);
    CHECK_STATUS_NOT(status, -1, "sigaction to restore default signal handler");
}

void myRTOSInit(double baseRatePeriod, int numSubrates)
{
    int i;
    int status;
    uid_t euid;
    size_t stackSize;
    unsigned long cpuMask = 0x1;
    unsigned int len = sizeof(cpuMask);

    UNUSED(baseRatePeriod);
    UNUSED(numSubrates);
    
    if (!MW_IS_CONCURRENT) {
        /* All threads created by this process will run on a single CPU */
        status = sched_setaffinity(0, len, (cpu_set_t *) &cpuMask);
        CHECK_STATUS(status, 0, "sched_setaffinity");
    }

#if MW_SP_SCHED_FIFO && !defined (_POSIX_THREAD_PRIORITY_SCHEDULING)
    fprintf(stderr, "Priority scheduling is NOT supported by your system.\n");
    fprintf(stderr, "The generated code will not run correctly because your\n");
    fprintf(stderr, "model contains multiple rates and uses multi-tasking\n");
    fprintf(stderr, "code generation mode. You can only run the generated code\n");
    fprintf(stderr, "in single-tasking mode in your system. Open\n");
    fprintf(stderr, "Simulation -> Configuration Parameters -> Solver dialog\n");
    fprintf(stderr, "and set \"Tasking mode for periodic sample times\" parameter to SingleTasking.\n");
    fprintf(stderr, "Re-build the Simulink model with the new settings and try executing the generated code again.\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
#endif

#if MW_SP_SCHED_FIFO
    /* Need root privileges for real-time scheduling */
    euid = geteuid();
    if (euid != 0) {
        fprintf(stderr, "You must have root privileges to run the generated code because\n");
        fprintf(stderr, "generated code requires SCHED_FIFO scheduling class to run correctly.\n");
        fprintf(stderr, "Try running the executable with the following command: sudo ./<executable name>\n");
        fflush(stderr);
        exit(EXIT_FAILURE);
    }
#endif

    status = sem_init(&baserateTaskSem, 0, 0);
    CHECK_STATUS(status, 0, "sem_init:baserateTaskSemSem");
    status = sem_init(&stopSem, 0, 0);
    CHECK_STATUS(status, 0, "sem_init:stopSem");
   
#if MW_SP_SCHED_FIFO
    /* Set scheduling policy of the main thread to SCHED_FIFO */
    sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
    status = sched_setscheduler(0, SCHED_FIFO, &sp);
    CHECK_STATUS(status, 0, "sched_setscheduler");
#endif

    /* Create threads executing the Simulink model */
    pthread_attr_init(&attr);
    status = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    CHECK_STATUS(status, 0, "pthread_attr_setinheritsched");
#if MW_SP_SCHED_FIFO
    status = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
#else
    status = pthread_attr_setschedpolicy(&attr, SCHED_OTHER);
#endif
    CHECK_STATUS(status, 0, "pthread_attr_setschedpolicy");
    status = pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
    CHECK_STATUS(status, 0, "pthread_attr_setdetachstate");

    /* Set thread stack size if necessary */
    status = pthread_attr_getstacksize(&attr, &stackSize);
    CHECK_STATUS(status, 0, "pthread_attr_getstacksize");
    if (stackSize < STACK_SIZE) {
        /* Make sure that stackSize is a multiple of 8 */
        stackSize = (STACK_SIZE + 7) & (~0x7);
        pthread_attr_setstacksize(&attr, stackSize);
        CHECK_STATUS(status, 0, "pthread_attr_setstacksize");
    }

    signal(SIGTERM, exitFcn);     /* kill */
    signal(SIGHUP, exitFcn);      /* kill -HUP */
    signal(SIGINT, exitFcn);      /* Interrupt from keyboard */
    signal(SIGQUIT, exitFcn);     /* Quit from keyboard */

#ifdef MW_STANDALONE_EXECUTION_PROFILER_ON
    status = pthread_mutex_init(&profilingDataStoreMutex, NULL);    
#endif
    
#ifdef MW_HAS_MULTIPLE_RATES
    MW_DEBUG_LOG("**creating subrate task threads**\n");   
    for (i = 0; i < MW_NUMBER_SUBRATES; i++) {
        taskId[i] = i;
        status = sem_init(&subrateTaskSem[i], 0, 0);
        CHECK_STATUS(status, 0, "sem_init");
        setThreadPriority(subratePriority[i], &attr, &sp);
        status = pthread_create(&subRateThread[i], &attr, (void *) subrateTask, (void *)&taskId[i]);
        CHECK_STATUS(status, 0, "pthread_create");
#ifdef DETECT_OVERRUNS
        status = pthread_mutex_init(&rateTaskFcnRunningMutex[i+1], NULL);
        CHECK_STATUS(status, 0, "pthread_mutex_init");
#endif 
#ifdef COREAFFINITYREQUIRED
        if (coreAffinity[i] >= 0) {
             cpu_set_t cpuset;
             CPU_ZERO(&cpuset);
             CPU_SET(coreAffinity[i], &cpuset);
             ret = pthread_setaffinity_np(subRateThread[i], sizeof(cpu_set_t), &cpuset);
             CHECK_STATUS(ret, "pthread_setaffinity_np");
         }
#endif
    }
#endif

    MW_DEBUG_LOG("**creating the base rate task thread**\n");    
    setThreadPriority(MW_BASERATE_PRIORITY, &attr, &sp);  
    status = pthread_create(&baseRateThread, &attr, &baseRateTask, NULL);
    CHECK_STATUS(status, 0, "pthread_create");

#ifdef DETECT_OVERRUNS
    status = pthread_mutex_init(&rateTaskFcnRunningMutex[0], NULL);
    CHECK_STATUS(status, 0, "pthread_mutex_init");
#endif

    MW_DEBUG_LOG("**creating the scheduler thread**\n");
    /* Set the priority higher (higher number) than the base rate */    
    setThreadPriority(MW_BASERATE_PRIORITY + 1, &attr, &sp);
    info.period = MW_BASERATE_PERIOD;
    status = pthread_create(&schedulerThread, &attr, &schedulerTask, (void *) &info);
    CHECK_STATUS(status, 0, "pthread_create");

#ifdef MW_HAS_APERIODIC_TASKS
    MW_DEBUG_LOG("**creating asynchronously triggered task threads**\n");
    /* Set the priority higher (higher number) than the base rate */
    sp.sched_priority = MW_BASERATE_PRIORITY + 1;
    for (i = 0; i < MW_NUMBER_APERIODIC_TASKS; i++) {
        status = pthread_create(&asyncThread[i], &attr, (void *) pAsyncTasks[i], NULL);
        CHECK_STATUS(status, 0, "pthread_create");
    }
#endif

#ifdef MW_NEEDS_BACKGROUND_TASK
    MW_DEBUG_LOG("**creating the background thread**\n");
    status = pthread_attr_setschedpolicy(&attr, SCHED_OTHER);
    CHECK_STATUS(status, 0, "pthread_attr_setschedpolicy");
    setThreadPriority(0, &attr, &sp);
    status = pthread_create(&backgroundThread, &attr, (void *)backgroundTask, NULL);
    CHECK_STATUS(status, 0, "pthread_create");
#if MW_SP_SCHED_FIFO == 0
    status = pthread_setschedparam(backgroundThread, SCHED_IDLE, &sp);
    CHECK_STATUS(status, 0, "pthread_setschedparam");
#ifdef MW_HAS_TARGET_SERVICES
    status = makeCSTaskIdle();
    CHECK_STATUS(status, 0, "pthread_setschedparam");
#endif 
#endif
#endif

    pthread_attr_destroy(&attr);
    fflush(stdout);
}