2011-01-31 Sebastian Huber <sebastian.huber@embedded-brains.de>

	* new-exceptions/bspsupport/ppc_exc.S,
	new-exceptions/bspsupport/ppc_exc_asm_macros.h,
	new-exceptions/bspsupport/ppc_exc_naked.S,
	new-exceptions/bspsupport/ppc_exc_prologue.c: Branch targets are now
	global for all minimal prologues.  Generate branch instruction in
	minimal prologues.

5 files changed, 94 insertions, 12 deletions

diff --git a/c/src/lib/libcpu/powerpc/ChangeLog b/c/src/lib/libcpu/powerpc/ChangeLog
index 62c3f0a084..73a58ff4af 100644
--- a/c/src/lib/libcpu/powerpc/ChangeLog
+++ b/c/src/lib/libcpu/powerpc/ChangeLog
@@ -1,5 +1,14 @@
 2011-01-31	Sebastian Huber <sebastian.huber@embedded-brains.de>
 
+	* new-exceptions/bspsupport/ppc_exc.S,
+	new-exceptions/bspsupport/ppc_exc_asm_macros.h,
+	new-exceptions/bspsupport/ppc_exc_naked.S,
+	new-exceptions/bspsupport/ppc_exc_prologue.c: Branch targets are now
+	global for all minimal prologues.  Generate branch instruction in
+	minimal prologues.
+
+2011-01-31	Sebastian Huber <sebastian.huber@embedded-brains.de>
+
 	* e500/mmu/mmu.c: Use common defines.
 
 2011-01-28	Joel Sherrill <joel.sherrilL@OARcorp.com>
diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc.S b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc.S
index 590e936462..511601d9ff 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc.S
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc.S
@@ -66,7 +66,14 @@ ppc_exc_min_prolog_auto:
 	stwu	r1, -EXCEPTION_FRAME_END(r1)
 	stw	VECTOR_REGISTER, VECTOR_OFFSET(r1)
 	mflr	VECTOR_REGISTER
-	bla	wrap_auto
+
+	/*
+	 * We store the absolute branch target address here.  It will be used
+	 * to generate the branch operation in ppc_exc_make_prologue().
+	 *
+	 * We add one to request the link in the generated branch instruction.
+	 */
+	.int	ppc_exc_wrap_auto + 1
 
 	.global ppc_exc_tgpr_clr_prolog_size
 ppc_exc_tgpr_clr_prolog_size = . - ppc_exc_tgpr_clr_prolog
@@ -87,7 +94,14 @@ ppc_exc_min_prolog_auto_async:
 	stw	r1, ppc_exc_lock_std@sdarel(r13)
 	stw	VECTOR_REGISTER, ppc_exc_vector_register_std@sdarel(r13)
 	mflr	VECTOR_REGISTER
-	bla	wrap_auto_async
+
+	/*
+	 * We store the absolute branch target address here.  It will be used
+	 * to generate the branch operation in ppc_exc_make_prologue().
+	 *
+	 * We add one to request the link in the generated branch instruction.
+	 */
+	.int	ppc_exc_wrap_auto_async + 1
 
 /******************************************************/
 /*  WRAPPERS                                          */
@@ -119,7 +133,8 @@ __ppc_exc_wrappers_start = .
 	WRAP	_FLVR=e500_mchk _PRI=mchk _SRR0=mcsrr0 _SRR1=mcsrr1 _RFI=rfmci
 
 	/* LR holds vector, VECTOR_REGISTER holds orig. LR */
-wrap_auto:
+        .global ppc_exc_wrap_auto
+ppc_exc_wrap_auto:
 	stw	FRAME_REGISTER, FRAME_OFFSET(r1)
 
 	/* Find address where we jumped from */
@@ -138,7 +153,8 @@ wrap_auto:
 	 */
 	b	wrap_no_save_frame_register_std
 
-wrap_auto_async:
+        .global ppc_exc_wrap_auto_async
+ppc_exc_wrap_auto_async:
 	stwu	r1, -EXCEPTION_FRAME_END(r1)
 	stw	FRAME_REGISTER, FRAME_OFFSET(r1)
 	/* find address where we jumped from */
diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
index 4f334898c0..32d06cbde2 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
@@ -106,9 +106,12 @@ ppc_exc_min_prolog_async_\_NAME:
 	/*	Load vector.
 	 */
 	li	VECTOR_REGISTER, ( \_VEC | 0xffff8000 )
-	/*	Branch (must be within 32MB)
+
+	/*
+	 * We store the absolute branch target address here.  It will be used
+	 * to generate the branch operation in ppc_exc_make_prologue().
 	 */
-	ba	wrap_\_FLVR
+	.int	ppc_exc_wrap_\_FLVR
 
 	.endm
 
@@ -129,7 +132,12 @@ ppc_exc_min_prolog_sync_\_NAME:
 	stwu	r1, -EXCEPTION_FRAME_END(r1)
 	stw	VECTOR_REGISTER, VECTOR_OFFSET(r1)
 	li	VECTOR_REGISTER, \_VEC
-	ba	wrap_nopush_\_FLVR
+
+	/*
+	 * We store the absolute branch target address here.  It will be used
+	 * to generate the branch operation in ppc_exc_make_prologue().
+	 */
+	.int	ppc_exc_wrap_nopush_\_FLVR
 
 	.endm
 
@@ -374,12 +382,14 @@ recover_check_twiddle_mchk_\_FLVR:
  */
 	.macro	WRAP _FLVR _PRI _SRR0 _SRR1 _RFI
 
-wrap_\_FLVR:
+	.global ppc_exc_wrap_\_FLVR
+ppc_exc_wrap_\_FLVR:
 
 	/* Push exception frame */
 	stwu	r1, -EXCEPTION_FRAME_END(r1)
 
-wrap_nopush_\_FLVR:
+	.global ppc_exc_wrap_nopush_\_FLVR
+ppc_exc_wrap_nopush_\_FLVR:
 
 	/* Save frame register */
 	stw	FRAME_REGISTER, FRAME_OFFSET(r1)
diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_naked.S b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_naked.S
index f0f8d3af92..c5df56f7a5 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_naked.S
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_naked.S
@@ -28,9 +28,15 @@ ppc_exc_min_prolog_tmpl_naked:
 	stwu	r1, -EXCEPTION_FRAME_END(r1)
 	stw	VECTOR_REGISTER, VECTOR_OFFSET(r1)
 	li	VECTOR_REGISTER, 0
-	ba	wrap_naked
 
-wrap_naked:
+	/*
+	 * We store the absolute branch target address here.  It will be used
+	 * to generate the branch operation in ppc_exc_make_prologue().
+	 */
+	.int	ppc_exc_wrap_naked
+
+        .global ppc_exc_wrap_naked
+ppc_exc_wrap_naked:
 
 	/* Save scratch registers */
 	stw	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0_OFFSET(r1)
diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_prologue.c b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_prologue.c
index 51ed847b2c..76fb8a62de 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_prologue.c
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_prologue.c
@@ -62,6 +62,44 @@ static const uint32_t *const ppc_exc_prologue_templates [] = {
   [PPC_EXC_NAKED] = ppc_exc_min_prolog_tmpl_naked
 };
 
+static bool ppc_exc_create_branch_op(
+  unsigned vector,
+  uint32_t *prologue,
+  size_t prologue_size
+)
+{
+  static const uintptr_t BRANCH_OP_CODE = 18 << 26;
+  static const uintptr_t BRANCH_OP_LINK = 0x1;
+  static const uintptr_t BRANCH_OP_ABS = 0x2;
+  static const uintptr_t BRANCH_OP_MSK = 0x3ffffff;
+  size_t branch_op_index = prologue_size / 4 - 1;
+  uintptr_t vector_address = (uintptr_t) ppc_exc_vector_address(vector);
+  uintptr_t branch_op_address = vector_address + 4 * branch_op_index;
+
+  /* This value may have BRANCH_OP_LINK set */
+  uintptr_t target_address = prologue [branch_op_index];
+
+  uintptr_t branch_target_address = target_address - branch_op_address;
+
+  /*
+   * We prefer to use a relative branch.  This has the benefit that custom
+   * minimal prologues in a read-only area are relocatable.
+   */
+  if ((branch_target_address & ~BRANCH_OP_MSK) != 0) {
+    /* Target to far for relative branch (PC ± 32M) */
+    if (target_address >= 0xfe000001 || target_address < 0x01fffffd) {
+      /* Can use an absolute branch */
+      branch_target_address = (target_address | BRANCH_OP_ABS) & BRANCH_OP_MSK;
+    } else {
+      return false;
+    }
+  }
+
+  prologue [branch_op_index] = BRANCH_OP_CODE | branch_target_address;
+
+  return true;
+}
+
 rtems_status_code ppc_exc_make_prologue(
   unsigned vector,
   ppc_exc_category category,
@@ -71,7 +109,6 @@ rtems_status_code ppc_exc_make_prologue(
 {
   const uint32_t *prologue_template = NULL;
   size_t prologue_template_size = 0;
-  uintptr_t vector_address = (uintptr_t) ppc_exc_vector_address(vector);
   bool fixup_vector = false;
 
   if (!ppc_exc_is_valid_category(category)) {
@@ -104,6 +141,10 @@ rtems_status_code ppc_exc_make_prologue(
 
     memcpy(prologue, prologue_template, prologue_template_size);
 
+    if (!ppc_exc_create_branch_op(vector, prologue, prologue_template_size)) {
+      return RTEMS_INVALID_ADDRESS;
+    }
+
     if (fixup_vector) {
       if (vector <= 0x7fffU) {
         prologue [PPC_EXC_PROLOG_VEC_OFFSET] =