asp/arch/arm_m_gcc/common/core_support.S

   1 /*
   2  *  TOPPERS/ASP Kernel
   3  *      Toyohashi Open Platform for Embedded Real-Time Systems/
   4  *      Advanced Standard Profile Kernel
   5  *
   6  *  Copyright (C) 2008-2011 by Embedded and Real-Time Systems Laboratory
   7  *              Graduate School of Information Science, Nagoya Univ., JAPAN
   8  *
   9  *  上記著作権者は，以下の(1)〜(4)の条件を満たす場合に限り，本ソフトウェ
  10  *  ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
  11  *  変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
  12  *  (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
  13  *      権表示，この利用条件および下記の無保証規定が，そのままの形でソー
  14  *      スコード中に含まれていること．
  15  *  (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
  16  *      用できる形で再配布する場合には，再配布に伴うドキュメント（利用
  17  *      者マニュアルなど）に，上記の著作権表示，この利用条件および下記
  18  *      の無保証規定を掲載すること．
  19  *  (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
  20  *      用できない形で再配布する場合には，次のいずれかの条件を満たすこ
  21  *      と．
  22  *    (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
  23  *        作権表示，この利用条件および下記の無保証規定を掲載すること．
  24  *    (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
  25  *        報告すること．
  26  *  (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
  27  *      害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
  28  *      また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
  29  *      由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
  30  *      免責すること．
  31  *
  32  *  本ソフトウェアは，無保証で提供されているものである．上記著作権者お
  33  *  よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
  34  *  に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
  35  *  アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
  36  *  の責任を負わない．
  37  *
  38  *  @(#) $Id: core_support.S 2202 2011-07-26 13:27:11Z ertl-honda $
  39  */
  40
  41 /*
  42  *  プロセッサ依存モジュール アセンブリ言語部（ARM-M用）
  43  */
  44
  45 #define TOPPERS_MACRO_ONLY
  46 #define UINT_C(val)             (val)           /* uint_t型の定数を作るマクロ */
  47 #define ULONG_C(val)    (val)           /* ulong_t型の定数を作るマクロ */
  48 #define CAST(type, val) (val)           /* 型キャストを行うマクロ */
  49
  50 #include "kernel_impl.h"
  51 #include "arm_m.h"
  52 #include "offset.h"
  53
  54
  55 /*
  56  *  タスクディスパッチャ
  57  */
  58         .text
  59         .syntax unified
  60         .code 16
  61         .global dispatch
  62         .type dispatch, function
  63 dispatch:
  64         /*
  65          *
  66          *  このルーチンは，タスクコンテキスト・CPUロック状態・ディパッチ許可状態
  67          *  ・（モデル上の）割込み優先度マスク全開状態で呼び出される．
  68          */
  69         stmfd sp!,{r4-r11,lr}         /* レジスタの保存 */
  70 #if defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)
  71         /* CORTEX-M4Fコア固有の処理 */
  72         mrs   r0, control                         /* CONTROL レジスタを読む */
  73         tst   r0, #CONTROL_FPCA           /* FPコプロセッサを使用したか */
  74         itttt ne
  75         vstmdbne sp!,{s16-s31}                /* 使用したのなら保存 */
  76         bicne   r1,r0, #CONTROL_FPCA    /* タスク切り替えまで浮動小数点レジスタを使わないのでFPCAをクリア */
  77         msrne   control, r1             /* これでディスパッチャ内部での割り込み受付は、FPレジスタを保存しない */
  78         isbne
  79         push  {r0}                                        /* FPCAビットを保存 */
  80 #endif  /* defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)*/
  81         ldr   r0, =p_runtsk           /* p_runtskを読み込む */
  82         ldr   r1, [r0]
  83         str   sp, [r1,#TCB_sp]        /* タスクスタックを保存 */
  84         ldr   lr, =dispatch_r         /* 実行再開番地を保存 */
  85         str   lr, [r1,#TCB_pc]
  86         b     dispatcher
  87
  88         .text
  89         .syntax unified
  90         .code 16
  91         .type dispatch_r, function
  92 dispatch_r:
  93 #if defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)
  94         /* CORTEX-M4Fコア固有の処理 */
  95     pop   {r0}                                    /* FPCAビットを保存 */
  96     tst   r0, #CONTROL_FPCA       /* FPコプロセッサを使用したタスクか */
  97     it    ne
  98     vldmiane  sp!,{s16-s31}               /* 使用したのならレジスタを復帰。これでFPCAビットは1になる */
  99 dispatch_m4f_2:
 100 #endif  /* defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)*/
 101         ldmfd sp!,{r4 - r11,lr}       /* レジスタの復帰 */
 102         /*
 103          * タスク例外処理ルーチンの起動
 104          * dispatcherから呼び出されるため，TCBのアドレスはr1に入っている
 105          */
 106         ldrb  r0,[r1,#TCB_enatex]
 107         tst   r0,#TCB_enatex_mask
 108         beq   dispatch_r_1            /* enatex が false ならリターン */
 109         ldr   r0,[r1,#TCB_texptn]     /* texptn が 0 ならリターン     */
 110         tst   r0,r0
 111         beq   dispatch_r_1
 112         ldr   r1, =ipmflg             /* ipmflgが false ならリターン  */
 113         ldr   r0, [r1]
 114         tst   r0,r0
 115         bne   call_texrtn             /* タスク例外ルーチンの呼び出し */
 116 dispatch_r_1:                     /* タスクへのcall_textnから戻る */
 117         mov   pc,lr
 118
 119
 120 /*
 121  *  CPU例外エントリ
 122  *
 123  *  割込みエントリと処理の内容は同等だが，ログの種類が異なるため，
 124  *  分けている．
 125  */
 126         .align 2
 127         .syntax unified
 128         .code 16
 129         .global core_exc_entry
 130         .type core_exc_entry, function
 131 core_exc_entry:
 132         /*
 133          *  例外/割込みが発生すると，発生時にアクティブなスタックにスクラ
 134          *  ッチレジスタ等が保存される．
 135          *  この内容に加えて，CPU例外ハンドラへの情報として，basepri の値と，
 136          *  EXC_RETURNの情報を加えて保存する．basepriの値は，CPU例外からの
 137          *  リターン時に割込み優先度マスクの値を元に戻すためにも用いられる．
 138          *
 139          *   -----------
 140          *  | EXC_RETURN|
 141          *   -----------
 142          *  |  basepri  |
 143          *   -----------
 144          *  |    R0     |
 145          *   -----------
 146          *  |    R1     |
 147          *   -----------
 148          *  |    R2     |
 149          *   -----------
 150          *  |    R3     |
 151          *   -----------
 152          *  |    R12    |
 153          *   -----------
 154          *  |    LR     |
 155          *   -----------
 156          *  |    PC     |
 157          *   -----------
 158          *  |   xPSR    |
 159          *   -----------
 160          *
 161          */
 162
 163         /*
 164          *  割込み発生時の割込み優先度マスクをスタックに保存するため取得
 165          */
 166         mrs   r2, basepri             /* baepriの値を取得 */
 167
 168         /*
 169          *  カーネル管理外の例外かチェック
 170          *  カーネル内のクリティカルセクションの実行中，全割込みロック状態，
 171          *  CPUロック状態，カーネル管理外の割込みハンドラ実行中のいずれかで
 172          *  発生したCPU例外を，カーネル管理外のCPU例外と呼ぶ
 173          *  CPUロック状態はbasepriがIIPM_LOCKで判断する．
 174          *  全割込みロック状態はFAULTMASKが'1'の場合
 175          */
 176         cmp   r2, #IIPM_LOCK              /* CPUロック状態ならカーネル管理外例外処理へ */
 177         beq   core_nonkernel_exc_entry
 178         mrs   r2, FAULTMASK               /* 全割込みロック状態ならカーネル管理外例外処理へ */
 179         cbnz  r2, core_nonkernel_exc_entry
 180
 181         /*
 182          * スタックを変更する必要があるかチェック
 183          * EXC_RETURN（割込み時にLRに設定される値）をチェックして，例外発生時に
 184          * アクティブなスタックを特定することで多重割込みか判定する．
 185          */
 186         tst   lr, #EXC_RETURN_PSP    /* 割込み元がMSPなら多重割込み */
 187         beq   core_exc_entry_1       /* 多重割込みならcore_exc_entry_1へ */
 188         mrs   r0, psp                /* 一段目の割込みの場合はPSP上に */
 189         stmfd r0!,{r2}               /* 割込み発生時の割込み優先度マスクを積む */
 190         stmfd r0!,{lr}               /* EXC_RETURN を積む             */
 191         msr   psp, r0                /* CPU例外ハンドラへの引数となる */
 192         push  {lr}                   /* MSP上にもEXC_RETURN を積む    */
 193         b     core_exc_entry_2
 194 core_exc_entry_1:                /* 多重割込みの場合 */
 195         push  {r2}                   /* 割込み発生時の割込み優先度マスクを積む */
 196         push  {lr}                   /* EXC_RETURN を積む             */
 197         mov   r0, sp                 /* CPU例外ハンドラへの引数となる */
 198
 199         /*
 200          *  共通処理
 201          */
 202 core_exc_entry_2:
 203         mrs   r3, ipsr               /* ハンドラアドレスを取得 */
 204         ldr   r1, =_kernel_exc_tbl
 205         ldr   r2, [r1, r3, lsl #2]
 206
 207 #ifdef LOG_EXC_ENTER
 208         push  {r0,r2,r3}
 209         mov   r0, r3                 /* 例外番号をパラメータに  */
 210         bl    log_exc_enter          /* log_exc_enterを呼び出す */
 211         pop   {r0,r2,r3}
 212         push  {r3}                   /* 例外番号をスタックへ    */
 213 #endif /* LOG_EXC_ENTER */
 214
 215         /*
 216          *  CPU例外ハンドラの呼び出し
 217          */
 218         blx   r2
 219
 220 #ifdef LOG_EXC_ENTER
 221         pop   {r0}                   /* 例外番号を引数に        */
 222         bl    log_exc_leave          /* log_exc_leaveを呼び出す */
 223 #endif /* LOG_EXC_ENTER */
 224
 225         b     ret_exc
 226
 227 /*
 228  *  カーネル管理外のCPU例外の出入口処理
 229  */
 230 core_nonkernel_exc_entry:
 231         tst   lr, #EXC_RETURN_PSP    /* 割込み元がMSPなら多重割込み */
 232         beq   core_nonkernel_exc_entry_1  /* 多重割込みなら */
 233         mrs   r0, psp                /* 一段目の割込みの場合はPSP上に */
 234         stmfd r0!,{r2}               /* 割込み発生時の割込み優先度マスクを積む */
 235         stmfd r0!,{lr}               /* EXC_RETURN を積む             */
 236         msr   psp, r0                /* CPU例外ハンドラへの引数となる */
 237         push  {lr}                   /* MSP上にもEXC_RETURN を積む    */
 238         b     core_nonkernel_exc_entry_2
 239 core_nonkernel_exc_entry_1:      /* 多重割込みの場合 */
 240         push  {r2}                   /* 割込み発生時の割込み優先度マスクを積む */
 241         push  {lr}                   /* EXC_RETURN を積む             */
 242         mov   r0, sp                 /* CPU例外ハンドラへの引数となる */
 243
 244 core_nonkernel_exc_entry_2:
 245         mrs   r3, ipsr               /* CPU例外ハンドラのアドレスを取得 */
 246         ldr   r1, =_kernel_exc_tbl
 247         ldr   r2, [r1, r3, lsl #2]
 248
 249         /*
 250          *  CPU例外ハンドラの呼び出し
 251          */
 252         blx   r2
 253
 254         /*
 255          *  割込みロック状態とする．
 256          */
 257         cpsid f
 258
 259         /*
 260          *  戻り先のコンテキストの判定
 261          *
 262          *  割込みハンドラ実行にLRにセットされるEXC_RETURNをチェックして，戻り
 263          *  先でMSPが使われていれば，割込み先が非タスクコンテキストと判定する．
 264          */
 265         pop   {r3}                     /* lrをスタックから取得         */
 266         tst   r3, #EXC_RETURN_PSP      /* 戻り先がPSPなら              */
 267         bne   core_nonkernel_ret_exc_1
 268         pop   {r1}                     /* 元の割込み優先度マスク(basepri) */
 269         b     core_nonkernel_ret_exc_2 /* の値をMSPから取得 */
 270
 271 core_nonkernel_ret_exc_1:
 272         /*
 273          *  PSP上からEXC_RETURNを削除
 274          */
 275         mrs   r2, psp
 276         add   r2, r2, #4
 277         /*
 278          *  元の割込み優先度マスク(basepri)の値をPSPから取得
 279          */
 280         ldmfd r2!, {r1}
 281         msr   psp, r2
 282
 283 core_nonkernel_ret_exc_2:
 284         msr   basepri, r1             /* 割込み優先度マスクを割込み前に状態へ */
 285         bx    r3                      /* リターン */
 286
 287 /*
 288  *  割込みエントリ
 289  */
 290         .align 2
 291         .syntax unified
 292         .code 16
 293         .global core_int_entry
 294         .type core_int_entry, function
 295 core_int_entry:
 296         /*
 297          *  割込み発生時の割込み優先度マスクをスタックに保存するため取得
 298          */
 299         mrs   r2, basepri             /* baepriの値を取得 */
 300
 301         /*
 302          * 多重割込みかチェック
 303          * EXC_RETURN（割込み時にLRに設定される値）をチェックして，例外発生時に
 304          * アクティブなスタックを特定することで多重割込みか判定する．
 305          */
 306         tst   lr, #EXC_RETURN_PSP    /* 割込み元がMSPなら多重割込み */
 307         beq   core_int_entry_1       /* 多重割込みならcore_int_entry_1へ */
 308         mrs   r0, psp                /* 一段目の割込みの場合はPSP上に */
 309         stmfd r0!,{r2}               /* 割込み発生時の割込み優先度マスクを積む */
 310         stmfd r0!,{lr}               /* EXC_RETURN を積む             */
 311         msr   psp, r0                /* CPU例外ハンドラへの引数となる */
 312         push  {lr}                   /* MSP上にもEXC_RETURN を積む    */
 313         b     core_int_entry_2
 314 core_int_entry_1:                /* 多重割込みの場合 */
 315         push  {r2}                   /* 割込み発生時の割込み優先度マスクを積む */
 316         push  {lr}                   /* EXC_RETURN を積む             */
 317         mov   r0, sp                 /* 未定義の割込みが発生した場合の情報とする */
 318
 319         /*
 320          *  共通処理
 321          */
 322 core_int_entry_2:
 323         mrs   r3, ipsr               /* ハンドラアドレスを取得 */
 324         ldr   r1, =_kernel_exc_tbl
 325         ldr   r2, [r1, r3, lsl #2]
 326
 327         /*
 328          *  basepriの設定
 329          *  NVIC優先度マスクが自動的に設定されるため優先度マスクの点では必要な
 330          *  いが，x_get_ipm()がbasepriを参照するため，basepriも更新する．
 331          */
 332         ldr   r1, =_kernel_int_iipm_tbl
 333         ldr   lr, [r1, r3, lsl #2]
 334         msr   basepri, lr
 335
 336 #ifdef LOG_INH_ENTER
 337         push  {r0,r2,r3}
 338         mov   r0, r3                 /* 例外番号をパラメータに  */
 339         bl    log_inh_enter          /* log_exc_enterを呼び出す */
 340         pop   {r0,r2,r3}
 341         push  {r3}                   /* 例外番号をスタックへ    */
 342 #endif /* LOG_EXC_ENTER */
 343
 344         /*
 345          *  割込みハンドラの呼び出し
 346          */
 347         blx   r2
 348
 349 #ifdef LOG_INH_LEAVE
 350         pop   {r0}                   /* 例外番号を引数に        */
 351         bl    log_exc_leave          /* log_exc_leaveを呼び出す */
 352 #endif /* LOG_INH_LEAVE */
 353
 354
 355
 356 /*
 357  *  割込み/例外出口
 358  *
 359  *  ret_exc/ret_intは，CPU例外/割込みハンドラから戻った直後に実行する
 360  *  ルーチンである．
 361  */
 362 ret_exc:
 363 ret_int:
 364         /*
 365          *  割込みロック状態とする．この時点では，CPUロック状態にはならない
 366          * （basepriとlock_flagとsaved_iipmは更新しない）．
 367          *
 368          *  割込みロック状態とするのは，戻り先のコンテキストのチェックと，
 369          *  戻り先が非タスクコンテキストであった場合のリターンをアトミック
 370          *  に行うためである．bsepriをCPUロックの値にすることでもアトミッ
 371          *  クなチェックと復帰は可能であるが，割込みからリターンしても，
 372          *  basepri の設定内容は元に戻らないため，使用することができない．
 373          *  一方，FAULTMASKは，割込みからのリターン処理によって，'0'にクリ
 374          *  アされる．
 375          */
 376         cpsid f
 377
 378         /*
 379          *  戻り先のコンテキストの判定
 380          *
 381          *  割込みハンドラ実行にLRにセットされるEXC_RETURNをチェックして，戻り
 382          *  先でMSPが使われていれば，割込み先が非タスクコンテキストと判定する．
 383          */
 384         pop   {r3}                     /* lrをスタックから取得         */
 385         tst   r3, #EXC_RETURN_PSP      /* 戻り先がPSPなら ret_int_1 へ */
 386         bne   ret_int_1
 387         pop   {r1}                     /* 元の割込み優先度マスク(basepri)をr1へ */
 388         b     ret_int_2                /* の値をMSPから取得 */
 389
 390         /*
 391          *  一段目の割込みの出口処理
 392          */
 393 ret_int_1:
 394         /*
 395          *  PSP上から，EXC_RETURN(r0)と元の割込み優先度マスク(basepri)(r1)
 396          *  を取得
 397          */
 398         mrs   r2, psp
 399         ldmfd r2!, {r0,r1}
 400         msr   psp, r2
 401
 402         /*
 403          *  reqflgをチェックする
 404          *
 405          *  カーネル管理内の割込みは禁止した状態で実行する必要があるため，
 406          *  FAULTMASKを'1'にした状態で実行する．
 407          *  reqflgをチェックする前に割込みを禁止するのは，reqflgをチェック
 408          *  した直後に割込みハンドラが起動され，その中でディスパッチが要求
 409          *  された場合に，すぐにディスパッチされないという問題が生じるため
 410          *  である．
 411          */
 412         ldr   r0, =reqflg             /* reqflgがfalseならそのまま戻る */
 413         ldr   r2, [r0]
 414         cbnz  r2, ret_int_3           /* trueならret_int_3へ           */
 415
 416 ret_int_2:
 417         /*
 418          *  ここには割込みロック状態（FAULTMASKがセット）された状態で来る．
 419          *  Threadモードからのリターンにより自動的に割込みロック解除状態になる．
 420          *  割込み優先度マスクは割込み前に状態に戻す．
 421          */
 422         msr   basepri, r1             /* 割込み優先度マスクを割込み前に状態へ */
 423         bx    r3                      /* リターン */
 424
 425 ret_int_3:
 426         /*
 427          *  ここでは，戻り先がタスクであり，PSP上にスクラッチレジスタと割
 428          *  込み優先度マスク(basepri)が保存された状態になっている．また，
 429          *  プロセッサは，Handlerモード・割込みロック状態となっている．
 430          *  また，r0には，reqflgのアドレス，r3には割込み受付時のlrの値が保
 431          *  持されている．
 432          */
 433         /*
 434          *  タスク例外ハンドラやディスパッチをする際にThreadモードへ遷移する
 435          *  ダミーのスタックフレームを作成して，bx命令でHandlerモードからリ
 436          *  ターンする．また，遅延ディスパッチする場合も，再び割り込んだタス
 437          *  クに戻る際には，svc命令で，svc_handlerを呼び出す．
 438          *  スタックフレームは，Configureation and Control Register(CCR)の
 439          *  STKALIGNが'1'の場合は，8byte境界にアラインされる．
 440          *  参考 : DDI0403B_arm_architecture_v7m_reference_manual(P.220)
 441          *  そのため，この時点のスタックは割込みや例外発生時に作成された
 442          *  スタックフレームから，8byte境界のサイズにしておくと，svc_handler
 443          *  等でスタックフレームのアライメントの有無の確認を省略できる．
 444          *  ただし，システム起動後は，動的にCCRのSTKALIGNの設定を変更するのは
 445          *  禁止とする．
 446          *  この時点は標準のスタックフレームは，割込み・例外発生時と同等であ
 447          *  るため，タスクスタック(PSP)は8byte境界になっている．
 448          */
 449         mov   r1, #0                   /* reqflgをfalseに */
 450         str   r1, [r0]
 451
 452         /*
 453          *  CPUロック状態に移行する．
 454          *
 455          *  カーネルの管理内の割込みを禁止するようにbasepriを設定し，
 456          *  lock_flag と saved_iipm を更新する．saved_iipmは，戻り先の割込み
 457          *  優先度マスク（の内部表現）に設定する．
 458          *  この時点でCPUロック状態とするのは，dispatcherへ分岐する時と，
 459          *  call_texrtnを呼び出す時に，CPUロック状態になっている必要がある
 460          *  ためである．
 461          *  なお，この処理の後，Threadモードへの移行処理を行なうため，割込み
 462          *  ロック状態(FAULTMASKを"1")は保持する．
 463          */
 464         ldr   r1, =IIPM_LOCK          /* CPUロック状態 */
 465         msr   basepri, r1
 466         mov   r1, #0x01               /* lock_flag を trueに */
 467         ldr   r0, =lock_flag
 468         str   r1, [r0]
 469
 470         /*
 471          *  割込み優先度マスクを，全解除状態（TIPM_ENAALL）に設定する
 472          *  すでにCPUロック状態なので，saved_iipmをIIPM_ENAALLとする．
 473          */
 474         ldr   r1, =IIPM_ENAALL
 475         ldr   r0, =saved_iipm
 476         str   r1, [r0]
 477
 478         /*
 479          *  Threadモードへ移行する．
 480          *
 481          *  dispatcherやcall_texrnを呼び出す場合は，Threadモードである必
 482          *  要があるため，PSPスタック上にダミーの例外フレームを置いて，
 483          *  擬似的に割込みハンドラからリターンする．
 484          *  リターンと同時にFAULTMASKが自動的にクリアされ，カーネル管理外の
 485          *  割込みが許可される．
 486          */
 487         ldr   r0, =ret_int_4          /* PC   */
 488         ldr   r1, =EPSR_T             /* xPSR(Tビットが'1'である必要がある) */
 489         mrs   r2, psp
 490
 491 #if defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)
 492         tst   r3,#FP_EXC_FRAME                          /* r3はLR。例外フレームには浮動小数点コンテキストを含んでいるか */
 493         it    eq                            /* 含まれている場合の処理。0ならば含まれている */
 494         vstmdbeq r2!,{s16-s31}                          /* preserved FPレジスタを保存。ここでlazystackingは解消される */
 495                                                                                 /* 共通処理 */
 496         stmfd r2!,{r3}                                          /* LRは、後でsvc_handlerから擬似リターンするときに使う */
 497         stmfd r2!,{r3}                                          /* アライメントのために2度プッシュ */
 498         orr   r3,#FP_EXC_FRAME                          /* r3はLR。EXC_RETURNをFPなしフレームに強制する */
 499         /* この先はショート例外フレームである */
 500 #endif  /* defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)*/
 501
 502         stmfd r2!, {r0-r1}            /* ダミーフレームをスタック上に積む   */
 503         sub   r2, #(EXC_FRAME_SIZE - (4*2)) /* r0-r3,r12,lrの内容は設定する必要がない */
 504         msr   psp,r2
 505         bx    r3                      /* Threadモードへ移行 */
 506
 507 ret_int_4:
 508         /*
 509          *  上記の処理により，Threadモードで実行される．
 510          *  dspflgがfalseである場合と，p_runtskとp_schedtskが同じ場合には，
 511          *  ディスパッチを行わない．このチェックが必要なのは，タスク例外処
 512          *  理ルーチンの呼出しが必要な場合に，ディスパッチが必要なくても，
 513          *  reqflgをtrueにするためである．
 514          */
 515         ldr   r0, =p_runtsk       /* ディスパッチを行わない場合でも，r1にp_runtsk の値(TCB) */
 516         ldr   r1, [r0]            /* が入っている必要があるので，先に読み込む */
 517         ldr   r0, =dspflg
 518         ldr   r2, [r0]
 519         cbz   r2, ret_int_r_1     /* dspflgがfalseならret_int_r_1へ */
 520         ldr   r0, =p_schedtsk
 521         ldr   r2, [r0]
 522         cmp   r1, r2              /* p_runtskとp_schedtskが同じなら */
 523         beq   ret_int_r_1         /*                  ret_int_r_1へ */
 524         stmfd sp!, {r4-r11}       /* 残りのレジスタを保存 */
 525         str   sp, [r1,#TCB_sp]    /* タスクスタックを保存 */
 526         ldr   lr, =ret_int_r      /* 実行再開番地を保存   */
 527         str   lr, [r1,#TCB_pc]
 528         b     dispatcher          /* ディスパッチャへ */
 529
 530 /*
 531  * 割込みによりプリエンプトされたタスクへのリターン処理
 532  *
 533  * Threadモードで，ディスパッチャや割込みの出口処理から呼び出される．
 534  * 割込みによりプリエンプトされたタスクへリターンするには，いったん
 535  * Handlerモードに移行し，PCに0xfffffffdを代入してリターンする必要
 536  * がある．そのため，SVCにより，SVCハンドラを呼び出し，Handlerモー
 537  * ドへ移行する．
 538  */
 539         .text
 540         .syntax unified
 541         .code 16
 542         .type ret_int_r, function
 543 ret_int_r:
 544         pop {r4-r11}                  /* レジスタの復帰 */
 545 ret_int_r_1:
 546         /*
 547          *  enatexがtrueで，texptnが0でなければ，タスク例外処理ルーチンを
 548          *  呼び出す．
 549          *  dispatcherから呼び出されるため，TCBのアドレスはr1に入っている
 550          */
 551         ldrb  r0, [r1,#TCB_enatex]
 552         tst   r0, #TCB_enatex_mask
 553         beq   ret_int_r_2           /* enatex が false なら ret_int_r_2へ */
 554         ldr   r0, [r1,#TCB_texptn]  /* texptn が 0 ならリターン     */
 555         cbz   r0, ret_int_r_2
 556         ldr   r1, =ipmflg             /* ipmflgが false ならリターン  */
 557         ldr   r0, [r1]
 558         cbz   r0, ret_int_r_2
 559         bl    call_texrtn           /* タスク例外ルーチンの呼び出し */
 560 ret_int_r_2:
 561         svc   0                     /* SVCの呼び出し */
 562
 563 /*
 564  *  SVCハンドラ
 565  */
 566         .text
 567         .syntax unified
 568         .thumb
 569         .thumb_func
 570         .global svc_handler
 571         .type svc_handler, function
 572 svc_handler:
 573         /*
 574          *  割込み処理からのリターンにより，CPUロック解除状態に移行するよ
 575          *  う準備する．
 576          */
 577         cpsid f                       /* 割込みロック状態へ */
 578         mrs   r0, psp
 579         add   r0, #EXC_FRAME_SIZE     /* スタックを捨てる   */
 580 #if defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)
 581         ldmfd r0!,{lr}
 582         ldmfd r0!,{lr}                                          /* LRはアライメント維持のために二度積まれている */
 583         tst   lr,#FP_EXC_FRAME                          /* 例外フレームには浮動小数点コンテキストを含んでいるか */
 584         it    eq                                                        /* 当該ビットが0なら含んでいる */
 585         vldmiaeq r0!,{s16-s31}                          /* 含んでいるならpreserved FPレジスタを復帰する */
 586 #endif  /* defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)*/
 587         msr   psp, r0
 588         mov   r0, #0
 589         ldr   r1, =lock_flag          /* CPUロック解除状態へ */
 590         str   r0, [r1]
 591         ldr   r1, =IIPM_ENAALL        /* 割込み優先度マスクを全解除状態に設定 */
 592         msr   basepri, r1
 593         bx    lr                      /* リターン     */
 594
 595 /*
 596  *  ディスパッチャの動作開始
 597  */
 598         .align  2
 599         .thumb
 600         .thumb_func
 601         .globl start_dispatch
 602 start_dispatch:
 603         /*
 604          *  このルーチンは，カーネル起動時に，すべての割込みを禁止した状態
 605          * （割込みロック状態と同等）で呼び出される．また，割込みモード（非
 606          *  タスクコンテキストと同等）で呼び出されることを想定している．
 607          *
 608          *  core_initializeで，lock_flagをtrueに，saved_iipmをIIPM_ENAALLに
 609          *  初期化しているため，カーネル管理外の割込みを許可することで，
 610          *  CPUロック状態・（モデル上の）割込み優先度マスク全解除状態になる．
 611          *  また，task_initializeでdisdspをfalseに初期化しているため，ディ
 612          *  スパッチ許可状態になっている．
 613          */
 614         ldr   r0,=istkpt              /* MSPを初期化   */
 615         ldr   r1,[r0]                 /* start_dispatch呼び出し時に呼び出し用に */
 616         msr   msp, r1                 /* 使用しているため初期化する             */
 617         ldr   r1, =IIPM_LOCK          /* カーネル管理内の割込みを禁止 */
 618         msr   basepri, r1
 619         cpsie f                       /* カーネル管理外の割込みを許可 */
 620         mov   r0, #CONTROL_PSP        /* PSPを有効に  */
 621         msr   control, r0
 622         isb                           /* control の操作後に必要 */
 623
 624 /*
 625  *  現在のコンテキストを捨ててディスパッチ
 626  */
 627         .align  2
 628         .thumb
 629         .thumb_func
 630     .globl exit_and_dispatch
 631 exit_and_dispatch:
 632         /* ディスパッチャ本体（dispatcher）へ */
 633
 634
 635 /*
 636  *  ディスパッチャ本体
 637  */
 638 dispatcher:
 639         /*
 640          *  このルーチンは，タスクコンテキスト・CPUロック状態・ディスパッチ
 641          *  許可状態・（モデル上の）割込み優先度マスク全解除状態で呼び出さ
 642          *  れる．
 643          *
 644          *  すなわち，Threadモード・lock_flagがtrue・disdspがfalse・dspflg
 645          *  がtrue・saved_iipmがIIPM_ENAALLとなっている．実行再開番地へもこ
 646          *  の状態のまま分岐する．
 647          */
 648 #ifdef LOG_DSP_ENTER
 649         ldr   r1, =p_runtsk     /* p_runtskをパラメータに */
 650         ldr   r0, [r1]
 651         bl    log_dsp_enter
 652 #endif /* LOG_DSP_ENTER */
 653 dispatcher_0:
 654         ldr   r0, =p_schedtsk   /* p_schedtskをp_runtskに */
 655         ldr   r1, [r0]
 656         ldr   r2, =p_runtsk
 657         str   r1, [r2]
 658         cbz   r1, dispatcher_1  /* p_runtskがNULLならdispatcher_1へ */
 659         ldr   sp, [r1,#TCB_sp]  /* タスクスタックを復帰 */
 660 #ifdef LOG_DSP_LEAVE
 661         mov   r0, r1            /* p_runtskをパラメータに */
 662         mov   r4, r1            /* r1はスクラッチレジスタなので保存 */
 663         bl    log_dsp_leave
 664         mov   r1, r4
 665 #endif /* LOG_DSP_LEAVE */
 666         ldr   pc, [r1,#TCB_pc]  /* 実行再開番地を復帰   */
 667 dispatcher_1:
 668         /*
 669          * CPUロック状態の解除と，非タスクコンテキスト実行状態への
 670          * 準備をする
 671          */
 672         mov   r0, #CONTROL_MSP  /* MSPを有効に  */
 673         msr   control, r0
 674         isb                     /* control の操作後に必要 */
 675         ldr   r2, =reqflg       /* r2 <- reqflg */
 676         ldr   r1, =IIPM_LOCK    /* 割込みロック状態の割込み優先度マスクの値 */
 677         mov   r0, #0
 678         ldr   r4, =lock_flag    /* CPUロック解除状態へ */
 679         str   r0, [r4]
 680 dispatcher_2:
 681         /*
 682          *  割込みを許可し，非タスクコンテキスト実行状態とし割込みを待つ．
 683          *
 684          *  ここで非タスクコンテキスト実行状態に切り換えるのは，ここで発生
 685          *  する割込み処理にどのスタックを使うかという問題の解決と，割込み
 686          *  ハンドラ内でのタスクディスパッチの防止という2つの意味がある．
 687          *
 688          *  プロセッサを割込み待ちに移行させる処理と，割込み許可とは，不可
 689          *  分に行なう必要がある．
 690          *  これを不可分に行なわない場合，割込みを許可した直後に割込
 691          *  みが入り，その中でタスクが実行可能状態になると，実行すべきタス
 692          *  クがあるにもかかわらずプロセッサが割込み待ちになってしまう．
 693          *  ARM-Mでは，PRIMASKをセットした状態でWFIを呼び出すことで実現できる．
 694          *  この状態で割込みが入ると，割込みは実行されず，WFIからリターンす
 695          *  ることになるので，一旦割込みを許可して割込みハンドラを実行する．
 696          *
 697          *  割込み待ちの間は，p_runtskをNULL（＝0）に設定しなければならな
 698          *  い．このように設定しないと，割込みハンドラからiget_tidを呼び出
 699          *  した際の動作が仕様に合致しなくなる．
 700          *
 701          */
 702 #ifdef TOPPERS_CUSTOM_IDLE
 703         toppers_asm_custom_idle
 704 #else
 705         cpsid i               /* PRIMASK をセット */
 706         msr   basepri, r0     /* 全割込み許可 */
 707         wfi
 708         cpsie i               /* PRIMASK をクリア（割込みを受け付ける） */
 709         msr   basepri, r1     /* CPUロック状態へ */
 710 #endif /* TOPPERS_CUSTOM_IDLE */
 711
 712         ldr   r6, [r2]        /* reqflgがfalseならdispatcher_2へ */
 713         cmp   r6, #0
 714         beq   dispatcher_2
 715         str   r0, [r2]        /* reqflgをfalseに */
 716
 717         /*
 718          *  CPUロック状態に戻す．割込み待ちの間に実行した割込みハンドラによ
 719          *  り，saved_iipmが書き換えられる可能性があるため，元の値に戻す必
 720          *  要がある．dispatcherが実行される時は，saved_iipmがIIPM_ENAALL
 721          *  となっているため，ここではsaved_iipmをIIPM_ENAALL（＝0）に戻せ
 722          *  ばよい．
 723          */
 724         mov   r0, #CONTROL_PSP  /* PSPを有効に  */
 725         msr   control, r0
 726         isb                     /* control の操作後に必要 */
 727         mov   r2, #1            /* lock_flagをtrueへ */
 728         ldr   r4, =lock_flag
 729         str   r2, [r4]
 730         ldr   r4, =saved_iipm   /* saved_iipm を0に */
 731         mov   r0, #0
 732         str   r0, [r4]
 733         b     dispatcher_0
 734
 735
 736 /*
 737  *  カーネルの終了処理の呼出し
 738  *
 739  *  スタックを非タスクコンテキスト用に切り替え．
 740  *
 741  */
 742         .text
 743         .syntax unified
 744         .code 16
 745         .globl call_exit_kernel
 746         .type call_exit_kernel, function
 747 call_exit_kernel:
 748         mov   r0, #CONTROL_MSP
 749         msr   control, r0       /* MSPを有効に  */
 750         isb                     /* control の操作後に必要 */
 751         b     exit_kernel       /* カーネルの終了処理を呼ぶ */
 752
 753
 754 /*
 755  *  タスク起動処理
 756  *
 757  *  dispatcherから呼び出されるため，TCBのアドレスはr1に入っている
 758  *
 759  */
 760         .text
 761         .syntax unified
 762         .code 16
 763         .global start_r
 764         .type start_r, function
 765 start_r:
 766 #if defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)
 767                 /* CORTEX-M4Fが浮動小数点演算を行ったか記録するための処理。*/
 768     mrs   r4, control                                           /*CONTROLレジスタの値を取得 */
 769     bic   r4, #CONTROL_FPCA                             /* FPCAビットをクリアして、FPコプロセッサ使用履歴をクリアする */
 770     msr   control, r4
 771     isb
 772 #endif  /* defined(__ARM_ARCH_7EM__) & !defined(__SOFTFP__)*/
 773         /* 以下、CORTEX-M3, M4F共通 */
 774         mov   r0, #0
 775         ldr   r4, =lock_flag                    /* CPUロック解除状態へ */
 776         str   r0, [r4]
 777         msr   basepri, r0                       /* 割込み許可   */
 778         ldr   lr, =ext_tsk                      /* 戻り番地設定 */
 779         ldr   r2, [r1, #TCB_p_tinib]            /* p_runtsk->p_tinibをr2に  */
 780         ldr   r0, [r2, #TINIB_exinf]            /* exinfを引数レジスタr0に  */
 781         ldr   r1, [r2, #TINIB_task]             /* タスク起動番地にジャンプ */
 782         mov   pc, r1
 783
 784 /*
 785  *  微少時間待ち
 786  */
 787         .text
 788         .syntax unified
 789         .code 16
 790         .globl sil_dly_nse
 791         .type sil_dly_nse, function
 792 sil_dly_nse:
 793         sub   r0, r0, #SIL_DLY_TIM1
 794         cmp   r0, #0
 795         bgt   sil_dly_nse1
 796         mov   pc, lr
 797 sil_dly_nse1:
 798         sub   r0, r0, #SIL_DLY_TIM2
 799         cmp   r0, #0
 800         bgt   sil_dly_nse1
 801         mov   pc, lr