gemrtos_core.c

Go to the documentation of this file.

 
/******************************************************************************
*                                                                             *
* License Agreement                                                           *
* Copyright (c) Ricardo L. Cayssials                                          *
* All rights reserved.                                                        *
*                                                                             *
******************************************************************************/
 
#include <gemrtos_core.h>
 
OPTIMEZE_CODE(3)
 
// Check https://stackoverflow.com/questions/1154709/how-can-i-hide-the-declaration-of-a-struct-in-c
// https://gcc.gnu.org/onlinedocs/gcc/Nios-II-Options.html#index-mno-bypass-cache
// #pragma GCC optimize('mno-cache-volatile')
 
#include <inttypes.h>
 
GS_TCB *gk_TCB_GetFree(void)
{
    SAMPLE_FUNCTION_BEGIN(36)
    
    g_kcb.KCB_NUMBER_OF_TCBs++;
    // from https://stackoverflow.com/questions/227897/how-to-allocate-aligned-memory-only-using-the-standard-library/3430102#3430102
    void *mem = malloc(sizeof(GS_TCB) + 15); // Adding 15 to make sure there exist an address module 16 to align the block
    GS_TCB *ptcb = (GS_TCB  *) (((uintptr_t)mem+15) & ~ (uintptr_t)0x0F);
 
    ptcb->BLOCK_HASH           = (unsigned int) ptcb + 1;
    ptcb->malloc_address       = mem;
    // ptcb->TCBState             = G_TCBState_FREE; 
    ptcb->TCB_NextTCBAEL       = (struct gs_ecb *) 0; 
    ptcb->TCB_NextTCBASL       = (struct gs_scb *) 0; 
    ptcb->TCB_NextTCBPSL       = (struct gs_scb *) 0; 
    ptcb->TCBType              = G_TCBType_UNDEFINED; 
    ptcb->TCBReadyPriority     = G_LOWEST_PRIORITY -1; 
    ptcb->TCBInherPriority     = G_LOWEST_PRIORITY -1; 
    ptcb->TCBRunPriority       = G_LOWEST_PRIORITY -1; 
    ptcb->TCBCurrentPriority   = G_LOWEST_PRIORITY -1; 
    ptcb->TCBPeriod            = 0x7FFFFFFFFFFFFFFFLL -1; 
    ptcb->TCBDeadline          = 0x7FFFFFFFFFFFFFFFLL -1; 
    ptcb->TCB_StackPointer     = (GS_STK *) 0; 
    ptcb->TCB_StackBottom      = (GS_STK *) 0; 
    ptcb->TCB_StackTop         = (GS_STK *) 0; 
    ptcb->TCB_TaskCode         = (void *) 0; 
    ptcb->TCB_TaskArg          = (void   *) 0; 
    ptcb->TCB_NextTCB          = (struct gs_tcb *) 0; 
    ptcb->TCB_PrevTCB          = (struct gs_tcb *) 0; 
    ptcb->TCB_AssocPCB         = 0; 
    ptcb->TCB_INTNumber        = -1; 
    ptcb->TCB_RDY_LCB_Index    = (GS_LCB *) G_TASK_LCB_DEFAULT; 
    ptcb->TCB_Abort_w_Deadline = G_FALSE;
    ptcb->TCBState             = G_TCBState_UNLINKED;
    ptcb->TCB_NextISRTCB       = (struct gs_tcb *) 0;
    ptcb->TCB_PrevISRTCB       = (struct gs_tcb *) 0;
    
    
    ptcb->TCB_NEXT_TCBs = g_kcb.KCB_ROOT_TCBs;
    if (g_kcb.KCB_ROOT_TCBs != (struct gs_tcb *) 0) g_kcb.KCB_ROOT_TCBs->TCB_PREV_TCBs = ptcb;
    ptcb->TCB_PREV_TCBs = (struct gs_tcb *) 0;
    g_kcb.KCB_ROOT_TCBs = (struct gs_tcb *) ptcb;
 
 
#if G_DEBUG_WHILEFOREVER_ENABLE == 1
    if (TCB_IsValid(ptcb) != G_TRUE) G_DEBUG_WHILEFOREVER;
#endif
 
    SAMPLE_FUNCTION_END(36)
    return ptcb;
}
 
 
GS_ECB *gk_ECB_GetFree(void)
{
    SAMPLE_FUNCTION_BEGIN(1)
    
    g_kcb.KCB_NUMBER_OF_ECBs++;;
    // from https://stackoverflow.com/questions/227897/how-to-allocate-aligned-memory-only-using-the-standard-library/3430102#3430102    
    void *mem = malloc(sizeof(GS_ECB) + 15); // Adding 15 to make sure there exist an address module 16 to align the block
    GS_ECB  *pecb = (GS_ECB  *) (((uintptr_t)mem+15) & ~ (uintptr_t)0x0F);
 
    pecb->BLOCK_HASH     = (unsigned int) pecb + 2;
    pecb->malloc_address = mem;    
    // pecb->ECBState       = GS_ECBState_FREE; 
    pecb->ECB_NextECBAEL = (struct gs_ecb *) 0; 
    pecb->ECB_NextECBASL = (struct gs_scb *) 0; 
    pecb->ECBType        = G_ECBType_NOT_SPECIFIED; 
    // pecb->ECBValue.i64   = G_LOWEST_PRIORITY -1; 
    pecb->ECB_NextECB    = (struct gs_ecb *) 0; 
    pecb->ECB_PrevECB    = (struct gs_ecb *) 0; 
    pecb->ECB_AssocTCB   = (struct gs_tcb *) 0; 
    pecb->ECB_AssocRCB   = (struct g_rcb *) 0; 
    pecb->ECB_NextTCBAEL = (struct gs_ecb *) 0; 
    pecb->ECB_PrevTCBAEL = (struct gs_ecb *) 0; 
    pecb->ECB_RRDS       = (struct gs_rrds *) 0;
    pecb->ECBValue.i64   = G_LOWEST_PRIORITY;
    pecb->ECBState       = GS_ECBState_UNLINKED;
    
    pecb->ECB_NEXT_ECBs = g_kcb.KCB_ROOT_ECBs;
    if (g_kcb.KCB_ROOT_ECBs != (struct gs_ecb *) 0) g_kcb.KCB_ROOT_ECBs->ECB_PREV_ECBs = pecb;
    pecb->ECB_PREV_ECBs = (struct gs_ecb *) 0;
    g_kcb.KCB_ROOT_ECBs = (struct gs_ecb *) pecb;    
    
    SAMPLE_FUNCTION_END(1)
 
#if G_DEBUG_WHILEFOREVER_ENABLE == 1
    if (pecb == (struct gs_ecb *) 0) G_DEBUG_WHILEFOREVER;
#endif
    
    return pecb;
}  
 
G_RCB *gk_RCB_GetFree(void)
{
    SAMPLE_FUNCTION_BEGIN(19)
    
    g_kcb.KCB_NUMBER_OF_RCBs++;;
    void *mem = malloc(sizeof(G_RCB) + 15); // Adding 15 to make sure there exist an address module 16 to align the block
    G_RCB  *prcb = (G_RCB  *) (((uintptr_t)mem+15) & ~ (uintptr_t)0x0F);
    // from https://stackoverflow.com/questions/227897/how-to-allocate-aligned-memory-only-using-the-standard-library/3430102#3430102    
 
    prcb->BLOCK_HASH     = (unsigned int) prcb + 3;
    prcb->malloc_address = mem;  
    // prcb->RCBType        = GK_RCBType_FREE; 
    prcb->RCBState       = GK_RCBState_UNDEFINED; 
    prcb->RCBCount       = (INT32) 0; 
    prcb->RCB_NextRCBASL = (struct gs_scb *) 0; 
    prcb->RCB_NextRCBGEL = (struct gs_ecb *) 0; 
    prcb->RCB_NextRCBWEL = (struct gs_ecb *) 0;
    prcb->RCBType = GK_RCBType_UNUSED;
    
    prcb->RCB_NEXT_RCBs = (G_RCB *) g_kcb.KCB_ROOT_RCBs;
    if (g_kcb.KCB_ROOT_RCBs != (struct g_rcb *) 0) {
        G_RCB *presource = (G_RCB *) g_kcb.KCB_ROOT_RCBs;
        presource->RCB_PREV_RCBs = (G_RCB *) prcb;
    }
    prcb->RCB_PREV_RCBs = (G_RCB *) 0;
    g_kcb.KCB_ROOT_RCBs = (struct g_rcb *) prcb;    
    
    SAMPLE_FUNCTION_END(19)
 
    return prcb;
}
 
GS_SCB *gk_SCB_GetFree(void)
{
    SAMPLE_FUNCTION_BEGIN(26)
    g_kcb.KCB_NUMBER_OF_SCBs++;
    
    void *mem = malloc(sizeof(GS_SCB) + 15); // Adding 15 to make sure there exist an address module 16 to align the block
    GS_SCB *pscb = (GS_SCB  *) (((uintptr_t)mem+15) & ~ (uintptr_t)0x0F);
    
    pscb->BLOCK_HASH     = (unsigned int) pscb + 4;
    pscb->malloc_address = mem;  
    pscb->SCBState       = G_SCBState_FREE;
    pscb->SCBState       = G_SCBState_UNLINKED;
    
    pscb->SCB_NEXT_SCBs = g_kcb.KCB_ROOT_SCBs;
    if (g_kcb.KCB_ROOT_SCBs != (struct gs_scb *) 0) g_kcb.KCB_ROOT_SCBs->SCB_PREV_SCBs = pscb;
    pscb->SCB_PREV_SCBs = (struct gs_scb *) 0;
    g_kcb.KCB_ROOT_SCBs = (struct gs_scb *) pscb;
 
    
    SAMPLE_FUNCTION_END(26)
    
#if G_DEBUG_WHILEFOREVER_ENABLE == 1
    if (pscb == (struct gs_scb *) 0) G_DEBUG_WHILEFOREVER;
#endif    
    
    return pscb;
}    
  
GS_RRDS *gk_RRDS_GetFree(void)
{
    SAMPLE_FUNCTION_BEGIN(48)
    g_kcb.KCB_NUMBER_OF_RRDSs++;
    void *mem = malloc(sizeof(GS_RRDS) + 15); // Adding 15 to make sure there exist an address module 16 to align the block
    GS_RRDS  *prrds = (GS_RRDS  *) (((uintptr_t)mem+15) & ~ (uintptr_t)0x0F);
    
    prrds->BLOCK_HASH      = (unsigned int) prrds + 5;
    prrds->malloc_address  = mem;
    
    prrds->RRDS_State = 0;           
    prrds->RRDS_Type = 0;            
    prrds->RRDS_NextRRDS = (struct gs_rrds *) 0;
    prrds->RRDS_NextSCB  = (struct gs_scb  *) 0;
    prrds->RRDS_AsocECB  = (struct gs_ecb  *) 0;    
    
    prrds->RRDS_NEXT_RRDSs = g_kcb.KCB_ROOT_RRDSs;
    if (g_kcb.KCB_ROOT_RRDSs != (struct gs_rrds *) 0) g_kcb.KCB_ROOT_RRDSs->RRDS_PREV_RRDSs = prrds;
    prrds->RRDS_PREV_RRDSs = (struct gs_rrds *) 0;
    g_kcb.KCB_ROOT_RRDSs = (struct gs_rrds *) prrds;    
    
    SAMPLE_FUNCTION_END(48)
    return prrds;
}
 
GS_LCB *gk_Get_LCB(void)
{
    g_kcb.KCB_NUMBER_OF_LCBs++;
    void *mem = malloc(sizeof(GS_LCB) + 15); // Adding 15 to make sure there exist an address module 16 to align the block
    GS_LCB *plcb = (GS_LCB  *) (((uintptr_t)mem+15) & ~ (uintptr_t)0x0F);    
 
    plcb->BLOCK_HASH      = (unsigned int) plcb + 6;
    plcb->malloc_address  = mem;    
    
    plcb->LCB_NextTCBRUNL = (struct gs_tcb *) 0;        /* Pointer to the TCB list of running tasks */
    plcb->LCBRunPriority  = (INT64) G_LOWEST_PRIORITY;  /* It is because it is empty*/
    plcb->LCB_NextTCBRDYL = (struct gs_tcb *) 0;       /* Pointer to the TCB of the Highest Priority Task */
    plcb->LCB_NextLCBFPL  = (struct gs_pcb *) 0;       
    plcb->LCBState        = GS_LCBState_UNLINKED;
    plcb->LCBType         = GS_LCBType_UNSPECIFIED;
 
    plcb->LCB_NEXT_LCBs = g_kcb.KCB_ROOT_LCBs;
    if (g_kcb.KCB_ROOT_LCBs != (struct gs_lcb *) 0) g_kcb.KCB_ROOT_LCBs->LCB_PREV_LCBs = plcb;
    plcb->LCB_PREV_LCBs = (struct gs_lcb *) 0;
    g_kcb.KCB_ROOT_LCBs = (struct gs_lcb *) plcb;
 
    
    // Link to the LCBL linked list
    gk_LCBL_Link((GS_LCB *) plcb);
 
    return plcb;
}
 
 
INT32 gk_Create_PCBs(int Nmbr_PCB)
{
    g_kcb.KCB_NUMBER_OF_PCBs = Nmbr_PCB;
    // from https://stackoverflow.com/questions/227897/how-to-allocate-aligned-memory-only-using-the-standard-library/3430102#3430102
    void *mem = malloc((Nmbr_PCB) * sizeof(GS_PCB) + 15); // Adding 15 to make sure there exist an address module 16 to align the block
    g_kcb.G_PCBTbl = (GS_PCB  *) (((uintptr_t)mem+15) & ~ (uintptr_t)0x0F);
#if G_DEBUG_WHILEFOREVER_ENABLE == 1    
    fprintf(stderr,"[ MESSAGE ] mem = %p, g_kcb.G_PCBTbl = %p, size = %d\n", (void *) mem, (void *) g_kcb.G_PCBTbl, (int) G_NUMBER_OF_PCB * sizeof(GS_PCB));
#endif
    return G_TRUE;
}
 
GS_TCB *gk_CreateTask(void *TaskCode,              
                     void *p_arg,                  
                     void *StkBotton,              
                     unsigned int StkSize,         
                     unsigned int TCBType,         
                     INT64 TCBReadyPriority,       
                     INT64 TCBRunPriority,         
                     INT64 TCBDeadline,            
                     INT64 TCBPeriod,              
                     GS_LCB *TCB_RDY_LCB_Index,        
                     int TCB_Abort_w_Deadline,     
                     INT64 TCBInherPriority,
                     int TCB_INTNumber)       
{
    GS_TCB  *ptcb;
    
    ptcb = gk_TCB_GetFree();
    // Initialize the gs_tcb structure of the task
    if (ptcb != (struct gs_tcb *) 0) {
        ptcb->TCB_StackBottom      = (GS_STK *) ((((INT32) StkBotton)-8)& ~0x3); 
        ptcb->TCB_StackTop         = (GS_STK *)((INT32) ptcb->TCB_StackBottom - ((INT32)(StkSize)& ~0x3)); 
        ptcb->TCB_TaskCode         = TaskCode;           
        ptcb->TCB_TaskArg          = p_arg;              
 
        ptcb->TCBType              = TCBType;      
        ptcb->TCBReadyPriority     = TCBReadyPriority;  
        ptcb->TCBRunPriority       = TCBRunPriority;  
        ptcb->TCBDeadline          = TCBDeadline;    
        ptcb->TCBPeriod            = TCBPeriod;    
        ptcb->TCB_RDY_LCB_Index    = TCB_RDY_LCB_Index;       
        ptcb->TCB_Abort_w_Deadline = TCB_Abort_w_Deadline;   
        ptcb->TCBInherPriority     = TCBInherPriority;  /* Load task priority into TCB              */
        
        // Initilize the stack of the task
        gk_TASK_STK_Init(ptcb);
        
        // Set the number of associated interrupt (valid only when TCBType == G_TCBType_ISR)
        // and link to the ISR linked list
        ptcb->TCB_INTNumber = TCB_INTNumber;
        ptcb->TCB_PrevISRTCB = (struct gs_tcb *) 0;
        if (TCBType == G_TCBType_ISR) {
            ptcb->TCB_NextISRTCB = g_kcb.KCB_ExtISR[TCB_INTNumber].G_TCB_ISR;
            if (g_kcb.KCB_ExtISR[TCB_INTNumber].G_TCB_ISR != (struct gs_tcb *) 0) {
                g_kcb.KCB_ExtISR[TCB_INTNumber].G_TCB_ISR->TCB_PrevISRTCB = (struct gs_tcb *) ptcb;
            }
            g_kcb.KCB_ExtISR[TCB_INTNumber].G_TCB_ISR = (struct gs_tcb *) ptcb;
        } else {
            ptcb->TCB_NextISRTCB = (struct gs_tcb *) 0;
        }
    }    
 
    return (ptcb);
}
 
 
void  gk_INIT_IRQ (void)
{
    int i; PRINT_DEBUG_LINE
    G_DEBUG_VERBOSE; PRINT_DEBUG_LINE // Print Debug information
    for (i=0 ;  i<ALT_NIRQ ;  i++)
    {
        if ((G_IRQ_ENABLED >> i) & 1) {
 
            g_kcb.KCB_ExtISR[i].G_TCB_ISR = gk_CreateTask((void *) alt_irq[i].handler,  
                                 (void *) alt_irq[i].context,               
                                 // (void *) &g_kcb.G_ISR_STACK[i][G_ISR_STACKSIZE-4],              ///< Botton of the Stack of the Task
                                 (void *) g_kcb.G_ISR_STACK + (i * G_ISR_STACKSIZE) + G_ISR_STACKSIZE-4,              
                                 (unsigned int) G_ISR_STACKSIZE,         
                                 (unsigned int) G_TCBType_ISR,         
                                 (INT64) G_ISR_PRIORITY,       
                                 (INT64) G_ISR_PRIORITY,         
                                 (INT64) G_TASK_PERIOD_DEFAULT,            
                                 (INT64) G_TASK_PERIOD_DEFAULT,              
                                 (GS_LCB *) G_TASK_LCB_DEFAULT,        
                                 (int) G_TRUE,     
                                 (INT64) G_ISR_PRIORITY,
                                 (int) i);  
                                     
            // g_kcb.KCB_ExtISR[i].G_TCB_ISR->TCB_INTNumber = i; 
            
            if ((G_IRQ_PENDING >> i) & 1) {                
                /* The interrupt was pending then insert in ready list and disable it */
                GRTOS_CMD_IRQ_ENB_CLR(i); 
                g_kcb.KCB_ExtISR[i].G_EXT_INT_Count++; // = (int) 1;
                gk_TCBRDYL_Link(g_kcb.KCB_ExtISR[i].G_TCB_ISR);   /* Insert Task in Ready List                */
            } else {
                gk_TCBWL_Link(g_kcb.KCB_ExtISR[i].G_TCB_ISR, G_TCBState_WAITING_COMPLETED); 
                // g_kcb.KCB_ExtISR[i].G_EXT_INT_Count = (int) 0;
                GRTOS_CMD_IRQ_ENB_SET(i); 
            }
        }
    }    
}
 
INT32 gk_ISR_COMPLETE (GS_TCB *ptcb) {
    
    int irq_nbr = ptcb->TCB_INTNumber;
    
#if G_DEBUG_WHILEFOREVER_ENABLE == 1
    if (g_kcb.KCB_ExtISR[irq_nbr].G_EXT_INT_Count == (int) 0) G_DEBUG_WHILEFOREVER; PRINT_DEBUG_LINE
#endif    
    
    if (ptcb->TCBType == G_TCBType_ISR) g_kcb.KCB_ExtISR[irq_nbr].G_EXT_INT_Count--;
    if (g_kcb.KCB_ExtISR[irq_nbr].G_EXT_INT_Count == (INT32) 0) {
        GRTOS_CMD_IRQ_ENB_SET(irq_nbr);
    }
    
    return G_TRUE;
}
 
void gk_ISR_RELEASE (int irq_nbr) {
 
    GS_TCB *ptcb = g_kcb.KCB_ExtISR[irq_nbr].G_TCB_ISR;
    
    GRTOS_CMD_IRQ_ENB_CLR(irq_nbr); PRINT_DEBUG_LINE
    
    while (ptcb != (GS_TCB *) 0) {
        // !!!! verificar si esta en estado waiting
        /* Limpio el registro de interrupcion porque ya fue activada  */
        if (ptcb->TCBType == G_TCBType_ISR) {
            if (ptcb->TCBState == G_TCBState_WAITING_COMPLETED) {     // if task is waiting
                /* Insert Task in Ready List                */                
                gk_TCB_Unlink((GS_TCB *) ptcb); 
                gk_TASK_STK_Init((GS_TCB *) ptcb); 
                gk_TCBRDYL_Link((GS_TCB *) ptcb); 
                
                g_kcb.KCB_ExtISR[irq_nbr].G_EXT_INT_Count++;
            }
        }
        ptcb = (GS_TCB *) ptcb->TCB_NextISRTCB;
    }
 
}
 
INT32 gu_SetTaskISR(struct gs_tcb *ptcb, unsigned int irq_nbr)
{
    if (G_Running == G_TRUE) GRTOS_USER_CRITICAL_SECTION_GET;
        gk_SetTaskISR(ptcb, irq_nbr);
    if (G_Running == G_TRUE) GRTOS_USER_CRITICAL_SECTION_RELEASE; 
    return G_TRUE;
}
 
INT32 gk_SetTaskISR(struct gs_tcb *ptcb, unsigned int irq_nbr)
{
    // Set the number of associated interrupt (valid only when TCBType == G_TCBType_ISR)
    // and link to the ISR linked list !!! check if task is running for consistency of Count
    ptcb->TCBType = G_TCBType_ISR;
    ptcb->TCB_INTNumber = irq_nbr;
    ptcb->TCB_PrevISRTCB = (struct gs_tcb *) 0;
    ptcb->TCB_NextISRTCB = g_kcb.KCB_ExtISR[irq_nbr].G_TCB_ISR;
    if (g_kcb.KCB_ExtISR[irq_nbr].G_TCB_ISR != (struct gs_tcb *) 0) {
        g_kcb.KCB_ExtISR[irq_nbr].G_TCB_ISR->TCB_PrevISRTCB = (struct gs_tcb *) ptcb;
    }
    g_kcb.KCB_ExtISR[irq_nbr].G_TCB_ISR = (struct gs_tcb *) ptcb;
    
    if (ptcb->TCBState != G_TCBState_WAITING_COMPLETED) {
        g_kcb.KCB_ExtISR[irq_nbr].G_EXT_INT_Count++;
    }
    
    return G_TRUE;
}
 
 
OPTIMEZE_RESTORE