X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Frtos%2Frtkernel.c;fp=src%2Frtos%2Frtkernel.c;h=ba1de25172bac7871015f80f909ca3479c117f84;hp=0000000000000000000000000000000000000000;hb=43c8aa28cb1570c11e1099c43e685af228190679;hpb=cf50bcb841238726697dc1250d6b6cb49fc6d19d diff --git a/src/rtos/rtkernel.c b/src/rtos/rtkernel.c new file mode 100644 index 0000000000..ba1de25172 --- /dev/null +++ b/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 +#include +#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, + ¤t_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, +};