pthread: Add support for pthread thread local storage

Refactors LWIP to use this for the LWIP thread local semaphore

components/freertos/Kconfig

@@ -114,11 +114,8 @@ config FREERTOS_THREAD_LOCAL_STORAGE_POINTERS
         FreeRTOS has the ability to store per-thread pointers in the task
         control block. This controls the number of pointers available.
 
-        Value 0 turns off this functionality.
-
-        If using the LWIP TCP/IP stack (with WiFi or Ethernet), this value must be at least 1. See the
-        LWIP_THREAD_LOCAL_STORAGE_INDEX config item in LWIP configuration to determine which thread-local-storage
-        pointer is reserved for LWIP.
+        This value must be at least 1. Index 0 is reserved for use by the pthreads API
+        thread-local-storage. Other indexes can be used for any desired purpose.
 
 choice FREERTOS_ASSERT
     prompt "FreeRTOS assertions"

components/lwip/Kconfig

@@ -26,13 +26,6 @@ config LWIP_MAX_SOCKETS
         the maximum amount of sockets here. The valid value is from 1
         to 16.
 
-config LWIP_THREAD_LOCAL_STORAGE_INDEX
-    int "Index for thread-local-storage pointer for lwip"
-    default 0
-    help
-        Specify the thread-local-storage-pointer index for lwip
-        use.
-
 config LWIP_SO_REUSE
     bool "Enable SO_REUSEADDR option"
     default y

components/lwip/port/freertos/sys_arch.c

@@ -32,6 +32,7 @@
 
 /* lwIP includes. */
 
+#include <pthread.h>
 #include "lwip/debug.h"
 #include "lwip/def.h"
 #include "lwip/sys.h"
@@ -46,6 +47,9 @@
 
 static sys_mutex_t g_lwip_protect_mutex = NULL;
 
+static pthread_key_t sys_thread_sem_key;
+static void sys_thread_sem_free(void* data);
+
 #if !LWIP_COMPAT_MUTEX
 /** Create a new mutex
  * @param mutex pointer to the mutex to create
@@ -433,6 +437,10 @@ sys_init(void)
     if (ERR_OK != sys_mutex_new(&g_lwip_protect_mutex)) {
         ESP_LOGE(TAG, "sys_init: failed to init lwip protect mutex\n");
     }
+
+    // Create the pthreads key for the per-thread semaphore storage
+    pthread_key_create(&sys_thread_sem_key, sys_thread_sem_free);
+
     esp_vfs_lwip_sockets_register();
 }
 
@@ -483,31 +491,31 @@ sys_arch_unprotect(sys_prot_t pval)
   sys_mutex_unlock(&g_lwip_protect_mutex);
 }
 
-#define SYS_TLS_INDEX CONFIG_LWIP_THREAD_LOCAL_STORAGE_INDEX
-/* 
+/*
  * get per thread semphore
  */
 sys_sem_t* sys_thread_sem_get(void)
 {
-  sys_sem_t *sem = (sys_sem_t*)pvTaskGetThreadLocalStoragePointer(xTaskGetCurrentTaskHandle(), SYS_TLS_INDEX);
-  if (!sem){
-    sem = sys_thread_sem_init();
+  sys_sem_t *sem = pthread_getspecific(sys_thread_sem_key);
+
+  if (!sem) {
+      sem = sys_thread_sem_init();
   }
   LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sem_get s=%p\n", sem));
   return sem;
 }
 
-static void sys_thread_tls_free(int index, void* data)
+static void sys_thread_sem_free(void* data) // destructor for TLS semaphore
 {
   sys_sem_t *sem = (sys_sem_t*)(data);
 
   if (sem && *sem){
-    LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sem del, i=%d sem=%p\n", index, *sem));
+    LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sem del, sem=%p\n", *sem));
     vSemaphoreDelete(*sem);
   }
 
-  if (sem){
-    LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sem pointer del, i=%d sem_p=%p\n", index, sem));
+  if (sem) {
+    LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sem pointer del, sem_p=%p\n", sem));
     free(sem);
   }
 }
@@ -528,20 +536,17 @@ sys_sem_t* sys_thread_sem_init(void)
     return 0;
   }
 
-  LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sem init sem_p=%p sem=%p cb=%p\n", sem, *sem, sys_thread_tls_free));
-  vTaskSetThreadLocalStoragePointerAndDelCallback(xTaskGetCurrentTaskHandle(), SYS_TLS_INDEX, sem, (TlsDeleteCallbackFunction_t)sys_thread_tls_free);
-
+  pthread_setspecific(sys_thread_sem_key, sem);
   return sem;
 }
 
 void sys_thread_sem_deinit(void)
 {
-  sys_sem_t *sem = (sys_sem_t*)pvTaskGetThreadLocalStoragePointer(xTaskGetCurrentTaskHandle(), SYS_TLS_INDEX);
-
-  sys_thread_tls_free(SYS_TLS_INDEX, (void*)sem);
-  vTaskSetThreadLocalStoragePointerAndDelCallback(xTaskGetCurrentTaskHandle(), SYS_TLS_INDEX, 0, 0);
-
-  return;
+  sys_sem_t *sem = pthread_getspecific(sys_thread_sem_key);
+  if (sem != NULL) {
+    sys_thread_sem_free(sem);
+    pthread_setspecific(sys_thread_sem_key, NULL);
+  }
 }
 
 void sys_delay_ms(uint32_t ms)

components/pthread/pthread.c

@@ -28,6 +28,8 @@
 #include "freertos/semphr.h"
 #include "freertos/list.h"
 
+#include "pthread_internal.h"
+
 #define LOG_LOCAL_LEVEL CONFIG_LOG_DEFAULT_LEVEL
 #include "esp_log.h"
 const static char *TAG = "esp_pthread";
@@ -145,6 +147,10 @@ static void pthread_task_func(void *arg)
     ESP_LOGV(TAG, "%s END %p", __FUNCTION__, task_arg->func);
     free(task_arg);
 
+    /* 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) {
         assert(false && "Failed to lock threads list!");
     }
@@ -332,40 +338,6 @@ int pthread_equal(pthread_t t1, pthread_t t2)
     return t1 == t2 ? 1 : 0;
 }
 
-/***************** KEY ******************/
-int pthread_key_create(pthread_key_t *key, void (*destructor)(void*))
-{
-    static int s_created;
-
-    //TODO: Key destructors not suppoted!
-    if (s_created) {
-        // key API supports just one key necessary by libstdcxx threading implementation
-        ESP_LOGE(TAG, "%s: multiple keys not supported!", __FUNCTION__);
-        return ENOSYS;
-    }
-    *key = 1;
-    s_created = 1;
-    return 0;
-}
-
-int pthread_key_delete(pthread_key_t key)
-{
-    ESP_LOGE(TAG, "%s: not supported!", __FUNCTION__);
-    return ENOSYS;
-}
-
-void *pthread_getspecific(pthread_key_t key)
-{
-    ESP_LOGE(TAG, "%s: not supported!", __FUNCTION__);
-    return NULL;
-}
-
-int pthread_setspecific(pthread_key_t key, const void *value)
-{
-    ESP_LOGE(TAG, "%s: not supported!", __FUNCTION__);
-    return ENOSYS;
-}
-
 /***************** ONCE ******************/
 int pthread_once(pthread_once_t *once_control, void (*init_routine)(void))
 {

components/pthread/pthread_internal.h

@@ -0,0 +1,16 @@
+// 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.
+#pragma once
+
+void pthread_internal_local_storage_destructor_callback();

components/pthread/pthread_local_storage.c

@@ -0,0 +1,226 @@
+// 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.
+#include <errno.h>
+#include <pthread.h>
+#include <string.h>
+#include "esp_err.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "sys/lock.h"
+#include "rom/queue.h"
+
+#include "pthread_internal.h"
+
+#define PTHREAD_TLS_INDEX 0
+
+typedef void (*pthread_destructor_t)(void*);
+
+/* This is a very naive implementation of key-indexed thread local storage, using two linked lists
+   (one is a global list of registered keys, one per thread for thread local storage values).
+
+   It won't work well if lots of keys & thread-local values are stored (O(n) lookup for both),
+   but it should work for small amounts of data.
+*/
+typedef struct key_entry_t_ {
+    pthread_key_t key;
+    pthread_destructor_t destructor;
+    SLIST_ENTRY(key_entry_t_) next;
+} key_entry_t;
+
+// List of all keys created with pthread_key_create()
+SLIST_HEAD(key_list_t, key_entry_t_) s_keys = SLIST_HEAD_INITIALIZER(s_keys);
+
+static _lock_t s_keys_lock;
+
+// List of all value entries associated with a thread via pthread_setspecific()
+typedef struct value_entry_t_ {
+    pthread_key_t key;
+    void *value;
+    SLIST_ENTRY(value_entry_t_) next;
+} value_entry_t;
+
+// Type for the head of the list, as saved as a FreeRTOS thread local storage pointer
+SLIST_HEAD(values_list_t_, value_entry_t_);
+typedef struct values_list_t_ values_list_t;
+
+int pthread_key_create(pthread_key_t *key, pthread_destructor_t destructor)
+{
+    key_entry_t *new_key = malloc(sizeof(key_entry_t));
+    if (new_key == NULL) {
+        return ENOMEM;
+    }
+
+    _lock_acquire_recursive(&s_keys_lock);
+
+    const key_entry_t *head = SLIST_FIRST(&s_keys);
+    new_key->key = (head == NULL) ? 1 : (head->key + 1);
+    new_key->destructor = destructor;
+    *key = new_key->key;
+
+    SLIST_INSERT_HEAD(&s_keys, new_key, next);
+
+    _lock_release_recursive(&s_keys_lock);
+    return 0;
+}
+
+static key_entry_t *find_key(pthread_key_t key)
+{
+    _lock_acquire_recursive(&s_keys_lock);
+    key_entry_t *result = NULL;;
+    SLIST_FOREACH(result, &s_keys, next) {
+        if(result->key == key) {
+            break;
+        }
+    }
+    _lock_release_recursive(&s_keys_lock);
+    return result;
+}
+
+int pthread_key_delete(pthread_key_t key)
+{
+
+    _lock_acquire_recursive(&s_keys_lock);
+
+    /* Ideally, we would also walk all tasks' thread local storage value_list here
+       and delete any values associated with this key. We do not do this...
+    */
+
+    key_entry_t *entry = find_key(key);
+    if (entry != NULL) {
+        SLIST_REMOVE(&s_keys, entry, key_entry_t_, next);
+        free(entry);
+    }
+
+    _lock_release_recursive(&s_keys_lock);
+
+    return 0;
+}
+
+/* Clean up callback for deleted tasks.
+
+   This is called from one of two places:
+
+   If the thread was created via pthread_create() then it's called by pthread_task_func() when that thread ends,
+   and the FreeRTOS thread-local-storage is removed before the FreeRTOS task is deleted.
+
+   For other tasks, this is called when the FreeRTOS idle task performs its task cleanup after the task is deleted.
+
+   (The reason for calling it early for pthreads is to keep the timing consistent with "normal" pthreads, so after
+   pthread_join() the task's destructors have all been called even if the idle task hasn't run cleanup yet.)
+*/
+static void pthread_local_storage_thread_deleted_callback(int index, void *v_tls)
+{
+    values_list_t *tls = (values_list_t *)v_tls;
+    assert(tls != NULL);
+
+    /* Walk the list, freeing all entries and calling destructors if they are registered */
+    value_entry_t *entry = SLIST_FIRST(tls);
+    while(entry != NULL) {
+        // This is a little slow, walking the linked list of keys once per value,
+        // but assumes that the thread's value list will have less entries
+        // than the keys list
+        key_entry_t *key = find_key(entry->key);
+        if (key != NULL && key->destructor != NULL) {
+            key->destructor(entry->value);
+        }
+        value_entry_t *next_entry = SLIST_NEXT(entry, next);
+        free(entry);
+        entry = next_entry;
+    }
+    free(tls);
+}
+
+/* this function called from pthread_task_func for "early" cleanup of TLS in a pthread */
+void pthread_internal_local_storage_destructor_callback()
+{
+    void *tls = pvTaskGetThreadLocalStoragePointer(NULL, PTHREAD_TLS_INDEX);
+    if (tls != NULL) {
+        pthread_local_storage_thread_deleted_callback(PTHREAD_TLS_INDEX, tls);
+        /* remove the thread-local-storage pointer to avoid the idle task cleanup
+           calling it again...
+        */
+        vTaskSetThreadLocalStoragePointerAndDelCallback(NULL,
+                                                        PTHREAD_TLS_INDEX,
+                                                        NULL,
+                                                        NULL);
+    }
+}
+
+static value_entry_t *find_value(const values_list_t *list, pthread_key_t key)
+{
+    value_entry_t *result = NULL;;
+    SLIST_FOREACH(result, list, next) {
+        if(result->key == key) {
+            break;
+        }
+    }
+    return result;
+}
+
+void *pthread_getspecific(pthread_key_t key)
+{
+    values_list_t *tls = (values_list_t *) pvTaskGetThreadLocalStoragePointer(NULL, PTHREAD_TLS_INDEX);
+    if (tls == NULL) {
+        return NULL;
+    }
+
+    value_entry_t *entry = find_value(tls, key);
+    if(entry != NULL) {
+        return entry->value;
+    }
+    return NULL;
+}
+
+int pthread_setspecific(pthread_key_t key, const void *value)
+{
+    key_entry_t *key_entry = find_key(key);
+    if (key_entry == NULL) {
+        return ENOENT; // this situation is undefined by pthreads standard
+    }
+
+    values_list_t *tls = pvTaskGetThreadLocalStoragePointer(NULL, PTHREAD_TLS_INDEX);
+    if (tls == NULL) {
+        tls = calloc(1, sizeof(values_list_t));
+        if (tls == NULL) {
+            return ENOMEM;
+        }
+        vTaskSetThreadLocalStoragePointerAndDelCallback(NULL,
+                                                        PTHREAD_TLS_INDEX,
+                                                        tls,
+                                                        pthread_local_storage_thread_deleted_callback);
+    }
+
+    value_entry_t *entry = find_value(tls, key);
+    if (entry != NULL) {
+        if (value != NULL) {
+            // cast on next line is necessary as pthreads API uses
+            // 'const void *' here but elsewhere uses 'void *'
+            entry->value = (void *) value;
+        } else { // value == NULL, remove the entry
+            SLIST_REMOVE(tls, entry, value_entry_t_, next);
+            free(entry);
+        }
+    } else if (value != NULL) {
+        entry = malloc(sizeof(value_entry_t));
+        if (entry == NULL) {
+            return ENOMEM;
+        }
+        entry->key = key;
+        entry->value = (void *) value; // see note above about cast
+        SLIST_INSERT_HEAD(tls, entry, next);
+    }
+
+    return 0;
+}

components/pthread/test/test_pthread_local_storage.c

@@ -0,0 +1,107 @@
+// Test pthread_create_key, pthread_delete_key, pthread_setspecific, pthread_getspecific
+#include <pthread.h>
+#include "unity.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+TEST_CASE("pthread local storage basics", "[pthread]")
+{
+    pthread_key_t key;
+    TEST_ASSERT_EQUAL(0, pthread_key_create(&key, NULL));
+
+    TEST_ASSERT_NULL(pthread_getspecific(key));
+    int val = 3;
+
+    printf("Setting to %p...\n", &val);
+    TEST_ASSERT_EQUAL(0, pthread_setspecific(key, &val));
+
+    printf("Reading back...\n");
+    TEST_ASSERT_EQUAL_PTR(&val, pthread_getspecific(key));
+
+    printf("Setting to NULL...\n");
+    TEST_ASSERT_EQUAL(0, pthread_setspecific(key, NULL));
+
+    printf("Reading back...\n");
+    TEST_ASSERT_NULL(pthread_getspecific(key));
+
+    TEST_ASSERT_EQUAL(0, pthread_key_delete(key));
+}
+
+static void test_pthread_destructor(void *);
+static void *expected_destructor_ptr;
+static void *actual_destructor_ptr;
+static void *thread_test_pthread_destructor(void *);
+
+TEST_CASE("pthread local storage destructor", "[pthread]")
+{
+    pthread_t thread;
+    pthread_key_t key = -1;
+
+    expected_destructor_ptr = NULL;
+    actual_destructor_ptr = NULL;
+
+    TEST_ASSERT_EQUAL(0, pthread_key_create(&key, test_pthread_destructor));
+
+    TEST_ASSERT_EQUAL(0, pthread_create(&thread, NULL, thread_test_pthread_destructor, (void *)key));
+    TEST_ASSERT_EQUAL(0, pthread_join(thread, NULL));
+
+    printf("Joined...\n");
+    TEST_ASSERT_NOT_NULL(expected_destructor_ptr);
+    TEST_ASSERT_NOT_NULL(actual_destructor_ptr);
+    TEST_ASSERT_EQUAL_PTR(expected_destructor_ptr, actual_destructor_ptr);
+
+    TEST_ASSERT_EQUAL(0, pthread_key_delete(key));
+}
+
+static void task_test_pthread_destructor(void *v_key);
+
+TEST_CASE("pthread local storage destructor in FreeRTOS task", "[pthread]")
+{
+    // Same as previous test case, but doesn't use pthread APIs therefore must wait
+    // for the idle task to call the destructor
+    pthread_key_t key = -1;
+
+    expected_destructor_ptr = NULL;
+    actual_destructor_ptr = NULL;
+
+    TEST_ASSERT_EQUAL(0, pthread_key_create(&key, test_pthread_destructor));
+
+    xTaskCreate(task_test_pthread_destructor,
+                "ptdest", 8192, (void *)key, UNITY_FREERTOS_PRIORITY+1,
+                NULL);
+
+    // Above task has higher priority to us, so should run immediately
+    // but we need to wait for the idle task cleanup to run
+    vTaskDelay(20);
+
+    TEST_ASSERT_NOT_NULL(expected_destructor_ptr);
+    TEST_ASSERT_NOT_NULL(actual_destructor_ptr);
+    TEST_ASSERT_EQUAL_PTR(expected_destructor_ptr, actual_destructor_ptr);
+
+    TEST_ASSERT_EQUAL(0, pthread_key_delete(key));
+}
+
+
+
+static void *thread_test_pthread_destructor(void *v_key)
+{
+    printf("Local storage thread running...\n");
+    pthread_key_t key = (pthread_key_t) v_key;
+    expected_destructor_ptr = &key; // address of stack variable in the task...
+    pthread_setspecific(key, expected_destructor_ptr);
+    printf("Local storage thread done.\n");
+    return NULL;
+}
+
+static void test_pthread_destructor(void *value)
+{
+    printf("Destructor called...\n");
+    actual_destructor_ptr = value;
+}
+
+static void task_test_pthread_destructor(void *v_key)
+{
+    /* call the pthread main routine, then delete ourselves... */
+    thread_test_pthread_destructor(v_key);
+    vTaskDelete(NULL);
+}

docs/api-guides/freertos-smp.rst

@@ -340,6 +340,11 @@ defined to call ``vTaskSetThreadLocalStoragePointerAndDelCallback()`` with a
 ``NULL`` pointer as the deletion call back. This results in the selected Thread 
 Local Storage Pointer to have no deletion call back.
 
+In IDF the FreeRTOS thread local storage at index 0 is reserved and is used to implement
+the pthreads API thread local storage (pthread_getspecific() & pthread_setspecific()).
+Other indexes can be used for any purpose, provided
+:ref:`CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS` is set to a high enough value.
+
 For more details see :component_file:`freertos/include/freertos/task.h`
 
 .. _esp-idf-freertos-configuration: