rtos: Support rt-kernel

Works for the proprietary rt-kernel from rt-labs.

See: https://rt-labs.com/product/rt-kernel/
Change-Id: Id2c2e292c15fb17eab25e3d07db05014daa2a2b0
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/6668
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

doc/openocd.texi

@@ -5000,7 +5000,7 @@ The value should normally correspond to a static mapping for the
 @var{rtos_type} can be one of @option{auto}, @option{none}, @option{eCos},
 @option{ThreadX}, @option{FreeRTOS}, @option{linux}, @option{ChibiOS},
 @option{embKernel}, @option{mqx}, @option{uCOS-III}, @option{nuttx},
-@option{RIOT}, @option{Zephyr}
+@option{RIOT}, @option{Zephyr}, @option{rtkernel}
 @xref{gdbrtossupport,,RTOS Support}.
 
 @item @code{-defer-examine} -- skip target examination at initial JTAG chain
@@ -11867,6 +11867,7 @@ Currently supported rtos's include:
 @item @option{RIOT}
 @item @option{hwthread} (This is not an actual RTOS. @xref{usingopenocdsmpwithgdb,,Using OpenOCD SMP with GDB}.)
 @item @option{Zephyr}
+@item @option{rtkernel}
 @end itemize
 
 At any time, it's possible to drop the selected RTOS using:
@@ -11908,6 +11909,8 @@ _tcb_name_offset.
 @end raggedright
 @item Zephyr symbols
 _kernel, _kernel_openocd_offsets, _kernel_openocd_size_t_size
+@item rtkernel symbols
+Multiple struct offsets.
 @end table
 
 For most RTOS supported the above symbols will be exported by default. However for

src/rtos/Makefile.am

@@ -21,6 +21,7 @@ noinst_LTLIBRARIES += %D%/librtos.la
 	%D%/mqx.c \
 	%D%/uCOS-III.c \
 	%D%/nuttx.c \
+	%D%/rtkernel.c \
 	%D%/hwthread.c \
 	%D%/zephyr.c \
 	%D%/riot.c \

src/rtos/rtkernel.c

@@ -0,0 +1,384 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   Copyright (C) 2016-2023 by Andreas Fritiofson                         *
+ *   andreas.fritiofson@gmail.com                                          *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/time_support.h>
+#include <jtag/jtag.h>
+#include "target/target.h"
+#include "target/target_type.h"
+#include "rtos.h"
+#include "helper/log.h"
+#include "helper/types.h"
+#include "rtos_standard_stackings.h"
+#include "target/armv7m.h"
+#include "target/cortex_m.h"
+
+#define ST_DEAD     BIT(0)    /* Task is waiting to be deleted */
+#define ST_WAIT     BIT(1)    /* Task is blocked: */
+#define ST_SEM      BIT(2)    /*  on semaphore */
+#define ST_MTX      BIT(3)    /*  on mutex */
+#define ST_SIG      BIT(4)    /*  on signal */
+#define ST_DLY      BIT(5)    /*  on timer */
+#define ST_FLAG     BIT(6)    /*  on flag */
+#define ST_FLAG_ALL BIT(7)    /*  on flag and flag mode is "ALL" */
+#define ST_MBOX     BIT(8)    /*  on mailbox */
+#define ST_STP      BIT(9)    /*  self stopped */
+#define ST_SUSPEND  BIT(10)   /* Task is suspended */
+#define ST_TT       BIT(11)   /* Time triggered task */
+#define ST_TT_YIELD BIT(12)   /* Time triggered task that yields */
+#define ST_CREATE   BIT(13)   /* Task was created by task_create() */
+
+struct rtkernel_params {
+	const char *target_name;
+	const struct rtos_register_stacking *stacking_info_cm3;
+	const struct rtos_register_stacking *stacking_info_cm4f;
+	const struct rtos_register_stacking *stacking_info_cm4f_fpu;
+};
+
+static const struct rtkernel_params rtkernel_params_list[] = {
+	{
+		"cortex_m",			/* target_name */
+		&rtos_standard_cortex_m3_stacking,	/* stacking_info */
+		&rtos_standard_cortex_m4f_stacking,
+		&rtos_standard_cortex_m4f_fpu_stacking,
+	},
+	{
+		"hla_target",			/* target_name */
+		&rtos_standard_cortex_m3_stacking,	/* stacking_info */
+		&rtos_standard_cortex_m4f_stacking,
+		&rtos_standard_cortex_m4f_fpu_stacking,
+	},
+};
+
+enum rtkernel_symbol_values {
+	sym_os_state = 0,
+	sym___off_os_state2chain = 1,
+	sym___off_os_state2current = 2,
+	sym___off_task2chain = 3,
+	sym___off_task2magic = 4,
+	sym___off_task2stack = 5,
+	sym___off_task2state = 6,
+	sym___off_task2name = 7,
+	sym___val_task_magic = 8,
+};
+
+struct symbols {
+	const char *name;
+	bool optional;
+};
+
+static const struct symbols rtkernel_symbol_list[] = {
+	{ "os_state", false },
+	{ "__off_os_state2chain", false },
+	{ "__off_os_state2current", false },
+	{ "__off_task2chain", false },
+	{ "__off_task2magic", false },
+	{ "__off_task2stack", false },
+	{ "__off_task2state", false },
+	{ "__off_task2name", false },
+	{ "__val_task_magic", false },
+	{ NULL, false }
+};
+
+static void *realloc_preserve(void *ptr, size_t old_size, size_t new_size)
+{
+	void *new_ptr = malloc(new_size);
+
+	if (new_ptr) {
+		memcpy(new_ptr, ptr, MIN(old_size, new_size));
+		free(ptr);
+	}
+
+	return new_ptr;
+}
+
+static int rtkernel_add_task(struct rtos *rtos, uint32_t task, uint32_t current_task)
+{
+	int retval;
+	int new_thread_count = rtos->thread_count + 1;
+	struct thread_detail *new_thread_details = realloc_preserve(rtos->thread_details,
+			rtos->thread_count * sizeof(struct thread_detail),
+			new_thread_count * sizeof(struct thread_detail));
+	if (!new_thread_details) {
+		LOG_ERROR("Error growing memory to %d threads", new_thread_count);
+		return ERROR_FAIL;
+	}
+	rtos->thread_details = new_thread_details;
+	struct thread_detail *thread = &new_thread_details[rtos->thread_count];
+
+	*thread = (struct thread_detail){ .threadid = task, .exists = true };
+
+	/* Read the task name */
+	uint32_t name;
+	retval = target_read_u32(rtos->target, task + rtos->symbols[sym___off_task2name].address, &name);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read task name pointer from target");
+		return retval;
+	}
+	uint8_t tmp_str[33];
+	retval = target_read_buffer(rtos->target, name, sizeof(tmp_str) - 1, tmp_str);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error reading task name from target");
+		return retval;
+	}
+	tmp_str[sizeof(tmp_str) - 1] = '\0';
+	LOG_DEBUG("task name at 0x%" PRIx32 ", value \"%s\"", name, tmp_str);
+
+	if (tmp_str[0] != '\0')
+		thread->thread_name_str = strdup((char *)tmp_str);
+	else
+		thread->thread_name_str = strdup("No Name");
+
+	/* Read the task state */
+	uint16_t state;
+	retval = target_read_u16(rtos->target, task + rtos->symbols[sym___off_task2state].address, &state);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read task state from target");
+		return retval;
+	}
+
+	LOG_DEBUG("task state 0x%" PRIx16, state);
+
+	char state_str[64] = "";
+	if (state & ST_TT)
+		strcat(state_str, "TT|");
+	if (task == current_task) {
+		strcat(state_str, "RUN");
+	} else {
+		if (state & (ST_TT | ST_TT_YIELD))
+			strcat(state_str, "YIELD");
+		else if (state & ST_DEAD)
+			strcat(state_str, "DEAD");
+		else if (state & ST_WAIT)
+			strcat(state_str, "WAIT");
+		else if (state & ST_SUSPEND)
+			strcat(state_str, "SUSP");
+		else
+			strcat(state_str, "READY");
+	}
+	if (state & ST_SEM)
+		strcat(state_str, "|SEM");
+	if (state & ST_MTX)
+		strcat(state_str, "|MTX");
+	if (state & ST_SIG)
+		strcat(state_str, "|SIG");
+	if (state & ST_DLY)
+		strcat(state_str, "|DLY");
+	if ((state & ST_FLAG) || (state & ST_FLAG_ALL))
+		strcat(state_str, "|FLAG");
+	if (state & ST_FLAG_ALL)
+		strcat(state_str, "_ALL");
+	if (state & ST_MBOX)
+		strcat(state_str, "|MBOX");
+	if (state & ST_STP)
+		strcat(state_str, "|STP");
+
+	thread->extra_info_str = strdup(state_str);
+
+	rtos->thread_count = new_thread_count;
+	if (task == current_task)
+		rtos->current_thread = task;
+	return ERROR_OK;
+}
+
+static int rtkernel_verify_task(struct rtos *rtos, uint32_t task)
+{
+	int retval;
+	uint32_t magic;
+	retval = target_read_u32(rtos->target, task + rtos->symbols[sym___off_task2magic].address, &magic);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not read task magic from target");
+		return retval;
+	}
+	if (magic != rtos->symbols[sym___val_task_magic].address) {
+		LOG_ERROR("Invalid task found (magic=0x%" PRIx32 ")", magic);
+		return ERROR_FAIL;
+	}
+	return retval;
+}
+
+static int rtkernel_update_threads(struct rtos *rtos)
+{
+	/* wipe out previous thread details if any */
+	/* do this first because rtos layer does not check our retval */
+	rtos_free_threadlist(rtos);
+	rtos->current_thread = 0;
+
+	if (!rtos->symbols) {
+		LOG_ERROR("No symbols for rt-kernel");
+		return -3;
+	}
+
+	/* read the current task */
+	uint32_t current_task;
+	int retval = target_read_u32(rtos->target,
+			rtos->symbols[sym_os_state].address + rtos->symbols[sym___off_os_state2current].address,
+			&current_task);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error reading current task");
+		return retval;
+	}
+	LOG_DEBUG("current task is 0x%" PRIx32, current_task);
+
+	retval = rtkernel_verify_task(rtos, current_task);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Current task is invalid");
+		return retval;
+	}
+
+	/* loop through kernel task list */
+	uint32_t chain = rtos->symbols[sym_os_state].address + rtos->symbols[sym___off_os_state2chain].address;
+	LOG_DEBUG("chain start at 0x%" PRIx32, chain);
+
+	uint32_t next = chain;
+	for (;;) {
+		retval = target_read_u32(rtos->target, next, &next);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read rt-kernel data structure from target");
+			return retval;
+		}
+		LOG_DEBUG("next entry at 0x%" PRIx32, next);
+		if (next == chain) {
+			LOG_DEBUG("end of chain detected");
+			break;
+		}
+		uint32_t task = next - rtos->symbols[sym___off_task2chain].address;
+		LOG_DEBUG("found task at 0x%" PRIx32, task);
+
+		retval = rtkernel_verify_task(rtos, task);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Invalid task found");
+			return retval;
+		}
+
+		retval = rtkernel_add_task(rtos, task, current_task);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not add task to rtos system");
+			return retval;
+		}
+	}
+	return ERROR_OK;
+}
+
+static int rtkernel_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
+		struct rtos_reg **reg_list, int *num_regs)
+{
+	uint32_t stack_ptr = 0;
+
+	if (!rtos)
+		return -1;
+
+	if (thread_id == 0)
+		return -2;
+
+	if (!rtos->rtos_specific_params)
+		return -1;
+
+	const struct rtkernel_params *param = rtos->rtos_specific_params;
+
+	/* Read the stack pointer */
+	int retval = target_read_u32(rtos->target, thread_id + rtos->symbols[sym___off_task2stack].address, &stack_ptr);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error reading stack pointer from rtkernel thread");
+		return retval;
+	}
+	LOG_DEBUG("stack pointer at 0x%" PRIx64 ", value 0x%" PRIx32,
+			thread_id + rtos->symbols[sym___off_task2stack].address,
+			stack_ptr);
+
+	/* Adjust stack pointer to ignore non-standard BASEPRI register stacking */
+	stack_ptr += 4;
+
+	/* Check for armv7m with *enabled* FPU, i.e. a Cortex M4F */
+	bool cm4_fpu_enabled = false;
+	struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target);
+	if (is_armv7m(armv7m_target)) {
+		if (armv7m_target->fp_feature != FP_NONE) {
+			/* Found ARM v7m target which includes a FPU */
+			uint32_t cpacr;
+
+			retval = target_read_u32(rtos->target, FPU_CPACR, &cpacr);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Could not read CPACR register to check FPU state");
+				return -1;
+			}
+
+			/* Check if CP10 and CP11 are set to full access. */
+			if (cpacr & 0x00F00000) {
+				/* Found target with enabled FPU */
+				cm4_fpu_enabled = true;
+			}
+		}
+	}
+
+	if (!cm4_fpu_enabled) {
+		LOG_DEBUG("cm3 stacking");
+		return rtos_generic_stack_read(rtos->target, param->stacking_info_cm3, stack_ptr, reg_list, num_regs);
+	}
+
+	/* Read the LR to decide between stacking with or without FPU */
+	uint32_t lr_svc;
+	retval = target_read_u32(rtos->target, stack_ptr + 0x20, &lr_svc);
+	if (retval != ERROR_OK) {
+		LOG_OUTPUT("Error reading stack frame from rtkernel thread\r\n");
+		return retval;
+	}
+
+	if ((lr_svc & 0x10) == 0) {
+		LOG_DEBUG("cm4f_fpu stacking");
+		return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f_fpu, stack_ptr, reg_list, num_regs);
+	}
+
+	LOG_DEBUG("cm4f stacking");
+	return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f, stack_ptr, reg_list, num_regs);
+}
+
+static int rtkernel_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[])
+{
+	*symbol_list = calloc(ARRAY_SIZE(rtkernel_symbol_list), sizeof(struct symbol_table_elem));
+	if (!*symbol_list)
+		return ERROR_FAIL;
+
+	for (size_t i = 0; i < ARRAY_SIZE(rtkernel_symbol_list); i++) {
+		(*symbol_list)[i].symbol_name = rtkernel_symbol_list[i].name;
+		(*symbol_list)[i].optional = rtkernel_symbol_list[i].optional;
+	}
+
+	return ERROR_OK;
+}
+
+static bool rtkernel_detect_rtos(struct target *target)
+{
+	return (target->rtos->symbols) &&
+			(target->rtos->symbols[sym___off_os_state2chain].address != 0);
+}
+
+static int rtkernel_create(struct target *target)
+{
+	for (size_t i = 0; i < ARRAY_SIZE(rtkernel_params_list); i++) {
+		if (strcmp(rtkernel_params_list[i].target_name, target->type->name) == 0) {
+			target->rtos->rtos_specific_params = (void *)&rtkernel_params_list[i];
+			return 0;
+		}
+	}
+
+	LOG_ERROR("Could not find target in rt-kernel compatibility list");
+	return -1;
+}
+
+const struct rtos_type rtkernel_rtos = {
+	.name = "rtkernel",
+
+	.detect_rtos = rtkernel_detect_rtos,
+	.create = rtkernel_create,
+	.update_threads = rtkernel_update_threads,
+	.get_thread_reg_list = rtkernel_get_thread_reg_list,
+	.get_symbol_list_to_lookup = rtkernel_get_symbol_list_to_lookup,
+};

src/rtos/rtos.c

@@ -29,6 +29,7 @@ extern struct rtos_type nuttx_rtos;
 extern struct rtos_type hwthread_rtos;
 extern struct rtos_type riot_rtos;
 extern struct rtos_type zephyr_rtos;
+extern struct rtos_type rtkernel_rtos;
 
 static struct rtos_type *rtos_types[] = {
 	&threadx_rtos,
@@ -43,6 +44,7 @@ static struct rtos_type *rtos_types[] = {
 	&nuttx_rtos,
 	&riot_rtos,
 	&zephyr_rtos,
+	&rtkernel_rtos,
 	/* keep this as last, as it always matches with rtos auto */
 	&hwthread_rtos,
 	NULL