trace.h: add macros to unwind and print the call stack (kernel only)

Adds [EIDF]PRINT_STACK() for debugging purposes. Depends on
CFG_CORE_UNWIND=y.
As a side-effect, also adds a few things that may be useful on their
own: __always_inline, read_pc(), read_fp(), read_lr().

Signed-off-by: Jerome Forissier <jerome.forissier@linaro.org>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>

core/arch/arm/include/arm32.h

@@ -29,6 +29,7 @@
 #ifndef ARM32_H
 #define ARM32_H
 
+#include <sys/cdefs.h>
 #include <stdint.h>
 #include <util.h>
 
@@ -533,6 +534,30 @@ static inline uint32_t read_cntfrq(void)
 	asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (frq));
 	return frq;
 }
+
+static __always_inline uint32_t read_pc(void)
+{
+	uint32_t val;
+
+	asm volatile ("adr %0, ." : "=r" (val));
+	return val;
+}
+
+static __always_inline uint32_t read_sp(void)
+{
+	uint32_t val;
+
+	asm volatile ("mov %0, sp" : "=r" (val));
+	return val;
+}
+
+static __always_inline uint32_t read_lr(void)
+{
+	uint32_t val;
+
+	asm volatile ("mov %0, lr" : "=r" (val));
+	return val;
+}
 #endif /*ASM*/
 
 #endif /*ARM32_H*/

core/arch/arm/include/arm64.h

@@ -27,6 +27,7 @@
 #ifndef ARM64_H
 #define ARM64_H
 
+#include <sys/cdefs.h>
 #include <stdint.h>
 #include <util.h>
 
@@ -221,6 +222,22 @@ static inline void write_at_s1e1r(uint64_t va)
 	asm volatile ("at	S1E1R, %0" : : "r" (va));
 }
 
+static __always_inline uint64_t read_pc(void)
+{
+	uint64_t val;
+
+	asm volatile ("adr %0, ." : "=r" (val));
+	return val;
+}
+
+static __always_inline uint64_t read_fp(void)
+{
+	uint64_t val;
+
+	asm volatile ("mov %0, x29" : "=r" (val));
+	return val;
+}
+
 /*
  * Templates for register read/write functions based on mrs/msr
  */

core/arch/arm/include/kernel/unwind.h

@@ -57,6 +57,15 @@ struct unwind_state {
 #endif /*ARM64*/
 
 bool unwind_stack(struct unwind_state *state);
+
+#if defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0)
+void print_stack(int level);
+#else
+static inline void print_stack(int level __unused)
+{
+}
+#endif
+
 #endif /*ASM*/
 
 #ifdef CFG_CORE_UNWIND

core/arch/arm/kernel/unwind_arm32.c

@@ -29,7 +29,10 @@
  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <arm.h>
+#include <kernel/misc.h>
 #include <kernel/unwind.h>
+#include <string.h>
 #include <trace.h>
 
 /* The register names */
@@ -358,6 +361,39 @@ bool unwind_stack(struct unwind_state *state)
 	return !finished;
 }
 
+#if defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0)
+
+void print_stack(int level)
+{
+	struct unwind_state state;
+
+	memset(&state, 0, sizeof(state));
+	state.registers[SP] = read_sp();
+	state.registers[LR] = read_lr();
+	state.registers[PC] = read_pc();
+
+	do {
+		switch (level) {
+		case TRACE_FLOW:
+			FMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		case TRACE_DEBUG:
+			DMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		case TRACE_INFO:
+			IMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		case TRACE_ERROR:
+			EMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		default:
+			break;
+		}
+	} while (unwind_stack(&state));
+}
+
+#endif /* defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0) */
+
 /*
  * These functions are referenced but never used
  */

core/arch/arm/kernel/unwind_arm64.c

@@ -28,8 +28,11 @@
  * SUCH DAMAGE.
  */
 
+#include <arm.h>
 #include <kernel/unwind.h>
 #include <kernel/thread.h>
+#include <string.h>
+#include <trace.h>
 
 bool unwind_stack(struct unwind_state *frame)
 {
@@ -47,3 +50,35 @@ bool unwind_stack(struct unwind_state *frame)
 
 	return true;
 }
+
+#if defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0)
+
+void print_stack(int level)
+{
+	struct unwind_state state;
+
+	memset(&state, 0, sizeof(state));
+	state.pc = read_pc();
+	state.fp = read_fp();
+
+	do {
+		switch (level) {
+		case TRACE_FLOW:
+			FMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		case TRACE_DEBUG:
+			DMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		case TRACE_INFO:
+			IMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		case TRACE_ERROR:
+			EMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		default:
+			break;
+		}
+	} while (unwind_stack(&state));
+}
+
+#endif /* defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0) */

lib/libutils/ext/include/compiler.h

@@ -29,7 +29,7 @@
 #define COMPILER_H
 
 /*
- * Macros that should be used instead of using __attributue__ directly to
+ * Macros that should be used instead of using __attribute__ directly to
  * ease portability and make the code easier to read.
  */
 

lib/libutils/ext/include/trace.h

@@ -167,4 +167,37 @@ void dhex_dump(const char *function, int line, int level,
 
 #endif /* TRACE_LEVEL */
 
+#if defined(__KERNEL__) && defined(CFG_CORE_UNWIND)
+#include <kernel/unwind.h>
+#define _PRINT_STACK
+#endif
+
+#if defined(_PRINT_STACK) && (TRACE_LEVEL >= TRACE_ERROR)
+#define EPRINT_STACK() print_stack(TRACE_ERROR)
+#else
+#define EPRINT_STACK() (void)0
+#endif
+
+#if defined(_PRINT_STACK) && (TRACE_LEVEL >= TRACE_INFO)
+#define IPRINT_STACK() print_stack(TRACE_INFO)
+#else
+#define IPRINT_STACK() (void)0
+#endif
+
+#if defined(_PRINT_STACK) && (TRACE_LEVEL >= TRACE_DEBUG)
+#define DPRINT_STACK() print_stack(TRACE_DEBUG)
+#else
+#define DPRINT_STACK() (void)0
+#endif
+
+#if defined(_PRINT_STACK) && (TRACE_LEVEL >= TRACE_FLOW)
+#define FPRINT_STACK() print_stack(TRACE_FLOW)
+#else
+#define FPRINT_STACK() (void)0
+#endif
+
+#if defined(__KERNEL__) && defined(CFG_CORE_UNWIND)
+#undef _PRINT_STACK
+#endif
+
 #endif /* TRACE_H */

lib/libutils/isoc/include/sys/cdefs.h

@@ -41,4 +41,6 @@
 #endif
 #endif
 
+#define __always_inline	__attribute__((always_inline)) inline
+
 #endif /*SYS_CDEFS_H*/