From 0b09c7006d350a58da6b1dd34687d3bd861ed1c8 Mon Sep 17 00:00:00 2001 From: Alexey Gerenkov Date: Thu, 24 Aug 2017 02:53:20 +0300 Subject: [PATCH] pthread: Initial version of thread API --- components/esp32/cpu_start.c | 9 +- components/newlib/include/pthread.h | 2 +- components/newlib/include/sys/features.h | 3 + components/pthread/Kconfig | 0 components/pthread/component.mk | 9 + components/pthread/pthread.c | 408 +++++++++++++++++++ components/pthread/test/component.mk | 5 + components/pthread/test/test_pthread.c | 11 + components/pthread/test/test_pthread_cxx.cpp | 44 ++ 9 files changed, 489 insertions(+), 2 deletions(-) create mode 100644 components/pthread/Kconfig create mode 100644 components/pthread/component.mk create mode 100644 components/pthread/pthread.c create mode 100644 components/pthread/test/component.mk create mode 100644 components/pthread/test/test_pthread.c create mode 100644 components/pthread/test/test_pthread_cxx.cpp diff --git a/components/esp32/cpu_start.c b/components/esp32/cpu_start.c index 8650290cf..0c560cce6 100644 --- a/components/esp32/cpu_start.c +++ b/components/esp32/cpu_start.c @@ -83,6 +83,7 @@ static bool app_cpu_started = false; static void do_global_ctors(void); static void main_task(void* args); extern void app_main(void); +extern int esp_pthread_init(void); extern int _bss_start; extern int _bss_end; @@ -252,6 +253,7 @@ static void intr_matrix_clear(void) void start_cpu0_default(void) { + esp_err_t err; esp_setup_syscall_table(); //Enable trace memory and immediately start trace. #if CONFIG_ESP32_TRAX @@ -290,7 +292,7 @@ void start_cpu0_default(void) esp_timer_init(); esp_set_time_from_rtc(); #if CONFIG_ESP32_APPTRACE_ENABLE - esp_err_t err = esp_apptrace_init(); + err = esp_apptrace_init(); if (err != ESP_OK) { ESP_EARLY_LOGE(TAG, "Failed to init apptrace module on CPU0 (%d)!", err); } @@ -298,6 +300,11 @@ void start_cpu0_default(void) #if CONFIG_SYSVIEW_ENABLE SEGGER_SYSVIEW_Conf(); #endif + err = esp_pthread_init(); + if (err != ESP_OK) { + ESP_EARLY_LOGE(TAG, "Failed to init pthread module (%d)!", err); + } + do_global_ctors(); #if CONFIG_INT_WDT esp_int_wdt_init(); diff --git a/components/newlib/include/pthread.h b/components/newlib/include/pthread.h index db1f9c1ca..907970fdf 100644 --- a/components/newlib/include/pthread.h +++ b/components/newlib/include/pthread.h @@ -31,7 +31,7 @@ extern "C" { #include #include -#include +#include #include struct _pthread_cleanup_context { diff --git a/components/newlib/include/sys/features.h b/components/newlib/include/sys/features.h index 1d90921af..792f68bbc 100644 --- a/components/newlib/include/sys/features.h +++ b/components/newlib/include/sys/features.h @@ -210,6 +210,9 @@ extern "C" { #endif /* __CYGWIN__ */ +#define _POSIX_THREADS 1 +#define _UNIX98_THREAD_MUTEX_ATTRIBUTES 1 + /* Per the permission given in POSIX.1-2008 section 2.2.1, define * _POSIX_C_SOURCE if _XOPEN_SOURCE is defined and _POSIX_C_SOURCE is not. * (_XOPEN_SOURCE indicates that XSI extensions are desired by an application.) diff --git a/components/pthread/Kconfig b/components/pthread/Kconfig new file mode 100644 index 000000000..e69de29bb diff --git a/components/pthread/component.mk b/components/pthread/component.mk new file mode 100644 index 000000000..cd69bb330 --- /dev/null +++ b/components/pthread/component.mk @@ -0,0 +1,9 @@ +# +# Component Makefile +# + +COMPONENT_SRCDIRS := . + +#COMPONENT_ADD_INCLUDEDIRS := include + +COMPONENT_ADD_LDFLAGS := -lpthread diff --git a/components/pthread/pthread.c b/components/pthread/pthread.c new file mode 100644 index 000000000..ca54f75e7 --- /dev/null +++ b/components/pthread/pthread.c @@ -0,0 +1,408 @@ +#include +#include +#include +#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; +} + diff --git a/components/pthread/test/component.mk b/components/pthread/test/component.mk new file mode 100644 index 000000000..5dd172bdb --- /dev/null +++ b/components/pthread/test/component.mk @@ -0,0 +1,5 @@ +# +#Component Makefile +# + +COMPONENT_ADD_LDFLAGS = -Wl,--whole-archive -l$(COMPONENT_NAME) -Wl,--no-whole-archive diff --git a/components/pthread/test/test_pthread.c b/components/pthread/test/test_pthread.c new file mode 100644 index 000000000..8625f84eb --- /dev/null +++ b/components/pthread/test/test_pthread.c @@ -0,0 +1,11 @@ +#include "unity.h" + +TEST_CASE("pthread C test 1", "[pthread]") +{ + // int delay_ms = 50; + // const delay_test_arg_t args = { .delay_us = delay_ms * 1000, .method = 1 }; + // xTaskCreatePinnedToCore(test_delay_task, "", 2048, (void*) &args, 3, NULL, 0); + // vTaskDelay(delay_ms / portTICK_PERIOD_MS + 1); + // xTaskCreatePinnedToCore(test_delay_task, "", 2048, (void*) &args, 3, NULL, 1); + // vTaskDelay(delay_ms / portTICK_PERIOD_MS + 1); +} diff --git a/components/pthread/test/test_pthread_cxx.cpp b/components/pthread/test/test_pthread_cxx.cpp new file mode 100644 index 000000000..800abab56 --- /dev/null +++ b/components/pthread/test/test_pthread_cxx.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include "unity.h" +#include "rom/ets_sys.h" + +std::shared_ptr global_sp; +std::mutex mtx; + +void thread_main() { + int i = 0; + //std::cout << "thread_main!" << std::endl; + ets_printf("thread_main\n"); + + while (1) { + // std::cout << "thread_main " << i << std::endl; + ets_printf("thread_main %d\n", i); + i++; + } + // auto local_sp = global_sp; // OK, copy constructor's parameter is reference-to-const + + // int i = *global_sp; // OK, operator* is const + // int j = *local_sp; // OK, does not operate on global_sp + + // *global_sp = 2; // NOT OK, modifies int visible to other threads + // *local_sp = 2; // NOT OK, modifies int visible to other threads + + // mtx.lock(); + // global_sp.reset(); // NOT OK, reset is non-const + // mtx.unlock(); + //local_sp.reset(); // OK, does not operate on global_sp +} + +//extern "C" +TEST_CASE("pthread CXX test 1", "[pthread]") +{ + std::cout << "Hello world!" << std::endl; + + global_sp.reset(new int(1)); + std::thread t1(thread_main); + // std::thread t2(thread_main); + t1.join(); + // t2.join(); +}