[PATCH] parisc: Add ALTERNATIVE_CODE() and ALT_COND_RUN_ON_QEMU

[Helge Deller <deller@gmx.de>]

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>

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++;
 	}