rp2040-playground/composite/tusb_cardem.c

#include <osmocom/core/linuxlist.h>
#include <osmocom/core/msgb.h>
#include <libsimtrace/usb_buffered_ep.h>
#include <libsimtrace/card_emu.h>

#include "tusb.h"
#include "device/usbd_pvt.h"
#include "shared.h"

/* endpoint indexes so we can use them as look-up into arrays */
enum tusb_cardem_epidx {
	EPIDX_OUT	= 0,
	EPIDX_IN	= 1,
	EPIDX_IRQ	= 2,
	_NUM_EPIDX
};

/* per-interface/instance structure */
struct tusb_cardem_inst {
	/* the interface to which this driver has been bound */
	int itf_num;
	struct usb_buffered_ep bep[_NUM_EPIDX];
	/* handle for card_emu.c core */
	struct card_handle *ch;
};

/* must have at least as many elements as there could be cardem interfaces */
static struct tusb_cardem_inst gtci[MAX_CARDEM_ITF];


/****************************************************************************
 * endpoint_nr -> cardem_inst + ep_idx map (used for dispatch tusb -> cardem)
 ****************************************************************************/

/* must have at least as many elements as there could be USB endpoint numbers */
#define MAX_EP 	16
struct tusb_ep_map {
	struct tusb_cardem_inst *tci;
	uint8_t ep_idx;
};
/* same endpoint number can be used for IN and OUT, so we need two arrays */
static struct tusb_ep_map g_tem_in[MAX_EP];
static struct tusb_ep_map g_tem_out[MAX_EP];

/* resolve the tusb_ep_map entry for a given endpoint address */
static struct tusb_ep_map *tem_for_epaddr(uint8_t epaddr)
{
	struct tusb_ep_map *map;

	/* determine map based on input/output direction */
	if (epaddr & 0x80)
		map = g_tem_in;
	else
		map = g_tem_out;

	/* resolve tusb_ep_map by indexing the array */
	uint8_t epnr = tu_edpt_number(epaddr);
	OSMO_ASSERT(epnr < MAX_EP);

	return &map[epnr];
}

/* return the usb_buffered_ep (+ optionally endpoint idx) for a given endpoint address */
static struct usb_buffered_ep *bep_for_epaddr(uint8_t epaddr, uint8_t *epidx)
{
	struct tusb_ep_map *tem = tem_for_epaddr(epaddr);

	if (epidx)
		*epidx = tem->ep_idx;

	return &tem->tci->bep[tem->ep_idx];
}



/****************************************************************************
 * actual tinyusb driver code
 ****************************************************************************/

/* called during tud_init() time. */
static void cardemd_init(void)
{
	printf("%s\r\n", __func__);
	for (unsigned int i = 0; i < ARRAY_SIZE(gtci); i++) {
		struct tusb_cardem_inst *tci = &gtci[i];
		for (unsigned int j = 0; j < ARRAY_SIZE(tci->bep); j++) {
			/* endpoint numbers are written in cardemd_open(), so we pass 0 here */
			bep_init(&tci->bep[j], 0);
		}
	}
}

/* called during usdbd_reset(); happens during bus reset or USB unplug */
static void cardemd_reset(uint8_t __unused rhport)
{
	printf("%s\r\n", __func__);
	for (unsigned int i = 0; i < ARRAY_SIZE(gtci); i++) {
		gtci[i].itf_num = -1;
		/* TODO: ? */
	}

	for (unsigned int i = 0; i < ARRAY_SIZE(g_tem_in); i++)
		g_tem_in[i].tci = NULL;

	for (unsigned int i = 0; i < ARRAY_SIZE(g_tem_out); i++)
		g_tem_out[i].tci = NULL;

	/* FIXME free all the buffered msgb */
}

/* called during process_set_config() / process_control_request() */
static uint16_t cardemd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
{
	struct tusb_cardem_inst *tci = &gtci[0];	// FIXME: multiple instances

	printf("%s\r\n", __func__);

	TU_VERIFY(TUSB_CLASS_VENDOR_SPECIFIC == itf_desc->bInterfaceClass &&
		  2 == itf_desc->bInterfaceSubClass &&
		  0 == itf_desc->bInterfaceProtocol, 0);

	uint16_t drv_len = sizeof(tusb_desc_interface_t);
	TU_VERIFY(max_len >= drv_len, 0);

	tci->itf_num = itf_desc->bInterfaceNumber;

	uint8_t const *p_desc = tu_desc_next(itf_desc);
	uint8_t const * desc_end = p_desc + max_len;
	if (itf_desc->bNumEndpoints) {
		/* skip non-endpoint descriptors */
		while ((TUSB_DESC_ENDPOINT != tu_desc_type(p_desc)) && (p_desc < desc_end))
			p_desc = tu_desc_next(p_desc);

		/* iterate over endpoints */
		for (int i = 0; i < itf_desc->bNumEndpoints; i++) {
			tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
			TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType);
			/* open each endpoint */
			printf("opening EP 0x%02x\r\n", desc_ep->bEndpointAddress);
			TU_ASSERT(usbd_edpt_open(rhport, desc_ep));

			uint8_t ep_nr = tu_edpt_number(desc_ep->bEndpointAddress);

			if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
				g_tem_in[ep_nr].tci = tci;
				if (TUSB_XFER_INTERRUPT == desc_ep->bmAttributes.xfer) {
					g_tem_in[ep_nr].ep_idx = EPIDX_IRQ;
					tci->bep[EPIDX_IRQ].ep = desc_ep->bEndpointAddress;
					printf("EPIDX_IRQ = 0x%02x\r\n", tci->bep[EPIDX_IRQ].ep);
				} else {
					g_tem_in[ep_nr].ep_idx = EPIDX_IN;
					tci->bep[EPIDX_IN].ep = desc_ep->bEndpointAddress;
					printf("EPIDX_IN  = 0x%02x\r\n", tci->bep[EPIDX_IN].ep);
				}
				/* FIXME: tud_vendor_n_write_flush() for each IN EP */
			} else {
				g_tem_out[ep_nr].tci = tci;
				g_tem_out[ep_nr].ep_idx = EPIDX_OUT;
				tci->bep[EPIDX_OUT].ep = desc_ep->bEndpointAddress;
				printf("EPIDX_OUT = 0x%02x\r\n", tci->bep[EPIDX_OUT].ep);

				/* enqueue a buffer for receiving data from OUT ep */
				struct msgb *msg = bep_msgb_alloc(&tci->bep[EPIDX_OUT]);
				if (msg) {
					bep_enqueue_msgb(msg);
					bep_refill_to_host(&tci->bep[EPIDX_OUT], true);
				}
			}

			p_desc = tu_desc_next(p_desc);
			drv_len += sizeof(tusb_desc_endpoint_t);
		}
	}

	tci->ch = card_emu_init(tci->itf_num, 0 /*fixme*/, &tci->bep[EPIDX_IN], &tci->bep[EPIDX_IRQ],
				false, false, false);
	card_emu_drv_init(tci->ch, 0 /* fixme */);


	return drv_len;
}

extern void dcd_edpt_reset_data_toggle(uint8_t rhport, uint8_t ep_addr);
extern void dcd_edpt_close_all (uint8_t rhport);

/* inbound control request from host */
static bool cardemd_control_xfer_cb(uint8_t __unused rhport, uint8_t __unused stage, tusb_control_request_t const *request)
{
	switch (request->bmRequestType_bit.type) {
	case TUSB_REQ_TYPE_STANDARD:
		switch (request->bRequest) {
		case TUSB_REQ_SET_INTERFACE:
			printf("SET_INTERFACE Stage: %u\r\n", stage);
			switch (stage) {
			case CONTROL_STAGE_SETUP:
				tud_control_status(rhport, request);
				return true;
			case CONTROL_STAGE_ACK:
				if (request->wIndex != 0) {
					// return RequestError
					return false;
				}
				if (request->wValue != 0) {
					// return RequestError
					return false;
				}
				/* from here: permitted alternate setting */
#if 0	/* global disable of any SET_INTERFACE */
				/* reset data toggle to DATA0  after configuration operation */
				printf("SET_INTERFACE: RESTTING DATA TOGGLE TO DATA0\r\n");
#if 0	/* try it via reset of data toggle */
				dcd_edpt_reset_data_toggle(0, 0x01);
				dcd_edpt_reset_data_toggle(0, 0x81);
				dcd_edpt_reset_data_toggle(0, 0x82);
#else	/* try it via closing of endpoints */
				dcd_edpt_close_all(rhport);
#endif
				return true;
#else	/* global disable of any SET_INTERFACE */
				/* return false here so that the calling function will cause a
				 * STALL during the ACK (Status) phase of the control transfer,
				 * making the entire request fail */
				return false;
#endif
			default:
				return true;
			}
		case TUSB_REQ_GET_INTERFACE:
			/* let generic tinyusb usbd care about it */
			return false;
		default:
			/* let generic tinyusb usbd care about it */
			return false;
		}
		break;
	default:
		/* we have no class specific control requests */
		return false;
	}
}

#include <libsimtrace/simtrace_prot.h>
static const struct simtrace_board_info g_board_info = {
	.hardware = {
		.manufacturer = "sysmocom",
		.model = "rp2040-cardem-breakout",
		.version = "0.1",
	},
	.software = {
		.provider = "sysmocom",
		.name = "rp2040-cardem-composite",
		.version = "FIXME",
		.buildhost = "FIXME",
		.crc = 0, // FIXME
	},
	.speed = {
		.max_baud_rate = 9600,
	},
	.cap_generic_bytes = 0, // FIXME
};

/* push (prepend) the generic simtrace_msg_hdr in front of msg and enqueue it for tx to USB host */
static int stp_push_hdr_and_send(struct msgb *msg, uint8_t msg_class, uint8_t msg_type, uint8_t seq_nr, uint8_t slot_nr)
{
	struct simtrace_msg_hdr *sth = (struct simtrace_msg_hdr *) msgb_push(msg, sizeof(*sth));
	struct usb_buffered_ep *bep = msg->dst;
	int rc;

	sth->msg_class = msg_class;
	sth->msg_type = msg_type;
	sth->seq_nr = seq_nr;
	sth->slot_nr = slot_nr;
	sth->msg_len = msgb_length(msg);

	rc = bep_enqueue_msgb(msg);
	if (rc >= 0)
		bep_refill_to_host(bep, false);
	return rc;
}

static void stp_tx_error(struct usb_buffered_ep *bep, uint8_t seq_nr, uint8_t slot_nr, uint8_t severity,
			 uint8_t subsystem, uint16_t code, const char *errmsg)
{
	struct msgb *msg = bep_msgb_alloc(bep);
	struct cardemu_usb_msg_error *err;

	LOGBEP(bep, "(slot_nr=%u, severity=%u, subsys=%u, code=0x%04x, msg=%s)\r\n",
		slot_nr, severity, subsystem, code, errmsg);

	if (!msg)
		return;

	err = (struct cardemu_usb_msg_error *) msgb_put(msg, sizeof(*err));
	err->severity = severity;
	err->subsystem = subsystem;
	err->code = code;
	err->msg_len = strlen(errmsg);
	if (err->msg_len) {
		uint8_t *out = msgb_put(msg, err->msg_len);
		memcpy(out, errmsg, err->msg_len);
	}

	stp_push_hdr_and_send(msg, SIMTRACE_MSGC_GENERIC, SIMTRACE_CMD_DO_ERROR, seq_nr, slot_nr);

}

/* handle SIMTRACE_MSGC_GENERIC received from USB host */
static int my_stp_generic_handler(struct tusb_cardem_inst *tci, const struct simtrace_msg_hdr *smh, struct usb_buffered_ep *bep)
{
	struct simtrace_board_info *bdinfo;
	struct msgb *msg;

	switch (smh->msg_type) {
	case SIMTRACE_CMD_BD_BOARD_INFO:
		LOGBEP(bep, "responding to BOARD_INFO\r\n");
		msg = bep_msgb_alloc(&tci->bep[EPIDX_IN]);
		if (!msg)
			break;
		bdinfo = (struct simtrace_board_info *) msgb_put(msg, sizeof(*bdinfo));
		memcpy(bdinfo, &g_board_info, sizeof(g_board_info));
		stp_push_hdr_and_send(msg, SIMTRACE_MSGC_GENERIC, SIMTRACE_CMD_BD_BOARD_INFO,
				      smh->seq_nr, smh->slot_nr);
		break;
	default:
		stp_tx_error(&tci->bep[EPIDX_IN], smh->seq_nr, smh->slot_nr, 1, 1, 0x0003, "unknown message type");
		return -1;
	}

	return 0;
}

/* handler function for CARDEM class simtrace protocol messages arriving on OUT endpoint */
int stp_cardem_handler(struct tusb_cardem_inst *tci, const struct simtrace_msg_hdr *smh, struct msgb *msg)
{
	struct cardemu_usb_msg_set_atr *atr;
	//struct cardemu_usb_msg_cardinsert *cardins;
	struct cardemu_usb_msg_config *cfg;
	struct llist_head *queue;

	int rc;

	switch (smh->msg_type) {
	case SIMTRACE_MSGT_DT_CEMU_TX_DATA:
		queue = card_emu_get_uart_tx_queue(tci->ch);
		llist_add_tail(&msg->list, queue);
		card_emu_have_new_uart_tx(tci->ch);
		return 1; /* tell caller to not free the msgb! */
	case SIMTRACE_MSGT_DT_CEMU_SET_ATR:
		atr = (struct cardemu_usb_msg_set_atr *) msg->l2h;
		if (msgb_l2len(msg) < sizeof(*atr))
			rc = -1;
		else if (msgb_l2len(msg) < sizeof(*atr) + atr->atr_len)
			rc = -1;
		else
			rc = card_emu_set_atr(tci->ch, atr->atr, atr->atr_len);
		break;
	case SIMTRACE_MSGT_BD_CEMU_STATUS:
		rc = card_emu_report_status(tci->ch, false);
		break;
	case SIMTRACE_MSGT_BD_CEMU_CONFIG:
		cfg = (struct cardemu_usb_msg_config *) msg->l2h;
		rc = card_emu_set_config(tci->ch, cfg, msgb_l2len(msg));
		break;
	case SIMTRACE_MSGT_BD_CEMU_STATS:
	case SIMTRACE_MSGT_DT_CEMU_CARDINSERT:
		stp_tx_error(&tci->bep[EPIDX_IN], smh->seq_nr, smh->slot_nr, 1, 2, 0x0005, "unsupported message type");
		break;
	default:
		stp_tx_error(&tci->bep[EPIDX_IN], smh->seq_nr, smh->slot_nr, 1, 2, 0x0003, "unknown message type");
		return -1;
	}

	if (rc)
		stp_tx_error(&tci->bep[EPIDX_IN], smh->seq_nr, smh->slot_nr, 1, 2, 0x0006, "error processing request");

	return 0;
}

/* handler function for simtrace protocol messages arriving on OUT endpoint */
static void my_ep_out_handler(struct tusb_cardem_inst *tci, struct msgb *msg, struct usb_buffered_ep *bep)
{
	struct simtrace_msg_hdr *smh;
	int rc = 0;

	if (msgb_length(msg) < sizeof(*smh)) {
		stp_tx_error(bep, 0, 0 /*slot_nr*/, 1, 1, 0x0001, "short message header");
		goto out;
	}

	smh = (struct simtrace_msg_hdr *) msg->data;

	if (msgb_length(msg) < smh->msg_len) {
		stp_tx_error(bep, smh->seq_nr, smh->slot_nr, 1, 1, 0x0002, "short message");
		goto out;
	}
	msg->l2h = msg->data + sizeof(*smh);

	LOGBEP(bep, "(slot_nr=%u, class=%u, msg_type=%u, seq_nr=%u, len=%u)\r\n",
		smh->slot_nr, smh->msg_class, smh->msg_type, smh->seq_nr, smh->msg_len);

	switch (smh->msg_class) {
	case SIMTRACE_MSGC_GENERIC:
		rc = my_stp_generic_handler(tci, smh, bep);
		break;
	case SIMTRACE_MSGC_CARDEM:
		rc = stp_cardem_handler(tci, smh, msg);
		break;
	case SIMTRACE_MSGC_MODEM:
	case SIMTRACE_MSGC_SNIFF:
	default:
		/* send error in return */
		stp_tx_error(&tci->bep[EPIDX_IN], smh->seq_nr, smh->slot_nr, 1, 0, 0x0004, "unknown message class");
		goto out;
	}

out:
	if (rc != 1)
		bep_msgb_free(msg);
}

/* USB transfer has completed. */
static bool cardemd_xfer_cb(uint8_t __unused rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
{
	struct msgb *msg;
	uint8_t epidx;
	struct tusb_cardem_inst *tci = &gtci[0];	// FIXME: multiple instances
	printf("%s(ep=0x%02x, result=%u, bytes=%u\r\n", __func__, ep_addr, result, xferred_bytes);

	struct usb_buffered_ep *bep = bep_for_epaddr(ep_addr, &epidx);
	if (!bep)
		return false;

	switch (epidx) {
	case EPIDX_OUT:
		/* 1) resolve last submitted buffer + dispatch it to handler */
		msg = bep_get_completed(bep);
		/* update our msgb structure with amount of bytes received */
		msgb_put(msg, xferred_bytes);
		/* dispatch to handler; transfers ownership */
		my_ep_out_handler(tci, msg, bep);
		/* 2) allocate a new buffer */
		msg = bep_msgb_alloc(bep);
		if (!msg)
			break;
		bep_enqueue_msgb(msg);
		/* 3) submit that new buffer */
		bep_refill_to_host(bep, true);
		break;
	case EPIDX_IN:
		/* resolve last submitted buffer + release it back to allocator/pool */
		bep_complete_in_progress(bep);
		/* submit next pending buffer, if any */
		bep_refill_to_host(bep, false);
		break;
	case EPIDX_IRQ:
		/* resolve last submitted buffer + release it back to allocator/pool */
		bep_complete_in_progress(bep);
		/* submit next pending buffer, if any */
		bep_refill_to_host(bep, false);
		break;
	default:
		OSMO_ASSERT(0);
	}

	return true;
}

static usbd_class_driver_t const _cardemd_driver = {
#if CFG_TUSB_DEBUG >= 2
	.name = "cardem",
#endif
	.init             = cardemd_init,
	.reset            = cardemd_reset,
	.open             = cardemd_open,
	.control_xfer_cb  = cardemd_control_xfer_cb,
	.xfer_cb          = cardemd_xfer_cb,
	.sof              = NULL
};

/* Implement callback to add our custom driver. Called during tud_init() */
usbd_class_driver_t const *usbd_app_driver_get_cb(uint8_t *driver_count)
{
	*driver_count = 1;
	return &_cardemd_driver;
}

/* notes:
 *  - endpoint busy condition can be checked via usbd_edpt_busy()
 *  - IN/IRQ transfers to the host are initiated with usbd_edpt_xfer()
 *  	- transfer completion signaled via xfer_cb()
 *  - OUT transfers to the host are initiated via usbd_edpt_xfer()
 *  	- transfer completion signaled via xfer_cb()
 *  	- memory used is allocated 'out of band'
 *
 *  - usbd_edpt_claim() / usbd_edpt_release() called around usbd_edpt_xfer()
 *  	- claim blocks the endpoint from other users
 *  	- if usbd_edpt_xfer() is successful, *DO NOT* release it
 *  	- tusb itself releases it before calling xfer_cb()
 */

/* = do we have to have a write-queue of buffers to the host?
 *
 * IN EP:
 * - traffic-carrying messages triggered by ISO7816
 *	- RX_DATA: waits for answer from host; only one in flight
 *	- PTS: waits fro answer from host; only one in flight
 *
 * - responses to host requests:
 * 	- STATS
 * 	- STATUS
 * 	- CONFIG
 *   either of those might happen in response to a host request, in parallel
 *   to ongoing DATA/PTS traffic. Only one of them at a time, as answers are immediate
 *
 * => write queue is needed, but usually will have only one entry backlog (beyond the
 *    current IN transfer)
 *
 * IRQ IN EP:
 * - we probably want to have a queue of depth of at least 1 (beyond ongoing IN xfer)
 * - queue depth should be limited to avoid noisy GPIO changes from causing OOM
 *
 * = execution context of call-backs (process? IRQ?)
 *
 * It seems that it's up to the application to regularly/repeatedly call tud_task()
 * and we'll be doing that from normal process context.
 *
 */

/* General High Level Flow
 *
 * = xfer_cb() for USB IN EP:
 *
 *   - process any generic requests in-line (no alloc+memcpy)
 *   - process any CARDEM STATS/STATUS/CONFIG requests in-line (no malloc+memcpy)
 *   - process SET_ATR in-line (no malloc+memcpy)
 *   - allocate+memcpy for TX_DATA, put into queue
 *
 * All responses are dynamically allocated + enqueued!
 *
 * = xfer_cb() for USB OUT EP:
 *
 *   - resolve msgb from endpoint / pointer (completion should happen in-order for each EP!)
 *   - release buffer back to pool
 *
 */