rtos: Freescale MQX rtos support

ARMv7E-M (CortexM4) architecture

- fix position offset of r2,r3 registers on exception stack
- switch 'calloc' arguments
- remove prototypes of internal function and typedefs
- add NULL check for alloc functions
- remove last line of license "Franklin Street, Fifth Floor"
  because of 'checkpatch' validation
- environment: jlink + twrk60n512

Change-Id: I70840ded15b17dd945ca190ce31e2775078da2d9
Signed-off-by: Marian Cingel <cingel.marian@gmail.com>
Reviewed-on: http://openocd.zylin.com/2353
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

5 files changed, 674 insertions, 3 deletions

diff --git a/src/rtos/Makefile.am b/src/rtos/Makefile.am
index 58923b8..62293ec 100644
--- a/src/rtos/Makefile.am
+++ b/src/rtos/Makefile.am
@@ -22,8 +22,8 @@ include $(top_srcdir)/common.mk
 
 METASOURCES = AUTO
 noinst_LTLIBRARIES = librtos.la
-noinst_HEADERS = rtos.h rtos_standard_stackings.h rtos_ecos_stackings.h linux_header.h rtos_chibios_stackings.h rtos_embkernel_stackings.h
-librtos_la_SOURCES = rtos.c rtos_standard_stackings.c rtos_ecos_stackings.c  rtos_chibios_stackings.c rtos_embkernel_stackings.c FreeRTOS.c ThreadX.c eCos.c linux.c ChibiOS.c embKernel.c
+noinst_HEADERS = rtos.h rtos_standard_stackings.h rtos_ecos_stackings.h linux_header.h rtos_chibios_stackings.h rtos_embkernel_stackings.h rtos_mqx_stackings.h
+librtos_la_SOURCES = rtos.c rtos_standard_stackings.c rtos_ecos_stackings.c  rtos_chibios_stackings.c rtos_embkernel_stackings.c rtos_mqx_stackings.c FreeRTOS.c ThreadX.c eCos.c linux.c ChibiOS.c embKernel.c mqx.c
 
 librtos_la_CFLAGS =
 if IS_MINGW
diff --git a/src/rtos/mqx.c b/src/rtos/mqx.c
new file mode 100644
index 0000000..bbb96b2
--- /dev/null
+++ b/src/rtos/mqx.c
@@ -0,0 +1,556 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Marian Cingel                                   *
+ *   cingel.marian@gmail.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.                                        *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdint.h>
+#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_mqx_stackings.h"
+
+/* constants */
+#define MQX_THREAD_NAME_LENGTH			(255)
+#define MQX_KERNEL_OFFSET_TDLIST		(0x0108)
+#define MQX_KERNEL_OFFSET_SYSTEM_TASK	(0x0050)
+#define MQX_KERNEL_OFFSET_ACTIVE_TASK	(0x001C)
+#define MQX_KERNEL_OFFSET_CAPABILITY	(0x0000)
+#define MQX_QUEUE_OFFSET_SIZE			(0x0008)
+#define MQX_TASK_OFFSET_STATE			(0x0008)
+#define MQX_TASK_OFFSET_ID				(0x000c)
+#define MQX_TASK_OFFSET_TEMPLATE		(0x0068)
+#define MQX_TASK_OFFSET_STACK			(0x0014)
+#define MQX_TASK_OFFSET_TDLIST			(0x006C)
+#define MQX_TASK_OFFSET_NEXT			(0x0000)
+#define MQX_TASK_TEMPLATE_OFFSET_NAME	(0x0010)
+#define MQX_TASK_OFFSET_ERROR_CODE		(0x005C)
+#define MQX_TASK_STATE_MASK				(0xFFF)
+
+/* types */
+enum mqx_symbols {
+	mqx_VAL_mqx_kernel_data,
+	mqx_VAL_MQX_init_struct,
+};
+
+enum mqx_arch {
+	mqx_arch_cortexm,
+};
+
+struct mqx_params {
+	const char *target_name;
+	const enum mqx_arch target_arch;
+	const struct rtos_register_stacking *stacking_info;
+};
+
+struct mqx_state {
+	uint32_t state;
+	char *name;
+};
+
+/* local data */
+static const struct mqx_state mqx_states[] = {
+	{ 0x0002, "READY" },
+	{ 0x0003, "BLOCKED" },
+	{ 0x0005, "RCV_SPECIFIC_BLOCKED" },
+	{ 0x0007, "RCV_ANY_BLOCKED" },
+	{ 0x0009, "DYING" },
+	{ 0x000B, "UNHANDLED_INT_BLOCKED" },
+	{ 0x000D, "SEND_BLOCKED" },
+	{ 0x000F, "BREAKPOINT_BLOCKED" },
+	{ 0x0211, "IO_BLOCKED" },
+	{ 0x0021, "SEM_BLOCKED" },
+	{ 0x0223, "MUTEX_BLOCKED" },
+	{ 0x0025, "EVENT_BLOCKED" },
+	{ 0x0229, "TASK_QUEUE_BLOCKED" },
+	{ 0x042B, "LWSEM_BLOCKED" },
+	{ 0x042D, "LWEVENT_BLOCKED" },
+};
+
+static const char * const mqx_symbol_list[] = {
+	"_mqx_kernel_data",
+	"MQX_init_struct",
+	NULL
+};
+
+static const struct mqx_params mqx_params_list[] = {
+	{ "cortex_m", mqx_arch_cortexm, &rtos_mqx_arm_v7m_stacking },
+};
+
+/*
+ * Perform simple address check to avoid bus fault.
+ */
+static int mqx_valid_address_check(
+	struct rtos *rtos,
+	uint32_t address
+)
+{
+	enum mqx_arch arch_type = ((struct mqx_params *)rtos->rtos_specific_params)->target_arch;
+	const char * targetname = ((struct mqx_params *)rtos->rtos_specific_params)->target_name;
+
+	/* Cortex M address range */
+	if (arch_type == mqx_arch_cortexm) {
+		if (
+			/* code and sram area */
+			(address && address <= 0x3FFFFFFFu) ||
+			/* external ram area*/
+			(address >= 0x6000000u && address <= 0x9FFFFFFFu)
+		) {
+			return ERROR_OK;
+		}
+		return ERROR_FAIL;
+	}
+	LOG_ERROR("MQX RTOS - unknown architecture %s", targetname);
+	return ERROR_FAIL;
+}
+
+/*
+ * Wrapper of 'target_read_buffer' fn.
+ * Include address check.
+ */
+static int mqx_target_read_buffer(
+	struct target *target,
+	uint32_t address,
+	uint32_t size,
+	uint8_t *buffer
+)
+{
+	int status = mqx_valid_address_check(target->rtos, address);
+	if (status != ERROR_OK) {
+		LOG_WARNING("MQX RTOS - target address 0x%X is not allowed to read", address);
+		return status;
+	}
+	status = target_read_buffer(target, address, size, buffer);
+	if (status != ERROR_OK) {
+		LOG_ERROR("MQX RTOS - reading target address 0x%X failed", address);
+		return status;
+	}
+	return ERROR_OK;
+}
+
+/*
+ * Get symbol address if present
+ */
+static int mqx_get_symbol(
+	struct rtos *rtos,
+	enum mqx_symbols symbol,
+	void *result
+)
+{
+	/* TODO: additional check ?? */
+	(*(int *)result) = (uint32_t)rtos->symbols[symbol].address;
+	return ERROR_OK;
+}
+
+/*
+ * Get value of struct member by passing
+ * member offset, width and name (debug purpose)
+ */
+static int mqx_get_member(
+	struct rtos *rtos,
+	const uint32_t base_address,
+	int32_t member_offset,
+	int32_t member_width,
+	const char *member_name,
+	void *result
+)
+{
+	int status = ERROR_FAIL;
+	status = mqx_target_read_buffer(
+		rtos->target, base_address + member_offset, member_width, result
+	);
+	if (status != ERROR_OK)
+		LOG_WARNING("MQX RTOS - cannot read \"%s\" at address 0x%X", member_name, base_address + member_offset);
+	return status;
+}
+
+/*
+ * Check whether scheduler started
+ */
+static int mqx_is_scheduler_running(
+	struct rtos *rtos
+)
+{
+	uint32_t kernel_data_symbol = 0;
+	uint32_t kernel_data_addr = 0;
+	uint32_t system_td_addr = 0;
+	uint32_t active_td_addr = 0;
+	uint32_t capability_value = 0;
+
+	/* get '_mqx_kernel_data' symbol */
+	if (ERROR_OK != mqx_get_symbol(
+		rtos, mqx_VAL_mqx_kernel_data, &kernel_data_symbol
+	)) {
+		return ERROR_FAIL;
+	}
+	/* get '_mqx_kernel_data' */
+	if (ERROR_OK != mqx_get_member(
+		rtos, kernel_data_symbol, 0, 4,
+		"_mqx_kernel_data", &kernel_data_addr
+	)) {
+		return ERROR_FAIL;
+	}
+	/* return if '_mqx_kernel_data' is NULL or default 0xFFFFFFFF */
+	if (0 == kernel_data_addr || (uint32_t)(-1) == kernel_data_addr)
+		return ERROR_FAIL;
+	/* get kernel_data->ADDRESSING_CAPABILITY */
+	if (ERROR_OK != mqx_get_member(
+		rtos, kernel_data_addr, MQX_KERNEL_OFFSET_CAPABILITY, 4,
+		"kernel_data->ADDRESSING_CAPABILITY", (void *)&capability_value
+	)) {
+		return ERROR_FAIL;
+	}
+	/* check first member, the '_mqx_kernel_data->ADDRESSING_CAPABILITY'.
+	   it supose to be set to value 8 */
+	if (capability_value != 8) {
+		LOG_WARNING("MQX RTOS - value of '_mqx_kernel_data->ADDRESSING_CAPABILITY' contains invalid value");
+		return ERROR_FAIL;
+	}
+	/* get active ptr */
+	if (ERROR_OK != mqx_get_member(
+		rtos, kernel_data_addr, MQX_KERNEL_OFFSET_ACTIVE_TASK, 4,
+		"kernel_data->ACTIVE_PTR", (void *)&active_td_addr
+	)) {
+		return ERROR_FAIL;
+	}
+	/* active task is system task, scheduler has not not run yet */
+	system_td_addr = kernel_data_addr + MQX_KERNEL_OFFSET_SYSTEM_TASK;
+	if (active_td_addr == system_td_addr) {
+		LOG_WARNING("MQX RTOS - scheduler does not run");
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
+/*
+ * API function, return 1 if MQX is present
+ */
+static int mqx_detect_rtos(
+	struct target *target
+)
+{
+	if (
+		(target->rtos->symbols != NULL) &&
+		(target->rtos->symbols[mqx_VAL_mqx_kernel_data].address != 0)
+	) {
+		return 1;
+	}
+	return 0;
+}
+
+/*
+ * API function, pass MQX extra info to context data
+ */
+static int mqx_create(
+	struct target *target
+)
+{
+	/* check target name against supported architectures */
+	int mqx_params_list_num = (sizeof(mqx_params_list)/sizeof(struct mqx_params));
+	for (int i = 0; i < mqx_params_list_num; i++) {
+		if (0 == strcmp(mqx_params_list[i].target_name, target->type->name)) {
+			target->rtos->rtos_specific_params = (void *)&mqx_params_list[i];
+			/* LOG_DEBUG("MQX RTOS - valid architecture: %s", target->type->name); */
+			return 0;
+		}
+	}
+	LOG_ERROR("MQX RTOS - could not find target \"%s\" in MQX compatibility list", target->type->name);
+	return -1;
+}
+
+/*
+ * API function, update list of threads
+ */
+static int mqx_update_threads(
+	struct rtos *rtos
+)
+{
+	uint32_t task_queue_addr = 0;
+	uint32_t kernel_data_addr = 0;
+	uint16_t task_queue_size = 0;
+	uint32_t active_td_addr = 0;
+
+	/* clear old data */
+	rtos_free_threadlist(rtos);
+	/* check scheduler */
+	if (ERROR_OK != mqx_is_scheduler_running(rtos))
+		return ERROR_FAIL;
+	/* get kernel_data symbol */
+	if (ERROR_OK != mqx_get_symbol(
+		rtos, mqx_VAL_mqx_kernel_data, &kernel_data_addr
+	)) {
+		return ERROR_FAIL;
+	}
+	/* read kernel_data */
+	if (ERROR_OK != mqx_get_member(
+		rtos, kernel_data_addr, 0, 4, "_mqx_kernel_data", &kernel_data_addr
+	)) {
+		return ERROR_FAIL;
+	}
+	/* get task queue address */
+	task_queue_addr = kernel_data_addr + MQX_KERNEL_OFFSET_TDLIST;
+	/* get task queue size */
+	if (ERROR_OK != mqx_get_member(
+		rtos, task_queue_addr, MQX_QUEUE_OFFSET_SIZE, 2,
+		"kernel_data->TD_LIST.SIZE", &task_queue_size
+	)) {
+		return ERROR_FAIL;
+	}
+	/* get active ptr */
+	if (ERROR_OK != mqx_get_member(
+		rtos, kernel_data_addr, MQX_KERNEL_OFFSET_ACTIVE_TASK, 4,
+		"kernel_data->ACTIVE_PTR", (void *)&active_td_addr
+	)) {
+		return ERROR_FAIL;
+	}
+
+	/* setup threads info */
+	rtos->thread_count = task_queue_size;
+	rtos->current_thread = 0;
+	rtos->thread_details = calloc(rtos->thread_count, sizeof(struct thread_detail));
+	if (NULL == rtos->thread_details)
+		return ERROR_FAIL;
+
+	/*	loop over each task and setup thread details,
+		the current_taskpool_addr is set to queue head
+		NOTE: debugging functions task create/destroy
+		might cause to show invalid data.
+	*/
+	for (
+		uint32_t i = 0, taskpool_addr = task_queue_addr;
+		i < (uint32_t)rtos->thread_count;
+		i++
+	) {
+		uint8_t task_name[MQX_THREAD_NAME_LENGTH + 1];
+		uint32_t task_addr = 0, task_template = 0, task_state = 0;
+		uint32_t task_name_addr = 0, task_id = 0, task_errno = 0;
+		uint32_t state_index = 0, state_max = 0;
+		uint32_t extra_info_length = 0;
+		char *state_name = "unknown state";
+
+		/* set current taskpool address */
+		if (ERROR_OK != mqx_get_member(
+			rtos, taskpool_addr, MQX_TASK_OFFSET_NEXT, 4,
+			"td_struct_ptr->NEXT", &taskpool_addr
+		)) {
+			return ERROR_FAIL;
+		}
+		/* get task address from taskpool */
+		task_addr = taskpool_addr - MQX_TASK_OFFSET_TDLIST;
+		/* get address of 'td_struct_ptr->TEMPLATE_LIST_PTR' */
+		if (ERROR_OK != mqx_get_member(
+			rtos, task_addr, MQX_TASK_OFFSET_TEMPLATE, 4,
+			"td_struct_ptr->TEMPLATE_LIST_PTR", &task_template
+		)) {
+			return ERROR_FAIL;
+		}
+		/* get address of 'td_struct_ptr->TEMPLATE_LIST_PTR->NAME' */
+		if (ERROR_OK != mqx_get_member(
+			rtos, task_template, MQX_TASK_TEMPLATE_OFFSET_NAME, 4,
+			"td_struct_ptr->TEMPLATE_LIST_PTR->NAME", &task_name_addr
+		)) {
+			return ERROR_FAIL;
+		}
+		/* get value of 'td_struct->TEMPLATE_LIST_PTR->NAME' */
+		if (ERROR_OK != mqx_get_member(
+			rtos, task_name_addr, 0, MQX_THREAD_NAME_LENGTH,
+			"*td_struct_ptr->TEMPLATE_LIST_PTR->NAME", task_name
+		)) {
+			return ERROR_FAIL;
+		}
+		/* always terminate last character by force,
+		   otherwise openocd might fail if task_name
+		   has corrupted data */
+		task_name[MQX_THREAD_NAME_LENGTH] = '\0';
+		/* get value of 'td_struct_ptr->TASK_ID' */
+		if (ERROR_OK != mqx_get_member(
+			rtos, task_addr, MQX_TASK_OFFSET_ID, 4,
+			"td_struct_ptr->TASK_ID", &task_id
+		)) {
+			return ERROR_FAIL;
+		}
+		/* get task errno */
+		if (ERROR_OK != mqx_get_member(
+			rtos, task_addr, MQX_TASK_OFFSET_ERROR_CODE, 4,
+			"td_struct_ptr->TASK_ERROR_CODE", &task_errno
+		)) {
+			return ERROR_FAIL;
+		}
+		/* get value of 'td_struct_ptr->STATE' */
+		if (ERROR_OK != mqx_get_member(
+			rtos, task_addr, MQX_TASK_OFFSET_STATE, 4,
+			"td_struct_ptr->STATE", &task_state
+		)) {
+			return ERROR_FAIL;
+		}
+		task_state &= MQX_TASK_STATE_MASK;
+		/* and search for defined state */
+		state_max = (sizeof(mqx_states)/sizeof(struct mqx_state));
+		for (state_index = 0; (state_index < state_max); state_index++) {
+			if (mqx_states[state_index].state == task_state) {
+				state_name = mqx_states[state_index].name;
+				break;
+			}
+		}
+
+		/* setup thread details struct */
+		rtos->thread_details[i].threadid = task_id;
+		rtos->thread_details[i].exists = true;
+		rtos->thread_details[i].display_str = NULL;
+		/* set thread name */
+		rtos->thread_details[i].thread_name_str = malloc(strlen((void *)task_name) + 1);
+		if (NULL == rtos->thread_details[i].thread_name_str)
+			return ERROR_FAIL;
+		strcpy(rtos->thread_details[i].thread_name_str, (void *)task_name);
+		/* set thread extra info
+		 * - task state
+		 * - task address
+		 * - task errno
+		 * calculate length as:
+		 * state length + address length + errno length + formatter length
+		 */
+		extra_info_length += strlen((void *)state_name) + 8 + 8 + 8;
+		rtos->thread_details[i].extra_info_str = malloc(extra_info_length + 1);
+		if (NULL == rtos->thread_details[i].extra_info_str)
+			return ERROR_FAIL;
+		snprintf(
+			rtos->thread_details[i].extra_info_str, extra_info_length, "%s : 0x%x : %u",
+			state_name, task_addr, task_errno
+		);
+		/* set active thread */
+		if (active_td_addr == task_addr)
+			rtos->current_thread = task_id;
+	}
+	return ERROR_OK;
+}
+
+/*
+ * API function, get info of selected thread
+ */
+static int mqx_get_thread_reg_list(
+	struct rtos *rtos,
+	int64_t thread_id,
+	char **hex_reg_list
+)
+{
+	int64_t stack_ptr = 0;
+	uint32_t my_task_addr = 0;
+	uint32_t task_queue_addr = 0;
+	uint32_t task_queue_size = 0;
+	uint32_t kernel_data_addr = 0;
+
+	*hex_reg_list = NULL;
+	if (thread_id == 0) {
+		LOG_ERROR("MQX RTOS - invalid threadid: 0x%X", (int)thread_id);
+		return ERROR_FAIL;
+	}
+	if (ERROR_OK != mqx_is_scheduler_running(rtos))
+		return ERROR_FAIL;
+	/* get kernel_data symbol */
+	if (ERROR_OK != mqx_get_symbol(
+		rtos, mqx_VAL_mqx_kernel_data, &kernel_data_addr
+	)) {
+		return ERROR_FAIL;
+	}
+	/* read kernel_data */
+	if (ERROR_OK != mqx_get_member(
+		rtos, kernel_data_addr, 0, 4, "_mqx_kernel_data", &kernel_data_addr
+	)) {
+		return ERROR_FAIL;
+	}
+	/* get task queue address */
+	task_queue_addr = kernel_data_addr + MQX_KERNEL_OFFSET_TDLIST;
+	/* get task queue size */
+	if (ERROR_OK != mqx_get_member(
+		rtos, task_queue_addr, MQX_QUEUE_OFFSET_SIZE, 2,
+		"kernel_data->TD_LIST.SIZE", &task_queue_size
+	)) {
+		return ERROR_FAIL;
+	}
+	/* search for taskid */
+	for (
+		uint32_t i = 0, taskpool_addr = task_queue_addr;
+		i < (uint32_t)rtos->thread_count;
+		i++
+	) {
+		uint32_t tmp_address = 0, task_addr = 0;
+		uint32_t task_id = 0;
+		/* set current taskpool address */
+		tmp_address = taskpool_addr;
+		if (ERROR_OK != mqx_get_member(
+			rtos, tmp_address, MQX_TASK_OFFSET_NEXT, 4,
+			"td_struct_ptr->NEXT", &taskpool_addr
+		)) {
+			return ERROR_FAIL;
+		}
+		/* get task address from taskpool */
+		task_addr = taskpool_addr - MQX_TASK_OFFSET_TDLIST;
+		/* get value of td_struct->TASK_ID */
+		if (ERROR_OK != mqx_get_member(
+			rtos, task_addr, MQX_TASK_OFFSET_ID, 4,
+			"td_struct_ptr->TASK_ID", &task_id
+		)) {
+			return ERROR_FAIL;
+		}
+		/* found taskid, break */
+		if (task_id == thread_id) {
+			my_task_addr = task_addr;
+			break;
+		}
+	}
+	if (!my_task_addr) {
+		LOG_ERROR("MQX_RTOS - threadid %" PRId64 " does not match any task", thread_id);
+		return ERROR_FAIL;
+	}
+	/* get task stack head address */
+	if (ERROR_OK != mqx_get_member(
+		rtos, my_task_addr, MQX_TASK_OFFSET_STACK, 4, "task->STACK_PTR", &stack_ptr
+	)) {
+		return ERROR_FAIL;
+	}
+	return rtos_generic_stack_read(
+		rtos->target, ((struct mqx_params *)rtos->rtos_specific_params)->stacking_info, stack_ptr, hex_reg_list
+	);
+}
+
+/* API function, export list of required symbols */
+static int mqx_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
+{
+	*symbol_list = malloc(sizeof(symbol_table_elem_t) * ARRAY_SIZE(mqx_symbol_list));
+	if (NULL == *symbol_list)
+		return ERROR_FAIL;
+	/* export required symbols */
+	for (int i = 0; i < (int)(ARRAY_SIZE(mqx_symbol_list)); i++)
+		(*symbol_list)[i].symbol_name = mqx_symbol_list[i];
+	return ERROR_OK;
+}
+
+struct rtos_type mqx_rtos = {
+	.name = "mqx",
+	.detect_rtos = mqx_detect_rtos,
+	.create = mqx_create,
+	.update_threads = mqx_update_threads,
+	.get_thread_reg_list = mqx_get_thread_reg_list,
+	.get_symbol_list_to_lookup = mqx_get_symbol_list_to_lookup,
+};
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index d09e110..f14e538 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -35,6 +35,7 @@ extern struct rtos_type eCos_rtos;
 extern struct rtos_type Linux_os;
 extern struct rtos_type ChibiOS_rtos;
 extern struct rtos_type embKernel_rtos;
+extern struct rtos_type mqx_rtos;
 
 static struct rtos_type *rtos_types[] = {
 	&ThreadX_rtos,
@@ -43,6 +44,7 @@ static struct rtos_type *rtos_types[] = {
 	&Linux_os,
 	&ChibiOS_rtos,
 	&embKernel_rtos,
+	&mqx_rtos,
 	NULL
 };
 
@@ -303,7 +305,7 @@ int rtos_thread_packet(struct connection *connection, char const *packet, int pa
 			if (detail->extra_info_str != NULL)
 				str_size += strlen(detail->extra_info_str);
 
-			char *tmp_str = malloc(str_size + 7);
+			char *tmp_str = calloc(str_size + 7, sizeof(char));
 			char *tmp_str_ptr = tmp_str;
 
 			if (detail->display_str != NULL)
diff --git a/src/rtos/rtos_mqx_stackings.c b/src/rtos/rtos_mqx_stackings.c
new file mode 100644
index 0000000..8f2d67e
--- /dev/null
+++ b/src/rtos/rtos_mqx_stackings.c
@@ -0,0 +1,81 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Marian Cingel                                   *
+ *   cingel.marian@gmail.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.                                        *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "rtos.h"
+#include "target/armv7m.h"
+
+
+/*
+ * standard exception stack
+ * ( stack base, higher memory address )
+ * - xpsr		- 0x48
+ * - pc			- 0x44
+ * - lr			- 0x40
+ * - r12		- 0x3C
+ * - r3			- 0x38
+ * - r2			- 0x34
+ * - r1			- 0x30
+ * - r0			- 0x2C
+ * extended stack in svc_pending handler
+ * - lr			- 0x28
+ * - r11		- 0x24
+ * - r10		- 0x20
+ * - r9			- 0x1C
+ * - r8			- 0x18
+ * - r7			- 0x14
+ * - r6			- 0x10
+ * - r5			- 0x0C
+ * - r4			- 0x08
+ * - BASEPRI	- 0x04
+ * - SHPR3		- 0x00 ( contains pend_svc exception priority )
+ * ( stack head, lower address, stored in 'task->STACK_PTR' )
+ */
+
+static const struct stack_register_offset rtos_mqx_arm_v7m_stack_offsets[ARMV7M_NUM_CORE_REGS] = {
+	{ 0x2C, 32 }, /* r0   */
+	{ 0x30, 32 }, /* r1   */
+	{ 0x34, 32 }, /* r2   */
+	{ 0x38, 32 }, /* r3   */
+	{ 0x08, 32 }, /* r4   */
+	{ 0x0C, 32 }, /* r5   */
+	{ 0x10, 32 }, /* r6   */
+	{ 0x14, 32 }, /* r7   */
+	{ 0x18, 32 }, /* r8   */
+	{ 0x1C, 32 }, /* r9   */
+	{ 0x20, 32 }, /* r10  */
+	{ 0x24, 32 }, /* r11  */
+	{ 0x3C, 32 }, /* r12  */
+	{  -2 , 32 }, /* sp   */
+	{ 0x28, 32 }, /* lr   */
+	{ 0x44, 32 }, /* pc   */
+	{ 0x48, 32 }, /* xPSR */
+};
+
+const struct rtos_register_stacking rtos_mqx_arm_v7m_stacking = {
+	0x4C,					/* stack_registers_size, calculate offset base address */
+	-1,						/* stack_growth_direction */
+	ARMV7M_NUM_CORE_REGS,	/* num_output_registers */
+	0,						/* stack_alignment */
+	rtos_mqx_arm_v7m_stack_offsets	/* register_offsets */
+};
+
diff --git a/src/rtos/rtos_mqx_stackings.h b/src/rtos/rtos_mqx_stackings.h
new file mode 100644
index 0000000..9536c40
--- /dev/null
+++ b/src/rtos/rtos_mqx_stackings.h
@@ -0,0 +1,32 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Marian Cingel                                   *
+ *   cingel.marian@gmail.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.                                        *
+ ***************************************************************************/
+
+#ifndef INCLUDED_RTOS_MQX_STACKINGS_H_
+#define INCLUDED_RTOS_MQX_STACKINGS_H_
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "rtos.h"
+
+extern const struct rtos_register_stacking rtos_mqx_arm_v7m_stacking;
+
+#endif	/* ifndef INCLUDED_RTOS_MQX_STACKINGS_H_ */
+