3 files changed, 28 insertions, 0 deletions

diff --git a/sys/powerpc/aim/aim_machdep.c b/sys/powerpc/aim/aim_machdep.c
index da2c9ec64568..f026dfff0093 100644
--- a/sys/powerpc/aim/aim_machdep.c
+++ b/sys/powerpc/aim/aim_machdep.c
@@ -148,6 +148,7 @@ extern Elf_Addr	_GLOBAL_OFFSET_TABLE_[];
 
 extern void	*rstcode, *rstcodeend;
 extern void	*trapcode, *trapcodeend;
+extern void	*hypertrapcode, *hypertrapcodeend;
 extern void	*generictrap, *generictrap64;
 extern void	*alitrap, *aliend;
 extern void	*dsitrap, *dsiend;
@@ -360,6 +361,11 @@ aim_cpu_init(vm_offset_t toc)
 		bcopy(&restorebridge, (void *)EXC_TRC, trap_offset);
 		bcopy(&restorebridge, (void *)EXC_BPT, trap_offset);
 	}
+	#else
+	trapsize = (size_t)&hypertrapcodeend - (size_t)&hypertrapcode;
+	bcopy(&hypertrapcode, (void *)(EXC_HEA + trap_offset), trapsize);
+	bcopy(&hypertrapcode, (void *)(EXC_HMI + trap_offset), trapsize);
+	bcopy(&hypertrapcode, (void *)(EXC_HVI + trap_offset), trapsize);
 	#endif
 
 	bcopy(&rstcode, (void *)(EXC_RST + trap_offset), (size_t)&rstcodeend -
diff --git a/sys/powerpc/aim/trap_subr64.S b/sys/powerpc/aim/trap_subr64.S
index 1303b6c1ca3c..f6fd0a135f8f 100644
--- a/sys/powerpc/aim/trap_subr64.S
+++ b/sys/powerpc/aim/trap_subr64.S
@@ -446,6 +446,20 @@ CNAME(trapcode):
 	blrl				/* Branch to generictrap */
 CNAME(trapcodeend):
 
+/* Same thing for traps setting HSRR0/HSS1 */
+	.globl	CNAME(hypertrapcode),CNAME(hypertrapcodeend)
+	.p2align 3
+CNAME(hypertrapcode):
+	mtsprg1	%r1			/* save SP */
+	mflr	%r1			/* Save the old LR in r1 */
+	mtsprg2 %r1			/* And then in SPRG2 */
+	ld	%r1,TRAP_GENTRAP(0)
+	addi	%r1,%r1,(generichypertrap-generictrap)
+	mtlr	%r1
+	li	%r1, 0xe0		/* How to get the vector from LR */
+	blrl				/* Branch to generictrap */
+CNAME(hypertrapcodeend):
+
 /*
  * For SLB misses: do special things for the kernel
  *
@@ -757,6 +771,13 @@ realtrap:
  * SPRG2 - Original LR
  */
 
+generichypertrap:
+	mtsprg3 %r1
+	mfspr	%r1, SPR_HSRR0
+	mtsrr0	%r1
+	mfspr	%r1, SPR_HSRR1
+	mtsrr1	%r1
+	mfsprg3	%r1
 	.globl	CNAME(generictrap)
 generictrap:
 	/* Save R1 for computing the exception vector */
diff --git a/sys/powerpc/powerpc/interrupt.c b/sys/powerpc/powerpc/interrupt.c
index ca86e3c5c1bc..58075fb9a69c 100644
--- a/sys/powerpc/powerpc/interrupt.c
+++ b/sys/powerpc/powerpc/interrupt.c
@@ -86,6 +86,7 @@ powerpc_interrupt(struct trapframe *framep)
 
 	switch (framep->exc) {
 	case EXC_EXI:
+	case EXC_HVI:
 		critical_enter();
 		PIC_DISPATCH(root_pic, framep);
 		critical_exit();