gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    T-Kernel 2.0 Software Package
    4:  *
    5:  *    Copyright 2011 by Ken Sakamura.
    6:  *    This software is distributed under the latest version of T-License 2.x.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by T-Engine Forum(http://www.t-engine.org/) at 2011/05/17.
   10:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   11:  *
   12:  *----------------------------------------------------------------------
   13:  */
   14: 
   15: /*
   16:  *      cpu_init.c (EM1-D512)
   17:  *      CPU-Dependent Initialization/Finalization
   18:  */
   19: 
   20: #include "kernel.h"
   21: #include "task.h"
   22: #include "cpu_insn.h"
   23: 
   24: EXPORT MONHDR   SaveMonHdr;       /* For saving monitor exception handler */
   25: EXPORT ATR      available_cop;       /* Available coprocessor */
   26: 
   27: /*
   28:  * CPU-dependent initialization
   29:  */
   30: EXPORT ER cpu_initialize( void )
   31: {
   32: IMPORT void dispatch_entry( void );     /* calling dispatcher */
   33: IMPORT void call_entry( void );         /* calling system call */
   34: IMPORT void _tk_ret_int( void );        /* exclusively used to invoke tk_ret_int() */
   35: IMPORT void call_dbgspt( void );        /* calling debugger support */
   36: IMPORT void rettex_entry( void );       /* return from task exception */
   37: 
   38:         UW     r;
   39: 
   40:         /* Save monitor exception handler */
   41:         SaveMonHdr.default_hdr = SCArea->intvec[VECNO_DEFAULT];
   42:         SaveMonHdr.idebug_hdr  = SCArea->intvec[VECNO_IDEBUG];
   43:         SaveMonHdr.ddebug_hdr  = SCArea->intvec[VECNO_DDEBUG];
   44:         SaveMonHdr.monitor_hdr = SCArea->intvec[VECNO_MONITOR];
   45:         SaveMonHdr.abortsw_hdr = SCArea->intvec[VECNO_ABORTSW];
   46:         SaveMonHdr.gio_hdr[0]  = SCArea->intvec[VECNO_GIO0];
   47:         SaveMonHdr.gio_hdr[1]  = SCArea->intvec[VECNO_GIO1];
   48:         SaveMonHdr.gio_hdr[2]  = SCArea->intvec[VECNO_GIO2];
   49:         SaveMonHdr.gio_hdr[3]  = SCArea->intvec[VECNO_GIO3];
   50:         SaveMonHdr.gio_hdr[4]  = SCArea->intvec[VECNO_GIO4];
   51:         SaveMonHdr.gio_hdr[5]  = SCArea->intvec[VECNO_GIO5];
   52:         SaveMonHdr.gio_hdr[6]  = SCArea->intvec[VECNO_GIO6];
   53:         SaveMonHdr.gio_hdr[7]  = SCArea->intvec[VECNO_GIO7];
   54: 
   55:         /* Initialize task space */
   56:         Asm("mrc p15, 0, %0, cr2,  c0, 1": "=r"(r));   /* TTBR1 */
   57:         Asm("mcr p15, 0, %0, cr2,  c0, 0":: "r"(r));   /* TTBR0 */
   58:         Asm("mcr p15, 0, %0, cr13, c0, 1":: "r"(0));   /* CONTEXTIDR */
   59:         ISB();
   60:         PurgeTLB();    /* invlidate TLB */
   61: 
   62:         /* available coprocessor(s) */
   63:         available_cop = TA_NULL;
   64: 
   65:         /* install the exception handler used by the OS */
   66:         define_inthdr(SWI_SVC,     call_entry);
   67:         define_inthdr(SWI_RETINT,   _tk_ret_int);
   68:         define_inthdr(SWI_DISPATCH, dispatch_entry);
   69:         define_inthdr(SWI_RETTEX,   rettex_entry);
   70: #if USE_DBGSPT
   71:         define_inthdr(SWI_DEBUG,    call_dbgspt);
   72: #endif
   73: 
   74:         return E_OK;
   75: }
   76: 
   77: /*
   78:  * CPU-dependent finalization
   79:  */
   80: EXPORT void cpu_shutdown( void )
   81: {
   82:         /* Restore saved monitor exception handler */
   83:         SCArea->intvec[VECNO_DEFAULT] = SaveMonHdr.default_hdr;
   84:         SCArea->intvec[VECNO_IDEBUG]  = SaveMonHdr.idebug_hdr;
   85:         SCArea->intvec[VECNO_DDEBUG]  = SaveMonHdr.ddebug_hdr;
   86:         SCArea->intvec[VECNO_MONITOR] = SaveMonHdr.monitor_hdr;
   87:         SCArea->intvec[VECNO_ABORTSW] = SaveMonHdr.abortsw_hdr;
   88:         SCArea->intvec[VECNO_GIO0]    = SaveMonHdr.gio_hdr[0];
   89:         SCArea->intvec[VECNO_GIO1]    = SaveMonHdr.gio_hdr[1];
   90:         SCArea->intvec[VECNO_GIO2]    = SaveMonHdr.gio_hdr[2];
   91:         SCArea->intvec[VECNO_GIO3]    = SaveMonHdr.gio_hdr[3];
   92:         SCArea->intvec[VECNO_GIO4]    = SaveMonHdr.gio_hdr[4];
   93:         SCArea->intvec[VECNO_GIO5]    = SaveMonHdr.gio_hdr[5];
   94:         SCArea->intvec[VECNO_GIO6]    = SaveMonHdr.gio_hdr[6];
   95:         SCArea->intvec[VECNO_GIO7]    = SaveMonHdr.gio_hdr[7];
   96: }
   97: 
   98: /* ------------------------------------------------------------------------- */
   99: 
  100: /*
  101:  * Task exception handler startup reservation
  102:  */
  103: EXPORT void request_tex( TCB *tcb )
  104: {
  105:         /* Cannot set to the task operating at protected level 0 */
  106:         if ( tcb->isysmode == 0 ) {
  107:                 tcb->reqdct = 1;
  108:         }
  109: }
  110: 
  111: /*
  112:  * Setting up the start of task exception handler
  113:  *
  114:  *      Initial stack status
  115:  *              system stack                        user stack
  116:  *              +---------------+           +---------------+
  117:  *      ssp ->       | R12   = ip  |        usp -> | (xxxxxxxxxxx) |
  118:  *              | R14_svc = lr      |            |         |
  119:  *              | SPSR_svc  |
  120:  *              +---------------+
  121:  *
  122:  *      Modified stack status ( modified parts are maked with * )
  123:  *              +---------------+           +---------------+
  124:  *      ssp ->       | R12         |      usp* -> | texcd              |*
  125:  *              | texhdr    |*         | retadr       |*
  126:  *              | SPSR_svc  |*               | CPSR               |*
  127:  *              +---------------+           +---------------+
  128:  *                                              | (xxxxxxxxxxx) |
  129:  */
  130: EXPORT void setup_texhdr( UW *ssp )
  131: {
  132:         FP     texhdr;
  133:         INT    texcd;
  134:         UINT   m;
  135:         UW     *usp;
  136: 
  137:         /* called in interrupt-disabled state */
  138: 
  139:         ctxtsk->reqdct = 0;    /* release DCT */
  140: 
  141:         /* obtain exception code */
  142:         m = 0x00000001;
  143:         for ( texcd = 0; texcd <= 31; texcd++ ) {
  144:                 if ( (ctxtsk->exectex & m) != 0 ) break;
  145:                 m <<= 1;
  146:         }
  147:         if ( texcd > 31 ) return; /* exception is not generated / released */
  148: 
  149:         ctxtsk->exectex = 0;
  150:         ctxtsk->pendtex &= ~m;
  151:         ctxtsk->texflg |= ( texcd == 0 )? TEX0_RUNNING: TEX1_RUNNING;
  152:         texhdr = ctxtsk->texhdr;
  153: 
  154:         /* obtain user stack pointer */
  155:         Asm("stmia %0, {sp}^ ; nop":: "r"(&usp));
  156: 
  157:         /* reset user stack to the initial value if exception code is 0 */
  158:         if ( texcd == 0 ) usp = ctxtsk->istack;
  159: 
  160:         usp -= 3;
  161: 
  162:         /* set up user stack pointer */
  163:         Asm("ldmia %0, {sp}^ ; nop":: "r"(&usp));
  164: 
  165:         ENABLE_INTERRUPT;
  166: 
  167:         /* adjust stack
  168:          *     we need to access user stack, and this may cause
  169:          *     a page fault.
  170:          */
  171:         *(usp + 0) = texcd;
  172:         *(usp + 1) = *(ssp + 1);       /* retadr */
  173:         *(usp + 2) = *(ssp + 2);       /* CPSR */
  174:         *(ssp + 1) = (UW)texhdr & ~1;
  175:         *(ssp + 2) &= ~(PSR_T|PSR_J);
  176:         if ( ((UW)texhdr & 1) != 0 ) *(ssp + 2) |= PSR_T;
  177: }