arm: add connecting exceptions at runtime

Add sys_exc_connect() (and its x86-compatible alias nanoCpuExcConnect())
that allows connecting an exception handler at runtime.

The current implementation is a bit of a bastard, to avoid disturbing
the current implementation of the exception handlers. Instead of hooking
_exc_wrapper() in all vectors and adapting the exception handlers, the
current exception handlers are still hooked directly in the vectors.
When an exception is hooked at runtime, _exc_wrapper() gets installed in
the vector and the real handler gets inserted in _sw_exc_table; this
means that the scheme only works with non-XIP kernels.

This should be enhanced so that _exc_wrapper() is hooked in all vectors,
and that current exception handlers (for faults mostly) are reworked to
be inserted in the _sw_exc_table and wrapped in _exc_wrapper().

Change-Id: Icaa14f4835b57873d2905b7fbcbb94eeb3b247d1
Signed-off-by: Benjamin Walsh <benjamin.walsh@windriver.com>

arch/arm/core/cortex_m/Makefile

@@ -5,6 +5,6 @@ asflags-y = $(ccflags-y)
 
 obj-y = vector_table.o reset.o \
 	prep_c.o scs.o scb.o nmi.o \
-	sw_isr_table.o
+	sw_isr_table.o exc_manage.o exc_wrapper.o
 
 obj-$(CONFIG_ERRNO) += errno.o

arch/arm/core/cortex_m/exc_manage.c

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2015 Wind River Systems, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file dynamic exception management
+ */
+
+/* can only be used with non-XIP kernels, since they don't have their vector
+ * table in the FLASH
+ */
+#if !defined(CONFIG_XIP)
+
+#include <nanokernel.h>
+#include <arch/cpu.h>
+#include <misc/__assert.h>
+#include <toolchain.h>
+#include <sections.h>
+#include "vector_table.h"
+
+static inline int exc_can_be_connected(int num)
+{
+	static const uint16_t connectable_exceptions = (
+		(1 << _EXC_MPU_FAULT) |
+		(1 << _EXC_BUS_FAULT) |
+		(1 << _EXC_USAGE_FAULT) |
+		(1 << _EXC_DEBUG) |
+		0
+	);
+
+	return !!(connectable_exceptions & (1 << num));
+}
+
+/*
+ * Can be initialized with garbage, it doesn't matter until the wrapper is
+ * inserted in the vector table, at which point the relevant entry will contain
+ * the pointer to the handler.
+ */
+sys_exc_handler_t *_sw_exc_table[_NUM_EXC] __noinit;
+
+extern void _exc_wrapper(void);
+void sys_exc_connect(unsigned int num, sys_exc_handler_t *handler, void *unused)
+{
+	__ASSERT(exc_can_be_connected(num), "not a connectable exception");
+
+	_sw_exc_table[num] = handler;
+
+	/*
+	 * The compiler sets thumb bit (bit0) of the value of the _vector_table
+	 * symbol, probably because it is in the .text section: to get the correct
+	 * offset in the table, mask bit0.
+	 */
+	((void **)(((uint32_t)_vector_table) & 0xfffffffe))[num] = _exc_wrapper;
+}
+
+FUNC_ALIAS(sys_exc_connect, nanoCpuExcConnect, void);
+
+#include <misc/printk.h>
+void sys_exc_esf_dump(NANO_ESF *esf)
+{
+	printk("r0/a1:  %x  ", esf->a1);
+	printk("r1/a2:  %x  ", esf->a2);
+	printk("r2/a3:  %x\n", esf->a3);
+	printk("r3/a4:  %x  ", esf->a4);
+	printk("r12/ip: %x  ", esf->ip);
+	printk("r14/lr: %x\n", esf->lr);
+	printk("r15/pc: %x  ", esf->pc);
+	printk("xpsr:   %x\n", esf->xpsr);
+}
+
+#endif /* CONFIG_XIP */

arch/arm/core/cortex_m/exc_wrapper.S

@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2015 Wind River Systems, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file dynamic exception handler wrapper
+ */
+
+#define _ASMLANGUAGE
+
+#include <toolchain.h>
+#include <arch/cpu.h>
+
+_ASM_FILE_PROLOGUE
+
+GDATA(_sw_exc_table)
+GTEXT(_exc_wrapper)
+
+SECTION_FUNC(TEXT, _exc_wrapper)
+
+	_GDB_STUB_EXC_ENTRY
+
+	ldr ip, =_SCS_ICSR
+	ldr ip, [ip]
+	ands.w ip, #_SCS_ICSR_RETTOBASE
+
+	itte eq			/* is the RETTOBASE bit zero ? */
+	mrseq r0, MSP	/* if so, we're not returning to thread mode, thus this
+					 * is a nested exception: the stack frame is on the MSP */
+	addeq.w r0, #4	/* ESF returned here is off by 4 because of the push {r2}
+					 * in_GDB_STUB_EXC_ENTRY */
+	mrsne r0, PSP	/* if not, we are returning to thread mode, thus this is
+					 * not a nested exception: the stack frame is on the PSP */
+
+	/* r0 now contains the pointer to the full ESF */
+
+	push {lr}	/* lr is now the first item on the stack */
+
+	ldr r1, =_sw_exc_table
+
+	mrs r2, IPSR	/* get exception number */
+
+	/* find and call handler: table is 4-bytes wide, shift index by 2 */
+	ldr r1, [r1, r2, LSL #2]
+	blx r1
+
+	pop {lr}
+
+	/* exception return is done in _ExcExit(), including _GDB_STUB_EXC_EXIT */
+	b _ExcExit
+
+	.end

include/arch/arm/cortex_m/exc.h

@@ -49,8 +49,60 @@ typedef struct __esf NANO_ESF;
 extern const NANO_ESF _default_esf;
 
 extern void _ExcExit(void);
+
+#if !defined(CONFIG_XIP)
+
+/* currently, exception connecting is only available to non-XIP kernels */
+
+/**
+ * @brief signature for an exception handler
+ */
+
+#define sys_exc_handler_sig(x) void (x)(NANO_ESF *esf)
+
+/**
+ * @brief exception handler data type
+ */
+
+typedef sys_exc_handler_sig(sys_exc_handler_t);
+
+/**
+ * @brief connect a handler to an exception vector
+ *
+ * Connect the @a handler to the exception vector @a num.
+ *
+ * The @a unused parameter is only there to match the x86 signature.
+ *
+ * @param num Exception vector number
+ * @param handler Exception handler to connect
+ * @param unused Unused
+ *
+ * @return N/A
+ */
+
+extern void sys_exc_connect(unsigned int num, sys_exc_handler_t *handler,
+							void *unused);
+
+/**
+ * @brief alias of sys_exc_connect
+ *
+ * See sys_exc_connect().
+ */
+
+extern void nanoCpuExcConnect(unsigned int, sys_exc_handler_t *, void *);
+
+/**
+ * @brief display the contents of a exception stack frame
+ *
+ * @return N/A
+ */
+
+extern void sys_exc_esf_dump(NANO_ESF *esf);
+
 #endif
 
+#endif /* _ASMLANGUAGE */
+
 #ifdef __cplusplus
 }
 #endif