gonzui


Format: Advanced Search

tkernel_2/kernel/sysdepend/cpu/em1d/cpu_support.Sbare sourcepermlink (0.04 seconds)

Search this content:

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