diff --git a/components/lwp/lwp.h b/components/lwp/lwp.h index 5901ee74dda6f88ca22c237b1ce976bb9318bb5d..fcecc76e69a24cae554eea4a53dd0d9f56ea6d97 100644 --- a/components/lwp/lwp.h +++ b/components/lwp/lwp.h @@ -92,7 +92,7 @@ struct rt_lwp struct rt_wqueue wait_queue; /*for console */ - struct lwp_avl_struct *futex_head; + struct lwp_avl_struct *address_search_head; /* for addressed object fast rearch */ #ifdef RT_USING_GDBSERVER int debug; uint32_t bak_first_ins; @@ -174,6 +174,12 @@ struct __pthread { #define FUTEX_WAIT 0 #define FUTEX_WAKE 1 +/* for pmutex op */ +#define PMUTEX_INIT 0 +#define PMUTEX_LOCK 1 +#define PMUTEX_UNLOCK 2 +#define PMUTEX_DESTROY 3 + #ifdef __cplusplus } #endif diff --git a/components/lwp/lwp_futex.c b/components/lwp/lwp_futex.c index f4704f42092e5dfcce41add7498647a84fa9f5d9..0d5afa859aebf2a49c139cbfce0ba93d848f90bd 100644 --- a/components/lwp/lwp_futex.c +++ b/components/lwp/lwp_futex.c @@ -73,11 +73,11 @@ struct rt_futex* futex_create(int *uaddr, struct rt_lwp *lwp) futex->uaddr = uaddr; futex->node.avl_key = (avl_key_t)uaddr; - futex->node.data = &lwp->futex_head; + futex->node.data = &lwp->address_search_head; rt_list_init(&(futex->waiting_thread)); /* insert into futex head */ - lwp_avl_insert(&futex->node, &lwp->futex_head); + lwp_avl_insert(&futex->node, &lwp->address_search_head); return futex; } @@ -86,7 +86,7 @@ static struct rt_futex* futex_get(void *uaddr, struct rt_lwp *lwp) struct rt_futex *futex = RT_NULL; struct lwp_avl_struct *node = RT_NULL; - node = lwp_avl_find((avl_key_t)uaddr, lwp->futex_head); + node = lwp_avl_find((avl_key_t)uaddr, lwp->address_search_head); if (!node) { return RT_NULL; diff --git a/components/lwp/lwp_pid.c b/components/lwp/lwp_pid.c index dff3d3f590aead0742945463f6577b0b43266643..42214936241a89b341d781a979ef64035b1e78da 100644 --- a/components/lwp/lwp_pid.c +++ b/components/lwp/lwp_pid.c @@ -111,7 +111,7 @@ struct rt_lwp* lwp_new(void) pid_struct.pidmap[i] = lwp; rt_list_init(&lwp->t_grp); rt_list_init(&lwp->object_list); - lwp->futex_head = RT_NULL; + lwp->address_search_head = RT_NULL; rt_wqueue_init(&lwp->wait_queue); lwp->ref = 1; @@ -126,14 +126,12 @@ static void lwp_user_obj_free(struct rt_lwp *lwp) struct rt_list_node *list = RT_NULL, *node = RT_NULL; struct rt_object *object = RT_NULL; - list = &(lwp->object_list), node = list->next; + list = &(lwp->object_list); level = rt_hw_interrupt_disable(); - while (list != node) + while ((node = list->next) != list) { object = rt_list_entry(node, struct rt_object, lwp_obj_list); - node = node->next; - /* remove from kernel object list */ switch (object->type) { @@ -168,12 +166,16 @@ static void lwp_user_obj_free(struct rt_lwp *lwp) rt_timer_delete((rt_timer_t)object); break; case RT_Object_Class_Channel: + /* remove from object list */ + rt_list_remove(&object->list); break; case RT_Object_Class_Custom: rt_custom_object_destroy(object); break; default: LOG_E("input object type(%d) error", object->type); + /* remove from object list */ + rt_list_remove(&object->list); break; } } diff --git a/components/lwp/lwp_pmutex.c b/components/lwp/lwp_pmutex.c new file mode 100644 index 0000000000000000000000000000000000000000..f60ae0046a3f1d358de5134552543853e211017e --- /dev/null +++ b/components/lwp/lwp_pmutex.c @@ -0,0 +1,286 @@ +/* + * Copyright (c) 2006-2021, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2021/01/02 bernard the first version + */ + +#include +#include +#ifdef RT_USING_USERSPACE +#include +#endif +#include "clock_time.h" + +struct rt_pmutex +{ + rt_mutex_t kmutex; + struct lwp_avl_struct node; + struct rt_object *custom_obj; +}; + +static struct rt_mutex _pmutex_lock; + +static int pmutex_system_init(void) +{ + rt_mutex_init(&_pmutex_lock, "pmtxLock", RT_IPC_FLAG_FIFO); + return 0; +} +INIT_PREV_EXPORT(pmutex_system_init); + +static void pmutex_destory(void *data) +{ + rt_base_t level = 0; + struct rt_pmutex *pmutex = (struct rt_pmutex *)data; + + if (pmutex) + { + level = rt_hw_interrupt_disable(); + /* remove pmutex from pmutext avl */ + lwp_avl_remove(&pmutex->node, (struct lwp_avl_struct **)pmutex->node.data); + rt_hw_interrupt_enable(level); + + rt_mutex_delete(pmutex->kmutex); + + /* release object */ + rt_free(pmutex); + } + + return ; +} + +static struct rt_pmutex* pmutex_create(void *umutex, struct rt_lwp *lwp) +{ + struct rt_pmutex *pmutex = RT_NULL; + struct rt_object *obj = RT_NULL; + + if (!lwp) + { + return RT_NULL; + } + pmutex = (struct rt_pmutex *)rt_malloc(sizeof(struct rt_pmutex)); + if (!pmutex) + { + return RT_NULL; + } + pmutex->kmutex = rt_mutex_create("pmutex", RT_IPC_FLAG_PRIO); + if (!pmutex->kmutex) + { + rt_free(pmutex); + return RT_NULL; + } + obj = rt_custom_object_create("pmutex", (void *)pmutex, pmutex_destory); + if (!obj) + { + rt_mutex_delete(pmutex->kmutex); + rt_free(pmutex); + return RT_NULL; + } + + pmutex->node.avl_key = (avl_key_t)umutex; + pmutex->node.data = &lwp->address_search_head; + pmutex->custom_obj = obj; + + /* insert into pmutex head */ + lwp_avl_insert(&pmutex->node, &lwp->address_search_head); + return pmutex; +} + +static struct rt_pmutex* pmutex_get(void *umutex, struct rt_lwp *lwp) +{ + struct rt_pmutex *pmutex = RT_NULL; + struct lwp_avl_struct *node = RT_NULL; + + node = lwp_avl_find((avl_key_t)umutex, lwp->address_search_head); + if (!node) + { + return RT_NULL; + } + pmutex = rt_container_of(node, struct rt_pmutex, node); + return pmutex; +} + +static int _pthread_mutex_init(void *umutex) +{ + struct rt_lwp *lwp = RT_NULL; + struct rt_pmutex *pmutex = RT_NULL; + rt_err_t lock_ret = 0; + + /* umutex union is 6 x (void *) */ + if (!lwp_user_accessable(umutex, sizeof(void *) * 6)) + { + rt_set_errno(EINVAL); + return -RT_EINVAL; + } + + lock_ret = rt_mutex_take_interruptible(&_pmutex_lock, RT_WAITING_FOREVER); + if (lock_ret != RT_EOK) + { + rt_set_errno(EAGAIN); + return -RT_EINTR; + } + + lwp = lwp_self(); + pmutex = pmutex_get(umutex, lwp); + if (pmutex == RT_NULL) + { + /* create a pmutex according to this umutex */ + pmutex = pmutex_create(umutex, lwp); + if (pmutex == RT_NULL) + { + rt_mutex_release(&_pmutex_lock); + rt_set_errno(ENOMEM); + return -RT_ENOMEM; + } + } + else + { + rt_base_t level = rt_hw_interrupt_disable(); + + pmutex->kmutex->value = 1; + pmutex->kmutex->owner = RT_NULL; + pmutex->kmutex->original_priority = 0xFF; + pmutex->kmutex->hold = 0; + + rt_hw_interrupt_enable(level); + } + + rt_mutex_release(&_pmutex_lock); + + return 0; +} + +static int _pthread_mutex_lock_timeout(void *umutex, struct timespec *timeout) +{ + struct rt_lwp *lwp = RT_NULL; + struct rt_pmutex *pmutex = RT_NULL; + rt_err_t lock_ret = 0; + rt_int32_t time = RT_WAITING_FOREVER; + + if (timeout) + { + if (!lwp_user_accessable((void *)timeout, sizeof(struct timespec))) + { + rt_set_errno(EINVAL); + return -RT_EINVAL; + } + time = clock_time_to_tick(timeout); + } + + lock_ret = rt_mutex_take_interruptible(&_pmutex_lock, RT_WAITING_FOREVER); + if (lock_ret != RT_EOK) + { + rt_set_errno(EAGAIN); + return -RT_EINTR; + } + + lwp = lwp_self(); + pmutex = pmutex_get(umutex, lwp); + if (pmutex == RT_NULL) + { + rt_mutex_release(&_pmutex_lock); + rt_set_errno(EINVAL); + return -RT_EINVAL; + } + + rt_mutex_release(&_pmutex_lock); + + lock_ret = rt_mutex_take_interruptible(pmutex->kmutex, time); + if (lock_ret != RT_EOK) + { + rt_set_errno(EAGAIN); + } + return lock_ret; +} + +static int _pthread_mutex_unlock(void *umutex) +{ + struct rt_lwp *lwp = RT_NULL; + struct rt_pmutex *pmutex = RT_NULL; + rt_err_t lock_ret = 0; + + lock_ret = rt_mutex_take_interruptible(&_pmutex_lock, RT_WAITING_FOREVER); + if (lock_ret != RT_EOK) + { + rt_set_errno(EAGAIN); + return -RT_EINTR; + } + + lwp = lwp_self(); + pmutex = pmutex_get(umutex, lwp); + if (pmutex == RT_NULL) + { + rt_mutex_release(&_pmutex_lock); + rt_set_errno(EINVAL); + return -RT_EINVAL; + } + + rt_mutex_release(&_pmutex_lock); + + lock_ret = rt_mutex_release(pmutex->kmutex); + if (lock_ret != RT_EOK) + { + rt_set_errno(EPERM); + } + return lock_ret; +} + +static int _pthread_mutex_destroy(void *umutex) +{ + struct rt_lwp *lwp = RT_NULL; + struct rt_pmutex *pmutex = RT_NULL; + rt_err_t lock_ret = 0; + rt_base_t level = 0; + + lock_ret = rt_mutex_take_interruptible(&_pmutex_lock, RT_WAITING_FOREVER); + if (lock_ret != RT_EOK) + { + rt_set_errno(EAGAIN); + return -RT_EINTR; + } + + lwp = lwp_self(); + pmutex = pmutex_get(umutex, lwp); + if (pmutex == RT_NULL) + { + rt_mutex_release(&_pmutex_lock); + rt_set_errno(EINVAL); + return -RT_EINVAL; + } + + level = rt_hw_interrupt_disable(); + rt_custom_object_destroy(pmutex->custom_obj); + rt_hw_interrupt_enable(level); + + rt_mutex_release(&_pmutex_lock); + + return RT_EOK; +} + +int sys_pmutex(void *umutex, int op, void *arg) +{ + int ret = -RT_EINVAL; + + switch (op) + { + case PMUTEX_INIT: + ret = _pthread_mutex_init(umutex); + break; + case PMUTEX_LOCK: + ret = _pthread_mutex_lock_timeout(umutex, (struct timespec*)arg); + break; + case PMUTEX_UNLOCK: + ret = _pthread_mutex_unlock(umutex); + break; + case PMUTEX_DESTROY: + ret = _pthread_mutex_destroy(umutex); + break; + default: + rt_set_errno(EINVAL); + break; + } + return ret; +} diff --git a/components/lwp/lwp_syscall.c b/components/lwp/lwp_syscall.c index bac6291fcad3fad5cf925e64dde8cfd195750395..781dd3176eea8351e8aebf8b9a2f53dec8c69ef7 100644 --- a/components/lwp/lwp_syscall.c +++ b/components/lwp/lwp_syscall.c @@ -2518,6 +2518,7 @@ int sys_clock_getres(clockid_t clk, struct timespec *ts) } int sys_futex(int *uaddr, int op, int val, void *timeout, void *uaddr2, int val3); +int sys_pmutex(void *umutex, int op, void *arg); const static void* func_table[] = { @@ -2668,6 +2669,7 @@ const static void* func_table[] = (void *)sys_clock_getres, (void *)sys_clone, /* 130 */ (void *)sys_futex, + (void *)sys_pmutex, }; const void *lwp_get_sys_api(rt_uint32_t number)