src/rtos/rtkernel.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2016-2023 by Andreas Fritiofson                         *
   5  *   andreas.fritiofson@gmail.com                                          *
   6  ***************************************************************************/
   7
   8 #ifdef HAVE_CONFIG_H
   9 #include "config.h"
  10 #endif
  11
  12 #include <helper/time_support.h>
  13 #include <jtag/jtag.h>
  14 #include "target/target.h"
  15 #include "rtos.h"
  16 #include "helper/log.h"
  17 #include "helper/types.h"
  18 #include "rtos_standard_stackings.h"
  19 #include "target/armv7m.h"
  20 #include "target/cortex_m.h"
  21
  22 #define ST_DEAD     BIT(0)    /* Task is waiting to be deleted */
  23 #define ST_WAIT     BIT(1)    /* Task is blocked: */
  24 #define ST_SEM      BIT(2)    /*  on semaphore */
  25 #define ST_MTX      BIT(3)    /*  on mutex */
  26 #define ST_SIG      BIT(4)    /*  on signal */
  27 #define ST_DLY      BIT(5)    /*  on timer */
  28 #define ST_FLAG     BIT(6)    /*  on flag */
  29 #define ST_FLAG_ALL BIT(7)    /*  on flag and flag mode is "ALL" */
  30 #define ST_MBOX     BIT(8)    /*  on mailbox */
  31 #define ST_STP      BIT(9)    /*  self stopped */
  32 #define ST_SUSPEND  BIT(10)   /* Task is suspended */
  33 #define ST_TT       BIT(11)   /* Time triggered task */
  34 #define ST_TT_YIELD BIT(12)   /* Time triggered task that yields */
  35 #define ST_CREATE   BIT(13)   /* Task was created by task_create() */
  36
  37 struct rtkernel_params {
  38         const char *target_name;
  39         const struct rtos_register_stacking *stacking_info_cm3;
  40         const struct rtos_register_stacking *stacking_info_cm4f;
  41         const struct rtos_register_stacking *stacking_info_cm4f_fpu;
  42 };
  43
  44 static const struct rtkernel_params rtkernel_params_list[] = {
  45         {
  46                 "cortex_m",                     /* target_name */
  47                 &rtos_standard_cortex_m3_stacking,      /* stacking_info */
  48                 &rtos_standard_cortex_m4f_stacking,
  49                 &rtos_standard_cortex_m4f_fpu_stacking,
  50         },
  51         {
  52                 "hla_target",                   /* target_name */
  53                 &rtos_standard_cortex_m3_stacking,      /* stacking_info */
  54                 &rtos_standard_cortex_m4f_stacking,
  55                 &rtos_standard_cortex_m4f_fpu_stacking,
  56         },
  57 };
  58
  59 enum rtkernel_symbol_values {
  60         sym_os_state = 0,
  61         sym___off_os_state2chain = 1,
  62         sym___off_os_state2current = 2,
  63         sym___off_task2chain = 3,
  64         sym___off_task2magic = 4,
  65         sym___off_task2stack = 5,
  66         sym___off_task2state = 6,
  67         sym___off_task2name = 7,
  68         sym___val_task_magic = 8,
  69 };
  70
  71 struct symbols {
  72         const char *name;
  73         bool optional;
  74 };
  75
  76 static const struct symbols rtkernel_symbol_list[] = {
  77         { "os_state", false },
  78         { "__off_os_state2chain", false },
  79         { "__off_os_state2current", false },
  80         { "__off_task2chain", false },
  81         { "__off_task2magic", false },
  82         { "__off_task2stack", false },
  83         { "__off_task2state", false },
  84         { "__off_task2name", false },
  85         { "__val_task_magic", false },
  86         { NULL, false }
  87 };
  88
  89 static void *realloc_preserve(void *ptr, size_t old_size, size_t new_size)
  90 {
  91         void *new_ptr = malloc(new_size);
  92
  93         if (new_ptr) {
  94                 memcpy(new_ptr, ptr, MIN(old_size, new_size));
  95                 free(ptr);
  96         }
  97
  98         return new_ptr;
  99 }
 100
 101 static int rtkernel_add_task(struct rtos *rtos, uint32_t task, uint32_t current_task)
 102 {
 103         int retval;
 104         int new_thread_count = rtos->thread_count + 1;
 105         struct thread_detail *new_thread_details = realloc_preserve(rtos->thread_details,
 106                         rtos->thread_count * sizeof(struct thread_detail),
 107                         new_thread_count * sizeof(struct thread_detail));
 108         if (!new_thread_details) {
 109                 LOG_ERROR("Error growing memory to %d threads", new_thread_count);
 110                 return ERROR_FAIL;
 111         }
 112         rtos->thread_details = new_thread_details;
 113         struct thread_detail *thread = &new_thread_details[rtos->thread_count];
 114
 115         *thread = (struct thread_detail){ .threadid = task, .exists = true };
 116
 117         /* Read the task name */
 118         uint32_t name;
 119         retval = target_read_u32(rtos->target, task + rtos->symbols[sym___off_task2name].address, &name);
 120         if (retval != ERROR_OK) {
 121                 LOG_ERROR("Could not read task name pointer from target");
 122                 return retval;
 123         }
 124         uint8_t tmp_str[33];
 125         retval = target_read_buffer(rtos->target, name, sizeof(tmp_str) - 1, tmp_str);
 126         if (retval != ERROR_OK) {
 127                 LOG_ERROR("Error reading task name from target");
 128                 return retval;
 129         }
 130         tmp_str[sizeof(tmp_str) - 1] = '\0';
 131         LOG_DEBUG("task name at 0x%" PRIx32 ", value \"%s\"", name, tmp_str);
 132
 133         if (tmp_str[0] != '\0')
 134                 thread->thread_name_str = strdup((char *)tmp_str);
 135         else
 136                 thread->thread_name_str = strdup("No Name");
 137
 138         /* Read the task state */
 139         uint16_t state;
 140         retval = target_read_u16(rtos->target, task + rtos->symbols[sym___off_task2state].address, &state);
 141         if (retval != ERROR_OK) {
 142                 LOG_ERROR("Could not read task state from target");
 143                 return retval;
 144         }
 145
 146         LOG_DEBUG("task state 0x%" PRIx16, state);
 147
 148         char state_str[64] = "";
 149         if (state & ST_TT)
 150                 strcat(state_str, "TT|");
 151         if (task == current_task) {
 152                 strcat(state_str, "RUN");
 153         } else {
 154                 if (state & (ST_TT | ST_TT_YIELD))
 155                         strcat(state_str, "YIELD");
 156                 else if (state & ST_DEAD)
 157                         strcat(state_str, "DEAD");
 158                 else if (state & ST_WAIT)
 159                         strcat(state_str, "WAIT");
 160                 else if (state & ST_SUSPEND)
 161                         strcat(state_str, "SUSP");
 162                 else
 163                         strcat(state_str, "READY");
 164         }
 165         if (state & ST_SEM)
 166                 strcat(state_str, "|SEM");
 167         if (state & ST_MTX)
 168                 strcat(state_str, "|MTX");
 169         if (state & ST_SIG)
 170                 strcat(state_str, "|SIG");
 171         if (state & ST_DLY)
 172                 strcat(state_str, "|DLY");
 173         if ((state & ST_FLAG) || (state & ST_FLAG_ALL))
 174                 strcat(state_str, "|FLAG");
 175         if (state & ST_FLAG_ALL)
 176                 strcat(state_str, "_ALL");
 177         if (state & ST_MBOX)
 178                 strcat(state_str, "|MBOX");
 179         if (state & ST_STP)
 180                 strcat(state_str, "|STP");
 181
 182         thread->extra_info_str = strdup(state_str);
 183
 184         rtos->thread_count = new_thread_count;
 185         if (task == current_task)
 186                 rtos->current_thread = task;
 187         return ERROR_OK;
 188 }
 189
 190 static int rtkernel_verify_task(struct rtos *rtos, uint32_t task)
 191 {
 192         int retval;
 193         uint32_t magic;
 194         retval = target_read_u32(rtos->target, task + rtos->symbols[sym___off_task2magic].address, &magic);
 195         if (retval != ERROR_OK) {
 196                 LOG_ERROR("Could not read task magic from target");
 197                 return retval;
 198         }
 199         if (magic != rtos->symbols[sym___val_task_magic].address) {
 200                 LOG_ERROR("Invalid task found (magic=0x%" PRIx32 ")", magic);
 201                 return ERROR_FAIL;
 202         }
 203         return retval;
 204 }
 205
 206 static int rtkernel_update_threads(struct rtos *rtos)
 207 {
 208         /* wipe out previous thread details if any */
 209         /* do this first because rtos layer does not check our retval */
 210         rtos_free_threadlist(rtos);
 211         rtos->current_thread = 0;
 212
 213         if (!rtos->symbols) {
 214                 LOG_ERROR("No symbols for rt-kernel");
 215                 return -3;
 216         }
 217
 218         /* read the current task */
 219         uint32_t current_task;
 220         int retval = target_read_u32(rtos->target,
 221                         rtos->symbols[sym_os_state].address + rtos->symbols[sym___off_os_state2current].address,
 222                         &current_task);
 223         if (retval != ERROR_OK) {
 224                 LOG_ERROR("Error reading current task");
 225                 return retval;
 226         }
 227         LOG_DEBUG("current task is 0x%" PRIx32, current_task);
 228
 229         retval = rtkernel_verify_task(rtos, current_task);
 230         if (retval != ERROR_OK) {
 231                 LOG_ERROR("Current task is invalid");
 232                 return retval;
 233         }
 234
 235         /* loop through kernel task list */
 236         uint32_t chain = rtos->symbols[sym_os_state].address + rtos->symbols[sym___off_os_state2chain].address;
 237         LOG_DEBUG("chain start at 0x%" PRIx32, chain);
 238
 239         uint32_t next = chain;
 240         for (;;) {
 241                 retval = target_read_u32(rtos->target, next, &next);
 242                 if (retval != ERROR_OK) {
 243                         LOG_ERROR("Could not read rt-kernel data structure from target");
 244                         return retval;
 245                 }
 246                 LOG_DEBUG("next entry at 0x%" PRIx32, next);
 247                 if (next == chain) {
 248                         LOG_DEBUG("end of chain detected");
 249                         break;
 250                 }
 251                 uint32_t task = next - rtos->symbols[sym___off_task2chain].address;
 252                 LOG_DEBUG("found task at 0x%" PRIx32, task);
 253
 254                 retval = rtkernel_verify_task(rtos, task);
 255                 if (retval != ERROR_OK) {
 256                         LOG_ERROR("Invalid task found");
 257                         return retval;
 258                 }
 259
 260                 retval = rtkernel_add_task(rtos, task, current_task);
 261                 if (retval != ERROR_OK) {
 262                         LOG_ERROR("Could not add task to rtos system");
 263                         return retval;
 264                 }
 265         }
 266         return ERROR_OK;
 267 }
 268
 269 static int rtkernel_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 270                 struct rtos_reg **reg_list, int *num_regs)
 271 {
 272         uint32_t stack_ptr = 0;
 273
 274         if (!rtos)
 275                 return -1;
 276
 277         if (thread_id == 0)
 278                 return -2;
 279
 280         if (!rtos->rtos_specific_params)
 281                 return -1;
 282
 283         const struct rtkernel_params *param = rtos->rtos_specific_params;
 284
 285         /* Read the stack pointer */
 286         int retval = target_read_u32(rtos->target, thread_id + rtos->symbols[sym___off_task2stack].address, &stack_ptr);
 287         if (retval != ERROR_OK) {
 288                 LOG_ERROR("Error reading stack pointer from rtkernel thread");
 289                 return retval;
 290         }
 291         LOG_DEBUG("stack pointer at 0x%" PRIx64 ", value 0x%" PRIx32,
 292                         thread_id + rtos->symbols[sym___off_task2stack].address,
 293                         stack_ptr);
 294
 295         /* Adjust stack pointer to ignore non-standard BASEPRI register stacking */
 296         stack_ptr += 4;
 297
 298         /* Check for armv7m with *enabled* FPU, i.e. a Cortex M4F */
 299         bool cm4_fpu_enabled = false;
 300         struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target);
 301         if (is_armv7m(armv7m_target)) {
 302                 if (armv7m_target->fp_feature != FP_NONE) {
 303                         /* Found ARM v7m target which includes a FPU */
 304                         uint32_t cpacr;
 305
 306                         retval = target_read_u32(rtos->target, FPU_CPACR, &cpacr);
 307                         if (retval != ERROR_OK) {
 308                                 LOG_ERROR("Could not read CPACR register to check FPU state");
 309                                 return -1;
 310                         }
 311
 312                         /* Check if CP10 and CP11 are set to full access. */
 313                         if (cpacr & 0x00F00000) {
 314                                 /* Found target with enabled FPU */
 315                                 cm4_fpu_enabled = true;
 316                         }
 317                 }
 318         }
 319
 320         if (!cm4_fpu_enabled) {
 321                 LOG_DEBUG("cm3 stacking");
 322                 return rtos_generic_stack_read(rtos->target, param->stacking_info_cm3, stack_ptr, reg_list, num_regs);
 323         }
 324
 325         /* Read the LR to decide between stacking with or without FPU */
 326         uint32_t lr_svc;
 327         retval = target_read_u32(rtos->target, stack_ptr + 0x20, &lr_svc);
 328         if (retval != ERROR_OK) {
 329                 LOG_OUTPUT("Error reading stack frame from rtkernel thread\r\n");
 330                 return retval;
 331         }
 332
 333         if ((lr_svc & 0x10) == 0) {
 334                 LOG_DEBUG("cm4f_fpu stacking");
 335                 return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f_fpu, stack_ptr, reg_list, num_regs);
 336         }
 337
 338         LOG_DEBUG("cm4f stacking");
 339         return rtos_generic_stack_read(rtos->target, param->stacking_info_cm4f, stack_ptr, reg_list, num_regs);
 340 }
 341
 342 static int rtkernel_get_symbol_list_to_lookup(struct symbol_table_elem *symbol_list[])
 343 {
 344         *symbol_list = calloc(ARRAY_SIZE(rtkernel_symbol_list), sizeof(struct symbol_table_elem));
 345         if (!*symbol_list)
 346                 return ERROR_FAIL;
 347
 348         for (size_t i = 0; i < ARRAY_SIZE(rtkernel_symbol_list); i++) {
 349                 (*symbol_list)[i].symbol_name = rtkernel_symbol_list[i].name;
 350                 (*symbol_list)[i].optional = rtkernel_symbol_list[i].optional;
 351         }
 352
 353         return ERROR_OK;
 354 }
 355
 356 static bool rtkernel_detect_rtos(struct target *target)
 357 {
 358         return (target->rtos->symbols) &&
 359                         (target->rtos->symbols[sym___off_os_state2chain].address != 0);
 360 }
 361
 362 static int rtkernel_create(struct target *target)
 363 {
 364         for (size_t i = 0; i < ARRAY_SIZE(rtkernel_params_list); i++) {
 365                 if (strcmp(rtkernel_params_list[i].target_name, target_type_name(target)) == 0) {
 366                         target->rtos->rtos_specific_params = (void *)&rtkernel_params_list[i];
 367                         return 0;
 368                 }
 369         }
 370
 371         LOG_ERROR("Could not find target in rt-kernel compatibility list");
 372         return -1;
 373 }
 374
 375 const struct rtos_type rtkernel_rtos = {
 376         .name = "rtkernel",
 377
 378         .detect_rtos = rtkernel_detect_rtos,
 379         .create = rtkernel_create,
 380         .update_threads = rtkernel_update_threads,
 381         .get_thread_reg_list = rtkernel_get_thread_reg_list,
 382         .get_symbol_list_to_lookup = rtkernel_get_symbol_list_to_lookup,
 383 };