From a264a96321b30dd6bc67f1cb4d066dc035e73903 Mon Sep 17 00:00:00 2001 From: Ken Van Hoeylandt Date: Sat, 19 Sep 2026 15:13:22 +0200 Subject: [PATCH] Device hotplug support --- TactilityKernel/include/tactility/device.h | 27 ++++ TactilityKernel/include/tactility/driver.h | 7 + TactilityKernel/source/device.cpp | 86 +++++++++++- TactilityKernel/source/driver.cpp | 5 + TactilityKernel/source/kernel_init.cpp | 14 +- .../tests/source/device_hotplug_test.cpp | 129 ++++++++++++++++++ 6 files changed, 265 insertions(+), 3 deletions(-) create mode 100644 TactilityKernel/tests/source/device_hotplug_test.cpp diff --git a/TactilityKernel/include/tactility/device.h b/TactilityKernel/include/tactility/device.h index d919d9f23..e5c4a9c1b 100644 --- a/TactilityKernel/include/tactility/device.h +++ b/TactilityKernel/include/tactility/device.h @@ -113,6 +113,7 @@ error_t device_remove(struct Device* device); * device's lock for its entire body, so doing so would deadlock. * @param[in,out] device non-null device pointer * @retval ERROR_INVALID_STATE if the device is already started or not added + * @retval ERROR_NOT_FOUND probe() reported the device isn't present * @retval ERROR_RESOURCE when driver binding fails * @retval ERROR_NONE on success */ @@ -389,6 +390,32 @@ bool device_has_active_by_type(const struct DeviceType* type); */ error_t device_get_first_by_compatible(const char* compatible, struct Device** out_device); +/** Consecutive device_hotplug_poll_once() calls a probe() result must hold before it takes effect. */ +#define DEVICE_HOTPLUG_DEBOUNCE_COUNT 2 + +/** + * Registers @a device with the hotplug poller (see device_hotplug_poll_once()): started once + * probe() (or immediately if NULL) reports present, stopped when it reports absent. A device + * never registered here is never touched by the poller. + * @warning Call only at boot, before polling starts. + */ +void device_hotplug_register(struct Device* device); + +/** Reverses device_hotplug_register(). No-op if not registered. */ +void device_hotplug_unregister(struct Device* device); + +/** + * Starts/stops every device_hotplug_register()'d device per its driver's probe() (or + * unconditionally, once, if probe is NULL), debounced across DEVICE_HOTPLUG_DEBOUNCE_COUNT + * calls. Never removes/destructs a device. + * @retval ERROR_NONE on success + * @retval (other) a mandatory (probe == NULL) device failed to start + */ +error_t device_hotplug_poll_once(void); + +/** Starts a periodic timer that calls device_hotplug_poll_once(). */ +void device_hotplug_start_polling(void); + #ifdef __cplusplus } #endif diff --git a/TactilityKernel/include/tactility/driver.h b/TactilityKernel/include/tactility/driver.h index 7ae115a3a..921d1a7b9 100644 --- a/TactilityKernel/include/tactility/driver.h +++ b/TactilityKernel/include/tactility/driver.h @@ -23,6 +23,12 @@ struct Driver { error_t (*start_device)(struct Device* dev); /** Function to deinitialize the driver for a device */ error_t (*stop_device)(struct Device* dev); + /** + * Optional: reports whether the hardware is physically present. Checked by driver_bind() + * and periodically by device_hotplug_poll_once(). NULL means always present. + * @warning Must not call device_add()/device_remove() - may run while the ledger is locked. + */ + bool (*probe)(struct Device* device); /** Contains the driver's functions */ const void* api; /** Which type of devices this driver creates (can be NULL) */ @@ -100,6 +106,7 @@ error_t driver_remove_destruct(struct Driver* driver); * * @param driver The driver to bind. * @param device The device to bind to. + * @retval ERROR_NOT_FOUND probe() reported the device isn't present * @return ERROR_NONE if successful, or an error code otherwise. */ error_t driver_bind(struct Driver* driver, struct Device* device); diff --git a/TactilityKernel/source/device.cpp b/TactilityKernel/source/device.cpp index b852a68e6..26a0dde22 100644 --- a/TactilityKernel/source/device.cpp +++ b/TactilityKernel/source/device.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -43,6 +44,11 @@ struct DeviceInternal { } state; /** Attached child devices */ std::vector children {}; + // device_hotplug_poll_once() debounce state. + struct { + uint8_t consecutive = 0; // probe() results differing from `present` so far + bool present = false; + } hotplug; // Outstanding device_get() holders. Guarded by `mutex`. Independent of state.started - // device_get()/device_put() bracket construct/destruct, not start/stop, so a ref can be held // across a device_stop(). device_destruct() refuses to run while this is > 0. @@ -225,7 +231,7 @@ error_t device_start(Device* device) { unlock_internal(internal); if (bind_error != ERROR_NONE) { - return ERROR_RESOURCE; + return bind_error == ERROR_NOT_FOUND ? ERROR_NOT_FOUND : ERROR_RESOURCE; } device_listener_notify(device, DEVICE_EVENT_STARTED); @@ -559,4 +565,82 @@ error_t device_get_first_by_compatible(const char* compatible, Device** out_devi return error; } +struct HotplugAction { + Device* device; + bool start; // vs. stop + bool mandatory; // probe == nullptr; a start failure here is a real error +}; + +// Boot-time-only writes, so no lock: registration finishes before polling starts. +static std::vector hotplug_registered_devices; + +static Timer* hotplug_timer = nullptr; + +static void hotplug_timer_callback(void*) { + device_hotplug_poll_once(); +} + +void device_hotplug_register(Device* device) { + hotplug_registered_devices.push_back(device); +} + +void device_hotplug_unregister(Device* device) { + const auto iterator = std::ranges::find(hotplug_registered_devices, device); + if (iterator != hotplug_registered_devices.end()) { + hotplug_registered_devices.erase(iterator); + } +} + +error_t device_hotplug_poll_once(void) { + // Collect actions first: start_device()/stop_device() may add/remove child devices + // (ledger_lock), so don't call them mid-iteration. + std::vector actions; + for (Device* device : hotplug_registered_devices) { + auto* internal = device->internal; + auto* driver = internal->driver; + if (driver == nullptr) { + continue; + } + + if (driver->probe == nullptr) { + if (!internal->state.started) { + actions.push_back({ device, true, true }); + } + continue; + } + + bool present = driver->probe(device); + if (present == internal->hotplug.present) { + internal->hotplug.consecutive = 0; + continue; + } + + if (++internal->hotplug.consecutive < DEVICE_HOTPLUG_DEBOUNCE_COUNT) { + continue; + } + + internal->hotplug.present = present; + internal->hotplug.consecutive = 0; + actions.push_back({ device, present, false }); + } + + error_t first_mandatory_error = ERROR_NONE; + for (const auto& action : actions) { + error_t error = action.start ? device_start(action.device) : device_stop(action.device); + if (action.mandatory && action.start && error != ERROR_NONE && first_mandatory_error == ERROR_NONE) { + first_mandatory_error = error; + } + } + + return first_mandatory_error; +} + +void device_hotplug_start_polling(void) { + if (hotplug_timer != nullptr) { + return; + } + hotplug_timer = timer_alloc(TIMER_TYPE_PERIODIC, pdMS_TO_TICKS(500), hotplug_timer_callback, nullptr); + timer_start(hotplug_timer); +} + } // extern "C" diff --git a/TactilityKernel/source/driver.cpp b/TactilityKernel/source/driver.cpp index 9a05d6ec5..fd2630693 100644 --- a/TactilityKernel/source/driver.cpp +++ b/TactilityKernel/source/driver.cpp @@ -129,6 +129,11 @@ error_t driver_bind(Driver* driver, Device* device) { goto error; } + if (driver->probe != nullptr && !driver->probe(device)) { + error = ERROR_NOT_FOUND; + goto error; + } + if (driver->start_device != nullptr) { error = driver->start_device(device); if (error != ERROR_NONE) { diff --git a/TactilityKernel/source/kernel_init.cpp b/TactilityKernel/source/kernel_init.cpp index c31cc8531..2af14de6a 100644 --- a/TactilityKernel/source/kernel_init.cpp +++ b/TactilityKernel/source/kernel_init.cpp @@ -65,13 +65,16 @@ error_t kernel_init(Module* const dts_modules[], const DtsDevice dts_devices[]) dts_module++; } + // Starting is deferred to device_hotplug_poll_once() below so probe() can run first. + // DISABLED devices are never registered, so the poller never starts them. const DtsDevice* dts_device = dts_devices; while (dts_device->device != nullptr) { if (dts_device->status == DTS_DEVICE_STATUS_OKAY) { - if (device_construct_add_start(dts_device->device, dts_device->compatible) != ERROR_NONE) { - LOG_E(TAG, "kernel_init failed to construct+add+start device: %s (%s)", dts_device->device->name, dts_device->compatible); + if (device_construct_add(dts_device->device, dts_device->compatible) != ERROR_NONE) { + LOG_E(TAG, "kernel_init failed to construct+add device: %s (%s)", dts_device->device->name, dts_device->compatible); return ERROR_RESOURCE; } + device_hotplug_register(dts_device->device); } else if (dts_device->status == DTS_DEVICE_STATUS_DISABLED) { if (device_construct_add(dts_device->device, dts_device->compatible) != ERROR_NONE) { LOG_E(TAG, "kernel_init failed to construct+add device: %s (%s)", dts_device->device->name, dts_device->compatible); @@ -83,6 +86,13 @@ error_t kernel_init(Module* const dts_modules[], const DtsDevice dts_devices[]) dts_device++; } + // A mandatory (probe == NULL) device's start failure aborts boot, as before. + if (device_hotplug_poll_once() != ERROR_NONE) { + LOG_E(TAG, "kernel_init failed to start a mandatory device"); + return ERROR_RESOURCE; + } + device_hotplug_start_polling(); + LOG_I(TAG, "init done"); return ERROR_NONE; } diff --git a/TactilityKernel/tests/source/device_hotplug_test.cpp b/TactilityKernel/tests/source/device_hotplug_test.cpp new file mode 100644 index 000000000..3d90d4579 --- /dev/null +++ b/TactilityKernel/tests/source/device_hotplug_test.cpp @@ -0,0 +1,129 @@ +#include "doctest.h" +#include +#include +#include + +static Module hotplug_test_module = { + .name = "hotplug_test_module", + .start = nullptr, + .stop = nullptr +}; + +static bool probe_present = false; +static int probe_driver_start_called = 0; +static int probe_driver_stop_called = 0; + +static bool probe_driver_probe(Device*) { return probe_present; } +static error_t probe_driver_start(Device*) { probe_driver_start_called++; return ERROR_NONE; } +static error_t probe_driver_stop(Device*) { probe_driver_stop_called++; return ERROR_NONE; } + +static Driver probe_driver = { + .name = "hotplug_test_probe_driver", + .compatible = (const char*[]) { "hotplug_test,probe", nullptr }, + .start_device = probe_driver_start, + .stop_device = probe_driver_stop, + .probe = probe_driver_probe, + .api = nullptr, + .device_type = nullptr, + .owner = &hotplug_test_module, + .internal = nullptr, +}; + +TEST_CASE("device_hotplug_poll_once starts/stops a probe-capable device after debounce") { + probe_present = false; + probe_driver_start_called = 0; + probe_driver_stop_called = 0; + + static Device device { + .name = "hotplug_probe_device", + .config = nullptr, + .parent = nullptr, + }; + + CHECK_EQ(driver_construct_add(&probe_driver), ERROR_NONE); + CHECK_EQ(device_construct_add(&device, "hotplug_test,probe"), ERROR_NONE); + device_hotplug_register(&device); + + // Not present: repeated polling never starts it. + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK_EQ(probe_driver_start_called, 0); + CHECK_FALSE(device_is_ready(&device)); + + // Present: needs DEVICE_HOTPLUG_DEBOUNCE_COUNT consecutive polls before it actually starts. + probe_present = true; + for (int i = 0; i < DEVICE_HOTPLUG_DEBOUNCE_COUNT - 1; i++) { + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK_FALSE(device_is_ready(&device)); + } + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK(device_is_ready(&device)); + CHECK_EQ(probe_driver_start_called, 1); + + // Already started: further polls while still present don't restart it. + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK_EQ(probe_driver_start_called, 1); + + // Absent again: needs the same debounce before it stops. + probe_present = false; + for (int i = 0; i < DEVICE_HOTPLUG_DEBOUNCE_COUNT - 1; i++) { + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK(device_is_ready(&device)); + } + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK_FALSE(device_is_ready(&device)); + CHECK_EQ(probe_driver_stop_called, 1); + + // Only started/stopped, never removed/destructed. + CHECK(device_is_added(&device)); + + device_hotplug_unregister(&device); + CHECK_EQ(device_remove(&device), ERROR_NONE); + CHECK_EQ(device_destruct(&device), ERROR_NONE); + CHECK_EQ(driver_remove_destruct(&probe_driver), ERROR_NONE); +} + +static int mandatory_driver_start_called = 0; + +static error_t mandatory_driver_start(Device*) { mandatory_driver_start_called++; return ERROR_NONE; } +static error_t mandatory_driver_stop(Device*) { return ERROR_NONE; } + +static Driver mandatory_driver = { + .name = "hotplug_test_mandatory_driver", + .compatible = (const char*[]) { "hotplug_test,mandatory", nullptr }, + .start_device = mandatory_driver_start, + .stop_device = mandatory_driver_stop, + .probe = nullptr, + .api = nullptr, + .device_type = nullptr, + .owner = &hotplug_test_module, + .internal = nullptr, +}; + +TEST_CASE("device_hotplug_poll_once starts a mandatory (probe == NULL) device unconditionally on the first call") { + mandatory_driver_start_called = 0; + + static Device device { + .name = "hotplug_mandatory_device", + .config = nullptr, + .parent = nullptr, + }; + + CHECK_EQ(driver_construct_add(&mandatory_driver), ERROR_NONE); + CHECK_EQ(device_construct_add(&device, "hotplug_test,mandatory"), ERROR_NONE); + device_hotplug_register(&device); + + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK(device_is_ready(&device)); + CHECK_EQ(mandatory_driver_start_called, 1); + + // Already started: further polls don't restart it. + CHECK_EQ(device_hotplug_poll_once(), ERROR_NONE); + CHECK_EQ(mandatory_driver_start_called, 1); + + device_hotplug_unregister(&device); + CHECK_EQ(device_stop(&device), ERROR_NONE); + CHECK_EQ(device_remove(&device), ERROR_NONE); + CHECK_EQ(device_destruct(&device), ERROR_NONE); + CHECK_EQ(driver_remove_destruct(&mandatory_driver), ERROR_NONE); +}