SysTick_Handler works as a normal function but not on SysTick interrupt (on QEMU)

Issue

I am trying to implement a simple RTOS with round robin scheduling. Since I do not have a physical board yet, I am running the ELF file on QEMU (qemu-system-gnuarmlinux). For development I am using Eclipse CDT. I use the following command to run the code on QEMU:

/opt/xpack-qemu-arm-7.0.0-1/bin/qemu-system-gnuarmeclipse -M STM32F4-Discovery -kernel /mnt/d/eclipse-workspace/rtos/Debug/rtos.elf

Each task has an associated struct:

struct TCB {
    int32_t *stackPt;
    struct TCB *nextPt;
};

At initialization, the structs are chained up in a circular linked list via the nextPt, their stacks (stackPt) are set as TCB_STACK[threadNumber][STACK_SIZE-16]; and the stack's program counter is set up as TCB_STACK[0][STACK_SIZE - 2] = (int32_t)(taskA);. The current thread's pointer is maintained as: currentTcbPt.
Then the systick is set up to interrupt at every 10ms. An assembly setup function sets up initial stack pointer to the thread stack pointed to by currentTcbPt. This function is as follows:

osSchedulerLaunch:          // This routine loads up the first thread's stack pointer into SP
    CPSID I
    LDR R0,=currentTcbPt
    LDR R2,[R0]             // R2 = address of current TCB

    LDR SP,[R2]

    POP {R4-R11}
    POP {R0-R3}
    POP {R12}
    ADD SP,SP,#4            // Skip 4 bytes to discard LR
    POP {LR}
    ADD SP,SP,#4            // Skip 4 bytes to discard PSR

    CPSIE I
    BX LR

Now, my SysTick_Handler looks like this:

__attribute__( ( naked ) ) void SysTick_Handler(void) {
    __asm(
            "CPSID I                    \n"
            "PUSH {R0-R12}              \n"
            "LDR R0,=currentTcbPt       \n"
            "LDR R1,[R0]                \n"
            "STR SP,[R1]                \n"
            "LDR R1,[R1,#4]             \n"
            "STR R1,[R0]                \n"
            "LDR SP,[R1]                \n"
            "POP {R4-R11}               \n"
            "POP {R0-R3}                \n"
            "POP {R12}                  \n"
            "ADD SP,SP,#4               \n"
            "POP {LR}                   \n"
            "ADD SP,SP,#4               \n"
            "CPSIE I                    \n"
            "BX LR                      \n"
            :[currentTcbPt] "=&r" (currentTcbPt)
            );

}

I have added extra register operations so I can use it as a normal function.

Problem

**First**, I disable interrupts in the `onSchedulerLaunch` function (comment out `CPSIE I`) and in the systick handler. Also renaming `SysTick_Handler` to a random function name (say `Foo`).
Then, I call this `Foo` function at the end of each task (tasks do not have an infinite loop). This works absolutely fine. The tasks get switched over and over as intended.

**Second**, I enable interrupts, set the function's name back to `SysTick_Handler`, re enable interrupts and `extern "C"` and remove the call from the end of tasks. Now, as soon as the SysTick exception happens, the function get's executed, but I get a Usage Fault with a stack register print on terminal.

OS init
Launching scheduler
t2
t2
[UsageFault]
Stack frame:
 R0 =  00000003
 R1 =  2000008C
 R2 =  00000000
 R3 =  000004B8
 R12 = 00000000
 LR =  0800148D
 PC =  000004B8
 PSR = 20000000
FSR/FAR:
 CFSR =  00000000
 HFSR =  00000000
 DFSR =  00000000
 AFSR =  00000000
Misc
 LR/EXC_RETURN= FFFFFFF9

On examining the asm code using -d in_asm option in QEMU and also using remote gdb, The problem seems to happen at the first line of the next task (the same address in PC above).

Question

What could be the cause of this problem ? Is it perhaps specific to QEMU or is there something wrong with the assembly code ?

EDIT: See the full code to reproduce https://gist.github.com/shivangsgangadia/b78c7c66492d5332c7b4d1806be9c5f6
The order of execution of function would be something like:

RTOS rtos();
rtos.addThreads(&task_a, &task_b, &task_c);
rtos.osKernelLaunch();

Solution

The problem was setting the T-bit in the Execution PSR register for individual task stacks. The course content that I was following skipped over the fact that the PSR is composed of 4 bytes, and the T-bit in the most significant byte.
Initially, I was setting it as: TCB_STACK[threadNumber][STACK_SIZE-1] = (1U << 6);. This was causing the BX LR to not pick up the return address properly.
Setting the 6th bit in the 4th byte, i.e., TCB_STACK[threadNumber][STACK_SIZE-1] = (1U << 24); solved the problem and now the scheduler works flawlessly.

Answered By - Shivang Gangadia