实现上下文切换 - 第二个函数不再运行

我正在尝试在x86-64上使用C++和内联汇编(AT&T(实现上下文切换。
如果我保存并重新加载同一函数的上下文，它似乎可以正常工作。
但是，当我尝试生成函数时，在尝试加载第二个函数上下文后，它给了我使用 GDB 的 seg 错误/损坏堆栈。

例如，它打印
Print1
打印 2
打印 1
//损坏的堆栈和程序停止运行

但是，如果我在重新加载线程 1(第一个函数(之前保存它的上下文，就不会有问题。

例如，它打印打印 1
打印 1 打印
1

我正在为上下文保存和堆栈创建内存空间。保存上下文时，堆栈指针和基指针将保存到结构中。之后，堆栈指针将指向上下文存储器以推送寄存器值。

我想知道是什么原因导致堆栈损坏以及为什么我无法加载第二个函数的上下文。如果可能的话，请帮助我指出代码中的任何错误。谢谢！

#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, %0nt" // save stack pointer
"movq %%rbp, %1nt" // save rbp
"movq %3, %%rspnt" // point to context mem then push register values into it
"pushq %%raxnt"
"pushq %%rbxnt"
"pushq %%rcxnt"
"pushq %%rdxnt"
"pushq %%rsint"
"pushq %%rdint"
"pushq %%r8nt"
"pushq %%r9nt"
"pushq %%r10nt"
"pushq %%r11nt"
"pushq %%r12nt"
"pushq %%r13nt"
"pushq %%r14nt"
"pushq %%r15nt"
"pushfqnt"
"movq %%rsp, %2nt" // save rsp into context sp (end of context mem)
"movq %4, %%rspnt" // 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, %%rbpnt" // restore stackbp into rbp
"movq %1, %%rspnt" // point to context memory to pop
"popfqnt"
"popq %%r15nt"
"popq %%r14nt"
"popq %%r13nt"
"popq %%r12nt"
"popq %%r11nt"
"popq %%r10nt"
"popq %%r9nt"
"popq %%r8nt"
"popq %%rdint"
"popq %%rsint"
"popq %%rdxnt"
"popq %%rcxnt"
"popq %%rbxnt"
"popq %%raxnt"
"movq %2, %%rspnt" // 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);
}