usbd.c

// SPDX-License-Identifier: LGPL-2.1-or-later
/*
 * libiio - Library for interfacing industrial I/O (IIO) devices
 *
 * Copyright (C) 2016 Analog Devices, Inc.
 * Author: Paul Cercueil <paul.cercueil@analog.com>
 */

#include "../debug.h"
#include "ops.h"
#include "thread-pool.h"

#include <fcntl.h>
#include <linux/usb/functionfs.h>
#include <stdint.h>
#include <string.h>

/* u8"IIO" for non-c11 compilers */
#define NAME "\x0049\x0049\x004F"

#define LE32(x) ((__BYTE_ORDER != __BIG_ENDIAN) ? (x) : __bswap_constant_32(x))
#define LE16(x) ((__BYTE_ORDER != __BIG_ENDIAN) ? (x) : __bswap_constant_16(x))

#define IIO_USD_CMD_RESET_PIPES 0
#define IIO_USD_CMD_OPEN_PIPE 1
#define IIO_USD_CMD_CLOSE_PIPE 2


struct usb_ffs_header {
	struct usb_functionfs_descs_head_v2 header;
	uint32_t nb_fs, nb_hs, nb_ss;
} __attribute__((packed));

struct usb_ffs_strings {
	struct usb_functionfs_strings_head head;
	uint16_t lang;
	const char string[sizeof(NAME)];
} __attribute__((packed));

struct usbd_pdata {
	struct iio_context *ctx;
	char *ffs;
	int ep0_fd;
	bool debug, use_aio;
	struct thread_pool **pool;
	unsigned int nb_pipes;

	const void *xml_zstd;
	size_t xml_zstd_len;
};

struct usbd_client_pdata {
	struct usbd_pdata *pdata;
	int ep_in, ep_out;
};

static const struct usb_ffs_strings ffs_strings = {
	.head = {
		.magic = LE32(FUNCTIONFS_STRINGS_MAGIC),
		.length = LE32(sizeof(ffs_strings)),
		.str_count = LE32(1),
		.lang_count = LE32(1),
	},

	.lang = LE16(0x409),
	.string = NAME,
};

static void usbd_client_thread(struct thread_pool *pool, void *d)
{
	struct usbd_client_pdata *pdata = d;

	interpreter(pdata->pdata->ctx, pdata->ep_in, pdata->ep_out,
			pdata->pdata->debug, false, true,
			pdata->pdata->use_aio, pool,
			pdata->pdata->xml_zstd, pdata->pdata->xml_zstd_len);

	close(pdata->ep_in);
	close(pdata->ep_out);
	free(pdata);
}

static int usb_open_pipe(struct usbd_pdata *pdata, unsigned int pipe_id)
{
	struct usbd_client_pdata *cpdata;
	char buf[256];
	int err;

	if (pipe_id >= pdata->nb_pipes)
		return -EINVAL;

	cpdata = malloc(sizeof(*cpdata));
	if (!cpdata)
		return -ENOMEM;

	/* Either we open this pipe for the first time, or it was closed before.
	 * In that case we called thread_pool_stop() without waiting for all the
	 * threads to finish. We do that here. Since the running thread might still
	 * have a open handle to the endpoints make sure that they have exited
	 * before opening the endpoints again. */
	thread_pool_stop_and_wait(pdata->pool[pipe_id]);

	snprintf(buf, sizeof(buf), "%s/ep%u", pdata->ffs, pipe_id * 2 + 1);
	cpdata->ep_out = open(buf, O_WRONLY);
	if (cpdata->ep_out < 0) {
		err = -errno;
		goto err_free_cpdata;
	}

	snprintf(buf, sizeof(buf), "%s/ep%u", pdata->ffs, pipe_id * 2 + 2);
	cpdata->ep_in = open(buf, O_RDONLY);
	if (cpdata->ep_in < 0) {
		err = -errno;
		goto err_close_ep_out;
	}

	cpdata->pdata = pdata;

	err = thread_pool_add_thread(pdata->pool[pipe_id],
			usbd_client_thread, cpdata, "usbd_client_thd");
	if (!err)
		return 0;

	close(cpdata->ep_in);
err_close_ep_out:
	close(cpdata->ep_out);
err_free_cpdata:
	free(cpdata);
	return err;
}

static int usb_close_pipe(struct usbd_pdata *pdata, unsigned int pipe_id)
{
	if (pipe_id >= pdata->nb_pipes)
		return -EINVAL;

	thread_pool_stop(pdata->pool[pipe_id]);
	return 0;
}

static void usb_close_pipes(struct usbd_pdata *pdata)
{
	unsigned int i;

	for (i = 0; i < pdata->nb_pipes; i++)
		usb_close_pipe(pdata, i);
}

static int handle_event(struct usbd_pdata *pdata,
		const struct usb_functionfs_event *event)
{
	int ret = 0;

	if (event->type == FUNCTIONFS_SETUP) {
		const struct usb_ctrlrequest *req = &event->u.setup;

		switch (req->bRequest) {
		case IIO_USD_CMD_RESET_PIPES:
			usb_close_pipes(pdata);
			break;
		case IIO_USD_CMD_OPEN_PIPE:
			ret = usb_open_pipe(pdata, le16toh(req->wValue));
			break;
		case IIO_USD_CMD_CLOSE_PIPE:
			ret = usb_close_pipe(pdata, le16toh(req->wValue));
			break;
		}
	}

	return ret;
}

static void usbd_main(struct thread_pool *pool, void *d)
{
	int stop_fd = thread_pool_get_poll_fd(pool);
	struct usbd_pdata *pdata = d;
	unsigned int i;

	for (;;) {
		struct usb_functionfs_event event;
		struct pollfd pfd[2];
		int ret;

		pfd[0].fd = pdata->ep0_fd;
		pfd[0].events = POLLIN;
		pfd[0].revents = 0;
		pfd[1].fd = stop_fd;
		pfd[1].events = POLLIN;
		pfd[1].revents = 0;

		poll_nointr(pfd, 2);

		if (pfd[1].revents & POLLIN) /* STOP event */
			break;

		if (!(pfd[0].revents & POLLIN)) /* Should never happen. */
			continue;

		ret = read(pdata->ep0_fd, &event, sizeof(event));
		if (ret != sizeof(event)) {
			IIO_WARNING("Short read!\n");
			continue;
		}

		ret = handle_event(pdata, &event);
		if (ret) {
			IIO_ERROR("Unable to handle event: %i\n", ret);
			break;
		}

		/* Clear out the errors on ep0 when we close endpoints */
		ret = read(pdata->ep0_fd, NULL, 0);
	}

	for (i = 0; i < pdata->nb_pipes; i++) {
		thread_pool_stop_and_wait(pdata->pool[i]);
		thread_pool_destroy(pdata->pool[i]);
	}

	free(pdata->ffs);
	free(pdata->pool);
	free(pdata);
}

static struct usb_ffs_header * create_header(
		unsigned int nb_pipes, uint32_t size)
{
	/* Packet sizes for USB high-speed, full-speed, super-speed */
	const unsigned int packet_sizes[3] = { 64, 512, 1024, };
	struct usb_ffs_header *hdr;
	unsigned int i, pipe_id;
	uintptr_t ptr;

	hdr = zalloc(size);
	if (!hdr) {
		errno = ENOMEM;
		return NULL;
	}

	hdr->header.magic = LE32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2);
	hdr->header.length = htole32(size);
	hdr->header.flags = LE32(FUNCTIONFS_HAS_FS_DESC |
			FUNCTIONFS_HAS_HS_DESC |
			FUNCTIONFS_HAS_SS_DESC);

	hdr->nb_fs = htole32(nb_pipes * 2 + 1);
	hdr->nb_hs = htole32(nb_pipes * 2 + 1);
	hdr->nb_ss = htole32(nb_pipes * 4 + 1);

	ptr = ((uintptr_t) hdr) + sizeof(*hdr);

	for (i = 0; i < 3; i++) {
		struct usb_interface_descriptor *desc =
			(struct usb_interface_descriptor *) ptr;
		struct usb_endpoint_descriptor_no_audio *ep;
		struct usb_ss_ep_comp_descriptor *comp;

		desc->bLength = sizeof(*desc);
		desc->bDescriptorType = USB_DT_INTERFACE;
		desc->bNumEndpoints = nb_pipes * 2;
		desc->bInterfaceClass = USB_CLASS_COMM;
		desc->iInterface = 1;

		ep = (struct usb_endpoint_descriptor_no_audio *)
			(ptr + sizeof(*desc));

		for (pipe_id = 0; pipe_id < nb_pipes; pipe_id++) {
			ep->bLength = sizeof(*ep);
			ep->bDescriptorType = USB_DT_ENDPOINT;
			ep->bEndpointAddress = (pipe_id + 1) | USB_DIR_IN;
			ep->bmAttributes = USB_ENDPOINT_XFER_BULK;
			ep->wMaxPacketSize = htole16(packet_sizes[i]);
			ep++;

			if (i == 2) {
				comp = (struct usb_ss_ep_comp_descriptor *) ep;
				comp->bLength = USB_DT_SS_EP_COMP_SIZE;
				comp->bDescriptorType = USB_DT_SS_ENDPOINT_COMP;
				comp++;
				ep = (struct usb_endpoint_descriptor_no_audio *) comp;
			}

			ep->bLength = sizeof(*ep);
			ep->bDescriptorType = USB_DT_ENDPOINT;
			ep->bEndpointAddress = (pipe_id + 1) | USB_DIR_OUT;
			ep->bmAttributes = USB_ENDPOINT_XFER_BULK;
			ep->wMaxPacketSize = htole16(packet_sizes[i]);
			ep++;

			if (i == 2) {
				comp = (struct usb_ss_ep_comp_descriptor *) ep;
				comp->bLength = USB_DT_SS_EP_COMP_SIZE;
				comp->bDescriptorType = USB_DT_SS_ENDPOINT_COMP;
				comp++;
				ep = (struct usb_endpoint_descriptor_no_audio *) comp;
			}
		}

		ptr += sizeof(*desc) + nb_pipes * 2 * sizeof(*ep);
	}

	return hdr;
}

static int write_header(int fd, unsigned int nb_pipes)
{
	uint32_t size = sizeof(struct usb_ffs_header) +
		3 * sizeof(struct usb_interface_descriptor) +
		6 * nb_pipes * sizeof(struct usb_endpoint_descriptor_no_audio) +
		2 * nb_pipes * sizeof(struct usb_ss_ep_comp_descriptor);
	struct usb_ffs_header *hdr;
	int ret;

	hdr = create_header(nb_pipes, size);
	if (!hdr)
		return -errno;

	ret = write(fd, hdr, size);
	free(hdr);
	if (ret < 0)
		return -errno;

	ret = write(fd, &ffs_strings, sizeof(ffs_strings));
	if (ret < 0)
		return -errno;

	return 0;
}

int init_usb_daemon(const char *ffs, unsigned int nb_pipes)
{
	char buf[256];
	int ep0_fd, ret;

	snprintf(buf, sizeof(buf), "%s/ep0", ffs);

	ep0_fd = open(buf, O_RDWR);
	if (ep0_fd < 0) {
		return -errno;
	}

	ret = write_header(ep0_fd, nb_pipes);
	if (ret < 0) {
		close(ep0_fd);
		return ret;
	}

	return ep0_fd;
}

int start_usb_daemon(struct iio_context *ctx, const char *ffs,
		bool debug, bool use_aio, unsigned int nb_pipes,
		int ep0_fd, struct thread_pool *pool,
		const void *xml_zstd, size_t xml_zstd_len)
{
	struct usbd_pdata *pdata;
	unsigned int i;
	int ret;

	pdata = zalloc(sizeof(*pdata));
	if (!pdata)
		return -ENOMEM;

	pdata->nb_pipes = nb_pipes;

	pdata->pool = calloc(nb_pipes, sizeof(*pdata->pool));
	if (!pdata->pool) {
		ret = -ENOMEM;
		goto err_free_pdata;
	}

	pdata->ffs = strdup(ffs);
	if (!pdata->ffs) {
		ret = -ENOMEM;
		goto err_free_pdata_pool;
	}

	for (i = 0; i < nb_pipes; i++) {
		pdata->pool[i] = thread_pool_new();
		if (!pdata->pool[i]) {
			ret = -errno;
			goto err_free_pools;
		}
	}

	pdata->ctx = ctx;
	pdata->debug = debug;
	pdata->use_aio = use_aio;
	pdata->xml_zstd = xml_zstd;
	pdata->xml_zstd_len = xml_zstd_len;
	pdata->ep0_fd = ep0_fd;

	ret = thread_pool_add_thread(pool, usbd_main, pdata, "usbd_main_thd");
	if (!ret)
		return 0;

err_free_pools:
	/* If we get here, usbd_main was not started, so the pools can be
	 * destroyed directly */
	for (i = 0; i < nb_pipes; i++) {
		if (pdata->pool[i])
			thread_pool_destroy(pdata->pool[i]);
	}
	free(pdata->ffs);
err_free_pdata_pool:
	free(pdata->pool);
err_free_pdata:
	free(pdata);
	return ret;
}