gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    micro T-Kernel 3.00.05
    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/11.
   10:  *
   11:  *----------------------------------------------------------------------
   12:  */
   13: 
   14: #include <sys/machine.h>
   15: #ifdef CPU_CORE_ARMV7A
   16: /*
   17:  *      cpu_cntl.c (ARMv7-A)
   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: #if USE_FPU
   33: EXPORT TCB      *knl_fpu_ctx;        /* Task in FPU context */
   34: #endif /* USE_FPU */
   35: 
   36: /* ------------------------------------------------------------------------ */
   37: /*
   38:  * Set task register contents (Used in tk_set_reg())
   39:  */
   40: EXPORT void knl_set_reg( TCB *tcb, CONST T_REGS *regs, CONST T_EIT *eit, CONST T_CREGS *cregs )
   41: {
   42:         SStackFrame    *ssp;
   43:         INT    i;
   44: 
   45:         ssp = tcb->tskctxb.ssp;
   46: 
   47:         if ( cregs != NULL ) {
   48:                 ssp = cregs->ssp;
   49:                 tcb->tskctxb.ssp  = ssp;
   50:         }
   51: 
   52:         if ( regs != NULL ) {
   53:                 for ( i = 0; i < 4; ++i ) {
   54:                         ssp->r[i] = regs->r[i];
   55:                 }
   56:                 for ( i = 4; i < 12; ++i){
   57:                         ssp->r_[i - 4] = regs->r[i];
   58:                 }
   59:                 ssp->ip = regs->r[12];
   60:                 ssp->lr = regs->lr;
   61:         }
   62: 
   63:         if ( eit != NULL ) {
   64:                 ssp->pc       = eit->pc;
   65:                 ssp->cpsr = eit->cpsr;
   66:         }
   67: }
   68: 
   69: /* ------------------------------------------------------------------------ */
   70: /*
   71:  * Get task register contents (Used in tk_get_reg())
   72:  */
   73: EXPORT void knl_get_reg( TCB *tcb, T_REGS *regs, T_EIT *eit, T_CREGS *cregs )
   74: {
   75:         SStackFrame    *ssp;
   76:         INT            i;
   77: 
   78:         ssp = tcb->tskctxb.ssp;
   79: 
   80:         if ( regs != NULL ) {
   81:                 for ( i = 0; i < 4; ++i ) {
   82:                         regs->r[i] = ssp->r[i];
   83:                 }
   84:                 for ( i = 4; i < 12; ++i ){
   85:                         regs->r[i] = ssp->r_[i - 4];
   86:                 }
   87:                 regs->r[12] = ssp->ip;
   88:                 regs->lr = ssp->lr;
   89:         }
   90: 
   91:         if ( eit != NULL ) {
   92:                 eit->pc       = ssp->pc;
   93:                 eit->cpsr     = ssp->cpsr;
   94:                 eit->taskmode = 0;
   95:         }
   96: 
   97:         if ( cregs != NULL ) {
   98:                 cregs->ssp   = tcb->tskctxb.ssp;
   99:         }
  100: }
  101: 
  102: #if USE_FPU
  103: 
  104: LOCAL void save_fpuctx(FPUContext *fpu)
  105: {
  106:         UW     bk_fpexe, bk_fpscr;
  107: 
  108:         Asm("fmrx %0, fpexc":"=r"(bk_fpexe));          // bk_fpexe = FPEXC
  109:         Asm("orr ip, %0, #0x40000000"::"r"(bk_fpexe)); // FPEXC.EN = 1
  110:         Asm("fmxr fpexc, ip");                         // VFP enable
  111: 
  112:         Asm("mov ip, %0"::"r"(fpu));
  113: 
  114:         // save VFP context
  115:         Asm("fmrx %0, fpscr":"=r"(bk_fpscr));          // Floating-Point Status and Control Register
  116:         Asm("stmia ip!, {r0, %0}"::"r"(bk_fpscr));     // (r0 is padding)
  117:         Asm("fstmiad ip!, {d0-d15}");
  118:         Asm("fstmiad ip!, {d16-d31}");
  119: 
  120:         Asm("fmxr fpexc, %0"::"r"(bk_fpexe));          // restore FPEXC
  121: }
  122: 
  123: #ifdef USE_FUNC_TK_SET_CPR
  124: /* ------------------------------------------------------------------------ */
  125: /*
  126:  * Set task register contents (Used in tk_set_reg())
  127:  */
  128: EXPORT ER knl_set_cpr( TCB *tcb, INT copno, CONST T_COPREGS *copregs)
  129: {
  130:         FPUContext     *fpu;
  131:         INT            i;
  132:         
  133:         fpu = tcb->isstack;
  134:         fpu--;
  135: 
  136:         if (tcb == knl_fpu_ctx) {
  137:                 save_fpuctx(fpu);     /* save FPU context */
  138:                 knl_fpu_ctx = NULL;
  139:         }
  140: 
  141:         if( copregs != NULL ){
  142:                 for ( i = 0; i < 32; i++ ) {
  143:                         fpu->d[i] = copregs->d[i];
  144:                 }
  145:                 fpu->fpscr = copregs->fpscr;
  146:         }
  147: 
  148:         return E_OK;
  149: }
  150: 
  151: #endif /* USE_FUNC_TK_SET_CPR */
  152: 
  153: #ifdef USE_FUNC_TK_GET_CPR
  154: /* ------------------------------------------------------------------------ */
  155: /*
  156:  * Get task FPU register contents (Used in tk_get_cpr())
  157:  */
  158: EXPORT ER knl_get_cpr( TCB *tcb, INT copno, T_COPREGS *copregs)
  159: {
  160:         FPUContext     *fpu;
  161:         INT            i;
  162: 
  163:         fpu = tcb->isstack;
  164:         fpu--;
  165: 
  166:         if (tcb == knl_fpu_ctx) {
  167:                 save_fpuctx(fpu);     /* save FPU context */
  168:         }
  169: 
  170:         if (copregs != NULL) {
  171:                 for (i = 0; i < 32; i++) {
  172:                         copregs->d[i] = fpu->d[i];
  173:                 }
  174:                 copregs->fpscr = fpu->fpscr;
  175:         }
  176: 
  177:         return E_OK;
  178: }
  179: #endif /* USE_FUNC_TK_GET_CPR */
  180: #endif /* USE_FPU */
  181: 
  182: 
  183: #endif /* CPU_CORE_ARMV7A */