[PATCH v2 2/4] s390/kernel: expand the exception table logic to allow new handling options

[Ilya Leoshkevich <iii@linux.ibm.com>]

This is a s390 port of commit 548acf19234d ("x86/mm: Expand the
exception table logic to allow new handling options"), which is needed
for implementing BPF_PROBE_MEM on s390.

The new handler field is made 64-bit in order to allow pointing from
dynamically allocated entries to handlers in kernel text. Unlike on x86,
NULL is used instead of ex_handler_default. This is because exception
tables are used by boot/text_dma.S, and it would be a pain to preserve
ex_handler_default.

The new infrastructure is ignored in early_pgm_check_handler, since
there is no pt_regs.

Signed-off-by: Ilya Leoshkevich <iii@linux.ibm.com>
Reviewed-by: Heiko Carstens <heiko.carstens@de.ibm.com>
---
 arch/s390/include/asm/extable.h | 50 +++++++++++++++++++++++++++++----
 arch/s390/include/asm/linkage.h |  3 +-
 arch/s390/kernel/kprobes.c      |  4 +--
 arch/s390/kernel/traps.c        |  7 ++---
 arch/s390/mm/fault.c            |  4 +--
 scripts/sorttable.c             | 25 +++++++++++++++++
 6 files changed, 76 insertions(+), 17 deletions(-)

diff --git a/arch/s390/include/asm/extable.h b/arch/s390/include/asm/extable.h
index ae27f756b409..8f0c60f2fe3a 100644
--- a/arch/s390/include/asm/extable.h
+++ b/arch/s390/include/asm/extable.h
@@ -1,12 +1,20 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 #ifndef __S390_EXTABLE_H
 #define __S390_EXTABLE_H
+
+#include <asm/ptrace.h>
+#include <linux/compiler.h>
+
 /*
- * The exception table consists of pairs of addresses: the first is the
- * address of an instruction that is allowed to fault, and the second is
- * the address at which the program should continue.  No registers are
- * modified, so it is entirely up to the continuation code to figure out
- * what to do.
+ * The exception table consists of three addresses:
+ *
+ * - Address of an instruction that is allowed to fault.
+ * - Address at which the program should continue.
+ * - Optional address of handler that takes pt_regs * argument and runs in
+ *   interrupt context.
+ *
+ * No registers are modified, so it is entirely up to the continuation code
+ * to figure out what to do.
  *
  * All the routines below use bits of fixup code that are out of line
  * with the main instruction path.  This means when everything is well,
@@ -17,6 +25,7 @@
 struct exception_table_entry
 {
 	int insn, fixup;
+	long handler;
 };
 
 extern struct exception_table_entry *__start_dma_ex_table;
@@ -29,6 +38,37 @@ static inline unsigned long extable_fixup(const struct exception_table_entry *x)
 	return (unsigned long)&x->fixup + x->fixup;
 }
 
+typedef bool (*ex_handler_t)(const struct exception_table_entry *,
+			     struct pt_regs *);
+
+static inline ex_handler_t
+ex_fixup_handler(const struct exception_table_entry *x)
+{
+	return (ex_handler_t)((unsigned long)&x->handler + x->handler);
+}
+
+static inline bool ex_handle(const struct exception_table_entry *x,
+			     struct pt_regs *regs)
+{
+	ex_handler_t handler = ex_fixup_handler(x);
+
+	if (unlikely(handler))
+		return handler(x, regs);
+	regs->psw.addr = extable_fixup(x);
+	return true;
+}
+
 #define ARCH_HAS_RELATIVE_EXTABLE
 
+static inline void swap_ex_entry_fixup(struct exception_table_entry *a,
+				       struct exception_table_entry *b,
+				       struct exception_table_entry tmp,
+				       int delta)
+{
+	a->fixup = b->fixup + delta;
+	b->fixup = tmp.fixup - delta;
+	a->handler = b->handler + delta;
+	b->handler = tmp.handler - delta;
+}
+
 #endif
diff --git a/arch/s390/include/asm/linkage.h b/arch/s390/include/asm/linkage.h
index 1b52c07b5642..83ff5a35d522 100644
--- a/arch/s390/include/asm/linkage.h
+++ b/arch/s390/include/asm/linkage.h
@@ -14,9 +14,10 @@
 
 #define __EX_TABLE(_section, _fault, _target)				\
 	stringify_in_c(.section	_section,"a";)				\
-	stringify_in_c(.align	4;)					\
+	stringify_in_c(.align	8;)					\
 	stringify_in_c(.long	(_fault) - .;)				\
 	stringify_in_c(.long	(_target) - .;)				\
+	stringify_in_c(.quad	0 - .;)					\
 	stringify_in_c(.previous)
 
 #define EX_TABLE(_fault, _target)					\
diff --git a/arch/s390/kernel/kprobes.c b/arch/s390/kernel/kprobes.c
index 548d0ea9808d..d2a71d872638 100644
--- a/arch/s390/kernel/kprobes.c
+++ b/arch/s390/kernel/kprobes.c
@@ -523,10 +523,8 @@ static int kprobe_trap_handler(struct pt_regs *regs, int trapnr)
 		 * zero, try to fix up.
 		 */
 		entry = s390_search_extables(regs->psw.addr);
-		if (entry) {
-			regs->psw.addr = extable_fixup(entry);
+		if (entry && ex_handle(entry, regs))
 			return 1;
-		}
 
 		/*
 		 * fixup_exception() could not handle it,
diff --git a/arch/s390/kernel/traps.c b/arch/s390/kernel/traps.c
index ff9cc4c3290e..8d1e8a1a97df 100644
--- a/arch/s390/kernel/traps.c
+++ b/arch/s390/kernel/traps.c
@@ -50,11 +50,8 @@ void do_report_trap(struct pt_regs *regs, int si_signo, int si_code, char *str)
         } else {
                 const struct exception_table_entry *fixup;
 		fixup = s390_search_extables(regs->psw.addr);
-                if (fixup)
-			regs->psw.addr = extable_fixup(fixup);
-		else {
+		if (!fixup || !ex_handle(fixup, regs))
 			die(regs, str);
-		}
         }
 }
 
@@ -251,7 +248,7 @@ void monitor_event_exception(struct pt_regs *regs)
 	case BUG_TRAP_TYPE_NONE:
 		fixup = s390_search_extables(regs->psw.addr);
 		if (fixup)
-			regs->psw.addr = extable_fixup(fixup);
+			ex_handle(fixup, regs);
 		break;
 	case BUG_TRAP_TYPE_WARN:
 		break;
diff --git a/arch/s390/mm/fault.c b/arch/s390/mm/fault.c
index d53c2e2ea1fd..c4c6cafef3f4 100644
--- a/arch/s390/mm/fault.c
+++ b/arch/s390/mm/fault.c
@@ -255,10 +255,8 @@ static noinline void do_no_context(struct pt_regs *regs)
 
 	/* Are we prepared to handle this kernel fault?  */
 	fixup = s390_search_extables(regs->psw.addr);
-	if (fixup) {
-		regs->psw.addr = extable_fixup(fixup);
+	if (fixup && ex_handle(fixup, regs))
 		return;
-	}
 
 	/*
 	 * Oops. The kernel tried to access some bad page. We'll have to
diff --git a/scripts/sorttable.c b/scripts/sorttable.c
index ec6b5e81eba1..a4b8415ba71c 100644
--- a/scripts/sorttable.c
+++ b/scripts/sorttable.c
@@ -255,6 +255,29 @@ static void x86_sort_relative_table(char *extab_image, int image_size)
 	}
 }
 
+static void s390_sort_relative_table(char *extab_image, int image_size)
+{
+	int i;
+
+	for (i = 0; i < image_size; i += 16) {
+		char *loc = extab_image + i;
+
+		w(r((uint32_t *)loc) + i, (uint32_t *)loc);
+		w(r((uint32_t *)(loc + 4)) + (i + 4), (uint32_t *)(loc + 4));
+		w8(r8((uint64_t *)(loc + 8)) + (i + 8), (uint64_t *)(loc + 8));
+	}
+
+	qsort(extab_image, image_size / 16, 16, compare_relative_table);
+
+	for (i = 0; i < image_size; i += 16) {
+		char *loc = extab_image + i;
+
+		w(r((uint32_t *)loc) - i, (uint32_t *)loc);
+		w(r((uint32_t *)(loc + 4)) - (i + 4), (uint32_t *)(loc + 4));
+		w8(r8((uint64_t *)(loc + 8)) - (i + 8), (uint64_t *)(loc + 8));
+	}
+}
+
 static int do_file(char const *const fname, void *addr)
 {
 	int rc = -1;
@@ -297,6 +320,8 @@ static int do_file(char const *const fname, void *addr)
 		custom_sort = x86_sort_relative_table;
 		break;
 	case EM_S390:
+		custom_sort = s390_sort_relative_table;
+		break;
 	case EM_AARCH64:
 	case EM_PARISC:
 	case EM_PPC:
-- 
2.25.4