Implementation of a thread context switcher - c

Question

I'm trying to implement a thread library using an assembly code to save the registers on a struct tcb, get the new thread from queue e load its tcb to registers.
I'm using all 16 general-purpose registers and eflags. However, I'm facing some difficults, because the assembly code is not working properly. It keep showing Segmentation Fault.
That's what I've done in assembly to switch context:
change_context:
// Saving current context
pushfq
pushq %rax
movq tcb, %rax
popq 0(%rax)
movq %rcx, 8(%rax)
movq %rdx, 16(%rax)
movq %rbx, 24(%rax)
movq %rsi, 32(%rax)
movq %rdi, 40(%rax)
movq %rbp, 48(%rax)
movq %r8, 56(%rax)
movq %r9, 64(%rax)
movq %r10, 72(%rax)
movq %r11, 80(%rax)
movq %r12, 88(%rax)
movq %r13, 96(%rax)
movq %r14, 104(%rax)
movq %r15, 112(%rax)
popq 120(%rax)
movq %rsp, 128(%rax)
// Find new context
call get_next_thread
//Restauring context
movq current_running, %rax
movq 8(%rax), %rcx
movq 16(%rax), %rdx
movq 24(%rax), %rbx
movq 32(%rax), %rsi
movq 40(%rax), %rdi
movq 48(%rax), %rbp
movq 56(%rax), %r8
movq 64(%rax), %r9
movq 72(%rax), %r10
movq 80(%rax), %r11
movq 88(%rax), %r12
movq 96(%rax), %r13
movq 104(%rax), %r14
movq 112(%rax), %r15
pushq 120(%rax)
popfq
movq 128(%rax), %rsp
pushq 0(%rax)
popq %rax
ret
'tcb' is a pointer to current thread control block and it has the following face:
struct _tcb{
uint64_t registers[15];
uint64_t flags;
uint64_t stack_ptr;
uint64_t *stack;
}
When creating the threads, I initialize it with zeros, except %rdi that points to void *arg and stack_ptr that points to last position of stack, where I put the value of the start_routine.
The thread creation function is similar to the default from "pthread.h"
int create_thr(pthread_t *thread,
void *(*start_routine)(void *),
void *arg);
The only difference is that I'm not using attributes.
I've debugged it with valgrind and gdb. I found out that its first complain is from line "popq 0(%rax)"; it says "invalid write of size 8". Later it gets Segmentation Fault on functions like printf and usleep. I'm using a Linux 64-bit.
Any ideas?