20 files changed, 54 insertions, 38 deletions

diff --git a/firmware/target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S b/firmware/target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
index f71216c674..690641c850 100644
--- a/firmware/target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
+++ b/firmware/target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
@@ -241,7 +241,7 @@ lcd_write_yuv420_lines:
     tst         r7, #DBOP_BUSY          @ fifo not empty?
     beq         1b                      @
 
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -545,6 +545,6 @@ lcd_write_yuv420_lines_odither:
     tst         r7, #DBOP_BUSY          @ fifo not empty?
     beq         1b                      @
 
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/as3525/sansa-clip/lcd-as-clip.S b/firmware/target/arm/as3525/sansa-clip/lcd-as-clip.S
index 942ddf79f8..431dc62da0 100644
--- a/firmware/target/arm/as3525/sansa-clip/lcd-as-clip.S
+++ b/firmware/target/arm/as3525/sansa-clip/lcd-as-clip.S
@@ -19,6 +19,7 @@
  *
  ****************************************************************************/
 
+#include "config.h"
 #include "as3525.h"
 
     .text
@@ -90,5 +91,5 @@ lcd_grey_data:
     ands    r5, r5, #(1<<10)    @ wait until push fifo empties
     beq     1b
 
-    ldmfd   sp!, {r4-r7, pc}
+    ldmpc   regs=r4-r7
     .size   lcd_grey_data,.-lcd_grey_data
diff --git a/firmware/target/arm/ata-as-arm.S b/firmware/target/arm/ata-as-arm.S
index b1cafc2221..101bc4dcc1 100644
--- a/firmware/target/arm/ata-as-arm.S
+++ b/firmware/target/arm/ata-as-arm.S
@@ -146,7 +146,7 @@ copy_read_sectors:
 
     strb    r3, [r0], #1        /* store final byte */
 
-    ldmfd   sp!, {r4, r5, pc}
+    ldmpc   regs=r4-r5
 
     /* 16-bit aligned */
 .r_aligned:
@@ -195,7 +195,7 @@ copy_read_sectors:
     ldrneh  r3, [r2]
     strneh  r3, [r0], #2
 
-    ldmfd   sp!, {r4, r5, pc}
+    ldmpc   regs=r4-r5
 
 .r_end:
     .size   copy_read_sectors,.r_end-copy_read_sectors
@@ -300,7 +300,7 @@ copy_write_sectors:
     orr     r3, r3, r4, lsl #8
     strh    r3, [r2]            /* write final halfword */
 
-    ldmfd   sp!, {r4, r5, pc}
+    ldmpc   regs=r4-r5
 
     /* 16-bit aligned */
 .w_aligned:
@@ -349,7 +349,7 @@ copy_write_sectors:
     ldrneh  r3, [r0], #2
     strneh  r3, [r2]
 
-    ldmfd   sp!, {r4, r5, pc}
+    ldmpc   regs=r4-r5
 
 .w_end:
     .size   copy_write_sectors,.w_end-copy_write_sectors
diff --git a/firmware/target/arm/ipod/lcd-as-gray.S b/firmware/target/arm/ipod/lcd-as-gray.S
index 1364c1f965..cfd179a714 100644
--- a/firmware/target/arm/ipod/lcd-as-gray.S
+++ b/firmware/target/arm/ipod/lcd-as-gray.S
@@ -97,7 +97,7 @@ lcd_write_data_shifted:
     subs    r1, r1, #1
     bne     .sloop
 
-    ldmfd   sp!, {r4, pc}
+    ldmpc   regs=r4
     .size   lcd_write_data_shifted,.-lcd_write_data_shifted
     
 #elif defined IPOD_MINI
@@ -132,7 +132,7 @@ lcd_write_data_shifted:
     subs    r1, r1, #1
     bne     .sloop
 
-    ldr     pc, [sp], #4
+    ldrpc
     .size   lcd_write_data_shifted,.-lcd_write_data_shifted
 
 #endif
@@ -179,7 +179,7 @@ lcd_mono_data:
     subs    r1, r1, #1
     bne     .mloop
 
-    ldmfd   sp!, {r4, pc}
+    ldmpc   regs=r4
 
 .dibits:
     .byte   0x00, 0x03, 0x0C, 0x0F, 0x30, 0x33, 0x3C, 0x3F
@@ -267,6 +267,6 @@ lcd_grey_data:
     subs    r2, r2, #1
     bne     .greyloop
 
-    ldmfd   sp!, {r4-r7, pc}
+    ldmpc   regs=r4-r7
     .size   lcd_grey_data,.-lcd_grey_data
 
diff --git a/firmware/target/arm/ipod/video/lcd-as-video.S b/firmware/target/arm/ipod/video/lcd-as-video.S
index fa88dbc940..47155b8c75 100644
--- a/firmware/target/arm/ipod/video/lcd-as-video.S
+++ b/firmware/target/arm/ipod/video/lcd-as-video.S
@@ -19,6 +19,8 @@
  *
  ****************************************************************************/
 
+#include "config.h"
+
     .section .icode, "ax", %progbits
 
 /****************************************************************************
@@ -60,7 +62,7 @@ lcd_write_data:                       /* r1 = pixel count, must be even */
     ldrne   r3, [r0], #4
     strne   r3, [lr]
 
-    ldmfd   sp!, {r4, pc}
+    ldmpc   regs=r4
 
 /****************************************************************************
  * extern void lcd_write_yuv420_lines(unsigned char const * const src[3],
@@ -294,7 +296,7 @@ lcd_write_yuv420_lines:
 
     ldr         r3, [sp, #12]
     add         sp, sp, r3            /* deallocate buffer */
-    ldmfd       sp!, { r4-r10, pc }   /* restore registers */
+    ldmpc       regs=r4-r10           /* restore registers */
 
     .ltorg
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
diff --git a/firmware/target/arm/iriver/h10/lcd-as-h10.S b/firmware/target/arm/iriver/h10/lcd-as-h10.S
index b3f12e46b6..8e851d8441 100644
--- a/firmware/target/arm/iriver/h10/lcd-as-h10.S
+++ b/firmware/target/arm/iriver/h10/lcd-as-h10.S
@@ -232,7 +232,7 @@ lcd_write_yuv420_lines:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -533,6 +533,6 @@ lcd_write_yuv420_lines_odither:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/lcd-as-memframe.S b/firmware/target/arm/lcd-as-memframe.S
index 4532bab221..87cbb611ca 100644
--- a/firmware/target/arm/lcd-as-memframe.S
+++ b/firmware/target/arm/lcd-as-memframe.S
@@ -101,7 +101,7 @@ lcd_copy_buffer_rect:                   @
     add     r0, r0, r4, lsl #1          @
     subs    r3, r3, #1                  @ next line
     bgt     10b @ copy line             @
-    ldmfd   sp!, { r4-r11, pc }         @ restore regs and return
+    ldmpc   regs=r4-r11                 @ restore regs and return
     .ltorg                              @ dump constant pool
     .size   lcd_copy_buffer_rect, .-lcd_copy_buffer_rect
 
@@ -344,7 +344,7 @@ lcd_write_yuv420_lines:
     subs        r2, r2, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r10, pc }     @ restore registers and return
+    ldmpc       regs=r4-r10             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -691,6 +691,6 @@ lcd_write_yuv420_lines_odither:
     subs        r2, r2, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/memcpy-arm.S b/firmware/target/arm/memcpy-arm.S
index d17d659a28..2a55fb5656 100644
--- a/firmware/target/arm/memcpy-arm.S
+++ b/firmware/target/arm/memcpy-arm.S
@@ -98,7 +98,7 @@ memcpy:
         strcsb  r4, [r0], #1
         strcsb  ip, [r0]
 
-        ldmfd   sp!, {r0, r4, pc}
+        ldmpc   regs="r0, r4"
 
 9:      rsb ip, ip, #4
         cmp ip, #2
diff --git a/firmware/target/arm/memmove-arm.S b/firmware/target/arm/memmove-arm.S
index ce056d956b..d8cab048be 100644
--- a/firmware/target/arm/memmove-arm.S
+++ b/firmware/target/arm/memmove-arm.S
@@ -112,7 +112,7 @@ memmove:
         strneb  r3, [r0, #-1]!
         strcsb  r4, [r0, #-1]!
         strcsb  ip, [r0, #-1]
-        ldmfd   sp!, {r0, r4, pc}
+        ldmpc   regs="r0, r4"
 
 9:      cmp ip, #2
         ldrgtb  r3, [r1, #-1]!
diff --git a/firmware/target/arm/memset-arm.S b/firmware/target/arm/memset-arm.S
index 5dbde7a1d7..682da874ce 100644
--- a/firmware/target/arm/memset-arm.S
+++ b/firmware/target/arm/memset-arm.S
@@ -69,7 +69,7 @@ memset:
         stmgedb r0!, {r1, r3, ip, lr}
         stmgedb r0!, {r1, r3, ip, lr}
         bgt     3b
-        ldreq   pc, [sp], #4            @ Now <64 bytes to go.
+        ldrpc   cond=eq                 @ Now <64 bytes to go.
 /*
  * No need to correct the count; we're only testing bits from now on
  */
diff --git a/firmware/target/arm/memset16-arm.S b/firmware/target/arm/memset16-arm.S
index 63d626480e..5c787b1bed 100644
--- a/firmware/target/arm/memset16-arm.S
+++ b/firmware/target/arm/memset16-arm.S
@@ -59,7 +59,7 @@ memset16:
         stmgeia r0!, {r1, r3, ip, lr}
         stmgeia r0!, {r1, r3, ip, lr}
         bgt     2b
-        ldreq   pc, [sp], #4            @ Now <64 bytes to go.
+        ldrpc   cond=eq                 @ Now <64 bytes to go.
 /*
  * No need to correct the count; we're only testing bits from now on
  */
diff --git a/firmware/target/arm/olympus/mrobe-100/lcd-as-mr100.S b/firmware/target/arm/olympus/mrobe-100/lcd-as-mr100.S
index 6db6c7efad..09778014c1 100644
--- a/firmware/target/arm/olympus/mrobe-100/lcd-as-mr100.S
+++ b/firmware/target/arm/olympus/mrobe-100/lcd-as-mr100.S
@@ -99,6 +99,6 @@ lcd_grey_data:
     subs    r2, r2, #1
     bne     .greyloop
 
-    ldmfd   sp!, {r4-r7, pc}
+    ldmpc   regs=r4-r7
     .size   lcd_grey_data,.-lcd_grey_data
 
diff --git a/firmware/target/arm/pbell/vibe500/lcd-as-vibe500.S b/firmware/target/arm/pbell/vibe500/lcd-as-vibe500.S
index d5d51575a7..9079be6aa7 100644
--- a/firmware/target/arm/pbell/vibe500/lcd-as-vibe500.S
+++ b/firmware/target/arm/pbell/vibe500/lcd-as-vibe500.S
@@ -243,7 +243,7 @@ lcd_write_yuv420_lines:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -551,6 +551,6 @@ lcd_write_yuv420_lines_odither:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/pcm-pp.c b/firmware/target/arm/pcm-pp.c
index 6289b4c730..bffc69f771 100644
--- a/firmware/target/arm/pcm-pp.c
+++ b/firmware/target/arm/pcm-pp.c
@@ -74,9 +74,14 @@ void fiq_handler(void) ICODE_ATTR __attribute__((naked));
 void fiq_handler(void)
 {
     asm volatile (
-        "ldr pc, [pc, #-4] \n"
-    "fiq_function:         \n"
-        ".word 0           \n"
+#if ARM_ARCH == 4 && defined(USE_THUMB)
+        "ldr r12, [pc, #-4] \n"
+        "bx  r12            \n"
+#else
+        "ldr pc, [pc, #-4]  \n"
+#endif
+    "fiq_function:          \n"
+        ".word 0            \n"
     );
 }
 
diff --git a/firmware/target/arm/philips/hdd1630/lcd-as-hdd1630.S b/firmware/target/arm/philips/hdd1630/lcd-as-hdd1630.S
index 73ad84ae45..7be807ae98 100644
--- a/firmware/target/arm/philips/hdd1630/lcd-as-hdd1630.S
+++ b/firmware/target/arm/philips/hdd1630/lcd-as-hdd1630.S
@@ -248,7 +248,7 @@ lcd_write_yuv420_lines:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -565,6 +565,6 @@ lcd_write_yuv420_lines_odither:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/samsung/yh820/lcd-as-yh820.S b/firmware/target/arm/samsung/yh820/lcd-as-yh820.S
index 581a4f3fc3..d8dfccc37b 100644
--- a/firmware/target/arm/samsung/yh820/lcd-as-yh820.S
+++ b/firmware/target/arm/samsung/yh820/lcd-as-yh820.S
@@ -238,7 +238,7 @@ lcd_write_yuv420_lines:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r10, pc }     @ restore registers and return
+    ldmpc       regs=r4-r10             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -545,6 +545,6 @@ lcd_write_yuv420_lines_odither:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/samsung/yh920/lcd-as-yh920.S b/firmware/target/arm/samsung/yh920/lcd-as-yh920.S
index 2c7f5110bf..130addf460 100644
--- a/firmware/target/arm/samsung/yh920/lcd-as-yh920.S
+++ b/firmware/target/arm/samsung/yh920/lcd-as-yh920.S
@@ -113,6 +113,6 @@ lcd_grey_data:
     subs    r2, r2, #1
     bne     .greyloop
 
-    ldmfd   sp!, {r4-r5, pc}
+    ldmpc   regs=r4-r5
     .size   lcd_grey_data,.-lcd_grey_data
 
diff --git a/firmware/target/arm/samsung/yh925/lcd-as-yh925.S b/firmware/target/arm/samsung/yh925/lcd-as-yh925.S
index b3f12e46b6..8e851d8441 100644
--- a/firmware/target/arm/samsung/yh925/lcd-as-yh925.S
+++ b/firmware/target/arm/samsung/yh925/lcd-as-yh925.S
@@ -232,7 +232,7 @@ lcd_write_yuv420_lines:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -533,6 +533,6 @@ lcd_write_yuv420_lines_odither:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/sandisk/sansa-c200/lcd-as-c200.S b/firmware/target/arm/sandisk/sansa-c200/lcd-as-c200.S
index 581a4f3fc3..d8dfccc37b 100644
--- a/firmware/target/arm/sandisk/sansa-c200/lcd-as-c200.S
+++ b/firmware/target/arm/sandisk/sansa-c200/lcd-as-c200.S
@@ -238,7 +238,7 @@ lcd_write_yuv420_lines:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r10, pc }     @ restore registers and return
+    ldmpc       regs=r4-r10             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
 
@@ -545,6 +545,6 @@ lcd_write_yuv420_lines_odither:
     subs        r1, r1, #2              @ subtract block from width
     bgt         10b @ loop line         @
                                         @
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
+    ldmpc       regs=r4-r11             @ restore registers and return
     .ltorg                              @ dump constant pool
     .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/thread-arm.c b/firmware/target/arm/thread-arm.c
index c2d91cec25..9ea3d0bef9 100644
--- a/firmware/target/arm/thread-arm.c
+++ b/firmware/target/arm/thread-arm.c
@@ -77,7 +77,15 @@ static inline void load_context(const void* addr)
     asm volatile(
         "ldr     r0, [%0, #40]          \n" /* Load start pointer */
         "cmp     r0, #0                 \n" /* Check for NULL */
-        "ldmneia %0, { r0, pc }         \n" /* If not already running, jump to start */ 
+
+        /* If not already running, jump to start */
+#if ARM_ARCH == 4 && defined(USE_THUMB)
+        "ldmneia %0, { r0, r12 }        \n"
+        "bxne    r12                    \n"
+#else
+        "ldmneia %0, { r0, pc }         \n"
+#endif
+
         "ldmia   %0, { r4-r11, sp, lr } \n" /* Load regs r4 to r14 from context */
         : : "r" (addr) : "r0" /* only! */
     );