OVMS3-idf/components/pthread/pthread.c

589 lines
16 KiB
C
Raw Normal View History

2017-09-04 18:46:16 +00:00
// Copyright 2017 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// This module implements pthread API on top of FreeRTOS. API is implemented to the level allowing
// libstdcxx threading framework to operate correctly. So not all original pthread routines are supported.
// Moreover some implemened functions do not provide full functionality, e.g. pthread_create does not support
// thread's attributes customization (prio, stack size and so on). So if you are not satisfied with default
// behavior use native FreeRTOS API.
//
2017-08-23 23:53:20 +00:00
#include <errno.h>
#include <pthread.h>
#include <string.h>
#include "esp_err.h"
#include "esp_attr.h"
#include "rom/queue.h"
2017-08-23 23:53:20 +00:00
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "pthread_internal.h"
#include "esp_pthread.h"
2017-08-31 23:18:14 +00:00
#define LOG_LOCAL_LEVEL CONFIG_LOG_DEFAULT_LEVEL
2017-08-23 23:53:20 +00:00
#include "esp_log.h"
const static char *TAG = "pthread";
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
/** task state */
enum esp_pthread_task_state {
PTHREAD_TASK_STATE_RUN,
PTHREAD_TASK_STATE_EXIT
};
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
/** pthread thread FreeRTOS wrapper */
typedef struct esp_pthread_entry {
SLIST_ENTRY(esp_pthread_entry) list_node; ///< Tasks list node struct.
TaskHandle_t handle; ///< FreeRTOS task handle
TaskHandle_t join_task; ///< Handle of the task waiting to join
enum esp_pthread_task_state state; ///< pthread task state
bool detached; ///< True if pthread is detached
2017-08-23 23:53:20 +00:00
} esp_pthread_t;
2017-08-31 23:18:14 +00:00
/** pthread wrapper task arg */
2017-08-23 23:53:20 +00:00
typedef struct {
2017-08-31 23:18:14 +00:00
void *(*func)(void *); ///< user task entry
void *arg; ///< user task argument
esp_pthread_cfg_t cfg; ///< pthread configuration
2017-08-23 23:53:20 +00:00
} esp_pthread_task_arg_t;
2017-08-31 23:18:14 +00:00
/** pthread mutex FreeRTOS wrapper */
typedef struct {
2017-08-31 23:18:14 +00:00
SemaphoreHandle_t sem; ///< Handle of the task waiting to join
2017-09-04 18:46:16 +00:00
int type; ///< Mutex type. Currently supported PTHREAD_MUTEX_NORMAL and PTHREAD_MUTEX_RECURSIVE
} esp_pthread_mutex_t;
static SemaphoreHandle_t s_threads_mux = NULL;
static portMUX_TYPE s_mutex_init_lock = portMUX_INITIALIZER_UNLOCKED;
static SLIST_HEAD(esp_thread_list_head, esp_pthread_entry) s_threads_list
= SLIST_HEAD_INITIALIZER(s_threads_list);
static pthread_key_t s_pthread_cfg_key;
2017-08-23 23:53:20 +00:00
static int IRAM_ATTR pthread_mutex_lock_internal(esp_pthread_mutex_t *mux, TickType_t tmo);
static void esp_pthread_cfg_key_destructor(void *value)
{
free(value);
}
esp_err_t esp_pthread_init(void)
2017-08-23 23:53:20 +00:00
{
if (pthread_key_create(&s_pthread_cfg_key, esp_pthread_cfg_key_destructor) != 0) {
return ESP_ERR_NO_MEM;
}
2017-08-31 23:18:14 +00:00
s_threads_mux = xSemaphoreCreateMutex();
if (s_threads_mux == NULL) {
pthread_key_delete(s_pthread_cfg_key);
2017-08-31 23:18:14 +00:00
return ESP_ERR_NO_MEM;
}
return ESP_OK;
2017-08-23 23:53:20 +00:00
}
static void *pthread_list_find_item(void *(*item_check)(esp_pthread_t *, void *arg), void *check_arg)
2017-08-23 23:53:20 +00:00
{
esp_pthread_t *it;
SLIST_FOREACH(it, &s_threads_list, list_node) {
void *val = item_check(it, check_arg);
2017-08-31 23:18:14 +00:00
if (val) {
return val;
}
}
return NULL;
2017-08-23 23:53:20 +00:00
}
static void *pthread_get_handle_by_desc(esp_pthread_t *item, void *desc)
2017-08-31 23:18:14 +00:00
{
if (item == desc) {
return item->handle;
2017-08-31 23:18:14 +00:00
}
return NULL;
}
static void *pthread_get_desc_by_handle(esp_pthread_t *item, void *hnd)
2017-08-31 23:18:14 +00:00
{
if (hnd == item->handle) {
return item;
2017-08-31 23:18:14 +00:00
}
return NULL;
}
static inline TaskHandle_t pthread_find_handle(pthread_t thread)
{
return pthread_list_find_item(pthread_get_handle_by_desc, (void *)thread);
}
2017-08-23 23:53:20 +00:00
static esp_pthread_t *pthread_find(TaskHandle_t task_handle)
{
return pthread_list_find_item(pthread_get_desc_by_handle, task_handle);
2017-08-23 23:53:20 +00:00
}
static void pthread_delete(esp_pthread_t *pthread)
{
SLIST_REMOVE(&s_threads_list, pthread, esp_pthread_entry, list_node);
2017-08-31 23:18:14 +00:00
free(pthread);
}
/* Call this function to configure pthread stacks in Pthreads */
esp_err_t esp_pthread_set_cfg(const esp_pthread_cfg_t *cfg)
{
/* If a value is already set, update that value */
esp_pthread_cfg_t *p = pthread_getspecific(s_pthread_cfg_key);
if (!p) {
p = malloc(sizeof(esp_pthread_cfg_t));
if (!p) {
return ESP_ERR_NO_MEM;
}
}
*p = *cfg;
pthread_setspecific(s_pthread_cfg_key, p);
return 0;
}
esp_err_t esp_pthread_get_cfg(esp_pthread_cfg_t *p)
{
esp_pthread_cfg_t *cfg = pthread_getspecific(s_pthread_cfg_key);
if (cfg) {
*p = *cfg;
return ESP_OK;
}
memset(p, 0, sizeof(*p));
return ESP_ERR_NOT_FOUND;
}
2017-08-23 23:53:20 +00:00
static void pthread_task_func(void *arg)
{
2017-08-31 23:18:14 +00:00
esp_pthread_task_arg_t *task_arg = (esp_pthread_task_arg_t *)arg;
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "%s ENTER %p", __FUNCTION__, task_arg->func);
// wait for start
xTaskNotifyWait(0, 0, NULL, portMAX_DELAY);
2017-08-23 23:53:20 +00:00
if (task_arg->cfg.inherit_cfg) {
/* If inherit option is set, then do a set_cfg() ourselves for future forks */
esp_pthread_set_cfg(&task_arg->cfg);
}
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "%s START %p", __FUNCTION__, task_arg->func);
task_arg->func(task_arg->arg);
ESP_LOGV(TAG, "%s END %p", __FUNCTION__, task_arg->func);
free(task_arg);
2017-08-23 23:53:20 +00:00
/* preemptively clean up thread local storage, rather than
waiting for the idle task to clean up the thread */
pthread_internal_local_storage_destructor_callback();
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) != pdTRUE) {
2017-08-31 23:18:14 +00:00
assert(false && "Failed to lock threads list!");
}
esp_pthread_t *pthread = pthread_find(xTaskGetCurrentTaskHandle());
if (!pthread) {
assert(false && "Failed to find pthread for current task!");
}
if (pthread->detached) {
// auto-free for detached threads
pthread_delete(pthread);
} else {
// Remove from list, it indicates that task has exited
if (pthread->join_task) {
// notify join
xTaskNotify(pthread->join_task, 0, eNoAction);
} else {
pthread->state = PTHREAD_TASK_STATE_EXIT;
}
}
xSemaphoreGive(s_threads_mux);
ESP_LOGD(TAG, "Task stk_wm = %d", uxTaskGetStackHighWaterMark(NULL));
2017-08-31 23:18:14 +00:00
vTaskDelete(NULL);
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "%s EXIT", __FUNCTION__);
2017-08-23 23:53:20 +00:00
}
int pthread_create(pthread_t *thread, const pthread_attr_t *attr,
void *(*start_routine) (void *), void *arg)
{
2017-08-31 23:18:14 +00:00
TaskHandle_t xHandle = NULL;
ESP_LOGV(TAG, "%s", __FUNCTION__);
if (attr) {
2017-09-04 18:46:16 +00:00
ESP_LOGE(TAG, "%s: attrs not supported!", __FUNCTION__);
return ENOSYS;
2017-08-31 23:18:14 +00:00
}
esp_pthread_task_arg_t *task_arg = malloc(sizeof(esp_pthread_task_arg_t));
if (task_arg == NULL) {
ESP_LOGE(TAG, "Failed to allocate task args!");
return ENOMEM;
}
memset(task_arg, 0, sizeof(esp_pthread_task_arg_t));
esp_pthread_t *pthread = malloc(sizeof(esp_pthread_t));
if (pthread == NULL) {
ESP_LOGE(TAG, "Failed to allocate pthread data!");
free(task_arg);
return ENOMEM;
}
uint32_t stack_size = CONFIG_ESP32_PTHREAD_TASK_STACK_SIZE_DEFAULT;
BaseType_t prio = CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT;
esp_pthread_cfg_t *pthread_cfg = pthread_getspecific(s_pthread_cfg_key);
if (pthread_cfg) {
if (pthread_cfg->stack_size) {
stack_size = pthread_cfg->stack_size;
}
if (pthread_cfg->prio && pthread_cfg->prio < configMAX_PRIORITIES) {
prio = pthread_cfg->prio;
}
task_arg->cfg = *pthread_cfg;
}
2017-08-31 23:18:14 +00:00
memset(pthread, 0, sizeof(esp_pthread_t));
task_arg->func = start_routine;
task_arg->arg = arg;
BaseType_t res = xTaskCreate(&pthread_task_func, "pthread", stack_size,
task_arg, prio, &xHandle);
2017-08-31 23:18:14 +00:00
if(res != pdPASS) {
ESP_LOGE(TAG, "Failed to create task!");
free(pthread);
free(task_arg);
if (res == errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY) {
return ENOMEM;
} else {
return EAGAIN;
}
}
pthread->handle = xHandle;
2017-08-23 23:53:20 +00:00
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) != pdTRUE) {
2017-08-31 23:18:14 +00:00
assert(false && "Failed to lock threads list!");
2017-08-23 23:53:20 +00:00
}
SLIST_INSERT_HEAD(&s_threads_list, pthread, list_node);
xSemaphoreGive(s_threads_mux);
2017-08-31 23:18:14 +00:00
// start task
xTaskNotify(xHandle, 0, eNoAction);
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
*thread = (pthread_t)pthread; // pointer value fit into pthread_t (uint32_t)
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "Created task %x", (uint32_t)xHandle);
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
return 0;
2017-08-23 23:53:20 +00:00
}
int pthread_join(pthread_t thread, void **retval)
{
2017-08-31 23:18:14 +00:00
esp_pthread_t *pthread = (esp_pthread_t *)thread;
int ret = 0;
bool wait = false;
2017-08-23 23:53:20 +00:00
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "%s %p", __FUNCTION__, pthread);
2017-08-31 23:18:14 +00:00
// find task
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) != pdTRUE) {
2017-08-31 23:18:14 +00:00
assert(false && "Failed to lock threads list!");
}
TaskHandle_t handle = pthread_find_handle(thread);
if (!handle) {
2017-09-04 18:46:16 +00:00
// not found
2017-08-31 23:18:14 +00:00
ret = ESRCH;
} else if (pthread->join_task) {
2017-09-04 18:46:16 +00:00
// already have waiting task to join
2017-08-31 23:18:14 +00:00
ret = EINVAL;
} else if (handle == xTaskGetCurrentTaskHandle()) {
2017-09-04 18:46:16 +00:00
// join to self not allowed
2017-08-31 23:18:14 +00:00
ret = EDEADLK;
} else {
2017-08-31 23:18:14 +00:00
esp_pthread_t *cur_pthread = pthread_find(xTaskGetCurrentTaskHandle());
if (cur_pthread && cur_pthread->join_task == handle) {
2017-09-04 18:46:16 +00:00
// join to each other not allowed
2017-08-31 23:18:14 +00:00
ret = EDEADLK;
} else {
if (pthread->state == PTHREAD_TASK_STATE_RUN) {
pthread->join_task = xTaskGetCurrentTaskHandle();
wait = true;
2017-08-31 23:18:14 +00:00
} else {
pthread_delete(pthread);
}
}
2017-08-23 23:53:20 +00:00
}
xSemaphoreGive(s_threads_mux);
if (ret == 0 && wait) {
2017-08-31 23:18:14 +00:00
xTaskNotifyWait(0, 0, NULL, portMAX_DELAY);
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) != pdTRUE) {
assert(false && "Failed to lock threads list!");
}
pthread_delete(pthread);
xSemaphoreGive(s_threads_mux);
2017-08-23 23:53:20 +00:00
}
2017-08-23 23:53:20 +00:00
if (retval) {
2017-08-31 23:18:14 +00:00
*retval = 0; // no exit code in FreeRTOS
2017-08-23 23:53:20 +00:00
}
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "%s %p EXIT %d", __FUNCTION__, pthread, ret);
return ret;
2017-08-23 23:53:20 +00:00
}
int pthread_detach(pthread_t thread)
{
2017-08-31 23:18:14 +00:00
esp_pthread_t *pthread = (esp_pthread_t *)thread;
int ret = 0;
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) != pdTRUE) {
2017-08-31 23:18:14 +00:00
assert(false && "Failed to lock threads list!");
}
TaskHandle_t handle = pthread_find_handle(thread);
if (!handle) {
2017-08-31 23:18:14 +00:00
ret = ESRCH;
} else {
pthread->detached = true;
}
xSemaphoreGive(s_threads_mux);
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "%s %p EXIT %d", __FUNCTION__, pthread, ret);
return ret;
2017-08-23 23:53:20 +00:00
}
int pthread_cancel(pthread_t thread)
{
2017-09-04 18:46:16 +00:00
ESP_LOGE(TAG, "%s: not supported!", __FUNCTION__);
return ENOSYS;
2017-08-23 23:53:20 +00:00
}
int sched_yield( void )
{
2017-08-31 23:18:14 +00:00
vTaskDelay(0);
return 0;
}
2017-08-23 23:53:20 +00:00
pthread_t pthread_self(void)
{
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) != pdTRUE) {
2017-08-31 23:18:14 +00:00
assert(false && "Failed to lock threads list!");
}
esp_pthread_t *pthread = pthread_find(xTaskGetCurrentTaskHandle());
if (!pthread) {
assert(false && "Failed to find current thread ID!");
}
xSemaphoreGive(s_threads_mux);
2017-08-31 23:18:14 +00:00
return (pthread_t)pthread;
2017-08-23 23:53:20 +00:00
}
int pthread_equal(pthread_t t1, pthread_t t2)
{
2017-08-31 23:18:14 +00:00
return t1 == t2 ? 1 : 0;
2017-08-23 23:53:20 +00:00
}
/***************** ONCE ******************/
int pthread_once(pthread_once_t *once_control, void (*init_routine)(void))
{
2017-08-31 23:18:14 +00:00
if (once_control == NULL || init_routine == NULL || !once_control->is_initialized) {
ESP_LOGE(TAG, "%s: Invalid args!", __FUNCTION__);
return EINVAL;
}
2017-08-23 23:53:20 +00:00
uint32_t res = 1;
#if defined(CONFIG_SPIRAM_SUPPORT)
if (esp_ptr_external_ram(once_control)) {
uxPortCompareSetExtram((uint32_t *) &once_control->init_executed, 0, &res);
} else {
#endif
uxPortCompareSet((uint32_t *) &once_control->init_executed, 0, &res);
#if defined(CONFIG_SPIRAM_SUPPORT)
}
#endif
// Check if compare and set was successful
if (res == 0) {
ESP_LOGV(TAG, "%s: call init_routine %p", __FUNCTION__, once_control);
init_routine();
2017-08-23 23:53:20 +00:00
}
2017-08-31 23:18:14 +00:00
return 0;
2017-08-23 23:53:20 +00:00
}
/***************** MUTEX ******************/
static int mutexattr_check(const pthread_mutexattr_t *attr)
{
2017-08-31 23:18:14 +00:00
if (attr->type < PTHREAD_MUTEX_NORMAL || attr->type > PTHREAD_MUTEX_RECURSIVE) {
return EINVAL;
}
return 0;
}
int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr)
2017-08-23 23:53:20 +00:00
{
2017-08-31 23:18:14 +00:00
int type = PTHREAD_MUTEX_NORMAL;
if (!mutex) {
return EINVAL;
}
if (attr) {
if (!attr->is_initialized) {
return EINVAL;
}
int res = mutexattr_check(attr);
if (res) {
return res;
}
type = attr->type;
}
esp_pthread_mutex_t *mux = (esp_pthread_mutex_t *)malloc(sizeof(esp_pthread_mutex_t));
if (!mux) {
return ENOMEM;
2017-08-31 23:18:14 +00:00
}
mux->type = type;
2017-08-31 23:18:14 +00:00
if (mux->type == PTHREAD_MUTEX_RECURSIVE) {
mux->sem = xSemaphoreCreateRecursiveMutex();
} else {
mux->sem = xSemaphoreCreateMutex();
}
if (!mux->sem) {
free(mux);
return EAGAIN;
2017-08-31 23:18:14 +00:00
}
2017-08-31 23:18:14 +00:00
*mutex = (pthread_mutex_t)mux; // pointer value fit into pthread_mutex_t (uint32_t)
2017-08-31 23:18:14 +00:00
return 0;
2017-08-23 23:53:20 +00:00
}
int pthread_mutex_destroy(pthread_mutex_t *mutex)
2017-08-23 23:53:20 +00:00
{
2017-08-31 23:18:14 +00:00
esp_pthread_mutex_t *mux;
2017-08-31 23:18:14 +00:00
ESP_LOGV(TAG, "%s %p", __FUNCTION__, mutex);
2017-08-31 23:18:14 +00:00
if (!mutex) {
return EINVAL;
}
mux = (esp_pthread_mutex_t *)*mutex;
// check if mux is busy
int res = pthread_mutex_lock_internal(mux, 0);
if (res == EBUSY) {
return EBUSY;
}
2017-08-31 23:18:14 +00:00
vSemaphoreDelete(mux->sem);
free(mux);
2017-08-31 23:18:14 +00:00
return 0;
}
static int IRAM_ATTR pthread_mutex_lock_internal(esp_pthread_mutex_t *mux, TickType_t tmo)
{
if (mux->type == PTHREAD_MUTEX_RECURSIVE) {
if (xSemaphoreTakeRecursive(mux->sem, tmo) != pdTRUE) {
return EBUSY;
}
} else {
if (xSemaphoreTake(mux->sem, tmo) != pdTRUE) {
return EBUSY;
}
}
return 0;
}
static int pthread_mutex_init_if_static(pthread_mutex_t *mutex) {
int res = 0;
if ((intptr_t) *mutex == PTHREAD_MUTEX_INITIALIZER) {
portENTER_CRITICAL(&s_mutex_init_lock);
if ((intptr_t) *mutex == PTHREAD_MUTEX_INITIALIZER) {
res = pthread_mutex_init(mutex, NULL);
}
portEXIT_CRITICAL(&s_mutex_init_lock);
}
return res;
}
int IRAM_ATTR pthread_mutex_lock(pthread_mutex_t *mutex)
{
2017-08-31 23:18:14 +00:00
if (!mutex) {
return EINVAL;
}
int res = pthread_mutex_init_if_static(mutex);
if (res != 0) {
return res;
}
return pthread_mutex_lock_internal((esp_pthread_mutex_t *)*mutex, portMAX_DELAY);
}
int IRAM_ATTR pthread_mutex_trylock(pthread_mutex_t *mutex)
{
2017-08-31 23:18:14 +00:00
if (!mutex) {
return EINVAL;
}
int res = pthread_mutex_init_if_static(mutex);
if (res != 0) {
return res;
}
return pthread_mutex_lock_internal((esp_pthread_mutex_t *)*mutex, 0);
}
int IRAM_ATTR pthread_mutex_unlock(pthread_mutex_t *mutex)
{
2017-08-31 23:18:14 +00:00
esp_pthread_mutex_t *mux;
2017-08-31 23:18:14 +00:00
if (!mutex) {
return EINVAL;
}
mux = (esp_pthread_mutex_t *)*mutex;
2017-08-31 23:18:14 +00:00
if (mux->type == PTHREAD_MUTEX_RECURSIVE) {
xSemaphoreGiveRecursive(mux->sem);
} else {
xSemaphoreGive(mux->sem);
2017-08-31 23:18:14 +00:00
}
return 0;
2017-08-23 23:53:20 +00:00
}
int pthread_mutexattr_init(pthread_mutexattr_t *attr)
{
2017-08-31 23:18:14 +00:00
if (!attr) {
return EINVAL;
}
attr->type = PTHREAD_MUTEX_NORMAL;
attr->is_initialized = 1;
return 0;
}
int pthread_mutexattr_destroy(pthread_mutexattr_t *attr)
{
2017-08-31 23:18:14 +00:00
if (!attr) {
return EINVAL;
}
attr->is_initialized = 0;
return 0;
}
int pthread_mutexattr_gettype(const pthread_mutexattr_t *attr, int *type)
{
2017-09-04 18:46:16 +00:00
ESP_LOGE(TAG, "%s: not supported!", __FUNCTION__);
return ENOSYS;
}
int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int type)
{
2017-08-31 23:18:14 +00:00
if (!attr) {
return EINVAL;
}
pthread_mutexattr_t tmp_attr = {.type = type};
int res = mutexattr_check(&tmp_attr);
2017-09-04 18:46:16 +00:00
if (!res) {
2017-08-31 23:18:14 +00:00
attr->type = type;
}
return res;
}