restore: Add threads restoration

Now threads restortion (and TLS as well) works.
Threads test reports the following

    2775 (main): Counter value:    3 tls_data =    1
    2775 (main): ( 0) fsgs_base 7f9597aa46f0
    2775 (main): ( 0) fsgs_base        0
    2775 (thr3): Counter value:    4 tls_data =    4
    2775 (thr3): ( 0) fsgs_base 42c57940
    2775 (thr3): ( 0) fsgs_base        0
    2775 (thr2): Counter value:    3 tls_data =    2
    2775 (thr2): ( 0) fsgs_base 42456940
    2775 (thr2): ( 0) fsgs_base        0
    2775 (thr1): Counter value:    4 tls_data =    3
    2775 (thr1): ( 0) fsgs_base 40c62940
    2775 (thr1): ( 0) fsgs_base        0
    2775 (main): Counter value:    4 tls_data =    1
    2775 (main): ( 0) fsgs_base 7f9597aa46f0
    2775 (main): ( 0) fsgs_base        0
    2775 (thr1): Counter value:    5 tls_data =    3
    2775 (thr1): ( 0) fsgs_base 40c62940
    2775 (thr1): ( 0) fsgs_base        0

as expected.

This commits merges all preliminary commits into
the final one (sigreturn branch was always experimental
and forced update).

Still some problems remain:

1) While creating threads with clone() the
   flags are to be revisited. We use some predefined
   set here but it's not really correct.

2) No setup of pids in PCB thread zone.

3) No restore of FPU.

But at least on some basic tasks restore works well.
Signed-off-by: Cyrill Gorcunov <gorcunov@gmail.com>

cr-restore.c

@@ -1287,23 +1287,50 @@ err_or_found:
 
 static void sigreturn_restore(pid_t pid)
 {
-	long code_len, vma_len, args_offset, new_sp, hint;
-	void *args_rip, *exec_mem, *exec_start;
+	long restore_task_code_len, restore_task_vma_len;
+	long restore_thread_code_len, restore_thread_vma_len;
+	void *exec_mem = MAP_FAILED;
+	void *restore_thread_exec_start;
+	void *restore_task_exec_start;
+	long args_offset, new_sp, hint;
 	long ret;
 
-	struct restore_core_args *args;
-	restorer_fcall_t restorer_fcall;
+	struct task_restore_core_args *args;
 	char path[64];
 
 	LIST_HEAD(self_vma_list);
 	struct vma_area *vma_area;
-	int fd_vmas, num;
+	int fd_vmas = -1;
+	int num;
+
+	struct thread_restore_args *thread_args;
+	struct pstree_entry pstree_entry;
+	int *fd_core_threads;
+	int fd_pstree = -1;
+
+	RLOCK_T(rlock);
+
+	restore_task_code_len	= 0;
+	restore_task_vma_len	= 0;
+	restore_thread_code_len	= 0;
+	restore_thread_vma_len	= 0;
 
 	if (parse_maps(getpid(), &self_vma_list, false))
 		goto err;
 
 	pr_info_vma_list(&self_vma_list);
 
+	BUILD_BUG_ON(sizeof(struct task_restore_core_args) > RESTORE_ARGS_SIZE);
+	BUILD_BUG_ON(sizeof(struct task_restore_core_args) & 1);
+	BUILD_BUG_ON(sizeof(struct thread_restore_args) & 1);
+
+	snprintf(path, sizeof(path), "pstree-%d.img", pid);
+	fd_pstree = open(path, O_RDONLY, CR_FD_PERM);
+	if (fd_pstree < 0) {
+		pr_perror("Can't open %s\n", path);
+		goto err;
+	}
+
 	snprintf(path, sizeof(path), "vmas-%d.img", getpid());
 	unlink(path);
 	fd_vmas = open(path, O_CREAT | O_WRONLY, CR_FD_PERM);
@@ -1316,32 +1343,71 @@ static void sigreturn_restore(pid_t pid)
 	list_for_each_entry(vma_area, &self_vma_list, list) {
 		ret = write(fd_vmas, &vma_area->vma, sizeof(vma_area->vma));
 		if (ret != sizeof(vma_area->vma)) {
-			close(fd_vmas);
 			pr_perror("\nUnable to write vma entry (%li written)\n", num);
 			goto err;
 		}
 		num++;
 	}
 
-	close(fd_vmas);
-
+	close_safe(&fd_vmas);
 	free_mappings(&self_vma_list);
 
-	restorer_fcall	= restorer;
-	code_len	= restorer_fcall(RESTORER_CMD__GET_SELF_LEN) - (long)restorer;
-	args_offset	= restorer_fcall(RESTORER_CMD__GET_ARG_OFFSET) - (long)restorer;
-	code_len	= round_up(code_len, 16);
-	vma_len		= round_up(code_len + RESTORER_STACK_SIZE + RESTORER_STACK_FRAME, PAGE_SIZE);
+	restore_task_code_len	= restore_task(RESTORE_CMD__GET_SELF_LEN) - (long)restore_task;
+	restore_task_code_len	= round_up(restore_task_code_len, 16);
+
+	args_offset		= restore_task(RESTORE_CMD__GET_ARG_OFFSET) - (long)restore_task;
+	restore_task_vma_len	= round_up(restore_task_code_len + RESTORE_STACK_SIZE + RESTORE_STACK_FRAME, PAGE_SIZE);
+
+	restore_thread_code_len = restore_thread(RESTORE_CMD__GET_SELF_LEN, NULL) - (long)restore_thread;
+	restore_thread_code_len	= round_up(restore_thread_code_len, 16);
+
+	/*
+	 * Thread statistics
+	 */
+	lseek(fd_pstree, MAGIC_OFFSET, SEEK_SET);
+	while (1) {
+		ret = read_ptr_safe_eof(fd_pstree, &pstree_entry, err);
+		if (!ret) {
+			pr_perror("Pid %d not found in process tree\n", pid);
+			goto err;
+		}
+
+		if (pstree_entry.pid != pid)
+			continue;
+
+		if (!pstree_entry.nr_threads)
+			break;
+
+		/*
+		 * Compute how many memory we will need
+		 * to restore all threads, every thread
+		 * requires own stack and heap, it's about
+		 * 40K per thread.
+		 */
+
+		BUILD_BUG_ON(sizeof(*thread_args) & 1);
+
+		restore_thread_vma_len = sizeof(*thread_args) * pstree_entry.nr_threads;
+		restore_thread_vma_len = round_up(restore_thread_vma_len, 16);
 
-	hint = restorer_vma_hint(pid, &self_vma_list, vma_len);
+		restore_thread_vma_len+= restore_thread_code_len;
+
+		pr_info("%d: %d threads require %dK of memory\n",
+			pid, pstree_entry.nr_threads, restore_thread_vma_len >> 10);
+		break;
+	}
+
+	hint = restorer_vma_hint(pid, &self_vma_list,
+				 restore_task_vma_len + restore_thread_vma_len);
 	if (hint == -1) {
-		pr_err("No suitable area for restorer bootstrap\n");
+		pr_err("No suitable area for task_restore bootstrap (%dK)\n",
+		       restore_task_vma_len + restore_thread_vma_len);
 		goto err;
 	} else
 		pr_info("Found bootstrap VMA hint at: %lx\n", hint);
 
-	/* VMA we need to run restorer code */
-	exec_mem = mmap((void *)hint, vma_len,
+	/* VMA we need to run task_restore code */
+	exec_mem = mmap((void *)hint, restore_task_vma_len + restore_thread_vma_len,
 			PROT_READ | PROT_WRITE | PROT_EXEC,
 			MAP_PRIVATE | MAP_ANON, 0, 0);
 	if (exec_mem == MAP_FAILED) {
@@ -1349,62 +1415,126 @@ static void sigreturn_restore(pid_t pid)
 		goto err;
 	}
 
-
 	/*
-	 * Prepare a stack for the restorer. It's a bit
+	 * Prepare a stack for the task_restore. It's a bit
 	 * tricky -- since compiler generates function
 	 * prologue we need to manually tune up stack
 	 * value.
 	 */
-	memzero(exec_mem, RESTORER_STACK_SIZE + RESTORER_STACK_FRAME);
-	exec_start = exec_mem + RESTORER_STACK_SIZE + RESTORER_STACK_FRAME;
+	memzero(exec_mem, RESTORE_STACK_SIZE + RESTORE_STACK_FRAME);
+	restore_task_exec_start = exec_mem + RESTORE_STACK_SIZE + RESTORE_STACK_FRAME;
 
 	/* Restorer content at the new location */
-	memcpy(exec_start, &restorer, code_len);
-	restorer_fcall = exec_start;
+	memcpy(restore_task_exec_start, &restore_task, restore_task_code_len);
 
 	/*
 	 * Adjust stack with red-zone area.
 	 */
-	new_sp = (long)exec_mem + RESTORER_STACK_SIZE - RESTORER_STACK_REDZONE;
+	new_sp = (long)exec_mem + RESTORE_STACK_SIZE - RESTORE_STACK_REDZONE;
+
+	/*
+	 * Thread restorer will be there.
+	 */
+	restore_thread_exec_start = (void *)((long)exec_mem + restore_task_vma_len);
+	memcpy(restore_thread_exec_start, &restore_thread, restore_thread_code_len);
 
 	/*
 	 * Pass arguments and run a command.
 	 */
-	args			= (struct restore_core_args *)(exec_start + args_offset);
-	args->rt_sigframe	= (void *)((long)exec_mem + RESTORER_STACK_SIZE + RESTORER_STACK_FRAME - RESTORER_STACK_REDZONE);
+	args			= (struct task_restore_core_args *)(restore_task_exec_start + args_offset);
+	args->rt_sigframe	= (void *)((long)exec_mem + RESTORE_STACK_SIZE + RESTORE_STACK_FRAME - RESTORE_STACK_REDZONE);
 	args->self_entry	= exec_mem;
-	args->self_size		= vma_len;
+	args->self_size		= restore_task_vma_len;
+	args->lock		= &rlock;
+	args->pid		= pid;
 
 	strcpy(args->self_vmas_path, path);
 
 	snprintf(path, sizeof(path), "core-%d.img.out", pid);
 	strcpy(args->core_path, path);
 
-	pr_info("vma_len: %li code_len: %li exec_mem: %p exec_start: %p new_sp: %p args: %p\n",
-		vma_len, code_len, exec_mem, exec_start, new_sp, args);
+	pr_info("restore_task_vma_len: %li restore_task_code_len: %li\n"
+		"exec_mem: %p restore_task_exec_start: %p new_sp: %p\n"
+		"args: %p args->rt_sigframe: %p\n"
+		"args->self_entry: %p args->self_size: %p\n"
+		"args->self_vmas_path: %p args->core_path: %p\n"
+		"args_offset: %li\n",
+		restore_task_vma_len, restore_task_code_len,
+		exec_mem, restore_task_exec_start, new_sp, args,
+		args->rt_sigframe, args->self_entry, args->self_size,
+		args->self_vmas_path, args->core_path,
+		args_offset);
+
+	if (pstree_entry.nr_threads) {
+
+		int i;
+
+		/*
+		 * Now prepare run-time data for threads restore.
+		 */
+		thread_args = (struct thread_restore_args *)
+				((long)restore_thread_exec_start +
+				 (long)restore_thread_code_len);
+
+		args->nr_threads	= (long)pstree_entry.nr_threads;
+		args->clone_restore_fn	= (void *)restore_thread_exec_start;
+		args->thread_args	= thread_args;
+
+		pr_info("args->nr_threads: %li\n"
+			"args->clone_restore_fn: %p\n"
+			"args->thread_args: %p\n",
+			args->nr_threads,
+			args->clone_restore_fn,
+			args->thread_args);
 
-	pr_info("args: %p args->rt_sigframe: %p args->self_entry: %p \nargs->self_size: %p "
-		"args->self_vmas_path: %p args->core_path: %p\n",
-		args, args->rt_sigframe, args->self_entry, args->self_size,
-		args->self_vmas_path, args->core_path);
+		/*
+		 * Fill up per-thread data.
+		 */
+		lseek(fd_pstree, sizeof(u32) * pstree_entry.nr_children, SEEK_CUR);
+		for (i = 0; i < pstree_entry.nr_threads; i++) {
+			read_ptr_safe(fd_pstree, &thread_args[i].pid, err);
+
+			thread_args[i].lock = args->lock;
+
+			snprintf(path, sizeof(path), "core-%d.img", thread_args[i].pid);
+			thread_args[i].fd_core = open(path, O_RDONLY, CR_FD_PERM);
+			if (thread_args[i].fd_core < 0) {
+				pr_perror("Can't open %s\n", path);
+				goto err;
+			}
+
+			pr_info("Thread %4d stack %8p heap %8p rt_sigframe %8p lock %8p\n",
+				i, (long)thread_args[i].stack,
+				thread_args[i].heap,
+				thread_args[i].rt_sigframe,
+				thread_args[i].lock);
+
+		}
+	}
+
+	close_safe(&fd_pstree);
 
 	/*
-	 * An indirect call to restorer, note it never resturns
+	 * An indirect call to task_restore, note it never resturns
 	 * and restoreing core is extremely destructive.
 	 */
 	asm volatile(
-		"movq %0, %%rbx						\t\n"
-		"movq %1, %%rax						\t\n"
-		"movl $"__stringify(RESTORER_CMD__RESTORE_CORE)", %%edi	\t\n"
-		"movq %%rbx, %%rsp					\t\n"
-		"callq *%%rax						\t\n"
+		"movq %0, %%rbx						\n"
+		"movq %1, %%rax						\n"
+		"movl $"__stringify(RESTORE_CMD__RESTORE_CORE)", %%edi	\n"
+		"movq %%rbx, %%rsp					\n"
+		"callq *%%rax						\n"
 		:
-		: "g"(new_sp), "g"(exec_start)
+		: "g"(new_sp), "g"(restore_task_exec_start)
 		: "rsp", "rdi", "rbx", "rax", "memory");
 
 err:
 	free_mappings(&self_vma_list);
+	close_safe(&fd_pstree);
+	close_safe(&fd_vmas);
+
+	if (exec_mem != MAP_FAILED)
+		munmap(exec_mem, restore_task_vma_len + restore_thread_vma_len);
 
 	/* Just to be sure */
 	sys_exit(0);

cr-show.c

@@ -334,8 +334,8 @@ static void show_pstree(struct list_head *head, char *name)
 	pr_info("----------------------------------------\n");
 
 	list_for_each_entry(item, head, list) {
-		pr_info("Process %d number of children: %d\n",
-			item->pid, item->nr_children);
+		pr_info("Process %d number of children: %d threads: %d\n",
+			item->pid, item->nr_children, item->nr_threads);
 		for (i = 0; i < item->nr_children; i++)
 			pr_info(" %d", item->children[i]);
 		if (item->nr_children)

include/compiler.h

@@ -27,6 +27,8 @@
 # define always_inline		__always_inline
 #endif
 
+#define __aligned(x)		__attribute__((aligned(x)))
+
 #ifndef offsetof
 # define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
 #endif

include/restorer.h

@@ -2,6 +2,7 @@
 #define CR_RESTORER_H__
 
 #include <signal.h>
+#include <limits.h>
 
 #include "compiler.h"
 #include "types.h"
@@ -11,31 +12,63 @@
 # error Only x86-64 is supported
 #endif
 
-#define RESTORER_ARGS_SIZE	512
-#define RESTORER_STACK_REDZONE	(128)
-#define RESTORER_STACK_FRAME	(16 << 10)
-#define RESTORER_STACK_SIZE	(32 << 10)
+/*
+ * These must! be power of two values.
+ */
+#define RESTORE_ARGS_SIZE		(512)
+#define RESTORE_STACK_REDZONE		(128)
+#define RESTORE_STACK_FRAME		(16 << 10)
+#define RESTORE_THREAD_STACK_SIZE	(16 << 10)
+#define RESTORE_THREAD_HEAP_SIZE	(16 << 10)
+#define RESTORE_STACK_SIZE		(32 << 10)
 
-extern long restorer(long cmd);
-
-typedef long (*restorer_fcall_t) (long cmd);
-
-#define RESTORER_CMD__NONE		0
-#define RESTORER_CMD__GET_ARG_OFFSET	1
-#define RESTORER_CMD__GET_SELF_LEN	2
-#define RESTORER_CMD__PR_ARG_STRING	3
-#define RESTORER_CMD__RESTORE_CORE	4
+#define RESTORE_CMD__NONE		0
+#define RESTORE_CMD__GET_ARG_OFFSET	1
+#define RESTORE_CMD__GET_SELF_LEN	2
+#define RESTORE_CMD__PR_ARG_STRING	3
+#define RESTORE_CMD__RESTORE_CORE	4
+#define RESTORE_CMD__RESTORE_THREAD	5
 
 #define ABI_RED_ZONE 128
 
 #define align_sigframe(sp)		round_down(sp, 16) - 8
 
-struct restore_core_args {
-	void	*self_entry;		/* restorer placed at */
-	void	*rt_sigframe;		/* sigframe placed at */
-	long	self_size;		/* size for restorer granted */
-	char	core_path[64];		/* path to a core file */
-	char	self_vmas_path[64];	/* path to a self-vmas file */
+typedef u32 rlock_t;
+#define RLOCK_T(v) rlock_t v __aligned(sizeof(u32)) = 0
+
+/* Make sure it's pow2 in size */
+struct thread_restore_args {
+	u32				pid;
+	u32				fd_core;
+	rlock_t				*lock;
+
+	u8				stack[RESTORE_THREAD_STACK_SIZE];
+	union {
+		struct core_entry	core_entry;
+		u8			heap[RESTORE_THREAD_HEAP_SIZE];
+	} __aligned(sizeof(long));
+	u8				rt_sigframe[RESTORE_STACK_FRAME];
+};
+
+extern long restore_task(long cmd);
+extern long restore_thread(long cmd, struct thread_restore_args *args);
+
+typedef long (*task_restore_fcall_t) (long cmd);
+typedef long (*thread_restore_fcall_t) (long cmd, struct thread_restore_args *args);
+
+struct task_restore_core_args {
+	void				*self_entry;		/* restorer placed at */
+	void				*rt_sigframe;		/* sigframe placed at */
+	long				self_size;		/* size for restorer granted */
+	char				core_path[64];
+	char				self_vmas_path[64];
+	u32				pid;
+	rlock_t				*lock;
+
+	/* threads restoration specifics */
+	thread_restore_fcall_t		clone_restore_fn;	/* helper address for clone() call */
+	long				nr_threads;		/* number of threads */
+	struct thread_restore_args	*thread_args;		/* array of thread arguments */
 };
 
 struct pt_regs {
@@ -137,4 +170,94 @@ struct rt_sigframe {
 	/* fp state follows here */
 };
 
+#define add_ord(c)			\
+	do {				\
+		if (c < 10)		\
+			c += '0';	\
+		else			\
+			c += 'a' - 10;	\
+	} while (0)
+
+static void always_inline write_char(char c)
+{
+	sys_write(1, &c, 1);
+}
+
+static void always_inline write_string(char *str)
+{
+	int len = 0;
+
+	while (str[len])
+		len++;
+
+	sys_write(1, str, len);
+}
+
+static void always_inline write_string_n(char *str)
+{
+	char new_line = '\n';
+
+	write_string(str);
+	sys_write(1, &new_line, 1);
+}
+
+static void always_inline write_hex_n(unsigned long num)
+{
+	unsigned char *s = (unsigned char *)&num;
+	unsigned char c;
+	int i;
+
+	for (i = sizeof(long)/sizeof(char) - 1; i >= 0; i--) {
+		c = (s[i] & 0xf0) >> 4;
+		add_ord(c);
+		sys_write(1, &c, 1);
+
+		c = (s[i] & 0x0f);
+		add_ord(c);
+		sys_write(1, &c, 1);
+	}
+
+	c = '\n';
+	sys_write(1, &c, 1);
+}
+
+#define FUTEX_WAIT		0
+#define FUTEX_WAKE		1
+#define FUTEX_FD		2
+#define FUTEX_REQUEUE		3
+#define FUTEX_CMP_REQUEUE	4
+#define FUTEX_WAKE_OP		5
+#define FUTEX_LOCK_PI		6
+#define FUTEX_UNLOCK_PI		7
+#define FUTEX_TRYLOCK_PI	8
+#define FUTEX_WAIT_BITSET	9
+#define FUTEX_WAKE_BITSET	10
+#define FUTEX_WAIT_REQUEUE_PI	11
+#define FUTEX_CMP_REQUEUE_PI	12
+
+static always_inline void r_lock(rlock_t *v)
+{
+	while (*v) {
+		asm volatile("lfence");
+		asm volatile("pause");
+	}
+	(*v)++;
+
+	asm volatile("sfence");
+}
+
+static always_inline void r_unlock(rlock_t *v)
+{
+	(*v)--;
+	asm volatile("sfence");
+}
+
+static always_inline void r_wait_unlock(rlock_t *v)
+{
+	while (*v) {
+		asm volatile("lfence");
+		asm volatile("pause");
+	}
+}
+
 #endif /* CR_RESTORER_H__ */

include/syscall-codes.h

@@ -19,13 +19,13 @@
 #define __NR_pause		34
 #define __NR_nanosleep		35
 #define __NR_getpid		39
+#define __NR_clone		56
 #define __NR_exit		60
 #define __NR_unlink		87
-
 #define __NR__sysctl		156
 #define __NR_prctl		157
 #define __NR_arch_prctl		158
-
+#define __NR_futex		202
 #define __NR_set_thread_area	205
 #define __NR_get_thread_area	211
 

include/syscall.h

@@ -232,6 +232,36 @@ static always_inline long sys_prctl(int code, unsigned long arg2, unsigned long
 	return syscall5(__NR_prctl, code, arg2, arg3, arg4, arg5);
 }
 
+static always_inline long sys_clone(unsigned long flags, void *child_stack,
+				    void *parent_tid, void *child_tid)
+{
+	return syscall4(__NR_clone, flags, (unsigned long)child_stack,
+			(unsigned long)parent_tid, (unsigned long)child_tid);
+}
+
+static always_inline long sys_futex(u32 *uaddr, int op, u32 val,
+				    struct timespec *utime,
+				    u32 *uaddr2, u32 val3)
+{
+	return syscall6(__NR_futex, (unsigned long)uaddr,
+			(unsigned long)op, (unsigned long)val,
+			(unsigned long)utime,
+			(unsigned long)uaddr2,
+			(unsigned long)val3);
+}
+
+static void always_inline local_sleep(long seconds)
+{
+	struct timespec req, rem;
+
+	req = (struct timespec){
+		.tv_sec		= seconds,
+		.tv_nsec	= 0,
+	};
+
+	sys_nanosleep(&req, &rem);
+}
+
 #else /* CONFIG_X86_64 */
 # error x86-32 bit mode not yet implemented
 #endif /* CONFIG_X86_64 */

restorer.c

@@ -6,6 +6,7 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <sched.h>
 
 #include "compiler.h"
 #include "types.h"
@@ -16,112 +17,165 @@
 #include "crtools.h"
 #include "restorer.h"
 
-#define get_rt_sigframe_addr(stack)					\
-	(struct rt_sigframe *)(stack - sizeof(long))
-
-#define lea_args_off(p)							\
+#define lea_args_off(to, label)						\
 	do {								\
 		asm volatile(						\
-			"leaq restore_args__(%%rip), %%rax	\n\t"	\
-			"movq %%rax, %0				\n\t"	\
-			: "=m"(p)					\
+			"leaq " #label "(%%rip), %%rax		\n"	\
+			"movq %%rax, %0				\n"	\
+			: "=m"(to)					\
 			:						\
 			: "memory");					\
 	} while (0)
 
-#define add_ord(c)			\
-	do {				\
-		if (c < 10)		\
-			c += '0';	\
-		else			\
-			c += 'a' - 10;	\
-	} while (0)
 
-static void always_inline write_char(char c)
+long restore_thread(long cmd, struct thread_restore_args *args)
 {
-	sys_write(1, &c, 1);
-}
+	long ret = -1;
 
-static void always_inline write_string(char *str)
-{
-	int len = 0;
+	switch (cmd) {
+	case RESTORE_CMD__RESTORE_THREAD:
+	{
+		struct core_entry *core_entry;
 
-	while (str[len])
-		len++;
+		struct user_fpregs_entry *fpregs;
+		struct user_regs_entry *gpregs;
+		struct rt_sigframe *rt_sigframe;
 
-	sys_write(1, str, len);
-}
+		unsigned long new_sp, fsgs_base;
 
-static void always_inline write_string_n(char *str)
-{
-	char new_line = '\n';
+		core_entry = &args->core_entry;
 
-	write_string(str);
-	sys_write(1, &new_line, 1);
-}
+		sys_lseek(args->fd_core, MAGIC_OFFSET, SEEK_SET);
+		ret = sys_read(args->fd_core, core_entry, sizeof(*core_entry));
+		if (ret != sizeof(*core_entry)) {
+			write_hex_n(__LINE__);
+			goto core_restore_end;
+		}
 
-static void always_inline write_hex_n(unsigned long num)
-{
-	unsigned char *s = (unsigned char *)&num;
-	unsigned char c;
-	int i;
-
-	for (i = sizeof(long)/sizeof(char) - 1; i >= 0; i--) {
-		c = (s[i] & 0xf0) >> 4;
-		add_ord(c);
-		sys_write(1, &c, 1);
-
-		c = (s[i] & 0x0f);
-		add_ord(c);
-		sys_write(1, &c, 1);
+		sys_close(args->fd_core);
+
+		rt_sigframe = (void *)args->rt_sigframe + 8;
+
+#define CPREGT1(d)	rt_sigframe->uc.uc_mcontext.d = core_entry->u.arch.gpregs.d
+#define CPREGT2(d,s)	rt_sigframe->uc.uc_mcontext.d = core_entry->u.arch.gpregs.s
+
+		CPREGT1(r8);
+		CPREGT1(r9);
+		CPREGT1(r10);
+		CPREGT1(r11);
+		CPREGT1(r12);
+		CPREGT1(r13);
+		CPREGT1(r14);
+		CPREGT1(r15);
+		CPREGT2(rdi, di);
+		CPREGT2(rsi, si);
+		CPREGT2(rbp, bp);
+		CPREGT2(rbx, bx);
+		CPREGT2(rdx, dx);
+		CPREGT2(rax, ax);
+		CPREGT2(rcx, cx);
+		CPREGT2(rsp, sp);
+		CPREGT2(rip, ip);
+		CPREGT2(eflags, flags);
+		CPREGT1(cs);
+		CPREGT1(gs);
+		CPREGT1(fs);
+
+		fsgs_base = core_entry->u.arch.gpregs.fs_base;
+		ret = sys_arch_prctl(ARCH_SET_FS, (void *)fsgs_base);
+		if (ret) {
+			write_hex_n(__LINE__);
+			write_hex_n(ret);
+			goto core_restore_end;
+		}
+
+		fsgs_base = core_entry->u.arch.gpregs.gs_base;
+		ret = sys_arch_prctl(ARCH_SET_GS, (void *)fsgs_base);
+		if (ret) {
+			write_hex_n(__LINE__);
+			write_hex_n(ret);
+			goto core_restore_end;
+		}
+
+		//r_unlock(args->lock);
+
+		new_sp = (long)rt_sigframe + 8;
+		asm volatile(
+			"movq %0, %%rax					\n"
+			"movq %%rax, %%rsp				\n"
+			"movl $"__stringify(__NR_rt_sigreturn)", %%eax	\n"
+			"syscall					\n"
+			:
+			: "r"(new_sp)
+			: "rax","rsp","memory");
+core_restore_end:
+		write_hex_n(__LINE__);
+		write_hex_n(sys_getpid());
+		for (;;)
+			local_sleep(5);
+		sys_exit(0);
 	}
+		break;
 
-	c = '\n';
-	sys_write(1, &c, 1);
-}
+	case RESTORE_CMD__GET_SELF_LEN:
+		goto self_len_start;
+self_len_end:
+		break;
 
-static void always_inline local_sleep(long seconds)
-{
-	struct timespec req, rem;
+	default:
+		goto core_restore_end;
+		break;
+	}
 
-	req = (struct timespec){
-		.tv_sec		= seconds,
-		.tv_nsec	= 0,
-	};
+	return ret;
 
-	sys_nanosleep(&req, &rem);
+self_len_start:
+	asm volatile(
+		".align 64				\n"
+		"self_thread:				\n"
+		"leaq self_thread(%%rip), %%rax		\n"
+		"addq $64, %%rax			\n"
+		"andq $~63, %%rax			\n"
+		"movq %%rax, %0				\n"
+		: "=r"(ret)
+		:
+		: "memory");
+	goto self_len_end;
 }
 
-long restorer(long cmd)
+long restore_task(long cmd)
 {
 	long ret = -1;
 
 	asm volatile(
-		"jmp 1f						\n\t"
-		"restore_args__:				\n\t"
-		".skip "__stringify(RESTORER_ARGS_SIZE)",0	\n\t"
-		"1:						\n\t"
+		"jmp 1f						\n"
+		"restore_args__:				\n"
+		".skip "__stringify(RESTORE_ARGS_SIZE)",0	\n"
+		"1:						\n"
 		:
 		:
 		: "memory");
 
+#define restore_lea_args_off(to)	\
+	lea_args_off(to, restore_args__)
+
 	switch (cmd) {
-	case RESTORER_CMD__PR_ARG_STRING:
+	case RESTORE_CMD__PR_ARG_STRING:
 	{
 		char *str = NULL;
 
-		lea_args_off(str);
+		restore_lea_args_off(str);
 		write_string(str);
 
 		ret = 0;
 	}
 		break;
 
-	case RESTORER_CMD__GET_ARG_OFFSET:
-		lea_args_off(ret);
+	case RESTORE_CMD__GET_ARG_OFFSET:
+		restore_lea_args_off(ret);
 		break;
 
-	case RESTORER_CMD__GET_SELF_LEN:
+	case RESTORE_CMD__GET_SELF_LEN:
 		goto self_len_start;
 self_len_end:
 		break;
@@ -132,11 +186,11 @@ self_len_end:
 	 * and jump execution to some predefined ip read from
 	 * core file.
 	 */
-	case RESTORER_CMD__RESTORE_CORE:
+	case RESTORE_CMD__RESTORE_CORE:
 	{
-		struct restore_core_args *args;
+		struct task_restore_core_args *args;
+		int fd_core, fd_thread;
 		int fd_self_vmas;
-		int fd_core;
 
 		struct core_entry core_entry;
 		struct vma_entry vma_entry;
@@ -146,9 +200,9 @@ self_len_end:
 		struct user_regs_entry *gpregs;
 		struct rt_sigframe *rt_sigframe;
 
-		unsigned long new_sp, *stack;
+		unsigned long new_sp, fsgs_base;
 
-		lea_args_off(args);
+		restore_lea_args_off(args);
 
 		write_string_n(args->core_path);
 		write_string_n(args->self_vmas_path);
@@ -228,11 +282,6 @@ self_len_end:
 			if (!(vma_entry.status & VMA_AREA_REGULAR))
 				continue;
 
-#if 0
-			vma_entry.fd	= -1UL; /* for a while */
-			vma_entry.pgoff	= 0;
-#endif
-
 			/*
 			 * Should map memory here. Note we map them as
 			 * writable since we're going to restore page
@@ -360,20 +409,16 @@ self_len_end:
 		CPREG1(gs);
 		CPREG1(fs);
 
-		/*
-		 * To update fsindex reload TLS segment
-		 * (otherwise TLS area might not be updated
-		 *  once program restored).
-		 */
-		ret = sys_arch_prctl(ARCH_SET_FS,
-				     &core_entry.u.arch.tls_array[FS_TLS].base_addr);
+		fsgs_base = core_entry.u.arch.gpregs.fs_base;
+		ret = sys_arch_prctl(ARCH_SET_FS, (void *)fsgs_base);
 		if (ret) {
 			write_hex_n(__LINE__);
 			write_hex_n(ret);
 			goto core_restore_end;
 		}
-		ret = sys_arch_prctl(ARCH_SET_GS,
-				     &core_entry.u.arch.tls_array[GS_TLS].base_addr);
+
+		fsgs_base = core_entry.u.arch.gpregs.gs_base;
+		ret = sys_arch_prctl(ARCH_SET_GS, (void *)fsgs_base);
 		if (ret) {
 			write_hex_n(__LINE__);
 			write_hex_n(ret);
@@ -385,6 +430,103 @@ self_len_end:
 		 */
 		rt_sigframe->uc.uc_sigmask.sig[0] = core_entry.task_sigset;
 
+		/*
+		 * Threads restoration. This requires some more comments. This
+		 * restorer routine and thread restorer routine has the following
+		 * memory map, prepared by a caller code.
+		 *
+		 * | <-- low addresses                                   high addresses --> |
+		 * +------------------------------------------------+-----------------------+
+		 * | own stack | rt_sigframe space | this proc body | thread restore zone   |
+		 * +------------------------------------------------+-----------------------+
+		 *        %sp->|       call %rip ->|
+		 *   params->|
+		 *
+		 * where each thread restore zone is the following
+		 *
+		 * | <-- low addresses                                     high addresses --> |
+		 * +--------------------------------------------------------------------------+
+		 * | thread restore proc | thread1 stack | thread1 heap | thread1 rt_sigframe |
+		 * +--------------------------------------------------------------------------+
+		 * |<- call %rip                   %sp ->|              |
+		 *                             params->| |              |
+		 *                                       |<-heap        |
+		 *                                                      |<-frame
+		 */
+
+		if (args->nr_threads) {
+			struct thread_restore_args *thread_args = args->thread_args;
+			long clone_flags = CLONE_VM | CLONE_FILES | CLONE_SIGHAND	|
+					   CLONE_THREAD | CLONE_SYSVSEM			|
+					   CLONE_CHILD_USEPID;
+			long parent_tid;
+			int i;
+
+			for (i = 0; i < args->nr_threads; i++) {
+
+				/* skip self */
+				if (thread_args[i].pid == args->pid)
+					continue;
+
+				new_sp = (long)thread_args[i].stack +
+						sizeof(thread_args[i].stack) -
+						ABI_RED_ZONE;
+
+				/* Threads will unlock it */
+				//r_lock(args->lock);
+
+				/*
+				 * To achieve functionality like libc's clone()
+				 * we need a pure assembly here, because clone()'ed
+				 * thread will run with own stack and we must not
+				 * have any additional instructions... oh, dear...
+				 */
+				asm volatile(
+					"clone_emul:				\n"
+					"movq %2, %%rsi				\n"
+					"subq $24, %%rsi			\n"
+					"movq %7, %%rdi				\n"
+					"movq %%rdi,16(%%rsi)			\n"
+					"movq %6, %%rdi				\n"
+					"movq %%rdi, 8(%%rsi)			\n"
+					"movq %5, %%rdi				\n"
+					"movq %%rdi, 0(%%rsi)			\n"
+					"movq %1, %%rdi				\n"
+					"movq %3, %%rdx				\n"
+					"movq %4, %%r10				\n"
+					"movl $"__stringify(__NR_clone)", %%eax	\n"
+					"syscall				\n"
+
+					"testq %%rax,%%rax			\n"
+					"jz thread_run				\n"
+
+					"movq %%rax, %0				\n"
+					"jmp clone_end				\n"
+
+					"thread_run:				\n"	/* new stack here */
+					"xorq %%rbp, %%rbp			\n"	/* clear ABI frame pointer */
+					"popq %%rax				\n"	/* clone_restore_fn  -- restore_thread */
+					"popq %%rdi				\n"	/* arguments */
+					"popq %%rsi				\n"
+					"callq *%%rax				\n"
+
+					"clone_end:				\n"
+					: "=r"(ret)
+					:	"g"(clone_flags),
+						"g"(new_sp),
+						"g"(&parent_tid),
+						"g"(&thread_args[i].pid),
+						"g"(args->clone_restore_fn),
+						"g"(RESTORE_CMD__RESTORE_THREAD),
+						"g"(&thread_args[i])
+					: "rax", "rdi", "rsi", "rdx", "r10", "memory");
+
+				//r_wait_unlock(args->lock);
+			}
+		}
+
+		//r_lock(args->lock);
+
 		/*
 		 * sigframe is on stack.
 		 */
@@ -396,10 +538,10 @@ self_len_end:
 		 * code insns from gcc.
 		 */
 		asm volatile(
-			"movq %0, %%rax					\t\n"
-			"movq %%rax, %%rsp				\t\n"
-			"movl $"__stringify(__NR_rt_sigreturn)", %%eax	\t\n"
-			"syscall					\t\n"
+			"movq %0, %%rax					\n"
+			"movq %%rax, %%rsp				\n"
+			"movl $"__stringify(__NR_rt_sigreturn)", %%eax	\n"
+			"syscall					\n"
 			:
 			: "r"(new_sp)
 			: "rax","rsp","memory");
@@ -414,7 +556,7 @@ core_restore_end:
 		break;
 
 	default:
-		ret = -1;
+		goto core_restore_end;
 		break;
 	}
 
@@ -422,12 +564,12 @@ core_restore_end:
 
 self_len_start:
 	asm volatile(
-		".align 64				\t\n"
-		"self:					\t\n"
-		"leaq self(%%rip), %%rax		\t\n"
-		"addq $64, %%rax			\t\n"
-		"andq $~63, %%rax			\t\n"
-		"movq %%rax, %0				\t\n"
+		".align 64				\n"
+		"self:					\n"
+		"leaq self(%%rip), %%rax		\n"
+		"addq $64, %%rax			\n"
+		"andq $~63, %%rax			\n"
+		"movq %%rax, %0				\n"
 		: "=r"(ret)
 		:
 		: "memory");

test/testee-threads.c

@@ -14,20 +14,70 @@
 #include <fcntl.h>
 #include <pthread.h>
 
+#define __NR_arch_prctl		158
+
+#define ARCH_SET_GS 0x1001
+#define ARCH_SET_FS 0x1002
+#define ARCH_GET_FS 0x1003
+#define ARCH_GET_GS 0x1004
+
+static long syscall2(int nr, unsigned long arg0, unsigned long arg1)
+{
+	long ret;
+	asm volatile(
+		"movl %1, %%eax		\t\n"
+		"movq %2, %%rdi		\t\n"
+		"movq %3, %%rsi		\t\n"
+		"syscall		\t\n"
+		"movq %%rax, %0		\t\n"
+		: "=r"(ret)
+		: "g" ((int)nr), "g" (arg0), "g" (arg1)
+		: "rax", "rdi", "rsi", "memory");
+	return ret;
+}
+
+static long sys_arch_prctl(int code, void *addr)
+{
+	return syscall2(__NR_arch_prctl, code, (unsigned long)addr);
+}
 
 static pthread_mutex_t mtx = PTHREAD_MUTEX_INITIALIZER;
 static int counter;
+static int thread_counter = 1;
+
+static __thread int tls_data;
+
+static void pr_fsgs_base(char *name)
+{
+	unsigned long fsgs_base = -1ul;
+	int ret;
+
+	ret = sys_arch_prctl(ARCH_GET_FS, &fsgs_base);
+
+	printf("%8d (%4s): (%2d) fsgs_base %8lx\n",
+	       getpid(), name, ret, fsgs_base);
+
+	ret = sys_arch_prctl(ARCH_GET_GS, &fsgs_base);
+
+	printf("%8d (%4s): (%2d) fsgs_base %8lx\n",
+	       getpid(), name, ret, fsgs_base);
+}
 
 static void *ff1(void *arg)
 {
 	void *map_unreadable = mmap(NULL, 1024, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
 	(void)map_unreadable;
 
+	tls_data = thread_counter++;
+
+	pr_fsgs_base("thr3");
+
 	while (1) {
 		pthread_mutex_lock(&mtx);
 
 		counter++;
-		printf("%d: Counter value: %d\n", getpid(), counter);
+		printf("%8d (thr3): Counter value: %4d tls_data = %4d\n",
+		       getpid(), counter, tls_data);
 
 		pthread_mutex_unlock(&mtx);
 		sleep(5);
@@ -52,11 +102,16 @@ static void *f1(void *arg)
 	if (pthread_create(&th, NULL, &ff1, NULL))
 		perror("Cant create thread");
 
+	tls_data = thread_counter++;
+
+	pr_fsgs_base("thr1");
+
 	while (1) {
 		pthread_mutex_lock(&mtx);
 
 		counter++;
-		printf("%d: Counter value: %d\n", getpid(), counter);
+		printf("%8d (thr1): Counter value: %4d tls_data = %4d\n",
+		       getpid(), counter, tls_data);
 
 		pthread_mutex_unlock(&mtx);
 		sleep(2);
@@ -70,11 +125,16 @@ static void *f2(void *arg)
 	void *map_unreadable = mmap(NULL, 1024, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
 	(void)map_unreadable;
 
+	tls_data = thread_counter++;
+
+	pr_fsgs_base("thr2");
+
 	while (1) {
 		pthread_mutex_lock(&mtx);
 
 		counter--;
-		printf("%d: Counter value: %d\n", getpid(), counter);
+		printf("%8d (thr2): Counter value: %4d tls_data = %4d\n",
+		       getpid(), counter, tls_data);
 
 		pthread_mutex_unlock(&mtx);
 		sleep(3);
@@ -90,12 +150,25 @@ int main(int argc, char *argv[])
 
 	printf("%s pid %d\n", argv[0], getpid());
 
+	tls_data = thread_counter++;
+
+	pr_fsgs_base("main");
+
+	printf("%8d (main): Counter value: %4d tls_data = %4d\n",
+	       getpid(), counter, tls_data);
+
 	rc1 = pthread_create(&th1, NULL, &f1, NULL);
 	rc2 = pthread_create(&th2, NULL, &f2, NULL);
 
 	if (rc1 | rc2)
 		exit(1);
 
+	while (1) {
+		printf("%8d (main): Counter value: %4d tls_data = %4d\n",
+		       getpid(), counter, tls_data);
+		sleep(2);
+	}
+
 	pthread_join(th1, NULL);
 	pthread_join(th2, NULL);