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