From abc042bf56c8b17093b328c42ebc3032c542d3ab Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 24 Sep 2026 17:00:45 +0300 Subject: [PATCH] Added old multiplexer driver --- servo_controller/CMakeLists.txt | 2 + servo_controller/Kconfig | 2 + servo_controller/drivers/CMakeLists.txt | 2 + servo_controller/drivers/Kconfig | 5 + .../drivers/multiplexer/CMakeLists.txt | 1 + servo_controller/drivers/multiplexer/Kconfig | 5 + .../multiplexer/cd74hc4067/CMakeLists.txt | 4 + .../drivers/multiplexer/cd74hc4067/Kconfig | 23 +++ .../multiplexer/cd74hc4067/cd74hc4067.c | 191 ++++++++++++++++++ .../multiplexer/cd74hc4067/cd74hc4067.h | 45 +++++ servo_controller/dts/bindings/cd74hc4067.yaml | 23 +++ servo_controller/prj.conf | 7 +- servo_controller/src/adc.c | 22 +- servo_controller/src/mux.c | 44 ++++ servo_controller/src/mux.h | 13 ++ 15 files changed, 377 insertions(+), 12 deletions(-) create mode 100644 servo_controller/drivers/CMakeLists.txt create mode 100644 servo_controller/drivers/Kconfig create mode 100644 servo_controller/drivers/multiplexer/CMakeLists.txt create mode 100644 servo_controller/drivers/multiplexer/Kconfig create mode 100644 servo_controller/drivers/multiplexer/cd74hc4067/CMakeLists.txt create mode 100644 servo_controller/drivers/multiplexer/cd74hc4067/Kconfig create mode 100644 servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.c create mode 100644 servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.h create mode 100644 servo_controller/dts/bindings/cd74hc4067.yaml create mode 100644 servo_controller/src/mux.c create mode 100644 servo_controller/src/mux.h diff --git a/servo_controller/CMakeLists.txt b/servo_controller/CMakeLists.txt index 8bcca10..f6feca6 100644 --- a/servo_controller/CMakeLists.txt +++ b/servo_controller/CMakeLists.txt @@ -6,5 +6,7 @@ set(BOARD_ROOT ${CMAKE_CURRENT_SOURCE_DIR}) find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(servo_controller) +add_subdirectory(drivers) + FILE(GLOB app_sources src/*.c) target_sources(app PRIVATE ${app_sources}) diff --git a/servo_controller/Kconfig b/servo_controller/Kconfig index b4cd3a9..244854a 100644 --- a/servo_controller/Kconfig +++ b/servo_controller/Kconfig @@ -3,6 +3,8 @@ source "Kconfig.zephyr" +rsource "drivers/Kconfig" + menu "USB options" depends on USB_DEVICE_STACK_NEXT diff --git a/servo_controller/drivers/CMakeLists.txt b/servo_controller/drivers/CMakeLists.txt new file mode 100644 index 0000000..062ed79 --- /dev/null +++ b/servo_controller/drivers/CMakeLists.txt @@ -0,0 +1,2 @@ + +add_subdirectory(multiplexer) \ No newline at end of file diff --git a/servo_controller/drivers/Kconfig b/servo_controller/drivers/Kconfig new file mode 100644 index 0000000..bfe288d --- /dev/null +++ b/servo_controller/drivers/Kconfig @@ -0,0 +1,5 @@ +menu "Servo2350 Device Drivers" + +rsource "multiplexer/Kconfig" + +endmenu \ No newline at end of file diff --git a/servo_controller/drivers/multiplexer/CMakeLists.txt b/servo_controller/drivers/multiplexer/CMakeLists.txt new file mode 100644 index 0000000..cb77a4a --- /dev/null +++ b/servo_controller/drivers/multiplexer/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory_ifdef(CONFIG_CD74HC4067 cd74hc4067) \ No newline at end of file diff --git a/servo_controller/drivers/multiplexer/Kconfig b/servo_controller/drivers/multiplexer/Kconfig new file mode 100644 index 0000000..6b67f4b --- /dev/null +++ b/servo_controller/drivers/multiplexer/Kconfig @@ -0,0 +1,5 @@ +menu "Multiplexer Drivers" + +rsource "cd74hc4067/Kconfig" + +endmenu \ No newline at end of file diff --git a/servo_controller/drivers/multiplexer/cd74hc4067/CMakeLists.txt b/servo_controller/drivers/multiplexer/cd74hc4067/CMakeLists.txt new file mode 100644 index 0000000..57896cd --- /dev/null +++ b/servo_controller/drivers/multiplexer/cd74hc4067/CMakeLists.txt @@ -0,0 +1,4 @@ + +target_sources(app PRIVATE ${CMAKE_CURRENT_LIST_DIR}/cd74hc4067.c) + +target_include_directories(app PRIVATE ${CMAKE_CURRENT_LIST_DIR}) \ No newline at end of file diff --git a/servo_controller/drivers/multiplexer/cd74hc4067/Kconfig b/servo_controller/drivers/multiplexer/cd74hc4067/Kconfig new file mode 100644 index 0000000..c1d2d51 --- /dev/null +++ b/servo_controller/drivers/multiplexer/cd74hc4067/Kconfig @@ -0,0 +1,23 @@ +menuconfig CD74HC4067 + bool "TI CD74HC4067 analog multiplexer" + depends on GPIO + help + Enable support for the TI CD74HC4067 16-channel analog + multiplexer controlled through GPIO selection pins. + +if CD74HC4067 + +config CD74HC4067_INIT_PRIORITY + int "Initialization priority" + default 80 + help + Device initialization priority for the CD74HC4067 driver. + + +# Creates "CONFIG_CD74HC4067_LOG_LEVEL" +module = CD74HC4067 +module-str = cd74hc4067 +source "subsys/logging/Kconfig.template.log_config" + +endif # CD74HC4067 + diff --git a/servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.c b/servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.c new file mode 100644 index 0000000..ef40a62 --- /dev/null +++ b/servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.c @@ -0,0 +1,191 @@ +#define DT_DRV_COMPAT cd74hc4067 + +#include +#include + +#include "cd74hc4067.h" + +LOG_MODULE_REGISTER(cd74hc4067); + +//------------------------------------------------------------------------- +// Declarations + +static int cd74hc4067_init(const struct device *dev); +static int cd74hc4067_enable_pin(const struct device *dev, bool state); + +//------------------------------------------------------------------------- +// Private + + +static int cd74hc4067_init(const struct device *dev) { + int ret; + + const struct cd74hc4067_cfg *cfg = dev->config; + struct cd74hc4067_data *data = dev->data; + + k_mutex_init(&data->lock); + data->current_channel = 0U; + data->enabled = !cfg->has_enable; + + // Init the select pins + for (int i = 0; i < CD74HC4067_SELECT_PIN_COUNT; i++) { + const struct gpio_dt_spec *pin = &(cfg->select[i]); + if (!gpio_is_ready_dt(pin)) { + LOG_ERR("Select pin %d not ready.\r\n", i); + return -ENODEV; + } + + ret = gpio_pin_configure_dt(pin, GPIO_OUTPUT_INACTIVE); + if (ret < 0) { + LOG_ERR("Could not configure select pin %d as output\r\n", i); + return -ENODEV; + } + } + + if (cfg->has_enable) { + // Init the enable pin + const struct gpio_dt_spec *enable = &cfg->enable; + LOG_DBG("Initializing enable pin.\r\n"); + + if (!gpio_is_ready_dt(enable)) { + LOG_ERR("Enable pin not ready.\r\n"); + return -ENODEV; + } + + ret = gpio_pin_configure_dt(enable, GPIO_OUTPUT_INACTIVE); + if (ret < 0) { + LOG_ERR("Could not configure ENABLE pin as output\r\n"); + return -ENODEV; + } + } + + return 0; +} + +static int cd74hc4067_enable_pin(const struct device *dev, bool state) { + const struct cd74hc4067_cfg *cfg = dev->config; + struct cd74hc4067_data *data = dev->data; + int ret = 0; + + if (!cfg->has_enable) { + if (!state) { + return -ENOTSUP; + } + + data->enabled = true; + return 0; + } + + if (!device_is_ready(cfg->enable.port)) { + LOG_ERR("%s enable GPIO not ready", dev->name); + return -ENODEV; + } + + ret = gpio_pin_set_dt(&cfg->enable, state ? 0 : 1); + if (ret == 0) { + data->enabled = state; + } + + return ret; +} + +//------------------------------------------------------------------------- +// Public + +int cd74hc4067_enable(const struct device *dev) { + struct cd74hc4067_data *data = dev->data; + int ret; + + ret = k_mutex_lock(&data->lock, K_FOREVER); + if (ret != 0) { + return ret; + } + + ret = cd74hc4067_enable_pin(dev, true); + + k_mutex_unlock(&data->lock); + return ret; +} + +int cd74hc4067_disable(const struct device *dev) { + struct cd74hc4067_data *data = dev->data; + int ret; + + ret = k_mutex_lock(&data->lock, K_FOREVER); + if (ret != 0) { + return ret; + } + + ret = cd74hc4067_enable_pin(dev, false); + + k_mutex_unlock(&data->lock); + return ret; +} + +int cd74hc4067_select_channel(const struct device *dev, uint8_t channel) { + if (channel >= CD74HC4067_SELECT_CHANNEL_COUNT) { + LOG_ERR("Channel not found.\r\n"); + return -ENODEV; + } + + int ret; + const struct cd74hc4067_cfg *cfg = dev->config; + struct cd74hc4067_data *data = dev->data; + + ret = k_mutex_lock(&data->lock, K_FOREVER); + if (ret < 0) { + LOG_ERR("Mutex error"); + return ret; + } + + for (int i = 0; i < CD74HC4067_SELECT_PIN_COUNT; i++) { + bool state = (channel >> i) & 0x1; + ret = gpio_pin_set_dt(&cfg->select[i], state); + if (ret < 0) { + LOG_ERR("Unable to set channel pin: %d", ret); + goto out; + } + } + + data->current_channel = channel; + +out: + k_mutex_unlock(&data->lock); + return ret; +} + +//------------------------------------------------------------------------- +// Devicetree + +// static const struct cd74hc4067_api cd74hc4067_api_funcs = { +// .enable = cd74hc4067_enable, +// .disable = cd74hc4067_disable, +// .select_channel = cd74hc4067_select_channel, +// }; + + +#define CD74HC4067_DEFINE(inst) \ + \ + static const struct cd74hc4067_cfg cd74hc4067_cfg_##inst = { \ + .select = { \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 0), \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 1), \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 2), \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 3), \ + }, \ + .enable = COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, enable_gpio), \ + (GPIO_DT_SPEC_GET(DT_DRV_INST(inst), enable_gpio)), \ + ((struct gpio_dt_spec){0})), \ + .has_enable = DT_INST_NODE_HAS_PROP(inst, enable_gpio), \ + }; \ + static struct cd74hc4067_data cd74hc4067_data_##inst; \ + DEVICE_DT_INST_DEFINE( inst, \ + cd74hc4067_init, \ + NULL, \ + &cd74hc4067_data_##inst, \ + &cd74hc4067_cfg_##inst, \ + POST_KERNEL, \ + CONFIG_CD74HC4067_INIT_PRIORITY, \ + NULL); + +DT_INST_FOREACH_STATUS_OKAY(CD74HC4067_DEFINE) \ No newline at end of file diff --git a/servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.h b/servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.h new file mode 100644 index 0000000..b7c7a4f --- /dev/null +++ b/servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.h @@ -0,0 +1,45 @@ +#ifndef SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_ +#define SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_ + +#include + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +// struct cd74hc4067_api { +// int (*enable)(const struct device *dev); +// int (*disable)(const struct device *dev); +// int (*select_channel)(const struct device *dev, uint8_t channel); +// }; + +struct cd74hc4067_data { + struct k_mutex lock; + uint8_t current_channel; + bool enabled; +}; + +#define CD74HC4067_SELECT_CHANNEL_COUNT 16U +#define CD74HC4067_SELECT_PIN_COUNT 4U +struct cd74hc4067_cfg { + struct gpio_dt_spec select[CD74HC4067_SELECT_PIN_COUNT]; + struct gpio_dt_spec enable; + bool has_enable; +}; + +int cd74hc4067_enable(const struct device *dev); +int cd74hc4067_disable(const struct device *dev); +int cd74hc4067_select_channel(const struct device *dev, uint8_t channel); + + +#ifdef __cplusplus +} +#endif + +#endif /* SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_ */ + diff --git a/servo_controller/dts/bindings/cd74hc4067.yaml b/servo_controller/dts/bindings/cd74hc4067.yaml new file mode 100644 index 0000000..add805a --- /dev/null +++ b/servo_controller/dts/bindings/cd74hc4067.yaml @@ -0,0 +1,23 @@ +description: | + TI CD74HC4067 16-channel analog multiplexer. + +compatible: "cd74hc4067" + +include: base.yaml + +properties: + select-gpios: + type: phandle-array + required: true + description: | + GPIO pins for channel selection (S0-S3). + Array must contain exactly 4 GPIOs: + - Index 0: S0 + - Index 1: S1 + - Index 2: S2 + - Index 3: S3 + + enable-gpio: + type: phandle + description: | + Optional GPIO for common enable pin (E). \ No newline at end of file diff --git a/servo_controller/prj.conf b/servo_controller/prj.conf index aa4d4e3..5caec1c 100644 --- a/servo_controller/prj.conf +++ b/servo_controller/prj.conf @@ -2,6 +2,9 @@ CONFIG_GPIO=y CONFIG_PWM=y CONFIG_ADC=y +# Mux +CONFIG_CD74HC4067=y + # Serial CONFIG_SERIAL=y CONFIG_CONSOLE=y @@ -29,4 +32,6 @@ CONFIG_LOG_MODE_IMMEDIATE=y # DEBUG CONFIG_DEBUG_THREAD_INFO=y # CONFIG_DEBUG=y -# CONFIG_DEBUG_OPTIMIZATIONS=y \ No newline at end of file +# CONFIG_DEBUG_OPTIMIZATIONS=y +CONFIG_OUTPUT_DISASSEMBLY=n # Create disassmebly files +#CONFIG_OUTPUT_DISASSEMBLE_ALL=n \ No newline at end of file diff --git a/servo_controller/src/adc.c b/servo_controller/src/adc.c index aa9a226..26b162e 100644 --- a/servo_controller/src/adc.c +++ b/servo_controller/src/adc.c @@ -1,6 +1,7 @@ #include "adc.h" #include "command_message.h" #include "usb.h" +#include "mux.h" #include #include @@ -79,11 +80,11 @@ int adc_init_all(void) { // TIMER k_timer_init(&adc_timer, adc_timer_handler, NULL); - // ret = mux_init(); - // if (ret != 0) { - // LOG_ERR("Failed to setup multiplexer: %d", ret); - // return ret; - // } + ret = mux_init(); + if (ret != 0) { + LOG_ERR("Failed to setup multiplexer: %d", ret); + return ret; + } if (!adc_is_ready_dt(&adc_mux_spec)) { LOG_ERR("ADC mux channel not ready"); @@ -143,12 +144,11 @@ int adc_read_id(int id) { // Read muxed channels (void)adc_sequence_init_dt(&adc_mux_spec, &sequence); - // Set mux TODO: - // ret = mux_select_channel(id); - // if (ret < 0) { - // LOG_ERR("Could not set multiplexer (%d)\n", ret); - // return 0; - // } + ret = mux_select_channel(id); + if (ret < 0) { + LOG_ERR("Could not set multiplexer (%d)\n", ret); + return 0; + } ret = adc_read_dt(&adc_mux_spec, &sequence); if (ret < 0) { diff --git a/servo_controller/src/mux.c b/servo_controller/src/mux.c new file mode 100644 index 0000000..e867222 --- /dev/null +++ b/servo_controller/src/mux.c @@ -0,0 +1,44 @@ +#include "mux.h" + +#include +#include + +LOG_MODULE_REGISTER(mux); + +#define MUX_NODE DT_ALIAS(mux) + +static const struct device *mux_dev = DEVICE_DT_GET(MUX_NODE); + +int mux_init(void) { + int ret; + + if (!DT_NODE_HAS_STATUS(MUX_NODE, okay)) { + LOG_ERR("mux alias missing or disabled in devicetree"); + return -ENODEV; + } + + if (!device_is_ready(mux_dev)) { + LOG_ERR("Multiplexer device not ready"); + return -ENODEV; + } + + ret = cd74hc4067_enable(mux_dev); + if ((ret != 0) && (ret != -ENOTSUP)) { + LOG_ERR("Failed to enable multiplexer: %d", ret); + return ret; + } + + ret = cd74hc4067_select_channel(mux_dev, 0U); + if (ret != 0) { + LOG_ERR("Failed to select initial channel: %d", ret); + return ret; + } + + LOG_INF("Multiplexer HAL initialized"); + + return 0; +} + +int mux_select_channel(int channel) { + return cd74hc4067_select_channel(mux_dev, channel); +} \ No newline at end of file diff --git a/servo_controller/src/mux.h b/servo_controller/src/mux.h new file mode 100644 index 0000000..06b16ec --- /dev/null +++ b/servo_controller/src/mux.h @@ -0,0 +1,13 @@ +#ifndef MUX_H +#define MUX_H + + +#include "cd74hc4067.h" + +#define MUX_CHANNELS CD74HC4067_SELECT_CHANNEL_COUNT + +int mux_init(void); +int mux_select_channel(int channel); + + +#endif // MUX_H