First

servo_controller/src/command_handler.c

@@ -0,0 +1,47 @@
+#include "command_handler.h"
+#include "led.h"
+
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
+
+int command_handler(struct command_message_t *msg) {
+	if (msg == NULL) {
+		LOG_ERR("Received NULL message pointer");
+		return -EINVAL;
+	}
+
+	LOG_DBG("Processing command: %d, length: %d", msg->command, msg->length);
+
+	switch (msg->command) {
+		case LED_TOGGLE: {
+			// Toggle LED
+			int led = msg->data[0];
+
+			if (!led) {
+				led0_toggle();
+			}
+			else {
+				led1_toggle();
+			}
+			break;
+		}
+
+		case LED_SET: {
+			// Set LED
+			int led = msg->data[0];
+			int state = msg->data[1];
+
+			led_set(led, state);
+
+			break;
+		}
+
+		default: {
+			LOG_WRN("Unknown command received: %d", msg->command);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}

servo_controller/src/command_handler.h

@@ -0,0 +1,16 @@
+#ifndef COMMAND_HANDLER_H
+#define COMMAND_HANDLER_H
+
+
+#include "command_message.h"
+
+/**
+ * @brief Process received command message
+ * 
+ * @param msg Command message to process
+ * @return 0 on success, negative errno on failure
+ */
+int command_handler(struct command_message_t *msg);
+
+
+#endif // COMMAND_HANDLER_H

servo_controller/src/command_message.c

@@ -0,0 +1,75 @@
+#include "command_message.h"
+
+#include <string.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);
+
+void command_message_init(struct command_message_t *msg) {
+	memset(msg, 0, sizeof(struct command_message_t));
+	msg->prefix = COMMAND_PREFIX;
+	msg->id = COMMAND_ID;
+}
+
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]) {
+	// Ensure length doesn't exceed available space
+	if (length > sizeof(msg->data) - 1) {
+		return;
+	}
+
+	command_message_init(msg);
+	msg->length = length;
+	msg->command = command;
+
+	// Copy the data
+	if (data != NULL) {
+		for (int i = 0; i < msg->length; i++) {
+			msg->data[i] = data[i];
+		}
+	}
+	msg->crc = command_calculate_crc(msg);
+}
+
+uint8_t command_calculate_crc(struct command_message_t *msg) {
+	uint32_t sum = 0;
+	uint8_t crc = 0;
+
+	uint8_t *byte_ptr = (uint8_t *)msg;
+	int loop_length = (sizeof(struct command_message_t) - sizeof(msg->data) + msg->length);
+
+	for (int i = 0; i < loop_length; i++) {
+		if (i == 4) { continue; }
+		sum += byte_ptr[i];
+	}
+
+	crc = 0x100 - (sum & 0xff);
+
+	return crc;
+}
+
+void command_create_ack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_ACK, NULL);
+}
+
+void command_create_nack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_NACK, NULL);
+}
+
+void command_log(struct command_message_t *msg) {
+	if (msg->length > sizeof(msg->data) - 1) {
+		LOG_ERR("Message length too long: %d", msg->length);
+		return;
+	}
+
+	LOG_INF("Prefix: %d\n\r", msg->prefix);
+	LOG_INF("Length: %d\n\r", msg->length);
+	LOG_INF("COMMAND_ID: %d\n\r", msg->id);
+	LOG_INF("Command: %d\n\r", msg->command);
+	LOG_INF("Data:\n\r");
+
+	for (int i = 0; i < msg->length; i++) {
+		LOG_INF("%d", msg->data[i]);
+	}
+
+	LOG_INF("CRC: %d\n\r", msg->crc);
+}

servo_controller/src/command_message.h

@@ -0,0 +1,75 @@
+#ifndef COMMAND_MESSAGE_H
+#define COMMAND_MESSAGE_H
+
+
+#include <stdint.h>
+
+
+#define COMMAND_PREFIX 0x69
+#define COMMAND_ID 0x00
+#define COMMAND_DATA_SIZE 160
+
+typedef enum {
+	COMMAND_ACK,
+	COMMAND_NACK,
+	LED_TOGGLE,
+	LED_SET,
+} commands_e;
+
+struct command_message_t {
+	uint8_t prefix;
+	uint8_t length;
+	uint8_t id;
+	uint8_t command;
+	uint8_t crc;
+	uint8_t data[COMMAND_DATA_SIZE];
+} __attribute__((packed));
+
+/**
+ * @brief Initialize command message to default state
+ * 
+ * @param msg Message to initialize
+ */
+void command_message_init(struct command_message_t *msg);
+
+/**
+ * @brief Create command message with data and CRC
+ * 
+ * @param msg Message to populate
+ * @param length Data length in bytes
+ * @param command Command type
+ * @param data Data payload
+ */
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]);
+
+/**
+ * @brief Calculate CRC for command message
+ * 
+ * @param msg Message to calculate CRC for
+ * @return CRC value
+ */
+uint8_t command_calculate_crc(struct command_message_t *msg);
+
+/**
+ * @brief Create ACK command message
+ * 
+ * @param msg Message to populate
+ */
+void command_create_ack(struct command_message_t *msg);
+
+/**
+ * @brief Create NACK command message
+ * 
+ * @param msg Message to populate
+ */
+void command_create_nack(struct command_message_t *msg);
+
+/**
+ * @brief Print the command with LOG
+ * 
+ * @param msg Message to calculate CRC for
+ */
+void command_log(struct command_message_t *msg);
+
+
+#endif // COMMAND_MESSAGE_H

servo_controller/src/led.c

@@ -0,0 +1,81 @@
+#include "led.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/gpio.h>
+
+LOG_MODULE_REGISTER(led, LOG_LEVEL_INF);
+
+static const struct gpio_dt_spec led0_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
+static const struct gpio_dt_spec led1_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
+static int led_states[2] = {0, 0};
+
+static int pin_init(const struct gpio_dt_spec *dt) {
+	int ret;
+
+	if (!device_is_ready(dt->port)) {
+		LOG_ERR("LED0 GPIO device not ready");
+		return -ENODEV;
+	}
+
+	ret = gpio_pin_configure_dt(dt, GPIO_OUTPUT_INACTIVE);
+	if (ret != 0) {
+		LOG_ERR("Failed to configure LED0 GPIO: %d", ret);
+		return ret;
+	}
+
+	// Turn it off
+	ret = gpio_pin_set_dt(dt, 0);
+	if (ret != 0) {
+		LOG_ERR("Failed to initialize LED");
+		return ret;
+	}
+
+	LOG_INF("LED driver initialized (GPIO %d)", dt->pin);
+
+	return 0;
+}
+
+int led_init() {
+	int ret;
+
+	ret = pin_init(&led0_gpio);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = pin_init(&led1_gpio);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return 0;
+}
+
+int led_set(int led, int state) {
+	int ret;
+	int set_led = led ? 1 : 0;
+	led_states[set_led] = state ? 1 : 0;
+
+	if (!set_led) {
+		ret = gpio_pin_set_dt(&led0_gpio, led_states[set_led]);
+		if (ret != 0) {
+			LOG_ERR("Failed to set LED0: %d", ret);
+		}
+	}
+	else {
+		ret = gpio_pin_set_dt(&led1_gpio, led_states[set_led]);
+		if (ret != 0) {
+			LOG_ERR("Failed to set LED1: %d", ret);
+		}
+	}
+
+	return ret;
+}
+
+int led0_toggle() {
+	return led_set(0, !led_states[0]);
+}
+
+int led1_toggle() {
+	return led_set(1, !led_states[1]);
+}

servo_controller/src/led.h

@@ -0,0 +1,11 @@
+#ifndef LED_H
+#define LED_H
+
+
+int led_init();
+int led_set(int led, int state);
+int led0_toggle();
+int led1_toggle();
+
+
+#endif // LED_H

servo_controller/src/main.c

@@ -0,0 +1,24 @@
+#include "led.h"
+#include "usb.h"
+
+#include <zephyr/logging/log.h>
+LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
+
+
+int main(void) {
+	int ret;
+
+	ret = usb_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable USB");
+		return 0;
+	}
+
+	ret = led_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable LED");
+		return 0;
+	}
+
+	return 0;
+}

servo_controller/src/servo.c

@@ -0,0 +1,37 @@
+#include "servo.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/pwm.h>
+
+LOG_MODULE_REGISTER(servo, LOG_LEVEL_INF);
+
+// ----------- SERVOS -----------
+static const struct pwm_dt_spec servo_pwm_specs[NUM_SERVO_CHANNELS] = {
+	PWM_DT_SPEC_GET(DT_ALIAS(servo1)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo2)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo3)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo4)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo5)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo6)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo7)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo8)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo9)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo10)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo11)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo12)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo13)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo14)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo15)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo16)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo17)),
+	PWM_DT_SPEC_GET(DT_ALIAS(servo18)),
+};
+
+
+static int pin_init(const ) {
+
+}
+
+int servo_init() {
+
+}

servo_controller/src/servo.h

@@ -0,0 +1,13 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+
+
+#define NUM_SERVO_CHANNELS 18
+
+
+
+int servo_init();
+
+
+#endif // SERVO_H

servo_controller/src/usb.c

@@ -0,0 +1,231 @@
+#include "usb.h"
+#include "usb_conf.h"
+#include "command_handler.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/uart.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/ring_buffer.h>
+
+// PICO-SDK
+#include "pico/bootrom.h"
+
+LOG_MODULE_REGISTER(usb, LOG_LEVEL_INF);
+
+// DEVICE
+const struct device *const uart_dev = DEVICE_DT_GET_ONE(zephyr_cdc_acm_uart);
+static struct usbd_context *usb_context;
+
+// THREAD
+static struct k_thread usb_thread_data;
+static k_tid_t usb_thread_id = NULL;
+#define USB_THREAD_STACK_SIZE 2048
+K_THREAD_STACK_DEFINE(usb_thread_stack, USB_THREAD_STACK_SIZE);
+
+// RX BUFFER
+#define RING_BUF_SIZE 255
+static uint8_t ring_buffer[RING_BUF_SIZE];
+static struct ring_buf ringbuf;
+struct k_sem rx_semaphore;
+
+// ACK / NACK messages
+#define RETURN_ACK true
+struct command_message_t ack_msg;
+struct command_message_t nack_msg;
+
+
+static void interrupt_handler(const struct device *dev, void *user_data) {
+	ARG_UNUSED(user_data);
+
+	while (true) {
+		uart_irq_update(dev);
+
+		if (uart_irq_is_pending(dev) <= 0) {
+			break;
+		}
+
+		if (uart_irq_rx_ready(dev)) {
+			int recv_len, rb_len;
+			uint8_t buffer[64];
+			size_t len = MIN(ring_buf_space_get(&ringbuf), sizeof(buffer));
+
+			if (len == 0) {
+				// ring buffer full, drops package(s)
+			    uart_irq_rx_disable(dev);
+				k_sem_give(&rx_semaphore);
+				break;
+			}
+
+			recv_len = uart_fifo_read(dev, buffer, len);
+			if (recv_len < 0) {
+				LOG_ERR("Failed to read UART FIFO");
+				recv_len = 0;
+			};
+
+			rb_len = ring_buf_put(&ringbuf, buffer, recv_len);
+			if (rb_len < recv_len) {
+				LOG_ERR("Drop %u bytes", recv_len - rb_len);
+			}
+
+			k_sem_give(&rx_semaphore);
+		}
+	}
+}
+
+static void usb_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+	struct command_message_t msg;
+	command_message_init(&msg);
+	
+	LOG_INF("USB command processing thread started");
+	
+	while (1) {
+		k_sem_take(&rx_semaphore, K_FOREVER);
+
+		int len;
+
+		// While ring buffer has data
+		do {
+			uint8_t buf_prefix;
+			len = ring_buf_get(&ringbuf, &buf_prefix, 1);
+
+			if (len && (buf_prefix == COMMAND_PREFIX)) {
+				uint8_t buf_header[4];
+				len = ring_buf_get(&ringbuf, buf_header, 4);
+
+				if ((len == 4) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) {
+					msg.length = buf_header[0];
+					msg.command = buf_header[2];
+					msg.crc = buf_header[3];
+
+					if (msg.length) {
+						len = ring_buf_get(&ringbuf, msg.data, msg.length);
+					}
+
+					uint8_t calculated_crc = command_calculate_crc(&msg);
+					if (calculated_crc != msg.crc) {
+						if (RETURN_ACK) {
+							// Send NACK
+							usb_send_command(&nack_msg);
+						}
+						continue;
+					}
+
+					int ret = command_handler(&msg);
+					if (ret == 0) {
+						if (RETURN_ACK) {
+							// Send ACK
+							usb_send_command(&ack_msg);
+						}
+					}
+					else {
+						if (RETURN_ACK) {
+							// Send NACK
+							usb_send_command(&nack_msg);
+						}
+					}
+
+				}
+				else {
+					// Command_id did not match, ignore
+					continue;
+				}
+			}
+			else {
+				// Prefix did not match, ignore
+				continue;
+			}
+		} while (len > 0);
+
+		uart_irq_rx_enable(uart_dev);
+	}
+	
+	LOG_INF("USB command processing thread exiting");
+}
+
+static void usb_msg_cb(struct usbd_context *const ctx, const struct usbd_msg *msg) {
+	if (msg->type == USBD_MSG_CDC_ACM_LINE_CODING) {
+		// Jump to BOOTSEL when baudrate changes to 1200
+		uint32_t baudrate;
+
+		if (uart_line_ctrl_get(msg->dev, UART_LINE_CTRL_BAUD_RATE, &baudrate) == 0) {
+			LOG_INF("Baudrate %u", baudrate);
+
+			if (baudrate == 1200) {
+				LOG_INF("Entering BOOTSEL...");
+				reset_usb_boot(0, 0);
+			}
+		}
+	}
+}
+
+int usb_init() {
+	ring_buf_init(&ringbuf, sizeof(ring_buffer), ring_buffer);
+	k_sem_init(&rx_semaphore, 0, 1);
+
+	command_create_ack(&ack_msg);
+	command_create_nack(&nack_msg);
+
+	int ret;
+
+	if (!device_is_ready(uart_dev)) {
+		LOG_ERR("CDC ACM device not ready");
+		return -ENODEV;
+	}
+
+	usb_context = usb_device_init(usb_msg_cb);
+	if (usb_context == NULL) {
+		LOG_ERR("Failed to initialize USB device");
+		return -ENODEV;
+	}
+
+	if (!usbd_can_detect_vbus(usb_context)) {
+		ret = usbd_enable(usb_context);
+		if (ret) {
+			LOG_ERR("Failed to enable device support");
+			return ret;
+		}
+	}
+	k_msleep(100);
+
+	uart_irq_callback_set(uart_dev, interrupt_handler);
+	uart_irq_rx_enable(uart_dev);
+
+	usb_thread_id = k_thread_create(
+		&usb_thread_data,
+		usb_thread_stack,
+		K_THREAD_STACK_SIZEOF(usb_thread_stack),
+		usb_thread,
+		NULL, NULL, NULL,
+		5,
+		0,
+		K_NO_WAIT
+	);
+
+	if (usb_thread_id == NULL) {
+		LOG_ERR("Failed to create USB thread");
+		return -ENOMEM;
+	}
+
+	return ret;
+}
+
+int usb_send_command(struct command_message_t *msg) {
+	if (!device_is_ready(uart_dev)) {
+		return -ENODEV;
+	}
+
+	// Message size: prefix + length + id + command + crc + data
+	size_t msg_size = 5 + msg->length;
+	uint8_t *msg_bytes = (uint8_t *)msg;
+
+	/* uart_poll_out blocks until sent, ensuring data integrity */
+	for (size_t i = 0; i < msg_size; i++) {
+		uart_poll_out(uart_dev, msg_bytes[i]);
+	}
+
+	return 0;
+}

servo_controller/src/usb.h

@@ -0,0 +1,12 @@
+#ifndef USB_H
+#define USB_H
+
+
+#include "command_message.h"
+
+
+int usb_init();
+int usb_send_command(struct command_message_t *msg);
+
+
+#endif // USB_H

servo_controller/src/usb_conf.c

@@ -0,0 +1,184 @@
+#include "usb_conf.h"
+
+#include <stdint.h>
+#include <zephyr/device.h>
+#include <zephyr/usb/usbd.h>
+#include <zephyr/usb/bos.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(usb_conf, LOG_LEVEL_DBG);
+
+/* By default, do not register the USB DFU class DFU mode instance. */
+static const char *const blocklist[] = {
+	"dfu_dfu",
+	NULL,
+};
+
+/*
+ * Instantiate a context named my_usb_context using the default USB device
+ * controller, the Zephyr project vendor ID, and the sample product ID.
+ * Zephyr project vendor ID must not be used outside of Zephyr samples.
+ */
+USBD_DEVICE_DEFINE(my_usb_context,
+		   DEVICE_DT_GET(DT_NODELABEL(zephyr_udc0)),
+		   CONFIG_USBD_VID, CONFIG_USBD_PID);
+
+USBD_DESC_LANG_DEFINE(my_usb_lang);
+USBD_DESC_MANUFACTURER_DEFINE(my_usb_mfr, CONFIG_USBD_MANUFACTURER);
+USBD_DESC_PRODUCT_DEFINE(my_usb_product, CONFIG_USBD_PRODUCT);
+IF_ENABLED(CONFIG_HWINFO, (USBD_DESC_SERIAL_NUMBER_DEFINE(my_usb_serial)));
+
+USBD_DESC_CONFIG_DEFINE(fs_cfg_desc, "FS Configuration");
+USBD_DESC_CONFIG_DEFINE(hs_cfg_desc, "HS Configuration");
+
+static const uint8_t attributes = (IS_ENABLED(CONFIG_USBD_SELF_POWERED) ?
+				   USB_SCD_SELF_POWERED : 0) |
+				  (IS_ENABLED(CONFIG_USBD_REMOTE_WAKEUP) ?
+				   USB_SCD_REMOTE_WAKEUP : 0);
+
+/* Full speed configuration */
+USBD_CONFIGURATION_DEFINE(sample_fs_config,
+			  attributes,
+			  CONFIG_USBD_MAX_POWER, &fs_cfg_desc);
+
+/* High speed configuration */
+USBD_CONFIGURATION_DEFINE(sample_hs_config,
+			  attributes,
+			  CONFIG_USBD_MAX_POWER, &hs_cfg_desc);
+
+#if CONFIG_SAMPLE_USBD_20_EXTENSION_DESC
+/*
+ * This does not yet provide valuable information, but rather serves as an
+ * example, and will be improved in the future.
+ */
+static const struct usb_bos_capability_lpm bos_cap_lpm = {
+	.bLength = sizeof(struct usb_bos_capability_lpm),
+	.bDescriptorType = USB_DESC_DEVICE_CAPABILITY,
+	.bDevCapabilityType = USB_BOS_CAPABILITY_EXTENSION,
+	.bmAttributes = 0UL,
+};
+
+USBD_DESC_BOS_DEFINE(my_usb_usbext, sizeof(bos_cap_lpm), &bos_cap_lpm);
+#endif
+
+static void usb_fix_code_triple(struct usbd_context *uds_ctx, const enum usbd_speed speed) {
+	/* Always use class code information from Interface Descriptors */
+	if (IS_ENABLED(CONFIG_USBD_CDC_ACM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_CDC_ECM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_CDC_NCM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_MIDI2_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_AUDIO2_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_VIDEO_CLASS)) {
+		/*
+		 * Class with multiple interfaces have an Interface
+		 * Association Descriptor available, use an appropriate triple
+		 * to indicate it.
+		 */
+		usbd_device_set_code_triple(uds_ctx, speed,
+					    USB_BCC_MISCELLANEOUS, 0x02, 0x01);
+	} else {
+		usbd_device_set_code_triple(uds_ctx, speed, 0, 0, 0);
+	}
+}
+
+struct usbd_context *usb_device_setup(usbd_msg_cb_t msg_cb) {
+	int err;
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_lang);
+	if (err) {
+		LOG_ERR("Failed to initialize language descriptor (%d)", err);
+		return NULL;
+	}
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_mfr);
+	if (err) {
+		LOG_ERR("Failed to initialize manufacturer descriptor (%d)", err);
+		return NULL;
+	}
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_product);
+	if (err) {
+		LOG_ERR("Failed to initialize product descriptor (%d)", err);
+		return NULL;
+	}
+
+	IF_ENABLED(CONFIG_HWINFO, (
+		err = usbd_add_descriptor(&my_usb_context, &my_usb_serial);
+	))
+	if (err) {
+		LOG_ERR("Failed to initialize SN descriptor (%d)", err);
+		return NULL;
+	}
+
+	if (USBD_SUPPORTS_HIGH_SPEED &&
+	    usbd_caps_speed(&my_usb_context) == USBD_SPEED_HS) {
+		err = usbd_add_configuration(&my_usb_context, USBD_SPEED_HS,
+					     &sample_hs_config);
+		if (err) {
+			LOG_ERR("Failed to add High-Speed configuration");
+			return NULL;
+		}
+
+		err = usbd_register_all_classes(&my_usb_context, USBD_SPEED_HS, 1,
+						blocklist);
+		if (err) {
+			LOG_ERR("Failed to add register classes");
+			return NULL;
+		}
+
+		usb_fix_code_triple(&my_usb_context, USBD_SPEED_HS);
+	}
+
+	err = usbd_add_configuration(&my_usb_context, USBD_SPEED_FS,
+				     &sample_fs_config);
+	if (err) {
+		LOG_ERR("Failed to add Full-Speed configuration");
+		return NULL;
+	}
+
+	err = usbd_register_all_classes(&my_usb_context, USBD_SPEED_FS, 1, blocklist);
+	if (err) {
+		LOG_ERR("Failed to add register classes");
+		return NULL;
+	}
+
+	usb_fix_code_triple(&my_usb_context, USBD_SPEED_FS);
+	usbd_self_powered(&my_usb_context, attributes & USB_SCD_SELF_POWERED);
+
+	if (msg_cb != NULL) {
+		err = usbd_msg_register_cb(&my_usb_context, msg_cb);
+		if (err) {
+			LOG_ERR("Failed to register message callback");
+			return NULL;
+		}
+	}
+
+#if CONFIG_SAMPLE_USBD_20_EXTENSION_DESC
+	(void)usbd_device_set_bcd_usb(&my_usb_context, USBD_SPEED_FS, 0x0201);
+	(void)usbd_device_set_bcd_usb(&my_usb_context, USBD_SPEED_HS, 0x0201);
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_usbext);
+	if (err) {
+		LOG_ERR("Failed to add USB 2.0 Extension Descriptor");
+		return NULL;
+	}
+#endif
+
+	return &my_usb_context;
+}
+
+struct usbd_context *usb_device_init(usbd_msg_cb_t msg_cb) {
+	int err;
+
+	if (usb_device_setup(msg_cb) == NULL) {
+		return NULL;
+	}
+
+	err = usbd_init(&my_usb_context);
+	if (err) {
+		LOG_ERR("Failed to initialize device support");
+		return NULL;
+	}
+
+	return &my_usb_context;
+}

servo_controller/src/usb_conf.h

@@ -0,0 +1,11 @@
+#ifndef USB_CONF_H
+#define USB_CONF_H
+
+
+#include <zephyr/usb/usbd.h>
+
+struct usbd_context *usb_device_setup(usbd_msg_cb_t msg_cb);
+struct usbd_context *usb_device_init(usbd_msg_cb_t msg_cb);
+
+
+#endif // USB_CONF_H