Task switching HardFault on Cortex-M0+ (arduino)

Hi All,

I am using an Arduino SAMD21 Cortex-M0+ board, and I am working on a preemptive task switcher which almost works except for one thing. If I trigger PendSV from SysTick it works, if I trigger PendSV from main program via yield() it works, but if I do both (use yield and also have an interrupt) then there is a hard fault. This is my Arduino sketch that rerpos the issue.. Inside the HardFault handler the stacked PC is 0xFFFFFFF8 and LR is pointing to the instruction in the Arduino's SysTick_Handler after it calls the sysTickHook(). Any idea how to debug this issue or what could be the cause? Thank you!

#define SSIZE 1024
 
void task() {
  while (1) {
    SerialUSB.println("task");
    yield();
    //delay(1000);
  }
}
 
struct Ctx {
  uint32_t r8;
  uint32_t r9;
  uint32_t r10;
  uint32_t r11;
  uint32_t r4;
  uint32_t r5;
  uint32_t r6;
  uint32_t r7;
  uint32_t r0;
  uint32_t r1;
  uint32_t r2;
  uint32_t r3;
  uint32_t r12;
  uint32_t lr;
  uint32_t pc;
  uint32_t psr;
};
 
struct TaskInfo {
  uint8_t* sp;
  uint8_t stack[SSIZE];
};
 
TaskInfo* _tasks[2];
volatile int _current_task = 0;
volatile bool _initialized = false;
 
void init_task(struct TaskInfo* taskInfo) {
  taskInfo->sp = &taskInfo->stack[SSIZE - 1];
  // 8 bytes align per ARM Cortex+ requirement when entering interrupt
  taskInfo->sp = (uint8_t*)((uintptr_t)taskInfo->sp & ~0x7);
 
  // clear registers
  for (unsigned i = 0; i < sizeof(Ctx); ++i) {
    *--taskInfo->sp = 0;
  }
 
  auto ctx = (Ctx*)taskInfo->sp;
  // compiler/architecture specific
  ctx->psr = 0x01000000;
  ctx->pc = (uintptr_t)task;
}
 
uint8_t* swap_stack(uint8_t* sp) {
  if (_initialized) {
    _tasks[_current_task]->sp = sp;
    _current_task = (_current_task + 1) % 2;
    sp = _tasks[_current_task]->sp;
  }
  return sp;
}
 
extern "C" {
  void __attribute__((naked)) PendSV_Handler() {
    noInterrupts();
    asm volatile("push {r4-r7}");
    asm volatile("mov r4,r8");
    asm volatile("mov r5,r9");
    asm volatile("mov r6,r10");
    asm volatile("mov r7,r11");
    asm volatile("push {r4-r7}");
 
    asm volatile("mov r0, sp");
    asm volatile("push {lr}");
    asm volatile("blx %0"
                 :
                 : "r"(swap_stack)
                 : "r0");
    asm volatile("mov r12, r0");
    asm volatile("pop {r0}");
    asm volatile("mov lr, r0");
 
    asm volatile("mov sp, r12");
    asm volatile("pop {r4-r7}");
    asm volatile("mov r8,r4");
    asm volatile("mov r9,r5");
    asm volatile("mov r10,r6");
    asm volatile("mov r11,r7");
    asm volatile("pop {r4-r7}");
    interrupts();
    asm volatile("bx lr");
  }
 
  int sysTickHook() {
    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; // comment this out
    return 0;
  }
}
 
void yield() {
  SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;  // comment this out
}
 
void setup() {
  SerialUSB.begin(115200);
  while (!SerialUSB)
    ;
  noInterrupts();
  _tasks[0] = new TaskInfo();
  _tasks[1] = new TaskInfo();
  init_task(_tasks[1]);
  _initialized = true;
  interrupts();
}
 
void loop() {
  SerialUSB.println("loop");
  yield();
  //delay(1000);
}