RZ/A3UL-EBKのコンソールサンプルとQEMUの割り込み

[前回](https://qiita.com/t-yano-tryhard/items/aea076b18dc078d46c6b)で送信用バッファに文字列を設定したところまで進めることが出来ました。

次はコンソール用レジスタへ値を書きこんで
実際に画面に文字列を表示する流れになるのですが
その前に[サンプルプログラム](https://www.renesas.com/jp/ja/document/scd/rza3ul-evk-example-projects?language=en&r=1619666)の動作を確認します。

- [文字列表示の仕組み](#文字列表示の仕組み)
  - [R_SCIF_UART_Write](#r_scif_uart_write)
  - [vectorテーブル](#vectorテーブル)
- [QEMU(arm)の割り込み処理](#qemuarmの割り込み処理)
  - [割り込み発生の条件](#割り込み発生の条件)
    - [arm_excp_unmasked](#arm_excp_unmasked)
    - [条件まとめ](#条件まとめ)
  - [割り込みの発生させかた](#割り込みの発生させかた)
    - [./softmmu/cpus.c:239](#softmmucpusc239)
    - [./accel/tcg/translate-all.c:764](#acceltcgtranslate-allc764)
    - [./accel/tcg/tcg-accel-ops.c:95](#acceltcgtcg-accel-opsc95)
    - [他のHWの割り込み発生状況](#他のhwの割り込み発生状況)
  - [実装方針(現時点)](#実装方針現時点)
  - [割り込み番号の変化のさせ方](#割り込み番号の変化のさせ方)
  - [複数割り込み](#複数割り込み)
    - [qemuのタイマー](#qemuのタイマー)

# 文字列表示の仕組み

文字列表示のためにコールされる関数は`uart_print_user_msg`です。
中身をよく見ると`R_SCIF_UART_Write`で書き込みを行って
次のwhile文で完了待ち合わせを行っています。

なので実際の書き込み関数は`R_SCIF_UART_Write`になります。

##  R_SCIF_UART_Write

`#define` が切られていて見づらいですが
取り除くと以下の様になります。
```c
fsp_err_t R_SCIF_UART_Write (uart_ctrl_t * const p_api_ctrl, uint8_t const * const p_src, uint32_t const bytes)
{
    scif_uart_instance_ctrl_t * p_ctrl = (scif_uart_instance_ctrl_t *) p_api_ctrl;

    /* Transmit interrupts must be disabled to start with. */
    p_ctrl->p_reg->SCR_b.TIE  = 0;
    p_ctrl->p_reg->SCR_b.TEIE = 0;

    p_ctrl->tx_src_bytes = bytes;
    p_ctrl->p_tx_src     = p_src;

    /* Trigger a TXI interrupt */
    p_ctrl->p_reg->SCR_b.TIE = 1;

    return FSP_SUCCESS;
}
```

つまり、ここでやっていることは以下です。
```txt
SCRレジスタを更新
↓
グローバル変数にポインタ等を設定
↓
SCRレジスタを更新
```
しかし、グローバル変数の内容を勝手に読み取って画面に表示されるわけではありません。

実はこの後は`scif_uart_txi_isr`⇒`scif_uart_txi_isr`⇒...⇒`scif_uart_tei_isr`という流れで
処理が行われて`uart_print_user_msg`内のwhileループを抜けることが出来ます。

## vectorテーブル
`scif_uart_txi_isr`が直接コールされている箇所はありませんが
`g_vector_table`に関数ポインタが設定されています。

コール箇所は以下です。
```c
void FIQ_ExecuteHandler (void)
{
<snip>
    if (target_int_id < BSP_ICU_VECTOR_MAX_ENTRIES)
    {
        p_vector = &g_vector_table[target_int_id];
        if (p_vector)
        {
            (*p_vector)(target_int_id);
```

`g_vector_table`のコメントにある通り
`target_int_id`が415の場合`scif_uart_txi_isr`がコールされ
416の場合`scif_uart_tei_isr`がコールされます。

この415や416という値は[`S3_0_C12_C12_0`レジスタ](https://developer.arm.com/documentation/ihi0069/latest/)を見ていることがわかります(*1)。
つまり、ICC_IAR1_EL1です。

`FIQ_ExecuteHandler`は`fiq_handler`からコールされています。
`fiq_handler`は`FiqSpx`からコールされています。
`FiqSpx`はどこからコールされているのかコード上には見当たりません。

ARMでは割り込みが発生するとvector tableを利用して割り込み処理を行います。
[ここ](https://developer.arm.com/documentation/100933/0100/AArch64-exception-vector-table)の`curr_el_spx_fiq`が`FiqSpx`に対応していることがわかります。

(*1) リンク先の
`12.2 AArch64 System register descriptions`に対比表が載っています。
超絶分かりにくいですが、これはまだましなほうで標準のシステムレジスタの場合は
[ここ](https://developer.arm.com/documentation/ddi0601/)からPDFを落としてきて
`0b11 0b000 0b1101 0b0000 0b101`こんな感じのを検索窓に入力して
検索するというとんでもないシステムになっています。


# QEMU(arm)の割り込み処理
qemuのソースコードに適当にgrepをかけると以下がヒットします。

target/arm/cpu.c:684
```c
 684 static bool arm_cpu_exec_interrupt(CPUState *cs, int interrupt_request)
 685 {
 741  found:
 742     cs->exception_index = excp_idx;
 743     env->exception.target_el = target_el;
 744     cc->tcg_ops->do_interrupt(cs);
 745     return true;
```

前後を追っていくと以下の様になっていることがわかります。

```
cpu_exec_loop
└cpu_handle_interrupt
  └cpu_exec_interrupt(arm_cpu_exec_interrupt)
    └do_interrupt(arm_cpu_do_interrupt)
      └arm_cpu_do_interrupt_aarch64
```

`arm_cpu_do_interrupt_aarch64`内で
プログラムカウンタを`vector_table`のアドレスに設定していることが見て取れます。
```c
10962     env->pc = addr;
```

## 割り込み発生の条件
もう少し詳しく見ていきましょう
`cpu_exec_loop`から`cpu_handle_interrupt`への遷移は
tcgのブロック単位で途切れる度に毎回入るのでここは自動です。

`cpu_handle_interrupt`内はどうでしょう？
`cpu->interrupt_request`が0で無いことが条件の一つのようです
```c
 787     if (unlikely(qatomic_read(&cpu->interrupt_request))) {
```

`arm_cpu_exec_interrupt`はもっと単純です。
```c
 696     if (interrupt_request & CPU_INTERRUPT_FIQ) {
 697         excp_idx = EXCP_FIQ;
 698         target_el = arm_phys_excp_target_el(cs, excp_idx, cur_el, secure);
 699         if (arm_excp_unmasked(cs, excp_idx, target_el,
 700                               cur_el, secure, hcr_el2)) {
 701             goto found;
 702         }
 703     }
```

`cpu->interrupt_request`が0x10で`arm_excp_unmasked`が0で無ければ割り込みが発生します。

### arm_excp_unmasked
arm_excp_unmaskedの戻り値は以下です。
どちらかが0で無くなれば割り込みが発生するようです。
```c
 681     return unmasked || pstate_unmasked;
```
遡ってみてみると結局以下だけが有効なことがわかります。
(割り込みでException Levelは変化しないので)
```c
 571     case EXCP_FIQ:
 572         pstate_unmasked = !(env->daif & PSTATE_F);
 573         break;
```

つまり、daifのFのビットが落ちていればtrueが返ります。

### 条件まとめ
まとめると以下のAND条件であることがわかります
```
cpu->interrupt_request <= FIQ(0x10)
cpu->env_ptr->daif & F == 0
```

## 割り込みの発生させかた
これまた適当にgrepを行うと以下でinterrupt_requestを設定していることがわかります。
```c
./softmmu/cpus.c:239:    cpu->interrupt_request |= mask;
./accel/tcg/translate-all.c:764:    cpu->interrupt_request |= mask;
./accel/tcg/tcg-accel-ops.c:95:    cpu->interrupt_request |= mask;
```

順番に見ていきます。

### ./softmmu/cpus.c:239
`cpu_interrupt()` -> `generic_handle_interrupt()`の中です。

### ./accel/tcg/translate-all.c:764
`cpu_interrupt()`の中のようです。

### ./accel/tcg/tcg-accel-ops.c:95
`tcg_handle_interrupt`の中のようです。


### 他のHWの割り込み発生状況
hw/arm内でfiqでgrepしてみるとgicへの接続で発生させているほか
`cpu_interrupt()`を呼ぶことで割り込みを発生させているものがあることに気づきます。
実際に`cpu_interrupt`をコールしてみると
./accel/tcg/tcg-accel-ops.c:95へのルートを通ることがわかります。

## 実装方針(現時点)

SCRのTEIEビットが0の時に
TIEビットが0->1へ変化した場合
`cpu_interrupt()`をコールすることで
とりあえず割り込みを発生させることが出来ることがわかりました。


## 割り込み番号の変化のさせ方
割り込み番号は`ICC_IAR1_EL1`レジスタの値であることがわかっています。
適当にgrepすると以下がヒットします。
```c
2332     { .name = "ICC_IAR1_EL1", .state = ARM_CP_STATE_BOTH,
2333       .opc0 = 3, .opc1 = 0, .crn = 12, .crm = 12, .opc2 = 0,
2334       .type = ARM_CP_IO | ARM_CP_NO_RAW,
2335       .access = PL1_R, .accessfn = gicv3_irq_access,
2336       .readfn = icc_iar1_read,
2337     },
```

しかし、read関数のみでwrite関数が見当たりません。
とりあえずread関数を見てみます。

HCR_IMOとかいうのが無ければ`cs->hppi.irq`が読み出す値のようです。
[この辺](https://developer.arm.com/documentation/ddi0595/2021-06/AArch64-Registers/HCR-EL2--Hypervisor-Configuration-Register?lang=en#fieldset_0-4_4)を見る限りどうも関係無さそうです。
また、汎用の書き込み関数も見当たりません。
とりあえずの疎通のため、無理やり設定してみます。

コードイメージ
```c
static uint16_t scr = 0;
static void a3ul_mmio_write(void *opaque, hwaddr addr, uint64_t value, unsigned size) {
    hwaddr paddr = addr | RZA_MMIO_BASE;
    uint32_t offset = addr & 0xff;
    uint16_t vs =  (uint16_t)value;
    RzA3ulState *s = (RzA3ulState*)opaque;
    GICv3CPUState *cs = &s->gic.cpu[0];
    CPUState *cpu = CPU(ARM_CPU(first_cpu));

    if ((paddr & 0xFFFFFF00) == RZG2L_SCIF0_BASE) {
         if (offset == SCR) {
             if (!(scr & SCR_TEIE) && !(scr & SCR_TIE) && (vs & SCR_TIE)) {
                 cs->hppi.irq = 415;
                 cpu_interrupt(cpu, CPU_INTERRUPT_FIQ);
             } 
             scr = vs;
         }
    }
}
```

動かないです。
デバッガで見てみると`icv_hppi_can_preempt`で弾かれているようです。
なのでそこも通してみます。

```c
static uint16_t scr = 0;
static void a3ul_mmio_write(void *opaque, hwaddr addr, uint64_t value, unsigned size) {
    hwaddr paddr = addr | RZA_MMIO_BASE;
    uint32_t offset = addr & 0xff;
    uint16_t vs =  (uint16_t)value;
    RzA3ulState *s = (RzA3ulState*)opaque;
    GICv3CPUState *cs = &s->gic.cpu[0];
    CPUState *cpu = CPU(ARM_CPU(first_cpu));

    if ((paddr & 0xFFFFFF00) == RZG2L_SCIF0_BASE) {
         if (offset == SCR) {
             if (!(scr & SCR_TEIE) && !(scr & SCR_TIE) && (vs & SCR_TIE)) {
                 cs->hppi.irq = 415;
                 cs->hppi.prio = 0xc0;
                 cs->hppi.grp = 1;
                 cs->icc_igrpen[cs->hppi.grp] = 415;

                 cpu_interrupt(cpu, CPU_INTERRUPT_FIQ);
             } 
             scr = vs;
         }
    }
}
```
これで`scif_uart_txi_isr`に入ってきました。

## 複数割り込み
[FSP](https://www.renesas.com/jp/ja/document/scd/rza3ul-evk-example-projects?language=en&r=1619666)のコードを見ると
`scif_uart_txi_isr`を文字列数 / 16文字分繰り返した後
`scif_uart_tei_isr`のコールが必要なことがわかります。

`scif_uart_txi_isr`は以下です。
```c
void scif_uart_txi_isr (IRQn_Type const irq)
{
    /* Save context if RTOS is used */
    FSP_CONTEXT_SAVE;

    /* Clear pending IRQ to make sure it doesn't fire again after exiting */
    R_BSP_IrqStatusClear(irq);

    /* Recover ISR context saved in open. */
    scif_uart_instance_ctrl_t * p_ctrl = (scif_uart_instance_ctrl_t *) R_FSP_IsrContextGet(irq);

    if ((NULL == p_ctrl->p_cfg->p_transfer_tx) && (0U != p_ctrl->tx_src_bytes))
    {
        /* Write 1byte (uint8_t) data to (uint8_t) data register */
        p_ctrl->p_reg->FTDR = *(p_ctrl->p_tx_src);

        /* Update pointer to the next data and number of remaining bytes in the control block. */
        p_ctrl->tx_src_bytes -= 1;
        p_ctrl->p_tx_src     += 1;

        /* If transfer is not used, write data until FIFO is full. */
        uint32_t fifo_count = (uint32_t) p_ctrl->p_reg->FDR_b.T;
        for (uint32_t cnt = fifo_count; (cnt < SCIF_UART_TX_FIFO_STAGES) && p_ctrl->tx_src_bytes; cnt++)
        {
            p_ctrl->p_reg->FTDR = *p_ctrl->p_tx_src;

            p_ctrl->tx_src_bytes -= 1;
            p_ctrl->p_tx_src     += 1;
        }

        /* Clear TDFE flag */
        p_ctrl->p_reg->FSR_b.TDFE = 0U;
    }

    if (0U == p_ctrl->tx_src_bytes)
    {
        /* After all data has been transmitted, disable transmit interrupts and enable the transmit end interrupt. */
        uint32_t scr_temp;
        scr_temp           = p_ctrl->p_reg->SCR;
        scr_temp          |= R_SCIFA0_SCR_TEIE_Msk;
        scr_temp          &= (uint32_t) (~R_SCIFA0_SCR_TIE_Msk);
        p_ctrl->p_reg->SCR = (uint16_t) scr_temp;

        p_ctrl->p_tx_src = NULL;
        r_scif_uart_call_callback(p_ctrl, 0U, UART_EVENT_TX_DATA_EMPTY);
    }

    /* Restore context if RTOS is used */
    FSP_CONTEXT_RESTORE;
}
```

文字列をFTDRに出力し終わったら
SCRのTIEビットを落とし、TEIEビットを立てていることがわかります。

従って、該当部分のレジスタ操作時に割り込みを発生させれば良いのですが
よく見るとそれではダメなことがわかります。

待ち合わせ部分である`uart_print_user_msg`の条件判定は以下です。
```c
    while ((UART_EVENT_TX_COMPLETE != g_uart_event) && (--local_timeout))
```
`g_uart_event`の変更は`user_uart_callback`のみで行われます。
```c
245 void user_uart_callback(uart_callback_args_t *p_args)
246 {
247     /* Logged the event in global variable */
248     g_uart_event = (uint8_t)p_args->event;
```
`user_uart_callback`は`g_uart0_cfg.p_callback`として設定されています。
```c
const uart_cfg_t g_uart0_cfg =
{ .channel = 0, .data_bits = UART_DATA_BITS_8, .parity = UART_PARITY_OFF, .stop_bits = UART_STOP_BITS_1, .p_callback =
          user_uart_callback,
  .p_context = NULL, .p_extend = &g_uart0_cfg_extend,
```
`g_uart0_cfg`はいろいろと周りくどいのですが最終的に`gp_renesas_isr_context`に設定されます。
従って、`R_FSP_IsrContextGet`コール時に取得出来るものは`g_uart0_cfg`になり、
`r_scif_uart_call_callback`をコールしたときに呼ばれる`p_ctrl->p_callback`は
`user_uart_callback`になります。

整理すると、SCRの該当レジスタ変化で割り込みを発生させると
以下の様になってしまいます。

```plantuml
@startuml

participant  uart_print_user_msg

uart_print_user_msg -> scif_uart_txi_isr : 割り込み(415)
scif_uart_txi_isr -> scif_uart_tei_isr :割り込み(416)
scif_uart_tei_isr -> g_uart_event : UART_EVENT_TX_COMPLETE
scif_uart_tei_isr --> scif_uart_txi_isr : 割り込み復帰
scif_uart_txi_isr -> g_uart_event : UART_EVENT_TX_DATA_EMPTY
scif_uart_txi_isr --> uart_print_user_msg : 割り込み復帰
uart_print_user_msg -> g_uart_event : チェック
note over uart_print_user_msg, g_uart_event
  UART_EVENT_TX_COMPLETEが来るまで待ち合わせているので
  whileループを抜けられない
end note
g_uart_event --> uart_print_user_msg : UART_EVENT_TX_DATA_EMPTY

@enduml
```

これの対策のためには、`UART_EVENT_TX_DATA_EMPTY`の設定が終わってから
`UART_EVENT_TX_COMPLETE`の設定を行う必要があります。

今回はこのためにqemuで用意されているタイマーを使用します。
つまり`scif_uart_txi_isr`で`UART_EVENT_TX_DATA_EMPTY`の設定と
割り込み復帰が終わってから416の割り込みが発生するように
レジスタ操作から割り込み発生までに遅延を設けます。

そうすると以下の様になります。


```plantuml
@startuml

participant  uart_print_user_msg
participant scif_uart_txi_isr
participant scif_uart_tei_isr
participant g_uart_event

uart_print_user_msg -> scif_uart_txi_isr : 割り込み(415)
scif_uart_txi_isr -> timer : 割り込み発生登録(416)
scif_uart_txi_isr -> g_uart_event : UART_EVENT_TX_DATA_EMPTY
scif_uart_txi_isr --> uart_print_user_msg : 割り込み復帰

timer -> scif_uart_tei_isr :割り込み(416)
scif_uart_tei_isr -> g_uart_event : UART_EVENT_TX_COMPLETE
scif_uart_tei_isr --> timer : 割り込み復帰
uart_print_user_msg -> g_uart_event : チェック
note over uart_print_user_msg, g_uart_event
  UART_EVENT_TX_COMPLETEになっているので
  whileループを抜けられる
end note
g_uart_event --> uart_print_user_msg : UART_EVENT_TX_COMPLETE

@enduml
```

### qemuのタイマー
qemuのタイマーは`timer_new_ms`で初期化を行います。
また、発火時間の設定は`timer_mod`で行います。

コード例は以下です。
```c
    mng.timer = timer_new_ms(QEMU_CLOCK_REALTIME, fiq_register_main, NULL);
    int64_t now = qemu_clock_get_ms(QEMU_CLOCK_REALTIME);
    timer_mod(mng.timer, now + 100);
```
こうすることで100ms後にあらかじめ登録した`fiq_register_main`がコールされます。

これを踏まえて改めて`rza3ul.c`を修正します。

<details>
<summary>rza3ul.c</summary>

```c
#include "qemu/osdep.h"
#include "qemu/datadir.h"
#include "qemu/log.h"
#include "kvm_arm.h"
#include "hw/loader.h"
#include "qemu/units.h"
#include "exec/address-spaces.h"
#include "qapi/error.h"
#include "target/arm/cpregs.h"
#include "hw/boards.h"
#include "hw/qdev-properties.h"
#include "hw/arm/rza3ul.h"
#include "elf.h"

#include <stdio.h>
#include <pthread.h>
#include <stdbool.h>
#include <unistd.h>
#include <sys/queue.h>
#include <time.h>
#include <stdlib.h>


#define RZA_MMIO_BASE               (0x10000000)
#define RZG2L_SCIF0_BASE            (0x1004B800)
#define RZG2L_SPIMULT_BASE          (0x10060000)
#define RZG2L_SYC_BASE              (0x11000000)
#define CPG_BASE                                        (0x11010000)            /* CPG base address */
#define RZG2L_SYSC_BASE             (0x11020000)
#define RZG2L_TZC_SPI_BASE          (0x11060000)
#define RZG2L_TZC_DDR_BASE          (0x11070000)
#define RZG2L_DDR_PHY_BASE          (0x11400000)
#define RZG2L_DDR_MEMC_BASE         (0x11410000)

// SYC
#define CNTFID0         (0x020)

// TZC
#define PID0_OFF                                        (0xfe0)
#define PID1_OFF                                        (0xfe4)
#define GATE_KEEPER_OFF                         (0x008)

#define CPG_PLL4_STBY                           (CPG_BASE + 0x0010)     /* PLL4 (SSCG) standby control register */
#define CPG_PLL4_CLK1                           (CPG_BASE + 0x0014)     /* PLL4 (SSCG) output clock setting register 1 */
#define CPG_PLL4_MON                            (CPG_BASE + 0x001C)     /* PLL4 (SSCG) monitor register */
#define CPG_PLL6_STBY                           (CPG_BASE + 0x0020)     /* PLL6 (SSCG) standby control register */
#define CPG_PLL6_CLK1                           (CPG_BASE + 0x0024)     /* PLL6 (SSCG) output clock setting register 1 */
#define CPG_PLL6_MON                            (CPG_BASE + 0x002C)     /* PLL6 (SSCG) monitor register */

#define CPG_CLKON_CA55                          (CPG_BASE + 0x0500)     /* Clock ON / OFF register CA55 */
#define CPG_CLKON_OCTA                          (CPG_BASE + 0x05F4)     /* Clock ON / OFF register OCTA */
#define CPG_CLKMON_CA55                         (CPG_BASE + 0x0680)     /* Clock monitor register CA55 */
#define CPG_RST_CA55                            (CPG_BASE + 0x0800)     /* Reset ONOFF register CA55 */
#define CPG_RST_OCTA                            (CPG_BASE + 0x08F4)     /* Reset ONOFF register OCTA */
#define CPG_RSTMON_CA55                         (CPG_BASE + 0x0980)     /* Reset monitor register CA55 */

#define PLL4_STBY_RESETB                                                        (1 << 0)
#define PLL4_STBY_RESETB_WEN                                            (1 << 16)
#define PLL4_MON_PLL4_RESETB                                            (1 << 0)
#define PLL4_MON_PLL4_LOCK                                                      (1 << 4)
#define PLL6_STBY_RESETB                                                        (1 << 0)
#define PLL6_STBY_RESETB_WEN                                            (1 << 16)
#define PLL6_MON_PLL6_RESETB                                            (1 << 0)
#define PLL6_MON_PLL6_LOCK                                                      (1 << 4)

// DDR
#define DENALI_CTL_00       (0x0000)
#define DENALI_CTL_56       (0x00E0)
#define DENALI_CTL_59       (0x00EC)
#define DENALI_CTL_67       (0x010C)
#define DENALI_CTL_146      (0x0248)
#define DENALI_CTL_147      (0x024C)
#define DDRPHY_R18          (0x100)
#define DDRPHY_R27                      (0x124)
#define DDRPHY_R36          (0x148)
#define DDRPHY_R37          (0x14c)
#define DDRPHY_R42                      (0x160)
#define DDRPHY_R46                      (0x170)

#define DDRMC_R000      DENALI_CTL_00
#define DDRMC_R004      DENALI_CTL_56
#define DDRMC_R005      DENALI_CTL_59
#define DDRMC_R008      DENALI_CTL_67
#define DDRMC_R021      DENALI_CTL_146
#define DDRMC_R022      DENALI_CTL_147

// spi
#define SPIM_CMNSR      0x0048

// scif
#define SCR                             (0x04)
#define FTDR                    (0x06)
#define FSR             (0x08)
#define FDR                             (0x0E)
#define FSR_TEND_SHIFT      (6)
#define FSR_TEND            (1<<FSR_TEND_SHIFT)
#define SCR_TIE     0x80
#define SCR_TEIE    0x04
#define FSR_TDFE    0x20

typedef struct str_fiq_a3ul {
    TAILQ_ENTRY(str_fiq_a3ul) entry;
    GICv3CPUState *cs;
    uint32_t ms;
    uint16_t fiqno;
} fiq_a3ul_t;

typedef struct {
    TAILQ_HEAD(tq_head, str_fiq_a3ul) head;
    QEMUTimer *timer;
} mng_fiq_t;
static void create_gic(RzA3ulState *s);
static uint64_t a3ul_mmio_read(void *opaque, hwaddr addr, unsigned size);
static void a3ul_mmio_write(void *opaque, hwaddr addr, uint64_t value, unsigned size);
static uint64_t rza3ul_uart(void* opaque, uint64_t addr, uint64_t value, unsigned size, int type);
static void tzc_init(void);
static void spi_init(void);
static int64_t timediff_ms(struct timespec st, struct timespec en);
static void fiq_register_main(void* arg);
void fiq_interrupt(void* opaque);
void delay_fiq_interrupt(GICv3CPUState *cs, uint16_t fiqno, uint32_t ms); 

static MemoryRegionOps mmio_op  = {
    .read =  a3ul_mmio_read,
    .write = a3ul_mmio_write,
    .endianness = DEVICE_LITTLE_ENDIAN 
};

static uint32_t cpg[0x10000] = {};
static uint32_t syc[0x10000] = {};
static uint32_t tzcd[0x1000] = {};
static uint32_t tzcs[0x1000] = {};
static uint32_t ddrp[0x10000] = {};
static uint32_t ddrm[0x10000] = {};
static uint32_t spirega[0x10000] = {};
static volatile unsigned short fsr = 0;
static uint16_t scr = 0;
static mng_fiq_t mng;

static void rza3ul_init(MachineState *machine)
{
    RzA3ulState *s = RZA3UL(machine);
    ssize_t image_size;
    char *sysboot_filename;
    MemoryRegion *sysmem;
    MemoryRegion *sram;
    MemoryRegion *ram;
    MemoryRegion *mmio;
    MemoryRegion *spi;
    AddressSpace *as;
    CPUState *cs;
    uint64_t entry;
    char *kfn = 0;
    if (machine->kernel_filename) kfn = strdup(machine->kernel_filename);
    Object *cpuobj = object_new(machine->cpu_type);

    MemoryRegionOps *ops = &mmio_op;
    
    if (object_property_find(cpuobj, "has_el3"))
        object_property_set_bool(cpuobj, "has_el3", true, &error_fatal);
    if (object_property_find(cpuobj, "has_el2"))
        object_property_set_bool(cpuobj, "has_el2", true, &error_fatal);

    qdev_realize(DEVICE(cpuobj), NULL, &error_fatal);
    s->cpus[0] = ARM_CPU(first_cpu);

    create_gic(s);

    sysmem = get_system_memory();
    sram = g_new(MemoryRegion, 1);
    mmio = g_new(MemoryRegion, 1);
    spi = g_new(MemoryRegion, 1);
    ram = g_new(MemoryRegion, 1);

    memory_region_init_ram(sram, NULL, "rza3ul.sram", 0x0100000, &error_fatal);
    memory_region_add_subregion(sysmem, 0, sram);
    memory_region_init_io(mmio, OBJECT(s), ops, s, "rza3ul.mmio", 0x10000000);
    memory_region_add_subregion(sysmem, RZA_MMIO_BASE, mmio);
    memory_region_init_ram(spi, NULL, "rza3ul.spi", 0x10000000, &error_fatal);
    memory_region_add_subregion(sysmem, 0x20000000, spi);
    memory_region_init_ram(ram, NULL, "rza3ul.ram", 0x80000000, &error_fatal);
    memory_region_add_subregion(sysmem, 0x40000000, ram);

    tzc_init();
    spi_init();
    if (machine->firmware) {
        g_print("has firmware %s\n", machine->firmware);
        sysboot_filename = qemu_find_file(QEMU_FILE_TYPE_BIOS, machine->firmware);
        if (sysboot_filename != NULL) {
            if (load_image_targphys(sysboot_filename, 0, 0x8000) < 0) {
                error_report("%s load failed", sysboot_filename);
                exit(1);
            }
            g_free(sysboot_filename);
        } else {
            error_report("BIOS not supported for this machine");
            exit(1);
        }
    }

    if (kfn) {
        cs = CPU(ARM_CPU(first_cpu));
        as = cpu_get_address_space(cs, ARMASIdx_S);
        image_size = load_elf_as(kfn, NULL, NULL, NULL, &entry, NULL, NULL, NULL, false, EM_AARCH64, true, false, as);
        if (image_size < 0) {
            g_print("kernel load failed(%ld)\n", image_size);
        } else {
            g_print("kernel:%s\n", kfn);
        }
        free(kfn);
    }

    TAILQ_INIT(&mng.head);
    mng.timer = timer_new_ms(QEMU_CLOCK_REALTIME, fiq_register_main, NULL);
    int64_t now = qemu_clock_get_ms(QEMU_CLOCK_REALTIME);
    timer_mod(mng.timer, now + 100);
}

static void create_gic(RzA3ulState *s) {
    uint32_t smp_cpus = MACHINE(s)->smp.cpus;
    SysBusDevice *gicbusdev;
    DeviceState *gicdev;

    object_initialize_child(OBJECT(s), "gic", &s->gic, gicv3_class_name());
    gicdev = DEVICE(&s->gic);

    qdev_prop_set_uint32(gicdev, "revision", 3);
    qdev_prop_set_uint32(gicdev, "num-cpu", smp_cpus);
    qdev_prop_set_uint32(gicdev, "num-irq", 32 * 31);
    qdev_prop_set_uint32(gicdev, "len-redist-region-count", 1);
    qdev_prop_set_uint32(gicdev, "redist-region-count[0]", smp_cpus);

    gicbusdev = SYS_BUS_DEVICE(&s->gic);
    sysbus_realize(gicbusdev, &error_fatal);
}

static uint64_t a3ul_mmio_read(void *opaque, hwaddr addr, unsigned size) {
    hwaddr paddr = addr | RZA_MMIO_BASE;
    uint32_t offset = paddr & 0xFFFF;
    uint32_t *tzc = tzcs;
    uint32_t *ddr = ddrm;
    uint64_t value = 0;

    switch(paddr & 0xFFFF0000)  {
        case RZG2L_SYC_BASE:
            if (offset == CNTFID0)
                return 24000000;
            return 0;
        case RZG2L_SYSC_BASE:
            if (addr == 0x1020a00)
                return 0x203;
            return 0;
        case CPG_BASE:
            return cpg[offset];
        case RZG2L_TZC_DDR_BASE:
            tzc = tzcd;
            /* Falls through. */
        case RZG2L_TZC_SPI_BASE:
            return tzc[offset];
        case RZG2L_DDR_PHY_BASE:
            ddr = ddrp;
            /* Falls through. */
        case RZG2L_DDR_MEMC_BASE:
            return ddr[offset];
        case RZG2L_SPIMULT_BASE:
            return spirega[offset];
    }
    if ((paddr & 0xFFFFFF00) == RZG2L_SCIF0_BASE) {
        value = rza3ul_uart(opaque, paddr, 0, size, 0);
    }
    return value;
}

static void a3ul_mmio_write(void *opaque, hwaddr addr, uint64_t value, unsigned size) {
    hwaddr paddr = addr | RZA_MMIO_BASE;
    uint32_t offset = paddr & 0xFFFF;

    uint32_t wen;
    uint32_t proc;
    uint32_t *tzc = tzcs;
    uint32_t bef;
    uint32_t *p;
    uint32_t *ddr = ddrm;

    switch(paddr & 0xFFFF0000) {
        case CPG_BASE:
            cpg[offset] = (uint32_t)value;
            proc = 1;
            switch (paddr) {
                case CPG_PLL4_STBY:
                    if (value & PLL4_STBY_RESETB_WEN) {
                        if (value & PLL4_STBY_RESETB) 
                            cpg[CPG_PLL4_MON & 0xffff] |= (PLL4_MON_PLL4_RESETB | PLL4_MON_PLL4_LOCK);
                        else 
                            cpg[CPG_PLL4_MON & 0xffff] &= ~(PLL4_MON_PLL4_RESETB | PLL4_MON_PLL4_LOCK);
                    }
                    break;
                case CPG_PLL6_STBY:
                    if (value & PLL6_STBY_RESETB_WEN) {
                        if (value & PLL6_STBY_RESETB) 
                            cpg[CPG_PLL6_MON & 0xffff] |= (PLL6_MON_PLL6_RESETB | PLL6_MON_PLL6_LOCK);
                        else 
                            cpg[CPG_PLL6_MON & 0xffff] &= ~(PLL6_MON_PLL6_RESETB | PLL6_MON_PLL6_LOCK);
                    }
                    break;
                default:
                    proc = 0;
                    break;
            }
            if (!proc && ((CPG_CLKON_CA55 <= paddr) && (paddr <= CPG_CLKON_OCTA))) {
                wen = ((value & 0xFFFF0000) >> 16);
                if (wen) { // reset 
                    p = &cpg[(paddr + (CPG_CLKMON_CA55 - CPG_CLKON_CA55)) & 0xffff];
                    bef = *p; 
                    *p = (wen & (value & 0xFFFF));
                }
            } else if (!proc && ((CPG_RST_CA55 <= paddr) && (paddr <= CPG_RST_OCTA))) {
                wen = ((value & 0xFFFF0000) >> 16);
                if (wen) { // reset 
                    p = &cpg[(paddr + (CPG_RSTMON_CA55 - CPG_RST_CA55)) & 0xffff];
                    bef = *p;
                    *p = (wen ^ (value & 0xFFFF));
                }
            }
            break;
        case RZG2L_SYC_BASE:
            syc[offset] = (uint32_t)value;
            break;
        case RZG2L_TZC_DDR_BASE:
            tzc = tzcd;    
            /* Falls through. */
        case RZG2L_TZC_SPI_BASE:
            if (offset == GATE_KEEPER_OFF) {
                if (value) 
                    tzc[offset] |= (value << 16);
                else
                    tzc[offset] = 0;
            } else {
                tzc[offset] = value;
            }
            break;
        case RZG2L_DDR_PHY_BASE:
            ddr = ddrp;
            if (offset == DDRPHY_R46 ){
                ddr[offset] = ((uint32_t)value) << 2;
            } else {
                ddr[offset] = value;
                if (offset == DDRPHY_R27 && (value & (~(uint32_t)0xFBFFFFFF)) == 0) {
                    ddr[DDRPHY_R42] = 3;
                } else if ((offset == DDRPHY_R18) && ((value & 0x10000000) == 0x10000000)) {
                    ddr[DDRPHY_R18] &= ~(uint32_t)0x10000000;
                    if (value == 0x50200000) {
                        ddr[DDRPHY_R36] |= 3;
                        ddr[DDRPHY_R37] &= ~(uint32_t)3;
                    } else if (value == 0x34200000) {
                        ddr[DDRPHY_R18] |= 3;
                    }
                }
            }
            break;
        case RZG2L_DDR_MEMC_BASE:
            ddr[offset] = (uint32_t)value;
            if ((offset == DDRMC_R000) && ((value & 1) == 1)) {
                ddr[DDRMC_R021] |= 0x02000000;
            } else if ((offset == DDRMC_R008) && ((value & 0x02800000) == 0x02800000)) {
                ddr[DDRMC_R022] |= (uint32_t)(1 << 3);
            } else if (offset == DDRMC_R004) {
                bef = ddr[DDRMC_R005];
                if ((value & 0x11) == 0x11) {
                    ddr[DDRMC_R005] = (bef & ~(uint32_t)0x7f000000) | 0x48000000;
                } else if ((value & 0x2) == 0x2) {
                    ddr[DDRMC_R005] = (bef & ~(uint32_t)0x7f000000) | 0x40000000;
                }
            }
            break;
        case RZG2L_SPIMULT_BASE:
            spirega[offset] = (uint32_t)value;
            break;
    }
    if ((paddr & 0xFFFFFF00) == RZG2L_SCIF0_BASE) {
        rza3ul_uart(opaque, paddr, value, size, 1);
    }
}

uint8_t prio = 0xc0;
struct timespec before = {0};

static void fiq_register_main(void* arg) {
    fiq_a3ul_t *p;
    struct timespec now;
    int64_t diff;
    bool has_guard_time = false;

    p = (fiq_a3ul_t*)mng.head.tqh_first;
    if (p) {
        clock_gettime(CLOCK_REALTIME, &now);
        if (before.tv_sec) {
            before = now;
            has_guard_time = true;
        } else {
            diff = timediff_ms(before, now);
            has_guard_time = (diff >= 100);
        }
        if (has_guard_time) {
            TAILQ_REMOVE(&mng.head, p, entry);
            fiq_interrupt(p);
        }
    }
    int64_t n = qemu_clock_get_ms(QEMU_CLOCK_REALTIME);
    timer_mod(mng.timer, n + 100);
}
void delay_fiq_interrupt(GICv3CPUState *cs, uint16_t fiqno, uint32_t ms) {
    fiq_a3ul_t *s;
    
    s = malloc(sizeof(*s));

    s->cs = cs;
    s->fiqno = fiqno;
    s->ms = ms;

    TAILQ_INSERT_TAIL(&mng.head, s, entry);

}
static inline int64_t timediff_ms(struct timespec st, struct timespec en) {
    int64_t ms = (en.tv_nsec - st.tv_nsec + 1000000000) / 1000;
    ms += ((en.tv_sec - st.tv_sec - 1) * 1000);
    return ms;
}
void fiq_interrupt(void* opaque) {
    fiq_a3ul_t* s = (fiq_a3ul_t*)opaque;
    CPUState *cpu = CPU(ARM_CPU(first_cpu));
    GICv3CPUState *cs = s->cs;
    uint16_t fiqno = s->fiqno;

    cs->hppi.prio = 0xc0;
    cs->hppi.irq = fiqno;
    cs->hppi.grp = 1;
    cs->icc_igrpen[cs->hppi.grp] = fiqno;
    cpu->env_ptr->daif &= ~PSTATE_F;

    cpu_interrupt(first_cpu, CPU_INTERRUPT_FIQ);
    free(s);
}

static uint64_t rza3ul_uart(void* opaque, uint64_t addr, uint64_t value, unsigned size, int type) {
    uint32_t offset = addr & 0xFF;
    char c = (char)value;
    short vs =  (short)value;
    uint64_t ret = 0;
    RzA3ulState *s = (RzA3ulState*)opaque;
    GICv3CPUState *cs = &s->gic.cpu[0];

    if (type) {
        if (offset == FTDR) {
           if (c != 0xd)
               g_print("%c", c);
        } else if (offset == SCR){
            if (vs & SCR_TIE) {
                delay_fiq_interrupt(cs, 415, 100);
            } else if ((vs & SCR_TEIE) && (scr & SCR_TIE) && (!(vs & SCR_TIE))) {
                delay_fiq_interrupt(cs, 416, 100);
            } else {
                delay_fiq_interrupt(cs, 415, 100);
            }
            scr = vs;
      } else if (offset == FSR) {
            if ((vs & FSR_TDFE)  == 0 && (scr & SCR_TIE) == SCR_TIE) {
                delay_fiq_interrupt(cs, 415, 0);
            }
            fsr = vs;
        }
    } else {
        if (offset == FDR) {
            ret = 0;
        } else if (offset == FSR) {
            ret = fsr + FSR_TEND; 
        }
    }
    return ret;
}
static void tzc_init(void) {
    tzcd[PID0_OFF] = 0x60;
    tzcd[PID1_OFF] = 0xb4;
    tzcs[PID0_OFF] = 0x60;
    tzcs[PID1_OFF] = 0xb4;
}
static void spi_init(void) {
    spirega[SPIM_CMNSR] = 1;
}

static void rza3ul_machine_init(MachineClass *mc)
{
    mc->desc = "RZ/A3UL (Cortex-A57)";
    mc->init = rza3ul_init;
    mc->block_default_type = IF_SD;
    mc->units_per_default_bus = 1;
    mc->min_cpus = RZ_A3UL_NUM_CPUS;
    mc->max_cpus = RZ_A3UL_NUM_CPUS;
    mc->default_cpus = RZ_A3UL_NUM_CPUS;
    mc->default_cpu_type = ARM_CPU_TYPE_NAME("cortex-a57");
}

DEFINE_MACHINE("rza3ul", rza3ul_machine_init)

```

</details>