Context switch injection - second function doesn't run again

I am trying to implement context switching using C ++ and inline assembly (AT&T) on x86-64.
It seems to work correctly if I save and reload the context for the same function. However, when I try to execute the functions it gave me a seg fault / corrupt stack using GDB after trying to load the second functional context.

For example, it prints Print1
Print2
Print1
// Corrupted stack and the program stops working

However, if I were to save the thread 1 context (1st function) before reloading it, there would be no problem.

For example, it prints Print1
Print1
Print1

I am creating memory space to store context and stack. When saving the context, the stack pointer and base pointer will be saved in the structure. After that, the stack pointer will point to the context memory to push the register values.

I would like to know what is causing the damaged stack and why I cannot load the second function context back. And if possible, please kindly help me point out any errors in my code. Thank!

#include <stdio.h>
#include <iostream>
#include <vector>
#include <map>
#include <memory>
#include <algorithm>
namespace CORO
{
using ThreadID = unsigned;
static int thread_count;

enum STATE
{
  READY,
  ACTIVE,
  WAITING,
  ENDED
};

struct thd_data
{
  int parent_ID = 0;
  int id = 0;
  STATE state = READY;

  int * stack_mem;
  void * stackptr;
  void * stackbp;
  void*(*funcptr)(void*);
  void * param = nullptr;
  int * context_mem;
  int * context_sp;

  thd_data()
  :stack_mem{new int[1024]}, context_mem{new int[1024]} 
  {

  }

  thd_data(const thd_data & rhs)
  :stack_mem{new int[1024]}, context_mem{new int[1024]} 
  {

  }

  thd_data & operator=(const thd_data & rhs)
  {

  }
};

static thd_data* curr_thd;

std::map<int, std::shared_ptr<thd_data>> threadmap;
std::vector<int>activeListID;

// Returns a pointer to next thread
thd_data * FindNextThread()
{
  int new_id;
  for(const auto & elem : activeListID)
  {
    if(elem != curr_thd->id)
    {
      new_id = elem;
      break;
    }
  }
  auto threadmap_elem = threadmap.find(new_id);
  if(threadmap_elem != threadmap.end())
  {
    return &(*threadmap_elem->second);
  }
  else
  {
    return nullptr;
  }
}

void thd_init()
{
  threadmap[0] = std::make_shared<thd_data>();
  auto main_thd = threadmap.find(0)->second;

  main_thd->state = ACTIVE;
  main_thd->id = 0;
  main_thd->param = nullptr;
  main_thd->funcptr = nullptr;
  activeListID.push_back(main_thd->id);

  curr_thd = &(*main_thd);
}

ThreadID new_thd( void*(*func)(void*), void *param)
{
  thread_count += 1; // increment counter

  threadmap[thread_count] = std::make_shared<thd_data>();
  auto thd = threadmap.find(thread_count)->second;

  thd->state = READY;
  thd->id = thread_count;
  activeListID.push_back(thd->id);

  thd->stackptr = thd->stack_mem+1024;
  thd->stackbp = thd->stack_mem;
  thd->funcptr = func;
  thd->param = param;
  return thd->id;
}

void thd_yield()
{
  // Find the next ready thread
  thd_data* thd = FindNextThread();

  if(thd == nullptr)
    return;

  // Move ID to the end of vector
  activeListID.erase(std::remove(activeListID.begin(), activeListID.end(), curr_thd->id), activeListID.end());
  activeListID.push_back(curr_thd->id);

  // Save context
  {
    asm volatile
    (
      "movq %%rsp, %0\n\t" // save stack pointer
      "movq %%rbp, %1\n\t" // save rbp
      "movq %3, %%rsp\n\t" // point to context mem then push register values into it
      "pushq %%rax\n\t"
      "pushq %%rbx\n\t"
      "pushq %%rcx\n\t"
      "pushq %%rdx\n\t"
      "pushq %%rsi\n\t"
      "pushq %%rdi\n\t"
      "pushq %%r8\n\t"
      "pushq %%r9\n\t"
      "pushq %%r10\n\t"
      "pushq %%r11\n\t"
      "pushq %%r12\n\t"
      "pushq %%r13\n\t"
      "pushq %%r14\n\t"
      "pushq %%r15\n\t"
      "pushfq\n\t"
      "movq %%rsp, %2\n\t" // save rsp into context sp (end of context mem)
      "movq %4, %%rsp\n\t" // restore stackptr into rsp
      :"+m"(curr_thd->stackptr)   
      ,"+m"(curr_thd->stackbp)    
      ,"+m"(curr_thd->context_sp)
      :"m"(curr_thd->context_mem) 
      ,"m"(curr_thd->stackptr)    
      :"rsp"
    );
  }

  curr_thd->state = WAITING;
  curr_thd = thd;

  // Calls function if thread is not running
  if(thd->state == READY)
  {
    thd->state = ACTIVE;
    thd->funcptr(thd->param);
  }
  else
  {
    // Restore context
    {
      asm volatile
      (
        "movq %0, %%rbp\n\t" // restore stackbp into rbp
        "movq %1, %%rsp\n\t" // point to context memory to pop
        "popfq\n\t"
        "popq %%r15\n\t"
        "popq %%r14\n\t"
        "popq %%r13\n\t"
        "popq %%r12\n\t"
        "popq %%r11\n\t"
        "popq %%r10\n\t"
        "popq %%r9\n\t"
        "popq %%r8\n\t"
        "popq %%rdi\n\t"
        "popq %%rsi\n\t"
        "popq %%rdx\n\t"
        "popq %%rcx\n\t"
        "popq %%rbx\n\t"
        "popq %%rax\n\t"
        "movq %2, %%rsp\n\t" // point to TCB stack pointer
        :
        :"m"(thd->stackbp)
        ,"m"(thd->context_sp)
        ,"m"(thd->stackptr)
        :"rsp"
      );
    }
  }
}
} // end namespace
void* print1(void *a)
{
  int i;
  for(i=0; i< 20; i++)
  {
    std::cout<<"Print1 i: "<<i<<std::endl;
    if((i+1)%4==0)
        CORO::thd_yield();
  }
  return NULL;
}

void* print2(void *a)
{
  int i;
  for(i=0; i< 20; i++)
  {
    std::cout<<"Print2 i: "<<i<<std::endl;
    if((i+1)%4==0)
        CORO::thd_yield();
  }
  return NULL;
}


int main()
{
  CORO::ThreadID id;
  CORO::thd_init();
  id = CORO::new_thd(print2, NULL);
  print1(NULL);
}