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_calls.c (EM1-D512)
   17:  *      CPU-Dependent System Call
   18:  */
   19: 
   20: #include "kernel.h"
   21: #include "task.h"
   22: #include "check.h"
   23: #include "cpu_task.h"
   24: 
   25: #include <sys/sysinfo.h>
   26: #include <tk/sysdef.h>
   27: #include "cpu_insn.h"
   28: 
   29: /*
   30:  * Dispatch enable/disable
   31:  */
   32: SYSCALL ER _tk_dis_dsp( void )
   33: {
   34:         CHECK_CTX(!in_loc());
   35: 
   36:         dispatch_disabled = DDS_DISABLE;
   37: 
   38:         return E_OK;
   39: }
   40: 
   41: /*
   42:  * Dispatch enable
   43:  */
   44: SYSCALL ER _tk_ena_dsp( void )
   45: {
   46:         CHECK_CTX(!in_loc());
   47: 
   48:         dispatch_disabled = DDS_ENABLE;
   49:         if ( ctxtsk != schedtsk ) {
   50:                 dispatch();
   51:         }
   52: 
   53:         return E_OK;
   54: }
   55: 
   56: /* ------------------------------------------------------------------------ */
   57: 
   58: /*
   59:  * High level programming language
   60:  */
   61: 
   62: /* High level programming language interrupt handler entry */
   63: EXPORT FP hll_inthdr[N_INTVEC];
   64: 
   65: /* High level programming language routine (Interrupt) */
   66: IMPORT void inthdr_startup();
   67: 
   68: /* High level programming language routine (Exception) */
   69: IMPORT void exchdr_startup();
   70: 
   71: /* For default handler */
   72: IMPORT void defaulthdr_startup();
   73: 
   74: /*
   75:  * Interrupt handler definition
   76:  */
   77: SYSCALL ER _tk_def_int( UINT dintno, CONST T_DINT *pk_dint )
   78: {
   79:         FP     inthdr;
   80: 
   81:         CHECK_PAR(dintno < N_INTVEC);
   82: 
   83:         if ( pk_dint != NULL ) {
   84:                 /* Set interrupt handler */
   85:                 CHECK_RSATR(pk_dint->intatr, TA_HLNG);
   86:                 CHECK_PAR( !(dintno == EIT_FIQ
   87:                                 && (pk_dint->intatr & TA_HLNG) != 0) );
   88: 
   89:                 inthdr = pk_dint->inthdr;
   90: 
   91:                 BEGIN_CRITICAL_SECTION;
   92:                 if ( (pk_dint->intatr & TA_HLNG) != 0 ) {
   93:                         hll_inthdr[dintno] = inthdr;
   94:                         inthdr = ( dintno == EIT_DEFAULT )? defaulthdr_startup:
   95:                                  ( dintno <  EIT_FIQ     )? exchdr_startup:
   96:                                  ( dintno <= EIT_GPIO(127) )? inthdr_startup:
   97:                                                             exchdr_startup;
   98:                 }
   99:                 define_inthdr(dintno, inthdr);
  100:                 END_CRITICAL_SECTION;
  101:         } else {
  102:                 /* Clear interrupt handler */
  103:                 switch ( dintno ) {
  104:                   case VECNO_DEFAULT: inthdr = SaveMonHdr.default_hdr; break;
  105:                   case VECNO_IDEBUG:  inthdr = SaveMonHdr.idebug_hdr;   break;
  106:                   case VECNO_DDEBUG:  inthdr = SaveMonHdr.ddebug_hdr;   break;
  107:                   case VECNO_MONITOR: inthdr = SaveMonHdr.monitor_hdr; break;
  108:                   case VECNO_ABORTSW: inthdr = SaveMonHdr.abortsw_hdr; break;
  109:                   case VECNO_GIO0:    inthdr = SaveMonHdr.gio_hdr[0];     break;
  110:                   case VECNO_GIO1:    inthdr = SaveMonHdr.gio_hdr[1];     break;
  111:                   case VECNO_GIO2:    inthdr = SaveMonHdr.gio_hdr[2];     break;
  112:                   case VECNO_GIO3:    inthdr = SaveMonHdr.gio_hdr[3];     break;
  113:                   case VECNO_GIO4:    inthdr = SaveMonHdr.gio_hdr[4];     break;
  114:                   case VECNO_GIO5:    inthdr = SaveMonHdr.gio_hdr[5];     break;
  115:                   case VECNO_GIO6:    inthdr = SaveMonHdr.gio_hdr[6];     break;
  116:                   case VECNO_GIO7:    inthdr = SaveMonHdr.gio_hdr[7];     break;
  117:                   default:            inthdr = NULL;
  118:                 }
  119: 
  120:                 BEGIN_CRITICAL_SECTION;
  121:                 define_inthdr(dintno, inthdr);
  122:                 hll_inthdr[dintno] = NULL;
  123:                 END_CRITICAL_SECTION;
  124:         }
  125: 
  126:         return E_OK;
  127: }
  128: 
  129: /* ------------------------------------------------------------------------ */
  130: 
  131: /*
  132:  * Get task space
  133:  */
  134: SYSCALL ER _tk_get_tsp( ID tskid, T_TSKSPC *pk_tskspc )
  135: {
  136:         TCB    *tcb;
  137:         ER     ercd = E_OK;
  138: 
  139:         CHECK_TSKID_SELF(tskid);
  140: 
  141:         tcb = get_tcb_self(tskid);
  142: 
  143:         BEGIN_CRITICAL_SECTION;
  144:         if ( tcb->state == TS_NONEXIST ) {
  145:                 ercd = E_NOEXS;
  146:         } else {
  147:                 pk_tskspc->uatb = tcb->tskctxb.uatb;
  148:                 pk_tskspc->lsid = tcb->tskctxb.lsid;
  149:         }
  150:         END_CRITICAL_SECTION;
  151: 
  152:         return ercd;
  153: }
  154: 
  155: /*
  156:  * Set task space
  157:  */
  158: SYSCALL ER _tk_set_tsp( ID tskid, CONST T_TSKSPC *pk_tskspc )
  159: {
  160:         TCB    *tcb;
  161:         ER     ercd = E_OK;
  162: 
  163:         CHECK_TSKID_SELF(tskid);
  164: 
  165:         tcb = get_tcb_self(tskid);
  166: 
  167:         BEGIN_CRITICAL_SECTION;
  168:         if ( tcb->state == TS_NONEXIST) {
  169:                 ercd = E_NOEXS;
  170:         } else {
  171:                 tcb->tskctxb.uatb = pk_tskspc->uatb;
  172:                 tcb->tskctxb.lsid = pk_tskspc->lsid;
  173: 
  174:                 /* When it is the currently running task,
  175:                    switch the current space */
  176:                 if ( tcb == ctxtsk ) {
  177:                         change_space(tcb->tskctxb.uatb, tcb->tskctxb.lsid);
  178:                 }
  179:         }
  180:         END_CRITICAL_SECTION;
  181: 
  182:         return ercd;
  183: }
  184: 
  185: /* ------------------------------------------------------------------------ */
  186: 
  187: /*
  188:  * Set task register contents
  189:  */
  190: LOCAL void set_reg( TCB *tcb, CONST T_REGS *regs, CONST T_EIT *eit, CONST T_CREGS *cregs )
  191: {
  192:         SStackFrame    *ssp;
  193:         UW     cpsr;
  194:         INT    i;
  195: 
  196:         ssp = tcb->tskctxb.ssp;
  197:         cpsr = ssp->spsr_svc;
  198: 
  199:         if ( cregs != NULL ) {
  200:                 ssp = cregs->ssp;
  201:         }
  202: 
  203:         if ( regs != NULL ) {
  204:                 for ( i = 0; i < 12; ++i ) {
  205:                         ssp->r[i] = regs->r[i];
  206:                 }
  207:                 ssp->ip = regs->r[12];
  208:                 if ( (cpsr & PSR_M(31)) == PSR_SVC ) {
  209:                         ssp->lr_svc = regs->lr;
  210:                 } else {
  211:                         ssp->lr_usr = regs->lr;
  212:                 }
  213:         }
  214: 
  215:         if ( eit != NULL ) {
  216:                 ssp->pc       = eit->pc;
  217:                 ssp->spsr_svc = (eit->cpsr & 0xff000000) | (cpsr & 0x00ffffff);
  218:                 if ( tcb->tskctxb.svc_ssp == NULL ) {
  219:                         ssp->taskmode = eit->taskmode;
  220:                 } else {
  221:                         /* The value immediately before the T-Kernel system call (SVC) during
  222:                            the call is made. */
  223:                         *(tcb->tskctxb.svc_ssp - 4) = eit->taskmode;
  224:                 }
  225:         }
  226: 
  227:         if ( cregs != NULL ) {
  228:                 tcb->tskctxb.ssp  = cregs->ssp;
  229:                 tcb->tskctxb.uatb = cregs->uatb;
  230:                 tcb->tskctxb.lsid = cregs->lsid;
  231: 
  232:                 ssp->usp = cregs->usp;
  233:         }
  234: }
  235: 
  236: /*
  237:  * Set task register contents
  238:  */
  239: SYSCALL ER _tk_set_reg( ID tskid,
  240:         CONST T_REGS *pk_regs, CONST T_EIT *pk_eit, CONST T_CREGS *pk_cregs )
  241: {
  242:         TCB            *tcb;
  243:         ER             ercd = E_OK;
  244: 
  245:         CHECK_INTSK();
  246:         CHECK_TSKID(tskid);
  247:         CHECK_NONSELF(tskid);
  248: 
  249:         tcb = get_tcb(tskid);
  250: 
  251:         BEGIN_CRITICAL_SECTION;
  252:         if ( tcb->state == TS_NONEXIST ) {
  253:                 ercd = E_NOEXS;
  254:         } else {
  255:                 set_reg(tcb, pk_regs, pk_eit, pk_cregs);
  256:         }
  257:         END_CRITICAL_SECTION;
  258: 
  259:         return ercd;
  260: }
  261: 
  262: /*
  263:  * Get task register contents
  264:  */
  265: LOCAL void get_reg( TCB *tcb, T_REGS *regs, T_EIT *eit, T_CREGS *cregs )
  266: {
  267:         SStackFrame    *ssp;
  268:         UW             cpsr;
  269:         INT            i;
  270: 
  271:         ssp = tcb->tskctxb.ssp;
  272:         cpsr = ssp->spsr_svc;
  273: 
  274:         if ( regs != NULL ) {
  275:                 for ( i = 0; i < 12; ++i ) {
  276:                         regs->r[i] = ssp->r[i];
  277:                 }
  278:                 regs->r[12] = ssp->ip;
  279:                 if ( (cpsr & PSR_M(31)) == PSR_SVC ) {
  280:                         regs->lr = ssp->lr_svc;
  281:                 } else {
  282:                         regs->lr = ssp->lr_usr;
  283:                 }
  284:         }
  285: 
  286:         if ( eit != NULL ) {
  287:                 eit->pc       = ssp->pc;
  288:                 eit->cpsr     = ssp->spsr_svc;
  289:                 if ( tcb->tskctxb.svc_ssp == NULL ) {
  290:                         eit->taskmode = ssp->taskmode;
  291:                 } else {
  292:                         /* The value immediately before the T-Kernel system call (SVC) during
  293:                            the call is made. */
  294:                         eit->taskmode = *(tcb->tskctxb.svc_ssp - 4);
  295:                 }
  296:         }
  297: 
  298:         if ( cregs != NULL ) {
  299:                 cregs->ssp   = tcb->tskctxb.ssp;
  300:                 cregs->uatb  = tcb->tskctxb.uatb;
  301:                 cregs->lsid  = tcb->tskctxb.lsid;
  302: 
  303:                 cregs->usp = ssp->usp;
  304:         }
  305: }
  306: 
  307: /*
  308:  * Get task register contents
  309:  */
  310: SYSCALL ER _tk_get_reg( ID tskid,
  311:                 T_REGS *pk_regs, T_EIT *pk_eit, T_CREGS *pk_cregs )
  312: {
  313:         TCB            *tcb;
  314:         ER             ercd = E_OK;
  315: 
  316:         CHECK_INTSK();
  317:         CHECK_TSKID(tskid);
  318:         CHECK_NONSELF(tskid);
  319: 
  320:         tcb = get_tcb(tskid);
  321: 
  322:         BEGIN_CRITICAL_SECTION;
  323:         if ( tcb->state == TS_NONEXIST ) {
  324:                 ercd = E_NOEXS;
  325:         } else {
  326:                 get_reg(tcb, pk_regs, pk_eit, pk_cregs);
  327:         }
  328:         END_CRITICAL_SECTION;
  329: 
  330:         return ercd;
  331: }
  332: 
  333: /*
  334:  * Set task coprocessor register
  335:  */
  336: SYSCALL ER _tk_set_cpr( ID tskid, INT copno, CONST T_COPREGS *pk_copregs )
  337: {
  338:         ATR    copatr = TA_COP0 << copno;
  339:         TCB    *tcb;
  340:         ER     ercd = E_OK;
  341: 
  342:         CHECK_INTSK();
  343:         CHECK_TSKID(tskid);
  344:         CHECK_NONSELF(tskid);
  345:         CHECK_PAR((copatr & available_cop) != 0);
  346: 
  347:         tcb = get_tcb(tskid);
  348: 
  349:         BEGIN_CRITICAL_SECTION;
  350:         if ( tcb->state == TS_NONEXIST ) {
  351:                 ercd = E_NOEXS;
  352:         } else if ( (tcb->tskatr & copatr) == 0 ) {
  353:                 ercd = E_PAR;
  354:         } else {
  355:                 /* No coprocessor */
  356:         }
  357:         END_CRITICAL_SECTION;
  358: 
  359:         return ercd;
  360: }
  361: 
  362: /*
  363:  * Get task coprocessor register
  364:  */
  365: SYSCALL ER _tk_get_cpr( ID tskid, INT copno, T_COPREGS *pk_copregs )
  366: {
  367:         ATR    copatr = TA_COP0 << copno;
  368:         TCB    *tcb;
  369:         ER     ercd = E_OK;
  370: 
  371:         CHECK_INTSK();
  372:         CHECK_TSKID(tskid);
  373:         CHECK_NONSELF(tskid);
  374:         CHECK_PAR((copatr & available_cop) != 0);
  375: 
  376:         tcb = get_tcb(tskid);
  377: 
  378:         BEGIN_CRITICAL_SECTION;
  379:         if ( tcb->state == TS_NONEXIST ) {
  380:                 ercd = E_NOEXS;
  381:         } else if ( (tcb->tskatr & copatr) == 0 ) {
  382:                 ercd = E_PAR;
  383:         } else {
  384:                 /* No coprocessor */
  385:         }
  386:         END_CRITICAL_SECTION;
  387: 
  388:         return ercd;
  389: }
  390: 
  391: /* ------------------------------------------------------------------------ */
  392: /*
  393:  *      Debugger support function
  394:  */
  395: #if USE_DBGSPT
  396: 
  397: /*
  398:  * Set task register
  399:  */
  400: SYSCALL ER _td_set_reg( ID tskid, CONST T_REGS *regs, CONST T_EIT *eit, CONST T_CREGS *cregs )
  401: {
  402:         TCB    *tcb;
  403:         ER     ercd = E_OK;
  404: 
  405:         CHECK_TSKID(tskid);
  406: 
  407:         tcb = get_tcb(tskid);
  408:         if ( tcb == ctxtsk ) {
  409:                 return E_OBJ;
  410:         }
  411: 
  412:         BEGIN_DISABLE_INTERRUPT;
  413:         if ( tcb->state == TS_NONEXIST ) {
  414:                 ercd = E_NOEXS;
  415:         } else {
  416:                 set_reg(tcb, regs, eit, cregs);
  417:         }
  418:         END_DISABLE_INTERRUPT;
  419: 
  420:         return ercd;
  421: }
  422: 
  423: /*
  424:  * Get task register
  425:  */
  426: SYSCALL ER _td_get_reg( ID tskid, T_REGS *regs, T_EIT *eit, T_CREGS *cregs )
  427: {
  428:         TCB    *tcb;
  429:         ER     ercd = E_OK;
  430: 
  431:         CHECK_TSKID(tskid);
  432: 
  433:         tcb = get_tcb(tskid);
  434:         if ( tcb == ctxtsk ) {
  435:                 return E_OBJ;
  436:         }
  437: 
  438:         BEGIN_DISABLE_INTERRUPT;
  439:         if ( tcb->state == TS_NONEXIST ) {
  440:                 ercd = E_NOEXS;
  441:         } else {
  442:                 get_reg(tcb, regs, eit, cregs);
  443:         }
  444:         END_DISABLE_INTERRUPT;
  445: 
  446:         return ercd;
  447: }
  448: 
  449: #endif /* USE_DBGSPT */