gonzui


Format: Advanced Search

t2ex/t2ex_source/kernel/sysdepend_t2ex/cpu/em1d/cpu_support.Sbare sourcepermlink (0.02 seconds)

Search this content:

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    T2EX Software Package
    4:  *
    5:  *    Copyright 2012 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 2012/12/12.
   10:  *    Modified by T-Engine Forum at 2013/07/14.
   11:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/04.
   12:  *
   13:  *----------------------------------------------------------------------
   14:  */
   15: /*
   16:  * This software package is available for use, modification, 
   17:  * and redistribution in accordance with the terms of the attached 
   18:  * T-License 2.x.
   19:  * If you want to redistribute the source code, you need to attach 
   20:  * the T-License 2.x document.
   21:  * There's no obligation to publish the content, and no obligation 
   22:  * to disclose it to the TRON Forum if you have modified the 
   23:  * software package.
   24:  * You can also distribute the modified source code. In this case, 
   25:  * please register the modification to T-Kernel traceability service.
   26:  * People can know the history of modifications by the service, 
   27:  * and can be sure that the version you have inherited some 
   28:  * modification of a particular version or not.
   29:  *
   30:  *    http://trace.tron.org/tk/?lang=en
   31:  *    http://trace.tron.org/tk/?lang=ja
   32:  *
   33:  * As per the provisions of the T-License 2.x, TRON Forum ensures that 
   34:  * the portion of the software that is copyrighted by Ken Sakamura or 
   35:  * the TRON Forum does not infringe the copyrights of a third party.
   36:  * However, it does not make any warranty other than this.
   37:  * DISCLAIMER: TRON Forum and Ken Sakamura shall not be held
   38:  * responsible for any consequences or damages caused directly or
   39:  * indirectly by the use of this software package.
   40:  *
   41:  * The source codes in bsd_source.tar.gz in this software package are 
   42:  * derived from NetBSD or OpenBSD and not covered under T-License 2.x.
   43:  * They need to be changed or redistributed according to the 
   44:  * representation of each source header.
   45:  */
   46: 
   47: /*
   48:  *      cpu_support.S
   49:  *
   50:  *      CPU operation specific to EM1-D512, etc.
   51:  */
   52: #define _in_asm_source_
   53: 
   54: #include <machine.h>
   55: #include <tk/errno.h>
   56: #include <tk/sysdef.h>
   57: #include <tk/asm.h>
   58: #include <sys/sysinfo.h>
   59: 
   60: #include "config.h"
   61: #include "cpu_conf.h"
   62: #include "isysconf.h"
   63: #include "tkdev_conf.h"
   64: #include "offset.h"
   65: 
   66: /* ------------------------------------------------------------------------ */
   67: /*
   68:  * Dispatcher
   69:  *      dispatch_to_schedtsk:
   70:  *              Discard current context and, dispatch to schedtsk forcibly.
   71:  *              Jumping to it directly using (bx) and never returns.
   72:  *              Called when stack is not defined (ssp is undefined).
   73:  *              Called when interrupt is disabled.
   74:  *      dispatch_entry:
   75:  *              Ordinary dispatch processing. Called by svc,  SWI_DISPATCH.
   76:  *      _ret_int_dispatch:
   77:  *              Called if tk_ret_int() requires dispatching.
   78:  *
   79:  *      Saved context
   80:  *      Save registers except for ssp(R13_svc) to stack. ssp is saved in TCB.
   81:  *
   82:  *              +---------------+
   83:  *      ssp ->       | R0--R11     |
   84:  *              | taskmode  |
   85:  *              | R13_usr = usp     | valid only in RNG1-3
   86:  *              | R14_usr   |
   87:  *              +---------------+
   88:  *              | R14_svc   | R14_svc before interrupt
   89:  *              |           | (valid only in case of tk_ret_int)
   90:  *              +---------------+
   91:  *              | R12     = ip      |                    ^ saved by interrupt entry
   92:  *              | R14_svc = lr      | return address(pc) | routine
   93:  *              | SPSR_svc  |                        V
   94:  *              +---------------+
   95:  */
   96: 
   97:         /* temporary stack used when dispatch_to_schedtsk is called
   98:            */
   99: #define TMP_STACK_SZ    (4*1024)
  100: #define TMP_STACK_TOP   (tmp_stack + TMP_STACK_SZ)
  101:         .lcomm tmp_stack, TMP_STACK_SZ
  102: 
  103:         .text
  104:         .balign        4
  105:         .globl Csym(dispatch_to_schedtsk)
  106:         .type  Csym(dispatch_to_schedtsk), %function
  107:         .globl Csym(dispatch_entry)
  108:         .type  Csym(dispatch_entry), %function
  109: 
  110: Csym(dispatch_to_schedtsk):
  111:         /* SVC mode / interrupt-disabled state CPSR.I=1 A=1 */
  112:         ldr    sp, =TMP_STACK_TOP         // set up a temporary stack
  113: 
  114:         ldr    ip, =Csym(dispatch_disabled)
  115:         ldr    r0, =1
  116:         str    r0, [ip]                   // disable dispatch
  117: 
  118:         ldr    r4, =Csym(ctxtsk)          // R4 = &ctxtsk
  119:         ldr    ip, =TASKMODE
  120:         ldr    r0, =0
  121: #if USE_DBGSPT
  122:         ldr    r8, [r4]
  123: #endif
  124:         str    r0, [r4]                   // ctxtsk = NULL
  125:         str    r0, [ip]                   // taskmode = 0
  126: 
  127:         cpsie  aif                              // enable interrupt
  128:         b      l_dispatch0
  129: 
  130: Csym(dispatch_entry):
  131:         /* SVC mode / interrupt-disabled state CPSR.I=1 A=1 */
  132:         ldr    ip, [sp, #2*4]
  133:         bic    ip, ip, #PSR_DI
  134:         str    ip, [sp, #2*4]                     // adjust SPSR_svc
  135: 
  136:         stmfd  sp!, {lr}                        // save context (R14_svc)
  137:                                                 // meaningless place holder for proper stack alignment
  138: _ret_int_dispatch:
  139:         /* SVC mode / interrupt-disabled state  CPSR.I=1 A=1 */
  140:         ldr    ip, =Csym(dispatch_disabled)
  141:         ldr    lr, =1
  142:         str    lr, [ip]                   // disable dispatch
  143: 
  144:         cpsie  aif                              // enable interrupt
  145: 
  146:         ldr    ip, =TASKMODE
  147:         ldr    ip, [ip]
  148:         sub    sp, sp, #15*4
  149:         stmia  sp, {r0-r11, ip, sp, lr}^        // save context
  150: 
  151:         ldr    r4, =Csym(ctxtsk)          // R4 = &ctxtsk
  152:         ldr    ip, =TASKMODE
  153:         ldr    r0, =0
  154:         ldr    r8, [r4]
  155:         str    sp, [r8, #TCB_tskctxb + CTXB_ssp] // save ssp to TCB
  156:         str    r0, [r4]                   // ctxtsk = NULL
  157:         str    r0, [ip]                   // taskmode = 0
  158: 
  159:   l_dispatch0:
  160:         bic    sp, sp, #7                 // align stack module 8 bytes
  161: 
  162:         /* interrupt-enabled state   CPSR.I=0 A=0 */
  163: #if USE_DBGSPT
  164:         ldr    ip, =hook_stop_jmp         // hook processing
  165:         ldr    pc, [ip]
  166:   ret_hook_stop:
  167: #endif
  168: 
  169:         ldr    r5, =Csym(schedtsk)                // R5 = &schedtsk
  170:         ldr    r6, =Csym(lowpow_discnt)   // R6 = &lowpow_discnt
  171: 
  172:   l_dispatch1:
  173:         cpsid  IMASK                            // disable interrupt
  174: 
  175:         ldr    r8, [r5]                   // R8 = schedtsk
  176:         cmp    r8, #0                             // is there schedtsk ?
  177:         bne    l_dispatch2
  178: 
  179:         /* enter low-power mode since there is no task to execute */
  180:         ldr    ip, [r6]                   // is low_pow disabled?
  181:         cmp    ip, #0
  182:         bleq   Csym(low_pow)                     // call low_pow()
  183: 
  184:         cpsie  aif                              // enable interrupt
  185:         b      l_dispatch1
  186: 
  187:   l_dispatch2:                                  // dispatch to schedtsk
  188:         /* interrupt-disabled state CPSR.I=1 A=1 */
  189:         str    r8, [r4]                   // ctxtsk = schedtsk
  190:         ldr    sp, [r8, #TCB_tskctxb + CTXB_ssp] // restore ssp from TCB
  191: 
  192:         /* switch task spaces */
  193:         ldr    r0, =0
  194:         ldr    r1, [r8, #TCB_tskctxb + CTXB_uatb]
  195:         ldr    r2, [r8, #TCB_tskctxb + CTXB_lsid]
  196:         mrc    p15, 0, ip, cr2, c0, 1             // TTBR1
  197:         cmp    r1, #0
  198:         andne  ip, ip, #0x7f
  199:         orrne  ip, ip, r1
  200:         mcr    p15, 0, r0, cr13, c0, 1            // set ASID to a meaningless value
  201:         .ISB   r0, r0                            // for synchronizing ASID and TTBR
  202:         mcr    p15, 0, ip, cr2,  c0, 0            // TTBR0
  203:         mcr    p15, 0, r2, cr13, c0, 1            // CONTEXTIDR
  204:         .ISB   r0, r0
  205: 
  206: #if USE_DBGSPT
  207:         ldr    ip, =hook_exec_jmp         // hook processing
  208:         ldr    pc, [ip]
  209:   ret_hook_exec:
  210: #endif
  211: 
  212:         ldr    ip, =Csym(dispatch_disabled)
  213:         ldr    lr, =0
  214:         str    lr, [ip]                   // enable dispatch
  215: 
  216:         clrex                                  // clear exclusive lock state
  217:                                                 // it is required for atomic
  218:                                                 // operation using LDREX/STREX
  219: 
  220:         ldr    ip, [r8, #TCB_reqdct]              // request DCT
  221:         cmp    ip, #1
  222: 
  223:         ldmia  sp, {r0-r11, ip, sp, lr}^        // restore context
  224:         nop
  225:         add    sp, sp, #15*4
  226:         ldr    lr, =TASKMODE
  227:         str    ip, [lr]
  228:         movne  ip, #0                           // ip = 0 when no DCT request
  229: 
  230:         ldmfd  sp!, {lr}                        // restore R14_svc
  231: 
  232:         ands   ip, ip, #TMF_CPL(3)               // DCT is ineffective in protection level 0
  233:         bne    dct_startup                        // jump to DCT processing
  234: 
  235:         EXC_RETURN
  236: 
  237: 
  238: #if USE_DBGSPT
  239: /*
  240:  * Hook routine invocation in task dispatcher
  241:  *      void stop( ID tskid, INT lsid, UINT tskstat )
  242:  *      void exec( ID tskid, INT lsid )
  243:  */
  244:         .text
  245:         .balign        4
  246: hook_stop:
  247:         cmp    r8, #0                     // r8 = ctxtsk
  248:         beq    l_notask
  249: 
  250:         ldrb   r2, [r8, #TCB_state]                      // tskstat
  251:         mov    r2, r2, lsl #1
  252:         ldr    r1, [r8, #TCB_tskctxb + CTXB_lsid] // lsid
  253:         ldr    r0, [r8, #TCB_tskid]                       // tskid
  254: 
  255:         ldr    ip, =Csym(hook_stopfn)
  256:         ldr    ip, [ip]
  257:         blx    ip                 // call stop(tskid, lsid, tskstat)
  258: 
  259:   l_notask:
  260:         b      ret_hook_stop
  261: 
  262: hook_exec:
  263:         mov    r9, sp                     // save sp
  264:         bic    sp, sp, #7         // align stack module 8 bytes
  265: 
  266:                                         // r8 = ctxtsk
  267:         ldr    r1, [r8, #TCB_tskctxb + CTXB_lsid] // lsid
  268:         ldr    r0, [r8, #TCB_tskid]                       // tskid
  269: 
  270:         ldr    ip, =Csym(hook_execfn)
  271:         ldr    ip, [ip]
  272:         blx    ip                 // call exec(tskid, lsid)
  273: 
  274:         mov    sp, r9                     // restore sp
  275:         b      ret_hook_exec
  276: 
  277: /*
  278:  * Setting and releasing of task dispatcher hook routine
  279:  */
  280:         .text
  281:         .balign        4
  282:         .globl Csym(hook_dsp)
  283:         .type  Csym(hook_dsp), %function
  284: Csym(hook_dsp):
  285:         ldr    r0, =hook_exec_jmp
  286:         ldr    r1, =hook_stop_jmp
  287:         ldr    r2, =hook_exec
  288:         ldr    r3, =hook_stop
  289:         str    r2, [r0]
  290:         str    r3, [r1]
  291:         bx     lr
  292: 
  293:         .globl Csym(unhook_dsp)
  294:         .type  Csym(unhook_dsp), %function
  295: Csym(unhook_dsp):
  296:         ldr    r0, =hook_exec_jmp
  297:         ldr    r1, =hook_stop_jmp
  298:         ldr    r2, =ret_hook_exec
  299:         ldr    r3, =ret_hook_stop
  300:         str    r2, [r0]
  301:         str    r3, [r1]
  302:         bx     lr
  303: 
  304:                         .data
  305:                         .balign      4
  306:   hook_exec_jmp:        .long  ret_hook_exec
  307:   hook_stop_jmp:        .long  ret_hook_stop
  308: 
  309: #endif /* USE_DBGSPT */
  310: 
  311: /* ------------------------------------------------------------------------ */
  312: /*
  313:  * High-level language support routine for interrupt handler
  314:  *      this is called from interrupt entry routine with information
  315:  *      in the stack as follows.
  316:  *              +---------------+   SVC stack
  317:  *      ssp ->       | R13_svc = ssp       |
  318:  *              | R12_usr = ip      |
  319:  *              | (padding) | stack alignment (if necessary)
  320:  *              | CPSR_xxx  |
  321:  *              | R14_svc = lr      |
  322:  *              +---------------+
  323:  *
  324:  *              +---------------+   exception stack
  325:  *              | R0 - R2   |
  326:  *              +===============+
  327:  *      isp ->       | R3          | only in the case of FIQ and IRQ
  328:  *              +---------------+
  329:  *      isp ->       | R12 = ip    | R12_usr or R12_fiq
  330:  *              | R14_xxx = lr      |
  331:  *              | SPSR              |
  332:  *              +---------------+
  333:  *
  334:  *      ip holds the vector table address
  335:  *      (ip - EIT_VECTBL) / 4 = vector number
  336:  */
  337:         .text
  338:         .balign        4
  339:         .globl Csym(defaulthdr_startup)
  340:         .type  Csym(defaulthdr_startup), %function
  341:         .globl Csym(inthdr_startup)
  342:         .type  Csym(inthdr_startup), %function
  343:         .globl Csym(exchdr_startup)
  344:         .type  Csym(exchdr_startup), %function
  345: Csym(defaulthdr_startup):
  346:         /* unknown mode / interrupt-disabled state CPSR.I=1 A=? F=? */
  347:         mrs    lr, cpsr
  348:         and    lr, lr, #PSR_M(31)
  349:         cmp    lr, #PSR_FIQ
  350:         cmpne  lr, #PSR_IRQ
  351:         stmnefd        sp!, {r3}              // save registers
  352:         stmfd  sp!, {r0-r2}
  353: 
  354:         ldr    r3, =EIT_VECTBL
  355:         sub    r3, ip, r3
  356:                                         // argument to the handler
  357:         mov    r0, r3, lsr #2             // r0 = dintno
  358:         add    r1, sp, #4*4               // r1 = sp
  359: 
  360:         ldr    r3, =EIT_DEFAULT * 4       // r3 = vector table offset
  361:         b      l_inthdr2
  362: 
  363: Csym(exchdr_startup):
  364:         /* unknown mode / interrupt-disabled state CPSR.I=1 A=? F=? */
  365:         stmfd  sp!, {r3}                // save registers
  366:         b      l_inthdr1
  367: 
  368: Csym(inthdr_startup):
  369:         /* unknown mode / interrupt-disabled state CPSR.I=1 A=1 F=? */
  370:         mrs    lr, cpsr
  371:         and    lr, lr, #PSR_M(31)
  372:         cmp    lr, #PSR_SVC
  373:         stmeqfd        sp!, {r3}              // save r3 in the case of SVC
  374:   l_inthdr1:
  375:         stmfd  sp!, {r0-r2}             // save registers
  376: 
  377:         ldr    r3, =EIT_VECTBL
  378:         sub    r3, ip, r3         // r3 = vector table offset
  379: 
  380:                                         // argument to the handler
  381:         mov    r0, r3, lsr #2             // r0 = dintno
  382:         add    r1, sp, #4*4               // r1 = sp
  383: 
  384:   l_inthdr2:
  385:         mrs    r2, cpsr           // save CPSR to r2
  386:         cpsid  IMASK, #PSR_SVC          // enter SVC mode, interrupt is disabled
  387: 
  388:         stmfd  sp!, {r2, lr}            // save CPSR and lr_svc
  389: 
  390:         mov    r2, sp
  391:         bic    sp, sp, #7         // align stack module 8 bytes
  392:         stmfd  sp!, {r2, ip}            // save sp and ip
  393: 
  394:         ldr    ip, =TASKINDP              // enter task independent portion
  395:         ldr    lr, [ip]
  396:         add    lr, lr, #1
  397:         str    lr, [ip]
  398: 
  399: #if USE_DBGSPT
  400:         ldr    ip, =hook_ienter_jmp
  401:         ldr    pc, [ip]
  402:   ret_hook_ienter:
  403: #endif
  404: 
  405:         ldr    ip, =Csym(hll_inthdr)
  406:         ldr    ip, [ip, r3]
  407:         blx    ip         // call hll_inthdr[n](dintno, sp)
  408: 
  409: #if USE_DBGSPT
  410:         ldr    ip, =hook_ileave_jmp
  411:         ldr    pc, [ip]
  412:   ret_hook_ileave:
  413: #endif
  414: 
  415:         ldmfd  sp!, {r2, ip}            // restore sp and ip
  416:         mov    sp, r2
  417:         ldmfd  sp!, {r2, r3}            // r2 contaisn the original mode
  418:                                         // r3 = R14_svc
  419:         orr    r2, r2, #PSR_DI
  420:         msr    cpsr_xc, r2                // restore original mode, interrupt is disabled
  421: 
  422:         ldr    ip, =TASKINDP
  423:         ldr    lr, [ip]
  424:         sub    lr, lr, #1
  425:         str    lr, [ip]
  426: 
  427:         ldmfd  sp!, {r0-r2}             // restore registers
  428:         swp    r3, r3, [sp]               // restore r3, R14_svc is saved
  429:         swi    SWI_RETINT         // tk_ret_int()
  430: 
  431: 
  432: #if USE_DBGSPT
  433: /*
  434:  * Invoking a hook routine of an interrupt handler
  435:  */
  436:         .text
  437:         .balign        4
  438: hook_ienter:
  439:         stmfd  sp!, {r0-r1}             // save registers
  440:         stmfd  sp!, {r2-r3}             // align stack module 8
  441: 
  442:         ldr    ip, =Csym(hook_ienterfn)
  443:         ldr    ip, [ip]
  444:         blx    ip                 // call enter(dintno, sp)
  445: 
  446:         ldmfd  sp!, {r2-r3}             // restore registers
  447:         ldmfd  sp,  {r0-r1}             // leave dintno and sp on the stack
  448:         b      ret_hook_ienter
  449: 
  450: hook_ileave:
  451:         ldmfd  sp!, {r0-r1}             // restore dintno and sp
  452: 
  453:         ldr    ip, =Csym(hook_ileavefn)
  454:         ldr    ip, [ip]
  455:         blx    ip                 // call leave(dintno, sp)
  456: 
  457:         b      ret_hook_ileave
  458: 
  459: /*
  460:  * Setting and relaseing hook routine of an interrupt handler
  461:  */
  462:         .text
  463:         .balign        4
  464:         .globl Csym(hook_int)
  465:         .type  Csym(hook_int), %function
  466: Csym(hook_int):
  467:         ldr    r0, =hook_ienter_jmp
  468:         ldr    r1, =hook_ileave_jmp
  469:         ldr    r2, =hook_ienter
  470:         ldr    r3, =hook_ileave
  471:         str    r2, [r0]
  472:         str    r3, [r1]
  473:         bx     lr
  474: 
  475:         .globl Csym(unhook_int)
  476:         .type  Csym(unhook_int), %function
  477: Csym(unhook_int):
  478:         ldr    r0, =hook_ienter_jmp
  479:         ldr    r1, =hook_ileave_jmp
  480:         ldr    r2, =ret_hook_ienter
  481:         ldr    r3, =ret_hook_ileave
  482:         str    r2, [r0]
  483:         str    r3, [r1]
  484:         bx     lr
  485: 
  486:                         .data
  487:                         .balign      4
  488:   hook_ienter_jmp:      .long        ret_hook_ienter
  489:   hook_ileave_jmp:      .long        ret_hook_ileave
  490: 
  491: #endif /* USE_DBGSPT */
  492: 
  493: /*
  494:  * Processing tk_ret_int()
  495:  *      interrupt stack looks as follows when this is called.
  496:  *              +---------------+
  497:  *      ssp ->       | R12_usr     | saved by svc SWI_RETINT
  498:  *              | R14_svc   |
  499:  *              | SPSR_svc  |
  500:  *              +---------------+
  501:  *
  502:  *              +---------------+
  503:  *      isp ->       | R14_svc     | saved when tk_ret_int was called
  504:  *              +---------------+
  505:  *              | R12_xxx   | saved at the time of interrupt
  506:  *              | R14_xxx   | <- return address
  507:  *              | SPSR_xxx  |
  508:  *              +---------------+
  509:  */
  510:         .text
  511:         .balign        4
  512:         .globl Csym(_tk_ret_int)
  513:         .type  Csym(_tk_ret_int), %function
  514: Csym(_tk_ret_int):
  515:         ldr    ip, [sp, #2*4]             // ip = SPSR
  516: 
  517:         and    lr, ip, #PSR_M(31)
  518:         cmp    lr, #PSR_SVC
  519:         beq    l_retint_svc               // is this tk_ret_int invoked from an SVC?
  520: 
  521:         stmfd  sp!, {r2, r3}            // r2 is used as temporary work register (r3 is saved as place holder)
  522:         add    r2, sp, #4
  523: 
  524:         orr    ip, ip, #PSR_DI
  525:         bic    ip, ip, #PSR_T
  526:         cmp    lr, #PSR_FIQ
  527:         msr    cpsr_xc, ip                // enter interrupted mode, interrupt is disabled
  528: 
  529:         ldr    ip, [sp, #0*4]             // copy isp to ssp
  530:         ldr    lr, [sp, #3*4]
  531:         str    ip, [r2, #0*4]             // R14_svc
  532:         str    lr, [r2, #3*4]             // SPSR_xxx
  533:         ldr    ip, [sp, #1*4]
  534:         ldr    lr, [sp, #2*4]
  535:         strne  ip, [r2, #1*4]           // R12_xxx (in non-FIQ cases only)
  536:         str    lr, [r2, #2*4]             // R14_xxx (return address)
  537:         add    sp, sp, #4*4               // pop isp
  538: 
  539:         cpsid  IMASK, #PSR_SVC          // enter SVC mode, interrupt is disabled
  540: 
  541:         ldmfd  sp!, {r2}                // restore r2
  542:         b      l_retint1
  543: 
  544:   l_retint_svc:
  545:         add    sp, sp, #3*4               // discard the value saved by svc SWI_RETINT
  546: 
  547:         cpsid  IMASK, #PSR_SVC          // disable interrupt
  548: 
  549:   l_retint1:
  550:         ldr    ip, =TASKINDP              // is there additional  multi-interrupt(s)?
  551:         ldr    ip, [ip]
  552:         cmp    ip, #0
  553:         bne    l_nodispatch
  554: 
  555:         ldr    ip, =Csym(dispatch_disabled)       // is dispatch disabled?
  556:         ldr    ip, [ip]
  557:         cmp    ip, #0
  558:         bne    l_nodispatch
  559: 
  560:         ldr    ip, [sp, #3*4]             // SPSR
  561:         tst    ip, #PSR_A|PSR_I|PSR_F     // is this exception during interrupt-disabled state?
  562:         bne    l_nodispatch
  563: 
  564:         ldr    ip, =Csym(ctxtsk)  // is dispatch necessary?
  565:         ldr    lr, =Csym(schedtsk)
  566:         ldr    ip, [ip]
  567:         ldr    lr, [lr]
  568:         cmp    ip, lr
  569:         bne    _ret_int_dispatch  // goto dispatch processing
  570: 
  571:   l_nodispatch:
  572:         ldmfd  sp!, {lr}                // restore lr
  573:         EXC_RETURN
  574: 
  575: 
  576: /* ------------------------------------------------------------------------ */
  577: 
  578: /*
  579:  * Unsupported system call
  580:  */
  581:         .text
  582:         .balign        4
  583:         .globl Csym(no_support)
  584:         .type  Csym(no_support), %function
  585: Csym(no_support):
  586:         ldr    r0, =E_RSFN
  587:         bx     lr
  588: 
  589: /*
  590:  * System call entry table
  591:  */
  592:         .text
  593:         .balign        4
  594: _svctbl:
  595:         .int   Csym(no_support)
  596: #define _tk_ret_int     no_support
  597: #include <sys/svc/tksvctbl.h>
  598: #undef  _tk_ret_int
  599: 
  600: /*
  601:  * System call entry
  602:  *      no need to save temporary registers
  603:  *      permanent registers are saved by the code generated by the compiler
  604:  *              +---------------+
  605:  *              | exinf             | save for hook
  606:  *              +---------------+
  607:  *              | svc_ssp   | save within call_entry
  608:  *              | taskmode  |
  609:  *              | r9                |
  610:  *              | r10               |
  611:  *              | fp                |
  612:  *              +===============+
  613:  *      ssp ->       | ip          | function code
  614:  *              | lr                | return address
  615:  *              | SPSR              |
  616:  *              +---------------+
  617:  *
  618:  *      Save ssp to CTXB.svc_ssp when a T-Kernel system call (SVC) is invoked.
  619:  *      However, if this is a nested SVC, svc_ssp is not updated.
  620:  *      Save the original svc_ssp to the system stack.
  621:  *      svc_ssp is restored when the system call returns.
  622:  *      svc_ssp = NULL means that SVC is not being callled.
  623:  *      With extended SVC, svc_ssp = NULL holds.
  624:  */
  625:         .text
  626:         .balign        4
  627:         .globl Csym(call_entry)
  628:         .type  Csym(call_entry), %function
  629: Csym(call_entry):
  630:         /* SVC mode / interrupt-disabled state CPSR.I=1 A=? F=? */
  631:         ldr    ip, [sp, #2*4]
  632:         and    ip, ip, #PSR_I|PSR_F       // cpsr_c doesn't include PSR_A
  633:         orr    ip, ip, #PSR_SVC
  634:         msr    cpsr_c, ip         // restore interrupt disabled status to the original.
  635: 
  636:         stmfd  sp!, {r9-r10, fp}        // save working registers
  637:         add    fp, sp, #3*4               // fp is the sp when call_entry was entered.
  638: 
  639:         ldr    ip, =TASKMODE              // obtain taskmode flag
  640:         ldr    r10, [ip]          // r10 = taskmode
  641:         stmfd  sp!, {r10}               // save taskmode
  642:         mov    lr, r10, lsl #16   // update taskmode flag
  643:         str    lr, [ip]
  644: 
  645:         ldr    r9, =Csym(ctxtsk)  // r9 = ctxtsk
  646:         ldr    r9, [r9]           //   if no ctxtsk (NULL) then
  647:         cmp    r9, #0                     //   assign a dumm value
  648:         ldreq  r9, =dummy_svc_ssp - (TCB_tskctxb + CTXB_svc_ssp)
  649:         ldr    ip, [r9, #TCB_tskctxb + CTXB_svc_ssp]
  650:         cmp    ip, #0
  651:         streq  fp, [r9, #TCB_tskctxb + CTXB_svc_ssp]    // update svc_ssp
  652:         stmfd  sp!, {ip}                                // save   svc_ssp
  653: 
  654: #if USE_DBGSPT
  655:         mov    ip, #0
  656:         stmfd  sp!, {ip}                // exinf = 0
  657:         ldr    ip, =hook_enter_jmp        // hook processing
  658:         ldr    pc, [ip]
  659:   ret_hook_enter:
  660: #endif
  661: 
  662:         ldr    lr, [fp, #0*4]             // lr = function code
  663:         cmp    lr, #0                     //        <  0: system call
  664:         bge    l_esvc_function            //        >= 0: extended system call
  665:  
  666:         /* T-Kernel system call */
  667:         ldr    ip, =TASKINDP              // if called from task-independent portion
  668:         ldr    ip, [ip]           // protection-level check is unnecessary
  669:         cmp    ip, #0
  670:         bhi    l_nochklevel
  671: 
  672:         ldr    ip, =Csym(svc_call_limit)
  673:         ldr    ip, [ip]           // limit value of the ring for call protection
  674:         and    r10, r10, #TMF_CPL(3)      // r10 is taskmode upon entry
  675:         cmp    r10, ip
  676:         bhi    l_oacv_err
  677:   l_nochklevel:
  678: 
  679:         mov    r10, lr, asr #16   // r10 = function number
  680:         ldr    ip, =N_TFN + 0xffff8000
  681:         cmp    r10, ip
  682:         bgt    l_illegal_svc
  683: 
  684:         mov    lr, lr, lsr #8
  685:         and    lr, lr, #0xff              // lr = number of arguments
  686:         subs   lr, lr, #4
  687:         bicle  sp, sp, #7               // align stack module 8 bytes
  688:         ble    l_nocopy
  689:         sub    sp, sp, lr, lsl #2
  690:         bic    sp, sp, #7         // align stack module 8 bytes
  691:   l_param_copy:
  692:         subs   lr, lr, #1
  693:         ldr    ip, [r4, lr, lsl #2]       // copy the arguments (from the fifth and later)
  694:         str    ip, [sp, lr, lsl #2]
  695:         bgt    l_param_copy
  696:   l_nocopy:
  697: 
  698:         ldr    ip, =_svctbl - (0xffff8000 << 2)
  699:         mov    lr, pc
  700:         ldr    pc, [ip, r10, lsl #2]      // T-Kernel system call
  701: 
  702:   l_retsvc:
  703: #if USE_DBGSPT
  704:         ldr    ip, =hook_leave_jmp        // hook processing
  705:         ldr    pc, [ip]
  706:   ret_hook_leave:
  707: #endif
  708: 
  709:         sub    sp, fp, #5*4
  710:         ldmfd  sp!, {r2-r3}             // svc_ssp, taskmode
  711: 
  712:         str    r2, [r9, #TCB_tskctxb + CTXB_svc_ssp]      // restore svc_ssp
  713:         mov    r2, r9                     // r2 = ctxtsk
  714: 
  715:         ldr    ip, =TASKMODE              // restore taskmode
  716:         str    r3, [ip]
  717: 
  718:         ldmfd  sp!, {r9-r10, fp}        // restore working registers
  719: 
  720:         ands   r3, r3, #TMF_CPL(3)       // DCT is not processed if called from
  721:         beq    l_nodct                    // protection level 0
  722: 
  723:         cpsid  IMASK                    // disable interrupt
  724: 
  725:         ldr    ip, =TASKINDP              // DCT is not processed if called from
  726:         ldr    ip, [ip]           // task independent portion
  727:         cmp    ip, #0
  728:         bne    l_nodct
  729: 
  730:         ldr    ip, [r2, #TCB_reqdct]      // is there DCT request?
  731:         cmp    ip, #1
  732:         bne    l_nodct
  733: 
  734:         ldr    ip, =Csym(dispatch_disabled)
  735:         ldr    ip, [ip]           // if dispatch is disabled,
  736:         cmp    ip, #0                     // DCT is not processed
  737:         bne    l_nodct
  738: 
  739:         b      dct_startup          // goto DCT processing
  740: 
  741:   l_nodct:
  742:         EXC_RETURN
  743: 
  744: 
  745:   l_esvc_function:
  746:         /* extended SVC */
  747:         mov    ip, #0
  748:         str    ip, [r9, #TCB_tskctxb + CTXB_svc_ssp]      // svc_ssp = NULL
  749: 
  750:         bic    sp, sp, #7         // align stack module 8 bytes
  751:         mov    r1, lr                     // r1 = function code
  752:         bl     Csym(svc_ientry)    // svc_ientry(pk_para, fncd)
  753: 
  754:         b      l_retsvc
  755: 
  756: 
  757:   l_illegal_svc:
  758:         ldr    r0, =E_RSFN
  759:         b      l_retsvc
  760: 
  761:   l_oacv_err:
  762:         ldr    r0, =E_OACV
  763:         b      l_retsvc
  764: 
  765:                         .data
  766:                         .balign 4
  767:   dummy_svc_ssp:        .long  -1
  768: 
  769: 
  770: #if USE_DBGSPT
  771: /*
  772:  * Invoking hook routine for system call, and extended SVC
  773:  *      void* enter( FN fncd, TD_CALINF *calinf, ... )
  774:  *      void leave( FN fncd, INT ret, void *exinf )
  775:  *
  776:  *      typedef struct td_calinf {
  777:  *              void        *ssp;  system stack pointer
  778:  *              void        *r11;  frame pointer when it is called
  779:  *      } TD_CALINF;
  780:  */
  781:         .text
  782:         .balign        4
  783: hook_enter:
  784:         stmfd  sp!, {r0-r3, r8} // save argument and work registers
  785:         mov    r8, sp                     // r8 = keep the stack position
  786: 
  787:         ldr    ip, [fp, #-1*4]            // frame pointer when it is called
  788:         stmfd  sp!, {fp, ip}            // create TD_CALINF
  789: 
  790:         bic    sp, sp, #7         // align stack module 8 bytes
  791: 
  792:         ldr    lr, [fp, #0*4]             // lr = function code
  793:         cmp    lr, #0                     //        <  0: system call
  794:         bge    he_param2          //      >= 0: extended system call
  795: 
  796:         mov    lr, lr, lsr #8
  797:         and    lr, lr, #0xff              // number of arguments
  798:         subs   lr, lr, #2
  799:         ble    he_param2
  800:         subs   lr, lr, #2
  801:         ble    he_param4
  802:         sub    sp, sp, lr, lsl #2
  803:         bic    sp, sp, #7         // align stack module 8 bytes
  804:   he_param_copy:
  805:         subs   lr, lr, #1
  806:         ldr    ip, [r4, lr, lsl #2]       // Fifth argument (and later)
  807:         str    ip, [sp, lr, lsl #2]
  808:         bgt    he_param_copy
  809:   he_param4:
  810:         str    r3, [sp, #-1*4]!   // forth argument
  811:         str    r2, [sp, #-1*4]!   // third argument
  812:   he_param2:
  813:         mov    r3, r1                     // second argument
  814:         mov    r2, r0                     // first argument
  815:         sub    r1, r8, #2*4               // calinf
  816:         ldr    r0, [fp, #0*4]             // fncd
  817:         ldr    ip, =Csym(hook_enterfn)
  818:         ldr    ip, [ip]
  819:         blx    ip                 // exinf = enter(fncd, ...)
  820:         str    r0, [fp, #-6*4]            // save exinf
  821: 
  822:         mov    sp, r8                     // restore stack position
  823:         ldmfd  sp!, {r0-r3, r8} // restore argument and work registers
  824:         b      ret_hook_enter
  825: 
  826: hook_leave:
  827:         sub    sp, fp, #6*4               // sp = saved position of exinf
  828:         swp    r2, r0, [sp]               // save ret and , restore exinf
  829:         mov    r1, r0                     // ret
  830:         ldr    r0, [fp, #0*4]             // fncd
  831: 
  832:         bic    sp, sp, #7         // align stack module 8 bytes
  833:         ldr    ip, =Csym(hook_leavefn)
  834:         ldr    ip, [ip]
  835:         blx    ip                 // call leave(fncd, ret, exinf)
  836: 
  837:         ldr    r0, [fp, #-6*4]            // restore ret
  838:         b      ret_hook_leave
  839: 
  840: /*
  841:  * Setting and releasing of hook routine for system call and extended SVC
  842:  */
  843:         .text
  844:         .balign        4
  845:         .globl Csym(hook_svc)
  846:         .type  Csym(hook_svc), %function
  847: Csym(hook_svc):
  848:         ldr    r0, =hook_enter_jmp
  849:         ldr    r1, =hook_leave_jmp
  850:         ldr    r2, =hook_enter
  851:         ldr    r3, =hook_leave
  852:         str    r2, [r0]
  853:         str    r3, [r1]
  854:         bx     lr
  855: 
  856:         .globl Csym(unhook_svc)
  857:         .type  Csym(unhook_svc), %function
  858: Csym(unhook_svc):
  859:         ldr    r0, =hook_enter_jmp
  860:         ldr    r1, =hook_leave_jmp
  861:         ldr    r2, =ret_hook_enter
  862:         ldr    r3, =ret_hook_leave
  863:         str    r2, [r0]
  864:         str    r3, [r1]
  865:         bx     lr
  866: 
  867:                         .data
  868:                         .balign      4
  869:   hook_enter_jmp:       .long ret_hook_enter
  870:   hook_leave_jmp:       .long ret_hook_leave
  871: 
  872: #endif /* USE_DBGSPT */
  873: 
  874: /* ------------------------------------------------------------------------ */
  875: 
  876: #if USE_DBGSPT
  877: /*
  878:  * Call entry table for debugger support functions
  879:  */
  880:         .text
  881:         .balign        4
  882: _tdsvctbl:
  883:         .int   Csym(no_support)
  884: #include <sys/svc/tdsvctbl.h>
  885: 
  886: /*
  887:  * Entry routine for debugger support functions
  888:  *              +---------------+
  889:  *      ssp ->       | ip          | function code
  890:  *              | lr                | return address
  891:  *              | SPSR              |
  892:  *              +---------------+
  893:  */
  894:         .text
  895:         .balign        4
  896:         .globl Csym(call_dbgspt)
  897:         .type  Csym(call_dbgspt), %function
  898: Csym(call_dbgspt):
  899:         /* SVC mode / interrupt-disabled CPSR.I=1 A=? F=? */
  900:         ldr    ip, [sp, #2*4]
  901:         and    ip, ip, #PSR_I|PSR_F       // cpsr_c doesn't include PSR_A
  902:         orr    ip, ip, #PSR_SVC
  903:         msr    cpsr_c, ip         // interrupt disabled status is restored to the original state in the caller
  904: 
  905:         stmfd  sp!, {r10, fp}           // save work registers
  906:         add    fp, sp, #2*4
  907: 
  908:         ldr    ip, =TASKINDP              // if called from task-independent portion
  909:         ldr    ip, [ip]           // no need to check protection levels.
  910:         cmp    ip, #0
  911:         bhi    b_nochklevel
  912: 
  913:         ldr    ip, =TASKMODE
  914:         ldr    lr, =Csym(svc_call_limit)
  915:         ldr    ip, [ip]
  916:         ldr    lr, [lr]           // limit value of the ring for call protection
  917:         and    ip, ip, #TMF_CPL(3)        // protection level upon call
  918:         cmp    ip, lr
  919:         bhi    b_oacv_err
  920:   b_nochklevel:
  921: 
  922:         ldr    lr, [fp, #0*4]             // lr = function code
  923:         mov    r10, lr, asr #16
  924:         ldr    ip, =N_TDFN + 0xffff8000
  925:         cmp    r10, ip
  926:         bgt    b_illegal_svc
  927: 
  928:         bic    sp, sp, #7         // align stack module 8 bytes
  929: 
  930:         ldr    ip, =_tdsvctbl - (0xffff8000 << 2)
  931:         mov    lr, pc
  932:         ldr    pc, [ip, r10, lsl #2]      // T-Kernel/DS service call
  933: 
  934:         sub    sp, fp, #2*4               // restore sp
  935: 
  936:   b_retsvc:
  937:         cpsid  IMASK                    // disable interrupt
  938:         ldmfd  sp!, {r10, fp}           // restore work registers
  939:         EXC_RETURN
  940: 
  941: 
  942:   b_illegal_svc:
  943:         ldr    r0, =E_RSFN
  944:         b      b_retsvc
  945: 
  946:   b_oacv_err:
  947:         ldr    r0, =E_OACV
  948:         b      b_retsvc
  949: 
  950: #endif /* USE_DBGSPT */
  951: 
  952: /* ------------------------------------------------------------------------ */
  953: /*
  954:  * High-level language support routine for timer handler
  955:  */
  956:         .text
  957:         .balign        4
  958:         .globl Csym(timer_handler_startup)
  959:         .type  Csym(timer_handler_startup), %function
  960: Csym(timer_handler_startup):
  961:         /* IRQ mode / interrupt-disabled state CPSR.I=1 A=? */
  962:         cpsid  IMASK, #PSR_SVC          // enter SVC mode, interrupt is disabled
  963: 
  964:         stmfd  sp!, {r0-r2, r4-r5, fp, lr}      // save registers
  965:         mov    fp, sp
  966:         bic    sp, sp, #7         // align stack module 8 bytes
  967: 
  968:         ldr    r4, =TASKINDP              // enter task-independent portion
  969:         ldr    r5, [r4]
  970:         add    r0, r5, #1
  971:         str    r0, [r4]
  972: 
  973:         bl     Csym(timer_handler) // call timer_handler()
  974:         /* return, interrupt disabled, CPSR.I=1 A=1 */
  975: 
  976:         str    r5, [r4]           // leave task-independent portion
  977: 
  978:         mov    sp, fp
  979:         ldmfd  sp!, {r0-r2, r4-r5, fp, lr}      // restore registers
  980: 
  981:         TK_RET_INT_FIQ PSR_IRQ         // tk_ret_int()
  982: 
  983: /* ------------------------------------------------------------------------ */
  984: /*
  985:  * delayed context trap(DCT)
  986:  *      invoke task exception handler
  987:  *      the stack looks as follows when this is called.
  988:  *              +---------------+
  989:  *      ssp ->       | R12     = ip        |
  990:  *              | R14_svc = lr      | return address(pc)
  991:  *              | SPSR_svc  |
  992:  *              +---------------+
  993:  */
  994:         .text
  995:         .balign        4
  996: dct_startup:
  997:         /* SVC mode / interrupt-disabled-state CPSR.I=1 A=1 */
  998:         stmfd  sp!, {r0-r3, fp, lr}     // save registers
  999:         add    fp, sp, #6*4
 1000:         bic    sp, sp, #7         // align stack module 8 bytes
 1001: 
 1002:         ldr    ip, [fp, #2*4]             // spsr
 1003:         and    ip, ip, #PSR_M(31) // processor mode of caller
 1004:         cmp    ip, #PSR_USR               // was it user mode?
 1005:         cmpne  ip, #PSR_SYS             // If not system mode,
 1006:         bne    l_nodct_startup            // DCT is not invoked
 1007: 
 1008:         mov    r0, fp
 1009:         bl     Csym(setup_texhdr)  // call setup_texhdr(ssp)
 1010:         /* return with interrupt-enabled state  */
 1011:         cpsid  IMASK                    // Inhibit interrupt
 1012: 
 1013:   l_nodct_startup:
 1014:         sub    sp, fp, #6*4
 1015:         ldmfd  sp!, {r0-r3, fp, lr}     // Restore registers
 1016:         EXC_RETURN
 1017: 
 1018: /*
 1019:  * return from task exception handler
 1020:  *              +---------------+
 1021:  *      usp ->       |PC           | return address from the handler
 1022:  *              |CPSR               | CPSR to be restored on return from the handler
 1023:  *              +---------------+
 1024:  *
 1025:  *              +---------------+
 1026:  *      ssp ->       | R12     = ip        |
 1027:  *              | R14_svc = lr      |  <- copy PC
 1028:  *              | SPSR_svc  |  <- copy CPSR
 1029:  *              +---------------+
 1030:  *
 1031:  *      if an illegal call is made, call default handler.
 1032:  */
 1033:         .text
 1034:         .balign        4
 1035:         .globl Csym(rettex_entry)
 1036:         .type  Csym(rettex_entry), %function
 1037: Csym(rettex_entry):
 1038:         /* SVC mode / interrupt-disabled state CPSR.I=1 A=? F=? */
 1039:         ldr    ip, [sp, #2*4]             // spsr
 1040:         tst    ip, #PSR_A|PSR_I|PSR_F     // call during an interrupt-disabled state is illegal
 1041:         bne    l_illegal_rettex
 1042:         and    ip, ip, #PSR_M(31)
 1043:         cmp    ip, #PSR_USR               // call neither from user mode nor system mode
 1044:         cmpne  ip, #PSR_SYS             // is illegal
 1045:         bne    l_illegal_rettex
 1046: 
 1047:         cpsid  IMASK, #PSR_SYS          // interrupt-disabled state, SYS mode
 1048:         mov    ip, sp                     // ip = usp
 1049: 
 1050:         /* we access user stack, and so we must not be in
 1051:            task-independent portion, and interrupt-disabled state */
 1052:         cpsie  IMASK, #PSR_SVC          // enable interrupt, SVC mode
 1053: 
 1054:         ldr    lr, [ip, #0*4]             // copy usp to ssp
 1055:         str    lr, [sp, #1*4]
 1056:         ldr    ip, [ip, #1*4]             // usp->CPSR
 1057:         ldr    lr, [sp, #2*4]             // ssp->SPSR
 1058:         bic    ip, ip, #PSR_M(31) // make sure mode is legal
 1059:         bic    ip, ip, #PSR_A|PSR_I|PSR_F
 1060:         and    lr, lr, #PSR_M(31)
 1061:         orr    ip, ip, lr
 1062:         str    ip, [sp, #2*4]
 1063: 
 1064:         cpsid  IMASK, #PSR_SYS          // interrupt-disabled, SYS mode
 1065:         add    sp, sp, #2*4               // pop user stack
 1066: 
 1067:         cpsid  IMASK, #PSR_SVC          // intterupt-disabled, SVC mode
 1068:         EXC_RETURN
 1069: 
 1070:   l_illegal_rettex:
 1071:         ldr    ip, =EITVEC(SWI_RETTEX)
 1072:         ldr    lr, =base(EITVEC(EIT_DEFAULT))
 1073:         ldr    lr, [lr, #offs(EITVEC(EIT_DEFAULT))]
 1074:         bx     lr
 1075: 
 1076: /* ------------------------------------------------------------------------ */