409 lines
12 KiB
C
409 lines
12 KiB
C
#include <errno.h>
|
|
#include <pthread.h>
|
|
#include <string.h>
|
|
#include "esp_err.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "freertos/semphr.h"
|
|
#include "freertos/list.h"
|
|
|
|
#define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE//CONFIG_LOG_DEFAULT_LEVEL
|
|
#include "esp_log.h"
|
|
const static char *TAG = "esp_pthread";
|
|
|
|
#define ESP_PTHREAD_LOG( format, ... ) \
|
|
do { \
|
|
ets_printf(format, ##__VA_ARGS__); \
|
|
} while(0)
|
|
|
|
#define ESP_PTHREAD_LOG_LEV( _tag_, _L_, level, format, ... ) \
|
|
do { \
|
|
if (LOG_LOCAL_LEVEL >= level) { \
|
|
ESP_PTHREAD_LOG(LOG_FORMAT(_L_, format), esp_log_early_timestamp(), _tag_ , ##__VA_ARGS__); \
|
|
} \
|
|
} while(0)
|
|
|
|
#define ESP_PTHREAD_LOGE( _tag_, format, ... ) ESP_PTHREAD_LOG_LEV(_tag_, E, ESP_LOG_ERROR, format, ##__VA_ARGS__)
|
|
#define ESP_PTHREAD_LOGW( _tag_, format, ... ) ESP_PTHREAD_LOG_LEV(_tag_, W, ESP_LOG_WARN, format, ##__VA_ARGS__)
|
|
#define ESP_PTHREAD_LOGI( _tag_, format, ... ) ESP_PTHREAD_LOG_LEV(_tag_, I, ESP_LOG_INFO, format, ##__VA_ARGS__)
|
|
#define ESP_PTHREAD_LOGD( _tag_, format, ... ) ESP_PTHREAD_LOG_LEV(_tag_, D, ESP_LOG_DEBUG, format, ##__VA_ARGS__)
|
|
#define ESP_PTHREAD_LOGV( _tag_, format, ... ) ESP_PTHREAD_LOG_LEV(_tag_, V, ESP_LOG_VERBOSE, format, ##__VA_ARGS__)
|
|
#define ESP_PTHREAD_LOGO( _tag_, format, ... ) ESP_PTHREAD_LOG_LEV(_tag_, E, ESP_LOG_NONE, format, ##__VA_ARGS__)
|
|
// #define ESP_PTHREAD_LOGE( tag, format, ... ) ESP_PTHREAD_LOG(tag ": " format "\n", ##__VA_ARGS__)
|
|
// #define ESP_PTHREAD_LOGW( tag, format, ... ) ESP_PTHREAD_LOG(tag ": " format "\n", ##__VA_ARGS__)
|
|
// #define ESP_PTHREAD_LOGI( tag, format, ... ) ESP_PTHREAD_LOG(tag ": " format "\n", ##__VA_ARGS__)
|
|
// #define ESP_PTHREAD_LOGD( tag, format, ... ) ESP_PTHREAD_LOG(tag ": " format "\n", ##__VA_ARGS__)
|
|
// #define ESP_PTHREAD_LOGV( tag, format, ... ) ESP_PTHREAD_LOG(tag ": " format "\n", ##__VA_ARGS__)
|
|
// #define ESP_PTHREAD_LOGO( tag, format, ... ) ESP_PTHREAD_LOG(tag ": " format "\n", ##__VA_ARGS__)
|
|
|
|
#define PTHREAD_TASK_PRIO_DEFAULT 5
|
|
|
|
#define ESP_PTHREAD_STATE_RUN 0
|
|
#define ESP_PTHREAD_STATE_EXIT 1
|
|
//#define ESP_PTHREAD_STATE_CANCEL 2
|
|
|
|
typedef struct {
|
|
ListItem_t list_item;
|
|
TaskHandle_t join_task;
|
|
int state;
|
|
} esp_pthread_t;
|
|
|
|
typedef struct {
|
|
void *(*func)(void *);
|
|
void *arg;
|
|
} esp_pthread_task_arg_t;
|
|
|
|
static SemaphoreHandle_t s_once_mux = NULL;
|
|
static SemaphoreHandle_t s_threads_mux = NULL;
|
|
|
|
static List_t s_threads_list;
|
|
//static List_t s_key_list;
|
|
|
|
int esp_pthread_init(void)
|
|
{
|
|
ESP_PTHREAD_LOGV(TAG, "%s", __FUNCTION__);
|
|
vListInitialise((List_t *)&s_threads_list);
|
|
// vListInitialise((List_t *)&s_key_list);
|
|
s_once_mux = xSemaphoreCreateMutex();
|
|
if (s_once_mux == NULL)
|
|
return ESP_FAIL;
|
|
s_threads_mux = xSemaphoreCreateMutex();
|
|
if (s_threads_mux == NULL) {
|
|
vSemaphoreDelete(s_once_mux);
|
|
return ESP_FAIL;
|
|
}
|
|
return ESP_OK;
|
|
}
|
|
|
|
static TaskHandle_t pthread_find_handle(pthread_t thread)
|
|
{
|
|
ListItem_t const *list_end = listGET_END_MARKER(&s_threads_list);
|
|
ListItem_t *list_item = listGET_HEAD_ENTRY(&s_threads_list);
|
|
while (list_item != list_end) {
|
|
esp_pthread_t *pthread = listGET_LIST_ITEM_OWNER(list_item);
|
|
if ((pthread_t)pthread == thread) {
|
|
return (TaskHandle_t)listGET_LIST_ITEM_VALUE(list_item);
|
|
}
|
|
list_item = listGET_NEXT(list_item);
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
static esp_pthread_t *pthread_find(TaskHandle_t task_handle)
|
|
{
|
|
ListItem_t const *list_end = listGET_END_MARKER(&s_threads_list);
|
|
ListItem_t *list_item = listGET_HEAD_ENTRY(&s_threads_list);
|
|
while (list_item != list_end) {
|
|
TaskHandle_t cur_handle = (TaskHandle_t)listGET_LIST_ITEM_VALUE(list_item);
|
|
if (task_handle == cur_handle) {
|
|
return (esp_pthread_t *)listGET_LIST_ITEM_OWNER(list_item);
|
|
}
|
|
list_item = listGET_NEXT(list_item);
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
static void pthread_task_func(void *arg)
|
|
{
|
|
esp_pthread_task_arg_t *task_arg = (esp_pthread_task_arg_t *)arg;
|
|
|
|
ESP_PTHREAD_LOGV(TAG, "%s ENTER %p", __FUNCTION__, task_arg->func);
|
|
|
|
// wait for start
|
|
xTaskNotifyWait(0, 0, NULL, portMAX_DELAY);
|
|
|
|
ESP_PTHREAD_LOGV(TAG, "%s START %p", __FUNCTION__, task_arg->func);
|
|
|
|
task_arg->func(task_arg->arg);
|
|
|
|
ESP_PTHREAD_LOGV(TAG, "%s END %p", __FUNCTION__, task_arg->func);
|
|
|
|
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) == pdTRUE) {
|
|
esp_pthread_t *pthread = pthread_find(xTaskGetCurrentTaskHandle());
|
|
if (pthread) {
|
|
pthread->state = ESP_PTHREAD_STATE_EXIT;
|
|
if (pthread->join_task) {
|
|
// notify join
|
|
xTaskNotify(pthread->join_task, 0, eNoAction);
|
|
}
|
|
} else {
|
|
assert(false && "Failed to find pthread for current task!");
|
|
}
|
|
xSemaphoreGive(s_threads_mux);
|
|
} else {
|
|
assert(false && "Failed to lock threads list!");
|
|
}
|
|
// TODO: Remove from list???
|
|
//free(task_arg->pthread);
|
|
free(task_arg);
|
|
vTaskDelete(NULL);
|
|
|
|
ESP_PTHREAD_LOGV(TAG, "%s EXIT %p", __FUNCTION__, task_arg->func);
|
|
}
|
|
|
|
int pthread_create(pthread_t *thread, const pthread_attr_t *attr,
|
|
void *(*start_routine) (void *), void *arg)
|
|
{
|
|
TaskHandle_t xHandle = NULL;
|
|
// int priority; // Priority
|
|
// int stacksize; // Stack size
|
|
// int initial_state; // Initial state
|
|
// int cpu; // CPU affinity
|
|
// int res;
|
|
|
|
ESP_PTHREAD_LOGV(TAG, "%s", __FUNCTION__);
|
|
if (attr) {
|
|
ESP_PTHREAD_LOGE(TAG, "Attrs not supported!");
|
|
return EINVAL;
|
|
}
|
|
// if (attr) {
|
|
// stacksize = attr->stack_size;
|
|
// if (stacksize < PTHREAD_STACK_MIN) {
|
|
// errno = EINVAL;
|
|
// return EINVAL;
|
|
// }
|
|
// priority = attr->sched_priority;
|
|
// initial_state = attr->initial_state;
|
|
// cpu = attr->cpuset;
|
|
// } else {
|
|
// stacksize = CONFIG_LUA_RTOS_LUA_THREAD_STACK_SIZE;
|
|
// initial_state = PTHREAD_INITIAL_STATE_RUN;
|
|
// priority = CONFIG_LUA_RTOS_LUA_TASK_PRIORITY;
|
|
|
|
// if (portNUM_PROCESSORS > 0) {
|
|
// cpu = 0;
|
|
// } else {
|
|
// cpu = tskNO_AFFINITY;
|
|
// }
|
|
// }
|
|
esp_pthread_task_arg_t *task_arg = malloc(sizeof(esp_pthread_task_arg_t));
|
|
if (task_arg == NULL) {
|
|
ESP_PTHREAD_LOGE(TAG, "Failed to allocate task args!");
|
|
errno = ENOMEM;
|
|
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_PTHREAD_LOGE(TAG, "Failed to allocate pthread data!");
|
|
free(task_arg);
|
|
errno = ENOMEM;
|
|
return ENOMEM;
|
|
}
|
|
memset(pthread, 0, sizeof(esp_pthread_t));
|
|
task_arg->func = start_routine;
|
|
task_arg->arg = arg;
|
|
//task_arg->pthread = pthread;
|
|
BaseType_t res = xTaskCreate(&pthread_task_func, "pthread", configMINIMAL_STACK_SIZE, task_arg, PTHREAD_TASK_PRIO_DEFAULT, &xHandle);
|
|
if(res != pdPASS) {
|
|
ESP_PTHREAD_LOGE(TAG, "Failed to create task!");
|
|
free(pthread);
|
|
free(task_arg);
|
|
if (res == errCOULD_NOT_ALLOCATE_REQUIRED_MEMORY) {
|
|
errno = ENOMEM;
|
|
return ENOMEM;
|
|
} else {
|
|
errno = EAGAIN;
|
|
return EAGAIN;
|
|
}
|
|
}
|
|
vListInitialiseItem((ListItem_t *)&pthread->list_item);
|
|
listSET_LIST_ITEM_OWNER((ListItem_t *)&pthread->list_item, pthread);
|
|
listSET_LIST_ITEM_VALUE((ListItem_t *)&pthread->list_item, (TickType_t)xHandle);
|
|
|
|
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) == pdTRUE) {
|
|
vListInsertEnd((List_t *)&s_threads_list, (ListItem_t *)&pthread->list_item);
|
|
xSemaphoreGive(s_threads_mux);
|
|
} else {
|
|
assert(false && "Failed to lock threads list!");
|
|
}
|
|
// start task
|
|
xTaskNotify(xHandle, 0, eNoAction);
|
|
|
|
*thread = (pthread_t)pthread; // pointer value fit into pthread_t (uint32_t)
|
|
|
|
ESP_PTHREAD_LOGV(TAG, "Created task %x", (uint32_t)xHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int pthread_join(pthread_t thread, void **retval)
|
|
{
|
|
esp_pthread_t *pthread = (esp_pthread_t *)thread;
|
|
bool wait = false;
|
|
int ret = 0;
|
|
|
|
ets_printf("%s\n", __FUNCTION__);
|
|
// find task
|
|
if (xSemaphoreTake(s_threads_mux, portMAX_DELAY) == pdTRUE) {
|
|
//uxBitsWaitedFor = listGET_LIST_ITEM_VALUE(list_item);
|
|
// TODO: check if task is joinable
|
|
// TODO: PTHREAD_CANCELED???
|
|
TaskHandle_t handle = pthread_find_handle(thread);
|
|
if (!handle) {
|
|
errno = ESRCH; // not found
|
|
ret = ESRCH;
|
|
} else if (pthread->join_task) {
|
|
errno = EINVAL; // already have waiting task to join
|
|
ret = EINVAL;
|
|
} else if (handle == xTaskGetCurrentTaskHandle()) {
|
|
errno = EDEADLK; // join to self or join to each other
|
|
ret = EDEADLK;
|
|
} else {
|
|
esp_pthread_t *cur_pthread = pthread_find(xTaskGetCurrentTaskHandle());
|
|
if (cur_pthread && cur_pthread->join_task == handle) {
|
|
errno = EDEADLK; // join to each other
|
|
ret = EDEADLK;
|
|
} else {
|
|
if (pthread->state == ESP_PTHREAD_STATE_RUN) {
|
|
pthread->join_task = xTaskGetCurrentTaskHandle();
|
|
wait = true;
|
|
}
|
|
}
|
|
}
|
|
xSemaphoreGive(s_threads_mux);
|
|
} else {
|
|
assert(false && "Failed to lock threads list!");
|
|
}
|
|
if (wait) {
|
|
// TODO: handle caller cancelation???
|
|
xTaskNotifyWait(0, 0, NULL, portMAX_DELAY);
|
|
}
|
|
if (retval) {
|
|
*retval = 0; // no exit code in FreeRTOS
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
int pthread_detach(pthread_t thread)
|
|
{
|
|
assert(false && "pthread_detach not supported!");
|
|
return -1;
|
|
}
|
|
|
|
int pthread_cancel(pthread_t thread)
|
|
{
|
|
assert(false && "pthread_cancel not supported!");
|
|
return -1;
|
|
}
|
|
|
|
pthread_t pthread_self(void)
|
|
{
|
|
esp_pthread_t *pthread = pthread_find(xTaskGetCurrentTaskHandle());
|
|
if (!pthread) {
|
|
assert(false && "Failed to find current thread ID!");
|
|
}
|
|
return (pthread_t)pthread;
|
|
}
|
|
|
|
int pthread_equal(pthread_t t1, pthread_t t2)
|
|
{
|
|
ets_printf("%s %x %x\n", __FUNCTION__, t1, t2);
|
|
return t1 == t2 ? 1 : 0;
|
|
}
|
|
|
|
/***************** KEY ******************/
|
|
int pthread_key_create(pthread_key_t *key, void (*destructor)(void*))
|
|
{
|
|
static int s_created;
|
|
//struct pthread_key *key;
|
|
|
|
ESP_PTHREAD_LOGV(TAG, "%s", __FUNCTION__);
|
|
|
|
if (s_created) {
|
|
assert(false && "CREATED!");
|
|
return ENOMEM;
|
|
}
|
|
|
|
// key = (struct pthread_key *)malloc(sizeof(struct pthread_key));
|
|
// if (!key) {
|
|
// errno = ENOMEM;
|
|
// return ENOMEM;
|
|
// }
|
|
// key->destructor = destructor;
|
|
|
|
// list_init(&key->specific,1);
|
|
|
|
// vListInsert((List_t *)s_key_list, (ListItem_t *)&);
|
|
// // Add key to key list
|
|
// res = list_add(&key_list, key, k);
|
|
// if (res) {
|
|
// free(key);
|
|
// errno = res;
|
|
// return res;
|
|
// }
|
|
*key = 1;
|
|
s_created = 1;
|
|
return 0;
|
|
}
|
|
|
|
int pthread_key_delete(pthread_key_t key)
|
|
{
|
|
ESP_PTHREAD_LOGV(TAG, "%s", __FUNCTION__);
|
|
assert(false && "NOT IMPLEMENTED!");
|
|
return -1;
|
|
}
|
|
|
|
void *pthread_getspecific(pthread_key_t key)
|
|
{
|
|
ets_printf("%s\n", __FUNCTION__);
|
|
assert(false && "NOT IMPLEMENTED!");
|
|
return NULL;
|
|
}
|
|
|
|
int pthread_setspecific(pthread_key_t key, const void *value)
|
|
{
|
|
ets_printf("%s\n", __FUNCTION__);
|
|
assert(false && "NOT IMPLEMENTED!");
|
|
return -1;
|
|
}
|
|
|
|
/***************** ONCE ******************/
|
|
int pthread_once(pthread_once_t *once_control, void (*init_routine)(void))
|
|
{
|
|
ESP_PTHREAD_LOGV(TAG, "%s", __FUNCTION__);
|
|
|
|
if (once_control == NULL || init_routine == NULL || !once_control->is_initialized) {
|
|
ESP_PTHREAD_LOGE(TAG, "%s: Invalid args!", __FUNCTION__);
|
|
return EINVAL;
|
|
}
|
|
|
|
TaskHandle_t cur_task = xTaskGetCurrentTaskHandle();
|
|
// do not take mutex if OS is not running yet
|
|
if (!cur_task || xSemaphoreTake(s_once_mux, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
if (!once_control->init_executed) {
|
|
ESP_PTHREAD_LOGV(TAG, "%s: call init_routine %p", __FUNCTION__, once_control);
|
|
init_routine();
|
|
once_control->init_executed = 1;
|
|
}
|
|
if (cur_task) {
|
|
xSemaphoreGive(s_once_mux);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
ESP_PTHREAD_LOGE(TAG, "%s: Failed to lock!", __FUNCTION__);
|
|
return EBUSY;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/***************** MUTEX ******************/
|
|
int pthread_mutex_lock(pthread_mutex_t *mutex)
|
|
{
|
|
ets_printf("%s\n", __FUNCTION__);
|
|
assert(false && "NOT IMPLEMENTED!");
|
|
return -1;
|
|
}
|
|
|
|
//int pthread_mutex_trylock(pthread_mutex_t *mutex);
|
|
int pthread_mutex_unlock(pthread_mutex_t *mutex)
|
|
{
|
|
ets_printf("%s\n", __FUNCTION__);
|
|
assert(false && "NOT IMPLEMENTED!");
|
|
return -1;
|
|
}
|
|
|