diff --git a/platforms/qurt/src/px4/tasks.cpp b/platforms/qurt/src/px4/tasks.cpp index 568f9af03b9..2c5c1987561 100644 --- a/platforms/qurt/src/px4/tasks.cpp +++ b/platforms/qurt/src/px4/tasks.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include "hrt_work.h" @@ -51,6 +52,7 @@ typedef struct task_entry { pthread_t tid; + px4_task_t id; char name[PX4_TASK_MAX_NAME_LENGTH + 4]; char stack[PX4_TASK_STACK_SIZE]; pthread_attr_t attr; @@ -59,14 +61,135 @@ typedef struct task_entry { char argv_storage[PX4_TASK_MAX_ARGC][PX4_TASK_MAX_ARGV_LENGTH]; char *argv[PX4_TASK_MAX_ARGC]; bool isused; + bool exiting; + bool joining; + bool waiting; - task_entry() : isused(false) {} + task_entry() : tid(0), id(-1), isused(false), exiting(false), joining(false), waiting(false) {} } task_entry_t; static task_entry_t taskmap[PX4_MAX_TASKS]; -static bool task_mutex_initialized = false; -static pthread_mutex_t task_mutex; +static pthread_mutex_t task_mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t task_cond = PTHREAD_COND_INITIALIZER; + +// Task handles must not alias a new task when a table slot is reused. Every +// successfully issued handle below next_task_id is either still in taskmap or +// has already been joined. This also lets a late px4_task_join() succeed after +// automatic reaping, without retaining an unbounded list of completed tasks. +static unsigned int next_task_id = 0; + +// All taskmap helpers require task_mutex to be held. +static task_entry_t *find_task(px4_task_t id) +{ + for (task_entry_t &task : taskmap) { + if (task.isused && task.id == id) { + return &task; + } + } + + return nullptr; +} + +static task_entry_t *find_current_task() +{ + const pthread_t tid = pthread_self(); + + for (task_entry_t &task : taskmap) { + // A joined pthread's native ID can be reused before its joiner gets + // task_mutex back. Do not mistake that old entry for the new thread. + if (task.isused && !task.exiting && task.tid == tid) { + return &task; + } + } + + return nullptr; +} + +// Construct and destroy with task_mutex held. Cancelling a task while it owns +// a join or waits on task_cond could strand a reserved slot or a locked mutex. +// QuRT does not expose pthread cleanup handlers, so deletion returns EBUSY +// during these waits instead. Cooperative task shutdown is unaffected. +class TaskWaitGuard +{ +public: + TaskWaitGuard() : _task(find_current_task()) + { + if (_task) { _task->waiting = true; } + } + + ~TaskWaitGuard() + { + if (_task) { _task->waiting = false; } + } + +private: + task_entry_t *_task; +}; + +static int join_task(task_entry_t &task) +{ + // Keep the slot reserved while the mutex is released. Only one caller may + // join this pthread, and it must terminate before its argv/attributes can + // be reused. In particular, never hold task_mutex across pthread_join(). + task.joining = true; + const pthread_t tid = task.tid; + TaskWaitGuard wait_guard; + pthread_mutex_unlock(&task_mutex); + const int ret = pthread_join(tid, nullptr); + pthread_mutex_lock(&task_mutex); + + if (ret == 0) { + pthread_attr_destroy(&task.attr); + task.tid = 0; + task.isused = false; + } + + task.joining = false; + pthread_cond_broadcast(&task_cond); + return ret; +} + +static int wait_for_task(px4_task_t id) +{ + if (id < 0 || static_cast(id) >= next_task_id) { + return -EINVAL; + } + + while (task_entry_t *task = find_task(id)) { + if (!task->exiting && task->tid == pthread_self()) { + return -EINVAL; + } + + if (!task->joining) { + return join_task(*task); + } + + // Another join/delete/spawn owns the pthread_join(). Recheck the + // handle after waking because its slot may already have been reused. + TaskWaitGuard wait_guard; + const int ret = pthread_cond_wait(&task_cond, &task_mutex); + + if (ret != 0) { + return ret; + } + } + + return 0; // This handle was already reaped. +} + +static void reap_exiting_tasks() +{ + for (task_entry_t &task : taskmap) { + if (task.isused && task.exiting && !task.joining && task.tid != pthread_self()) { + const int ret = join_task(task); + + if (ret != 0) { + PX4_ERR("Couldn't join exited task %d (%d)", task.id, ret); + } + } + } +} // These are some Qurt pthread stubs needed for compilation and linking extern "C" { @@ -114,12 +237,13 @@ static void *entry_adapter(void *ptr) { task_entry_t *data; data = (task_entry_t *) ptr; + int ret = -1; - if (data->main_entry) { data->main_entry(data->argc, data->argv); } + if (data->main_entry) { ret = data->main_entry(data->argc, data->argv); } else { PX4_ERR("No valid task entry points"); } - pthread_exit(nullptr); + px4_task_exit(ret); return nullptr; } @@ -133,13 +257,19 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma PX4_INFO("Creating pthread %s", name); - if (task_mutex_initialized == false) { - task_mutex_initialized = true; - pthread_mutex_init(&task_mutex, nullptr); - } - pthread_mutex_lock(&task_mutex); + // Normal module shutdown does not call px4_task_join(). Reap completed + // threads here so both their slots and pthread resources can be reused. + reap_exiting_tasks(); + + // Do not wrap task handles and risk operating on a replacement task. + if (next_task_id > static_cast(INT_MAX)) { + pthread_mutex_unlock(&task_mutex); + PX4_ERR("Task handles exhausted"); + return -1; + } + for (task_index = 0; task_index < PX4_MAX_TASKS; task_index++) { if (taskmap[task_index].isused == false) { break; } } @@ -256,10 +386,15 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma } taskmap[task_index].isused = true; + taskmap[task_index].exiting = false; + taskmap[task_index].joining = false; + taskmap[task_index].waiting = false; + const px4_task_t task_id = static_cast(next_task_id++); + taskmap[task_index].id = task_id; pthread_mutex_unlock(&task_mutex); - return task_index; + return task_id; } px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, @@ -275,126 +410,79 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int int px4_task_join(px4_task_t id) { - if (id < 0 || id >= PX4_MAX_TASKS) { - return -EINVAL; - } - - if (!task_mutex_initialized) { - return -EINVAL; - } - pthread_mutex_lock(&task_mutex); - pthread_t pid = taskmap[id].tid; + const int ret = wait_for_task(id); pthread_mutex_unlock(&task_mutex); - - if (pid == 0 || pthread_self() == pid) { - return -EINVAL; - } - - int rv = pthread_join(pid, nullptr); - - if (rv == 0) { - pthread_mutex_lock(&task_mutex); - - if (taskmap[id].isused && taskmap[id].tid == pid) { - pthread_attr_destroy(&taskmap[id].attr); - taskmap[id].isused = false; - } - - pthread_mutex_unlock(&task_mutex); - } - - return rv; + return ret; } int px4_task_delete(px4_task_t id) { - int rv = 0; + pthread_mutex_lock(&task_mutex); - PX4_ERR("Ignoring px4_task_delete for task %d", id); - - pthread_t pid; - PX4_WARN("Called px4_task_delete"); - - if (id < PX4_MAX_TASKS && taskmap[id].isused) { - pid = taskmap[id].tid; - - } else { + if (id < 0 || static_cast(id) >= next_task_id) { + pthread_mutex_unlock(&task_mutex); return -EINVAL; } - pthread_mutex_lock(&task_mutex); + task_entry_t *task = find_task(id); - if (pthread_self() == pid) { - pthread_join(pid, nullptr); - pthread_attr_destroy(&taskmap[id].attr); - taskmap[id].isused = false; + if (task == nullptr) { pthread_mutex_unlock(&task_mutex); - pthread_exit(nullptr); - - } else { - rv = pthread_cancel(pid); - pthread_mutex_unlock(&task_mutex); - - if (rv == 0) { - pthread_join(pid, nullptr); - } - - pthread_mutex_lock(&task_mutex); + return 0; // Already reaped; never cancel a replacement task. } - pthread_attr_destroy(&taskmap[id].attr); - taskmap[id].isused = false; - pthread_mutex_unlock(&task_mutex); + if (!task->exiting && pthread_self() == task->tid) { + pthread_mutex_unlock(&task_mutex); + px4_task_exit(0); + return 0; + } - return rv; + if (task->waiting) { + pthread_mutex_unlock(&task_mutex); + return -EBUSY; + } + + if (!task->exiting) { + const int ret = pthread_cancel(task->tid); + + if (ret != 0) { + pthread_mutex_unlock(&task_mutex); + return ret; + } + + // Cancellation may bypass px4_task_exit(). Prevent another caller + // from cancelling/signalling a native ID that has already been joined. + task->exiting = true; + } + + const int ret = wait_for_task(id); + pthread_mutex_unlock(&task_mutex); + return ret; } void px4_task_exit(int ret) { - PX4_ERR("Ignoring px4_task_exit with return value %d", ret); + pthread_mutex_lock(&task_mutex); + task_entry_t *task = find_current_task(); - int i; - pthread_t pid = pthread_self(); - - for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].tid == pid) { - pthread_mutex_lock(&task_mutex); - pthread_attr_destroy(&taskmap[i].attr); - taskmap[i].isused = false; - break; - } - } - - if (i >= PX4_MAX_TASKS) { - PX4_ERR("px4_task_exit: self task not found!"); - - } else { - PX4_DEBUG("px4_task_exit: %s", taskmap[i].name); - pthread_mutex_unlock(&task_mutex); + if (task) { + // Retain the slot until another thread has joined us. Clearing it + // here would permit reuse before pthread_exit() has completed. + task->exiting = true; } + pthread_mutex_unlock(&task_mutex); pthread_exit((void *)(unsigned long)ret); } int px4_task_kill(px4_task_t id, int sig) { - int rv = 0; - pthread_t pid; - PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name); - - if (id < PX4_MAX_TASKS && taskmap[id].tid != 0) { - pthread_mutex_lock(&task_mutex); - pid = taskmap[id].tid; - pthread_mutex_unlock(&task_mutex); - - } else { - return -EINVAL; - } - - rv = pthread_kill(pid, sig); - - return rv; + pthread_mutex_lock(&task_mutex); + task_entry_t *task = find_task(id); + const int ret = (task != nullptr && !task->exiting) ? pthread_kill(task->tid, sig) : -EINVAL; + pthread_mutex_unlock(&task_mutex); + return ret; } void px4_show_tasks() @@ -403,15 +491,18 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); + pthread_mutex_lock(&task_mutex); for (; idx < PX4_MAX_TASKS; idx++) { - if (taskmap[idx].isused) { + if (taskmap[idx].isused && !taskmap[idx].exiting) { PX4_INFO(" %-10s %u", taskmap[idx].name, (unsigned int) taskmap[idx].tid); count++; } } + pthread_mutex_unlock(&task_mutex); + if (count == 0) { PX4_INFO("No running tasks"); } @@ -419,17 +510,9 @@ void px4_show_tasks() px4_task_t px4_getpid() { - pthread_t pid = pthread_self(); - px4_task_t ret = -1; - pthread_mutex_lock(&task_mutex); - - for (int i = 0; i < PX4_MAX_TASKS; i++) { - if (taskmap[i].isused && taskmap[i].tid == pid) { - ret = i; - } - } - + task_entry_t *task = find_current_task(); + const px4_task_t ret = task ? task->id : -1; pthread_mutex_unlock(&task_mutex); return ret; @@ -438,17 +521,9 @@ px4_task_t px4_getpid() const char *px4_get_taskname() { - pthread_t pid = pthread_self(); - const char *prog_name = "UnknownApp"; - pthread_mutex_lock(&task_mutex); - - for (int i = 0; i < PX4_MAX_TASKS; i++) { - if (taskmap[i].isused && taskmap[i].tid == pid) { - prog_name = taskmap[i].name; - } - } - + task_entry_t *task = find_current_task(); + const char *prog_name = task ? task->name : "UnknownApp"; pthread_mutex_unlock(&task_mutex); return prog_name; @@ -485,12 +560,10 @@ int px4_prctl(int option, const char *arg2, px4_task_t pid) pthread_mutex_lock(&task_mutex); - for (int i = 0; i < PX4_MAX_TASKS; i++) { - if (taskmap[i].isused && taskmap[i].tid == (pthread_t) pid) { - rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2); - pthread_mutex_unlock(&task_mutex); - return rv; - } + task_entry_t *task = find_task(pid); + + if (task != nullptr && !task->exiting) { + rv = pthread_attr_setthreadname(&task->attr, arg2); } pthread_mutex_unlock(&task_mutex);