diff mbox

[kvm-unit-tests,v2,1/5] powerpc: add exception handler

Message ID 1458560014-28862-2-git-send-email-lvivier@redhat.com (mailing list archive)
State New, archived
Headers show

Commit Message

Laurent Vivier March 21, 2016, 11:33 a.m. UTC 
Signed-off-by: Laurent Vivier <lvivier@redhat.com>
---
v2:
    clearly restore r1 in call_handler
    use "exception_stack[cpu + 1]" instead of "exception_stack + cpu + 1"

 lib/powerpc/asm/hcall.h     |   1 +
 lib/powerpc/asm/ppc_asm.h   |   5 ++
 lib/powerpc/asm/processor.h |  11 ++++
 lib/powerpc/processor.c     |  38 ++++++++++++
 lib/powerpc/setup.c         |  19 ++++++
 lib/ppc64/asm-offsets.c     |  42 +++++++++++++
 lib/ppc64/asm/processor.h   |   1 +
 lib/ppc64/asm/ptrace.h      |  24 ++++++++
 powerpc/Makefile.common     |   1 +
 powerpc/cstart64.S          | 140 ++++++++++++++++++++++++++++++++++++++++++++
 10 files changed, 282 insertions(+)
 create mode 100644 lib/powerpc/asm/processor.h
 create mode 100644 lib/powerpc/processor.c
 create mode 100644 lib/ppc64/asm/processor.h
 create mode 100644 lib/ppc64/asm/ptrace.h

Comments

David Gibson March 21, 2016, 11:47 p.m. UTC | #1 
On Mon, 21 Mar 2016 12:33:30 +0100
Laurent Vivier <lvivier@redhat.com> wrote:

> Signed-off-by: Laurent Vivier <lvivier@redhat.com>

Reviewed-by: David Gibson <david@gibson.dropbear.id.au>

I'm still not overly fond of the trap >> 8 trick, but it's something we
can fix later if necessary.

> ---
> v2:
>     clearly restore r1 in call_handler
>     use "exception_stack[cpu + 1]" instead of "exception_stack + cpu + 1"
> 
>  lib/powerpc/asm/hcall.h     |   1 +
>  lib/powerpc/asm/ppc_asm.h   |   5 ++
>  lib/powerpc/asm/processor.h |  11 ++++
>  lib/powerpc/processor.c     |  38 ++++++++++++
>  lib/powerpc/setup.c         |  19 ++++++
>  lib/ppc64/asm-offsets.c     |  42 +++++++++++++
>  lib/ppc64/asm/processor.h   |   1 +
>  lib/ppc64/asm/ptrace.h      |  24 ++++++++
>  powerpc/Makefile.common     |   1 +
>  powerpc/cstart64.S          | 140 ++++++++++++++++++++++++++++++++++++++++++++
>  10 files changed, 282 insertions(+)
>  create mode 100644 lib/powerpc/asm/processor.h
>  create mode 100644 lib/powerpc/processor.c
>  create mode 100644 lib/ppc64/asm/processor.h
>  create mode 100644 lib/ppc64/asm/ptrace.h
> 
> diff --git a/lib/powerpc/asm/hcall.h b/lib/powerpc/asm/hcall.h
> index f6f9ea8..99bce79 100644
> --- a/lib/powerpc/asm/hcall.h
> +++ b/lib/powerpc/asm/hcall.h
> @@ -20,6 +20,7 @@
>  #define H_PAGE_INIT		0x2c
>  #define H_PUT_TERM_CHAR		0x58
>  #define H_RANDOM		0x300
> +#define H_SET_MODE		0x31C
>  
>  #ifndef __ASSEMBLY__
>  /*
> diff --git a/lib/powerpc/asm/ppc_asm.h b/lib/powerpc/asm/ppc_asm.h
> index f18100e..39620a3 100644
> --- a/lib/powerpc/asm/ppc_asm.h
> +++ b/lib/powerpc/asm/ppc_asm.h
> @@ -1,6 +1,11 @@
>  #ifndef _ASMPOWERPC_PPC_ASM_H
>  #define _ASMPOWERPC_PPC_ASM_H
>  
> +#include <asm/asm-offsets.h>
> +
> +#define SAVE_GPR(n, base)	std	n,GPR0+8*(n)(base)
> +#define REST_GPR(n, base)	ld	n,GPR0+8*(n)(base)
> +
>  #define LOAD_REG_IMMEDIATE(reg,expr)		\
>  	lis	reg,(expr)@highest;		\
>  	ori	reg,reg,(expr)@higher;		\
> diff --git a/lib/powerpc/asm/processor.h b/lib/powerpc/asm/processor.h
> new file mode 100644
> index 0000000..09692bd
> --- /dev/null
> +++ b/lib/powerpc/asm/processor.h
> @@ -0,0 +1,11 @@
> +#ifndef _ASMPOWERPC_PROCESSOR_H_
> +#define _ASMPOWERPC_PROCESSOR_H_
> +
> +#include <asm/ptrace.h>
> +
> +#ifndef __ASSEMBLY__
> +void handle_exception(int trap, void (*func)(struct pt_regs *, void *), void *);
> +void do_handle_exception(struct pt_regs *regs);
> +#endif /* __ASSEMBLY__ */
> +
> +#endif /* _ASMPOWERPC_PROCESSOR_H_ */
> diff --git a/lib/powerpc/processor.c b/lib/powerpc/processor.c
> new file mode 100644
> index 0000000..a78bc3c
> --- /dev/null
> +++ b/lib/powerpc/processor.c
> @@ -0,0 +1,38 @@
> +/*
> + * processor control and status function
> + */
> +
> +#include <libcflat.h>
> +#include <asm/processor.h>
> +#include <asm/ptrace.h>
> +
> +static struct {
> +	void (*func)(struct pt_regs *, void *data);
> +	void *data;
> +} handlers[16];
> +
> +void handle_exception(int trap, void (*func)(struct pt_regs *, void *),
> +		      void * data)
> +{
> +	trap >>= 8;
> +
> +	if (trap < 16) {
> +		handlers[trap].func = func;
> +		handlers[trap].data = data;
> +	}
> +}
> +
> +void do_handle_exception(struct pt_regs *regs)
> +{
> +	unsigned char v;
> +
> +	v = regs->trap >> 8;
> +
> +	if (v < 16 && handlers[v].func) {
> +		handlers[v].func(regs, handlers[v].data);
> +		return;
> +	}
> +
> +	printf("unhandled cpu exception 0x%lx\n", regs->trap);
> +	abort();
> +}
> diff --git a/lib/powerpc/setup.c b/lib/powerpc/setup.c
> index 0c0c882..e3cf952 100644
> --- a/lib/powerpc/setup.c
> +++ b/lib/powerpc/setup.c
> @@ -16,6 +16,8 @@
>  #include <alloc.h>
>  #include <asm/setup.h>
>  #include <asm/page.h>
> +#include <asm/ppc_asm.h>
> +#include <asm/hcall.h>
>  
>  extern unsigned long stacktop;
>  extern void io_init(void);
> @@ -33,6 +35,10 @@ struct cpu_set_params {
>  	unsigned dcache_bytes;
>  };
>  
> +#define EXCEPTION_STACK_SIZE	(32*1024) /* 32kB */
> +
> +static char exception_stack[NR_CPUS][EXCEPTION_STACK_SIZE];
> +
>  static void cpu_set(int fdtnode, u32 regval, void *info)
>  {
>  	static bool read_common_info = false;
> @@ -46,6 +52,11 @@ static void cpu_set(int fdtnode, u32 regval, void *info)
>  	}
>  	cpus[cpu] = regval;
>  
> +	/* set exception stack address for this CPU (in SPGR0) */
> +
> +	asm volatile ("mtsprg0 %[addr]" ::
> +		      [addr] "r" (exception_stack[cpu + 1]));
> +
>  	if (!read_common_info) {
>  		const struct fdt_property *prop;
>  		u32 *data;
> @@ -76,6 +87,14 @@ static void cpu_init(void)
>  	assert(ret == 0);
>  	__icache_bytes = params.icache_bytes;
>  	__dcache_bytes = params.dcache_bytes;
> +
> +	/* Interrupt Endianness */
> +
> +#if  __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
> +        hcall(H_SET_MODE, 1, 4, 0, 0);
> +#else
> +        hcall(H_SET_MODE, 0, 4, 0, 0);
> +#endif
>  }
>  
>  static void mem_init(phys_addr_t freemem_start)
> diff --git a/lib/ppc64/asm-offsets.c b/lib/ppc64/asm-offsets.c
> index 2d38a71..7843a20 100644
> --- a/lib/ppc64/asm-offsets.c
> +++ b/lib/ppc64/asm-offsets.c
> @@ -5,8 +5,50 @@
>   */
>  #include <libcflat.h>
>  #include <kbuild.h>
> +#include <asm/ptrace.h>
>  
>  int main(void)
>  {
> +	DEFINE(INT_FRAME_SIZE, STACK_INT_FRAME_SIZE);
> +
> +	DEFINE(GPR0, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[0]));
> +	DEFINE(GPR1, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[1]));
> +	DEFINE(GPR2, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[2]));
> +	DEFINE(GPR3, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[3]));
> +	DEFINE(GPR4, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[4]));
> +	DEFINE(GPR5, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[5]));
> +	DEFINE(GPR6, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[6]));
> +	DEFINE(GPR7, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[7]));
> +	DEFINE(GPR8, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[8]));
> +	DEFINE(GPR9, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[9]));
> +	DEFINE(GPR10, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[10]));
> +	DEFINE(GPR11, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[11]));
> +	DEFINE(GPR12, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[12]));
> +	DEFINE(GPR13, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[13]));
> +	DEFINE(GPR14, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[14]));
> +	DEFINE(GPR15, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[15]));
> +	DEFINE(GPR16, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[16]));
> +	DEFINE(GPR17, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[17]));
> +	DEFINE(GPR18, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[18]));
> +	DEFINE(GPR19, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[19]));
> +	DEFINE(GPR20, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[20]));
> +	DEFINE(GPR21, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[21]));
> +	DEFINE(GPR22, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[22]));
> +	DEFINE(GPR23, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[23]));
> +	DEFINE(GPR24, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[24]));
> +	DEFINE(GPR25, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[25]));
> +	DEFINE(GPR26, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[26]));
> +	DEFINE(GPR27, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[27]));
> +	DEFINE(GPR28, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[28]));
> +	DEFINE(GPR29, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[29]));
> +	DEFINE(GPR30, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[30]));
> +	DEFINE(GPR31, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[31]));
> +	DEFINE(_NIP, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, nip));
> +	DEFINE(_MSR, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, msr));
> +	DEFINE(_CTR, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, ctr));
> +	DEFINE(_LINK, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, link));
> +	DEFINE(_XER, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, xer));
> +	DEFINE(_CCR, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, ccr));
> +	DEFINE(_TRAP, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, trap));
>  	return 0;
>  }
> diff --git a/lib/ppc64/asm/processor.h b/lib/ppc64/asm/processor.h
> new file mode 100644
> index 0000000..066a51a
> --- /dev/null
> +++ b/lib/ppc64/asm/processor.h
> @@ -0,0 +1 @@
> +#include "../../powerpc/asm/processor.h"
> diff --git a/lib/ppc64/asm/ptrace.h b/lib/ppc64/asm/ptrace.h
> new file mode 100644
> index 0000000..076c9d9
> --- /dev/null
> +++ b/lib/ppc64/asm/ptrace.h
> @@ -0,0 +1,24 @@
> +#ifndef _ASMPPC64_PTRACE_H_
> +#define _ASMPPC64_PTRACE_H_
> +
> +#define KERNEL_REDZONE_SIZE	288
> +#define STACK_FRAME_OVERHEAD    112     /* size of minimum stack frame */
> +
> +#ifndef __ASSEMBLY__
> +struct pt_regs {
> +	unsigned long gpr[32];
> +	unsigned long nip;
> +	unsigned long msr;
> +	unsigned long ctr;
> +	unsigned long link;
> +	unsigned long xer;
> +	unsigned long ccr;
> +	unsigned long trap;
> +};
> +
> +#define STACK_INT_FRAME_SIZE    (sizeof(struct pt_regs) + \
> +				 STACK_FRAME_OVERHEAD + KERNEL_REDZONE_SIZE)
> +
> +#endif /* __ASSEMBLY__ */
> +
> +#endif /* _ASMPPC64_PTRACE_H_ */
> diff --git a/powerpc/Makefile.common b/powerpc/Makefile.common
> index 424983e..ab2caf6 100644
> --- a/powerpc/Makefile.common
> +++ b/powerpc/Makefile.common
> @@ -31,6 +31,7 @@ cflatobjs += lib/powerpc/io.o
>  cflatobjs += lib/powerpc/hcall.o
>  cflatobjs += lib/powerpc/setup.o
>  cflatobjs += lib/powerpc/rtas.o
> +cflatobjs += lib/powerpc/processor.o
>  
>  FLATLIBS = $(libcflat) $(LIBFDT_archive)
>  %.elf: CFLAGS += $(arch_CFLAGS)
> diff --git a/powerpc/cstart64.S b/powerpc/cstart64.S
> index c87e3d6..ceb6397 100644
> --- a/powerpc/cstart64.S
> +++ b/powerpc/cstart64.S
> @@ -9,6 +9,7 @@
>  #include <asm/hcall.h>
>  #include <asm/ppc_asm.h>
>  #include <asm/rtas.h>
> +#include <asm/ptrace.h>
>  
>  .section .init
>  
> @@ -45,6 +46,34 @@ start:
>  	add	r4, r4, r31
>  	bl	relocate
>  
> +	/* relocate vector table to base address 0x0 (MSR_IP = 0) */
> +
> +	/* source: r4, dest end: r5, destination: r6 */
> +
> +	LOAD_REG_ADDR(r4, __start_interrupts)
> +	LOAD_REG_ADDR(r5, __end_interrupts)
> +	sub	r5,r5,r4
> +	li	r6,0x100
> +
> +	sub	r4,r4,r6
> +	add	r5,r5,r6
> +	addi	r6,r6,-8
> +2:	li	r0,8
> +	mtctr	r0
> +	/* copy a cache line size */
> +3:	addi	r6,r6,8
> +	ldx	r0,r6,r4
> +	stdx	r0,0,r6
> +	bdnz	3b
> +	dcbst	0,r6
> +	/* flush icache */
> +	sync
> +	icbi	0,r6
> +	cmpld	0,r6,r5
> +	blt	2b
> +	sync
> +	isync
> +
>  	/* patch sc1 if needed */
>  	bl	hcall_have_broken_sc1
>  	cmpwi	r3, 0
> @@ -105,3 +134,114 @@ rtas_return_loc:
>  	ld	r0, 16(r1)
>  	mtlr	r0
>  	blr
> +
> +call_handler:
> +	/* save context */
> +
> +	/* GPRs */
> +
> +	.irp i, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 \
> +	        17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31
> +		SAVE_GPR(\i, r1)
> +	.endr
> +	mfsprg1	r0
> +	std	r0,GPR1(r1)
> +
> +	/* lr, xer, ccr */
> +
> +	mflr	r0
> +	std	r0,_LINK(r1)
> +
> +	mfxer	r0
> +	std	r0,_XER(r1)
> +
> +	mfcr	r0
> +	std	r0,_CCR(r1)
> +
> +	/* nip and msr */
> +
> +	mfsrr0	r0
> +	std	r0, _NIP(r1)
> +
> +	mfsrr1	r0
> +	std	r0, _MSR(r1)
> +
> +	/* FIXME: build stack frame */
> +
> +	/* call generic handler */
> +
> +	addi	r3,r1,STACK_FRAME_OVERHEAD
> +	bl	do_handle_exception
> +
> +	/* restore context */
> +
> +	ld	r0,_CTR(r1)
> +	mtctr	r0
> +
> +	ld	r0,_LINK(r1)
> +	mtlr	r0
> +
> +	ld	r0,_XER(r1)
> +	mtxer	r0
> +
> +	ld	r0,_CCR(r1)
> +	mtcr	r0
> +
> +	ld	r0, _NIP(r1)
> +	mtsrr0	r0
> +
> +	ld	r0, _MSR(r1)
> +	mtsrr1	r0
> +
> +	.irp i, 0, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 \
> +	        17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31
> +		REST_GPR(\i, r1)
> +	.endr
> +
> +	/* restore r1, as we don't need it anymore */
> +
> +	REST_GPR(1,r1)
> +
> +	rfid
> +	b .
> +
> +.section .text.ex
> +
> +.macro VECTOR vec
> +	. = \vec
> +
> +	mtsprg1	r1	/* save r1 */
> +	mfsprg0	r1	/* get exception stack address */
> +	subi	r1,r1, INT_FRAME_SIZE
> +
> +	/* save r0 and ctr to call generic handler */
> +
> +	SAVE_GPR(0,r1)
> +
> +	mfctr	r0
> +	std	r0,_CTR(r1)
> +
> +	LOAD_REG_ADDR(r0, call_handler)
> +	mtctr	r0
> +
> +	li	r0,\vec
> +	std	r0,_TRAP(r1)
> +
> +	bctr
> +.endm
> +
> +	. = 0x100
> +	.globl __start_interrupts
> +__start_interrupts:
> +
> +VECTOR(0x300)
> +VECTOR(0x400)
> +VECTOR(0x500)
> +VECTOR(0x600)
> +VECTOR(0x700)
> +VECTOR(0x800)
> +VECTOR(0x900)
> +
> +	.align 7
> +	.globl __end_interrupts
> +__end_interrupts:
> -- 
> 2.5.0
>
diff mbox

Patch

diff --git a/lib/powerpc/asm/hcall.h b/lib/powerpc/asm/hcall.h
index f6f9ea8..99bce79 100644
--- a/lib/powerpc/asm/hcall.h
+++ b/lib/powerpc/asm/hcall.h
@@ -20,6 +20,7 @@ 
 #define H_PAGE_INIT		0x2c
 #define H_PUT_TERM_CHAR		0x58
 #define H_RANDOM		0x300
+#define H_SET_MODE		0x31C
 
 #ifndef __ASSEMBLY__
 /*
diff --git a/lib/powerpc/asm/ppc_asm.h b/lib/powerpc/asm/ppc_asm.h
index f18100e..39620a3 100644
--- a/lib/powerpc/asm/ppc_asm.h
+++ b/lib/powerpc/asm/ppc_asm.h
@@ -1,6 +1,11 @@ 
 #ifndef _ASMPOWERPC_PPC_ASM_H
 #define _ASMPOWERPC_PPC_ASM_H
 
+#include <asm/asm-offsets.h>
+
+#define SAVE_GPR(n, base)	std	n,GPR0+8*(n)(base)
+#define REST_GPR(n, base)	ld	n,GPR0+8*(n)(base)
+
 #define LOAD_REG_IMMEDIATE(reg,expr)		\
 	lis	reg,(expr)@highest;		\
 	ori	reg,reg,(expr)@higher;		\
diff --git a/lib/powerpc/asm/processor.h b/lib/powerpc/asm/processor.h
new file mode 100644
index 0000000..09692bd
--- /dev/null
+++ b/lib/powerpc/asm/processor.h
@@ -0,0 +1,11 @@ 
+#ifndef _ASMPOWERPC_PROCESSOR_H_
+#define _ASMPOWERPC_PROCESSOR_H_
+
+#include <asm/ptrace.h>
+
+#ifndef __ASSEMBLY__
+void handle_exception(int trap, void (*func)(struct pt_regs *, void *), void *);
+void do_handle_exception(struct pt_regs *regs);
+#endif /* __ASSEMBLY__ */
+
+#endif /* _ASMPOWERPC_PROCESSOR_H_ */
diff --git a/lib/powerpc/processor.c b/lib/powerpc/processor.c
new file mode 100644
index 0000000..a78bc3c
--- /dev/null
+++ b/lib/powerpc/processor.c
@@ -0,0 +1,38 @@ 
+/*
+ * processor control and status function
+ */
+
+#include <libcflat.h>
+#include <asm/processor.h>
+#include <asm/ptrace.h>
+
+static struct {
+	void (*func)(struct pt_regs *, void *data);
+	void *data;
+} handlers[16];
+
+void handle_exception(int trap, void (*func)(struct pt_regs *, void *),
+		      void * data)
+{
+	trap >>= 8;
+
+	if (trap < 16) {
+		handlers[trap].func = func;
+		handlers[trap].data = data;
+	}
+}
+
+void do_handle_exception(struct pt_regs *regs)
+{
+	unsigned char v;
+
+	v = regs->trap >> 8;
+
+	if (v < 16 && handlers[v].func) {
+		handlers[v].func(regs, handlers[v].data);
+		return;
+	}
+
+	printf("unhandled cpu exception 0x%lx\n", regs->trap);
+	abort();
+}
diff --git a/lib/powerpc/setup.c b/lib/powerpc/setup.c
index 0c0c882..e3cf952 100644
--- a/lib/powerpc/setup.c
+++ b/lib/powerpc/setup.c
@@ -16,6 +16,8 @@ 
 #include <alloc.h>
 #include <asm/setup.h>
 #include <asm/page.h>
+#include <asm/ppc_asm.h>
+#include <asm/hcall.h>
 
 extern unsigned long stacktop;
 extern void io_init(void);
@@ -33,6 +35,10 @@  struct cpu_set_params {
 	unsigned dcache_bytes;
 };
 
+#define EXCEPTION_STACK_SIZE	(32*1024) /* 32kB */
+
+static char exception_stack[NR_CPUS][EXCEPTION_STACK_SIZE];
+
 static void cpu_set(int fdtnode, u32 regval, void *info)
 {
 	static bool read_common_info = false;
@@ -46,6 +52,11 @@  static void cpu_set(int fdtnode, u32 regval, void *info)
 	}
 	cpus[cpu] = regval;
 
+	/* set exception stack address for this CPU (in SPGR0) */
+
+	asm volatile ("mtsprg0 %[addr]" ::
+		      [addr] "r" (exception_stack[cpu + 1]));
+
 	if (!read_common_info) {
 		const struct fdt_property *prop;
 		u32 *data;
@@ -76,6 +87,14 @@  static void cpu_init(void)
 	assert(ret == 0);
 	__icache_bytes = params.icache_bytes;
 	__dcache_bytes = params.dcache_bytes;
+
+	/* Interrupt Endianness */
+
+#if  __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+        hcall(H_SET_MODE, 1, 4, 0, 0);
+#else
+        hcall(H_SET_MODE, 0, 4, 0, 0);
+#endif
 }
 
 static void mem_init(phys_addr_t freemem_start)
diff --git a/lib/ppc64/asm-offsets.c b/lib/ppc64/asm-offsets.c
index 2d38a71..7843a20 100644
--- a/lib/ppc64/asm-offsets.c
+++ b/lib/ppc64/asm-offsets.c
@@ -5,8 +5,50 @@ 
  */
 #include <libcflat.h>
 #include <kbuild.h>
+#include <asm/ptrace.h>
 
 int main(void)
 {
+	DEFINE(INT_FRAME_SIZE, STACK_INT_FRAME_SIZE);
+
+	DEFINE(GPR0, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[0]));
+	DEFINE(GPR1, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[1]));
+	DEFINE(GPR2, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[2]));
+	DEFINE(GPR3, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[3]));
+	DEFINE(GPR4, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[4]));
+	DEFINE(GPR5, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[5]));
+	DEFINE(GPR6, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[6]));
+	DEFINE(GPR7, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[7]));
+	DEFINE(GPR8, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[8]));
+	DEFINE(GPR9, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[9]));
+	DEFINE(GPR10, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[10]));
+	DEFINE(GPR11, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[11]));
+	DEFINE(GPR12, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[12]));
+	DEFINE(GPR13, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[13]));
+	DEFINE(GPR14, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[14]));
+	DEFINE(GPR15, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[15]));
+	DEFINE(GPR16, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[16]));
+	DEFINE(GPR17, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[17]));
+	DEFINE(GPR18, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[18]));
+	DEFINE(GPR19, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[19]));
+	DEFINE(GPR20, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[20]));
+	DEFINE(GPR21, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[21]));
+	DEFINE(GPR22, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[22]));
+	DEFINE(GPR23, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[23]));
+	DEFINE(GPR24, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[24]));
+	DEFINE(GPR25, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[25]));
+	DEFINE(GPR26, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[26]));
+	DEFINE(GPR27, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[27]));
+	DEFINE(GPR28, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[28]));
+	DEFINE(GPR29, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[29]));
+	DEFINE(GPR30, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[30]));
+	DEFINE(GPR31, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, gpr[31]));
+	DEFINE(_NIP, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, nip));
+	DEFINE(_MSR, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, msr));
+	DEFINE(_CTR, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, ctr));
+	DEFINE(_LINK, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, link));
+	DEFINE(_XER, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, xer));
+	DEFINE(_CCR, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, ccr));
+	DEFINE(_TRAP, STACK_FRAME_OVERHEAD+offsetof(struct pt_regs, trap));
 	return 0;
 }
diff --git a/lib/ppc64/asm/processor.h b/lib/ppc64/asm/processor.h
new file mode 100644
index 0000000..066a51a
--- /dev/null
+++ b/lib/ppc64/asm/processor.h
@@ -0,0 +1 @@ 
+#include "../../powerpc/asm/processor.h"
diff --git a/lib/ppc64/asm/ptrace.h b/lib/ppc64/asm/ptrace.h
new file mode 100644
index 0000000..076c9d9
--- /dev/null
+++ b/lib/ppc64/asm/ptrace.h
@@ -0,0 +1,24 @@ 
+#ifndef _ASMPPC64_PTRACE_H_
+#define _ASMPPC64_PTRACE_H_
+
+#define KERNEL_REDZONE_SIZE	288
+#define STACK_FRAME_OVERHEAD    112     /* size of minimum stack frame */
+
+#ifndef __ASSEMBLY__
+struct pt_regs {
+	unsigned long gpr[32];
+	unsigned long nip;
+	unsigned long msr;
+	unsigned long ctr;
+	unsigned long link;
+	unsigned long xer;
+	unsigned long ccr;
+	unsigned long trap;
+};
+
+#define STACK_INT_FRAME_SIZE    (sizeof(struct pt_regs) + \
+				 STACK_FRAME_OVERHEAD + KERNEL_REDZONE_SIZE)
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* _ASMPPC64_PTRACE_H_ */
diff --git a/powerpc/Makefile.common b/powerpc/Makefile.common
index 424983e..ab2caf6 100644
--- a/powerpc/Makefile.common
+++ b/powerpc/Makefile.common
@@ -31,6 +31,7 @@  cflatobjs += lib/powerpc/io.o
 cflatobjs += lib/powerpc/hcall.o
 cflatobjs += lib/powerpc/setup.o
 cflatobjs += lib/powerpc/rtas.o
+cflatobjs += lib/powerpc/processor.o
 
 FLATLIBS = $(libcflat) $(LIBFDT_archive)
 %.elf: CFLAGS += $(arch_CFLAGS)
diff --git a/powerpc/cstart64.S b/powerpc/cstart64.S
index c87e3d6..ceb6397 100644
--- a/powerpc/cstart64.S
+++ b/powerpc/cstart64.S
@@ -9,6 +9,7 @@ 
 #include <asm/hcall.h>
 #include <asm/ppc_asm.h>
 #include <asm/rtas.h>
+#include <asm/ptrace.h>
 
 .section .init
 
@@ -45,6 +46,34 @@  start:
 	add	r4, r4, r31
 	bl	relocate
 
+	/* relocate vector table to base address 0x0 (MSR_IP = 0) */
+
+	/* source: r4, dest end: r5, destination: r6 */
+
+	LOAD_REG_ADDR(r4, __start_interrupts)
+	LOAD_REG_ADDR(r5, __end_interrupts)
+	sub	r5,r5,r4
+	li	r6,0x100
+
+	sub	r4,r4,r6
+	add	r5,r5,r6
+	addi	r6,r6,-8
+2:	li	r0,8
+	mtctr	r0
+	/* copy a cache line size */
+3:	addi	r6,r6,8
+	ldx	r0,r6,r4
+	stdx	r0,0,r6
+	bdnz	3b
+	dcbst	0,r6
+	/* flush icache */
+	sync
+	icbi	0,r6
+	cmpld	0,r6,r5
+	blt	2b
+	sync
+	isync
+
 	/* patch sc1 if needed */
 	bl	hcall_have_broken_sc1
 	cmpwi	r3, 0
@@ -105,3 +134,114 @@  rtas_return_loc:
 	ld	r0, 16(r1)
 	mtlr	r0
 	blr
+
+call_handler:
+	/* save context */
+
+	/* GPRs */
+
+	.irp i, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 \
+	        17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31
+		SAVE_GPR(\i, r1)
+	.endr
+	mfsprg1	r0
+	std	r0,GPR1(r1)
+
+	/* lr, xer, ccr */
+
+	mflr	r0
+	std	r0,_LINK(r1)
+
+	mfxer	r0
+	std	r0,_XER(r1)
+
+	mfcr	r0
+	std	r0,_CCR(r1)
+
+	/* nip and msr */
+
+	mfsrr0	r0
+	std	r0, _NIP(r1)
+
+	mfsrr1	r0
+	std	r0, _MSR(r1)
+
+	/* FIXME: build stack frame */
+
+	/* call generic handler */
+
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	bl	do_handle_exception
+
+	/* restore context */
+
+	ld	r0,_CTR(r1)
+	mtctr	r0
+
+	ld	r0,_LINK(r1)
+	mtlr	r0
+
+	ld	r0,_XER(r1)
+	mtxer	r0
+
+	ld	r0,_CCR(r1)
+	mtcr	r0
+
+	ld	r0, _NIP(r1)
+	mtsrr0	r0
+
+	ld	r0, _MSR(r1)
+	mtsrr1	r0
+
+	.irp i, 0, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 \
+	        17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31
+		REST_GPR(\i, r1)
+	.endr
+
+	/* restore r1, as we don't need it anymore */
+
+	REST_GPR(1,r1)
+
+	rfid
+	b .
+
+.section .text.ex
+
+.macro VECTOR vec
+	. = \vec
+
+	mtsprg1	r1	/* save r1 */
+	mfsprg0	r1	/* get exception stack address */
+	subi	r1,r1, INT_FRAME_SIZE
+
+	/* save r0 and ctr to call generic handler */
+
+	SAVE_GPR(0,r1)
+
+	mfctr	r0
+	std	r0,_CTR(r1)
+
+	LOAD_REG_ADDR(r0, call_handler)
+	mtctr	r0
+
+	li	r0,\vec
+	std	r0,_TRAP(r1)
+
+	bctr
+.endm
+
+	. = 0x100
+	.globl __start_interrupts
+__start_interrupts:
+
+VECTOR(0x300)
+VECTOR(0x400)
+VECTOR(0x500)
+VECTOR(0x600)
+VECTOR(0x700)
+VECTOR(0x800)
+VECTOR(0x900)
+
+	.align 7
+	.globl __end_interrupts
+__end_interrupts: