From f5b7033742e0858610d37808f15b25602ba62995 Mon Sep 17 00:00:00 2001 From: Jonathan Dumaresq Date: Wed, 6 Apr 2016 16:00:06 -0400 Subject: [PATCH] RTOS support: Add FPU support for FreeRTOS MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit Add new structure for for working with FPU thread in thread view. This modification support both stacking. When FPU is activated, LR must be validated to check if the FPU register are push on the stack. This is mandatory to find the correct stack pointer position. the modified code was inspired and adapted from https://github.com/TauLabs/TauLabs/commit/88d2003bb83a6b72c796274c889e307f96645966 Change-Id: I6641926aa14e7216cacb399cbc8bb0db324cc9fc Signed-off-by: Jonathan Dumaresq Reviewed-on: http://openocd.zylin.com/3397 Tested-by: jenkins Reviewed-by: Sergey A. Borshch Reviewed-by: Harry Zhurov Reviewed-by: Anton Gusev Reviewed-by: Михаил Цивинский Reviewed-by: Freddie Chopin --- src/rtos/FreeRTOS.c | 54 ++++++++++++++++++++++++++-- src/rtos/rtos_standard_stackings.c | 57 ++++++++++++++++++++++++++++++ src/rtos/rtos_standard_stackings.h | 2 ++ 3 files changed, 111 insertions(+), 2 deletions(-) diff --git a/src/rtos/FreeRTOS.c b/src/rtos/FreeRTOS.c index 1ce6807240..93137733cb 100644 --- a/src/rtos/FreeRTOS.c +++ b/src/rtos/FreeRTOS.c @@ -30,6 +30,10 @@ #include "helper/log.h" #include "helper/types.h" #include "rtos_standard_stackings.h" +#include "target/armv7m.h" +#include "target/cortex_m.h" + + #define FREERTOS_MAX_PRIORITIES 63 @@ -45,7 +49,9 @@ struct FreeRTOS_params { const unsigned char list_elem_content_offset; const unsigned char thread_stack_offset; const unsigned char thread_name_offset; - const struct rtos_register_stacking *stacking_info; + 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 FreeRTOS_params FreeRTOS_params_list[] = { @@ -60,6 +66,8 @@ static const struct FreeRTOS_params FreeRTOS_params_list[] = { 0, /* thread_stack_offset; */ 52, /* thread_name_offset; */ &rtos_standard_Cortex_M3_stacking, /* stacking_info */ + &rtos_standard_Cortex_M4F_stacking, + &rtos_standard_Cortex_M4F_FPU_stacking, }, { "hla_target", /* target_name */ @@ -72,6 +80,8 @@ static const struct FreeRTOS_params FreeRTOS_params_list[] = { 0, /* thread_stack_offset; */ 52, /* thread_name_offset; */ &rtos_standard_Cortex_M3_stacking, /* stacking_info */ + &rtos_standard_Cortex_M4F_stacking, + &rtos_standard_Cortex_M4F_FPU_stacking, }, { "nds32_v3", /* target_name */ @@ -84,6 +94,8 @@ static const struct FreeRTOS_params FreeRTOS_params_list[] = { 0, /* thread_stack_offset; */ 52, /* thread_name_offset; */ &rtos_standard_NDS32_N1068_stacking, /* stacking_info */ + &rtos_standard_Cortex_M4F_stacking, + &rtos_standard_Cortex_M4F_FPU_stacking, }, }; @@ -418,7 +430,45 @@ static int FreeRTOS_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, ch thread_id + param->thread_stack_offset, stack_ptr); - return rtos_generic_stack_read(rtos->target, param->stacking_info, stack_ptr, hex_reg_list); + /* Check for armv7m with *enabled* FPU, i.e. a Cortex M4F */ + int cm4_fpu_enabled = 0; + struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target); + if (is_armv7m(armv7m_target)) { + if (armv7m_target->fp_feature == FPv4_SP) { + /* 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 = 1; + } + } + } + + if (cm4_fpu_enabled == 1) { + /* Read the LR to decide between stacking with or without FPU */ + uint32_t LR_svc = 0; + retval = target_read_buffer(rtos->target, + stack_ptr + 0x20, + param->pointer_width, + (uint8_t *)&LR_svc); + if (retval != ERROR_OK) { + LOG_OUTPUT("Error reading stack frame from FreeRTOS thread\r\n"); + return retval; + } + if ((LR_svc & 0x10) == 0) + return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f_fpu, stack_ptr, hex_reg_list); + else + return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f, stack_ptr, hex_reg_list); + } else + return rtos_generic_stack_read(rtos->target, param->stacking_info_cm3, stack_ptr, hex_reg_list); } static int FreeRTOS_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[]) diff --git a/src/rtos/rtos_standard_stackings.c b/src/rtos/rtos_standard_stackings.c index 1a58870fc5..7d72b4e280 100644 --- a/src/rtos/rtos_standard_stackings.c +++ b/src/rtos/rtos_standard_stackings.c @@ -45,6 +45,47 @@ static const struct stack_register_offset rtos_standard_Cortex_M3_stack_offsets[ { 0x3c, 32 }, /* xPSR */ }; +static const struct stack_register_offset rtos_standard_Cortex_M4F_stack_offsets[] = { + { 0x24, 32 }, /* r0 */ + { 0x28, 32 }, /* r1 */ + { 0x2c, 32 }, /* r2 */ + { 0x30, 32 }, /* r3 */ + { 0x00, 32 }, /* r4 */ + { 0x04, 32 }, /* r5 */ + { 0x08, 32 }, /* r6 */ + { 0x0c, 32 }, /* r7 */ + { 0x10, 32 }, /* r8 */ + { 0x14, 32 }, /* r9 */ + { 0x18, 32 }, /* r10 */ + { 0x1c, 32 }, /* r11 */ + { 0x34, 32 }, /* r12 */ + { -2, 32 }, /* sp */ + { 0x38, 32 }, /* lr */ + { 0x3c, 32 }, /* pc */ + { 0x40, 32 }, /* xPSR */ +}; + +static const struct stack_register_offset rtos_standard_Cortex_M4F_FPU_stack_offsets[] = { + { 0x64, 32 }, /* r0 */ + { 0x68, 32 }, /* r1 */ + { 0x6c, 32 }, /* r2 */ + { 0x70, 32 }, /* r3 */ + { 0x00, 32 }, /* r4 */ + { 0x04, 32 }, /* r5 */ + { 0x08, 32 }, /* r6 */ + { 0x0c, 32 }, /* r7 */ + { 0x10, 32 }, /* r8 */ + { 0x14, 32 }, /* r9 */ + { 0x18, 32 }, /* r10 */ + { 0x1c, 32 }, /* r11 */ + { 0x74, 32 }, /* r12 */ + { -2, 32 }, /* sp */ + { 0x78, 32 }, /* lr */ + { 0x7c, 32 }, /* pc */ + { 0x80, 32 }, /* xPSR */ +}; + + static const struct stack_register_offset rtos_standard_Cortex_R4_stack_offsets[] = { { 0x08, 32 }, /* r0 (a1) */ { 0x0c, 32 }, /* r1 (a2) */ @@ -198,6 +239,22 @@ const struct rtos_register_stacking rtos_standard_Cortex_M3_stacking = { rtos_standard_Cortex_M3_stack_offsets /* register_offsets */ }; +const struct rtos_register_stacking rtos_standard_Cortex_M4F_stacking = { + 0x44, /* stack_registers_size 4 more for LR*/ + -1, /* stack_growth_direction */ + ARMV7M_NUM_CORE_REGS, /* num_output_registers */ + rtos_standard_Cortex_M3_stack_align, /* stack_alignment */ + rtos_standard_Cortex_M4F_stack_offsets /* register_offsets */ +}; + +const struct rtos_register_stacking rtos_standard_Cortex_M4F_FPU_stacking = { + 0xcc, /* stack_registers_size 4 more for LR + 48 more for FPU S0-S15 register*/ + -1, /* stack_growth_direction */ + ARMV7M_NUM_CORE_REGS, /* num_output_registers */ + rtos_standard_Cortex_M3_stack_align, /* stack_alignment */ + rtos_standard_Cortex_M4F_FPU_stack_offsets /* register_offsets */ +}; + const struct rtos_register_stacking rtos_standard_Cortex_R4_stacking = { 0x48, /* stack_registers_size */ -1, /* stack_growth_direction */ diff --git a/src/rtos/rtos_standard_stackings.h b/src/rtos/rtos_standard_stackings.h index f931bb9f26..6768c4b4d7 100644 --- a/src/rtos/rtos_standard_stackings.h +++ b/src/rtos/rtos_standard_stackings.h @@ -28,6 +28,8 @@ #include "rtos.h" extern const struct rtos_register_stacking rtos_standard_Cortex_M3_stacking; +extern const struct rtos_register_stacking rtos_standard_Cortex_M4F_stacking; +extern const struct rtos_register_stacking rtos_standard_Cortex_M4F_FPU_stacking; extern const struct rtos_register_stacking rtos_standard_Cortex_R4_stacking; extern const struct rtos_register_stacking rtos_standard_NDS32_N1068_stacking; int64_t rtos_generic_stack_align8(struct target *target, -- 2.30.2