gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    micro T-Kernel 3.00.03
    4:  *
    5:  *    Copyright (C) 2006-2021 by Ken Sakamura.
    6:  *    This software is distributed under the T-License 2.2.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by TRON Forum(http://www.tron.org) at 2021/03/31.
   10:  *
   11:  *----------------------------------------------------------------------
   12:  */
   13: 
   14: #include <sys/machine.h>
   15: #ifdef CPU_CORE_ARMV7M
   16: /*
   17:  *      cpu_cntl.c (ARMv7-M)
   18:  *      CPU-Dependent Control
   19:  */
   20: #include "kernel.h"
   21: #include "../../../sysdepend.h"
   22: 
   23: #include "cpu_task.h"
   24: 
   25: 
   26: /* Temporal stack used when 'dispatch_to_schedtsk' is called */
   27: Noinit(EXPORT UB knl_tmp_stack[TMP_STACK_SIZE]);
   28: 
   29: /* Task independent status */
   30: EXPORT  W        knl_taskindp = 0;
   31: 
   32: /* ------------------------------------------------------------------------ */
   33: /*
   34:  * Set task register contents (Used in tk_set_reg())
   35:  */
   36: EXPORT void knl_set_reg( TCB *tcb, CONST T_REGS *regs, CONST T_EIT *eit, CONST T_CREGS *cregs )
   37: {
   38:         SStackFrame    *ssp;
   39:         INT    i;
   40: 
   41: #if USE_FPU
   42:         UW             *tmpp;
   43: 
   44:         tmpp = (UW*)(( cregs != NULL )? cregs->ssp: tcb->tskctxb.ssp);
   45:         if(tcb->tskatr & TA_FPU) {
   46:                 if(*tmpp & EXPRN_NO_FPU) {    /* FPU register is not saved */
   47:                         ssp = (SStackFrame*)tmpp;
   48:                 } else {              /* FPU register is saved */
   49:                         ssp = (SStackFrame*)(((SStackFrame_wFPU*)tmpp)->r_);
   50:                 }
   51:         } else {
   52:                 ssp = (SStackFrame*)tmpp;
   53:         }
   54: #else
   55:         ssp = (SStackFrame*)(( cregs != NULL )? cregs->ssp: tcb->tskctxb.ssp);
   56: #endif
   57:         
   58:         if ( regs != NULL ) {
   59:                 for ( i = 0; i < 4; ++i ) {
   60:                         ssp->r[i] = regs->r[i];
   61:                 }
   62:                 for ( i = 4; i < 12; ++i){
   63:                         ssp->r_[i - 4] = regs->r[i];
   64:                 }
   65:         }
   66: 
   67:         if ( eit != NULL ) {
   68:                 ssp->pc = eit->pc;
   69:         }
   70: 
   71:         if ( cregs != NULL ) {
   72:                 tcb->tskctxb.ssp  = cregs->ssp;
   73:         }
   74: }
   75: 
   76: 
   77: /* ------------------------------------------------------------------------ */
   78: /*
   79:  * Get task register contents (Used in tk_get_reg())
   80:  */
   81: EXPORT void knl_get_reg( TCB *tcb, T_REGS *regs, T_EIT *eit, T_CREGS *cregs )
   82: {
   83:         SStackFrame    *ssp;
   84:         INT            i;
   85: 
   86: #if USE_FPU
   87:         UW             *tmpp;
   88: 
   89:         tmpp = (UW*)tcb->tskctxb.ssp;
   90:         if(tcb->tskatr & TA_FPU) {
   91:                 if(*tmpp & EXPRN_NO_FPU) {    /* FPU register is not saved */
   92:                         ssp = (SStackFrame*)tmpp;
   93:                 } else {              /* FPU register is saved */
   94:                         ssp = (SStackFrame*)&(((SStackFrame_wFPU*)tmpp)->exp_ret);
   95:                 }
   96:         } else {
   97:                 ssp = (SStackFrame*)tmpp;
   98:         }
   99: #else
  100:         ssp = (SStackFrame*)tcb->tskctxb.ssp;
  101: #endif
  102: 
  103:         if ( regs != NULL ) {
  104:                 for ( i = 0; i < 4; ++i ) {
  105:                         regs->r[i] = ssp->r[i];
  106:                 }
  107:                 for ( i = 4; i < 12; ++i ){
  108:                         regs->r[i] = ssp->r_[i - 4];
  109:                 }
  110:                 regs->r[12] = ssp->ip;
  111:                 regs->lr = ssp->lr;
  112:         }
  113: 
  114:         if ( eit != NULL ) {
  115:                 eit->pc       = ssp->pc;
  116:                 eit->taskmode = 0;
  117:         }
  118: 
  119:         if ( cregs != NULL ) {
  120:                 cregs->ssp   = tcb->tskctxb.ssp;
  121:         }
  122: }
  123: 
  124: #if USE_FPU
  125: #ifdef USE_FUNC_TK_SET_CPR
  126: /* ------------------------------------------------------------------------ */
  127: /*
  128:  * Set task register contents (Used in tk_set_reg())
  129:  */
  130: EXPORT ER knl_set_cpr( TCB *tcb, INT copno, CONST T_COPREGS *copregs)
  131: {
  132:         SStackFrame_wFPU       *ssp;
  133:         INT    i;
  134: 
  135:         ssp = (SStackFrame_wFPU*)(tcb->tskctxb.ssp);
  136: 
  137:         if(ssp->ufpu & EXPRN_NO_FPU ) {        /* FPU register is not saved */
  138:                 return E_CTX;
  139:         }
  140: 
  141:         for ( i = 0; i < 16; ++i ) {
  142:                 ssp->s[i] = copregs->s[i];
  143:                 ssp->s_[i] = copregs->s[i + 16];
  144:         }
  145:         ssp->fpscr = copregs->fpscr;
  146: 
  147:         return E_OK;
  148: }
  149: 
  150: #endif /* USE_FUNC_TK_SET_CPR */
  151: 
  152: #ifdef USE_FUNC_TK_GET_CPR
  153: /* ------------------------------------------------------------------------ */
  154: /*
  155:  * Get task FPU register contents (Used in tk_get_cpr())
  156:  */
  157: EXPORT ER knl_get_cpr( TCB *tcb, INT copno, T_COPREGS *copregs)
  158: {
  159:         SStackFrame_wFPU       *ssp;
  160:         INT    i;
  161: 
  162:         ssp = (SStackFrame_wFPU*)(tcb->tskctxb.ssp);
  163: 
  164:         if(ssp->ufpu & EXPRN_NO_FPU ) {        /* FPU register is not saved */
  165:                 return E_CTX;
  166:         }
  167: 
  168:         for ( i = 0; i < 16; ++i ) {
  169:                 copregs->s[i] = ssp->s[i];
  170:                 copregs->s[i + 16] = ssp->s_[i];
  171:         }
  172:         copregs->fpscr = ssp->fpscr;
  173: 
  174:         return E_OK;
  175: }
  176: #endif /* USE_FUNC_TK_GET_CPR */
  177: #endif /* USE_FPU */
  178: 
  179: 
  180: /* ----------------------------------------------------------------------- */
  181: /*
  182:  *      Task dispatcher startup
  183:  */
  184: EXPORT void knl_force_dispatch( void )
  185: {
  186:         knl_dispatch_disabled = DDS_DISABLE_IMPLICIT;
  187:         knl_ctxtsk = NULL;
  188:         *(_UW*)SCB_ICSR = ICSR_PENDSVSET;      /* pendsv exception */
  189:         set_basepri(0);
  190: }
  191: 
  192: EXPORT void knl_dispatch( void )
  193: {
  194:         *(_UW*)SCB_ICSR = ICSR_PENDSVSET;      /* pendsv exception */
  195: }
  196: 
  197: #endif /* CPU_CORE_ARMV7M */