pthread: Initial version of thread API

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();

components/newlib/include/pthread.h

@@ -31,7 +31,7 @@ extern "C" {
 
 #include <sys/types.h>
 #include <time.h>
-#include <sys/sched.h>
+#include <sched.h>
 #include <sys/cdefs.h>
 
 struct _pthread_cleanup_context {

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.)

components/pthread/component.mk

@@ -0,0 +1,9 @@
+#
+# Component Makefile
+#
+
+COMPONENT_SRCDIRS := .
+
+#COMPONENT_ADD_INCLUDEDIRS := include
+
+COMPONENT_ADD_LDFLAGS := -lpthread

components/pthread/pthread.c

@@ -0,0 +1,408 @@
+#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;
+}
+

components/pthread/test/component.mk

@@ -0,0 +1,5 @@
+#
+#Component Makefile
+#
+
+COMPONENT_ADD_LDFLAGS = -Wl,--whole-archive -l$(COMPONENT_NAME) -Wl,--no-whole-archive

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);
+}

components/pthread/test/test_pthread_cxx.cpp

@@ -0,0 +1,44 @@
+#include <iostream>
+#include <thread>
+#include <mutex>
+#include "unity.h"
+#include "rom/ets_sys.h"
+
+std::shared_ptr<int> 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();
+}