Initial commit of the virtualizer v2.0 release.

This will be the basis for the VSM.

1 files changed, 583 insertions, 0 deletions

diff --git a/big-little/virtualisor/virt_handle.c b/big-little/virtualisor/virt_handle.c
new file mode 100644
index 0000000..6b4c1b5
--- /dev/null
+++ b/big-little/virtualisor/virt_handle.c
@@ -0,0 +1,583 @@
+/*
+ * $Copyright:
+ * ----------------------------------------------------------------
+ * This confidential and proprietary software may be used only as
+ * authorised by a licensing agreement from ARM Limited
+ *  (C) COPYRIGHT 2008-2011 ARM Limited
+ *       ALL RIGHTS RESERVED
+ * The entire notice above must be reproduced on all authorised
+ * copies and copies may only be made to the extent permitted
+ * by a licensing agreement from ARM Limited.
+ * ----------------------------------------------------------------
+ * File:     virt_handle.c
+ * ----------------------------------------------------------------
+ * $
+ */
+
+#include "virtualisor.h"
+#include "virt_helpers.h"
+#include "hyp_types.h"
+#include "cache_geom.h"
+#include "mem_trap.h"
+#include "gic_registers.h"
+#include "bl.h"
+
+extern cache_geometry host_cache_geometry[];
+extern cache_geometry target_cache_geometry[];
+extern cache_diff cache_delta[NUM_CPUS][MAX_CACHE_LEVELS];
+
+void trap_cp15_mrc_mcr_handle(unsigned hsr, gp_regs * regs)
+{
+        unsigned Op1, Op2, CRn, CRm, Rt, write, cpu_id = read_cpuid();
+
+        Op2 = (hsr >> 17) & 0x7;
+        Op1 = (hsr >> 14) & 0x7;
+        CRn = (hsr >> 10) & 0xf;
+        Rt = (hsr >> 5) & 0xf;
+        CRm = (hsr >> 1) & 0xf;
+        write = !(hsr & 0x1);
+
+        switch (CRn) {
+        case CRN_C0:
+                switch (Op1) {
+                case 0:
+                        switch (CRm) {
+                        case 0:
+                                switch (Op2) {
+                                case MIDR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_vmidr();
+                                        break;
+                                case CTR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_ctr();
+                                        break;
+                                case TCMTR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_tcmtr();
+                                        break;
+                                case TLBTR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_tlbtr();
+                                        break;
+                                case MPIDR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_vmpidr();
+                                        break;
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        case 1:
+                                switch (Op2) {
+                                case ID_PFR0:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_pfr0();
+                                        break;
+                                case ID_PFR1:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_pfr1();
+                                        break;
+                                case ID_DFR0:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_dfr0();
+                                        break;
+                                case ID_AFR0:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_afr0();
+                                        break;
+                                case ID_MMFR0:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_mmfr0();
+                                        break;
+                                case ID_MMFR1:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_mmfr1();
+                                        break;
+                                case ID_MMFR2:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_mmfr2();
+                                        break;
+                                case ID_MMFR3:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_mmfr3();
+                                        break;
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        case 2:
+                                switch (Op2) {
+                                case ID_ISAR0:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_isar0();
+                                        break;
+                                case ID_ISAR1:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_isar1();
+                                        break;
+                                case ID_ISAR2:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_isar2();
+                                        break;
+                                case ID_ISAR3:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_isar3();
+                                        break;
+                                case ID_ISAR4:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_isar4();
+                                        break;
+                                case ID_ISAR5:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_id_isar5();
+                                        break;
+                                default:
+                                        /* RAZ */
+                                        regs->r[Rt] = 0x0;
+                                }
+                                break;
+                        case 3:
+                        case 4:
+                        case 5:
+                        case 6:
+                        case 7:
+                                if (write)
+                                        goto error;
+                                /* RAZ */
+                                regs->r[Rt] = 0x0;
+                                break;
+                        default:
+                                goto error;
+                        }
+                        break;
+                case 1:
+                        switch (CRm) {
+                        case 0:
+                                switch (Op2) {
+                                case CCSIDR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] =
+                                                target_cache_geometry[cpu_id].
+                                                ccsidr[get_cache_level
+                                                       (target_cache_geometry[cpu_id].
+                                                        ccselr)];
+                                        break;
+                                case CLIDR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] =
+                                                target_cache_geometry[cpu_id].clidr;
+                                        break;
+                                case AIDR:
+                                        if (write)
+                                                goto error;
+                                        regs->r[Rt] = read_aidr();
+                                        break;
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        default:
+                                goto error;
+                        }
+                        break;
+                case 2:
+                        switch (CRm) {
+                        case 0:
+                                switch (Op2) {
+                                case CSSELR:
+                                        if (write)
+                                                target_cache_geometry[cpu_id].
+                                                        ccselr = regs->r[Rt];
+                                        else
+                                                regs->r[Rt] =
+                                                        target_cache_geometry[cpu_id].
+                                                        ccselr;
+                                        break;
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        default:
+                                goto error;
+                        }
+                        break;
+                default:
+                        goto error;
+                }
+                break;
+        case CRN_C7:
+                switch (Op1) {
+                case 0:
+                        switch (CRm) {
+                        case 6:
+                                switch (Op2) {
+                                case DCISW:
+                                {
+                                        if (!write)
+                                                goto error;
+                                        handle_cm_op(regs->r[Rt],
+                                                     dcisw, 
+                                                     &host_cache_geometry[cpu_id], 
+                                                     &target_cache_geometry[cpu_id],
+                                                     &cache_delta[cpu_id][0]);
+                                        break;
+                                }
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        case 10:
+                                switch (Op2) {
+                                case DCCSW:
+                                {
+                                        if (!write)
+                                                goto error;
+                                        handle_cm_op(regs->r[Rt],
+                                                     dccsw,
+                                                     &host_cache_geometry[cpu_id], 
+                                                     &target_cache_geometry[cpu_id],
+                                                     &cache_delta[cpu_id][0]);
+                                        break;
+                                }
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        case 14:
+                                switch (Op2) {
+                                case DCCISW:
+                                {
+                                        if (!write)
+                                                goto error;
+                                        handle_cm_op(regs->r[Rt],
+                                                     dccsw,
+                                                     &host_cache_geometry[cpu_id], 
+                                                     &target_cache_geometry[cpu_id],
+                                                     &cache_delta[cpu_id][0]);
+                                        break;
+                                }
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        default:
+                                goto error;
+                        }
+                        break;
+                default:
+                        goto error;
+                }
+                break;
+        case CRN_C9:
+                switch (Op1) {
+                case 1:
+                        switch (CRm) {
+                        case 0:
+                                switch (Op2) {
+                                case 2:
+                                        /*
+                                         * A write to the L2CTLR register means trouble
+                                         * as the A7 version does not have all the fields
+                                         * that the A15 has. Handling needs more thought
+                                         */
+                                        if (write) {
+                                                printf("%s: Unexpected L2CTLR write \n",
+                                                       __FUNCTION__);
+                                                goto error;
+                                        }
+                                        
+                                        /* 
+                                         * A read of the L2CTLR should return the total number
+                                         * of cpus across both the clusters in the "always on"
+                                         * configuration. Since there are only 2 bits for the
+                                         * number of cpus in the L2CTLR we need to flag any
+                                         * system with > 4 cpus.
+                                         */
+                                        if (!switcher) {
+                                                unsigned num_cpus = CLUSTER_CPU_COUNT(host_cluster)
+                                                        + CLUSTER_CPU_COUNT(!host_cluster);
+                                                
+                                                if (num_cpus > 4) {
+                                                        printf("%s: Unexpected L2CTLR read \n",
+                                                               __FUNCTION__);
+                                                        goto error;
+                                                }
+
+                                                regs->r[Rt] &= ~(0x3 << 24);
+                                                regs->r[Rt] |= (num_cpus - 1) << 24;
+                                        } else {
+                                                regs->r[Rt] = read_l2ctlr();
+                                        }
+                                        break;
+                                case 3:
+                                        /*
+                                         * A write to the L2ECTLR register means trouble
+                                         * as it does not exist on A7. Handling needs more
+                                         * thought
+                                         */
+                                        if (write) {
+                                                printf("%s: Unexpected L2ECTLR write \n",
+                                                       __FUNCTION__);
+                                                goto error;
+                                        } else {
+                                                regs->r[Rt] = read_l2ectlr();
+                                        }
+                                        break;
+                                default:
+                                        goto error;
+                                }
+                                break;
+                        default:
+                                goto error;
+                        }
+                        break;
+
+                        /*
+                         * Support for accesses to the PMON space. Its not been 
+                         * verified whether all the registers are readable &
+                         * writable. But then, execution will never reach here
+                         * if a reg is inaccessible. It will be a undef abort
+                         * instead.
+                         */
+                case 0:
+                        switch (CRm) {
+                        case 14:
+                                switch (Op2) {
+                                case 0:
+                                        if(write)
+                                                write_pmuserenr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmuserenr();
+                                        break;
+                                case 1:
+                                        if(write)
+                                                write_pmintenset(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmintenset();
+                                        break;
+                                case 2:
+                                        if(write)
+                                                write_pmintenclr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmintenclr();
+                                        break;
+                                case 3:
+                                        if(write)
+                                                write_pmovsset(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmovsset();
+                                        break;
+                                default:
+                                        goto error;
+                                }
+                                break;
+                                
+                        case 13:
+                                switch (Op2) {
+                                case 0:
+                                        if(write)
+                                                write_pmccntr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmccntr();
+                                        break;
+                                case 1:
+                                        if(write)
+                                                write_pmxevtyper(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmxevtyper();
+                                        break;
+                                case 2:
+                                        if(write)
+                                                write_pmxevcntr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmxevcntr();
+                                        break;
+                                default:
+                                        goto error;
+                                }
+                                break;
+                                
+                        case 12:
+                                switch (Op2) {
+                                case 0:
+                                        if(write)
+                                                write_pmcr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmcr();
+                                        break;
+                                case 1:
+                                        if(write)
+                                                write_pmcntenset(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmcntenset();
+                                        break;
+                                case 2:
+                                        if(write)
+                                                write_pmcntenclr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmcntenclr();
+                                        break;
+                                case 3:
+                                        if(write)
+                                                write_pmovsr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmovsr();
+                                        break;
+                                case 4:
+                                        if(write)
+                                                write_pmswinc(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmswinc();
+                                        break;
+                                case 5:
+                                        if(write)
+                                                write_pmselr(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmselr();
+                                        break;
+                                case 6:
+                                        if(write)
+                                                write_pmceid0(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmceid0();
+                                        break;
+                                case 7:
+                                        if(write)
+                                                write_pmceid1(regs->r[Rt]);
+                                        else
+                                                regs->r[Rt] = read_pmceid1();
+                                        break;
+                                default:
+                                        goto error;                                     
+                                }
+                                break;
+                        }
+                        break;
+                default:
+                        goto error;
+                }
+                break;
+        default:
+                goto error;
+        }
+
+        return;
+
+ error:
+        printf("%s: Unexpected cp15 instruction", __FUNCTION__);
+        printf(" : %s", write ? "MCR p15" : "MRC p15");
+        printf(", %d, %d, %d, %d, %d \n", Op1, Rt, CRn, CRm, Op2);
+        panic();
+
+}
+
+void trap_dabort_handle(unsigned hsr, gp_regs * regs)
+{
+        unsigned hdfar = 0x0, hpfar = 0x0, pa = 0x0, *data = 0x0;
+        unsigned write = 0x0;
+
+        hdfar = read_hdfar();
+        hpfar = read_hpfar();
+
+        pa = ((hpfar >> 4) << 12) + (hdfar & 0xfff);
+        data = &regs->r[(hsr >> 16) & 0xf];
+        write = (hsr >> 6) & 0x1;
+
+        /* Only distributor accesses are virtualised at the moment */
+        if ((pa & ~0xfff) == GIC_ID_PHY_BASE) {
+                handle_vgic_distif_abort(pa, data, write);
+        }
+
+        return;
+}
+
+void HandleVirtualisor(gp_regs * regs)
+{
+        unsigned cpu_id = read_cpuid(), cpu_no = read_midr(), rc = 0;
+        unsigned hsr = read_hsr(), elr = 0, vd_len = 0, index = 0;
+        virt_descriptor *vd_array = &virt_desc_section$$Base;
+        unsigned (*handler) (gp_regs *, unsigned, unsigned) = 0x0, sibling;
+        
+        /* Find our brother from another mother */
+        sibling = find_sibling_cpu();
+        
+        /*
+         * Perform the generic trap handling
+         */
+        switch (hsr >> 26) {
+        case TRAP_DABORT:
+                trap_dabort_handle(hsr, regs);
+                break;
+        case TRAP_CP15_32:
+                trap_cp15_mrc_mcr_handle(hsr, regs);
+                break;
+        default:
+                printf("%s: Unexpected trap", __FUNCTION__);
+                printf(": HSR=0x%x Regs=0x%x \n", hsr, (unsigned) regs);
+                panic();
+        }
+
+        /*
+         * Do any cpu specific trap handling.
+         */
+        vd_len = (unsigned)&virt_desc_section$$Length;
+        for (index = 0; index < (vd_len / sizeof(virt_descriptor)); index++) {
+        
+                if (cpu_no == vd_array[index].cpu_no) {
+                        handler = vd_array[index].trap_handle;
+                        if(handler) {
+                                rc = handler(regs, hsr, sibling);
+                                if (rc) {
+                                        printf("%s: failed on cpu%d \n",
+                                               __FUNCTION__,
+                                               cpu_no);
+                                        goto out;
+                                }
+                        }
+                }
+        }
+
+        /*
+         * This is a trap of the kind where we simply move
+         * onto the next instruction in the actual program.
+         * Move by 2 bytes if we came from Thumb mode else
+         * by 4 bytes.
+         */
+        elr = ((vm_context *) regs)->elr_hyp;
+        if (hsr & (1 << 25))
+                elr += 4;
+        else
+                elr += 2;
+        ((vm_context *) regs)->elr_hyp = elr;
+
+ out:
+        if (rc) {
+                printf("%s: Failed : Cpu%d : Host=0x%x : Target=0x%x\n ", 
+                       __FUNCTION__, cpu_id, cpu_no, sibling);
+                panic();
+        }
+
+        return;
+}