summaryrefslogtreecommitdiff
path: root/big-little/virtualisor/virt_handle.c
diff options
context:
space:
mode:
Diffstat (limited to 'big-little/virtualisor/virt_handle.c')
-rw-r--r--big-little/virtualisor/virt_handle.c1073
1 files changed, 545 insertions, 528 deletions
diff --git a/big-little/virtualisor/virt_handle.c b/big-little/virtualisor/virt_handle.c
index 3e3f3d7..a247534 100644
--- a/big-little/virtualisor/virt_handle.c
+++ b/big-little/virtualisor/virt_handle.c
@@ -18,7 +18,7 @@
* contributors may be used to endorse or promote products
* derived from this software without specific prior written
* permission.
- */
+ */
#include "virtualisor.h"
#include "virt_helpers.h"
@@ -34,556 +34,573 @@ 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();
+ 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);
+ 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].
- csselr)];
- 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].
- csselr = regs->r[Rt];
- else
- regs->r[Rt] =
- target_cache_geometry[cpu_id].
- csselr;
- 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],
- dccisw,
- &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;
- }
+ 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].csselr)];
+ 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].csselr =
+ regs->r[Rt];
+ else
+ regs->r[Rt] =
+ target_cache_geometry
+ [cpu_id].csselr;
+ 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],
+ dccisw,
+ &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;
+ }
- 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;
+ /*
+ * 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);
- /*
- * 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;
- }
+ if (num_cpus > 4) {
+ printf
+ ("%s: Unexpected L2CTLR read \n",
+ __FUNCTION__);
+ goto error;
+ }
- return;
+ 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();
+ 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;
+ unsigned hdfar = 0x0, hpfar = 0x0, pa = 0x0, *data = 0x0;
+ unsigned write = 0x0;
- hdfar = read_hdfar();
- hpfar = read_hpfar();
+ hdfar = read_hdfar();
+ hpfar = read_hpfar();
- pa = ((hpfar >> 4) << 12) + (hdfar & 0xfff);
- data = &regs->r[(hsr >> 16) & 0xf];
- write = (hsr >> 6) & 0x1;
+ 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);
- }
+ /* Only distributor accesses are virtualised at the moment */
+ if ((pa & ~0xfff) == GIC_ID_PHY_BASE) {
+ handle_vgic_distif_abort(pa, data, write);
+ }
- return;
+ return;
}
void HandleVirtualisor(gp_regs * regs)
{
- unsigned cpu_id = read_cpuid(), cpu_no = PART_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();
- }
+ unsigned cpu_id = read_cpuid(), cpu_no = PART_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++) {
- /*
- * 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;
- }
- }
- }
- }
+ 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;
+ /*
+ * 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();
- }
+ if (rc) {
+ printf("%s: Failed : Cpu%d : Host=0x%x : Target=0x%x\n ",
+ __FUNCTION__, cpu_id, cpu_no, sibling);
+ panic();
+ }
- return;
+ return;
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-01 14:50:10 +0000