3 files changed, 135 insertions, 0 deletions

diff --git a/arch/xtensa/Kconfig.debug b/arch/xtensa/Kconfig.debug
index be5fb4c6243f..a34010e0e51c 100644
--- a/arch/xtensa/Kconfig.debug
+++ b/arch/xtensa/Kconfig.debug
@@ -13,4 +13,15 @@ config LD_NO_RELAX
           Enabling this option improves the link time but increases the
           code size, and possibly execution time.
 
+config S32C1I_SELFTEST
+        bool "Perform S32C1I instruction self-test at boot"
+        default y
+        help
+          Enable this option to test S32C1I instruction behavior at boot.
+          Correct operation of this instruction requires some cooperation from hardware
+          external to the processor (such as bus bridge, bus fabric, or memory controller).
+          It is easy to make wrong hardware configuration, this test should catch it early.
+
+          Say 'N' on stable hardware.
+
 endmenu
diff --git a/arch/xtensa/include/asm/regs.h b/arch/xtensa/include/asm/regs.h
index 8a8aa61ccc8d..6aaf6d6a5fc4 100644
--- a/arch/xtensa/include/asm/regs.h
+++ b/arch/xtensa/include/asm/regs.h
@@ -52,6 +52,10 @@
 #define EXCCAUSE_SPECULATION                    7
 #define EXCCAUSE_PRIVILEGED                     8
 #define EXCCAUSE_UNALIGNED                      9
+#define EXCCAUSE_INSTR_DATA_ERROR               12
+#define EXCCAUSE_LOAD_STORE_DATA_ERROR          13
+#define EXCCAUSE_INSTR_ADDR_ERROR               14
+#define EXCCAUSE_LOAD_STORE_ADDR_ERROR          15
 #define EXCCAUSE_ITLB_MISS                      16
 #define EXCCAUSE_ITLB_MULTIHIT                  17
 #define EXCCAUSE_ITLB_PRIVILEGE                 18
diff --git a/arch/xtensa/kernel/setup.c b/arch/xtensa/kernel/setup.c
index e53a94b3edbc..45217617c60d 100644
--- a/arch/xtensa/kernel/setup.c
+++ b/arch/xtensa/kernel/setup.c
@@ -42,6 +42,7 @@
 #include <asm/page.h>
 #include <asm/setup.h>
 #include <asm/param.h>
+#include <asm/traps.h>
 
 #include <platform/hardware.h>
 
@@ -235,6 +236,123 @@ extern char _UserExceptionVector_text_end;
 extern char _DoubleExceptionVector_literal_start;
 extern char _DoubleExceptionVector_text_end;
 
+
+#ifdef CONFIG_S32C1I_SELFTEST
+#if XCHAL_HAVE_S32C1I
+
+static int __initdata rcw_word, rcw_probe_pc, rcw_exc;
+
+/*
+ * Basic atomic compare-and-swap, that records PC of S32C1I for probing.
+ *
+ * If *v == cmp, set *v = set.  Return previous *v.
+ */
+static inline int probed_compare_swap(int *v, int cmp, int set)
+{
+        int tmp;
+
+        __asm__ __volatile__(
+                        "       movi    %1, 1f\n"
+                        "       s32i    %1, %4, 0\n"
+                        "       wsr     %2, scompare1\n"
+                        "1:     s32c1i  %0, %3, 0\n"
+                        : "=a" (set), "=&a" (tmp)
+                        : "a" (cmp), "a" (v), "a" (&rcw_probe_pc), "0" (set)
+                        : "memory"
+                        );
+        return set;
+}
+
+/* Handle probed exception */
+
+void __init do_probed_exception(struct pt_regs *regs, unsigned long exccause)
+{
+        if (regs->pc == rcw_probe_pc) { /* exception on s32c1i ? */
+                regs->pc += 3;          /* skip the s32c1i instruction */
+                rcw_exc = exccause;
+        } else {
+                do_unhandled(regs, exccause);
+        }
+}
+
+/* Simple test of S32C1I (soc bringup assist) */
+
+void __init check_s32c1i(void)
+{
+        int n, cause1, cause2;
+        void *handbus, *handdata, *handaddr; /* temporarily saved handlers */
+
+        rcw_probe_pc = 0;
+        handbus  = trap_set_handler(EXCCAUSE_LOAD_STORE_ERROR,
+                        do_probed_exception);
+        handdata = trap_set_handler(EXCCAUSE_LOAD_STORE_DATA_ERROR,
+                        do_probed_exception);
+        handaddr = trap_set_handler(EXCCAUSE_LOAD_STORE_ADDR_ERROR,
+                        do_probed_exception);
+
+        /* First try an S32C1I that does not store: */
+        rcw_exc = 0;
+        rcw_word = 1;
+        n = probed_compare_swap(&rcw_word, 0, 2);
+        cause1 = rcw_exc;
+
+        /* took exception? */
+        if (cause1 != 0) {
+                /* unclean exception? */
+                if (n != 2 || rcw_word != 1)
+                        panic("S32C1I exception error");
+        } else if (rcw_word != 1 || n != 1) {
+                panic("S32C1I compare error");
+        }
+
+        /* Then an S32C1I that stores: */
+        rcw_exc = 0;
+        rcw_word = 0x1234567;
+        n = probed_compare_swap(&rcw_word, 0x1234567, 0xabcde);
+        cause2 = rcw_exc;
+
+        if (cause2 != 0) {
+                /* unclean exception? */
+                if (n != 0xabcde || rcw_word != 0x1234567)
+                        panic("S32C1I exception error (b)");
+        } else if (rcw_word != 0xabcde || n != 0x1234567) {
+                panic("S32C1I store error");
+        }
+
+        /* Verify consistency of exceptions: */
+        if (cause1 || cause2) {
+                pr_warn("S32C1I took exception %d, %d\n", cause1, cause2);
+                /* If emulation of S32C1I upon bus error gets implemented,
+                   we can get rid of this panic for single core (not SMP) */
+                panic("S32C1I exceptions not currently supported");
+        }
+        if (cause1 != cause2)
+                panic("inconsistent S32C1I exceptions");
+
+        trap_set_handler(EXCCAUSE_LOAD_STORE_ERROR, handbus);
+        trap_set_handler(EXCCAUSE_LOAD_STORE_DATA_ERROR, handdata);
+        trap_set_handler(EXCCAUSE_LOAD_STORE_ADDR_ERROR, handaddr);
+}
+
+#else /* XCHAL_HAVE_S32C1I */
+
+/* This condition should not occur with a commercially deployed processor.
+   Display reminder for early engr test or demo chips / FPGA bitstreams */
+void __init check_s32c1i(void)
+{
+        pr_warn("Processor configuration lacks atomic compare-and-swap support!\n");
+}
+
+#endif /* XCHAL_HAVE_S32C1I */
+#else /* CONFIG_S32C1I_SELFTEST */
+
+void __init check_s32c1i(void)
+{
+}
+
+#endif /* CONFIG_S32C1I_SELFTEST */
+
+
 void __init setup_arch(char **cmdline_p)
 {
         extern int mem_reserve(unsigned long, unsigned long, int);
@@ -244,6 +362,8 @@ void __init setup_arch(char **cmdline_p)
         boot_command_line[COMMAND_LINE_SIZE-1] = '\0';
         *cmdline_p = command_line;
 
+        check_s32c1i();
+
         /* Reserve some memory regions */
 
 #ifdef CONFIG_BLK_DEV_INITRD