WIP: firmware: sniffer: inbound USB handling

Change-Id: Ic953148600d82285281abcb573a07e2e9b8082a7

firmware/apps/cardem/main.c

@@ -220,6 +220,7 @@ extern int main(void)
 			}
 			last_simtrace_config = simtrace_config;
 		} else {
+			//FIXME: usb_proces() for every interface in this configuration?
 			if (config_func_ptrs[simtrace_config].run) {
 				config_func_ptrs[simtrace_config].run();
 			}

firmware/atmel_softpack_libraries/libchip_sam3s/source/USBD_HAL.c

@@ -1672,6 +1672,10 @@ uint8_t USBD_HAL_Halt(uint8_t bEndpoint, uint8_t ctl)
             UDP->UDP_RST_EP |= 1 << bEndpoint;
             UDP->UDP_RST_EP &= ~(1 << bEndpoint);
         }
+
+        /* This fixes a weird bug with regard to ping-pong OUT endpoints */
+        UDP->UDP_RST_EP |= 1 << bEndpoint;
+        UDP->UDP_RST_EP &= ~(1 << bEndpoint);
     }
 
     /* Return Halt status */

firmware/libcommon/include/usb_buf.h

@@ -42,5 +42,15 @@ int usb_drain_queue(uint8_t ep);
 void usb_buf_init(void);
 struct usb_buffered_ep *usb_get_buf_ep(uint8_t ep);
 
-int usb_refill_to_host(uint8_t ep);
-int usb_refill_from_host(uint8_t ep);
+struct usb_if {
+	uint8_t if_num;		/* interface number */
+	uint8_t ep_out;		/* OUT endpoint (0 if none) */
+	uint8_t ep_in;		/* IN endpint (0 if none) */
+	uint8_t ep_int;		/* INT endpoint (0 if none) */
+	void *data;		/* opaque data, passed through */
+	struct {
+		/* call-back to be called for inclming messages on OUT EP */
+		void (*rx_out)(struct msgb *msg, const struct usb_if *usb_if);
+	} ops;
+};
+void usb_process(const struct usb_if *usb_if);

firmware/libcommon/source/host_communication.c

@@ -57,7 +57,7 @@ static void usb_write_cb(uint8_t *arg, uint8_t status, uint32_t transferred,
 }
 
 /* check if the spcified IN endpoint is idle and submit the next buffer from queue */
-int usb_refill_to_host(uint8_t ep)
+static int usb_refill_to_host(uint8_t ep)
 {
 	struct usb_buffered_ep *bep = usb_get_buf_ep(ep);
 	struct msgb *msg;
@@ -130,7 +130,7 @@ static void usb_read_cb(uint8_t *arg, uint8_t status, uint32_t transferred,
 }
 
 /* refill the read queue for data received from host PC on OUT EP, if needed */
-int usb_refill_from_host(uint8_t ep)
+static int usb_refill_from_host(uint8_t ep)
 {
 	struct usb_buffered_ep *bep = usb_get_buf_ep(ep);
 	struct msgb *msg;
@@ -198,3 +198,45 @@ int usb_drain_queue(uint8_t ep)
 
 	return ret;
 }
+
+
+
+/* iterate over the queue of incoming USB commands and dispatch/execute
+ * them */
+static void process_any_usb_commands(const struct usb_if *usb_if)
+{
+	struct llist_head *queue = usb_get_queue(usb_if->ep_out);
+	struct llist_head *lh;
+	struct msgb *msg;
+	int i;
+
+	/* limit the number of iterations to 10, to ensure we don't get
+	 * stuck here without returning to main loop processing */
+	for (i = 0; i < 10; i++) {
+		/* de-queue the list head in an irq-safe way */
+		lh = llist_head_dequeue_irqsafe(queue);
+		if (!lh)
+			break;
+		msg = llist_entry(lh, struct msgb, list);
+		usb_if->ops.rx_out(msg, usb_if);
+	}
+}
+
+/* perform any action related to USB processing (IRQ/INT/OUT EP refill, handling OUT) */
+void usb_process(const struct usb_if *usb_if)
+{
+	/* first try to send any pending messages on IRQ */
+	if (usb_if->ep_int)
+		usb_refill_to_host(usb_if->ep_int);
+
+	/* then try to send any pending messages on IN */
+	if (usb_if->ep_in)
+		usb_refill_to_host(usb_if->ep_in);
+
+	/* ensure we can handle incoming USB messages from the
+	 * host */
+	if (usb_if->ep_out) {
+		usb_refill_from_host(usb_if->ep_out);
+		process_any_usb_commands(usb_if);
+	}
+}

firmware/libcommon/source/mode_cardemu.c

@@ -34,6 +34,8 @@
 
 #define TRACE_ENTRY()	TRACE_DEBUG("%s entering\r\n", __func__)
 
+static void dispatch_received_usb_msg(struct msgb *msg, const struct usb_if *usb_if);
+
 #ifdef PINS_CARDSIM
 static const Pin pins_cardsim[] = PINS_CARDSIM;
 #endif
@@ -56,9 +58,7 @@ struct cardem_inst {
 	struct ringbuf rb;
 	struct Usart_info usart_info;
 	int usb_pending_old;
-	uint8_t ep_out;
-	uint8_t ep_in;
-	uint8_t ep_int;
+	struct usb_if usb_if;
 	const Pin pin_insert;
 #ifdef DETECT_VCC_BY_ADC
 	uint32_t vcc_uv;
@@ -77,9 +77,16 @@ struct cardem_inst cardem_inst[] = {
 			.id = ID_USART1,
 			.state = USART_RCV
 		},
-		.ep_out = SIMTRACE_CARDEM_USB_EP_USIM1_DATAOUT,
-		.ep_in = SIMTRACE_CARDEM_USB_EP_USIM1_DATAIN,
-		.ep_int = SIMTRACE_CARDEM_USB_EP_USIM1_INT,
+		.usb_if = {
+			.if_num = 0,
+			.ep_out = SIMTRACE_CARDEM_USB_EP_USIM1_DATAOUT,
+			.ep_in = SIMTRACE_CARDEM_USB_EP_USIM1_DATAIN,
+			.ep_int = SIMTRACE_CARDEM_USB_EP_USIM1_INT,
+			.data = &cardem_inst[0],
+			.ops = {
+				.rx_out = dispatch_received_usb_msg,
+			},
+		},
 #ifdef PIN_SET_USIM1_PRES
 		.pin_insert = PIN_SET_USIM1_PRES,
 #endif
@@ -92,9 +99,16 @@ struct cardem_inst cardem_inst[] = {
 			.id = ID_USART0,
 			.state = USART_RCV
 		},
-		.ep_out = SIMTRACE_CARDEM_USB_EP_USIM2_DATAOUT,
-		.ep_in = SIMTRACE_CARDEM_USB_EP_USIM2_DATAIN,
-		.ep_int = SIMTRACE_CARDEM_USB_EP_USIM2_INT,
+		.usb_if = {
+			.if_num = 1,
+			.ep_out = SIMTRACE_CARDEM_USB_EP_USIM2_DATAOUT,
+			.ep_in = SIMTRACE_CARDEM_USB_EP_USIM2_DATAIN,
+			.ep_int = SIMTRACE_CARDEM_USB_EP_USIM2_INT,
+			.data = &cardem_inst[1],
+			.ops = {
+				.rx_out = dispatch_received_usb_msg,
+			},
+		}
 #ifdef PIN_SET_USIM2_PRES
 		.pin_insert = PIN_SET_USIM2_PRES,
 #endif
@@ -621,8 +635,9 @@ static void dispatch_usb_command_modem(struct msgb *msg, struct cardem_inst *ci)
 }
 
 /* handle a single USB command as received from the USB host */
-static void dispatch_usb_command(struct msgb *msg, struct cardem_inst *ci)
+static void dispatch_usb_command(struct msgb *msg, const struct usb_if *usb_if)
 {
+	struct cardem_inst *ci = usb_if->data;
 	struct simtrace_msg_hdr *sh = (struct simtrace_msg_hdr *) msg->l1h;
 
 	if (msgb_length(msg) < sizeof(*sh)) {
@@ -651,7 +666,8 @@ static void dispatch_usb_command(struct msgb *msg, struct cardem_inst *ci)
 	}
 }
 
-static void dispatch_received_msg(struct msgb *msg, struct cardem_inst *ci)
+/* handle a single USB transfer as received from the USB host */
+static void dispatch_received_usb_msg(struct msgb *msg, const struct usb_if *usb_if)
 {
 	struct msgb *segm;
 	struct simtrace_msg_hdr *mh;
@@ -662,7 +678,7 @@ static void dispatch_received_msg(struct msgb *msg, struct cardem_inst *ci)
 	mh = (struct simtrace_msg_hdr *) msg->data;
 	if (mh->msg_len == msgb_length(msg)) {
 		/* fast path: only one message in buffer */
-		dispatch_usb_command(msg, ci);
+		dispatch_usb_command(msg, usb_if);
 		return;
 	}
 
@@ -671,23 +687,23 @@ static void dispatch_received_msg(struct msgb *msg, struct cardem_inst *ci)
 	while (1) {
 		mh = (struct simtrace_msg_hdr *) msg->data;
 
-		segm = usb_buf_alloc(ci->ep_out);
+		segm = usb_buf_alloc(usb_if->ep_out);
 		if (!segm) {
 			TRACE_ERROR("%u: ENOMEM during msg segmentation\r\n",
-				    ci->num);
+				    usb_if->if_num);
 			break;
 		}
 
 		if (mh->msg_len > msgb_length(msg)) {
 			TRACE_ERROR("%u: Unexpected large message (%u bytes)\r\n",
-					ci->num, mh->msg_len);
+					usb_if->if_num, mh->msg_len);
 			usb_buf_free(segm);
 			break;
 		} else {
 			uint8_t *cur = msgb_put(segm, mh->msg_len);
 			segm->l1h = segm->head;
 			memcpy(cur, mh, mh->msg_len);
-			dispatch_usb_command(segm, ci);
+			dispatch_usb_command(segm, usb_if);
 		}
 		/* pull this message */
 		msgb_pull(msg, mh->msg_len);
@@ -699,35 +715,14 @@ static void dispatch_received_msg(struct msgb *msg, struct cardem_inst *ci)
 	usb_buf_free(msg);
 }
 
-/* iterate over the queue of incoming USB commands and dispatch/execute
- * them */
-static void process_any_usb_commands(struct llist_head *main_q,
-				     struct cardem_inst *ci)
-{
-	struct llist_head *lh;
-	struct msgb *msg;
-	int i;
-
-	/* limit the number of iterations to 10, to ensure we don't get
-	 * stuck here without returning to main loop processing */
-	for (i = 0; i < 10; i++) {
-		/* de-queue the list head in an irq-safe way */
-		lh = llist_head_dequeue_irqsafe(main_q);
-		if (!lh)
-			break;
-		msg = llist_entry(lh, struct msgb, list);
-		dispatch_received_msg(msg, ci);
-	}
-}
-
 /* main loop function, called repeatedly */
 void mode_cardemu_run(void)
 {
-	struct llist_head *queue;
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(cardem_inst); i++) {
 		struct cardem_inst *ci = &cardem_inst[i];
+		struct usb_if *usb_if = &ci->usb_if;
 
 		/* drain the ring buffer from UART into card_emu */
 		while (1) {
@@ -744,16 +739,6 @@ void mode_cardemu_run(void)
 
 		process_io_statechg(ci);
 
-		/* first try to send any pending messages on IRQ */
-		usb_refill_to_host(ci->ep_int);
-
-		/* then try to send any pending messages on IN */
-		usb_refill_to_host(ci->ep_in);
-
-		/* ensure we can handle incoming USB messages from the
-		 * host */
-		usb_refill_from_host(ci->ep_out);
-		queue = usb_get_queue(ci->ep_out);
-		process_any_usb_commands(queue, ci);
+		usb_process(&ci->usb_if);
 	}
 }

firmware/libcommon/source/sniffer.c

@@ -974,20 +974,42 @@ static void usb_send_change(uint32_t flags)
 	usb_msg_upd_len_and_submit(usb_msg);
 }
 
+/* handle incoming message from USB OUT EP */
+static void dispatch_usb_out(struct msgb *msg, const struct usb_if *usb_if)
+{
+	struct simtrace_msg_hdr *sh = (Struct simtrace_msg_hdr *) msg->l1h;
+
+	if (msg_length(msg) < sizeof(*sh)) {
+		usb_buf_free(msg);
+		return;
+	}
+	msg->l2h = msg->l1h + sizeof(*sh);
+
+	switch (sh->msg_class) {
+	case SIMTRACE_MSGC_GENERIC:
+		break;
+	default:
+		break;
+	}
+
+	usb_buf_free(msg);
+}
+
+static const struct usb_if sniffer_usb_if = {
+	.if_num = 0,
+	.ep_in = SIMTRACE_USB_EP_CARD_DATAIN,
+	.ep_int = SIMTRACE_USB_EP_CARD_INT,
+	.ep_out = SIMTRACE_USB_EP_CARD_DATAOUT,
+	.ops = {
+		.rx_out = dispatch_usb_out,
+	}
+};
+
 /* Main (idle/busy) loop of this USB configuration */
 void Sniffer_run(void)
 {
 	/* Handle USB queue */
-	/* first try to send any pending messages on INT */
-	usb_refill_to_host(SIMTRACE_USB_EP_CARD_INT);
-	/* then try to send any pending messages on IN */
-	usb_refill_to_host(SIMTRACE_USB_EP_CARD_DATAIN);
-	/* ensure we can handle incoming USB messages from the host */
-	/* currently we don't need any incoming data
-	usb_refill_from_host(SIMTRACE_USB_EP_CARD_DATAOUT);
-	struct llist_head *queue = usb_get_queue(SIMTRACE_USB_EP_CARD_DATAOUT);
-	process_any_usb_commands(queue);
-	*/
+	usb_process(&sniffer_usb_if);
 
 	/* WARNING: the signal data and flags are not synchronized. We have to hope 
 	 * the processing is fast enough to not land in the wrong state while data

host/include/osmocom/simtrace2/simtrace2_api.h

@@ -12,6 +12,8 @@ struct osmo_st2_transport {
 		uint8_t out;
 		uint8_t irq_in;
 	} usb_ep;
+	/* use non-blocking / asynchronous libusb I/O */
+	bool usb_async;
 
 	/* UDP */
 	int udp_fd;
@@ -39,8 +41,6 @@ struct osmo_st2_cardem_inst {
 	void *priv;
 };
 
-int osmo_st2_transp_tx_msg(struct osmo_st2_transport *transp, struct msgb *msg);
-
 int osmo_st2_slot_tx_msg(struct osmo_st2_slot *slot, struct msgb *msg,
                          uint8_t msg_class, uint8_t msg_type);
 

host/lib/simtrace2_api.c

@@ -57,23 +57,60 @@ static struct msgb *st_msgb_alloc(void)
 	return msgb_alloc_headroom(1024+32, 32, "SIMtrace");
 }
 
-/*! \brief Transmit a given command to the SIMtrace2 device */
-int osmo_st2_transp_tx_msg(struct osmo_st2_transport *transp, struct msgb *msg)
+
+static void usb_out_xfer_cb(struct libusb_transfer *xfer)
 {
-	int rc;
+	struct msgb *msg = xfer->user_data;
 
-	printf("<- %s\n", msgb_hexdump(msg));
-
-	if (transp->udp_fd < 0) {
-		int xfer_len;
-
-		rc = libusb_bulk_transfer(transp->usb_devh, transp->usb_ep.out,
-					  msgb_data(msg), msgb_length(msg),
-					  &xfer_len, 100000);
-	} else {
-		rc = write(transp->udp_fd, msgb_data(msg), msgb_length(msg));
+	switch (xfer->status) {
+	case LIBUSB_TRANSFER_COMPLETED:
+		break;
+	case LIBUSB_TRANSFER_NO_DEVICE:
+		fprintf(stderr, "USB device disappeared\n");
+		exit(1);
+		break;
+	default:
+		fprintf(stderr, "USB OUT transfer failed, status=%u\n", xfer->status);
+		exit(1);
+		break;
 	}
 
+	msgb_free(msg);
+	libusb_free_transfer(xfer);
+}
+
+
+static int st2_transp_tx_msg_usb_async(struct osmo_st2_transport *transp, struct msgb *msg)
+{
+	struct libusb_transfer *xfer;
+	int rc;
+
+	xfer = libusb_alloc_transfer(0);
+	OSMO_ASSERT(xfer);
+	xfer->dev_handle = transp->usb_devh;
+	xfer->flags = 0;
+	xfer->type = LIBUSB_TRANSFER_TYPE_BULK;
+	xfer->endpoint = transp->usb_ep.out;
+	xfer->timeout = 100000;
+	xfer->user_data = msg;
+	xfer->length = msgb_length(msg);
+	xfer->buffer = msgb_data(msg);
+	xfer->callback = usb_out_xfer_cb;
+
+	rc = libusb_submit_transfer(xfer);
+	OSMO_ASSERT(rc == 0);
+
+	return rc;
+}
+
+/*! \brief Transmit a given command to the SIMtrace2 device */
+static int st2_transp_tx_msg_usb_sync(struct osmo_st2_transport *transp, struct msgb *msg)
+{
+	int rc;
+	int xfer_len;
+	rc = libusb_bulk_transfer(transp->usb_devh, transp->usb_ep.out,
+				  msgb_data(msg), msgb_length(msg),
+				  &xfer_len, 100000);
 	msgb_free(msg);
 	return rc;
 }
@@ -98,9 +135,24 @@ static struct simtrace_msg_hdr *st_push_hdr(struct msgb *msg, uint8_t msg_class,
 int osmo_st2_slot_tx_msg(struct osmo_st2_slot *slot, struct msgb *msg,
 			 uint8_t msg_class, uint8_t msg_type)
 {
-	st_push_hdr(msg, msg_class, msg_type, slot->slot_nr);
+	struct osmo_st2_transport *transp = slot->transp;
+	int rc;
 
-	return osmo_st2_transp_tx_msg(slot->transp, msg);
+	OSMO_ASSERT(transp);
+
+	st_push_hdr(msg, msg_class, msg_type, slot->slot_nr);
+	printf("SIMtrace <- %s\n", msgb_hexdump(msg));
+
+	if (transp->udp_fd < 0) {
+		if (transp->usb_async)
+			rc = st2_transp_tx_msg_usb_async(transp, msg);
+		else
+			rc = st2_transp_tx_msg_usb_sync(transp, msg);
+	} else {
+		rc = write(transp->udp_fd, msgb_data(msg), msgb_length(msg));
+		msgb_free(msg);
+	}
+	return rc;
 }
 
 /***********************************************************************