/* Initialize Hypervisor Configuration */
    INIT_SPIN_LOCK(&arm_priv(vcpu)->hcr_lock);
    arm_priv(vcpu)->hcr =  (HCR_TSW_MASK |
        HCR_TACR_MASK |
        HCR_TIDCP_MASK |
        HCR_TSC_MASK |
        HCR_TWE_MASK |
        HCR_TWI_MASK |
        HCR_AMO_MASK |
        HCR_IMO_MASK |
        HCR_FMO_MASK |
        HCR_SWIO_MASK |
        HCR_VM_MASK);

    /*
     * Exception vectors.
     */
    .macro	ventry	label
        .align	7
        b	\label
    .endm

        .align	11
        .globl vectors;
    vectors:
    ventry	hyp_sync_invalid	/* Synchronous EL1t */
    ventry	hyp_irq_invalid		/* IRQ EL1t */
    ventry	hyp_fiq_invalid		/* FIQ EL1t */
    ventry	hyp_error_invalid	/* Error EL1t */

    ventry	hyp_sync		/* Synchronous EL1h */
    ventry	hyp_irq			/* IRQ EL1h */
    ventry	hyp_fiq_invalid		/* FIQ EL1h */
    ventry	hyp_error_invalid	/* Error EL1h */
    ........

    EXCEPTION_HANDLER hyp_sync
        PUSH_REGS
        mov	x1, EXC_HYP_SYNC_SPx
        CALL_EXCEPTION_CFUNC do_sync    
        PULL_REGS
    
    /* 
     *         .macro	PUSH_REGS
     *         sub	sp, sp, #0x20
     *         push	x28, x29
     *         push	x26, x27
     *         push	x24, x25
     *         push	x22, x23
     *        ......
     *         push	x0, x1
     *         add	x21, sp, #0x110
     *         mrs	x22, elr_el2
     *         mrs	x23, spsr_el2
     *         stp	x30, x21, [sp, #0xF0]
     *         stp	x22, x23, [sp, #0x100]
     *
     *
     *  .macro CALL_EXCEPTION_CFUNC cfunc
     *      mov	x0, sp                  x0 放下面 arch_regs_t *regs 的參數
     *      bl	\cfunc
     *  .endm
     */

    void do_sync(arch_regs_t *regs, unsigned long mode)
    {

    .......

        esr = mrs(esr_el2);
        far = mrs(far_el2);
        elr = mrs(elr_el2);

        ec = (esr & ESR_EC_MASK) >> ESR_EC_SHIFT;
        il = (esr & ESR_IL_MASK) >> ESR_IL_SHIFT;
        iss = (esr & ESR_ISS_MASK) >> ESR_ISS_SHIFT;

    .......


        switch (ec) {
        case EC_UNKNOWN:      /* 0x00 */
            /* We dont expect to get this trap so error */
            rc = VMM_EFAIL;
            break;
        case EC_TRAP_WFI_WFE:      /* 0x01 */
            /* WFI emulation */
            rc = cpu_vcpu_emulate_wfi_wfe(vcpu, regs, il, iss);
            break;
        case EC_TRAP_MCR_MRC_CP15_A32:      /* 0x03 */
            /* MCR/MRC CP15 emulation */
            rc = cpu_vcpu_emulate_mcr_mrc_cp15(vcpu, regs, il, iss);
            break;
    .........
            break;
        case EC_TRAP_HVC_A64:      /* 0x16 */
            /* HVC emulation for A64 guest */
            rc = cpu_vcpu_emulate_hvc64(vcpu, regs, il, iss);
            break;
        case EC_TRAP_MSR_MRS_SYSTEM:      /* 0x18 */
            /* MSR/MRS/SystemRegs emulation */
            rc = cpu_vcpu_emulate_msr_mrs_system(vcpu, regs, il, iss);
            break;
        case EC_TRAP_LWREL_INST_ABORT:      /* 0x20 */
            /* Stage2 instruction abort */
            fipa = (mrs(hpfar_el2) & HPFAR_FIPA_MASK) >> HPFAR_FIPA_SHIFT;
            fipa = fipa << HPFAR_FIPA_PAGE_SHIFT;
            fipa = fipa | (mrs(far_el2) & HPFAR_FIPA_PAGE_MASK);
            rc = cpu_vcpu_inst_abort(vcpu, regs, il, iss, fipa);
            break;
        case EC_TRAP_LWREL_DATA_ABORT:      /* 0x24 */
            /* Stage2 data abort */
            fipa = (mrs(hpfar_el2) & HPFAR_FIPA_MASK) >> HPFAR_FIPA_SHIFT;
            fipa = fipa << HPFAR_FIPA_PAGE_SHIFT;
            fipa = fipa | (mrs(far_el2) & HPFAR_FIPA_PAGE_MASK);
            rc = cpu_vcpu_data_abort(vcpu, regs, il, iss, fipa);
            break;

    /* GIC Distributor Interface Init */
        mrs	x4, mpidr_el1
        ldr	x5, __mpidr_mask
        and	x4, x4, x5			/* CPU affinity */
    __gic_dist_init:
        ldr	x0, __gic_dist_base		/* Dist GIC base */
        mov	x1, #0				/* non-0 cpus should at least */
        cmp	x4, xzr				/* program IGROUP0 */
        bne	1f
        mov	x1, #3				/* Enable group0 & group1 */
        str	w1, [x0, #0x00]			/* Ctrl Register */

    ldr	w1, [x0, #0x04]			/* Type Register */
    1:  and	x1, x1, #0x1f			/* No. of IGROUPn registers */
        add	x2, x0, #0x080			/* IGROUP0 Register */
        movn	x3, #0				/* All interrupts to group-1 */
    2:  str	w3, [x2], #4
        subs	x1, x1, #1
        bge	2b

    __gic_cpu_init:
        /* GIC Secured CPU Interface Init */
        ldr	x0, __gic_cpu_base		/* GIC CPU base */
        mov	x1, #0x80
        str	w1, [x0, #0x4]			/* GIC CPU Priority Mask */
        mov	x1, #0x3			/* Enable group0 & group1 */
        str	w1, [x0]			/* GIC CPU Control */

    struct vmm_host_irqs_ctrl {
    	vmm_spinlock_t lock;
    	struct vmm_host_irq *irq;
    	u32 (*active)(u32);
    	const struct vmm_devtree_nodeid *matches;
    };

    static struct vmm_host_irqs_ctrl hirqctrl;

    VMM_HOST_IRQ_INIT_DECLARE(ca15gic, "arm,cortex-a15-gic", gic_eoimode_init);

    __nidtbl struct vmm_devtree_nidtbl_entry __ca15gic = {
        .signature = VMM_DEVTREE_NIDTBL_SIGNATURE,
        .subsys = "host_irq",
        .nodeid.name =" ",
        .nodeid.type =" ",
        .nodeid.compatible ="arm,cortex-a15-gic",
        .nodeid.data = gic_eoimode_init,
    }

 if (vmm_smp_is_bootcpu()) {
        rc = gic_devtree_init(node, NULL, TRUE);
    }

    static int __init gic_devtree_init(struct vmm_devtree_node *node,
                                       struct vmm_devtree_node *parent,
                                       bool eoimode)
    {
        .........
       gic_init_bases(gic_cnt, eoimode, irq, cpu_base, cpu2_base, dist_base);
       .......

       if (parent) {
           if (vmm_devtree_read_u32(node, "parent_irq", &irq)) {
              irq = 1020;
           }
           gic_cascade_irq(gic_cnt, irq);
       } else {
           /*  會進到這邊 */
           vmm_host_irq_set_active_callback(gic_active_irq);
       }
    }

    static u32 gic_active_irq(u32 cpu_irq_nr)
    {
        u32 ret;

        ret = gic_read(gic_data[0].cpu_base + GIC_CPU_INTACK) & 0x3FF;
        ....
        return ret;
    }

    void vmm_handle_fast_eoi(struct vmm_host_irq *irq, u32 cpu, void *data)
    {
        irq_flags_t flags;
        struct vmm_host_irq_action *act;

        list_for_each_entry(act, &irq->action_list[cpu], head) {
            if (act->func(irq->num, act->dev) == VMM_IRQ_HANDLED) {
                break;
            }
        }
        if (irq->chip && irq->chip->irq_eoi) {
            irq->chip->irq_eoi(irq);
        }
    }

    static struct vmm_emulator vgic_dist_emulator = {
        .name = "vgic-dist",
        .match_table = vgic_dist_emuid_table,
        .endian = VMM_DEVEMU_LITTLE_ENDIAN,
        .probe = vgic_dist_emulator_probe,
        .remove = vgic_dist_emulator_remove,
        .reset = vgic_dist_emulator_reset,
        ......
    };
    
    static struct vmm_emulator vgic_cpu_emulator = {
        .name = "vgic-cpu",
        .match_table = vgic_cpu_emuid_table,
        .endian = VMM_DEVEMU_LITTLE_ENDIAN,
        .probe = vgic_cpu_emulator_probe,
        .remove = vgic_cpu_emulator_remove,
        .reset = vgic_cpu_emulator_reset,
    };

    static int __init vgic_emulator_init(void)
    {
        int i, rc;
        struct vmm_devtree_node *node;

        vgich.avail = FALSE;

        .......
        /* 取得devtree 內gic reg的位置及GIC Maintanence IRQ */
        .......

        /* 註冊vgic distributor 及 vgic cpu interface*/
        rc = vmm_devemu_register_emulator(&vgic_dist_emulator);
        rc = vmm_devemu_register_emulator(&vgic_cpu_emulator);

        ......
    }

    VMM_DECLARE_MODULE(MODULE_DESC,
                   MODULE_AUTHOR,
                   MODULE_LICENSE,
                   MODULE_IPRIORITY,
                   MODULE_INIT,
                   MODULE_EXIT);

    static int vgic_dist_emulator_probe(struct vmm_guest *guest,
                                        struct vmm_emudev *edev,
                                        const struct vmm_devtree_nodeid *eid)
    {
        int rc;
        u32 i, virq, hirq, len, parent_irq, num_irq;
        struct vgic_guest_state *s;

        .......

        rc = vmm_devtree_read_u32(edev->node, "parent_irq", &parent_irq);
        if (rc) {
            return rc;
        }

        if (vmm_devtree_read_u32(edev->node, "num_irq", &num_irq)) {
            num_irq = VGIC_MAX_NIRQ;
        }
        if (num_irq > VGIC_MAX_NIRQ) {
            num_irq = VGIC_MAX_NIRQ;
        }

        s = vgic_state_alloc(edev->node->name,
                             guest, guest->vcpu_count,
                             num_irq, parent_irq);

        len = vmm_devtree_attrlen(edev->node, "host2guest") / 8;

        for (i = 0; i < len; i++) {
                ......
               /** Register function callback for hirq(host irq) */
                rc = vmm_host_irq_register(hirq, "VGIC",
                                   vgic_routed_irq, s);
                ......
        }

    static struct vgic_guest_state *vgic_state_alloc(const char *name,
                                        struct vmm_guest *guest,
                                        u32 num_cpu,
                                        u32 num_irq,
                                        u32 parent_irq)
    {
        u32 i;
        struct vmm_vcpu *vcpu;
        struct vgic_guest_state *s = NULL;

        /* Alloc VGIC state */
        s = vmm_zalloc(sizeof(struct vgic_guest_state));

        s->guest = guest;

        s->num_cpu = num_cpu;
        s->num_irq = num_irq;
        s->id[0] = 0x90 /* id0 */;
        s->id[1] = 0x13 /* id1 */;
        s->id[2] = 0x04 /* id2 */;
        s->id[3] = 0x00 /* id3 */;
        s->id[4] = 0x0d /* id4 */;
        s->id[5] = 0xf0 /* id5 */;
        s->id[6] = 0x05 /* id6 */;
        s->id[7] = 0xb1 /* id7 */;

        for (i = 0; i < VGIC_NUM_IRQ(s); i++) {
            VGIC_SET_HOST_IRQ(s, i, UINT_MAX);
        }

        for (i = 0; i < VGIC_NUM_CPU(s); i++) {
            s->vstate[i].vcpu = vmm_manager_guest_vcpu(guest, i);
            s->vstate[i].parent_irq = parent_irq;
        }

        INIT_SPIN_LOCK(&s->dist_lock);

        /* Register guest irq handler */
        for (i = 0; i < VGIC_NUM_IRQ(s); i++) {
            vmm_devemu_register_irq_handler(guest, i,
                                            name, vgic_irq_handle, s);
        }

        /* setup 之後context switch時使用的save, restore function*/
        /* Setup save/restore hooks */
        list_for_each_entry(vcpu, &guest->vcpu_list, head) {
            arm_vgic_setup(vcpu,
                           vgic_save_vcpu_context,
                           vgic_restore_vcpu_context, s);
        }
        
        return s;
    }

    /* Handle host-to-guest routed IRQ generated by device */
    static vmm_irq_return_t vgic_routed_irq(int irq_no, void *dev)
    {
        ........
        /* Determine guest IRQ from host IRQ */
        virq = (u32)arch_atomic_read(&vgich.host2guest_irq[hirq]);
        if (VGIC_MAX_NIRQ <= virq) {
            goto done;
        }

        /* Lower the interrupt level.
         * This will clear previous interrupt state.
         */
        rc = vmm_devemu_emulate_irq(s->guest, virq, 0);

        /* Elevate the interrupt level.
         * This will force interrupt triggering.
         */
        rc = vmm_devemu_emulate_irq(s->guest, virq, 1);

    done:
        return VMM_IRQ_HANDLED;
    }

    /* Process IRQ asserted by device emulation framework */
    static void vgic_irq_handle(u32 irq, int cpu, int level, void *opaque)
    {
        .........

        if (irq < 32) {
            /* In case of PPIs and SGIs */
            cm = target = (1 << cpu);
        } else {
            /* In case of SGIs */
            cm = VGIC_ALL_CPU_MASK(s);
            target = VGIC_TARGET(s, irq);
            for (cpu = 0; cpu < VGIC_NUM_CPU(s); cpu++) {
                if (target & (1 << cpu)) {
                    break;
                }
            }
            if (VGIC_NUM_CPU(s) <= cpu) {
                vmm_spin_unlock_irqrestore_lite(&s->dist_lock, flags);
                return;
            }
        }

        /* Find out VCPU pointer */
        vs = &s->vstate[cpu];

        /* If level not changed then skip */
        if (level == VGIC_TEST_LEVEL(s, irq, cm)) {
            goto done;
       }

        /* Update IRQ state */
        if (level) {
            VGIC_SET_LEVEL(s, irq, cm);
            if (VGIC_TEST_ENABLED(s, irq, cm)) {
                VGIC_SET_PENDING(s, irq, target);
                irq_pending = TRUE;
            }
        } else {
           VGIC_CLEAR_LEVEL(s, irq, cm);
        }

        /* Directly updating VGIC HW for current VCPU */
        if (vs->vcpu == vmm_scheduler_current_vcpu()) {
            /* The VGIC HW state may have changed when the
             * VCPU was running hence, sync VGIC VCPU state.
             */
            __vgic_sync_vcpu_hwstate(s, vs);

            /* Flush IRQ state change to VGIC HW */
            if (irq_pending) {
                __vgic_flush_vcpu_hwstate_irq(s, vs, irq);
            }
        }

    done:
        ........
        /* Forcefully resume VCPU if waiting for IRQ */
        if (irq_pending) {
            vmm_vcpu_irq_wait_resume(vs->vcpu);
        }
    }

        /*
         * x4 -> load start
         * x5 -> load end
         * x6 -> execution start
         * x7 -> execution end
         */

    _start_mmu_init:

        /* Setup SP as-per load address */
        ldr	x0, __hvc_stack_end
        mov	sp, x0
        sub	sp, sp, x6
        add	sp, sp, x4

        .........

        bl	_setup_initial_ttbl

    u8 __attribute__ ((aligned(TTBL_TABLE_SIZE))) def_ttbl[TTBL_INITIAL_TABLE_SIZE] = { 0 };

    void __attribute__ ((section(".entry")))
        _setup_initial_ttbl(virtual_addr_t load_start, virtual_addr_t load_end,
                    virtual_addr_t exec_start, virtual_addr_t exec_end)
    {
        ..........

        lpae_entry.ttbl_base = to_load_pa((virtual_addr_t)&def_ttbl);   /* def_ttbl之後要放到 ttbr0_el2(Translation Base Register) 裡*/
        lpae_entry.next_ttbl = (u64 *)lpae_entry.ttbl_base;

        ..........

        /* Map physical = logical
         * Note: This mapping is using at boot time only
         */
        __setup_initial_ttbl(&lpae_entry, load_start, load_end, load_start,
                AINDEX_NORMAL_WB, TRUE);

        /* Map to logical addresses which are
         * covered by read-only linker sections
         * Note: This mapping is used at runtime
         */
        SETUP_RO_SECTION(lpae_entry, text);
        SETUP_RO_SECTION(lpae_entry, init);
        SETUP_RO_SECTION(lpae_entry, cpuinit);
        SETUP_RO_SECTION(lpae_entry, spinlock);
        SETUP_RO_SECTION(lpae_entry, rodata);

        /* Map rest of logical addresses which are
         * not covered by read-only linker sections
         * Note: This mapping is used at runtime
         */
        __setup_initial_ttbl(&lpae_entry, exec_start, exec_end, load_start,
                                AINDEX_NORMAL_WB, TRUE);
    }

    void __attribute__ ((section(".entry")))
        __setup_initial_ttbl(struct mmu_lpae_entry_ctrl *lpae_entry,
                    virtual_addr_t map_start, virtual_addr_t map_end,
                    virtual_addr_t pa_start, u32 aindex, bool writeable)
    {
        ........
        u64 *ttbl;

        /* align start addresses */
        map_start &= TTBL_L3_MAP_MASK;  /* 0xFFFFFFFFFFFFF000ULL 後面12的bit 直接map 到 output*/
        pa_start &= TTBL_L3_MAP_MASK;

        page_addr = map_start;

        while (page_addr < map_end) {

                /* Setup level1 table */
                ttbl = (u64 *) lpae_entry->ttbl_base;
                index = (page_addr & TTBL_L1_INDEX_MASK) >> TTBL_L1_INDEX_SHIFT;
                if (ttbl[index] & TTBL_VALID_MASK) {
                    /* Find level2 table */
                    ttbl =
                    (u64 *) (unsigned long)(ttbl[index] &
                                        TTBL_OUTADDR_MASK);
                } else {
                    /* Allocate new level2 table */
                    if (lpae_entry->ttbl_count == TTBL_INITIAL_TABLE_COUNT) {
                        while (1) ;	/* No initial table available */
                    }
                    for (i = 0; i < TTBL_TABLE_ENTCNT; i++) {
                        lpae_entry->next_ttbl[i] = 0x0ULL;
                    }
                    lpae_entry->ttbl_tree[lpae_entry->ttbl_count] =
                        ((virtual_addr_t) ttbl -
                        lpae_entry->ttbl_base) >> TTBL_TABLE_SIZE_SHIFT;
                    lpae_entry->ttbl_count++;
                    ttbl[index] |=
                        (((virtual_addr_t) lpae_entry->next_ttbl) &
                        TTBL_OUTADDR_MASK);
                    ttbl[index] |= (TTBL_TABLE_MASK | TTBL_VALID_MASK);
                    ttbl = lpae_entry->next_ttbl;
                    lpae_entry->next_ttbl += TTBL_TABLE_ENTCNT;
                }

                /* Setup level2 table */
                index = (page_addr & TTBL_L2_INDEX_MASK) >> TTBL_L2_INDEX_SHIFT;
                if (ttbl[index] & TTBL_VALID_MASK) {
                    /* Find level3 table */
                    ttbl =
                        (u64 *) (unsigned long)(ttbl[index] &
                            TTBL_OUTADDR_MASK);
                } else {
                    /* Allocate new level3 table */
                    ......
                }

                /* Setup level3 table */
                index = (page_addr & TTBL_L3_INDEX_MASK) >> TTBL_L3_INDEX_SHIFT;
                if (!(ttbl[index] & TTBL_VALID_MASK)) {
                    /* Update level3 table */
                    .......
                }

                /* Point to next page */
                page_addr += TTBL_L3_BLOCK_SIZE;
        }
    }

    /* Update the httbr value */
    ldr	x1, __httbr_set_addr
    sub	x1, x1, x6
    add	x1, x1, x4
    ldr	x0, [x1]
    sub	x0, x0, x6
    add	x0, x0, x4
    str	x0, [x1]

	/* Setup Hypervisor Translation Control Register */
	ldr	x0, __htcr_set  
	msr     tcr_el2, x0

- __htcr_set:	(TCR_T0SZ_VAL(39) | TCR_PS_40BITS | (0x3 << TCR_SH0_SHIFT) | (0x1 << TCR_ORGN0_SHIFT) | (0x1 << TCR_IRGN0_SHIFT))
    - TCR_T0SZ_VAL(39) = (64-39) & 0x3f
        - The size offset of the memory region addressed by TTBR0_EL2. The region size is 2^64-T0SZ bytes.(將 region size 設為 2^39 bytes)
        - Initial lookup level為level 1
    - TCR_PS_40BITS = 2 << 16: Physical Address Size 設為40 bits, 1 TB。
    - TCR_SH0_SHIFT = 12; 0x3: Shareability attribute for memory associated: Inner Shareable
    - TCR_ORGN0_SHIFT = 10; 0x1: Outer cacheability attribute for memory associated: Write-Back Write-Allocate Cacheable
    - TCR_IRGN0_SHIFT = 8; 0x1: Inner cacheability attribute for memory associated:  Write-Back Write-Allocate Cacheable
    - [Write-Back Write-Allocate](http://upload.wikimedia.org/wikipedia/commons/c/c2/Write-back_with_write-allocation.svg)

	/* Setup Hypervisor Translation Base Register */
	ldr	x0, __httbr_set  /* def_ttbl 的位置 */
	msr	ttbr0_el2, x0

    void arch_vcpu_switch(struct vmm_vcpu *tvcpu,
                          struct vmm_vcpu *vcpu,
                          arch_regs_t *regs)
    {
        u32 ite;
        irq_flags_t flags;

        /* Save user registers & banked registers */
        if (tvcpu) {
            arm_regs(tvcpu)->pc = regs->pc;
            arm_regs(tvcpu)->lr = regs->lr;
            arm_regs(tvcpu)->sp = regs->sp;
            for (ite = 0; ite < CPU_GPR_COUNT; ite++) {
                arm_regs(tvcpu)->gpr[ite] = regs->gpr[ite];
            }
            arm_regs(tvcpu)->pstate = regs->pstate;
            if (tvcpu->is_normal) {
                /* Update last host CPU */
                arm_priv(tvcpu)->last_hcpu = vmm_smp_processor_id();
                /* Save VGIC context */
                arm_vgic_save(tvcpu);
                /* Save sysregs context */
                cpu_vcpu_sysregs_save(tvcpu);
                /* Save VFP and SIMD context */
                cpu_vcpu_vfp_save(tvcpu);
                /* Save generic timer */
                if (arm_feature(tvcpu, ARM_FEATURE_GENERIC_TIMER)) {
                    generic_timer_vcpu_context_save(tvcpu,
                                                    arm_gentimer_context(tvcpu));
                }
            }
        }
        /* Restore user registers & special registers */
        regs->pc = arm_regs(vcpu)->pc;
        regs->lr = arm_regs(vcpu)->lr;
        regs->sp = arm_regs(vcpu)->sp;
        for (ite = 0; ite < CPU_GPR_COUNT; ite++) {
            regs->gpr[ite] = arm_regs(vcpu)->gpr[ite];
        }
        regs->pstate = arm_regs(vcpu)->pstate;
        if (vcpu->is_normal) {
            /* Restore hypervisor context */
            vmm_spin_lock_irqsave(&arm_priv(vcpu)->hcr_lock, flags);
            msr(hcr_el2, arm_priv(vcpu)->hcr);
            vmm_spin_unlock_irqrestore(&arm_priv(vcpu)->hcr_lock, flags);
            msr(cptr_el2, arm_priv(vcpu)->cptr);
            msr(hstr_el2, arm_priv(vcpu)->hstr);
            /* Restore Stage2 MMU context */
            mmu_lpae_stage2_chttbl(vcpu->guest->id,
                                   arm_guest_priv(vcpu->guest)->ttbl);
            /* Restore generic timer */
            if (arm_feature(vcpu, ARM_FEATURE_GENERIC_TIMER)) {
                generic_timer_vcpu_context_restore(vcpu,
                                                   arm_gentimer_context(vcpu));
            }
            /* Restore VFP and SIMD context */
            cpu_vcpu_vfp_restore(vcpu);
            /* Restore sysregs context */
            cpu_vcpu_sysregs_restore(vcpu);
            /* Restore VGIC context */
            arm_vgic_restore(vcpu);
            /* Flush TLB if moved to new host CPU */
            if (arm_priv(vcpu)->last_hcpu != vmm_smp_processor_id()) {
                /* Invalidate all guest TLB enteries because
                 * we might have stale guest TLB enteries from
                 * our previous run on new_hcpu host CPU
                 */
                inv_tlb_guest_allis();
                /* Ensure changes are visible */
                dsb();
                isb();
            }
        }
        /* Clear exclusive monitor */
        clrex();
    }

    #define inv_tlb_guest_allis()	asm volatile("tlbi alle1is\n\t" \
    					     "dsb sy\n\t" \
    					     "isb\n\t" \
    					     ::: "memory", "cc") 

    int __cpuinit vmm_scheduler_init(void){
    ....
    /* Initialize timer events (Per Host CPU) */
            INIT_TIMER_EVENT(&schedp->ev, &scheduler_timer_event, schedp);
            INIT_TIMER_EVENT(&schedp->sample_ev,
                                    &scheduler_sample_event, schedp);
    ....
    /* Start timer events */
            vmm_timer_event_start(&schedp->ev, 0);
            vmm_timer_event_start(&schedp->sample_ev, SAMPLE_EVENT_PERIOD);
    ....

    static void scheduler_timer_event(struct vmm_timer_event *ev)
    {
        struct vmm_scheduler_ctrl *schedp = &this_cpu(sched);

        if (schedp->irq_regs) {
            vmm_scheduler_switch(schedp, schedp->irq_regs);
        }
    }

    static void vmm_scheduler_switch(struct vmm_scheduler_ctrl *schedp,
                                     arch_regs_t *regs)   // arch_reg 只有在user mode 被更新                                 
    {
            struct vmm_vcpu *vcpu = schedp->current_vcpu;
            if (!regs) {
                    /* This should never happen !!! */
                    vmm_panic("%s: null pointer to regs.\n", __func__);
            }

            if (vcpu) {
                    if (!vcpu->preempt_count) {   
                            vmm_scheduler_next(schedp, &schedp->ev, regs);
                    } else {
                            vmm_timer_event_restart(&schedp->ev);
                    }
            } else {
                    vmm_scheduler_next(schedp, &schedp->ev, regs);
            }
    }

    static void vmm_scheduler_next(struct vmm_scheduler_ctrl *schedp,
                                   struct vmm_timer_event *ev,
                                   arch_regs_t *regs)
    {
          /* 紀錄current state & timestamp*/
           .....

            u64 next_time_slice = VMM_VCPU_DEF_TIME_SLICE;
            u64 tstamp = vmm_timer_timestamp();
          ......  
            /* First time scheduling */
            if (!current) {
                    rc = rq_dequeue(schedp, &next, &next_time_slice);
                    vmm_write_lock_irqsave_lite(&next->sched_lock, nf);
    }
    arch_vcpu_switch(NULL, next, regs); 
                    next->state_ready_nsecs += tstamp - next->state_tstamp;
                    arch_atomic_write(&next->state, VMM_VCPU_STATE_RUNNING);
                    next->state_tstamp = tstamp;
                    schedp->current_vcpu = next;
                    schedp->current_vcpu_irq_ns = schedp->irq_process_ns;

               /*下一個timer_event開始 */                

                    vmm_timer_event_start(ev, next_time_slice);
                    vmm_write_unlock_irqrestore_lite(&next->sched_lock, nf);
                    return;
            }

    # MSR (immediate)
    #	Syntax:
    # 		msr<c> <spec_reg>, #<const>
    #	Fields:
    #		cond = bits[31:28]
    #		R = bits[22:22]
    #		mask = bits[19:16]
    #		imm12 = bits[11:0]
    #	Hypercall Fields:
    #		inst_cond[31:28] = cond
    #		inst_op[27:24] = 0xf
    #		inst_id[23:20] = 0
    #		inst_subid[19:17] = 2
    #		inst_fields[16:13] = mask
    #		inst_fields[12:1] = imm12
    #		inst_fields[0:0] = R
    
    def convert_msr_i_inst(hxstr):
    	hx = int(hxstr, 16)
    	inst_id = 0
    	inst_subid = 2
    	cond = (hx >> 28) & 0xF
    	R = (hx >> 22) & 0x1
    	mask = (hx >> 16) & 0xF
    	imm12 = (hx >> 0) & 0xFFF
    	rethx = 0x0F000000
    	rethx = rethx | (cond << 28)
    	rethx = rethx | (inst_id << 20)
    	rethx = rethx | (inst_subid << 17)
    	rethx = rethx | (mask << 13)
    	rethx = rethx | (imm12 << 1)
    	rethx = rethx | (R << 0)
    	return rethx

    _start_vect:
    	ldr	pc, __reset
    	ldr	pc, __undefined_instruction
    	ldr	pc, __software_interrupt  /* __software_interrupt:	.word _soft_irq  */
        /* EXCEPTION_HANDLER _soft_irq, 4
         *PUSH_REGS 
         *CALL_EXCEPTION_CFUNC do_soft_irq
         *PULL_REGS
         */ 
    	......

    void do_soft_irq(arch_regs_t *regs)
    {
        ........
        /* If vcpu priviledge is user then generate exception
         * and return without emulating instruction
         */
        if ((arm_priv(vcpu)->cpsr & CPSR_MODE_MASK) == CPSR_MODE_USER) {
            vmm_vcpu_irq_assert(vcpu, CPU_SOFT_IRQ, 0x0);
        } else {
            if (regs->cpsr & CPSR_THUMB_ENABLED) {
                rc = cpu_vcpu_hypercall_thumb(vcpu, regs,
                        *((u32 *)regs->pc));
            } else {
                rc = cpu_vcpu_hypercall_arm(vcpu, regs,
                        *((u32 *)regs->pc));
                }
            }
        ........
    }

    int cpu_vcpu_hypercall_arm(struct vmm_vcpu *vcpu,
                               arch_regs_t *regs, u32 inst)
    {
        u32 id = ARM_INST_DECODE(inst,
                                 ARM_INST_HYPERCALL_ID_MASK,
                                 ARM_INST_HYPERCALL_ID_SHIFT);

        return hcall_funcs[id](id, inst, regs, vcpu);
    }

    static int (* const hcall_funcs[])(u32 id, u32 inst,
                                       arch_regs_t *regs, struct vmm_vcpu *vcpu) = {
        arm_hypercall_cps_and_co,	/* ARM_HYPERCALL_CPS_ID */
        arm_hypercall_ldm_ue,		/* ARM_HYPERCALL_LDM_UE_ID0 */
        arm_hypercall_ldm_ue,		/* ARM_HYPERCALL_LDM_UE_ID1 */
        arm_hypercall_ldm_ue,		/* ARM_HYPERCALL_LDM_UE_ID2 */
        .......
    };

    static int (* const cps_and_co_funcs[])(u32 inst,
                                            arch_regs_t *regs, struct vmm_vcpu *vcpu) = {
        arm_hypercall_cps,	/* ARM_HYPERCALL_CPS_SUBID */
        arm_hypercall_mrs,	/* ARM_HYPERCALL_MRS_SUBID */
        arm_hypercall_msr_i,	/* ARM_HYPERCALL_MSR_I_SUBID */
        arm_hypercall_msr_r,	/* ARM_HYPERCALL_MSR_R_SUBID */
        arm_hypercall_rfe,	/* ARM_HYPERCALL_RFE_SUBID */
        arm_hypercall_srs,	/* ARM_HYPERCALL_SRS_SUBID */
        arm_hypercall_wfx,	/* ARM_HYPERCALL_WFI_SUBID */
        arm_hypercall_smc	/* ARM_HYPERCALL_SMC_SUBID */
    };

    /* Emulate 'mrs' hypercall */
    static int arm_hypercall_mrs(u32 inst,
                                 arch_regs_t *regs, struct vmm_vcpu *vcpu)
    {
        register u32 Rd;
        Rd = ARM_INST_BITS(inst,
                           ARM_HYPERCALL_MRS_RD_END,
                           ARM_HYPERCALL_MRS_RD_START);
        if (Rd == 15) {
            arm_unpredictable(regs, vcpu, inst, __func__);
            return VMM_EFAIL;
        }
        if (ARM_INST_BIT(inst, ARM_HYPERCALL_MRS_R_START)) {
            cpu_vcpu_reg_write(vcpu, regs, Rd,
                               cpu_vcpu_spsr_retrieve(vcpu));
        } else {
            cpu_vcpu_reg_write(vcpu, regs, Rd,
                               cpu_vcpu_cpsr_retrieve(vcpu, regs));
        }
        regs->pc += 4;
        return VMM_OK;
    }