前回で送信用バッファに文字列を設定したところまで進めることが出来ました。

次はコンソール用レジスタへ値を書きこんで
実際に画面に文字列を表示する流れになるのですが
その前にサンプルプログラムの動作を確認します。

文字列表示の仕組み
- R_SCIF_UART_Write
- vectorテーブル
QEMU(arm)の割り込み処理

文字列表示の仕組み

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

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

R_SCIF_UART_Write

#define が切られていて見づらいですが
取り除くと以下の様になります。

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;
}

つまり、ここでやっていることは以下です。

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に関数ポインタが設定されています。

コール箇所は以下です。

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レジスタを見ていることがわかります(*1)。
つまり、ICC_IAR1_EL1です。

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

ARMでは割り込みが発生するとvector tableを利用して割り込み処理を行います。
ここのcurr_el_spx_fiqがFiqSpxに対応していることがわかります。

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

QEMU(arm)の割り込み処理

qemuのソースコードに適当にgrepをかけると以下がヒットします。

target/arm/cpu.c:684

 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のアドレスに設定していることが見て取れます。

10962     env->pc = addr;

割り込み発生の条件

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

cpu_handle_interrupt内はどうでしょう？
cpu->interrupt_requestが0で無いことが条件の一つのようです

 787     if (unlikely(qatomic_read(&cpu->interrupt_request))) {

arm_cpu_exec_interruptはもっと単純です。

 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で無くなれば割り込みが発生するようです。

 681     return unmasked || pstate_unmasked;

遡ってみてみると結局以下だけが有効なことがわかります。
(割り込みでException Levelは変化しないので)

 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を設定していることがわかります。

./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すると以下がヒットします。

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が読み出す値のようです。
この辺を見る限りどうも関係無さそうです。
また、汎用の書き込み関数も見当たりません。
とりあえずの疎通のため、無理やり設定してみます。

コードイメージ

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で弾かれているようです。
なのでそこも通してみます。

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のコードを見ると
scif_uart_txi_isrを文字列数 / 16文字分繰り返した後
scif_uart_tei_isrのコールが必要なことがわかります。

scif_uart_txi_isrは以下です。

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の条件判定は以下です。

    while ((UART_EVENT_TX_COMPLETE != g_uart_event) && (--local_timeout))

g_uart_eventの変更はuser_uart_callbackのみで行われます。

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として設定されています。

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の該当レジスタ変化で割り込みを発生させると
以下の様になってしまいます。

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

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

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

qemuのタイマー

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

コード例は以下です。

    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を修正します。

rza3ul.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)