Merge pull request #218 from smartobjectoriented/215-rework-thread

rework thread creation to match pthread

Author: clemdiep

so3/arch/arm32/asm-offsets.c

@@ -78,6 +78,7 @@ int main(void)
 	DEFINE(OFFSET_PSR, offsetof(cpu_regs_t, psr));
 	DEFINE(OFFSET_SP_USR, offsetof(cpu_regs_t, sp_usr));
 	DEFINE(OFFSET_LR_USR, offsetof(cpu_regs_t, lr_usr));
+	DEFINE(OFFSET_TLS_USR, offsetof(cpu_regs_t, tls_usr));
 
 	BLANK();
 

so3/arch/arm32/context.S

@@ -31,17 +31,14 @@
 
 .global __switch_context
 .global __thread_prologue_kernel
-.global __thread_prologue_user
 .global __exec_prologue_user
-.global __thread_prologue_user_pre_launch
 
 .globl __get_syscall_args_ext
 .globl __get_syscall_arg
 
 .global __mmu_switch_ttbr0
 .global __exec
 .global __write
-.global __save_context
 
 .global __enable_vfp
 
@@ -51,7 +48,6 @@
 #ifdef CONFIG_MMU
 
 .extern __check_ptrace_traceme
-.extern ret_from_fork
 .extern pre_launch_proc
 
 #endif
@@ -84,39 +80,6 @@ __thread_prologue_kernel:
 
 	bl	thread_prologue
 
-@ User thread initial entry point
-@ Called once per thread
-@ r4: th_fn, r5: th_arg, r6: user stack
-__thread_prologue_user:
-
-	@ Prepare to jump into C code
-	mov r0, r4 @ tcb->th_fn
-	mov r1, r5 @ tcb->th_arg
-
-#ifdef CONFIG_MMU
-	@ Check if the thread must stopped because of ptrace/tracee
-	stmfd sp!, {r0, r1}
-	bl	__check_ptrace_traceme
-	ldmfd sp!, {r0, r1}
-#endif
-
-	@ IRQ enabling - must be done in SVC mode of course ;-)
-	@ We should take care about protecting against signal receipt:
-	@ since the stack is not initialized yet, the signal processing should be kept disabled.
-	cpsie   i
-
- 	@ Switch into user mode
- 	mrs  r4, cpsr
-	bic	r4, r4, #PSR_MODE_MASK
-	orr	r4, r4, #PSR_USR_MODE
- 	msr	 cpsr, r4
-
-	@ User stack initialisation
-	mov  sp, r6
-
-	bl	thread_prologue
-
-
 #ifdef CONFIG_AVZ
 
 ENTRY(cpu_do_idle)
@@ -208,37 +171,6 @@ __mmu_switch_ttbr0:
 	nop
 	nop
 
-@ Store the current registers into a cpu_regs structure passed in r0 (as first argument)
-__save_context:
-
-	@ Adjust the kernel stack pointer so that we can proceed with ret_from_fork
-	@ SVC_STACK_FRAME_SIZE/4 registers are preserved when at the syscall vector entry point
-
-	@ Adjust the sp which is stored on the stack. Make sure
-	@ it refers to this stack and not the one issue from the copy
-	@ as during fork().
-
-	str		r1, [r1, #(OFFSET_SP-SVC_STACK_FRAME_SIZE)]
-
-	sub		r2, r1, #SVC_STACK_FRAME_SIZE
-
-	@ Prepare to configure sp during the context switch.
-	str		r2, [r0, #(OFFSET_TCB_CPU_REGS + OFFSET_SP)]
-
-	@ Prepare the lr to branch to ret_from_fork
-	ldr		r1, .LCret_from_fork
-	str		r1, [r0, #(OFFSET_TCB_CPU_REGS + OFFSET_LR)]
-
-	@ Preserve r7 which contains the syscall number (used to compare against SIG_RETURN)
-	str		r7, [r0, #(OFFSET_TCB_CPU_REGS + OFFSET_R7)]
-
-	@ The other registers are not important.
-
-	mov 	pc, lr
-
-.LCret_from_fork:
-	.word ret_from_fork
-
 .LCcurrent:
 	.word current_thread
 

so3/arch/arm32/exception.S

@@ -118,6 +118,10 @@ __prepare_sig_handler:
 	tst	sp, #0x7	@ 8-bytes aligned
 	bne	__stack_alignment_fault
 
+	@ Copy TLS to the new stack frame
+	ldr	r0, [sp, #OFFSET_TLS_USR]
+	str	r0, [sp, #(-SVC_STACK_FRAME_SIZE + OFFSET_TLS_USR)]
+
 	str	sp, [sp, #(-SVC_STACK_FRAME_SIZE + OFFSET_SP)]	@ save sp
 
 	@ Build a new stack frame based on the current
@@ -169,7 +173,7 @@ __prepare_sig_handler:
 @ ARM EABI: the syscall nr is stored in r7
 .align  5
 syscall_interrupt:
- 
+
 	@ At the exception entry, the stack must be 8-byte aligned.
 	@ If it is not the case (gcc might not respect the AAPCS convention for optimization purposes),
 	@ sp will be adjusted. The original sp is preserved and will be correctly restored at the exit.
@@ -199,9 +203,13 @@ syscall_interrupt:
 	add	lr, sp, #OFFSET_SP_USR
 	stmia	lr, {sp, lr}^
 
+	@ Save user space TLS context
+	mrc	p15, 0, r0, c13, c0, 0
+	str 	r0, [sp, #OFFSET_TLS_USR]
+
 	cmp	r7, #SYSCALL_sigreturn
 	beq	__after_push_sp_usr
-	
+
 	ldr	r0, [sp, #OFFSET_SP_USR]
 	ldr 	r1, .LCcurrent
 	ldr 	r1, [r1]
@@ -254,6 +262,10 @@ __ret_from_fork:
 	check_pending_signal
 #endif /* CONFIG_IPC_SIGNAL */
 
+	@ Restore user space TLS context
+	ldr 	lr, [sp, #OFFSET_TLS_USR]
+	mcr	p15, 0, lr, c13, c0, 0
+
 	@ get the saved spsr and adjust the stack pointer
     	ldr 	lr, [sp, #OFFSET_PSR]
     	msr	spsr, lr
@@ -270,14 +282,14 @@ __ret_from_fork:
 
 	ldmia 	sp, {sp, lr, pc}^
 
- 
+
 
 @ Used at entry point of a fork'd process (setting the return value to 0)
 ret_from_fork:
 	mov	r0, #0
 
 	b	__ret_from_fork
- 
+
 .align  5
 prefetch_abort:
 
@@ -363,6 +375,10 @@ irq:
 	addeq	lr, sp, #OFFSET_SP_USR
 	stmeqia	lr, {sp, lr}^
 
+	@ Save user space TLS context
+	mrc	p15, 0, r0, c13, c0, 0
+	str 	r0, [sp, #OFFSET_TLS_USR]
+
 	@ Retrieve the lr_irq to set the pc out of this routine
 	ldr 	lr, [r0, #4]		@ retrieve lr_irq to set lr_svc
 	sub 	lr, lr, #4		@ Adjust the lr since it is automatically set from pc (in advance of 2 instructions due to the pipeline)
@@ -388,6 +404,10 @@ irq:
 	check_pending_signal
 #endif /* CONFIG_IPC_SIGNAL */
 
+	@ Restore user space TLS context
+	ldr 	lr, [sp, #OFFSET_TLS_USR]
+	mcr	p15, 0, lr, c13, c0, 0
+
     	ldr 	lr, [sp, #OFFSET_PSR]	@ get the saved spsr and adjust the stack pointer
     	msr	spsr, lr
 

so3/arch/arm32/include/asm/processor.h

@@ -349,9 +349,10 @@ typedef struct cpu_regs {
 	__u32   lr;
 	__u32   pc;
 	__u32   psr;
-	__u32	sp_usr;
+	__u32   sp_usr;
 	__u32   lr_usr;
-	__u32   padding;  /* padding to keep 8-bytes alignment */
+	__u32   tls_usr;
+	/* Already aligned to 8-bytes, no padding required */
 } cpu_regs_t;
 
 #define cpu_relax()	wfe()

so3/arch/arm32/thread.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (C) 2022 Daniel Rossier <daniel.rossier@heig-vd.ch>
- * 
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
@@ -16,21 +16,72 @@
  *
  */
 
+#include <process.h>
 #include <thread.h>
 #include <memory.h>
 
 /**
  * Set the CPU registers with thread related information
  *
- * @param tcb
+ * @param tcb Thread to set registers.
+ * @param args Clone arguments with values to set to registers.
  */
-void arch_prepare_cpu_regs(tcb_t *tcb)
+void arch_prepare_cpu_regs(tcb_t *tcb, clone_args_t *args)
 {
-	tcb->cpu_regs.r4 = (unsigned long) tcb->th_fn;
-	tcb->cpu_regs.r5 = (unsigned long) tcb->th_arg; /* First argument */
+	cpu_regs_t *user_regs;
+
+	if (args->pcb == NULL) {
+		/* Kernel thread must have a function to call */
+		BUG_ON(args->fn == NULL);
+		tcb->cpu_regs.r4 = (unsigned long) args->fn;
+		tcb->cpu_regs.r5 = (unsigned long) args->fn_arg;
+
+		tcb->cpu_regs.lr = (unsigned long) __thread_prologue_kernel;
+		tcb->cpu_regs.sp = get_kernel_stack_top(tcb->stack_slotID);
+	} else {
+		user_regs = (cpu_regs_t *) arch_get_kernel_stack_frame(tcb);
+
+		if (args->fn) {
+			/* Special userspace case to start root process. */
+			BUG_ON(args->stack == 0);
+			arch_restart_user_thread(tcb, args->fn, args->stack);
+		} else {
+			/* Normal userspace that will copy userspace registers */
+			if (args->stack)
+				user_regs->sp_usr = args->stack;
+
+			if (args->flags & CLONE_SETTLS)
+				user_regs->tls_usr = args->tls;
 
-	if (tcb->pcb)
-		tcb->cpu_regs.r6 = get_user_stack_top(tcb->pcb, tcb->pcb_stack_slotID);
+			/* Copy userspace registers */
+			*user_regs = *(cpu_regs_t *) arch_get_kernel_stack_frame(current());
+		}
+
+		tcb->cpu_regs.lr = (unsigned long) ret_from_fork;
+		/* Take into account the user registers frame on the kernel stack */
+		tcb->cpu_regs.sp = (addr_t) user_regs;
+	}
+}
+
+/**
+ * Restart a user thread by reseting all registers to 0 and settings entry point and stack.
+ *
+ * @param tcb Thread to restart.
+ * @param fn_entry Userspace entry point of the thread.
+ * @param stack_top New top address of the userspace stack.
+ */
+void arch_restart_user_thread(tcb_t *tcb, th_fn_t fn_entry, addr_t stack_top)
+{
+	cpu_regs_t *user_regs = (cpu_regs_t *) arch_get_kernel_stack_frame(tcb);
+
+	/* Reset all user's registers to zero except for PC, PSTATE and SP
+	 * which needs to be set to the properly start the thread.
+	 */
+	*user_regs = (cpu_regs_t) {
+		.pc = (u32) fn_entry,
+		.psr = PSR_USR_MODE,
+		.sp_usr = stack_top,
+	};
 }
 
 /**
@@ -41,3 +92,13 @@ addr_t arch_get_args_base(void)
 {
 	return (CONFIG_KERNEL_VADDR - PAGE_SIZE);
 }
+
+/**
+ * Get the top address of the kernel stack of a user thread with space for registers values.
+ *
+ * @param tcb Thread to get the stack address.
+ */
+addr_t arch_get_kernel_stack_frame(tcb_t *tcb)
+{
+	return get_kernel_stack_top(tcb->stack_slotID) - SVC_STACK_FRAME_SIZE;
+}

so3/arch/arm64/asm-offsets.c

@@ -85,6 +85,7 @@ int main(void)
 	DEFINE(OFFSET_SP, offsetof(struct cpu_regs, sp));
 	DEFINE(OFFSET_PC, offsetof(struct cpu_regs, pc));
 	DEFINE(OFFSET_PSTATE, offsetof(struct cpu_regs, pstate));
+	DEFINE(OFFSET_TLS_USR, offsetof(struct cpu_regs, tls_usr));
 
 	BLANK();