I am trying to implement the following model in C++ between user-space thread and std::thread.

The kernel thread in the diagram will correspond to std::thread. I have implemented a stack switch mechanism to implement the user-space thread. The std::threads wait on a thread-safe queue of some context struct -- where the context struct represents the context of a user-space thread. It contains the stack pointer and other important register values.
I implemented it but I feel some of the implementation details can be improved with respect to Context Switching from a user-space thread to std::thread.
Here is the very minimal code to represent the basic utility functions.
struct threadContext {
    // registers set
    // pointer to user space thread context if this is not a user space thread
};
// shared data structure
std::unordered_map<std::thread::id,int> Map;
threadContext kThread[4];
// this run function is part of a thread pool class
void run(unsigned i) {
    // wait on a thread safe queue
    // pop a user thread context pointer p
    yield_to(p) // this will switch stack & instruction pointer to a different 
                // location
    // push p to queue if p is not done
    // loop
}
void userFunc() {
    std::cout <<"Hello from user Func" << std::endl;
    yield(); // this will switch stack pointer to the original locaiton
             // which is inside the run function
   
}
Inside the yield() function I need to load the parent std::thread's context and save the current user thread's context. But we have more than 1 std::thread so I have to use a mutex and unordered_map (indexed with std::thread::get_id()) to get to the current running std::thread from kthread[4] array.
So basically, when a user-space thread wants to give control back to its parent std::thread, it needs to know which thread context to load to jump safely at parentstd::thread. For this, I had to use a lock-based data structure.
Is there any way to avoid the mutex for getting control back at the std::thread Or have a completely different method to give control back to the parent std::thread?
Sorry, I intentionally excluded some implementation detail. Please suggest if it is required. (For ContextSwitch implementation you might want to check my previous question asked a few weeks back on different issues related to user-space thread implementation
Pass arguments to a user space thread )
Thanks!
 
    