Cortex M3 - Calling a SVC inside a C function, and returning to thread mode

I am writing System Calls, as recommended by Joseph Yiu (M3 Guide), by taking the arguments from the stack. The Assembly SVC Handler is like:

     SVC_Handler:
     MOV R0, #0
     MSR CONTROL, R0
     MRS R4, PSP
     CMP LR, #0xFFFFFFFD
     BEQ KernelEntry
     B   KernelExit

    KernelEntry: 
    /* save current user stack */
    B svchandler_main

    KernelExit:
    /* restore the very same saved user stack saved before*/
    MOV LR, #0xFFFFFFFFD
    BX      LR 

Well, so the svchandler_main is C function that recovers the immediates (arguments of the the system call) creates the kernel stack and branches to 0xFFFFFFFF9 (MSP privileged). The system call itself is made like:

#define svc(code) asm volatile ("svc %[immediate]"::[immediate] "I" (code))
void SysCall_MyCall(int32_t args)
{
  svc(CallBack_Number);
}

That said, the callback function, running in handler mode:

void SysCallBack(void* args)
{
  /* <my c routine>*/
  asm volatile("svc #0"); //to exit the kernel mode
}

The last SV Call is performed so that the SVC_Handler in assembly will identify it is coming from handler mode (MSP privileged) and will exit the kernel - kind of a cooperative scheduling on the kernel. The problem is the context is saved with PSP pointing inside SysCall_MyCall, and it returns to there, and never exits. If I use inline functions, I will lose the handy svchandler_main. Any ideas? I didnt write the svchandler_main here because it is a classic code found on ARM application notes. Thanks.