2007-11-03 Ray Xu <rayx.cn@gmail.com>

        * start/start.S, startup/bspclean.c: Add veneer between ARM and
        Thumb instructions.  also moved bss init after CPSR/stack init in
	start.S

3 files changed, 41 insertions, 13 deletions

diff --git a/c/src/lib/libbsp/arm/rtl22xx/ChangeLog b/c/src/lib/libbsp/arm/rtl22xx/ChangeLog
index de1af2f27d..3c5c8e76b6 100644
--- a/c/src/lib/libbsp/arm/rtl22xx/ChangeLog
+++ b/c/src/lib/libbsp/arm/rtl22xx/ChangeLog
@@ -1,3 +1,9 @@
+2007-11-03      Ray Xu <rayx.cn@gmail.com>
+
+        * start/start.S, startup/bspclean.c: Add veneer between ARM and 
+        Thumb instructions.  also moved bss init after CPSR/stack init in
+	start.S
+
 2007-09-12	Joel Sherrill <joel.sherrill@OARcorp.com>
 
 	PR 1257/bsps
diff --git a/c/src/lib/libbsp/arm/rtl22xx/start/start.S b/c/src/lib/libbsp/arm/rtl22xx/start/start.S
index d917762a76..5932b69c85 100644
--- a/c/src/lib/libbsp/arm/rtl22xx/start/start.S
+++ b/c/src/lib/libbsp/arm/rtl22xx/start/start.S
@@ -28,6 +28,7 @@
 .equ PSR_T,              0x20
 
 .text
+.code	32
 .globl  _start
 _start:
         /* 
@@ -37,22 +38,11 @@ _start:
          * I'll just set the CPSR for SVC mode, interrupts 
          * off, and ARM instructions.
          */
-        mov     r0, #(PSR_MODE_SVC | PSR_I | PSR_F)
-        msr     cpsr, r0
-
-        /* zero the bss */
-        ldr     r1, =_bss_end_
-        ldr     r0, =_bss_start_
-
-_bss_init:        
-        mov     r2, #0
-        cmp     r0, r1
-        strlot  r2, [r0], #4
-        blo     _bss_init        /* loop while r0 < r1 */ 
         
         /* --- Initialize stack pointer registers */
         /* Enter IRQ mode and set up the IRQ stack pointer */
         mov     r0, #(PSR_MODE_IRQ | PSR_I | PSR_F)     /* No interrupts */
+        bic	r0, r0, #PSR_T
         msr     cpsr, r0
         ldr     r1, =_irq_stack_size
         ldr     sp, =_irq_stack
@@ -60,6 +50,7 @@ _bss_init:
 
         /* Enter FIQ mode and set up the FIQ stack pointer */
         mov     r0, #(PSR_MODE_FIQ | PSR_I | PSR_F)     /* No interrupts */
+        bic	r0, r0, #PSR_T
         msr     cpsr, r0
         ldr     r1, =_fiq_stack_size
         ldr     sp, =_fiq_stack
@@ -67,20 +58,22 @@ _bss_init:
 
         /* Enter ABT mode and set up the ABT stack pointer */
         mov     r0, #(PSR_MODE_ABT | PSR_I | PSR_F)     /* No interrupts */
+        bic	r0, r0, #PSR_T
         msr     cpsr, r0
+        bic	r0, r0, #PSR_T        
         ldr     r1, =_abt_stack_size
         ldr     sp, =_abt_stack
         add     sp, sp, r1
         
         /* Set up the SVC stack pointer last and stay in SVC mode */
         mov     r0, #(PSR_MODE_SVC | PSR_I | PSR_F)     /* No interrupts */
+        bic	r0, r0, #PSR_T
         msr     cpsr, r0
         ldr     r1, =_svc_stack_size
         ldr     sp, =_svc_stack
         add     sp, sp, r1
         sub     sp, sp, #0x64   
               
-
         /* 
          * Initialize the exception vectors. This includes the
          * exceptions vectors (0x00000000-0x0000001c), and the 
@@ -90,18 +83,38 @@ _bss_init:
         adr     r1, vector_block
         ldmia   r1!, {r2-r9}
         stmia   r0!, {r2-r9}
+        
         ldmia   r1!, {r2-r9}
         stmia   r0!, {r2-r9}
 
+
+        /* zero the bss */
+        ldr     r1, =_bss_end_
+        ldr     r0, =_bss_start_
+
+_bss_init:        
+        mov     r2, #0
+        cmp     r0, r1
+        strlot  r2, [r0], #4
+        blo     _bss_init        /* loop while r0 < r1 */ 
+
+
+#ifdef __thumb__
         /* Now we are prepared to start the BSP's C code */
+	ldr	r3, =boot_card
+        bx      r3
+#else
         bl      boot_card
 
+
         /* 
          * Theoretically, we could return to what started us up,
          * but we'd have to have saved the registers and stacks.
          * Instead, we'll just reset.
          */
         bl      bsp_reset
+#endif
+	.code	32
 
         /* We shouldn't get here. If we do, hang */
 _hang:  b       _hang
diff --git a/c/src/lib/libbsp/arm/rtl22xx/startup/bspclean.c b/c/src/lib/libbsp/arm/rtl22xx/startup/bspclean.c
index 3e4986a4a1..2ea71522aa 100644
--- a/c/src/lib/libbsp/arm/rtl22xx/startup/bspclean.c
+++ b/c/src/lib/libbsp/arm/rtl22xx/startup/bspclean.c
@@ -19,7 +19,16 @@ int uart_poll_read(int);
 
 void rtemsReboot (void)
 {
+#ifdef __thumb__
+  int tmp;
+  asm volatile (" .code 16            \n" \
+                "ldr %[tmp], =_start  \n" \
+                "bx  %[tmp]           \n" \
+                "nop                  \n" \
+                : [tmp]"=&r" (tmp) );
+#else                   
   asm volatile ("b _start");
+#endif   
 }
 
 void bsp_cleanup(void)