/*
 *  TOPPERS Software
 *      Toyohashi Open Platform for Embedded Real-Time Systems
 *
 *  Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
 *                              Toyohashi Univ. of Technology, JAPAN
 *  Copyright (C) 2006-2016 by Embedded and Real-Time Systems Laboratory
 *              Graduate School of Information Science, Nagoya Univ., JAPAN
 *  Copyright (C) 2018, 2019 by Naoki Saito
 *             Nagoya Municipal Industrial Research Institute, JAPAN
 *
 *  上記著作権者は，以下の(1)〜(4)の条件を満たす場合に限り，本ソフトウェ
 *  ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
 *  変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
 *  (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
 *      権表示，この利用条件および下記の無保証規定が，そのままの形でソー
 *      スコード中に含まれていること．
 *  (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
 *      用できる形で再配布する場合には，再配布に伴うドキュメント（利用
 *      者マニュアルなど）に，上記の著作権表示，この利用条件および下記
 *      の無保証規定を掲載すること．
 *  (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
 *      用できない形で再配布する場合には，次のいずれかの条件を満たすこ
 *      と．
 *    (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
 *        作権表示，この利用条件および下記の無保証規定を掲載すること．
 *    (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
 *        報告すること．
 *  (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
 *      害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
 *      また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
 *      由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
 *      免責すること．
 *
 *  本ソフトウェアは，無保証で提供されているものである．上記著作権者お
 *  よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
 *  に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
 *  アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
 *  の責任を負わない．
 *
 *  $Id: start.S 384 2019-04-16 11:01:09Z nmir-saito $
 */

#define TOPPERS_MACRO_ONLY
#define UINT64_C(val)   (val)           /* uint64_t型の定数を作るマクロ */
#include "kernel_impl.h"

    .text
    .align 4
    .global start
    .global vectors
start:
    // コア0のみ利用
    mrs x0, mpidr_el1
    and x0, x0, #3
    cbz x0, 2f
1:
    // コア0以外
    wfi
    b   1b
2:
    // これ以降はコア0のみ
    msr daifset, #(AARCH64_DAIF_IRQ | AARCH64_DAIF_FIQ)      // CPSR.{I,F}=1

    /*
     * スタック先頭番地を取り出しておく
     */
    adrp x1, _kernel_istkpt
    add  x1, x1, :lo12:_kernel_istkpt
    ldr  x1, [x1]

    /*
     *  EL毎の処理
     */
    mrs x0, CurrentEL
    and x0, x0, #(0x3 << 2)
    cmp x0, #(0x3 << 2)    //  EL3?
    bne start_el2

    /*
     *  EL3
     */
start_el3:
    mov x2, #0x5b1    // RW=1(EL2 is AArch64), HCE=1, SMD=1, NS=1(non-secure)
    msr scr_el3, x2
    isb

    /* Set up CNTFRQ_EL0 */
    ldr x0, =19200000 // 19.2MHz
    msr CNTFRQ_EL0, x0

    // EL2へ移行
    mov x2, #0x0c9    // I=F=1, MODE=1001 (EL2h)
    msr spsr_el3, x2
    adr x2, start_el2
    msr elr_el3, x2
    eret

    /*
     *  EL2
     */
start_el2:
    cmp x0, #(0x1 << 2)    //  EL2?
    beq start_el1

    msr sp_el1, x1

    mov x0, #(1 << 31)      // RW=1(EL1 is AArch64)
    orr x0, x0, #(1 << 1)   // SWIO hardwired on Pi3
    msr hcr_el2, x0         // TSC=0(SMC is not trapped)
    isb

    mrs x0, cnthctl_el2     // EL1PCTEN=1, EL1PCEN=1
    orr x0, x0, #3
    msr cnthctl_el2, x0
    msr cntvoff_el2, xzr
    isb

    // EL1へ移行
    mov x2, #0x0c5         // I,F=1, mode=0101(EL1h)
    msr spsr_el2, x2
    adr x2, start_el1
    msr elr_el2, x2
    eret

    /*
     *  EL1h
     */
start_el1:
    // スタックポインタとフレームポインタの初期化
    mov sp, x1
    mov x29, xzr

    /*
     *  例外ベクタテーブルのベースアドレス設定
     */
    ldr x0, =vectors
    msr vbar_el1, x0

    /*
     *  bssセクションのクリア
     */
    ldr   x1, =__start_bss
    ldr   x2, =__end_bss
    cmp   x1, x2
    b.hs  4f
3:
    str   xzr, [x1], #8
    cmp   x1, x2
    b.lo  3b

4:
    /*
     *  sta_ker呼び出し
     */
    bl sta_ker
    b .