subsys/usb/device_next/usbd_desc.c

/*
 * Copyright (c) 2017 PHYTEC Messtechnik GmbH
 * Copyright (c) 2017, 2018 Intel Corporation
 * Copyright (c) 2022 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <string.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/usb/usbd.h>
#include <zephyr/drivers/hwinfo.h>

#include "usbd_desc.h"
#include "usbd_device.h"

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(usbd_desc, CONFIG_USBD_LOG_LEVEL);

/*
 * The last index of the initializer_string without null character is:
 *   ascii_idx_max = bLength / 2 - 2
 * Use this macro to determine the last index of ASCII7 string.
 */
#define USB_BSTRING_ASCII_IDX_MAX(n)	(n / 2 - 2)

/*
 * The last index of the bString is:
 *   utf16le_idx_max = sizeof(initializer_string) * 2 - 2 - 1
 *   utf16le_idx_max = bLength - 2 - 1
 * Use this macro to determine the last index of UTF16LE string.
 */
#define USB_BSTRING_UTF16LE_IDX_MAX(n)	(n - 3)

/**
 * @brief Transform ASCII-7 string descriptor to UTF16-LE
 *
 * This function transforms ASCII-7 string descriptor
 * into a UTF16-LE.
 *
 * @param[in] dn     Pointer to descriptor node
 */
static void usbd_ascii7_to_utf16le(struct usbd_desc_node *const dn)
{
	struct usb_string_descriptor *desc = dn->desc;
	int idx_max = USB_BSTRING_UTF16LE_IDX_MAX(desc->bLength);
	int ascii_idx_max = USB_BSTRING_ASCII_IDX_MAX(desc->bLength);
	uint8_t *buf = (uint8_t *)&desc->bString;

	LOG_DBG("idx_max %d, ascii_idx_max %d, buf %p",
		idx_max, ascii_idx_max, buf);

	for (int i = idx_max; i >= 0; i -= 2) {
		LOG_DBG("char %c : %x, idx %d -> %d",
			buf[ascii_idx_max],
			buf[ascii_idx_max],
			ascii_idx_max, i);
		__ASSERT(buf[ascii_idx_max] > 0x1F && buf[ascii_idx_max] < 0x7F,
			 "Only printable ascii-7 characters are allowed in USB "
			 "string descriptors");
		buf[i] = 0U;
		buf[i - 1] = buf[ascii_idx_max--];
	}

	dn->utf16le = true;
}

/**
 * @brief Get common USB descriptor
 *
 * Get descriptor from internal descriptor list.
 *
 * @param[in] dn     Pointer to descriptor node
 *
 * @return 0 on success, other values on fail.
 */
static int usbd_get_sn_from_hwid(struct usbd_desc_node *const dn)
{
	static const char hex[] = "0123456789ABCDEF";
	struct usb_string_descriptor *desc = dn->desc;
	uint8_t *desc_data = (uint8_t *)&desc->bString;
	uint8_t hwid[16];
	ssize_t hwid_len;
	ssize_t min_len;

	hwid_len = hwinfo_get_device_id(hwid, sizeof(hwid));
	if (hwid_len < 0) {
		if (hwid_len == -ENOSYS) {
			LOG_WRN("hwinfo not implemented");
			return 0;
		}

		return hwid_len;
	}

	min_len = MIN(hwid_len, desc->bLength / 2);
	for (size_t i = 0; i < min_len; i++) {
		desc_data[i * 2] = hex[hwid[i] >> 4];
		desc_data[i * 2 + 1] = hex[hwid[i] & 0xF];
	}

	LOG_HEXDUMP_DBG(&desc->bString, desc->bLength, "SerialNumber");

	return 0;
}

static inline bool desc_type_equal(const struct usbd_desc_node *const a,
				   const struct usbd_desc_node *const b)
{
	const struct usb_desc_header *const head_a = a->desc;
	const struct usb_desc_header *const head_b = b->desc;

	return head_a->bDescriptorType == head_b->bDescriptorType;
}

/*
 * Add descriptor node to the descriptor list in ascending order by index
 * and sorted by bDescriptorType. For the string descriptors, the function
 * does not care about index zero for the language string descriptor,
 * so if it is not added first, the device will be non-compliant.
 */
static int desc_add_and_update_idx(struct usbd_contex *const uds_ctx,
				   struct usbd_desc_node *const new_nd)
{
	struct usbd_desc_node *tmp_nd;

	SYS_DLIST_FOR_EACH_CONTAINER(&uds_ctx->descriptors, tmp_nd, node) {
		struct usbd_desc_node *next_nd;

		if (!desc_type_equal(tmp_nd, new_nd)) {
			continue;
		}

		next_nd = SYS_DLIST_PEEK_NEXT_CONTAINER(&uds_ctx->descriptors,
							tmp_nd,
							node);

		if (next_nd == NULL) {
			/* Last node of the same bDescriptorType or tail */
			new_nd->idx = tmp_nd->idx + 1;
			sys_dlist_append(&uds_ctx->descriptors, &new_nd->node);
			LOG_DBG("Add %u behind %u", new_nd->idx, tmp_nd->idx);

			return 0;
		}

		if (!desc_type_equal(next_nd, new_nd)) {
			/* Last node of the same bDescriptorType */
			new_nd->idx = tmp_nd->idx + 1;
			sys_dlist_insert(&next_nd->node, &new_nd->node);
			LOG_DBG("Add %u before %u", new_nd->idx, next_nd->idx);

			return 0;
		}

		if (tmp_nd->idx != (next_nd->idx - 1)) {
			/* Add between nodes of the same bDescriptorType */
			new_nd->idx = tmp_nd->idx + 1;
			sys_dlist_insert(&next_nd->node, &new_nd->node);
			LOG_DBG("Add %u between %u and %u",
				tmp_nd->idx, next_nd->idx, new_nd->idx);
			return 0;
		}
	}

	/* If there are none of same bDescriptorType, node idx is set to 0. */
	new_nd->idx = 0;
	sys_dlist_append(&uds_ctx->descriptors, &new_nd->node);
	LOG_DBG("Added first descriptor node (usage type %u)", new_nd->utype);

	return 0;
}

void *usbd_get_descriptor(struct usbd_contex *const uds_ctx,
			  const uint8_t type, const uint8_t idx)
{
	struct usbd_desc_node *tmp;
	struct usb_desc_header *dh;

	SYS_DLIST_FOR_EACH_CONTAINER(&uds_ctx->descriptors, tmp, node) {
		dh = tmp->desc;
		if (tmp->idx == idx && dh->bDescriptorType == type) {
			return tmp->desc;
		}
	}

	return NULL;
}

int usbd_desc_remove_all(struct usbd_contex *const uds_ctx)
{
	struct usbd_desc_node *tmp;
	sys_dnode_t *node;

	while ((node = sys_dlist_get(&uds_ctx->descriptors))) {
		tmp = CONTAINER_OF(node, struct usbd_desc_node, node);
		LOG_DBG("Remove descriptor node %p", tmp);
	}

	return 0;
}

int usbd_add_descriptor(struct usbd_contex *const uds_ctx,
			struct usbd_desc_node *const desc_nd)
{
	struct usb_device_descriptor *dev_desc = uds_ctx->desc;
	struct usb_desc_header *head;
	int ret = 0;

	usbd_device_lock(uds_ctx);

	if (dev_desc == NULL || usbd_is_initialized(uds_ctx)) {
		ret = -EPERM;
		goto add_descriptor_error;
	}

	/* Check if descriptor list is initialized */
	if (!sys_dnode_is_linked(&uds_ctx->descriptors)) {
		LOG_DBG("Initialize descriptors list");
		sys_dlist_init(&uds_ctx->descriptors);
	}

	head = desc_nd->desc;
	if (sys_dnode_is_linked(&desc_nd->node)) {
		ret = -EALREADY;
		goto add_descriptor_error;
	}

	ret = desc_add_and_update_idx(uds_ctx, desc_nd);
	if (ret) {
		ret = -EINVAL;
		goto add_descriptor_error;
	}

	if (head->bDescriptorType == USB_DESC_STRING) {
		switch (desc_nd->utype) {
		case USBD_DUT_STRING_LANG:
			break;
		case USBD_DUT_STRING_MANUFACTURER:
			dev_desc->iManufacturer = desc_nd->idx;
			break;
		case USBD_DUT_STRING_PRODUCT:
			dev_desc->iProduct = desc_nd->idx;
			break;
		case USBD_DUT_STRING_SERIAL_NUMBER:
			if (!desc_nd->custom_sn) {
				ret = usbd_get_sn_from_hwid(desc_nd);
				desc_nd->utf16le = false;
			}

			dev_desc->iSerialNumber = desc_nd->idx;
			break;
		default:
			break;
		}

		if (desc_nd->idx && !desc_nd->utf16le) {
			usbd_ascii7_to_utf16le(desc_nd);
		}
	}

add_descriptor_error:
	usbd_device_unlock(uds_ctx);
	return ret;
}