add thread clone support

components/lwp/Kconfig

@@ -32,3 +32,8 @@ config LWP_CONSOLE_INPUT_BUFFER_SIZE
     int "The input buffer size of lwp console device"
     default 1024
     depends on RT_USING_LWP
+
+config LWP_TID_MAX_NR
+    int "The maximum number of lwp thread id"
+    default 64
+    depends on RT_USING_LWP

components/lwp/arch/arm/cortex-a/lwp_gcc.S

@@ -62,6 +62,35 @@ set_user_context:
     cps #Mode_SVC
     mov pc, lr
 
+/*
+int lwp_set_thread_context(void *new_thread_stack, void *origin_thread_stack, void *user_stack, void **thread_sp, int tid);
+*/
+.global lwp_set_thread_context
+lwp_set_thread_context:
+    sub r12, r1, sp /* origin, r12 = origin sp offset */
+    sub r0, r0, r12 /* new sp */
+    stmfd r0!, {lr}
+    stmfd r0!, {lr}
+    stmfd r0!, {r12}
+    sub r12, sp, fp  /* new, r1 = origin fp offset */
+    sub r1, r0, r12
+    add r1, #12  /* new fp */
+    stmfd r0!, {r1} /* fp */
+    stmfd r0!, {r1 - r10}
+    mov r12, #0
+    stmfd r0!, {r12} /* new thread return value */
+    mrs r1, cpsr
+    stmfd r0!, {r1} /* spsr */
+    mov r1, #0
+    stmfd r0!, {r1} /* now user lr is 0 */
+    stmfd r0!, {r2} /* user sp */
+#ifdef RT_USING_FPU
+    stmfd r0!, {r1} /* not use fpu */
+#endif
+    str r0, [r3]
+    ldr r0, [sp]
+    mov pc, lr
+
 /*
  * void SVC_Handler(void);
  */

components/lwp/lwp.c

@@ -739,6 +739,7 @@ void lwp_cleanup(struct rt_thread *tid)
     level = rt_hw_interrupt_disable();
     lwp = (struct rt_lwp *)tid->lwp;
 
+    lwp_tid_put(tid->tid);
     rt_list_remove(&tid->sibling);
     lwp_ref_dec(lwp);
     rt_hw_interrupt_enable(level);
@@ -870,6 +871,7 @@ pid_t lwp_execve(char *filename, int argc, char **argv, char **envp)
         {
             struct rt_lwp *lwp_self;
 
+            tid->tid = 0;
             LOG_D("lwp kernel => (0x%08x, 0x%08x)\n", (rt_uint32_t)tid->stack_addr, (rt_uint32_t)tid->stack_addr + tid->stack_size);
             level = rt_hw_interrupt_disable();
             lwp_self = (struct rt_lwp *)rt_thread_self()->lwp;

components/lwp/lwp.h

@@ -116,6 +116,11 @@ void lwp_set_thread_area(void *p);
 void* rt_cpu_get_thread_idr(void);
 void rt_cpu_set_thread_idr(void *p);
 
+int lwp_tid_get(void);
+void lwp_tid_put(int tid);
+rt_thread_t lwp_tid_get_thread(int tid);
+void lwp_tid_set_thread(int tid, rt_thread_t thread);
+
 #ifdef RT_USING_USERSPACE
 void lwp_mmu_switch(struct rt_thread *thread);
 #endif

components/lwp/lwp_syscall.c

@@ -76,6 +76,8 @@ static void kmem_put(void *kptr)
 }
 #endif
 
+int sys_futex(int *uaddr, int op, int val, void *timeout, void *uaddr2, int val3);
+
 /* The same socket option is defined differently in the user interfaces and the
  * implementation. The options should be converted in the kernel. */
 
@@ -343,7 +345,7 @@ static void lwp_user_thread(void *parameter)
     user_stack &= ~7; //align 8
     set_user_context((void*)user_stack);
 
-    lwp_user_entry(parameter, tid->user_entry, lwp->data_entry, (void*)user_stack);
+    lwp_user_entry(parameter, tid->user_entry, lwp->data_entry, RT_NULL);
 }
 
 /* thread/process */
@@ -359,6 +361,14 @@ void sys_exit(int value)
     lwp = (struct rt_lwp*)tid->lwp;
 
     level = rt_hw_interrupt_disable();
+    if (tid->clear_child_tid)
+    {
+        int t = 0;
+        int *clear_child_tid = tid->clear_child_tid;
+
+        tid->clear_child_tid = RT_NULL;
+        lwp_put_to_user(clear_child_tid, &t, sizeof t);
+    }
     main_thread = rt_list_entry(lwp->t_grp.prev, struct rt_thread, sibling);
     if (main_thread == tid)
     {
@@ -1135,7 +1145,7 @@ rt_thread_t sys_thread_create(void *arg[])
     rt_base_t level;
     void *user_stack = 0;
     struct rt_lwp *lwp = 0;
-    rt_thread_t tid;
+    rt_thread_t thread;
 
     lwp = rt_thread_self()->lwp;
     lwp_ref_inc(lwp);
@@ -1147,30 +1157,31 @@ rt_thread_t sys_thread_create(void *arg[])
     if (!user_stack)
     {
         rt_set_errno(EINVAL);
-        return RT_NULL;
+        goto fail;
     }
-    tid = rt_thread_create((const char*)arg[0],
+    thread = rt_thread_create((const char*)arg[0],
             lwp_user_thread,
             (void*)arg[2],
             ALLOC_KERNEL_STACK_SIZE,
             (rt_uint8_t)(size_t)arg[4],
             (rt_uint32_t)arg[5]);
-    if (!tid)
+    if (!thread)
     {
         goto fail;
     }
 
-    tid->cleanup = lwp_cleanup;
-    tid->user_entry = (void (*)(void *))arg[1];
-    tid->user_stack = (void *)user_stack;
-    tid->user_stack_size = (uint32_t)arg[3];
-    tid->lwp = (void*)lwp;
+    thread->cleanup = lwp_cleanup;
+    thread->user_entry = (void (*)(void *))arg[1];
+    thread->user_stack = (void *)user_stack;
+    thread->user_stack_size = (uint32_t)arg[3];
+    thread->lwp = (void*)lwp;
+    thread->tid = 0;
 
     level = rt_hw_interrupt_disable();
-    rt_list_insert_after(&lwp->t_grp, &tid->sibling);
+    rt_list_insert_after(&lwp->t_grp, &thread->sibling);
     rt_hw_interrupt_enable(level);
 
-    return tid;
+    return thread;
 
 fail:
 #ifndef RT_USING_USERSPACE
@@ -1186,6 +1197,154 @@ fail:
     return RT_NULL;
 }
 
+#define CLONE_VM    0x00000100
+#define CLONE_FS    0x00000200
+#define CLONE_FILES 0x00000400
+#define CLONE_SIGHAND   0x00000800
+#define CLONE_PTRACE    0x00002000
+#define CLONE_VFORK 0x00004000
+#define CLONE_PARENT    0x00008000
+#define CLONE_THREAD    0x00010000
+#define CLONE_NEWNS 0x00020000
+#define CLONE_SYSVSEM   0x00040000
+#define CLONE_SETTLS    0x00080000
+#define CLONE_PARENT_SETTID 0x00100000
+#define CLONE_CHILD_CLEARTID    0x00200000
+#define CLONE_DETACHED  0x00400000
+#define CLONE_UNTRACED  0x00800000
+#define CLONE_CHILD_SETTID  0x01000000
+#define CLONE_NEWCGROUP 0x02000000
+#define CLONE_NEWUTS    0x04000000
+#define CLONE_NEWIPC    0x08000000
+#define CLONE_NEWUSER   0x10000000
+#define CLONE_NEWPID    0x20000000
+#define CLONE_NEWNET    0x40000000
+#define CLONE_IO    0x80000000
+
+/* arg[] -> flags
+ *          stack
+ *          new_tid
+ *          tls
+ *          set_clear_tid_address
+ *          quit_func
+ *          start_args
+ *          */
+#define SYS_CLONE_ARGS_NR 7
+int lwp_set_thread_context(void *new_thread_stack, void *origin_thread_stack, void *user_stack, void **thread_sp, int tid);
+long sys_clone(void *arg[])
+{
+    rt_base_t level;
+    struct rt_lwp *lwp = 0;
+    rt_thread_t thread = RT_NULL;
+    rt_thread_t self = RT_NULL;
+    int tid = -1;
+
+    unsigned long flags = 0;
+    void *user_stack = RT_NULL;
+    int *new_tid = RT_NULL;
+    void *tls = RT_NULL;
+    /*
+       musl call flags (CLONE_VM | CLONE_FS | CLONE_FILES | CLONE_SIGHAND
+       | CLONE_THREAD | CLONE_SYSVSEM | CLONE_SETTLS
+       | CLONE_PARENT_SETTID | CLONE_CHILD_CLEARTID | CLONE_DETACHED);
+       */
+
+    /* check args */
+    if (!lwp_user_accessable(arg, sizeof(void *[SYS_CLONE_ARGS_NR])))
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    flags = (unsigned long)(size_t)arg[0];
+    if ((flags & (CLONE_VM | CLONE_FS | CLONE_FILES | CLONE_THREAD | CLONE_SYSVSEM))
+            != (CLONE_VM | CLONE_FS | CLONE_FILES | CLONE_THREAD | CLONE_SYSVSEM))
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    user_stack = arg[1];
+    new_tid = (int *)arg[2];
+    tls = (void *)arg[3];
+
+    if ((flags & CLONE_PARENT_SETTID) == CLONE_PARENT_SETTID)
+    {
+        if (!lwp_user_accessable(new_tid, sizeof(int)))
+        {
+            rt_set_errno(EINVAL);
+            return -1;
+        }
+    }
+
+    self = rt_thread_self();
+    lwp = self->lwp;
+    lwp_ref_inc(lwp);
+    if (!user_stack)
+    {
+        rt_set_errno(EINVAL);
+        goto fail;
+    }
+    if ((tid = lwp_tid_get()) == -1)
+    {
+        rt_set_errno(ENOMEM);
+        goto fail;
+    }
+    thread = rt_thread_create((const char*)"pthread",
+            RT_NULL,
+            RT_NULL,
+            ALLOC_KERNEL_STACK_SIZE,
+            self->init_priority,
+            self->init_tick);
+    if (!thread)
+    {
+        goto fail;
+    }
+
+    thread->cleanup = lwp_cleanup;
+    thread->user_entry = RT_NULL;
+    thread->user_stack = RT_NULL;
+    thread->user_stack_size = 0;
+    thread->lwp = (void*)lwp;
+    thread->tid = tid;
+
+    if ((flags & CLONE_SETTLS) == CLONE_SETTLS)
+    {
+        thread->thread_idr = tls;
+    }
+    if ((flags & CLONE_PARENT_SETTID) == CLONE_PARENT_SETTID)
+    {
+        *new_tid = (int)(tid);
+    }
+    if ((flags & CLONE_CHILD_CLEARTID) == CLONE_CHILD_CLEARTID)
+    {
+        thread->clear_child_tid = (int*)arg[4];
+    }
+
+    level = rt_hw_interrupt_disable();
+    rt_list_insert_after(&lwp->t_grp, &thread->sibling);
+    rt_hw_interrupt_enable(level);
+
+    /* copy origin stack */
+    rt_memcpy(thread->stack_addr, self->stack_addr, ALLOC_KERNEL_STACK_SIZE);
+    lwp_tid_set_thread(tid, thread);
+    tid = lwp_set_thread_context((void*)((char*)thread->stack_addr + ALLOC_KERNEL_STACK_SIZE),
+            (void*)((char*)self->stack_addr + ALLOC_KERNEL_STACK_SIZE), user_stack, &thread->sp, tid);
+    if (tid)
+    {
+        rt_thread_startup(thread);
+    }
+    return (long)tid;
+
+fail:
+    lwp_tid_put(tid);
+    if (lwp)
+    {
+        lwp_ref_dec(lwp);
+    }
+    return -1;
+}
+
 rt_err_t sys_thread_delete(rt_thread_t thread)
 {
     return rt_thread_delete(thread);
@@ -2206,7 +2365,10 @@ int sys_set_thread_area(void *p)
 
 long sys_set_tid_address(int *tidptr)
 {
-    return 0;
+    rt_thread_t thread = rt_thread_self();
+
+    thread->clear_child_tid = tidptr;
+    return thread->tid;
 }
 
 int sys_access(const char *filename, int mode)
@@ -2400,47 +2562,47 @@ const static void* func_table[] =
     SYSCALL_USPACE(sys_mmap2),
     SYSCALL_USPACE(sys_munmap),
 
-    SYSCALL_USPACE(sys_shmget),
+    SYSCALL_USPACE(sys_shmget), /* 55 */
     SYSCALL_USPACE(sys_shmrm),
     SYSCALL_USPACE(sys_shmat),
     SYSCALL_USPACE(sys_shmdt),
 
     (void *)sys_device_init,
-    (void *)sys_device_register,
+    (void *)sys_device_register, /* 60 */
     (void *)sys_device_control,
     (void *)sys_device_find,
     (void *)sys_device_open,
     (void *)sys_device_close,
-    (void *)sys_device_read,
+    (void *)sys_device_read,    /* 65 */
     (void *)sys_device_write,
 
     (void *)sys_stat,
     (void *)sys_thread_find,
 
     SYSCALL_NET(sys_accept),
-    SYSCALL_NET(sys_bind),
+    SYSCALL_NET(sys_bind),      /* 70 */
     SYSCALL_NET(sys_shutdown),
     SYSCALL_NET(sys_getpeername),
     SYSCALL_NET(sys_getsockname),
     SYSCALL_NET(sys_getsockopt),
-    SYSCALL_NET(sys_setsockopt),
+    SYSCALL_NET(sys_setsockopt), /* 75 */
     SYSCALL_NET(sys_connect),
     SYSCALL_NET(sys_listen),
     SYSCALL_NET(sys_recv),
     SYSCALL_NET(sys_recvfrom),
-    SYSCALL_NET(sys_send),
+    SYSCALL_NET(sys_send),      /* 80 */
     SYSCALL_NET(sys_sendto),
     SYSCALL_NET(sys_socket),
 
     SYSCALL_NET(sys_closesocket),
     SYSCALL_NET(sys_getaddrinfo),
-    SYSCALL_NET(sys_gethostbyname2_r),
+    SYSCALL_NET(sys_gethostbyname2_r), /* 85 */
 
     (void *)sys_notimpl,    //(void *)network,
     (void *)sys_notimpl,    //(void *)network,
     (void *)sys_notimpl,    //(void *)network,
     (void *)sys_notimpl,    //(void *)network,
-    (void *)sys_notimpl,    //(void *)network,
+    (void *)sys_notimpl,    //(void *)network, /* 90 */
     (void *)sys_notimpl,    //(void *)network,
     (void *)sys_notimpl,    //(void *)network,
     (void *)sys_notimpl,    //(void *)network,
@@ -2451,44 +2613,45 @@ const static void* func_table[] =
     (void *)sys_notimpl,
 #endif
 
-    (void *)sys_notimpl,    //(void *)sys_hw_interrupt_disable,
+    (void *)sys_notimpl,    //(void *)sys_hw_interrupt_disable, /* 95 */
     (void *)sys_notimpl,    //(void *)sys_hw_interrupt_enable,
 
     (void *)sys_tick_get,
     (void *)sys_exit_group,
 
     (void *)sys_notimpl,    //(void *)rt_delayed_work_init,
-    (void *)sys_notimpl,    //(void *)rt_work_submit,
+    (void *)sys_notimpl,    //(void *)rt_work_submit,           /* 100 */
     (void *)sys_notimpl,    //(void *)rt_wqueue_wakeup,
     (void *)sys_thread_mdelay,
-    (void*)sys_sigaction,
-    (void*)sys_sigprocmask,
-    (void*)sys_thread_kill,
-    (void*)sys_thread_sighandler_set,
-    (void*)sys_thread_sigprocmask,
-    (void*)sys_notimpl,
-    (void*)sys_notimpl,
-    (void*)sys_waitpid,
+    (void *)sys_sigaction,
+    (void *)sys_sigprocmask,
+    (void *)sys_thread_kill,      /* 105 */
+    (void *)sys_thread_sighandler_set,
+    (void *)sys_thread_sigprocmask,
+    (void *)sys_notimpl,
+    (void *)sys_notimpl,
+    (void *)sys_waitpid,          /* 110 */
 
     (void *)sys_timer_create,
     (void *)sys_timer_delete,
     (void *)sys_timer_start,
     (void *)sys_timer_stop,
-    (void *)sys_timer_control,
+    (void *)sys_timer_control,  /* 115 */
     (void *)sys_getcwd,
     (void *)sys_chdir,
     (void *)sys_unlink,
     (void *)sys_mkdir,
-    (void *)sys_rmdir,
+    (void *)sys_rmdir,          /* 120 */
     (void *)sys_getdents,
     (void *)sys_get_errno,
     (void *)sys_set_thread_area,
     (void *)sys_set_tid_address,
-    (void *)sys_access,
+    (void *)sys_access,         /* 125 */
     (void *)sys_pipe,
     (void *)sys_clock_settime,
     (void *)sys_clock_gettime,
     (void *)sys_clock_getres,
+    (void *)sys_clone,           /* 130 */
 };
 
 const void *lwp_get_sys_api(rt_uint32_t number)

components/lwp/lwp_tid.c

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2006-2021, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2021-01-15     shaojinchun  first version
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+
+#include "lwp.h"
+
+#ifdef RT_USING_USERSPACE
+#include "lwp_user_mm.h"
+
+#ifdef RT_USING_GDBSERVER
+#include <hw_breakpoint.h>
+#include <lwp_gdbserver.h>
+#endif
+
+#endif
+
+#define DBG_TAG    "LWP_TID"
+#define DBG_LVL    DBG_INFO
+#include <rtdbg.h>
+
+static rt_thread_t lwp_tid_ary[LWP_TID_MAX_NR + 1];
+static rt_thread_t *lwp_tid_free_head = RT_NULL;
+static int lwp_tid_ary_alloced = 1; /* 0 is reserved */
+
+int lwp_tid_get(void)
+{
+    int ret = 0;
+    rt_base_t level = rt_hw_interrupt_disable();
+    rt_thread_t *p = lwp_tid_free_head;
+
+    if (p)
+    {
+        lwp_tid_free_head = (rt_thread_t*)*p;
+    }
+    else if (lwp_tid_ary_alloced < LWP_TID_MAX_NR)
+    {
+        p = lwp_tid_ary + lwp_tid_ary_alloced;
+        lwp_tid_ary_alloced++;
+    }
+    if (p)
+    {
+        *p = RT_NULL;
+        ret = p - lwp_tid_ary;
+    }
+    rt_hw_interrupt_enable(level);
+    return ret;
+}
+
+void lwp_tid_put(int tid)
+{
+    rt_thread_t *p = RT_NULL;
+    rt_base_t level = rt_hw_interrupt_disable();
+
+    if (tid > 0 && tid < LWP_TID_MAX_NR)
+    {
+        p = lwp_tid_ary + tid;
+        *p = (rt_thread_t)lwp_tid_free_head;
+        lwp_tid_free_head = p;
+    }
+    rt_hw_interrupt_enable(level);
+}
+
+rt_thread_t lwp_tid_get_thread(int tid)
+{
+    rt_thread_t thread = RT_NULL;
+
+    if (tid > 0 && tid < LWP_TID_MAX_NR)
+    {
+        thread = lwp_tid_free_head[tid];
+    }
+    return thread;
+}
+
+void lwp_tid_set_thread(int tid, rt_thread_t thread)
+{
+    if (tid > 0 && tid < LWP_TID_MAX_NR)
+    {
+        lwp_tid_ary[tid] = thread;
+    }
+}

include/rtdef.h

@@ -734,6 +734,8 @@ struct rt_thread
     int debug_suspend;
     struct rt_hw_exp_stack *regs;
     void * thread_idr;                                 /** lwp thread indicator */
+    int tid;
+    int *clear_child_tid;
 #endif
 #endif