Added old multiplexer driver

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

servo_controller/Kconfig

@@ -3,6 +3,8 @@
 
 source "Kconfig.zephyr"
 
+rsource "drivers/Kconfig"
+
 menu "USB options"
 	depends on USB_DEVICE_STACK_NEXT
 

servo_controller/drivers/CMakeLists.txt

@@ -0,0 +1,2 @@
+
+add_subdirectory(multiplexer)

servo_controller/drivers/Kconfig

@@ -0,0 +1,5 @@
+menu "Servo2350 Device Drivers"
+
+rsource "multiplexer/Kconfig"
+
+endmenu

servo_controller/drivers/multiplexer/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory_ifdef(CONFIG_CD74HC4067 cd74hc4067)

servo_controller/drivers/multiplexer/Kconfig

@@ -0,0 +1,5 @@
+menu "Multiplexer Drivers"
+
+rsource "cd74hc4067/Kconfig"
+
+endmenu

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

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
+

servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.c

@@ -0,0 +1,191 @@
+#define DT_DRV_COMPAT cd74hc4067
+
+#include <errno.h>
+#include <zephyr/logging/log.h>
+
+#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)

servo_controller/drivers/multiplexer/cd74hc4067/cd74hc4067.h

@@ -0,0 +1,45 @@
+#ifndef SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_
+#define SERVO2350_DRIVERS_MULTIPLEXER_CD74HC4067_H_
+
+#include <stdint.h>
+
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/gpio.h>
+
+#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_ */
+

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

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
+# CONFIG_DEBUG_OPTIMIZATIONS=y
+CONFIG_OUTPUT_DISASSEMBLY=n		# Create disassmebly files
+#CONFIG_OUTPUT_DISASSEMBLE_ALL=n

servo_controller/src/adc.c

@@ -1,6 +1,7 @@
 #include "adc.h"
 #include "command_message.h"
 #include "usb.h"
+#include "mux.h"
 
 #include <zephyr/logging/log.h>
 #include <zephyr/drivers/adc.h>
@@ -79,11 +80,11 @@ int adc_init_all(void) {
 	// TIMER
 	k_timer_init(&adc_timer, adc_timer_handler, NULL);
 
-	// ret = mux_init();
+	ret = mux_init();
-	// if (ret != 0) {
+	if (ret != 0) {
-	// 	LOG_ERR("Failed to setup multiplexer: %d", ret);
+		LOG_ERR("Failed to setup multiplexer: %d", ret);
-	// 	return 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);
-		// ret = mux_select_channel(id);
+		if (ret < 0) {
-		// if (ret < 0) {
+			LOG_ERR("Could not set multiplexer (%d)\n", ret);
-		// 	LOG_ERR("Could not set multiplexer (%d)\n", ret);
+			return 0;
-		// 	return 0;
+		}
-		// }
 		
 		ret = adc_read_dt(&adc_mux_spec, &sequence);
 		if (ret < 0) {

servo_controller/src/mux.c

@@ -0,0 +1,44 @@
+#include "mux.h"
+
+#include <zephyr/logging/log.h>
+#include <stdlib.h>
+
+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);
+}

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