TnKernel - контроль стека задачи

полдня вчера убил на поиск траблы, оказалось стек задачи перетирает чужие области.
Седня полня убил чтобы сделать контроль. Пока получилось так

tn_port_cm3_iar.s
Код
;-----------------------------------------------------------------------------
tn_switch_context:

   ;--- Check head of task stack
       ldr    r1,  =tn_curr_run_task ;-- r1=addr of pointer to task struct
       ldr    r0,  [r1]              ;-- r0=addr of task struct

       ldr    r1, [r0, #0x28]        ;-- r1=stack_start
       ldr    r2, [r0, #0x2C]        ;-- r2=stack_size ( qnty of int )
       lsl    r2,r2,#2               ;-- r2=stack_size ( qnty of bytes )
      
       sub    r0, r1, r2              
       add    r0, r0, #4             ;-- r0=addr of head stack
       ldr    r0,[r0]                ;-- r0=value of head stack
      
       mov    r1, #0xBEEF            ;-- r1=load magic word
       movt   r1, #0xDEAD          
      
       cmp    r0,r1                  ;-- check magic dword
       beq    task_stack_ok
      
       nop                           ;-- Check fail!!! ( for breakpoint )
      
    task_stack_ok:
    
   ;---  Just Invoke PendSV exception
       ldr    r1,  =ICSR_ADDR
       ldr    r0,  =PENDSVSET
       str    r0,  [r1]

       bx     lr

пока сделано под режим ARM, с опцией USE_MUTEXES

вопросы:
№1 корректно ли разместил (в том ли месте ) проверялку ? Вроде, должна запускатся в контексе задачи а не в прерывании PendSV
№2 можно как-то упростить код ? а то в асме CM3 слабоват =(