update for current ChibiOS version

libCrashDebug/src/mriPlatform.c

@@ -144,6 +144,7 @@ static const char g_targetFpuXML[] =
 
 
 static RegisterContext* g_pContext;
+static RegisterContext  g_crashContext;
 static IMemory*         g_pMemory;
 static IComm*           g_pComm;
 static char             g_packetBuffer[16 * 1024];
@@ -171,6 +172,7 @@ static void readBytesFromBufferAsHex(Buffer* pBuffer, void* pBytes, size_t byteC
 __throws void mriPlatform_Init(RegisterContext* pContext, IMemory* pMem)
 {
     g_pContext = pContext;
+    g_crashContext = *pContext;
     g_pMemory = pMem;
     g_memoryFaultEncountered = FALSE;
 
@@ -728,16 +730,20 @@ struct symbols_t {
 };
 
 static struct symbols_t chibios_symbol_list[] = {
-    {"ch", 0},       /* System data structure */
-    {"ch_debug", 0}, /* Memory Signature containing offsets of fields in rlist */
+    {"ch", 0},        /* Legacy system data structure. */
+    {"ch_system", 0}, /* Current system data structure. */
+    {"ch0", 0},       /* Current uniprocessor OS instance. */
+    {"ch_debug", 0},  /* Memory signature containing structure offsets. */
 };
 #define CHIBIOS_VAL_CH 0
-#define CHIBIOS_VAL_CH_DEBUG 1
+#define CHIBIOS_VAL_CH_SYSTEM 1
+#define CHIBIOS_VAL_CH0 2
+#define CHIBIOS_VAL_CH_DEBUG 3
 
 static uint8_t curr_symbol = 0;
 int __mriPlatform_SetSymbolRequest(Buffer* pBuffer)
 {
-    if (curr_symbol >= 2) // based on number of vars in chibios_symbol_list
+    if (curr_symbol >= ARRAY_SIZE(chibios_symbol_list))
         return FALSE;
     uint8_t i;
     for (i=0; i<strlen(chibios_symbol_list[curr_symbol].name); i++)
@@ -747,14 +753,14 @@ int __mriPlatform_SetSymbolRequest(Buffer* pBuffer)
 
 void __mriPlatform_SetSymbolAddress(Buffer* pBuffer, uint32_t address)
 {
-    if (curr_symbol >= 2)
+    if (curr_symbol >= ARRAY_SIZE(chibios_symbol_list))
         return;
     char symbolname[20] = {0};
     uint8_t i = 0;
-    while (!Buffer_IsNextCharEqualTo(pBuffer, ':') && i < sizeof(symbolname)) {
+    while (!Buffer_IsNextCharEqualTo(pBuffer, ':') && i < sizeof(symbolname) - 1) {
         symbolname[i++] = Buffer_ReadByteAsHex(pBuffer);
     }
-    symbolname[i++] = '\0';
+    symbolname[i] = '\0';
     if (strcmp(symbolname, chibios_symbol_list[curr_symbol].name) == 0) {
         chibios_symbol_list[curr_symbol].value = address;
         curr_symbol++;
@@ -918,6 +924,21 @@ struct chibios_chdebug {
     uint8_t   cf_off_refs;            
     uint8_t   cf_off_preempt;         
     uint8_t   cf_off_time;            
+    uint8_t   ch_off_reserved[4];
+    uint8_t   ch_intctxsize;
+    uint8_t   ch_intervalsize;
+    uint8_t   ch_instancesnum;
+    uint8_t   cf_off_sys_state;
+    uint8_t   cf_off_sys_instances;
+    uint8_t   cf_off_sys_reglist;
+    uint8_t   cf_off_sys_rfcu;
+    uint8_t   cf_off_sys_reserved[4];
+    uint8_t   cf_off_inst_rlist_current;
+    uint8_t   cf_off_inst_rlist;
+    uint8_t   cf_off_inst_vtlist;
+    uint8_t   cf_off_inst_reglist;
+    uint8_t   cf_off_inst_core_id;
+    uint8_t   cf_off_inst_rfcu;
 };
 
 #define GET_CH_KERNEL_MAJOR(coded_version) ((coded_version >> 11) & 0x1f)
@@ -951,6 +972,61 @@ uint32_t current_thread = 0;
 /* Offset of the rlist structure within the system data structure (ch) */
 #define CH_RLIST_OFFSET 0x00
 
+#define CHIBIOS_LEGACY_SIGNATURE_SIZE offsetof(struct chibios_chdebug, ch_off_reserved)
+#define CHIBIOS_CURRENT_SIGNATURE_SIZE (offsetof(struct chibios_chdebug, cf_off_inst_rfcu) + 1)
+
+static struct stack_register_offset rtos_chibios_current_stack_offsets[TOTAL_REG_COUNT-2] = {
+    { R0,   -1, 32 },
+    { R1,   -1, 32 },
+    { R2,   -1, 32 },
+    { R3,   -1, 32 },
+    { R4,    0, 32 },
+    { R5,    0, 32 },
+    { R6,    0, 32 },
+    { R7,    0, 32 },
+    { R8,    0, 32 },
+    { R9,    0, 32 },
+    { R10,   0, 32 },
+    { R11,   0, 32 },
+    { R12,  -1, 32 },
+    { SP,   -2, 32 },
+    { LR,    0, 32 },
+    { PC,    0, 32 },
+    { XPSR, -1, 32 },
+};
+
+static struct rtos_register_stacking rtos_chibios_current_stacking = {
+    0,
+    -1,
+    TOTAL_REG_COUNT-2,
+    rtos_chibios_current_stack_offsets
+};
+
+static int chibios_is_current_signature(const struct chibios_chdebug *signature)
+{
+    return signature->ch_size >= CHIBIOS_CURRENT_SIGNATURE_SIZE;
+}
+
+static int chibios_configure_current_stacking(const struct chibios_chdebug *signature)
+{
+    const uint8_t core_registers_size = 9 * sizeof(uint32_t);
+    if (signature->ch_intctxsize < core_registers_size) {
+        WriteStringToGdbConsole("ChibiOS saved context is smaller than expected\n");
+        return FALSE;
+    }
+
+    const uint8_t core_offset = signature->ch_intctxsize - core_registers_size;
+    int i;
+    for (i = R4; i <= R11; i++) {
+        rtos_chibios_current_stack_offsets[i].offset = core_offset + (i - R4) * sizeof(uint32_t);
+    }
+    rtos_chibios_current_stack_offsets[LR].offset = signature->ch_intctxsize - sizeof(uint32_t);
+    rtos_chibios_current_stack_offsets[PC].offset = signature->ch_intctxsize - sizeof(uint32_t);
+    rtos_chibios_current_stacking.stack_registers_size = signature->ch_intctxsize;
+    stacking_info = &rtos_chibios_current_stacking;
+    return TRUE;
+}
+
 static int chibios_update_memory_signature()
 {
     struct chibios_chdebug *signature;
@@ -964,14 +1040,15 @@ static int chibios_update_memory_signature()
         return FALSE;
     }
 
-    read_address_to_mem(signature, chibios_symbol_list[CHIBIOS_VAL_CH_DEBUG].value, sizeof(*signature));
+    const uint32_t signature_address = chibios_symbol_list[CHIBIOS_VAL_CH_DEBUG].value;
+    read_address_to_mem(signature, signature_address, CHIBIOS_LEGACY_SIGNATURE_SIZE);
 
     if (strncmp(signature->ch_identifier, "main", 4) != 0) {
         WriteStringToGdbConsole("Memory signature identifier does not contain magic bytes.\n");
         goto errfree;
     }
 
-    if (signature->ch_size < sizeof(*signature)) {
+    if (signature->ch_size < CHIBIOS_LEGACY_SIGNATURE_SIZE) {
         WriteStringToGdbConsole("ChibiOS/RT memory signature claims to be smaller "
                 "than expected\n");
         goto errfree;
@@ -982,6 +1059,14 @@ static int chibios_update_memory_signature()
                     " expected. Assuming compatibility...\n");
     }
 
+    if (signature->ch_size > CHIBIOS_LEGACY_SIGNATURE_SIZE) {
+        uint32_t signature_size = signature->ch_size;
+        if (signature_size > sizeof(*signature)) {
+            signature_size = sizeof(*signature);
+        }
+        read_address_to_mem(signature, signature_address, signature_size);
+    }
+
     // WriteStringToGdbConsole("Successfully loaded memory map of ChibiOS/RT target \n");
 
     /* Currently, we have the inherent assumption that all address pointers
@@ -1045,20 +1130,67 @@ static int chibios_update_stacking()
 
     /* Found ARM v7m target with no or disabled FPU */
     stacking_info = &rtos_chibios_arm_v7m_stacking;
+	return TRUE;
+}
+
+static int chibios_update_current_threads(const struct chibios_chdebug *signature)
+{
+    const uint32_t system = chibios_symbol_list[CHIBIOS_VAL_CH_SYSTEM].value;
+    uint32_t instance = chibios_symbol_list[CHIBIOS_VAL_CH0].value;
+    if (system != 0) {
+        instance = Platform_MemRead32((void*)(uint64_t)(system + signature->cf_off_sys_instances));
+    }
+    if (instance == 0) {
+        WriteStringToGdbConsole("Could not find a ChibiOS OS instance\n");
+        return FALSE;
+    }
+
+    uint32_t registry;
+    if (signature->cf_off_sys_reglist != 0) {
+        if (system == 0) {
+            WriteStringToGdbConsole("Could not find the ChibiOS system registry\n");
+            return FALSE;
+        }
+        registry = system + signature->cf_off_sys_reglist;
+    } else {
+        registry = instance + signature->cf_off_inst_reglist;
+    }
+
+    memset(thread_details, 0, sizeof(thread_details));
+    thread_count = 0;
+    uint32_t previous_node = registry;
+    uint32_t node = Platform_MemRead32((void*)(uint64_t)registry);
+    while (node != registry) {
+        if ((node == 0) || (thread_count >= ARRAY_SIZE(thread_details))) {
+            WriteStringToGdbConsole("ChibiOS registry integrity check failed\n");
+            return FALSE;
+        }
+
+        const uint32_t thread = node - signature->cf_off_newer;
+        const uint32_t older_node = Platform_MemRead32(
+            (void*)(uint64_t)(thread + signature->cf_off_older));
+        if (older_node != previous_node) {
+            WriteStringToGdbConsole("ChibiOS registry integrity check failed, "
+                                    "double linked list violation\n");
+            return FALSE;
+        }
+
+        thread_details[thread_count].threadid = thread;
+        thread_details[thread_count].exists = 1;
+        thread_count++;
+        previous_node = node;
+        node = Platform_MemRead32((void*)(uint64_t)(thread + signature->cf_off_newer));
+    }
+
+    current_thread = Platform_MemRead32(
+        (void*)(uint64_t)(instance + signature->cf_off_inst_rlist_current));
     return TRUE;
 }
 
 static int chibios_update_threads()
 {
     int tasks_found = 0;
-    int symbols = TRUE;
-    uint8_t i;
-    for (i=0; i<ARRAY_SIZE(chibios_symbol_list); i++) {
-        if (chibios_symbol_list[i].value == 0) {
-            symbols=FALSE;
-        }
-    }
-    if (!symbols) {
+    if (chibios_symbol_list[CHIBIOS_VAL_CH_DEBUG].value == 0) {
         WriteStringToGdbConsole("No symbols for ChibiOS\n");
         return FALSE;
     }
@@ -1071,6 +1203,20 @@ static int chibios_update_threads()
         }
     }
 
+    if (chibios_is_current_signature(chibios_signature)) {
+        if ((chibios_symbol_list[CHIBIOS_VAL_CH_SYSTEM].value == 0) &&
+            (chibios_symbol_list[CHIBIOS_VAL_CH0].value == 0)) {
+            WriteStringToGdbConsole("No system symbols for current ChibiOS\n");
+            return FALSE;
+        }
+        if (!chibios_update_current_threads(chibios_signature)) {
+            return FALSE;
+        }
+    } else if (chibios_symbol_list[CHIBIOS_VAL_CH].value == 0) {
+        WriteStringToGdbConsole("No system symbol for legacy ChibiOS\n");
+        return FALSE;
+    } else {
+
     /* ChibiOS does not save the current thread count. We have to first
      * parse the double linked thread list to check for errors and the number of
      * threads. */
@@ -1179,6 +1325,47 @@ static int chibios_update_threads()
     current_thrd = Platform_MemRead32((void*)(uint64_t)(rlist + signature->cf_off_name));
 
     current_thread = current_thrd;
+    }
+
+    if (chibios_is_current_signature(chibios_signature)) {
+        size_t thd_idx;
+        for (thd_idx = 0; thd_idx < thread_count; thd_idx++) {
+            const uint32_t thread = thread_details[thd_idx].threadid;
+            uint32_t name_ptr = Platform_MemRead32(
+                (void*)(uint64_t)(thread + chibios_signature->cf_off_name));
+            char tmp_str[CHIBIOS_THREAD_NAME_STR_SIZE] = {0};
+            if (name_ptr != 0) {
+                read_address_to_mem(tmp_str, name_ptr, sizeof(tmp_str) - 1);
+            }
+            if (tmp_str[0] == '\0') {
+                strcpy(tmp_str, "No Name");
+            }
+            strcpy(thread_details[thd_idx].thread_name_str, tmp_str);
+
+            const uint8_t thread_state = Platform_MemRead8(
+                (void*)(uint64_t)(thread + chibios_signature->cf_off_state));
+            const char *state_desc = "Unknown";
+            if (thread_state < CHIBIOS_NUM_STATES) {
+                state_desc = chibios_thread_states[thread_state];
+            }
+            snprintf(thread_details[thd_idx].extra_info_str,
+                     sizeof(thread_details[thd_idx].extra_info_str),
+                     "Name: %s State: %s", tmp_str, state_desc);
+        }
+    }
+
+    /* GDB associates the initial stop registers with the first reported
+     * thread. Keep the ChibiOS current thread first so that association is
+     * correct before GDB starts requesting per-thread register contexts. */
+    size_t thd_idx;
+    for (thd_idx = 1; thd_idx < thread_count; thd_idx++) {
+        if (thread_details[thd_idx].threadid == current_thread) {
+            const struct thread_detail current_detail = thread_details[thd_idx];
+            thread_details[thd_idx] = thread_details[0];
+            thread_details[0] = current_detail;
+            break;
+        }
+    }
 
     return TRUE;
 }
@@ -1223,11 +1410,23 @@ int Platform_RtosGetThreadContext(Buffer* pBuffer, uint32_t thread_id)
     if ((thread_id == 0) || !chibios_signature)
         return FALSE;
 
+    if (thread_id == current_thread) {
+        *g_pContext = g_crashContext;
+        return TRUE;
+    }
+
+    *g_pContext = g_crashContext;
+
     /* Update stacking if it can only be determined from runtime information */
-    if ((stacking_info == 0) &&
-        (!chibios_update_stacking())) {
-        WriteStringToGdbConsole("Failed to determine exact stacking\n");
-        return FALSE;
+    if (stacking_info == 0) {
+        if (chibios_is_current_signature(chibios_signature)) {
+            if (!chibios_configure_current_stacking(chibios_signature)) {
+                return FALSE;
+            }
+        } else if (!chibios_update_stacking()) {
+            WriteStringToGdbConsole("Failed to determine exact stacking\n");
+            return FALSE;
+        }
     }
 
     /* Read the stack pointer */