parisc: Add ALTERNATIVE_CODE() and ALT_COND_RUN_ON_QEMU

The macro ALTERNATIVE_CODE() allows assembly code to patch in a series
of new assembler statements given at a specific start address.
The ALT_COND_RUN_ON_QEMU condition is true if the kernel is started in a
qemu emulation.

Signed-off-by: Helge Deller <deller@gmx.de>

2 files changed, 27 insertions, 7 deletions

diff --git a/arch/parisc/include/asm/alternative.h b/arch/parisc/include/asm/alternative.h
index 793d8baa3a10..0ec54f43d6d2 100644
--- a/arch/parisc/include/asm/alternative.h
+++ b/arch/parisc/include/asm/alternative.h
@@ -8,6 +8,7 @@
 #define ALT_COND_NO_ICACHE      0x04    /* if system has no i-cache  */
 #define ALT_COND_NO_SPLIT_TLB   0x08    /* if split_tlb == 0  */
 #define ALT_COND_NO_IOC_FDC     0x10    /* if I/O cache does not need flushes */
+#define ALT_COND_RUN_ON_QEMU    0x20    /* if running on QEMU */
 
 #define INSN_PxTLB      0x02            /* modify pdtlb, pitlb */
 #define INSN_NOP        0x08000240      /* nop */
@@ -21,7 +22,7 @@
 
 struct alt_instr {
         s32 orig_offset;        /* offset to original instructions */
-        u32 len;                /* end of original instructions */
+        s32 len;                /* end of original instructions */
         u32 cond;               /* see ALT_COND_XXX */
         u32 replacement;        /* replacement instruction or code */
 };
@@ -40,12 +41,20 @@ void apply_alternatives(struct alt_instr *start, struct alt_instr *end,
 
 #else
 
+/* to replace one single instructions by a new instruction */
 #define ALTERNATIVE(from, to, cond, replacement)\
         .section .altinstructions, "aw" !       \
         .word (from - .), (to - from)/4 !       \
         .word cond, replacement         !       \
         .previous
 
+/* to replace multiple instructions by new code */
+#define ALTERNATIVE_CODE(from, num_instructions, cond, new_instr_ptr)\
+        .section .altinstructions, "aw" !       \
+        .word (from - .), -num_instructions !   \
+        .word cond, (new_instr_ptr - .) !       \
+        .previous
+
 #endif  /*  __ASSEMBLY__  */
 
 #endif /* __ASM_PARISC_ALTERNATIVE_H */
diff --git a/arch/parisc/kernel/alternative.c b/arch/parisc/kernel/alternative.c
index ca1f5ca0540a..3c66d5c4d90d 100644
--- a/arch/parisc/kernel/alternative.c
+++ b/arch/parisc/kernel/alternative.c
@@ -28,7 +28,8 @@ void __init_or_module apply_alternatives(struct alt_instr *start,
 
         for (entry = start; entry < end; entry++, index++) {
 
-                u32 *from, len, cond, replacement;
+                u32 *from, cond, replacement;
+                s32 len;
 
                 from = (u32 *)((ulong)&entry->orig_offset + entry->orig_offset);
                 len = entry->len;
@@ -49,6 +50,8 @@ void __init_or_module apply_alternatives(struct alt_instr *start,
                         continue;
                 if ((cond & ALT_COND_NO_ICACHE) && (cache_info.ic_size != 0))
                         continue;
+                if ((cond & ALT_COND_RUN_ON_QEMU) && !running_on_qemu)
+                        continue;
 
                 /*
                  * If the PDC_MODEL capabilities has Non-coherent IO-PDIR bit
@@ -74,11 +77,19 @@ void __init_or_module apply_alternatives(struct alt_instr *start,
                 if (replacement == INSN_NOP && len > 1)
                         replacement = 0xe8000002 + (len-2)*8; /* "b,n .+8" */
 
-                pr_debug("Do    %d: Cond 0x%x, Replace %02d instructions @ 0x%px with 0x%08x\n",
-                        index, cond, len, from, replacement);
-
-                /* Replace instruction */
-                *from = replacement;
+                pr_debug("ALTERNATIVE %3d: Cond %2x, Replace %2d instructions to 0x%08x @ 0x%px (%pS)\n",
+                        index, cond, len, replacement, from, from);
+
+                if (len < 0) {
+                        /* Replace multiple instruction by new code */
+                        u32 *source;
+                        len = -len;
+                        source = (u32 *)((ulong)&entry->replacement + entry->replacement);
+                        memcpy(from, source, 4 * len);
+                } else {
+                        /* Replace by one instruction */
+                        *from = replacement;
+                }
                 applied++;
         }