5 files changed, 154 insertions, 0 deletions

diff --git a/arch/s390/kernel/Makefile b/arch/s390/kernel/Makefile
index 7b742ffac7d3..addf61024153 100644
--- a/arch/s390/kernel/Makefile
+++ b/arch/s390/kernel/Makefile
@@ -76,6 +76,7 @@ obj-$(CONFIG_KPROBES)		+= kprobes.o
 obj-$(CONFIG_FUNCTION_TRACER)	+= mcount.o ftrace.o
 obj-$(CONFIG_CRASH_DUMP)	+= crash_dump.o
 obj-$(CONFIG_UPROBES)		+= uprobes.o
+obj-$(CONFIG_ALTERNATIVES)	+= alternative.o
 
 obj-$(CONFIG_PERF_EVENTS)	+= perf_event.o perf_cpum_cf.o perf_cpum_sf.o
 obj-$(CONFIG_PERF_EVENTS)	+= perf_cpum_cf_events.o
diff --git a/arch/s390/kernel/alternative.c b/arch/s390/kernel/alternative.c
new file mode 100644
index 000000000000..315986a06cf5
--- /dev/null
+++ b/arch/s390/kernel/alternative.c
@@ -0,0 +1,110 @@
+#include <linux/module.h>
+#include <asm/alternative.h>
+#include <asm/facility.h>
+
+#define MAX_PATCH_LEN (255 - 1)
+
+static int __initdata_or_module alt_instr_disabled;
+
+static int __init disable_alternative_instructions(char *str)
+{
+	alt_instr_disabled = 1;
+	return 0;
+}
+
+early_param("noaltinstr", disable_alternative_instructions);
+
+struct brcl_insn {
+	u16 opc;
+	s32 disp;
+} __packed;
+
+static u16 __initdata_or_module nop16 = 0x0700;
+static u32 __initdata_or_module nop32 = 0x47000000;
+static struct brcl_insn __initdata_or_module nop48 = {
+	0xc004, 0
+};
+
+static const void *nops[] __initdata_or_module = {
+	&nop16,
+	&nop32,
+	&nop48
+};
+
+static void __init_or_module add_jump_padding(void *insns, unsigned int len)
+{
+	struct brcl_insn brcl = {
+		0xc0f4,
+		len / 2
+	};
+
+	memcpy(insns, &brcl, sizeof(brcl));
+	insns += sizeof(brcl);
+	len -= sizeof(brcl);
+
+	while (len > 0) {
+		memcpy(insns, &nop16, 2);
+		insns += 2;
+		len -= 2;
+	}
+}
+
+static void __init_or_module add_padding(void *insns, unsigned int len)
+{
+	if (len > 6)
+		add_jump_padding(insns, len);
+	else if (len >= 2)
+		memcpy(insns, nops[len / 2 - 1], len);
+}
+
+static void __init_or_module __apply_alternatives(struct alt_instr *start,
+						  struct alt_instr *end)
+{
+	struct alt_instr *a;
+	u8 *instr, *replacement;
+	u8 insnbuf[MAX_PATCH_LEN];
+
+	/*
+	 * The scan order should be from start to end. A later scanned
+	 * alternative code can overwrite previously scanned alternative code.
+	 */
+	for (a = start; a < end; a++) {
+		int insnbuf_sz = 0;
+
+		instr = (u8 *)&a->instr_offset + a->instr_offset;
+		replacement = (u8 *)&a->repl_offset + a->repl_offset;
+
+		if (!test_facility(a->facility))
+			continue;
+
+		if (unlikely(a->instrlen % 2 || a->replacementlen % 2)) {
+			WARN_ONCE(1, "cpu alternatives instructions length is "
+				     "odd, skipping patching\n");
+			continue;
+		}
+
+		memcpy(insnbuf, replacement, a->replacementlen);
+		insnbuf_sz = a->replacementlen;
+
+		if (a->instrlen > a->replacementlen) {
+			add_padding(insnbuf + a->replacementlen,
+				    a->instrlen - a->replacementlen);
+			insnbuf_sz += a->instrlen - a->replacementlen;
+		}
+
+		s390_kernel_write(instr, insnbuf, insnbuf_sz);
+	}
+}
+
+void __init_or_module apply_alternatives(struct alt_instr *start,
+					 struct alt_instr *end)
+{
+	if (!alt_instr_disabled)
+		__apply_alternatives(start, end);
+}
+
+extern struct alt_instr __alt_instructions[], __alt_instructions_end[];
+void __init apply_alternative_instructions(void)
+{
+	apply_alternatives(__alt_instructions, __alt_instructions_end);
+}
diff --git a/arch/s390/kernel/module.c b/arch/s390/kernel/module.c
index 1a27f307a920..6d9f73bb4142 100644
--- a/arch/s390/kernel/module.c
+++ b/arch/s390/kernel/module.c
@@ -31,6 +31,7 @@
 #include <linux/kernel.h>
 #include <linux/moduleloader.h>
 #include <linux/bug.h>
+#include <asm/alternative.h>
 
 #if 0
 #define DEBUGP printk
@@ -429,6 +430,22 @@ int module_finalize(const Elf_Ehdr *hdr,
 		    const Elf_Shdr *sechdrs,
 		    struct module *me)
 {
+	const Elf_Shdr *s;
+	char *secstrings;
+
+	if (IS_ENABLED(CONFIG_ALTERNATIVES)) {
+		secstrings = (void *)hdr + sechdrs[hdr->e_shstrndx].sh_offset;
+		for (s = sechdrs; s < sechdrs + hdr->e_shnum; s++) {
+			if (!strcmp(".altinstructions",
+				    secstrings + s->sh_name)) {
+				/* patch .altinstructions */
+				void *aseg = (void *)s->sh_addr;
+
+				apply_alternatives(aseg, aseg + s->sh_size);
+			}
+		}
+	}
+
 	jump_label_apply_nops(me);
 	return 0;
 }
diff --git a/arch/s390/kernel/setup.c b/arch/s390/kernel/setup.c
index b2c9af9b88d5..c07e6d6a91cc 100644
--- a/arch/s390/kernel/setup.c
+++ b/arch/s390/kernel/setup.c
@@ -66,6 +66,7 @@
 #include <asm/sclp.h>
 #include <asm/sysinfo.h>
 #include <asm/numa.h>
+#include <asm/alternative.h>
 #include "entry.h"
 
 /*
@@ -957,6 +958,8 @@ void __init setup_arch(char **cmdline_p)
 	conmode_default();
 	set_preferred_console();
 
+	apply_alternative_instructions();
+
 	/* Setup zfcpdump support */
 	setup_zfcpdump();
 
diff --git a/arch/s390/kernel/vmlinux.lds.S b/arch/s390/kernel/vmlinux.lds.S
index 6e2c42bd1c3b..7c9fcf7cb43d 100644
--- a/arch/s390/kernel/vmlinux.lds.S
+++ b/arch/s390/kernel/vmlinux.lds.S
@@ -104,6 +104,29 @@ SECTIONS
 		EXIT_DATA
 	}
 
+	/*
+	 * struct alt_inst entries. From the header (alternative.h):
+	 * "Alternative instructions for different CPU types or capabilities"
+	 * Think locking instructions on spinlocks.
+	 * Note, that it is a part of __init region.
+	 */
+	. = ALIGN(8);
+	.altinstructions : {
+		__alt_instructions = .;
+		*(.altinstructions)
+		__alt_instructions_end = .;
+	}
+
+	/*
+	 * And here are the replacement instructions. The linker sticks
+	 * them as binary blobs. The .altinstructions has enough data to
+	 * get the address and the length of them to patch the kernel safely.
+	 * Note, that it is a part of __init region.
+	 */
+	.altinstr_replacement : {
+		*(.altinstr_replacement)
+	}
+
 	/* early.c uses stsi, which requires page aligned data. */
 	. = ALIGN(PAGE_SIZE);
 	INIT_DATA_SECTION(0x100)