FreeRTOS port with openocd RTOS option

dumarjo wrote on Wednesday, January 13, 2016:

HI,

We have worked with openOCD team to get the RTOS option of openOCD working with FreeRTOS for cortex-M MCU. We need to apply some modifications to get this work when FPU and/OR MPU is used.

The standard cortex-M3 or M4 is working properly with openOCD.

The problem is that openOCD cannot known if the port of freeRTOS is using MPU or FPU option. Without this knowledge, openOCD return a standard stack frame starting from the saved stack pointer on the taskTCB. This give false information in the FPU port since freeRTOS push on the stack the FPU register before saving the stack pointer in the TaskTCB.

What we did, we recreated the standard stack frame and save the stack pointer in the taskTCB and after that we push LR and if the FPU bit is set/clear/ we push the S16 to S31 register. This way on the FPU port we have the “standard stack frame” and openOCD continue to work with the FPU port. With this modification we can have gdb display task with all there calling stack.

Here the modification that we have made to the cortex-M4F port

portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters )
{
	/* Simulate the stack frame as it would be created by a context switch
	interrupt. */

	/* Offset added to account for the way the MCU uses the stack on entry/exit
	of interrupts, and to ensure alignment. */
	pxTopOfStack--;

	*pxTopOfStack = portINITIAL_XPSR;	/* xPSR */
	pxTopOfStack--;
	*pxTopOfStack = ( portSTACK_TYPE ) pxCode;	/* PC */
	pxTopOfStack--;
	*pxTopOfStack = 0xDEEDBEEF;	/* LR */

	/* Save code space by skipping register initialisation. */
	pxTopOfStack -= 5;	/* R12, R3, R2 and R1. */
	*pxTopOfStack = ( portSTACK_TYPE ) pvParameters;	/* R0 */

	pxTopOfStack -= 8;	/* R11, R10, R9, R8, R7, R6, R5 and R4. */

	/* A save method is being used that requires each task to maintain its
	own exec return value. */
	pxTopOfStack--;
	*pxTopOfStack = portINITIAL_EXEC_RETURN;

	pxTopOfStack++;

	return pxTopOfStack;
}
void vPortSVCHandler( void )
{
	__asm volatile (
					"	ldr	r3, pxCurrentTCBConst2		\n" /* Restore the context. */
					"	ldr r1, [r3]					\n" /* Use pxCurrentTCBConst to get the pxCurrentTCB address. */
					"	ldr r0, [r1]					\n" /* The first item in pxCurrentTCB is the task top of stack. */
					"   ldr r14 , [r0, #-4]            	\n"
					"	ldmia r0!, {r4-r11}				\n" /* Pop the registers that are not automatically saved on exception entry and the critical nesting count. */
					"	msr psp, r0						\n" /* Restore the task stack pointer. */
					"	mov r0, #0 						\n"
					"	msr	basepri, r0					\n"
					"	bx r14							\n"
					"									\n"
					"	.align 2						\n"
					"pxCurrentTCBConst2: .word pxCurrentTCB				\n"
				);

}
void xPortPendSVHandler( void )
{
	/* This is a naked function. */

	__asm volatile
	(
	"	mrs r0, psp							\n"
	"										\n"
	"	ldr	r3, pxCurrentTCBConst			\n" /* Get the location of the current TCB. */
	"	ldr	r2, [r3]						\n"
	"										\n"
	"	stmdb r0!, {r4-r11}					\n" /* Save the core registers. */
	"										\n"
	"	str r0, [r2]						\n" /* Save the new top of stack into the first member of the TCB. */
	"	stmdb r0!, {lr}					    \n"
	"										\n"
	"	tst lr, #0x10						\n" /* Is the task using the FPU context?  If so, push high vfp registers. */
	"	it eq								\n"
	"	vstmdbeq r0!, {s16-s31}				\n"

	"	stmdb sp!, {r3, lr}					\n"
	"	mov r0, %0 							\n"
	"	msr basepri, r0						\n"
	"	bl vTaskSwitchContext				\n"
	"	mov r0, #0							\n"
	"	msr basepri, r0						\n"
	"	ldmia sp!, {r3, lr}					\n"
	"										\n"
	"	ldr r1, [r3]						\n" /* The first item in pxCurrentTCB is the task top of stack. */
	"	ldr r0, [r1]						\n"
	"										\n"
	"   ldr r14 , [r0, #-4]!            	\n"
	"	tst r14, #0x10						\n" /* Is the task using the FPU context?  If so, pop the high vfp registers too. */
	"	it eq								\n"
	"	vldmdbeq r0!, {s16-s31}				\n"

	"	ldr r0, [r1]						\n"
	"										\n"
	"	ldmia r0!, {r4-r11}					\n" /* Pop the core registers. */

	"										\n"
	"	msr psp, r0							\n"
	"	bx r14								\n"
	"										\n"
	"	.align 2							\n"
	"pxCurrentTCBConst: .word pxCurrentTCB	\n"
	::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY)
	);
}

For the MPU section, we use the MPU with the FPU at the same time, so we take the FPU port and merge it with the MPU port. We did the same modification to the FPU port + we need to move the MPU settings after the pcTaskName in the tskTaskControlBlock. A modification is also needed in the port.c file to store the control register after the pushing the standard register.

Here a sample of the modification for the MPU

StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters, BaseType_t xRunPrivileged )
{
	/* Simulate the stack frame as it would be created by a context switch
	interrupt. */
	pxTopOfStack--; /* Offset added to account for the way the MCU uses the stack on entry/exit of interrupts. */
	*pxTopOfStack = portINITIAL_XPSR;	/* xPSR */
	pxTopOfStack--;
	*pxTopOfStack = ( StackType_t ) pxCode;	/* PC */
	pxTopOfStack--;
	*pxTopOfStack = 0xDEEDBEEF;	/* LR */
	pxTopOfStack -= 5;	/* R12, R3, R2 and R1. */
	*pxTopOfStack = ( StackType_t ) pvParameters;	/* R0 */


	pxTopOfStack -= 8;	/* R11, R10, R9, R8, R7, R6, R5, R4 */

	pxTopOfStack--; // privileged data
	if( xRunPrivileged == pdTRUE )
	{
		*pxTopOfStack = portINITIAL_CONTROL_IF_PRIVILEGED;
	}
	else
	{
		*pxTopOfStack = portINITIAL_CONTROL_IF_UNPRIVILEGED;
	}

	/* A save method is being used that requires each task to maintain its
	own exec return value. */
	pxTopOfStack--;
	*pxTopOfStack = portINITIAL_EXEC_RETURN;


	pxTopOfStack++;
	pxTopOfStack++;
	return pxTopOfStack;
}
static void prvRestoreContextOfFirstTask( void )
{
	__asm volatile
	(
		"	ldr r0, =0xE000ED08				\n" /* Use the NVIC offset register to locate the stack. */
		"	ldr r0, [r0]					\n"
		"	ldr r0, [r0]					\n"
		"	msr msp, r0						\n" /* Set the msp back to the start of the stack. */

		"	ldr	r3, pxCurrentTCBConst2		\n" /* Restore the context. */
		"	ldr r1, [r3]					\n" /* Use pxCurrentTCBConst to get the pxCurrentTCB address. */
		"	ldr r0, [r1]					\n" /* The first item in pxCurrentTCB is the task top of stack. */


		"   ldr r3 , [r0, #-4]            	\n"	/* restore the privilege mode */
		"	msr control, r3					\n" /* set the privilege mode depending of what was initialised */

		"   ldr r14 , [r0, #-8]            	\n" /* restore the link register mode for FPU operation */

		"	ldmia r0!, {r4-r11}		\n" /* Pop the registers that are not automatically saved on exception entry and the critical nesting count. */


			/* MPU configuration */
		"	ldr	r3, pxCurrentTCBConst2		\n" /* Get the pxCurrentTCP pointer. */
		"	ldr r1, [r3]					\n" /* The first item in pxCurrentTCB is the task top of stack. */
		"	add r1, r1, #68					\n" /* Move onto the second item in the TCB... */
		"	ldr r2, =0xe000ed9c				\n" /* Region Base Address register. */
		"	ldmia r1!, {r4-r7}				\n" /* Read 2 sets of MPU registers. */
		"	stmia r2!, {r4-r7}				\n" /* Write 2 sets of MPU registers. */
			/* END of MPU configuration */
		"	msr psp, r0						\n" /* Restore the task stack pointer. */
		"	isb								\n"
		"	mov r0, #0 						\n"
		"	msr	basepri, r0					\n"
		"	bx r14							\n"
		"									\n"
		"	.align 2						\n"
		"pxCurrentTCBConst2: .word pxCurrentTCB				\n"
	);
}
void xPortPendSVHandler( void )
{
	/* This is a naked function. */

	__asm volatile
	(
	"	mrs r0, psp							\n"
	"	isb									\n"
	"										\n"
	"	ldr	r3, pxCurrentTCBConst			\n" /* Get the location of the current TCB. */
	"	ldr	r2, [r3]						\n"

	"	stmdb r0!, {r4-r11}					\n" /* Save the core registers. */

	"	str r0, [r2]						\n" /* Save the new top of stack into the first member of the TCB. */
	"										\n"

	"	mrs r1, control						\n" /* get the currect execution mode (privilege or user) */
	"	stmdb r0!, {r1}						\n" /* push the privilege mode to the stack frame*/

	"	stmdb r0!, {lr}					    \n" /* save the current status of the FPU */
	"	tst lr, #0x10						\n" /* Is the task using the FPU context?  If so, push high vfp registers. */
	"	it eq								\n"
	"	vstmdbeq r0!, {s16-s31}				\n"
	"										\n"
	"	stmdb sp!, {r3}						\n"
	"	mov r0, %0 							\n"
	"	msr basepri, r0						\n"
	"	dsb									\n"
	"   isb									\n"
	"	bl vTaskSwitchContext				\n"
	"	mov r0, #0							\n"
	"	msr basepri, r0						\n"
	"	ldmia sp!, {r3}						\n"
/* MPU configuration */
	"	ldr	r3, pxCurrentTCBConst		\n" /* Get the pxCurrentTCP pointer. */
	"	ldr r1, [r3]					\n" /* The first item in pxCurrentTCB is the task top of stack. */
	"	add r1, r1, #68					\n" /* Move onto the second item in the TCB... */
	"	ldr r2, =0xe000ed9c				\n" /* Region Base Address register. */
	"	ldmia r1!, {r4-r7}				\n" /* Read 2 sets of MPU registers. */
	"	stmia r2!, {r4-r7}				\n" /* Write 2 sets of MPU registers. */
/* END of MPU configuration */
	"	ldr r1, [r3]						\n" /* The first item in pxCurrentTCB is the task top of stack. */
	"	ldr r0, [r1]						\n"
	"										\n"

	"   ldr r3 , [r0, #-4]!            	\n"
	"	msr control, r3						\n"


	"   ldr r14 , [r0, #-4]!            	\n"
	"	tst r14, #0x10						\n" /* Is the task using the FPU context?  If so, pop the high vfp registers too. */
	"	it eq								\n"
	"	vldmdbeq r0!, {s16-s31}				\n"


	"	ldr r0, [r1]						\n" /* restore current stack pointer */
	"										\n"
	"	ldmia r0!, {r4-r11}					\n" /* Pop the core registers. */

	"										\n"
	"	msr psp, r0							\n"
	"	isb									\n"
	"										\n"
	#ifdef WORKAROUND_PMU_CM001 /* XMC4000 specific errata workaround. */
		#if WORKAROUND_PMU_CM001 == 1
	"			push { r14 }				\n"
	"			pop { pc }					\n"
		#endif
	#endif
	"										\n"
	"	bx r14								\n"
	"										\n"
	"	.align 2							\n"
	"pxCurrentTCBConst: .word pxCurrentTCB	\n"
	::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY)
	);
}

With this modification we can use the multithread display in Eclipse (via GDB). Those modifications are not very intruisive.

I’m sorry for the long post here but I want to have a good reference to this for other people know that this is possible to use with freeRTOS.

Regards

Jonathan

rtel wrote on Wednesday, January 13, 2016:

Hi Jonathan,

Sounds like you have been doing some great work, that will be of value to the whole community. We should find a way of highlighting this on the FreeRTOS.org website.

Your post will get archived on the FreeRTOS.org site, so if you want to link to the post, then you can link directly to the archived version, where we will have more control over it. Let me know if you would like me to create the archive now - the archive will then get updated ever couple of weeks or so.

From the data types you are using it looks like you have based this on an older version of FreeRTOS - not too much has changed, but be aware more memory barrier instructions are used now, which you would have to add in if you would also like the code to run on ARM Cortex-M7 parts.

I will have to step through the code to draw out the stack frames to form an opinion of its impact - unfortunately I don’t have time right now, but will do. From a very quick look, and from your description, it seems you are changing the place at which the exec return value is saved, so all that goes before matches the standard (non FPU) Cortex-M port.

dumarjo wrote on Wednesday, January 13, 2016:

Hi,

Le 2016-01-13 09:40, Real Time Engineers ltd. a écrit :

Hi Jonathan,

Sounds like you have been doing some great work, that will be of value
to the whole community. We should find a way of highlighting this on
the FreeRTOS.org website.

Thanx !

Your post will get archived on the FreeRTOS.org site, so if you want
to link to the post, then you can link directly to the archived
version, where we will have more control over it. Let me know if you
would like me to create the archive now - the archive will then get
updated ever couple of weeks or so.

No problem at all. You can publish this.

From the data types you are using it looks like you have based this on
an older version of FreeRTOS - not too much has changed, but be aware
more memory barrier instructions are used now, which you would have to
add in if you would also like the code to run on ARM Cortex-M7 parts.

I know. the FPU part is done on the 7.2.0 freeRTOS version. We have made
some hack in the task.c file as well. For the MPU/F port, I will check
the memory barrier.

I will have to step through the code to draw out the stack frames to
form an opinion of its impact - unfortunately I don’t have time right
now, but will do. From a very quick look, and from your description,
it seems you are changing the place at which the exec return value is
saved, so all that goes before matches the standard (non FPU) Cortex-M
port.

This is exactly this. We save the standard register and push the control
and/or LR after that.

The only change in the taskTCB needed is the move part of the MPU
settings. The offset of the the pcTaskName must identical when the MPU
is used.

Regards

Jonathan

rtel wrote on Wednesday, January 13, 2016:

Currently the MPU port relies on the MPU settings being second in the TCB, so keep that in mind too.

dumarjo wrote on Wednesday, January 13, 2016:

Hi,

Le 2016-01-13 12:23, Real Time Engineers ltd. a écrit :

Currently the MPU port relies on the MPU settings being second in the
TCB, so keep that in mind too.

I know this. We have changed that in the port too. Probably using a C
method that return the current offset of the MPU setting can be useful
instead of hardcoding the offset in the port.

Regards

Jonathan