Make iso7816_tx_is_idle() more readable

let's use #defines and existing inline function where appropriate

.gitignore

@@ -5,5 +5,7 @@ CMakeCache.txt
 *.elf
 *.elf.map
 *.hex
+*.pio.h
+*.a
 CMakeFiles
 Makefile

CMakeLists.txt

@@ -15,6 +15,11 @@ add_compile_options(-Wall
         -Wno-format
         -Wno-unused-functions
         -Wno-maybe-uninitialized
+	-g -O0
       )
 
+add_subdirectory(libosmocore)
+add_subdirectory(libosmo-rp2)
+add_subdirectory(libsimtrace)
+add_subdirectory(composite)
 add_subdirectory(iso7816)

composite/CMakeLists.txt

@@ -0,0 +1,33 @@
+add_executable(composite)
+
+target_compile_options(composite PRIVATE -DPARAM_ASSERTIONS_ENABLE_ALL=1)
+
+pico_generate_pio_header(composite ${CMAKE_CURRENT_LIST_DIR}/iso7816_tx.pio)
+
+target_sources(composite PRIVATE
+        composite.c
+        usb_descriptors.c
+        usb_buffered_ep.c
+        tusb_cardem.c
+        card_emu_rp2.c
+        audio.c
+        rp2_timer.c
+      )
+
+# Make sure TinyUSB can find tusb_config.h
+target_include_directories(composite PUBLIC
+        ${CMAKE_CURRENT_LIST_DIR})
+
+target_link_libraries(composite PRIVATE
+        libsimtrace
+        libosmocore
+        pico_stdlib
+        pico_multicore
+        hardware_pio
+        tinyusb_device
+        tinyusb_board
+        cmsis_core
+        )
+pico_enable_stdio_usb(composite 0)
+
+pico_add_extra_outputs(composite)

composite/audio.c

@@ -0,0 +1,340 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "bsp/board.h"
+#include "tusb.h"
+#include "usb_descriptors.h"
+#include "class/audio/audio_device.h"
+
+#ifdef HAVE_AUDIO
+
+/* could also be 16kHz for AMR-WB calls? */
+#define AUDIO_SAMPLE_RATE   8000
+
+#undef TU_LOG2
+#define TU_LOG2 printf
+
+static uint32_t sampFreq;
+static uint8_t clkValid;
+
+static audio_control_range_4_n_t(1) sampleFreqRng; 						// Sample frequency range state
+
+// Audio test data
+static uint16_t test_buffer_audio[(CFG_TUD_AUDIO_EP_SZ - 2) / 2];
+static uint16_t startVal = 0;
+
+int audio_init(void)
+{
+	TU_LOG2("%s()\r\n", __func__);
+
+	sampFreq = AUDIO_SAMPLE_RATE;
+	clkValid = 1;
+
+	sampleFreqRng.wNumSubRanges = 1;
+	sampleFreqRng.subrange[0].bMin = AUDIO_SAMPLE_RATE;
+	sampleFreqRng.subrange[0].bMax = AUDIO_SAMPLE_RATE;
+	sampleFreqRng.subrange[0].bRes = 0;
+
+	return 0;
+}
+
+// Invoked when audio class specific set request received for an EP
+bool tud_audio_set_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff)
+{
+  (void) rhport;
+  (void) pBuff;
+
+  // We do not support any set range requests here, only current value requests
+  TU_VERIFY(p_request->bRequest == AUDIO_CS_REQ_CUR);
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t ep = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) ep;
+
+  TU_LOG2("%s(chan=%u, sel=%u, ep=0x%02x)\r\n", __func__, channelNum, ctrlSel, ep);
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific set request received for an interface
+bool tud_audio_set_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff)
+{
+  (void) rhport;
+  (void) pBuff;
+
+  // We do not support any set range requests here, only current value requests
+  TU_VERIFY(p_request->bRequest == AUDIO_CS_REQ_CUR);
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t itf = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) itf;
+
+  TU_LOG2("%s(chan=%u, sel=%u, itf=%u)\r\n", __func__, channelNum, ctrlSel, itf);
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific set request received for an entity
+bool tud_audio_set_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t itf = TU_U16_LOW(p_request->wIndex);
+  uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
+
+  (void) itf;
+
+  TU_LOG2("%s(chan=%u, sel=%u, itf=%u, entity=%u)\r\n", __func__, channelNum, ctrlSel, itf, entityID);
+
+  // We do not support any set range requests here, only current value requests
+  TU_VERIFY(p_request->bRequest == AUDIO_CS_REQ_CUR);
+
+#if 0
+  // If request is for our feature unit
+  if ( entityID == 2 )
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_FU_CTRL_MUTE:
+        // Request uses format layout 1
+        TU_VERIFY(p_request->wLength == sizeof(audio_control_cur_1_t));
+
+        mute[channelNum] = ((audio_control_cur_1_t*) pBuff)->bCur;
+
+        TU_LOG2("    Set Mute: %d of channel: %u\r\n", mute[channelNum], channelNum);
+      return true;
+
+      case AUDIO_FU_CTRL_VOLUME:
+        // Request uses format layout 2
+        TU_VERIFY(p_request->wLength == sizeof(audio_control_cur_2_t));
+
+        volume[channelNum] = (uint16_t) ((audio_control_cur_2_t*) pBuff)->bCur;
+
+        TU_LOG2("    Set Volume: %d dB of channel: %u\r\n", volume[channelNum], channelNum);
+      return true;
+
+        // Unknown/Unsupported control
+      default:
+        TU_BREAKPOINT();
+      return false;
+    }
+  }
+#endif
+  return false;    // Yet not implemented
+}
+
+// Invoked when audio class specific get request received for an EP
+bool tud_audio_get_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t ep = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) ep;
+
+  //	return tud_control_xfer(rhport, p_request, &tmp, 1);
+
+  TU_LOG2("%s(chan=%u, sel=%u, ep=0x%02x)\r\n", __func__, channelNum, ctrlSel, ep);
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific get request received for an interface
+bool tud_audio_get_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t itf = TU_U16_LOW(p_request->wIndex);
+
+  (void) channelNum; (void) ctrlSel; (void) itf;
+
+  TU_LOG2("%s(chan=%u, sel=%u, itf=%u)\r\n", __func__, channelNum, ctrlSel, itf);
+
+  return false; 	// Yet not implemented
+}
+
+// Invoked when audio class specific get request received for an entity
+bool tud_audio_get_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  (void) rhport;
+
+  // Page 91 in UAC2 specification
+  uint8_t channelNum = TU_U16_LOW(p_request->wValue);
+  uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+  uint8_t itf = TU_U16_LOW(p_request->wIndex);
+  uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
+
+  TU_LOG2("%s(chan=%u, sel=%u, itf=%u, entity=%u)\r\n", __func__, channelNum, ctrlSel, itf, entityID);
+
+  // Input terminal (Microphone input)
+  if (entityID == 1)
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_TE_CTRL_CONNECTOR:
+      {
+        // The terminal connector control only has a get request with only the CUR attribute.
+        audio_desc_channel_cluster_t ret;
+
+        // Those are dummy values for now
+        ret.bNrChannels = 1;
+        ret.bmChannelConfig = (audio_channel_config_t) 0;
+        ret.iChannelNames = 0;
+
+        TU_LOG2("    Get terminal connector\r\n");
+
+        return tud_audio_buffer_and_schedule_control_xfer(rhport, p_request, (void*) &ret, sizeof(ret));
+      }
+      break;
+
+        // Unknown/Unsupported control selector
+      default:
+        TU_BREAKPOINT();
+        return false;
+    }
+  }
+
+  // Clock Source unit
+  if ( entityID == 4 )
+  {
+    switch ( ctrlSel )
+    {
+      case AUDIO_CS_CTRL_SAM_FREQ:
+        // channelNum is always zero in this case
+        switch ( p_request->bRequest )
+        {
+          case AUDIO_CS_REQ_CUR:
+            TU_LOG2("    Get Sample Freq.\r\n");
+            return tud_control_xfer(rhport, p_request, &sampFreq, sizeof(sampFreq));
+
+          case AUDIO_CS_REQ_RANGE:
+            TU_LOG2("    Get Sample Freq. range\r\n");
+            return tud_control_xfer(rhport, p_request, &sampleFreqRng, sizeof(sampleFreqRng));
+
+           // Unknown/Unsupported control
+          default:
+            TU_BREAKPOINT();
+            return false;
+        }
+      break;
+
+      case AUDIO_CS_CTRL_CLK_VALID:
+        // Only cur attribute exists for this request
+        TU_LOG2("    Get Sample Freq. valid\r\n");
+        return tud_control_xfer(rhport, p_request, &clkValid, sizeof(clkValid));
+
+      // Unknown/Unsupported control
+      default:
+        TU_BREAKPOINT();
+        return false;
+    }
+  }
+
+  TU_LOG2("  Unsupported entity: %d\r\n", entityID);
+  return false; 	// Yet not implemented
+}
+
+bool tud_audio_tx_done_pre_load_cb(uint8_t rhport, uint8_t itf, uint8_t ep_in, uint8_t cur_alt_setting)
+{
+	(void) rhport;
+	(void) itf;
+	(void) ep_in;
+	(void) cur_alt_setting;
+
+	/* tud_audio core tells us that the previous Tx was completed and we need new data to transmit */
+	//printf("%s()\r\n", __func__);
+
+	tud_audio_write((uint8_t *)test_buffer_audio, CFG_TUD_AUDIO_EP_SZ - 2);
+
+	return true;
+}
+
+bool tud_audio_tx_done_post_load_cb(uint8_t rhport, uint16_t n_bytes_copied, uint8_t itf, uint8_t ep_in, uint8_t cur_alt_setting)
+{
+	(void) rhport;
+	(void) n_bytes_copied;
+	(void) itf;
+	(void) ep_in;
+	(void) cur_alt_setting;
+
+	/* tud_audio core tells us that the previous Tx was completed (at n_bytes_copied) */
+	//printf("%s(%u)\r\n", __func__, n_bytes_copied);
+
+	for (size_t cnt = 0; cnt < (CFG_TUD_AUDIO_EP_SZ - 2) / 2; cnt++) {
+		test_buffer_audio[cnt] = startVal++;
+	}
+
+	return true;
+}
+
+bool tud_audio_rx_done_pre_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting)
+{
+	(void) rhport;
+	(void) func_id;
+	(void) ep_out;
+	(void) cur_alt_setting;
+
+	//printf("%s(%u)\r\n", __func__, n_bytes_received);
+
+	uint8_t buf[n_bytes_received];
+
+	/* read whatever data was received from the host */
+	tud_audio_read(buf, n_bytes_received);
+
+	return true;
+}
+
+#if 0
+bool tud_audio_rx_done_post_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting)
+{
+	(void) rhport;
+	(void) n_bytes_received;
+	(void) func_id;
+	(void) ep_out;
+	(void) cur_alt_setting;
+}
+#endif
+
+bool tud_audio_set_itf_close_EP_cb(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+	(void) rhport;
+	uint8_t const itf = tu_u16_low(p_request->wIndex);
+	uint8_t const alt = tu_u16_low(p_request->wValue);
+
+	printf("%s(itf=%u, alt=%u)\r\n", __func__, itf, alt);
+
+	switch (itf) {
+	case ITF_NUM_AUDIO_STREAM_OUT:
+		break;
+	case ITF_NUM_AUDIO_STREAM_IN:
+		startVal = 0;
+		break;
+	}
+
+	return true;
+}
+
+#else /* HAVE_AUDIO */
+
+int audio_init(void)
+{
+	return 0;
+}
+
+#endif /* HAVE_AUDIO */

composite/card_emu_rp2.c

@@ -0,0 +1,289 @@
+#include <errno.h>
+#include <osmocom/core/timer.h>
+#include <libsimtrace/card_emu.h>
+#include "hardware/pio.h"
+#include "shared.h"
+#include "iso7816_tx.pio.h"
+
+/***********************************************************************
+ * ISO7816 UART driver underneath card_emu.c
+ ***********************************************************************/
+
+struct rp2_cardem_uart_inst {
+	uint32_t clk_freq_khz;
+	uint16_t fidi;
+	PIO pio;
+	uint tx_sm;
+	uint tx_prog_offset;
+	uint rx_prog_offset;
+	struct {
+		uint32_t total;
+		struct osmo_timer_list half;
+		struct osmo_timer_list full;
+	} waiting_time;
+	/* pointer to libsimtrace/card_emu state structure */
+	struct card_handle *ch;
+};
+
+static struct rp2_cardem_uart_inst g_cui[MAX_CARDEM_ITF] = {
+	[0] = {
+		.clk_freq_khz = 0,
+		.pio = pio0,
+		.tx_sm = 0,
+	},
+};
+
+/* timer call-back when half of the waiting time has expired */
+static void waiting_time_half_cb(void *data)
+{
+	struct rp2_cardem_uart_inst *cui = (struct rp2_cardem_uart_inst *) data;
+	card_emu_wtime_half_expired(cui->ch);
+}
+
+/* timer call-back when entire waiting time has expired */
+static void waiting_time_full_cb(void *data)
+{
+	struct rp2_cardem_uart_inst *cui = (struct rp2_cardem_uart_inst *) data;
+	card_emu_wtime_expired(cui->ch);
+}
+
+/* re-start the waiting time timers */
+static void wt_timer_restart(struct rp2_cardem_uart_inst *cui)
+{
+	uint32_t baud_rate = (cui->clk_freq_khz * 1000) / cui->fidi;
+	uint32_t bit_duration_us = 1000000 / baud_rate;
+	/* FIXME: do some checks regarding precision and bounds here! */
+	uint32_t timer_us = bit_duration_us * cui->waiting_time.total;
+	osmo_timer_schedule(&cui->waiting_time.half, 0, timer_us/2);
+	osmo_timer_schedule(&cui->waiting_time.full, 0, timer_us);
+}
+
+/* stop the waiting time timers */
+static void wt_timer_stop(struct rp2_cardem_uart_inst *cui)
+{
+	osmo_timer_del(&cui->waiting_time.half);
+	osmo_timer_del(&cui->waiting_time.full);
+}
+
+/* call-back from card_emu.c to wait for UART Tx to become idle */
+void card_emu_uart_wait_tx_idle(uint8_t uart_chan)
+{
+	const struct rp2_cardem_uart_inst *cui = &g_cui[uart_chan];
+
+	/* how do we know once the PIO unit has completed tx of a character? */
+	/* - state machine TX fifo must be empty (FSTAT:TXEMPTY), AND
+	 * - FDEBUG:TXSTALL bit must be set */
+	while (true) {
+		if (iso7816_tx_is_idle(cui->pio, cui->tx_sm))
+			break;
+	}
+}
+
+/* call-back from card_emu.c to enable/disable transmit and/or receive */
+void card_emu_uart_enable(uint8_t uart_chan, uint8_t rxtx)
+{
+	struct rp2_cardem_uart_inst *cui = &g_cui[uart_chan];
+
+	switch (rxtx) {
+	case ENABLE_TX:
+		iso7816_tx_enable(cui->pio, cui->tx_sm, true);
+		// FIXME: we actually want to start the timer after completing TX! */
+		wt_timer_restart(cui);
+		break;
+	case ENABLE_TX_TIMER_ONLY:
+		/* we only want the WT timer running, but not actually transmit */
+		wt_timer_restart(cui);
+		break;
+	case ENABLE_RX:
+		wt_timer_stop(cui);
+		iso7816_tx_enable(cui->pio, cui->tx_sm, false);
+		//iso7816_rx_enable(pio, sm, true);
+		break;
+	case 0:
+	default:
+		wt_timer_stop(cui);
+		iso7816_tx_enable(cui->pio, cui->tx_sm, false);
+		//iso7816_rx_enable(pio, sm, false);
+		break;
+	}
+}
+
+/* call-back from card_emu.c to transmit a byte */
+int card_emu_uart_tx(uint8_t uart_chan, uint8_t byte)
+{
+	struct rp2_cardem_uart_inst *cui = &g_cui[uart_chan];
+	iso7816_tx_program_putc(cui->pio, cui->tx_sm, byte);
+	return 0;
+}
+
+/* call-back from card_emu.c to change UART baud rate */
+int card_emu_uart_update_fidi(uint8_t uart_chan, unsigned int fidi)
+{
+	struct rp2_cardem_uart_inst *cui = &g_cui[uart_chan];
+	cui->fidi = fidi;
+	/* FIXME: change actual baud rate */
+	/* original SIMtrace2 code on SAM3S does *not* restart WT here */
+	return 0;
+}
+
+/*! Update WT on USART peripheral.  Will automatically re-start timer with new value.
+ *  \param[in] usart USART peripheral to configure
+ *  \param[in] wt inactivity Waiting Time before card_emu_wtime_expired is called (0 to disable) */
+void card_emu_uart_update_wt(uint8_t uart_chan, uint32_t wt)
+{
+	struct rp2_cardem_uart_inst *cui = &g_cui[uart_chan];
+
+	cui->waiting_time.total = wt;
+	/* reset and start the timer */
+	card_emu_uart_reset_wt(uart_chan);
+}
+
+/*! Reset and re-start waiting timeout count down on USART peripheral.
+ *  \param[in] usart USART peripheral to configure */
+void card_emu_uart_reset_wt(uint8_t uart_chan)
+{
+	struct rp2_cardem_uart_inst *cui = &g_cui[uart_chan];
+	wt_timer_stop(cui);
+	wt_timer_restart(cui);
+}
+
+/*! Trigger transmit of first byte of ATR byte TS ?!? */
+void card_emu_uart_interrupt(uint8_t uart_chan)
+{
+	//struct rp2_cardem_uart_inst *cui = &g_cui[uart_chan];
+	// FIXME
+}
+
+
+static void cardem_uart_init(struct rp2_cardem_uart_inst *cui)
+{
+	osmo_timer_setup(&cui->waiting_time.half, waiting_time_half_cb, cui);
+	osmo_timer_setup(&cui->waiting_time.full, waiting_time_full_cb, cui);
+	cui->tx_prog_offset = pio_add_program(cui->pio, &iso7816_tx_program);
+	//cui->rx_prog_offset = pio_add_program(cui->pio, &iso7816_rx_program);
+}
+
+/* shared/common interrupt handler */
+static void pio_irq0_handler(struct rp2_cardem_uart_inst *cui)
+{
+	/* read interrupt status register.  No need to clear, those are implicitly
+	 * cleared only when the not-full / not-empty conditions are resolved */
+	uint32_t ints0 = cui->pio->ints0;
+	uint32_t txflag = (1 << (pis_sm0_tx_fifo_not_full + cui->tx_sm));
+	bool txnfull  = ints0 & txflag;
+
+	if (txnfull) {
+		/* transmit next byte and check if more bytes are to be transmitted */
+		if (card_emu_tx_byte(cui->ch) == 0) {
+			/* stop the TX ready interrupt of no more bytes to transmit */
+			pio_set_irq0_source_enabled(cui->pio, txflag, false);
+		}
+	}
+
+#if 0
+	bool rxnempty = ints0 & (pis_sm0_rx_fifo_not_empty << cui->sm);
+	if (rxnempty) {
+		/* FIXME: read next byte from the FIFO and push it further up */
+	}
+#endif
+}
+
+void pio0_irq0_handler(void)
+{
+	pio_irq0_handler(&g_cui[0]);
+	// FIXME: handle further state machines in case of multiple instances
+}
+
+
+/***********************************************************************
+ * RP2040 GPIO handling
+ ***********************************************************************/
+
+
+#include "hardware/clocks.h"
+
+#define GPIO_SIM_VCC	18		/* pico pin 24 */
+#define GPIO_SIM_RST	19		/* pico pin 25 */
+#define GPIO_SIM_IO	21		/* pico pin 27 */
+#define GPIO_SIM_CLK	20 /* must be at GP20 due to the GPIN0 function; pico pin 26 */
+
+
+static void gpio_callback(uint gpio, uint32_t events)
+{
+	struct card_handle *ch;
+
+	/* dispatch VCC or RST level changes to core card_emu.c */
+	switch (gpio) {
+	case GPIO_SIM_VCC:
+		ch = g_cui[0].ch;
+		if (!ch)
+			return;
+		if (events & GPIO_IRQ_EDGE_RISE) {
+			card_emu_io_statechg(ch, CARD_IO_VCC, true);
+			/* determine CLK frequency */
+			g_cui[0].clk_freq_khz = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLKSRC_GPIN0);
+			if (g_cui[0].clk_freq_khz)
+				card_emu_io_statechg(ch, CARD_IO_CLK, true);
+			/* TODO: continuously measure CLK in background to detect CLK stop or change */
+		}
+		if (events & GPIO_IRQ_EDGE_FALL)
+			card_emu_io_statechg(ch, CARD_IO_VCC, false);
+		break;
+	case GPIO_SIM_RST:
+		if (!ch)
+			return;
+		if (events & GPIO_IRQ_EDGE_RISE)
+			card_emu_io_statechg(ch, CARD_IO_RST, false);
+		if (events & GPIO_IRQ_EDGE_FALL)
+			card_emu_io_statechg(ch, CARD_IO_RST, true);
+		break;
+	default:
+		printf("unhandled GP%u: 0x%x\n", gpio, events);
+		break;
+	}
+}
+
+
+int card_emu_drv_init(struct card_handle *ch, uint8_t uart_chan)
+{
+	gpio_set_irq_callback(gpio_callback);
+	irq_set_enabled(IO_IRQ_BANK0, true);
+
+	switch (uart_chan) {
+	case 0:
+		g_cui[uart_chan].ch = ch;
+
+		/* GP20 (CLK) as external clock input GPIN0 */
+		gpio_set_function(GPIO_SIM_CLK, GPIO_FUNC_GPCK);
+		gpio_pull_down(GPIO_SIM_CLK);
+
+		/* GPIO_SIM_VCC as input; edge-triggered interrupts */
+		gpio_init(GPIO_SIM_VCC);
+		gpio_set_dir(GPIO_SIM_VCC, false);
+		gpio_set_slew_rate(GPIO_SIM_VCC, GPIO_SLEW_RATE_SLOW);
+		gpio_pull_down(GPIO_SIM_VCC);
+		gpio_set_input_enabled(GPIO_SIM_VCC, true);
+		gpio_set_irq_enabled(GPIO_SIM_VCC, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
+
+		/* GPIO_SIM_RST as input; edge-triggered interrupts */
+		gpio_init(GPIO_SIM_RST);
+		gpio_set_dir(GPIO_SIM_RST, false);
+		gpio_set_slew_rate(GPIO_SIM_RST, GPIO_SLEW_RATE_SLOW);
+		gpio_pull_up(GPIO_SIM_RST);
+		gpio_set_input_enabled(GPIO_SIM_RST, true);
+		gpio_set_irq_enabled(GPIO_SIM_RST, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
+
+		/* GPIO_SIM_IO bi-directional; handled by PIO0 */
+		gpio_pull_up(GPIO_SIM_IO);
+		gpio_set_function(GPIO_SIM_IO, GPIO_FUNC_PIO0);
+
+		cardem_uart_init(&g_cui[0]);
+
+		break;
+	default:
+		printf("%s: unknown uart_chan %u\r\n", __func__, uart_chan);
+		return -EINVAL;
+	}
+
+	return 0;
+}

composite/composite.c

@@ -0,0 +1,46 @@
+#include <stdio.h>
+
+#include <osmocom/core/timer.h>
+
+#include "bsp/board.h"
+#include "pico/stdlib.h"
+#include "pico/multicore.h"
+#include "hardware/pio.h"
+#include "hardware/uart.h"
+#include "hardware/clocks.h"
+
+#include "tusb.h"
+#include "shared.h"
+
+extern int audio_init(void);
+
+int main()
+{
+	gpio_debug_pins_init();
+	board_init();
+
+#if 1
+	stdio_init_all();
+	/* 115200 bps on GP0/GP1 of RP2040 (at least on pico) */
+	setup_default_uart();
+	tusb_init();
+	audio_init();
+	rp2_timer_init();
+#else
+	stdio_usb_init();
+
+	while (!tud_cdc_connected()) {
+		printf(".");
+		sleep_ms(500);
+	}
+#endif
+	printf("Starting Composite USB playground\n");
+
+	printf("Entering main loop\n");
+	while (true) {
+		tud_task();
+		osmo_timers_prepare();
+		osmo_timers_update();
+	}
+
+}

composite/iso7816_tx.pio

@@ -0,0 +1,114 @@
+; Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+;
+; SPDX-License-Identifier: BSD-3-Clause
+;
+
+.program iso7816_tx
+.side_set 1 opt
+
+; An ISO7816-3 UART transmit program. Adapted from pico-examples/pio/uart_tx/uart_tx.pio
+; OUT pin 0 and side-set pin 0 are both mapped to UART TX pin.
+
+; The general idea is that we run at 5x oversampling, i.e. 5 PIO instruction cycles
+; per bit on the wire.  This is the minimum oversampling stated by ISO 7816-3
+
+
+	pull	   side 1 [4]	; Assert stop bit, or stall with line in idle state
+	set x, 8   side 0 [4]	; Preload bit counter, assert start bit for 5 clocks
+bitloop:			; This loop will run 8 times (8n1 UART)
+	out pins, 1		; Shift 1 bit from OSR to the first OUT pin
+	jmp x-- bitloop   [3]	; Each loop iteration is 5 cycles.
+
+	set x, 8   side 1 [4]	; one additional stop bit so we get to GT=12etu
+
+
+% c-sdk {
+#include "hardware/clocks.h"
+
+
+extern void pio0_irq0_handler(void);
+
+static inline void iso7816_tx_program_init(PIO pio, uint sm, uint offset, uint pin_tx, uint baud)
+{
+	// Tell PIO to initially drive output-high on the selected pin, then map PIO
+	// onto that pin with the IO muxes.
+	pio_sm_set_pins_with_mask(pio, sm, 1u << pin_tx, 1u << pin_tx);
+	pio_sm_set_pindirs_with_mask(pio, sm, 1u << pin_tx, 1u << pin_tx);
+	pio_gpio_init(pio, pin_tx);
+
+	pio_sm_config c = iso7816_tx_program_get_default_config(offset);
+
+	// OUT shifts to right, no autopull
+	sm_config_set_out_shift(&c, true, false, 32);
+
+	// We are mapping both OUT and side-set to the same pin, because sometimes
+	// we need to assert user data onto the pin (with OUT) and sometimes
+	// assert constant values (start/stop bit)
+	sm_config_set_out_pins(&c, pin_tx, 1);
+	sm_config_set_sideset_pins(&c, pin_tx);
+
+	// SM transmits 1 bit per 5 execution cycles.
+	float div = (float)clock_get_hz(clk_sys) / (5 * baud);
+	printf("baud=%u, div=%f\n", baud, div);
+	sm_config_set_clkdiv(&c, div);
+
+	irq_set_exclusive_handler(PIO0_IRQ_0, pio0_irq0_handler);
+	irq_set_enabled(PIO0_IRQ_0, true);
+
+	pio_sm_init(pio, sm, offset, &c);
+	pio_sm_set_enabled(pio, sm, true);
+}
+
+static inline void iso7816_tx_enable(PIO pio, uint sm, bool enable)
+{
+	/* enable IRQ0 for tx-fifo-not-full */
+	pio_set_irq0_source_enabled(pio, (pis_sm0_tx_fifo_not_full << sm), true);
+
+	pio_sm_set_enabled(pio, sm, enable);
+	// FIXME: change GPIO to input
+}
+
+static inline bool compute_even_parity_bit(uint8_t x)
+{
+	x ^= x >> 4;
+	x ^= x >> 2;
+	x ^= x >> 1;
+	return (x) & 1;
+}
+
+static inline void iso7816_tx_program_putc(PIO pio, uint sm, uint8_t c)
+{
+	uint32_t c_with_parity = c;
+
+	if (compute_even_parity_bit(c))
+		c_with_parity |= 0x100;
+
+	pio_sm_put_blocking(pio, sm, c_with_parity);
+}
+
+static inline void iso7816_tx_program_puts(PIO pio, uint sm, const uint8_t *s, size_t s_len)
+{
+	while (s_len--)
+		iso7816_tx_program_putc(pio, sm, *s++);
+}
+
+static inline bool iso7816_tx_is_idle(PIO pio, uint sm)
+{
+	uint32_t flag = (1 << (PIO_FDEBUG_TXSTALL_LSB + sm));
+
+	/* how do we know once the PIO unit has completed tx of a character?
+	   - state machine TX fifo must be empty (FSTAT:TXEMPTY), AND
+	   - FDEBUG:TXSTALL bit must be set
+	*/
+
+	/* clear the sticky STALL flag */
+	pio->fdebug |= flag;
+
+	if (pio_sm_is_tx_fifo_empty(pio, sm) && pio->fdebug & flag)
+		return true;
+	else
+		return false;
+}
+
+
+%}

composite/rp2_timer.c

@@ -0,0 +1,18 @@
+#include "RP2040.h"
+#include "core_cm0plus.h"
+
+
+/* value that increments by one every milli-second */
+volatile unsigned long jiffies;
+
+/* handler for the SysTick interrupt/exception. Just increments jiffies */
+void SysTick_Handler(void)
+{
+	jiffies++;
+}
+
+void rp2_timer_init(void)
+{
+	/* generate SysTick interrupt every 1ms (1000*1us), where 1us is the standard tick */
+	SysTick_Config(1000);
+}

composite/shared.h

@@ -0,0 +1,10 @@
+#pragma once
+#include <stdint.h>
+
+/* maximum number of cardem interfaces */
+#define MAX_CARDEM_ITF	1
+
+void rp2_timer_init(void);
+
+struct card_handle;
+int card_emu_drv_init(struct card_handle *ch, uint8_t uart_chan);

composite/tusb_cardem.c

@@ -0,0 +1,546 @@
+
+#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
+ *
+ */

composite/tusb_config.h

@@ -0,0 +1,129 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ */
+
+#ifndef _TUSB_CONFIG_H_
+#define _TUSB_CONFIG_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+//--------------------------------------------------------------------
+// COMMON CONFIGURATION
+//--------------------------------------------------------------------
+
+// defined by board.mk
+#ifndef CFG_TUSB_MCU
+  #error CFG_TUSB_MCU must be defined
+#endif
+
+// RHPort number used for device can be defined by board.mk, default to port 0
+#ifndef BOARD_DEVICE_RHPORT_NUM
+  #define BOARD_DEVICE_RHPORT_NUM     0
+#endif
+
+// RHPort max operational speed can defined by board.mk
+// Default to Highspeed for MCU with internal HighSpeed PHY (can be port specific), otherwise FullSpeed
+#ifndef BOARD_DEVICE_RHPORT_SPEED
+  #if (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX || \
+       CFG_TUSB_MCU == OPT_MCU_NUC505  || CFG_TUSB_MCU == OPT_MCU_CXD56 || CFG_TUSB_MCU == OPT_MCU_SAMX7X)
+    #define BOARD_DEVICE_RHPORT_SPEED   OPT_MODE_HIGH_SPEED
+  #else
+    #define BOARD_DEVICE_RHPORT_SPEED   OPT_MODE_FULL_SPEED
+  #endif
+#endif
+
+// Device mode with rhport and speed defined by board.mk
+#if   BOARD_DEVICE_RHPORT_NUM == 0
+  #define CFG_TUSB_RHPORT0_MODE     (OPT_MODE_DEVICE | BOARD_DEVICE_RHPORT_SPEED)
+#elif BOARD_DEVICE_RHPORT_NUM == 1
+  #define CFG_TUSB_RHPORT1_MODE     (OPT_MODE_DEVICE | BOARD_DEVICE_RHPORT_SPEED)
+#else
+  #error "Incorrect RHPort configuration"
+#endif
+
+#ifndef CFG_TUSB_OS
+#define CFG_TUSB_OS               OPT_OS_NONE
+#endif
+
+// CFG_TUSB_DEBUG is defined by compiler in DEBUG build
+//#define CFG_TUSB_DEBUG           10
+
+/* USB DMA on some MCUs can only access a specific SRAM region with restriction on alignment.
+ * Tinyusb use follows macros to declare transferring memory so that they can be put
+ * into those specific section.
+ * e.g
+ * - CFG_TUSB_MEM SECTION : __attribute__ (( section(".usb_ram") ))
+ * - CFG_TUSB_MEM_ALIGN   : __attribute__ ((aligned(4)))
+ */
+#ifndef CFG_TUSB_MEM_SECTION
+#define CFG_TUSB_MEM_SECTION
+#endif
+
+#ifndef CFG_TUSB_MEM_ALIGN
+#define CFG_TUSB_MEM_ALIGN          __attribute__ ((aligned(4)))
+#endif
+
+//--------------------------------------------------------------------
+// DEVICE CONFIGURATION
+//--------------------------------------------------------------------
+
+#ifndef CFG_TUD_ENDPOINT0_SIZE
+#define CFG_TUD_ENDPOINT0_SIZE    64
+#endif
+
+//------------- CLASS -------------//
+#define CFG_TUD_HID               0
+#define CFG_TUD_CDC               0
+#define CFG_TUD_AUDIO             1
+#define CFG_TUD_MSC               0
+#define CFG_TUD_MIDI              0
+#define CFG_TUD_VENDOR            0
+
+// HID buffer size Should be sufficient to hold ID (if any) + Data
+#define CFG_TUD_HID_EP_BUFSIZE    16
+
+#define CFG_TUD_CDC_EP_BUFSIZE   (TUD_OPT_HIGH_SPEED ? 512 : 64)
+#define CFG_TUD_CDC_RX_BUFSIZE   CFG_TUD_CDC_EP_BUFSIZE
+#define CFG_TUD_CDC_TX_BUFSIZE   CFG_TUD_CDC_EP_BUFSIZE
+
+#include "usb_descriptors.h"
+#define CFG_TUD_AUDIO_FUNC_1_DESC_LEN		TUD_AUDIO_PHONE_MONO_DESCRIPTOR_LEN
+#define CFG_TUD_AUDIO_FUNC_1_N_AS_INT		2
+#define CFG_TUD_AUDIO_EP_SZ			(16 + 1) * 2 /* 16kHz * 2 bytes/sample; +1 for async xfer adj */
+#define	CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ	64
+
+#define CFG_TUD_AUDIO_ENABLE_EP_IN	1
+#define CFG_TUD_AUDIO_ENABLE_EP_OUT	1
+#define CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX	CFG_TUD_AUDIO_EP_SZ
+#define CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ	CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX + 1
+#define CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX	CFG_TUD_AUDIO_EP_SZ
+#define CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ	CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX + 1
+
+#ifdef __cplusplus
+ }
+#endif
+
+#endif /* _TUSB_CONFIG_H_ */

composite/usb_buffered_ep.c

@@ -0,0 +1,141 @@
+/****************************************************************************
+ * buffered endpoint
+ ****************************************************************************/
+
+/* USB buffer library
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "tusb.h"
+#include "device/usbd_pvt.h"
+#include "trace.h"
+#include <libsimtrace/usb_buffered_ep.h>
+#include <errno.h>
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/msgb.h>
+
+#define USB_MAX_QLEN 3
+#define USB_ALLOC_SIZE	280
+#define USB_HEADROOM_SIZE 8
+
+void bep_init(struct usb_buffered_ep *bep, uint8_t ep)
+{
+	bep->ep = ep;
+	bep->msg_in_progress = NULL;
+	INIT_LLIST_HEAD(&bep->queue);
+	bep->queue_len = 0;
+}
+
+struct msgb *bep_msgb_alloc(struct usb_buffered_ep *bep)
+{
+	struct msgb *msg = msgb_alloc_headroom(USB_ALLOC_SIZE, USB_HEADROOM_SIZE, "USB");
+	if (!msg)
+		return NULL;
+	msg->dst = bep;
+	return msg;
+}
+
+void bep_msgb_free(struct msgb *msg)
+{
+	msgb_free(msg);
+}
+
+/* IN/IRQ EP: dequeue next pending message and send it to host */
+int bep_refill_to_host(struct usb_buffered_ep *bep, bool is_out_ep)
+{
+	unsigned long x;
+	const uint8_t rhport = 0;
+	unsigned int len;
+
+	if (usbd_edpt_busy(rhport, bep->ep)) {
+		LOGBEP(bep, "skipping: edpt_busy\r\n");
+		return 0;
+	}
+
+	local_irq_save(x);
+	if (bep->msg_in_progress) {
+		local_irq_restore(x);
+		LOGBEP(bep, "skipping: msg_in_progress\r\n");
+		return 0;
+	}
+
+	if (llist_empty(&bep->queue)) {
+		local_irq_restore(x);
+		LOGBEP(bep, "skipping: queue empty\r\n");
+		return 0;
+	}
+
+	bep->msg_in_progress = msgb_dequeue_count(&bep->queue, &bep->queue_len);
+	local_irq_restore(x);
+
+	if (is_out_ep)
+		len = msgb_tailroom(bep->msg_in_progress);
+	else
+		len = msgb_length(bep->msg_in_progress);
+
+	TU_VERIFY(usbd_edpt_xfer(rhport, bep->ep, msgb_data(bep->msg_in_progress), len));
+
+	LOGBEP(bep, "success (msg=%p, data=%p/%u)!\r\n", bep->msg_in_progress, msgb_data(bep->msg_in_progress), len);
+	return 1;
+}
+
+/* assume the last in-progress msgb has completed + return it */
+struct msgb *bep_get_completed(struct usb_buffered_ep *bep)
+{
+	unsigned long x;
+	struct msgb *msg;
+
+	local_irq_save(x);
+	msg = bep->msg_in_progress;
+	bep->msg_in_progress = NULL;
+	local_irq_restore(x);
+	LOGBEP(bep, "(msg=%p)\r\n", msg);
+
+	return msg;
+}
+
+/* assume the last in-progress msgb has completed + free it */
+void bep_complete_in_progress(struct usb_buffered_ep *bep)
+{
+	struct msgb *msg = bep_get_completed(bep);
+	LOGBEP(bep, "(msg=%p)\r\n", msg);
+	bep_msgb_free(msg);
+}
+
+/* enqueue a USB buffer for transmission to host */
+int bep_enqueue_msgb(struct msgb *msg)
+{
+	struct usb_buffered_ep *ep = msg->dst;
+
+	if (!msg->dst) {
+		TRACE_ERROR("%s: msg without dst\r\n", __func__);
+		bep_msgb_free(msg);
+		return -EINVAL;
+	}
+
+	/* no need for irqsafe operation, as the usb_tx_queue is
+	 * processed only by the main loop context */
+
+	if (ep->queue_len >= USB_MAX_QLEN) {
+		struct msgb *evict;
+		/* free the first pending buffer in the queue */
+		TRACE_INFO("EP%02x: dropping first queue element (qlen=%u)\r\n",
+			   ep->ep, ep->queue_len);
+		evict = msgb_dequeue_count(&ep->queue, &ep->queue_len);
+		OSMO_ASSERT(evict);
+		bep_msgb_free(evict);
+	}
+
+	LOGBEP(ep, "(msg=%p)\r\n", msg);
+	msgb_enqueue_count(&ep->queue, msg, &ep->queue_len);
+	return 0;
+}

composite/usb_descriptors.c

@@ -0,0 +1,115 @@
+#include "tusb.h"
+#include "class/audio/audio.h"
+#include "usb_descriptors.h"
+
+/* SIMtrace2 "cardem" compatible interface descriptor */
+#define TUD_CARDEM_DESCRIPTOR(_itfnum, _stridx, _epout, _epin, _epirq)	\
+	9, TUSB_DESC_INTERFACE, _itfnum, 0, 3, 0xff, 2, 0, _stridx, \
+	7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(64), 0, \
+	7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(64), 0, \
+	7, TUSB_DESC_ENDPOINT, _epirq, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(64), 1
+
+#define TUD_CARDEM_DESCRIPTOR_LEN (9 + 7 + 7 + 7)
+
+/* Device Descriptor */
+static tusb_desc_device_t const desc_device = {
+	.bLength		= sizeof(tusb_desc_device_t),
+	.bDescriptorType	= TUSB_DESC_DEVICE,
+	.bcdUSB			= 0x0200,
+	.bDeviceClass		= 0x00,
+	.bDeviceSubClass	= 0x00,
+	.bDeviceProtocol	= 0x00,
+	.bMaxPacketSize0	= CFG_TUD_ENDPOINT0_SIZE,
+
+	.idVendor		= 0x1d50,
+	.idProduct		= 0xaaaa,
+	.bcdDevice		= 0x0100,
+
+	.iManufacturer		= 1,
+	.iProduct		= 2,
+	.iSerialNumber		= 3,
+
+	.bNumConfigurations	= 1
+};
+
+/* Invoked when received GET DEVICE DESCRIPTOR */
+uint8_t const* tud_descriptor_device_cb(void)
+{
+	return (uint8_t const *) &desc_device;
+}
+
+
+/* Configuration Descriptor */
+#ifdef HAVE_AUDIO
+#define CONFIG_TOTAL_LEN \
+	(TUD_CONFIG_DESC_LEN + TUD_CARDEM_DESCRIPTOR_LEN + TUD_AUDIO_PHONE_MONO_DESCRIPTOR_LEN)
+#else
+#define CONFIG_TOTAL_LEN \
+	(TUD_CONFIG_DESC_LEN + TUD_CARDEM_DESCRIPTOR_LEN)
+#endif
+static uint8_t const desc_config[] = {
+	// Config number, interface count, string index, total length, attribute, power in mA
+	TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, 100),
+
+	TUD_CARDEM_DESCRIPTOR(ITF_NUM_CARDEM, 4, /*epout*/ 0x01, /*epin*/ 0x81, /*epirq*/ 0x82),
+
+#ifdef HAVE_AUDIO
+	// interface number, string index, epout, epin, epsize, epfb
+	TUD_AUDIO_PHONE_MONO_DESCRIPTOR(ITF_NUM_AUDIO_CTRL, 5, 0x02, 0x83, CFG_TUD_AUDIO_EP_SZ, 0x84),
+#endif
+};
+
+/* Invoked when received GET CONFIGURATION DESCRIPTOR */
+uint8_t const* tud_descriptor_configuration_cb(uint8_t index)
+{
+	// This example use the same configuration for both high and full speed mode
+	return desc_config;
+}
+
+/* String Descriptors */
+char const  *string_desc_arr[] = {
+	(const char []) { 0x09, 0x04},	/* 0: supported language == English */
+	"sysmocom - s.f.m.c. GmbH",	/* 1: Manufacturer */
+	"pico-pcm-cardem",		/* 2: Product */
+	"12345",			/* 3: Serial, should use chip ID */
+	"SIMtrace2 compatible card-emulation",	/* 4: CARDEM */
+#ifdef HAVE_AUDIO
+	"Cellular Modem PCM Audio",	/* 5: Audio Interface */
+#endif
+};
+
+static uint16_t _desc_str[32];
+
+// Invoked when received GET STRING DESCRIPTOR request
+// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete
+uint16_t const* tud_descriptor_string_cb(uint8_t index, uint16_t langid)
+{
+	(void) langid;
+
+	uint8_t chr_count;
+
+	if (index == 0) {
+		memcpy(&_desc_str[1], string_desc_arr[0], 2);
+		chr_count = 1;
+	} else {
+		// Convert ASCII string into UTF-16
+		if (!(index < sizeof(string_desc_arr)/sizeof(string_desc_arr[0])))
+			return NULL;
+
+    		const char* str = string_desc_arr[index];
+
+		// Cap at max char
+		chr_count = (uint8_t) strlen(str);
+		if (chr_count > 31)
+			chr_count = 31;
+
+		for (uint8_t i = 0; i < chr_count; i++) {
+			_desc_str[1+i] = str[i];
+		}
+	}
+
+	// first byte is length (including header), second byte is string type
+	_desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8 ) | (2*chr_count + 2));
+
+	return _desc_str;
+}

composite/usb_descriptors.h

@@ -0,0 +1,101 @@
+#pragma once
+
+//#define HAVE_AUDIO
+
+/* TODO: put into tinyusb */
+#define AUDIO_TERM_TYPE_PH_PHONE_LINE	0x0501
+
+#define TUD_AUDIO_PHONE_MONO_DESCRIPTOR_LEN ( \
+		TUD_AUDIO_DESC_IAD_LEN + \
+		TUD_AUDIO_DESC_STD_AC_LEN + \
+		TUD_AUDIO_DESC_CS_AC_LEN + \
+		TUD_AUDIO_DESC_CLK_SRC_LEN + \
+		TUD_AUDIO_DESC_INPUT_TERM_LEN + \
+		TUD_AUDIO_DESC_OUTPUT_TERM_LEN + \
+		TUD_AUDIO_DESC_INPUT_TERM_LEN + \
+		TUD_AUDIO_DESC_OUTPUT_TERM_LEN + \
+	\
+		TUD_AUDIO_DESC_STD_AS_INT_LEN + \
+		TUD_AUDIO_DESC_STD_AS_INT_LEN + \
+		TUD_AUDIO_DESC_CS_AS_INT_LEN + \
+		TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN + \
+		TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN + \
+		TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN + \
+		TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN + \
+	\
+		TUD_AUDIO_DESC_STD_AS_INT_LEN + \
+		TUD_AUDIO_DESC_STD_AS_INT_LEN + \
+		TUD_AUDIO_DESC_CS_AS_INT_LEN + \
+		TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN + \
+		TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN + \
+		TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN \
+	)
+
+#define TUD_AUDIO_PHONE_MONO_DESCRIPTOR(_itfnum, _stridx, _epout, _epin, _epsize, _epfb) \
+\
+	/* Standard Interface Association Descriptor (IAD) */ \
+	TUD_AUDIO_DESC_IAD(/*_firstitfs*/ _itfnum, /*_nitfs*/ 0x03, /*_stridx*/ 0x00), \
+	/* Standard AC Interface Descriptor(4.7.1) */\
+	TUD_AUDIO_DESC_STD_AC(/*_itfnum*/ _itfnum, /*_nEPs*/ 0x00, /*_stridx*/ _stridx),\
+	/* Class-Specific AC Interface Header Descriptor(4.7.2) */\
+	TUD_AUDIO_DESC_CS_AC(/*_bcdADC*/ 0x0200, /*_category*/ AUDIO_FUNC_TELEPHONE, \
+		/*_totallen*/	TUD_AUDIO_DESC_CLK_SRC_LEN + \
+				TUD_AUDIO_DESC_INPUT_TERM_LEN + \
+				TUD_AUDIO_DESC_OUTPUT_TERM_LEN + \
+				TUD_AUDIO_DESC_INPUT_TERM_LEN + \
+				TUD_AUDIO_DESC_OUTPUT_TERM_LEN, \
+		/*_ctrl*/ AUDIO_CS_AS_INTERFACE_CTRL_LATENCY_POS),\
+	/* Clock Source Descriptor(4.7.2.1) */\
+	TUD_AUDIO_DESC_CLK_SRC(/*_clkid*/ 0x04, /*_attr*/ AUDIO_CLOCK_SOURCE_ATT_INT_FIX_CLK, /*_ctrl*/ (AUDIO_CTRL_R << AUDIO_CLOCK_SOURCE_CTRL_CLK_FRQ_POS), /*_assocTerm*/ 0x02,  /*_stridx*/ 0x00),\
+	/* Input Terminal Descriptor(4.7.2.4) [USB->Phone] */\
+	TUD_AUDIO_DESC_INPUT_TERM(/*_termid*/ 0x01, /*_termtype*/ AUDIO_TERM_TYPE_USB_STREAMING, /*_assocTerm*/ 0x00, /*_clkid*/ 0x04, /*_nchannelslogical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_idxchannelnames*/ 0x00, /*_ctrl*/ 0 * (AUDIO_CTRL_R << AUDIO_IN_TERM_CTRL_CONNECTOR_POS), /*_stridx*/ 0x00),\
+	/* Output Terminal Descriptor(4.7.2.5) [USB->Phone] */\
+	TUD_AUDIO_DESC_OUTPUT_TERM(/*_termid*/ 0x02, /*_termtype*/ AUDIO_TERM_TYPE_PH_PHONE_LINE, /*_assocTerm*/ 0x01, /*_srcid*/ 0x01, /*_clkid*/ 0x04, /*_ctrl*/ 0x0000, /*_stridx*/ 0x00),\
+\
+	/* Input Terminal Descriptor(4.7.2.4) [Phone->USB] */\
+	TUD_AUDIO_DESC_INPUT_TERM(/*_termid*/ 0x11, /*_termtype*/ AUDIO_TERM_TYPE_PH_PHONE_LINE, /*_assocTerm*/ 0x00, /*_clkid*/ 0x04, /*_nchannelslogical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_idxchannelnames*/ 0x00, /*_ctrl*/ 0 * (AUDIO_CTRL_R << AUDIO_IN_TERM_CTRL_CONNECTOR_POS), /*_stridx*/ 0x00),\
+	/* Output Terminal Descriptor(4.7.2.5) [Phone->USB] */\
+	TUD_AUDIO_DESC_OUTPUT_TERM(/*_termid*/ 0x12, /*_termtype*/ AUDIO_TERM_TYPE_USB_STREAMING, /*_assocTerm*/ 0x11, /*_srcid*/ 0x11, /*_clkid*/ 0x04, /*_ctrl*/ 0x0000, /*_stridx*/ 0x00),\
+\
+	/* Standard AS Interface Descriptor(4.9.1) */\
+	/* Interface 1, Alternate 0 - default alternate setting with 0 bandwidth */\
+	TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum) + 1), /*_altset*/ 0x00, /*_nEPs*/ 0x00, /*_stridx*/ 0x00),\
+	/* Standard AS Interface Descriptor(4.9.1) */\
+	/* Interface 1, Alternate 1 - alternate interface for data streaming */\
+	TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum) + 1), /*_altset*/ 0x01, /*_nEPs*/ 0x02, /*_stridx*/ 0x00),\
+	/* Class-Specific AS Interface Descriptor(4.9.2) */\
+	TUD_AUDIO_DESC_CS_AS_INT(/*_termid*/ 0x01, /*_ctrl*/ AUDIO_CTRL_NONE, /*_formattype*/ AUDIO_FORMAT_TYPE_I, /*_formats*/ AUDIO_DATA_FORMAT_TYPE_I_PCM, /*_nchannelsphysical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_stridx*/ 0x00),\
+	/* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\
+	TUD_AUDIO_DESC_TYPE_I_FORMAT(/*_nBytesPerSample*/2, /*_nBitsUsedPerSample*/ 16),\
+	/* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\
+	TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epout, /*_attr*/ (uint8_t) (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ TUD_OPT_HIGH_SPEED ? 0x04 : 0x01),\
+	/* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\
+	TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000),\
+	/* Standard AS Isochronous Feedback Endpoint Descriptor(4.10.2.1) */\
+	TUD_AUDIO_DESC_STD_AS_ISO_FB_EP(/*_ep*/ _epfb, /*_interval*/ 1),\
+\
+	/* Interface 2, Alternate 0 - default alternate setting with 0 bandwidth */\
+	TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum) + 2), /*_altset*/ 0x00, /*_nEPs*/ 0x00, /*_stridx*/ 0x00),\
+	/* Standard AS Interface Descriptor(4.9.1) */\
+	/* Interface 2, Alternate 1 - alternate interface for data streaming */\
+	TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum) + 2), /*_altset*/ 0x01, /*_nEPs*/ 0x01, /*_stridx*/ 0x00),\
+	/* Class-Specific AS Interface Descriptor(4.9.2) */\
+	TUD_AUDIO_DESC_CS_AS_INT(/*_termid*/ 0x11, /*_ctrl*/ AUDIO_CTRL_NONE, /*_formattype*/ AUDIO_FORMAT_TYPE_I, /*_formats*/ AUDIO_DATA_FORMAT_TYPE_I_PCM, /*_nchannelsphysical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_stridx*/ 0x00),\
+	/* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\
+	TUD_AUDIO_DESC_TYPE_I_FORMAT(/*_nBytesPerSample*/2, /*_nBitsUsedPerSample*/ 16),\
+	/* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\
+	TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (uint8_t) (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ TUD_OPT_HIGH_SPEED ? 0x04 : 0x01),\
+	/* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\
+	TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000)
+
+
+/* Interface numbers */
+enum {
+	ITF_NUM_CARDEM = 0,
+#ifdef HAVE_AUDIO
+	ITF_NUM_AUDIO_CTRL,
+	ITF_NUM_AUDIO_STREAM_OUT,
+	ITF_NUM_AUDIO_STREAM_IN,
+#endif
+	ITF_NUM_TOTAL
+};

iso7816/CMakeLists.txt

@@ -3,9 +3,16 @@ add_executable(iso7816_cardem)
 pico_generate_pio_header(iso7816_cardem ${CMAKE_CURRENT_LIST_DIR}/iso7816_rx.pio)
 pico_generate_pio_header(iso7816_cardem ${CMAKE_CURRENT_LIST_DIR}/iso7816_tx.pio)
 
-target_sources(iso7816_cardem PRIVATE iso7816_cardem.c)
+target_sources(iso7816_cardem PRIVATE 
+  card_emu.c
+  iso7816_cardem.c
+)
 
 target_link_libraries(iso7816_cardem PRIVATE
+        libosmocore
+        libosmo-rp2
+        libsimtrace
+        cmsis_core
         pico_stdlib
         pico_multicore
         hardware_pio

iso7816/card_emu.h

@@ -0,0 +1,78 @@
+/* ISO7816-3 state machine for the card side
+ *
+ * (C) 2010-2017 by Harald Welte <hwelte@hmw-consulting.de>
+ * (C) 2018 by sysmocom -s.f.m.c. GmbH, Author: Kevin Redon <kredon@sysmocom.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#pragma once
+
+#include <stdint.h>
+
+#include <osmocom/core/linuxlist.h>
+
+struct card_handle;
+
+enum card_io {
+	CARD_IO_VCC,
+	CARD_IO_RST,
+	CARD_IO_CLK,
+};
+
+/** initialise card slot
+ *  @param[in] slot_num slot number (arbitrary number)
+ *  @param[in] uart_chan UART peripheral channel
+ *  @param[in] in_ep USB IN end point number
+ *  @param[in] irq_ep USB INTerrupt end point number
+ *  @param[in] vcc_active initial VCC signal state (true = on)
+ *  @param[in] in_reset initial RST signal state (true = reset asserted)
+ *  @param[in] clocked initial CLK signat state (true = active)
+ *  @return main card handle reference
+ */
+struct card_handle *card_emu_init(uint8_t slot_num, uint8_t uart_chan, uint8_t in_ep, uint8_t irq_ep, bool vcc_active, bool in_reset, bool clocked);
+
+/* process a single byte received from the reader */
+void card_emu_process_rx_byte(struct card_handle *ch, uint8_t byte);
+
+/* transmit a single byte to the reader */
+int card_emu_tx_byte(struct card_handle *ch);
+
+/* hardware driver informs us that a card I/O signal has changed */
+void card_emu_io_statechg(struct card_handle *ch, enum card_io io, int active);
+
+/* User sets a new ATR to be returned during next card reset */
+int card_emu_set_atr(struct card_handle *ch, const uint8_t *atr, uint8_t len);
+
+struct llist_head *card_emu_get_uart_tx_queue(struct card_handle *ch);
+void card_emu_have_new_uart_tx(struct card_handle *ch);
+void card_emu_report_status(struct card_handle *ch, bool report_on_irq);
+
+void card_emu_wtime_half_expired(void *ch);
+void card_emu_wtime_expired(void *ch);
+
+
+#define ENABLE_TX		0x01
+#define ENABLE_RX		0x02
+#define ENABLE_TX_TIMER_ONLY	0x03
+
+int card_emu_uart_update_fidi(uint8_t uart_chan, unsigned int fidi);
+void card_emu_uart_update_wt(uint8_t uart_chan, uint32_t wt);
+void card_emu_uart_reset_wt(uint8_t uart_chan);
+int card_emu_uart_tx(uint8_t uart_chan, uint8_t byte);
+void card_emu_uart_enable(uint8_t uart_chan, uint8_t rxtx);
+void card_emu_uart_wait_tx_idle(uint8_t uart_chan);
+void card_emu_uart_interrupt(uint8_t uart_chan);
+
+int card_emu_get_vcc(uint8_t uart_chan);
+
+struct cardemu_usb_msg_config;
+int card_emu_set_config(struct card_handle *ch, const struct cardemu_usb_msg_config *scfg,
+			unsigned int scfg_len);

iso7816/iso7816_cardem.c

@@ -9,6 +9,7 @@
 #include "tusb.h"
 
 #include "iso7816_tx.pio.h"
+#include "iso7816_rx.pio.h"
 
 #define GPIO_SIM_VCC	18		/* pico pin 24 */
 #define GPIO_SIM_RST	19		/* pico pin 25 */
@@ -34,6 +35,7 @@ struct cardem_uart_instance {
 	PIO pio;
 	uint sm;
 	uint tx_prog_offset;
+	uint rx_prog_offset;
 
 };
 
@@ -132,6 +134,7 @@ static void cardem_inst_init(struct cardem_uart_instance *cst)
 	cst->pio = pio0;
 	cst->sm = 0;
 	cst->tx_prog_offset = pio_add_program(cst->pio, &iso7816_tx_program);
+	cst->rx_prog_offset = pio_add_program(cst->pio, &iso7816_rx_program);
 }
 
 static void measure_freqs(void) {
@@ -200,7 +203,7 @@ int main()
 	gpio_set_input_enabled(GPIO_SIM_RST, true);
 	gpio_set_irq_enabled(GPIO_SIM_RST, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
 
-	gpio_pull_up(GPIO_SIM_IO);
+	/* GPIO_SIM_IO bi-directional; handled by PIO0 */
 	gpio_pull_up(GPIO_SIM_IO);
 	gpio_set_function(GPIO_SIM_IO, GPIO_FUNC_PIO0);
 

iso7816/iso7816_rx.pio

@@ -18,7 +18,7 @@ good_stop:			; No delay before returning to start; a little slack is
 	push			; important in case the TX clock is slightly too fast
 
 % c-sdk {
-static inline void iso7816_rx_program_init(PIO pio uint sm, uint offset, uint pin, uint buad)
+static inline void iso7816_rx_program_init(PIO pio, uint sm, uint offset, uint pin, uint baud)
 {
 	pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false);
 	pio_gpio_init(pio, pin);
@@ -36,14 +36,19 @@ static inline void iso7816_rx_program_init(PIO pio uint sm, uint offset, uint pi
 	pio_sm_set_enabled(pio, sm, true);
 }
 
+static inline void iso7816_rx_enable(PIO pio, uint sm, bool enable)
+{
+	pio_sm_set_enabled(pio, sm, enable);
+}
+
 static inline char iso7816_rx_program_getc(PIO pio, uint sm)
 {
 	/* read upper 16 bits of the FIFO, as data is left-justified */
-	io_rw_16 *rxfifo_shift = (io_rw_16 *)&pio->fxf[sm] + 2;
+	io_rw_16 *rxfifo_shift = (io_rw_16 *)&pio->txf[sm] + 2;
 	while (pio_sm_is_rx_fifo_empty(pio, sm))
-		tight_loop_cointents();
+		tight_loop_contents();
 	/* FIXME: evaluate parity bit */
-	return (char ) (*rxfifo_shift >> 7)
+	return (char ) (*rxfifo_shift >> 7);
 }
 
 %}

iso7816/iso7816_tx.pio

@@ -9,7 +9,9 @@
 ; An ISO7816-3 UART transmit program. Adapted from pico-examples/pio/uart_tx/uart_tx.pio
 ; OUT pin 0 and side-set pin 0 are both mapped to UART TX pin.
 
-; The general idea is 
+; The general idea is that we run at 5x oversampling, i.e. 5 PIO instruction cycles
+; per bit on the wire.  This is the minimum oversampling stated by ISO 7816-3
+
 
 	pull	   side 1 [4]	; Assert stop bit, or stall with line in idle state
 	set x, 8   side 0 [4]	; Preload bit counter, assert start bit for 5 clocks
@@ -51,12 +53,18 @@ static inline void iso7816_tx_program_init(PIO pio, uint sm, uint offset, uint p
 	pio_sm_set_enabled(pio, sm, true);
 }
 
+static inline void iso7816_tx_enable(PIO pio, uint sm, bool enable)
+{
+	pio_sm_set_enabled(pio, sm, enable);
+	// FIXME: change GPIO to input
+}
+
 static inline bool compute_even_parity_bit(uint8_t x)
 {
-        x ^= x >> 4;
-        x ^= x >> 2;
-        x ^= x >> 1;
-        return (x) & 1;
+	x ^= x >> 4;
+	x ^= x >> 2;
+	x ^= x >> 1;
+	return (x) & 1;
 }
 
 static inline void iso7816_tx_program_putc(PIO pio, uint sm, uint8_t c)
@@ -75,4 +83,20 @@ static inline void iso7816_tx_program_puts(PIO pio, uint sm, const uint8_t *s, s
 		iso7816_tx_program_putc(pio, sm, *s++);
 }
 
+static inline bool iso7816_tx_is_idle(PIO pio, uint sm)
+{
+	/* how do we know once the PIO unit has completed tx of a character?
+	   - state machine TX fifo must be empty (FSTAT:TXEMPTY), AND
+	   - FDEBUG:TXSTALL bit must be set
+	*/
+
+	/* clear the sticky STALL flag */
+	pio->fdebug |= (sm << 24);
+	if (pio->fstat & (sm << 24) && pio->fdebug & (sm << 24))
+		return true;
+	else
+		return false;
+}
+
+
 %}

iso7816/mode_cardemu.c

@@ -0,0 +1,48 @@
+#include "card_emu.h"
+
+/* call-back from card_emu.c to wait for UART Tx to become idle */
+void card_emu_uart_wait_tx_idle(uint8_t uart_chan)
+{
+	/* FIXME: how do we know once the PIO unit has completed tx of a character? */
+}
+
+/* call-back from card_emu.c to enable/disable transmit and/or receive */
+void card_emu_uart_enable(uint8_t uart_chan, uint8_t rxtx)
+{
+	switch (rxtx) {
+	case ENABLE_TX:
+		//iso7816_tx_enable(pio, sm, true);
+		break;
+	case ENABLE_TX_TIMER_ONLY:
+		break;
+	case ENABLE_RX:
+		break;
+	case 0:
+	default:
+		break;
+}
+
+/* call-back from card_emu.c to transmit a byte */
+int card_emu_uart_tx(uint8_t uart_chan, uint8_t byte)
+{
+	//iso7816_tx_program_putc(pio, sm, byte);
+}
+
+/* call-back from card_emu.c to change UART baud rate */
+int card_emu_uart_update_fidi(uint8_t uart_chan, unsigned int fidi)
+{
+}
+
+/*! Update WT on USART peripheral.  Will automatically re-start timer with new value.
+ *  \param[in] usart USART peripheral to configure
+ *  \param[in] wt inactivity Waiting Time before card_emu_wtime_expired is called (0 to disable) */
+void card_emu_uart_update_wt(uint8_t uart_chan, uint32_t wt)
+{
+}
+
+/*! Reset and re-start waiting timeout count down on USART peripheral.
+ *  \param[in] usart USART peripheral to configure */
+void card_emu_uart_reset_wt(uint8_t uart_chan)
+{
+}
+

libosmo-rp2/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(src)

libosmo-rp2/include/osmocom/rp2/usb_buf.h

@@ -0,0 +1,42 @@
+/* USB buffer library
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#pragma once
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/msgb.h>
+
+/* buffered USB endpoint (with queue of msgb) */
+struct usb_buffered_ep {
+	/* endpoint number */
+	uint8_t ep;
+	/* OUT endpoint (1) or IN/IRQ (0)? */
+	//uint8_t out_from_host;
+	/* currently any transfer in progress? */
+	volatile uint32_t in_progress;
+	/* Tx queue (IN) / Rx queue (OUT) */
+	struct llist_head queue;
+	/* current length of queue */
+	unsigned int queue_len;
+};
+
+struct msgb *usb_buf_alloc(uint8_t ep);
+void usb_buf_free(struct msgb *msg);
+int usb_buf_submit(struct msgb *msg);
+struct llist_head *usb_get_queue(uint8_t ep);
+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);

libosmo-rp2/src/CMakeLists.txt

@@ -0,0 +1,9 @@
+file(GLOB FILES *.c *.h)
+add_library(libosmo-rp2 ${FILES})
+
+target_link_libraries(libosmo-rp2  PRIVATE
+                      libosmocore
+                      pico_stdlib
+                      cmsis_core)
+
+target_include_directories(libosmo-rp2 PUBLIC ../include)

libosmo-rp2/src/usb_buf.c

@@ -0,0 +1,103 @@
+/* USB buffer library
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include "trace.h"
+#include <osmocom/rp2/usb_buf.h>
+//#include "simtrace_usb.h"
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/msgb.h>
+#include <errno.h>
+
+#define USB_ALLOC_SIZE	280
+#define USB_MAX_QLEN	3
+
+#define BOARD_USB_NUMENDPOINTS 10
+
+static struct usb_buffered_ep usb_buffered_ep[BOARD_USB_NUMENDPOINTS];
+
+struct usb_buffered_ep *usb_get_buf_ep(uint8_t ep)
+{
+	if (ep >= ARRAY_SIZE(usb_buffered_ep))
+		return NULL;
+	return &usb_buffered_ep[ep];
+}
+
+
+/***********************************************************************
+ * User API
+ ***********************************************************************/
+
+struct llist_head *usb_get_queue(uint8_t ep)
+{
+	struct usb_buffered_ep *bep = usb_get_buf_ep(ep);
+	if (!bep)
+		return NULL;
+	return &bep->queue;
+}
+
+/* allocate a USB buffer for use with given end-point */
+struct msgb *usb_buf_alloc(uint8_t ep)
+{
+	struct msgb *msg;
+
+	msg = msgb_alloc(USB_ALLOC_SIZE, "USB");
+	if (!msg)
+		return NULL;
+	msg->dst = usb_get_buf_ep(ep);
+	return msg;
+}
+
+/* release/return the USB buffer to the pool */
+void usb_buf_free(struct msgb *msg)
+{
+	msgb_free(msg);
+}
+
+/* submit a USB buffer for transmission to host */
+int usb_buf_submit(struct msgb *msg)
+{
+	struct usb_buffered_ep *ep = msg->dst;
+
+	if (!msg->dst) {
+		TRACE_ERROR("%s: msg without dst\r\n", __func__);
+		usb_buf_free(msg);
+		return -EINVAL;
+	}
+
+	/* no need for irqsafe operation, as the usb_tx_queue is
+	 * processed only by the main loop context */
+
+	if (ep->queue_len >= USB_MAX_QLEN) {
+		struct msgb *evict;
+		/* free the first pending buffer in the queue */
+		TRACE_INFO("EP%02x: dropping first queue element (qlen=%u)\r\n",
+			   ep->ep, ep->queue_len);
+		evict = msgb_dequeue_count(&ep->queue, &ep->queue_len);
+		OSMO_ASSERT(evict);
+		usb_buf_free(evict);
+	}
+
+	msgb_enqueue_count(&ep->queue, msg, &ep->queue_len);
+	return 0;
+}
+
+void usb_buf_init(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(usb_buffered_ep); i++) {
+		struct usb_buffered_ep *ep = &usb_buffered_ep[i];
+		INIT_LLIST_HEAD(&ep->queue);
+		ep->ep = i;
+	}
+}

libosmocore/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(src)

libosmocore/include/osmocom/core/bit16gen.h

@@ -0,0 +1,124 @@
+/*
+ * bit16gen.h
+ *
+ * Copyright (C) 2014  Max <max.suraev@fairwaves.co>
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#pragma once
+
+#include <osmocom/core/utils.h>
+
+/*! load unaligned n-byte integer (little-endian encoding) into uint16_t, into the least significant octets.
+ *  \param[in] p Buffer where integer is stored
+ *  \param[in] n Number of bytes stored in p
+ *  \returns 16 bit unsigned integer
+ */
+static inline uint16_t osmo_load16le_ext(const void *p, uint8_t n)
+{
+	uint8_t i;
+	uint16_t r = 0;
+	const uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(r));
+	for(i = 0; i < n; r |= ((uint16_t)q[i] << (8 * i)), i++);
+	return r;
+}
+
+/*! load unaligned n-byte integer (big-endian encoding) into uint16_t, into the MOST significant octets.
+ * WARNING: for n < sizeof(uint16_t), the result is not returned in the least significant octets, as one might expect.
+ * To always return the same value as fed to osmo_store16be_ext() before, use osmo_load16be_ext_2().
+ *  \param[in] p Buffer where integer is stored
+ *  \param[in] n Number of bytes stored in p
+ *  \returns 16 bit unsigned integer
+ */
+static inline uint16_t osmo_load16be_ext(const void *p, uint8_t n)
+{
+	uint8_t i;
+	uint16_t r = 0;
+	const uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(r));
+	for(i = 0; i < n; r |= ((uint16_t)q[i] << (16 - 8* (1 + i))), i++);
+	return r;
+}
+
+/*! load unaligned n-byte integer (big-endian encoding) into uint16_t, into the least significant octets.
+ *  \param[in] p Buffer where integer is stored
+ *  \param[in] n Number of bytes stored in p
+ *  \returns 16 bit unsigned integer
+ */
+static inline uint16_t osmo_load16be_ext_2(const void *p, uint8_t n)
+{
+	uint8_t i;
+	uint16_t r = 0;
+	const uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(r));
+	for(i = 0; i < n; r |= ((uint16_t)q[i] << (16 - 8* (1 + i + (sizeof(r) - n)))), i++);
+	return r;
+}
+
+
+/*! store unaligned n-byte integer (little-endian encoding) from uint16_t
+ *  \param[in] x unsigned 16 bit integer
+ *  \param[out] p Buffer to store integer
+ *  \param[in] n Number of bytes to store
+ */
+static inline void osmo_store16le_ext(uint16_t x, void *p, uint8_t n)
+{
+	uint8_t i;
+	uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(x));
+	for(i = 0; i < n; q[i] = (x >> i * 8) & 0xFF, i++);
+}
+
+/*! store unaligned n-byte integer (big-endian encoding) from uint16_t
+ *  \param[in] x unsigned 16 bit integer
+ *  \param[out] p Buffer to store integer
+ *  \param[in] n Number of bytes to store
+ */
+static inline void osmo_store16be_ext(uint16_t x, void *p, uint8_t n)
+{
+	uint8_t i;
+	uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(x));
+	for(i = 0; i < n; q[i] = (x >> ((n - 1 - i) * 8)) & 0xFF, i++);
+}
+
+
+/* Convenience function for most-used cases */
+
+
+/*! load unaligned 16-bit integer (little-endian encoding) */
+static inline uint16_t osmo_load16le(const void *p)
+{
+	return osmo_load16le_ext(p, 16 / 8);
+}
+
+/*! load unaligned 16-bit integer (big-endian encoding) */
+static inline uint16_t osmo_load16be(const void *p)
+{
+	return osmo_load16be_ext(p, 16 / 8);
+}
+
+
+/*! store unaligned 16-bit integer (little-endian encoding) */
+static inline void osmo_store16le(uint16_t x, void *p)
+{
+	osmo_store16le_ext(x, p, 16 / 8);
+}
+
+/*! store unaligned 16-bit integer (big-endian encoding) */
+static inline void osmo_store16be(uint16_t x, void *p)
+{
+	osmo_store16be_ext(x, p, 16 / 8);
+}

libosmocore/include/osmocom/core/bit32gen.h

@@ -0,0 +1,124 @@
+/*
+ * bit32gen.h
+ *
+ * Copyright (C) 2014  Max <max.suraev@fairwaves.co>
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#pragma once
+
+#include <osmocom/core/utils.h>
+
+/*! load unaligned n-byte integer (little-endian encoding) into uint32_t, into the least significant octets.
+ *  \param[in] p Buffer where integer is stored
+ *  \param[in] n Number of bytes stored in p
+ *  \returns 32 bit unsigned integer
+ */
+static inline uint32_t osmo_load32le_ext(const void *p, uint8_t n)
+{
+	uint8_t i;
+	uint32_t r = 0;
+	const uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(r));
+	for(i = 0; i < n; r |= ((uint32_t)q[i] << (8 * i)), i++);
+	return r;
+}
+
+/*! load unaligned n-byte integer (big-endian encoding) into uint32_t, into the MOST significant octets.
+ * WARNING: for n < sizeof(uint32_t), the result is not returned in the least significant octets, as one might expect.
+ * To always return the same value as fed to osmo_store32be_ext() before, use osmo_load32be_ext_2().
+ *  \param[in] p Buffer where integer is stored
+ *  \param[in] n Number of bytes stored in p
+ *  \returns 32 bit unsigned integer
+ */
+static inline uint32_t osmo_load32be_ext(const void *p, uint8_t n)
+{
+	uint8_t i;
+	uint32_t r = 0;
+	const uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(r));
+	for(i = 0; i < n; r |= ((uint32_t)q[i] << (32 - 8* (1 + i))), i++);
+	return r;
+}
+
+/*! load unaligned n-byte integer (big-endian encoding) into uint32_t, into the least significant octets.
+ *  \param[in] p Buffer where integer is stored
+ *  \param[in] n Number of bytes stored in p
+ *  \returns 32 bit unsigned integer
+ */
+static inline uint32_t osmo_load32be_ext_2(const void *p, uint8_t n)
+{
+	uint8_t i;
+	uint32_t r = 0;
+	const uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(r));
+	for(i = 0; i < n; r |= ((uint32_t)q[i] << (32 - 8* (1 + i + (sizeof(r) - n)))), i++);
+	return r;
+}
+
+
+/*! store unaligned n-byte integer (little-endian encoding) from uint32_t
+ *  \param[in] x unsigned 32 bit integer
+ *  \param[out] p Buffer to store integer
+ *  \param[in] n Number of bytes to store
+ */
+static inline void osmo_store32le_ext(uint32_t x, void *p, uint8_t n)
+{
+	uint8_t i;
+	uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(x));
+	for(i = 0; i < n; q[i] = (x >> i * 8) & 0xFF, i++);
+}
+
+/*! store unaligned n-byte integer (big-endian encoding) from uint32_t
+ *  \param[in] x unsigned 32 bit integer
+ *  \param[out] p Buffer to store integer
+ *  \param[in] n Number of bytes to store
+ */
+static inline void osmo_store32be_ext(uint32_t x, void *p, uint8_t n)
+{
+	uint8_t i;
+	uint8_t *q = (uint8_t *)p;
+	OSMO_ASSERT(n <= sizeof(x));
+	for(i = 0; i < n; q[i] = (x >> ((n - 1 - i) * 8)) & 0xFF, i++);
+}
+
+
+/* Convenience function for most-used cases */
+
+
+/*! load unaligned 32-bit integer (little-endian encoding) */
+static inline uint32_t osmo_load32le(const void *p)
+{
+	return osmo_load32le_ext(p, 32 / 8);
+}
+
+/*! load unaligned 32-bit integer (big-endian encoding) */
+static inline uint32_t osmo_load32be(const void *p)
+{
+	return osmo_load32be_ext(p, 32 / 8);
+}
+
+
+/*! store unaligned 32-bit integer (little-endian encoding) */
+static inline void osmo_store32le(uint32_t x, void *p)
+{
+	osmo_store32le_ext(x, p, 32 / 8);
+}
+
+/*! store unaligned 32-bit integer (big-endian encoding) */
+static inline void osmo_store32be(uint32_t x, void *p)
+{
+	osmo_store32be_ext(x, p, 32 / 8);
+}

libosmocore/include/osmocom/core/linuxlist.h

@@ -0,0 +1,650 @@
+/*! \file linuxlist.h
+ *
+ * Simple doubly linked list implementation.
+ *
+ * Some of the internal functions ("__xxx") are useful when
+ * manipulating whole llists rather than single entries, as
+ * sometimes we already know the next/prev entries and we can
+ * generate better code by using them directly rather than
+ * using the generic single-entry routines.
+ */
+
+#pragma once
+
+/*! \defgroup linuxlist Simple doubly linked list implementation
+ *  @{
+ * \file linuxlist.h */
+
+#include <stddef.h>
+#include <stdbool.h>
+
+#ifndef inline
+#define inline __inline__
+#endif
+
+static inline void prefetch(const void *x) {;}
+
+/*! Cast a member of a structure out to the containing structure.
+ *  \param[in] ptr    the pointer to the member.
+ *  \param[in] type   the type of the container struct this is embedded in.
+ *  \param[in] member the name of the member within the struct.
+ */
+#define container_of(ptr, type, member) ({			\
+	const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
+	(type *)( (char *)__mptr - offsetof(type, member) );})
+
+
+/*!
+ * These are non-NULL pointers that will result in page faults
+ * under normal circumstances, used to verify that nobody uses
+ * non-initialized llist entries.
+ */
+#define LLIST_POISON1  ((void *) 0x00100100)
+#define LLIST_POISON2  ((void *) 0x00200200)
+
+/*! (double) linked list header structure */
+struct llist_head {
+	/*! Pointer to next and previous item */
+	struct llist_head *next, *prev;
+};
+
+/*! Define a new llist_head pointing to a given llist_head.
+ *  \param[in] name another llist_head to be pointed.
+ */
+#define LLIST_HEAD_INIT(name) { &(name), &(name) }
+
+/*! Define a statically-initialized variable of type llist_head.
+ *  \param[in] name variable (symbol) name.
+ */
+#define LLIST_HEAD(name) \
+	struct llist_head name = LLIST_HEAD_INIT(name)
+
+/*! Initialize a llist_head to point back to itself.
+ *  \param[in] ptr llist_head to be initialized.
+ */
+#define INIT_LLIST_HEAD(ptr) do { \
+	(ptr)->next = (ptr); (ptr)->prev = (ptr); \
+} while (0)
+
+/*
+ * Insert a new entry between two known consecutive entries.
+ *
+ * This is only for internal llist manipulation where we know
+ * the prev/next entries already!
+ */
+static inline void __llist_add(struct llist_head *_new,
+			       struct llist_head *prev,
+			       struct llist_head *next)
+{
+	next->prev = _new;
+	_new->next = next;
+	_new->prev = prev;
+	prev->next = _new;
+}
+
+/*! Add a new entry into a linked list (at head).
+ *  \param _new the entry to be added.
+ *  \param head llist_head to prepend the element to.
+ *
+ * Insert a new entry after the specified head.
+ * This is good for implementing stacks.
+ */
+static inline void llist_add(struct llist_head *_new, struct llist_head *head)
+{
+	__llist_add(_new, head, head->next);
+}
+
+/*! Add a new entry into a linked list (at tail).
+ *  \param _new the entry to be added.
+ *  \param head llist_head to append the element to.
+ *
+ * Insert a new entry before the specified head.
+ * This is useful for implementing queues.
+ */
+static inline void llist_add_tail(struct llist_head *_new, struct llist_head *head)
+{
+	__llist_add(_new, head->prev, head);
+}
+
+/*
+ * Delete a llist entry by making the prev/next entries
+ * point to each other.
+ *
+ * This is only for internal llist manipulation where we know
+ * the prev/next entries already!
+ */
+static inline void __llist_del(struct llist_head * prev, struct llist_head * next)
+{
+	next->prev = prev;
+	prev->next = next;
+}
+
+/*! Delete a single entry from a linked list.
+ *  \param entry the element to delete.
+ *
+ * Note: llist_empty on entry does not return true after this, the entry is
+ * in an undefined state.
+ */
+static inline void llist_del(struct llist_head *entry)
+{
+	__llist_del(entry->prev, entry->next);
+	entry->next = (struct llist_head *)LLIST_POISON1;
+	entry->prev = (struct llist_head *)LLIST_POISON2;
+}
+
+/*! Delete a single entry from a linked list and reinitialize it.
+ *  \param entry the element to delete and reinitialize.
+ */
+static inline void llist_del_init(struct llist_head *entry)
+{
+	__llist_del(entry->prev, entry->next);
+	INIT_LLIST_HEAD(entry);
+}
+
+/*! Delete from one llist and add as another's head.
+ *  \param llist the entry to move.
+ *  \param head  the head that will precede our entry.
+ */
+static inline void llist_move(struct llist_head *llist, struct llist_head *head)
+{
+	__llist_del(llist->prev, llist->next);
+	llist_add(llist, head);
+}
+
+/*! Delete from one llist and add as another's tail.
+ *  \param llist the entry to move.
+ *  \param head  the head that will follow our entry.
+ */
+static inline void llist_move_tail(struct llist_head *llist,
+				   struct llist_head *head)
+{
+	__llist_del(llist->prev, llist->next);
+	llist_add_tail(llist, head);
+}
+
+/*! Test whether a linked list is empty.
+ *  \param[in] head the llist to test.
+ *  \returns 1 if the list is empty, 0 otherwise.
+ */
+static inline int llist_empty(const struct llist_head *head)
+{
+	return head->next == head;
+}
+
+static inline void __llist_splice(struct llist_head *llist,
+				  struct llist_head *head)
+{
+	struct llist_head *first = llist->next;
+	struct llist_head *last = llist->prev;
+	struct llist_head *at = head->next;
+
+	first->prev = head;
+	head->next = first;
+
+	last->next = at;
+	at->prev = last;
+}
+
+/*! Join two linked lists.
+ *  \param llist the new linked list to add.
+ *  \param head  the place to add llist within the other list.
+ */
+static inline void llist_splice(struct llist_head *llist, struct llist_head *head)
+{
+	if (!llist_empty(llist))
+		__llist_splice(llist, head);
+}
+
+/*! Join two llists and reinitialise the emptied llist.
+ *  \param llist the new linked list to add.
+ *  \param head  the place to add it within the first llist.
+ *
+ * The llist is reinitialised.
+ */
+static inline void llist_splice_init(struct llist_head *llist,
+				     struct llist_head *head)
+{
+	if (!llist_empty(llist)) {
+		__llist_splice(llist, head);
+		INIT_LLIST_HEAD(llist);
+	}
+}
+
+/*! Get the struct containing this list entry.
+ *  \param ptr    the llist_head pointer.
+ *  \param type   the type of the struct this is embedded in.
+ *  \param member the name of the llist_head within the struct.
+ */
+#define llist_entry(ptr, type, member) \
+	container_of(ptr, type, member)
+
+/*! Get the first element from a linked list.
+ *  \param ptr    the list head to take the element from.
+ *  \param type   the type of the struct this is embedded in.
+ *  \param member the name of the list_head within the struct.
+ *
+ * Note, that list is expected to be not empty.
+ */
+#define llist_first_entry(ptr, type, member) \
+	llist_entry((ptr)->next, type, member)
+
+/*! Get the last element from a list.
+ *  \param ptr    the list head to take the element from.
+ *  \param type   the type of the struct this is embedded in.
+ *  \param member the name of the llist_head within the struct.
+ *
+ * Note, that list is expected to be not empty.
+ */
+#define llist_last_entry(ptr, type, member) \
+	llist_entry((ptr)->prev, type, member)
+
+/*! Return the last element of the list.
+ *  \param head the llist head of the list.
+ *  \returns last element of the list, head if the list is empty.
+ */
+#define llist_last(head) (head)->prev
+
+/*! Get the first element from a list, or NULL.
+ *  \param ptr    the list head to take the element from.
+ *  \param type   the type of the struct this is embedded in.
+ *  \param member the name of the list_head within the struct.
+ *
+ * Note that if the list is empty, it returns NULL.
+ */
+#define llist_first_entry_or_null(ptr, type, member) \
+	(!llist_empty(ptr) ? llist_first_entry(ptr, type, member) : NULL)
+
+/*! Iterate over a linked list.
+ *  \param pos  the llist_head to use as a loop counter.
+ *  \param head the head of the list over which to iterate.
+ */
+#define llist_for_each(pos, head) \
+	for (pos = (head)->next, prefetch(pos->next); pos != (head); \
+		pos = pos->next, prefetch(pos->next))
+
+/*! Iterate over a linked list (no prefetch).
+ *  \param pos  the llist_head to use as a loop counter.
+ *  \param head the head of the list over which to iterate.
+ *
+ * This variant differs from llist_for_each() in that it's the
+ * simplest possible llist iteration code, no prefetching is done.
+ * Use this for code that knows the llist to be very short (empty
+ * or 1 entry) most of the time.
+ */
+#define __llist_for_each(pos, head) \
+	for (pos = (head)->next; pos != (head); pos = pos->next)
+
+/*! Iterate over a linked list backwards.
+ *  \param pos  the llist_head to use as a loop counter.
+ *  \param head the head of the list over which to iterate.
+ */
+#define llist_for_each_prev(pos, head) \
+	for (pos = (head)->prev, prefetch(pos->prev); pos != (head); \
+		pos = pos->prev, prefetch(pos->prev))
+
+/*! Iterate over a linked list, safe against removal of llist entry.
+ *  \param pos  the llist_head to use as a loop counter.
+ *  \param n    another llist_head to use as temporary storage.
+ *  \param head the head of the list over which to iterate.
+ */
+#define llist_for_each_safe(pos, n, head) \
+	for (pos = (head)->next, n = pos->next; pos != (head); \
+		pos = n, n = pos->next)
+
+/*! Iterate over a linked list of a given type.
+ *  \param pos    the 'type *' to use as a loop counter.
+ *  \param head   the head of the list over which to iterate.
+ *  \param member the name of the llist_head within the struct pos.
+ */
+#define llist_for_each_entry(pos, head, member)				\
+	for (pos = llist_entry((head)->next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head); 					\
+	     pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next))
+
+/*! Iterate backwards over a linked list of a given type.
+ *  \param pos    the 'type *' to use as a loop counter.
+ *  \param head   the head of the list over which to iterate.
+ *  \param member the name of the llist_head within the struct pos.
+ */
+#define llist_for_each_entry_reverse(pos, head, member)			\
+	for (pos = llist_entry((head)->prev, typeof(*pos), member),	\
+		     prefetch(pos->member.prev);			\
+	     &pos->member != (head); 					\
+	     pos = llist_entry(pos->member.prev, typeof(*pos), member),	\
+		     prefetch(pos->member.prev))
+
+/*! Iterate over a linked list of a given type,
+ *  continuing after an existing point.
+ *  \param pos    the 'type *' to use as a loop counter.
+ *  \param head   the head of the list over which to iterate.
+ *  \param member the name of the llist_head within the struct pos.
+ */
+#define llist_for_each_entry_continue(pos, head, member) 		\
+	for (pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head);					\
+	     pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next))
+
+/*! Iterate over llist of given type, safe against removal of llist entry.
+ *  \param pos    the 'type *' to use as a loop counter.
+ *  \param n      another 'type *' to use as temporary storage.
+ *  \param head   the head of the list over which to iterate.
+ *  \param member the name of the llist_head within the struct pos.
+ */
+#define llist_for_each_entry_safe(pos, n, head, member)			\
+	for (pos = llist_entry((head)->next, typeof(*pos), member),	\
+		n = llist_entry(pos->member.next, typeof(*pos), member);	\
+	     &pos->member != (head); 					\
+	     pos = n, n = llist_entry(n->member.next, typeof(*n), member))
+
+/*! Iterate over an rcu-protected llist.
+ *  \param pos  the llist_head to use as a loop counter.
+ *  \param head the head of the list over which to iterate.
+ */
+#define llist_for_each_rcu(pos, head) \
+	for (pos = (head)->next, prefetch(pos->next); pos != (head); \
+		pos = pos->next, ({ smp_read_barrier_depends(); 0;}), prefetch(pos->next))
+
+#define __llist_for_each_rcu(pos, head) \
+	for (pos = (head)->next; pos != (head); \
+		pos = pos->next, ({ smp_read_barrier_depends(); 0;}))
+
+/*! Iterate over an rcu-protected llist, safe against removal of llist entry.
+ *  \param pos  the llist_head to use as a loop counter.
+ *  \param n    another llist_head to use as temporary storage.
+ *  \param head the head of the list over which to iterate.
+ */
+#define llist_for_each_safe_rcu(pos, n, head) \
+	for (pos = (head)->next, n = pos->next; pos != (head); \
+		pos = n, ({ smp_read_barrier_depends(); 0;}), n = pos->next)
+
+/*! Iterate over an rcu-protected llist of a given type.
+ *  \param pos    the 'type *' to use as a loop counter.
+ *  \param head   the head of the list over which to iterate.
+ *  \param member the name of the llist_struct within the struct.
+ */
+#define llist_for_each_entry_rcu(pos, head, member)			\
+	for (pos = llist_entry((head)->next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head); 					\
+	     pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     ({ smp_read_barrier_depends(); 0;}),		\
+		     prefetch(pos->member.next))
+
+
+/*! Iterate over an rcu-protected llist, continuing after existing point.
+ *  \param pos  the llist_head to use as a loop counter.
+ *  \param head the head of the list over which to iterate.
+ */
+#define llist_for_each_continue_rcu(pos, head) \
+	for ((pos) = (pos)->next, prefetch((pos)->next); (pos) != (head); \
+		(pos) = (pos)->next, ({ smp_read_barrier_depends(); 0;}), prefetch((pos)->next))
+
+/*! Count number of llist items by iterating.
+ *  \param head the llist head to count items of.
+ *  \returns Number of items.
+ *
+ * This function is not efficient, mostly useful for small lists and non time
+ * critical cases like unit tests.
+ */
+static inline unsigned int llist_count(const struct llist_head *head)
+{
+	struct llist_head *entry;
+	unsigned int i = 0;
+	llist_for_each(entry, head)
+		i++;
+	return i;
+}
+
+
+
+/*! Double linked lists with a single pointer list head.
+ * Mostly useful for hash tables where the two pointer list head is
+ * too wasteful.
+ * You lose the ability to access the tail in O(1).
+ */
+
+struct hlist_head {
+	struct hlist_node *first;
+};
+
+struct hlist_node {
+	struct hlist_node *next, **pprev;
+};
+
+#define HLIST_HEAD_INIT { .first = NULL }
+#define HLIST_HEAD(name) struct hlist_head name = {  .first = NULL }
+#define INIT_HLIST_HEAD(ptr) ((ptr)->first = NULL)
+static inline void INIT_HLIST_NODE(struct hlist_node *h)
+{
+	h->next = NULL;
+	h->pprev = NULL;
+}
+
+#define READ_ONCE(x)	x
+#define WRITE_ONCE(a, b) a = b
+
+/*! Has node been removed from list and reinitialized?.
+ * \param[in] h: Node to be checked
+ * \return 1 if node is unhashed; 0 if not
+ *
+ * Not that not all removal functions will leave a node in unhashed
+ * state.  For example, hlist_nulls_del_init_rcu() does leave the
+ * node in unhashed state, but hlist_nulls_del() does not.
+ */
+static inline int hlist_unhashed(const struct hlist_node *h)
+{
+	return !h->pprev;
+}
+
+/*! Version of hlist_unhashed for lockless use.
+ * \param[in] n Node to be checked
+ * \return 1 if node is unhashed; 0 if not
+ *
+ * This variant of hlist_unhashed() must be used in lockless contexts
+ * to avoid potential load-tearing.  The READ_ONCE() is paired with the
+ * various WRITE_ONCE() in hlist helpers that are defined below.
+ */
+static inline int hlist_unhashed_lockless(const struct hlist_node *h)
+{
+	return !READ_ONCE(h->pprev);
+}
+
+/*!Is the specified hlist_head structure an empty hlist?.
+ * \param[in] h Structure to check.
+ * \return 1 if hlist is empty; 0 if not
+ */
+static inline int hlist_empty(const struct hlist_head *h)
+{
+	return !READ_ONCE(h->first);
+}
+
+static inline void __hlist_del(struct hlist_node *n)
+{
+	struct hlist_node *next = n->next;
+	struct hlist_node **pprev = n->pprev;
+
+	WRITE_ONCE(*pprev, next);
+	if (next)
+		WRITE_ONCE(next->pprev, pprev);
+}
+
+/*! Delete the specified hlist_node from its list.
+ *  \param[in] n: Node to delete.
+ *
+ * Note that this function leaves the node in hashed state.  Use
+ * hlist_del_init() or similar instead to unhash @n.
+ */
+static inline void hlist_del(struct hlist_node *n)
+{
+	__hlist_del(n);
+	n->next = (struct hlist_node *)LLIST_POISON1;
+	n->pprev = (struct hlist_node **)LLIST_POISON2;
+}
+
+/*! Delete the specified hlist_node from its list and initialize.
+ * \param[in] n Node to delete.
+ *
+ * Note that this function leaves the node in unhashed state.
+ */
+static inline void hlist_del_init(struct hlist_node *n)
+{
+	if (!hlist_unhashed(n)) {
+		__hlist_del(n);
+		INIT_HLIST_NODE(n);
+	}
+}
+
+/*! add a new entry at the beginning of the hlist.
+ * \param[in] n new entry to be added
+ * \param[in] h hlist head to add it after
+ *
+ * Insert a new entry after the specified head.
+ * This is good for implementing stacks.
+ */
+static inline void hlist_add_head(struct hlist_node *n, struct hlist_head *h)
+{
+	struct hlist_node *first = h->first;
+	WRITE_ONCE(n->next, first);
+	if (first)
+		WRITE_ONCE(first->pprev, &n->next);
+	WRITE_ONCE(h->first, n);
+	WRITE_ONCE(n->pprev, &h->first);
+}
+
+/*! add a new entry before the one specified.
+ * @n: new entry to be added
+ * @next: hlist node to add it before, which must be non-NULL
+ */
+static inline void hlist_add_before(struct hlist_node *n,
+				    struct hlist_node *next)
+{
+	WRITE_ONCE(n->pprev, next->pprev);
+	WRITE_ONCE(n->next, next);
+	WRITE_ONCE(next->pprev, &n->next);
+	WRITE_ONCE(*(n->pprev), n);
+}
+
+/*! add a new entry after the one specified
+ * \param[in] n new entry to be added
+ * \param[in] prev hlist node to add it after, which must be non-NULL
+ */
+static inline void hlist_add_behind(struct hlist_node *n,
+				    struct hlist_node *prev)
+{
+	WRITE_ONCE(n->next, prev->next);
+	WRITE_ONCE(prev->next, n);
+	WRITE_ONCE(n->pprev, &prev->next);
+
+	if (n->next)
+		WRITE_ONCE(n->next->pprev, &n->next);
+}
+
+/*! create a fake hlist consisting of a single headless node.
+ * \param[in] n Node to make a fake list out of
+ *
+ * This makes @n appear to be its own predecessor on a headless hlist.
+ * The point of this is to allow things like hlist_del() to work correctly
+ * in cases where there is no list.
+ */
+static inline void hlist_add_fake(struct hlist_node *n)
+{
+	n->pprev = &n->next;
+}
+
+/*! Is this node a fake hlist?.
+ * \param[in] h Node to check for being a self-referential fake hlist.
+ */
+static inline bool hlist_fake(struct hlist_node *h)
+{
+	return h->pprev == &h->next;
+}
+
+/*!is node the only element of the specified hlist?.
+ * \param[in] n Node to check for singularity.
+ * \param[in] h Header for potentially singular list.
+ *
+ * Check whether the node is the only node of the head without
+ * accessing head, thus avoiding unnecessary cache misses.
+ */
+static inline bool
+hlist_is_singular_node(struct hlist_node *n, struct hlist_head *h)
+{
+	return !n->next && n->pprev == &h->first;
+}
+
+/*! Move an hlist.
+ * \param[in] old hlist_head for old list.
+ * \param[in] new hlist_head for new list.
+ *
+ * Move a list from one list head to another. Fixup the pprev
+ * reference of the first entry if it exists.
+ */
+static inline void hlist_move_list(struct hlist_head *old,
+				   struct hlist_head *_new)
+{
+	_new->first = old->first;
+	if (_new->first)
+		_new->first->pprev = &_new->first;
+	old->first = NULL;
+}
+
+#define hlist_entry(ptr, type, member) container_of(ptr,type,member)
+
+#define hlist_for_each(pos, head) \
+	for (pos = (head)->first; pos ; pos = pos->next)
+
+#define hlist_for_each_safe(pos, n, head) \
+	for (pos = (head)->first; pos && ({ n = pos->next; 1; }); \
+	     pos = n)
+
+#define hlist_entry_safe(ptr, type, member) \
+	({ typeof(ptr) ____ptr = (ptr); \
+	   ____ptr ? hlist_entry(____ptr, type, member) : NULL; \
+	})
+
+/*! iterate over list of given type.
+ * \param[out] pos	the type * to use as a loop cursor.
+ * \param[in] head	the head for your list.
+ * \param[in] member	the name of the hlist_node within the struct.
+ */
+#define hlist_for_each_entry(pos, head, member)				\
+	for (pos = hlist_entry_safe((head)->first, typeof(*(pos)), member);\
+	     pos;							\
+	     pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member))
+
+/*! iterate over a hlist continuing after current point.
+ * \param[out] pos	the type * to use as a loop cursor.
+ * \param[in] member	the name of the hlist_node within the struct.
+ */
+#define hlist_for_each_entry_continue(pos, member)			\
+	for (pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member);\
+	     pos;							\
+	     pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member))
+
+/*! iterate over a hlist continuing from current point.
+ * \param[out] pos	the type * to use as a loop cursor.
+ * \param[in] member	the name of the hlist_node within the struct.
+ */
+#define hlist_for_each_entry_from(pos, member)				\
+	for (; pos;							\
+	     pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member))
+
+/*! hlist_for_each_entry_safe - iterate over list of given type safe against removal of list entry.
+ * \param[out] pos the type * to use as a loop cursor.
+ * \param[out] n a &struct hlist_node to use as temporary storage
+ * \param[in] head the head for your list.
+ * \param[in] member the name of the hlist_node within the struct
+ */
+#define hlist_for_each_entry_safe(pos, n, head, member) 		\
+	for (pos = hlist_entry_safe((head)->first, typeof(*pos), member);\
+	     pos && ({ n = pos->member.next; 1; });			\
+	     pos = hlist_entry_safe(n, typeof(*pos), member))
+
+
+/*!
+ *  @}
+ */

libosmocore/include/osmocom/core/linuxrbtree.h

@@ -0,0 +1,153 @@
+/*
+  Red Black Trees
+  (C) 1999  Andrea Arcangeli <andrea@suse.de>
+  
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  linux/include/linux/rbtree.h
+
+  To use rbtrees you'll have to implement your own insert and search cores.
+  This will avoid us to use callbacks and to drop drammatically performances.
+  I know it's not the cleaner way,  but in C (not in C++) to get
+  performances and genericity...
+
+  Some example of insert and search follows here. The search is a plain
+  normal search over an ordered tree. The insert instead must be implemented
+  int two steps: as first thing the code must insert the element in
+  order as a red leaf in the tree, then the support library function
+  rb_insert_color() must be called. Such function will do the
+  not trivial work to rebalance the rbtree if necessary.
+
+-----------------------------------------------------------------------
+static inline struct page * rb_search_page_cache(struct inode * inode,
+						 unsigned long offset)
+{
+	struct rb_node * n = inode->i_rb_page_cache.rb_node;
+	struct page * page;
+
+	while (n)
+	{
+		page = rb_entry(n, struct page, rb_page_cache);
+
+		if (offset < page->offset)
+			n = n->rb_left;
+		else if (offset > page->offset)
+			n = n->rb_right;
+		else
+			return page;
+	}
+	return NULL;
+}
+
+static inline struct page * __rb_insert_page_cache(struct inode * inode,
+						   unsigned long offset,
+						   struct rb_node * node)
+{
+	struct rb_node ** p = &inode->i_rb_page_cache.rb_node;
+	struct rb_node * parent = NULL;
+	struct page * page;
+
+	while (*p)
+	{
+		parent = *p;
+		page = rb_entry(parent, struct page, rb_page_cache);
+
+		if (offset < page->offset)
+			p = &(*p)->rb_left;
+		else if (offset > page->offset)
+			p = &(*p)->rb_right;
+		else
+			return page;
+	}
+
+	rb_link_node(node, parent, p);
+
+	return NULL;
+}
+
+static inline struct page * rb_insert_page_cache(struct inode * inode,
+						 unsigned long offset,
+						 struct rb_node * node)
+{
+	struct page * ret;
+	if ((ret = __rb_insert_page_cache(inode, offset, node)))
+		goto out;
+	rb_insert_color(node, &inode->i_rb_page_cache);
+ out:
+	return ret;
+}
+-----------------------------------------------------------------------
+*/
+
+#pragma once
+
+#include <stdlib.h>
+
+struct rb_node
+{
+	unsigned long  rb_parent_color;
+#define	RB_RED		0
+#define	RB_BLACK	1
+	struct rb_node *rb_right;
+	struct rb_node *rb_left;
+} __attribute__((aligned(sizeof(long))));
+    /* The alignment might seem pointless, but allegedly CRIS needs it */
+
+struct rb_root
+{
+	struct rb_node *rb_node;
+};
+
+
+#define rb_parent(r)   ((struct rb_node *)((r)->rb_parent_color & ~3))
+#define rb_color(r)   ((r)->rb_parent_color & 1)
+#define rb_is_red(r)   (!rb_color(r))
+#define rb_is_black(r) rb_color(r)
+#define rb_set_red(r)  do { (r)->rb_parent_color &= ~1; } while (0)
+#define rb_set_black(r)  do { (r)->rb_parent_color |= 1; } while (0)
+
+static inline void rb_set_parent(struct rb_node *rb, struct rb_node *p)
+{
+	rb->rb_parent_color = (rb->rb_parent_color & 3) | (unsigned long)p;
+}
+static inline void rb_set_color(struct rb_node *rb, int color)
+{
+	rb->rb_parent_color = (rb->rb_parent_color & ~1) | color;
+}
+
+#define RB_ROOT	{ NULL, }
+#define	rb_entry(ptr, type, member) container_of(ptr, type, member)
+
+#define RB_EMPTY_ROOT(root)	((root)->rb_node == NULL)
+#define RB_EMPTY_NODE(node)	(rb_parent(node) == node)
+#define RB_CLEAR_NODE(node)	(rb_set_parent(node, node))
+
+extern void rb_insert_color(struct rb_node *, struct rb_root *);
+extern void rb_erase(struct rb_node *, struct rb_root *);
+
+/* Find logical next and previous nodes in a tree */
+extern struct rb_node *rb_next(const struct rb_node *);
+extern struct rb_node *rb_prev(const struct rb_node *);
+extern struct rb_node *rb_first(const struct rb_root *);
+extern struct rb_node *rb_last(const struct rb_root *);
+
+/* Fast replacement of a single node without remove/rebalance/add/rebalance */
+extern void rb_replace_node(struct rb_node *victim, struct rb_node *_new, 
+			    struct rb_root *root);
+
+static inline void rb_link_node(struct rb_node * node, struct rb_node * parent,
+				struct rb_node ** rb_link)
+{
+	node->rb_parent_color = (unsigned long )parent;
+	node->rb_left = node->rb_right = NULL;
+
+	*rb_link = node;
+}

libosmocore/include/osmocom/core/msgb.h

@@ -0,0 +1,551 @@
+#pragma once
+
+/* (C) 2008 by Harald Welte <laforge@gnumonks.org>
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <stdint.h>
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/core/bit16gen.h>
+#include <osmocom/core/bit32gen.h>
+//#include <osmocom/core/defs.h>
+
+/*! \defgroup msgb Message buffers
+ *  @{
+ */
+
+/*! \file msgb.h
+ *  \brief Osmocom message buffers
+ * The Osmocom message buffers are modelled after the 'struct skb'
+ * inside the Linux kernel network stack.  As they exist in userspace,
+ * they are much simplified.  However, terminology such as headroom,
+ * tailroom, push/pull/put etc. remains the same.
+ */
+
+#define MSGB_DEBUG
+
+/*! \brief Osmocom message buffer */
+struct msgb {
+	struct llist_head list; /*!< \brief linked list header */
+
+
+	/* Part of which TRX logical channel we were received / transmitted */
+	/* FIXME: move them into the control buffer */
+	union {
+		void *dst; /*!< \brief reference of origin/destination */
+		struct gsm_bts_trx *trx;
+	};
+	struct gsm_lchan *lchan; /*!< \brief logical channel */
+
+	unsigned char *l1h; /*!< \brief pointer to Layer1 header (if any) */
+	unsigned char *l2h; /*!< \brief pointer to A-bis layer 2 header: OML, RSL(RLL), NS */
+	unsigned char *l3h; /*!< \brief pointer to Layer 3 header. For OML: FOM; RSL: 04.08; GPRS: BSSGP */
+	unsigned char *l4h; /*!< \brief pointer to layer 4 header */
+
+	unsigned long cb[5]; /*!< \brief control buffer */
+
+	uint16_t data_len;   /*!< \brief length of underlying data array */
+	uint16_t len;	     /*!< \brief length of bytes used in msgb */
+
+	unsigned char *head;	/*!< \brief start of underlying memory buffer */
+	unsigned char *tail;	/*!< \brief end of message in buffer */
+	unsigned char *data;	/*!< \brief start of message in buffer */
+	unsigned char _data[0]; /*!< \brief optional immediate data array */
+};
+
+extern struct msgb *msgb_alloc(uint16_t size, const char *name);
+extern void msgb_free(struct msgb *m);
+extern void msgb_enqueue(struct llist_head *queue, struct msgb *msg);
+extern struct msgb *msgb_dequeue(struct llist_head *queue);
+extern void msgb_reset(struct msgb *m);
+uint16_t msgb_length(const struct msgb *msg);
+extern const char *msgb_hexdump(const struct msgb *msg);
+extern int msgb_resize_area(struct msgb *msg, uint8_t *area,
+	int old_size, int new_size);
+extern struct msgb *msgb_copy(const struct msgb *msg, const char *name);
+static int msgb_test_invariant(const struct msgb *msg) __attribute__((pure));
+
+/*! Free all msgbs from a queue built with msgb_enqueue().
+ * \param[in] queue  list head of a msgb queue.
+ */
+static inline void msgb_queue_free(struct llist_head *queue)
+{
+	struct msgb *msg;
+	while ((msg = msgb_dequeue(queue))) msgb_free(msg);
+}
+
+/*! Enqueue message buffer to tail of a queue and increment queue size counter
+ * \param[in] queue linked list header of queue
+ * \param[in] msg message buffer to be added to the queue
+ * \param[in] count pointer to variable holding size of the queue
+ *
+ * The function will append the specified message buffer \a msg to the queue
+ * implemented by \ref llist_head \a queue using function \ref msgb_enqueue_count,
+ * then increment \a count
+ */
+static inline void msgb_enqueue_count(struct llist_head *queue, struct msgb *msg,
+					unsigned int *count)
+{
+	msgb_enqueue(queue, msg);
+	(*count)++;
+}
+
+/*! Dequeue message buffer from head of queue and decrement queue size counter
+ * \param[in] queue linked list header of queue
+ * \param[in] count pointer to variable holding size of the queue
+ * \returns message buffer (if any) or NULL if queue empty
+ *
+ * The function will remove the first message buffer from the queue
+ * implemented by \ref llist_head \a queue using function \ref msgb_enqueue_count,
+ * and decrement \a count, all if queue is not empty.
+ */
+static inline struct msgb *msgb_dequeue_count(struct llist_head *queue,
+						unsigned int *count)
+{
+	struct msgb *msg = msgb_dequeue(queue);
+	if (msg)
+		(*count)--;
+	return msg;
+}
+
+#ifdef MSGB_DEBUG
+//#include <osmocom/core/panic.h>
+#include <stdio.h>
+#define MSGB_ABORT(msg, fmt, args ...) do {		\
+	printf("msgb(%p): " fmt, msg, ## args);	\
+	} while(0)
+#else
+#define MSGB_ABORT(msg, fmt, args ...)
+#endif
+
+/*! \brief obtain L1 header of msgb */
+#define msgb_l1(m)	((void *)(m->l1h))
+/*! \brief obtain L2 header of msgb */
+#define msgb_l2(m)	((void *)(m->l2h))
+/*! \brief obtain L3 header of msgb */
+#define msgb_l3(m)	((void *)(m->l3h))
+/*! \brief obtain SMS header of msgb */
+#define msgb_sms(m)	((void *)(m->l4h))
+
+/*! \brief determine length of L1 message
+ *  \param[in] msgb message buffer
+ *  \returns size of L1 message in bytes
+ *
+ * This function computes the number of bytes between the tail of the
+ * message and the layer 1 header.
+ */
+static inline unsigned int msgb_l1len(const struct msgb *msgb)
+{
+	return msgb->tail - (uint8_t *)msgb_l1(msgb);
+}
+
+/*! \brief determine length of L2 message
+ *  \param[in] msgb message buffer
+ *  \returns size of L2 message in bytes
+ *
+ * This function computes the number of bytes between the tail of the
+ * message and the layer 2 header.
+ */
+static inline unsigned int msgb_l2len(const struct msgb *msgb)
+{
+	return msgb->tail - (uint8_t *)msgb_l2(msgb);
+}
+
+/*! \brief determine length of L3 message
+ *  \param[in] msgb message buffer
+ *  \returns size of L3 message in bytes
+ *
+ * This function computes the number of bytes between the tail of the
+ * message and the layer 3 header.
+ */
+static inline unsigned int msgb_l3len(const struct msgb *msgb)
+{
+	return msgb->tail - (uint8_t *)msgb_l3(msgb);
+}
+
+/*! \brief determine the length of the header
+ *  \param[in] msgb message buffer
+ *  \returns number of bytes between start of buffer and start of msg
+ *
+ * This function computes the length difference between the underlying
+ * data buffer and the used section of the \a msgb.
+ */
+static inline unsigned int msgb_headlen(const struct msgb *msgb)
+{
+	return msgb->len - msgb->data_len;
+}
+
+/*! \brief determine how much tail room is left in msgb
+ *  \param[in] msgb message buffer
+ *  \returns number of bytes remaining at end of msgb
+ *
+ * This function computes the amount of octets left in the underlying
+ * data buffer after the end of the message.
+ */
+static inline int msgb_tailroom(const struct msgb *msgb)
+{
+	return (msgb->head + msgb->data_len) - msgb->tail;
+}
+
+/*! \brief determine the amount of headroom in msgb
+ *  \param[in] msgb message buffer
+ *  \returns number of bytes left ahead of message start in msgb
+ *
+ * This function computes the amount of bytes left in the underlying
+ * data buffer before the start of the actual message.
+ */
+static inline int msgb_headroom(const struct msgb *msgb)
+{
+	return (msgb->data - msgb->head);
+}
+
+/*! \brief append data to end of message buffer
+ *  \param[in] msgb message buffer
+ *  \param[in] len number of bytes to append to message
+ *  \returns pointer to start of newly-appended data
+ *
+ * This function will move the \a tail pointer of the message buffer \a
+ * len bytes further, thus enlarging the message by \a len bytes.
+ *
+ * The return value is a pointer to start of the newly added section at
+ * the end of the message and can be used for actually filling/copying
+ * data into it.
+ */
+static inline unsigned char *msgb_put(struct msgb *msgb, unsigned int len)
+{
+	unsigned char *tmp = msgb->tail;
+	if (msgb_tailroom(msgb) < (int) len)
+		MSGB_ABORT(msgb, "Not enough tailroom msgb_put (%u < %u)\n",
+			   msgb_tailroom(msgb), len);
+	msgb->tail += len;
+	msgb->len += len;
+	return tmp;
+}
+
+/*! \brief append a uint8 value to the end of the message
+ *  \param[in] msgb message buffer
+ *  \param[in] word unsigned 8bit byte to be appended
+ */
+static inline void msgb_put_u8(struct msgb *msgb, uint8_t word)
+{
+	uint8_t *space = msgb_put(msgb, 1);
+	space[0] = word & 0xFF;
+}
+
+/*! \brief append a uint16 value to the end of the message
+ *  \param[in] msgb message buffer
+ *  \param[in] word unsigned 16bit byte to be appended
+ */
+static inline void msgb_put_u16(struct msgb *msgb, uint16_t word)
+{
+	uint8_t *space = msgb_put(msgb, 2);
+	osmo_store16be(word, space);
+}
+
+/*! \brief append a uint32 value to the end of the message
+ *  \param[in] msgb message buffer
+ *  \param[in] word unsigned 32bit byte to be appended
+ */
+static inline void msgb_put_u32(struct msgb *msgb, uint32_t word)
+{
+	uint8_t *space = msgb_put(msgb, 4);
+	osmo_store32be(word, space);
+}
+
+/*! \brief remove data from end of message
+ *  \param[in] msgb message buffer
+ *  \param[in] len number of bytes to remove from end
+ */
+static inline unsigned char *msgb_get(struct msgb *msgb, unsigned int len)
+{
+	unsigned char *tmp = msgb->tail - len;
+	if (msgb_length(msgb) < len)
+		MSGB_ABORT(msgb, "msgb too small to get %u (len %u)\n",
+			   len, msgb_length(msgb));
+	msgb->tail -= len;
+	msgb->len -= len;
+	return tmp;
+}
+
+/*! \brief remove uint8 from end of message
+ *  \param[in] msgb message buffer
+ *  \returns 8bit value taken from end of msgb
+ */
+static inline uint8_t msgb_get_u8(struct msgb *msgb)
+{
+	uint8_t *space = msgb_get(msgb, 1);
+	return space[0];
+}
+
+/*! \brief remove uint16 from end of message
+ *  \param[in] msgb message buffer
+ *  \returns 16bit value taken from end of msgb
+ */
+static inline uint16_t msgb_get_u16(struct msgb *msgb)
+{
+	uint8_t *space = msgb_get(msgb, 2);
+	return osmo_load16be(space);
+}
+
+/*! \brief remove uint32 from end of message
+ *  \param[in] msgb message buffer
+ *  \returns 32bit value taken from end of msgb
+ */
+static inline uint32_t msgb_get_u32(struct msgb *msgb)
+{
+	uint8_t *space = msgb_get(msgb, 4);
+	return osmo_load32be(space);
+}
+
+/*! \brief prepend (push) some data to start of message
+ *  \param[in] msgb message buffer
+ *  \param[in] len number of bytes to pre-pend
+ *  \returns pointer to newly added portion at start of \a msgb
+ *
+ * This function moves the \a data pointer of the \ref msgb further
+ * to the front (by \a len bytes), thereby enlarging the message by \a
+ * len bytes.
+ *
+ * The return value is a pointer to the newly added section in the
+ * beginning of the message.  It can be used to fill/copy data into it.
+ */
+static inline unsigned char *msgb_push(struct msgb *msgb, unsigned int len)
+{
+	if (msgb_headroom(msgb) < (int) len)
+		MSGB_ABORT(msgb, "Not enough headroom msgb_push (%u < %u)\n",
+			   msgb_headroom(msgb), len);
+	msgb->data -= len;
+	msgb->len += len;
+	return msgb->data;
+}
+
+/*! \brief prepend a uint8 value to the head of the message
+ *  \param[in] msgb message buffer
+ *  \param[in] word unsigned 8bit byte to be prepended
+ */
+static inline void msgb_push_u8(struct msgb *msg, uint8_t word)
+{
+	uint8_t *space = msgb_push(msg, 1);
+	space[0] = word;
+}
+
+/*! \brief prepend a uint16 value to the head of the message
+ *  \param[in] msgb message buffer
+ *  \param[in] word unsigned 16bit byte to be prepended
+ */
+static inline void msgb_push_u16(struct msgb *msg, uint16_t word)
+{
+	uint16_t *space = (uint16_t *) msgb_push(msg, 2);
+	osmo_store16be(word, space);
+}
+
+/*! \brief prepend a uint32 value to the head of the message
+ *  \param[in] msgb message buffer
+ *  \param[in] word unsigned 32bit byte to be prepended
+ */
+static inline void msgb_push_u32(struct msgb *msg, uint32_t word)
+{
+	uint32_t *space = (uint32_t *) msgb_push(msg, 4);
+	osmo_store32be(word, space);
+}
+
+/*! \brief remove (pull) a header from the front of the message buffer
+ *  \param[in] msgb message buffer
+ *  \param[in] len number of octets to be pulled
+ *  \returns pointer to new start of msgb
+ *
+ * This function moves the \a data pointer of the \ref msgb further back
+ * in the message, thereby shrinking the size of the message by \a len
+ * bytes.
+ */
+static inline unsigned char *msgb_pull(struct msgb *msgb, unsigned int len)
+{
+	msgb->len -= len;
+	return msgb->data += len;
+}
+
+/*! \brief remove (pull) all headers in front of l3h from the message buffer.
+ *  \param[in] msgb message buffer with a valid l3h
+ *  \returns pointer to new start of msgb (l3h)
+ *
+ * This function moves the \a data pointer of the \ref msgb further back
+ * in the message, thereby shrinking the size of the message.
+ * l1h and l2h will be cleared.
+ */
+static inline unsigned char *msgb_pull_to_l3(struct msgb *msg)
+{
+	unsigned char *ret = msgb_pull(msg, msg->l3h - msg->data);
+	msg->l1h = msg->l2h = NULL;
+	return ret;
+}
+
+/*! \brief remove (pull) all headers in front of l2h from the message buffer.
+ *  \param[in] msgb message buffer with a valid l2h
+ *  \returns pointer to new start of msgb (l2h)
+ *
+ * This function moves the \a data pointer of the \ref msgb further back
+ * in the message, thereby shrinking the size of the message.
+ * l1h will be cleared.
+ */
+static inline unsigned char *msgb_pull_to_l2(struct msgb *msg)
+{
+	unsigned char *ret = msgb_pull(msg, msg->l2h - msg->data);
+	msg->l1h = NULL;
+	return ret;
+}
+
+/*! \brief remove uint8 from front of message
+ *  \param[in] msgb message buffer
+ *  \returns 8bit value taken from end of msgb
+ */
+static inline uint8_t msgb_pull_u8(struct msgb *msgb)
+{
+	uint8_t *space = msgb_pull(msgb, 1) - 1;
+	return space[0];
+}
+
+/*! \brief remove uint16 from front of message
+ *  \param[in] msgb message buffer
+ *  \returns 16bit value taken from end of msgb
+ */
+static inline uint16_t msgb_pull_u16(struct msgb *msgb)
+{
+	uint8_t *space = msgb_pull(msgb, 2) - 2;
+	return osmo_load16be(space);
+}
+
+/*! \brief remove uint32 from front of message
+ *  \param[in] msgb message buffer
+ *  \returns 32bit value taken from end of msgb
+ */
+static inline uint32_t msgb_pull_u32(struct msgb *msgb)
+{
+	uint8_t *space = msgb_pull(msgb, 4) - 4;
+	return osmo_load32be(space);
+}
+
+/*! \brief Increase headroom of empty msgb, reducing the tailroom
+ *  \param[in] msg message buffer
+ *  \param[in] len amount of extra octets to be reserved as headroom
+ *
+ * This function reserves some memory at the beginning of the underlying
+ * data buffer.  The idea is to reserve space in case further headers
+ * have to be pushed to the \ref msgb during further processing.
+ *
+ * Calling this function leads to undefined reusults if it is called on
+ * a non-empty \ref msgb.
+ */
+static inline void msgb_reserve(struct msgb *msg, int len)
+{
+	msg->data += len;
+	msg->tail += len;
+}
+
+/*! \brief Trim the msgb to a given absolute length
+ *  \param[in] msg message buffer
+ *  \param[in] len new total length of buffer
+ *  \returns 0 in case of success, negative in case of error
+ */
+static inline int msgb_trim(struct msgb *msg, int len)
+{
+	if (len < 0)
+		MSGB_ABORT(msg, "Negative length is not allowed\n");
+	if (len > msg->data_len)
+		return -1;
+
+	msg->len = len;
+	msg->tail = msg->data + len;
+
+	return 0;
+}
+
+/*! \brief Trim the msgb to a given layer3 length
+ *  \param[in] msg message buffer
+ *  \param[in] l3len new layer3 length
+ *  \returns 0 in case of success, negative in case of error
+ */
+static inline int msgb_l3trim(struct msgb *msg, int l3len)
+{
+	return msgb_trim(msg, (msg->l3h - msg->data) + l3len);
+}
+
+/*! \brief Allocate message buffer with specified headroom
+ *  \param[in] size size in bytes, including headroom
+ *  \param[in] headroom headroom in bytes
+ *  \param[in] name human-readable name
+ *  \returns allocated message buffer with specified headroom
+ *
+ * This function is a convenience wrapper around \ref msgb_alloc
+ * followed by \ref msgb_reserve in order to create a new \ref msgb with
+ * user-specified amount of headroom.
+ */
+static inline struct msgb *msgb_alloc_headroom(int size, int headroom,
+						const char *name)
+{
+	assert(size > headroom);
+
+	struct msgb *msg = msgb_alloc(size, name);
+	if (msg)
+		msgb_reserve(msg, headroom);
+	return msg;
+}
+
+/*! \brief Check a message buffer for consistency
+ *  \param[in] msg message buffer
+ *  \returns 0 (false) if inconsistent, != 0 (true) otherwise
+ */
+static inline int msgb_test_invariant(const struct msgb *msg)
+{
+	const unsigned char *lbound;
+	if (!msg || !msg->data || !msg->tail ||
+	    (msg->data + msg->len != msg->tail) ||
+	    (msg->data < msg->head) ||
+	    (msg->tail > msg->head + msg->data_len))
+		return 0;
+
+	lbound = msg->head;
+
+	if (msg->l1h) {
+		if (msg->l1h < lbound)
+			return 0;
+		lbound = msg->l1h;
+	}
+	if (msg->l2h) {
+		if (msg->l2h < lbound)
+			return 0;
+		lbound = msg->l2h;
+	}
+	if (msg->l3h) {
+		if (msg->l3h < lbound)
+			return 0;
+		lbound = msg->l3h;
+	}
+	if (msg->l4h) {
+		if (msg->l4h < lbound)
+			return 0;
+		lbound = msg->l4h;
+	}
+
+	return lbound <= msg->head +  msg->data_len;
+}
+
+/* non inline functions to ease binding */
+
+uint8_t *msgb_data(const struct msgb *msg);
+
+void *msgb_talloc_ctx_init(void *root_ctx, unsigned int pool_size);
+//void msgb_set_talloc_ctx(void *ctx) OSMO_DEPRECATED("Use msgb_talloc_ctx_init() instead");
+
+/*! @} */

libosmocore/include/osmocom/core/talloc.h

@@ -0,0 +1,39 @@
+/* Memory allocation library
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#pragma once
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdarg.h>
+
+/* minimalistic emulation of core talloc API functions used by msgb.c */
+
+#define __TALLOC_STRING_LINE1__(s)    #s
+#define __TALLOC_STRING_LINE2__(s)   __TALLOC_STRING_LINE1__(s)
+#define __TALLOC_STRING_LINE3__  __TALLOC_STRING_LINE2__(__LINE__)
+#define __location__ __FILE__ ":" __TALLOC_STRING_LINE3__
+
+#define talloc_zero(ctx, type) (type *)_talloc_zero(ctx, sizeof(type), #type)
+#define talloc_zero_size(ctx, size) _talloc_zero(ctx, size, __location__)
+void *_talloc_zero(const void *ctx, size_t size, const char *name);
+
+#define talloc_free(ctx) _talloc_free(ctx, __location__)
+int _talloc_free(void *ptr, const char *location);
+
+/* Unsupported! */
+#define talloc_size(ctx, size) talloc_named_const(ctx, size, __location__)
+void *talloc_named_const(const void *context, size_t size, const char *name);
+void talloc_set_name_const(const void *ptr, const char *name);
+char *talloc_strdup(const void *t, const char *p);
+void *talloc_pool(const void *context, size_t size);
+void talloc_report(const void *ptr, FILE *f);

libosmocore/include/osmocom/core/timer.h

@@ -0,0 +1,126 @@
+/*
+ * (C) 2008, 2009 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*! \defgroup timer Osmocom timers
+ *  @{
+ */
+
+/*! \file timer.h
+ *  \brief Osmocom timer handling routines
+ */
+
+#pragma once
+
+/* override 'struct timeval' for jififes based timers */
+struct osmo_timeval {
+	unsigned long expires;
+};
+#ifdef timerisset
+#undef timerisset
+#endif
+#define timerisset(tvp)		((tvp)->expires)
+
+#ifdef timerclear
+#undef timerclear
+#endif
+#define timerclear(tvp)		(tvp)->expires = 0
+
+#ifdef timercmp
+#undef timercmp
+#endif
+#define timercmp(a, b, CMP)	(a)->expires CMP (b)->expires
+
+#ifdef timersub
+#undef timersub
+#endif
+#define timersub(a, b, result)	(result)->expires = (a)->expires - (b)->expires
+
+#ifdef timeradd
+#undef timeradd
+#endif
+#define timeradd(a, b, result)	(result)->expires = (a)->expires + (b)->expires
+struct timezone;
+
+#include <stdbool.h>
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/linuxrbtree.h>
+
+/**
+ * Timer management:
+ *      - Create a struct osmo_timer_list
+ *      - Fill out timeout and use add_timer or
+ *        use osmo_timer_schedule to schedule a timer in
+ *        x seconds and microseconds from now...
+ *      - Use osmo_timer_del to remove the timer
+ *
+ *  Internally:
+ *      - We hook into select.c to give a timeval of the
+ *        nearest timer. On already passed timers we give
+ *        it a 0 to immediately fire after the select
+ *      - osmo_timers_update will call the callbacks and
+ *        remove the timers.
+ *
+ */
+/*! \brief A structure representing a single instance of a timer */
+struct osmo_timer_list {
+	struct rb_node node;	  /*!< \brief rb-tree node header */
+	struct llist_head list;   /*!< \brief internal list header */
+	struct osmo_timeval timeout;   /*!< \brief expiration time */
+	unsigned int active  : 1; /*!< \brief is it active? */
+
+	void (*cb)(void*);	  /*!< \brief call-back called at timeout */
+	void *data;		  /*!< \brief user data for callback */
+};
+
+/**
+ * timer management
+ */
+
+void osmo_timer_setup(struct osmo_timer_list *timer, void (*cb)(void *data), void *data);
+
+void osmo_timer_add(struct osmo_timer_list *timer);
+
+void osmo_timer_schedule(struct osmo_timer_list *timer, int seconds, int microseconds);
+
+void osmo_timer_del(struct osmo_timer_list *timer);
+
+int osmo_timer_pending(struct osmo_timer_list *timer);
+
+int osmo_timer_remaining(const struct osmo_timer_list *timer,
+			 const struct osmo_timeval *now,
+			 struct osmo_timeval *remaining);
+/*
+ * internal timer list management
+ */
+struct osmo_timeval *osmo_timers_nearest(void);
+void osmo_timers_prepare(void);
+int osmo_timers_update(void);
+int osmo_timers_check(void);
+
+int osmo_gettimeofday(struct osmo_timeval *tv, struct timezone *tz);
+
+#if 0
+/**
+ * timer override
+ */
+
+extern bool osmo_gettimeofday_override;
+extern struct timeval osmo_gettimeofday_override_time;
+void osmo_gettimeofday_override_add(time_t secs, suseconds_t usecs);
+#endif
+
+/*! @} */

libosmocore/include/osmocom/core/utils.h

@@ -0,0 +1,47 @@
+/* General utilities
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#pragma once
+
+#include "pico/platform.h"
+#include "RP2040.h"
+#define ARRAY_SIZE(x)	count_of(x)
+
+#if 1
+#define local_irq_save(x)					\
+	({							\
+		x = __get_PRIMASK();				\
+	 	__disable_irq();				\
+	})
+
+#define local_irq_restore(x)					\
+	 	__set_PRIMASK(x)
+#else
+#warning "local_irq_{save,restore}() not implemented"
+#define local_irq_save(x)
+#define local_irq_restore(x)
+#endif
+
+/*! A mapping between human-readable string and numeric value */
+struct value_string {
+	uint32_t value;  /*!< numeric value */
+	const char *str; /*!< human-readable string */
+};
+
+const char *get_value_string(const struct value_string *vs, uint32_t val);
+const char *get_value_string_or_null(const struct value_string *vs,
+				     uint32_t val);
+
+int get_string_value(const struct value_string *vs, const char *str);
+
+
+#define OSMO_ASSERT(x) assert(x)

libosmocore/include/trace.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <stdio.h>
+
+/* minimal compatibility #defines to make rp2040 provide atmel-style macros */
+#define TRACE_ERROR(x, args ...)	printf(x, ## args)
+#define TRACE_INFO	TRACE_ERROR
+#define TRACE_INFO_WP	TRACE_ERROR
+#define TRACE_DEBUG	TRACE_ERROR

libosmocore/src/CMakeLists.txt

@@ -0,0 +1,8 @@
+file(GLOB FILES *.c *.h)
+add_library(libosmocore ${FILES})
+
+target_link_libraries(libosmocore  PRIVATE
+                      pico_stdlib
+                      cmsis_core)
+
+target_include_directories(libosmocore PUBLIC ../include)

libosmocore/src/msgb.c

@@ -0,0 +1,348 @@
+/* (C) 2008 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*! \addtogroup msgb
+ *  @{
+ */
+
+/*! \file msgb.c
+ */
+
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <inttypes.h>
+
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/talloc.h>
+
+void *tall_msgb_ctx = NULL;
+
+/*! \brief Allocate a new message buffer
+ * \param[in] size Length in octets, including headroom
+ * \param[in] name Human-readable name to be associated with msgb
+ * \returns dynamically-allocated \ref msgb
+ *
+ * This function allocates a 'struct msgb' as well as the underlying
+ * memory buffer for the actual message data (size specified by \a size)
+ * using the talloc memory context previously set by \ref msgb_set_talloc_ctx
+ */
+struct msgb *msgb_alloc(uint16_t size, const char *name)
+{
+	struct msgb *msg;
+
+	msg = _talloc_zero(tall_msgb_ctx, sizeof(*msg) + size, name);
+
+	if (!msg) {
+		//LOGP(DRSL, LOGL_FATAL, "unable to allocate msgb\n");
+		return NULL;
+	}
+
+	msg->data_len = size;
+	msg->len = 0;
+	msg->data = msg->_data;
+	msg->head = msg->_data;
+	msg->tail = msg->_data;
+
+	return msg;
+}
+
+/*! \brief Release given message buffer
+ * \param[in] m Message buffer to be free'd
+ */
+void msgb_free(struct msgb *m)
+{
+	talloc_free(m);
+}
+
+/*! \brief Enqueue message buffer to tail of a queue
+ * \param[in] queue linked list header of queue
+ * \param[in] msg message buffer to be added to the queue
+ *
+ * The function will append the specified message buffer \a msg to the
+ * queue implemented by \ref llist_head \a queue
+ */
+void msgb_enqueue(struct llist_head *queue, struct msgb *msg)
+{
+	llist_add_tail(&msg->list, queue);
+}
+
+/*! \brief Dequeue message buffer from head of queue
+ * \param[in] queue linked list header of queue
+ * \returns message buffer (if any) or NULL if queue empty
+ *
+ * The function will remove the first message buffer from the queue
+ * implemented by \ref llist_head \a queue.
+ */
+struct msgb *msgb_dequeue(struct llist_head *queue)
+{
+	struct llist_head *lh;
+
+	if (llist_empty(queue))
+		return NULL;
+
+	lh = queue->next;
+
+	if (lh) {
+		llist_del(lh);
+		return llist_entry(lh, struct msgb, list);
+	} else
+		return NULL;
+}
+
+/*! \brief Re-set all message buffer pointers
+ *  \param[in] msg message buffer that is to be resetted
+ *
+ * This will re-set the various internal pointers into the underlying
+ * message buffer, i.e. remvoe all headroom and treat the msgb as
+ * completely empty.  It also initializes the control buffer to zero.
+ */
+void msgb_reset(struct msgb *msg)
+{
+	msg->len = 0;
+	msg->data = msg->_data;
+	msg->head = msg->_data;
+	msg->tail = msg->_data;
+
+	msg->trx = NULL;
+	msg->lchan = NULL;
+	msg->l2h = NULL;
+	msg->l3h = NULL;
+	msg->l4h = NULL;
+
+	memset(&msg->cb, 0, sizeof(msg->cb));
+}
+
+/*! \brief get pointer to data section of message buffer
+ *  \param[in] msg message buffer
+ *  \returns pointer to data section of message buffer
+ */
+uint8_t *msgb_data(const struct msgb *msg)
+{
+	return msg->data;
+}
+
+/*! \brief get length of message buffer
+ *  \param[in] msg message buffer
+ *  \returns length of data section in message buffer
+ */
+uint16_t msgb_length(const struct msgb *msg)
+{
+	return msg->len;
+}
+
+/*! \brief Set the talloc context for \ref msgb_alloc
+ * Deprecated, use msgb_talloc_ctx_init() instead.
+ *  \param[in] ctx talloc context to be used as root for msgb allocations
+ */
+void msgb_set_talloc_ctx(void *ctx)
+{
+	tall_msgb_ctx = ctx;
+}
+
+/*! \brief Initialize a msgb talloc context for \ref msgb_alloc.
+ * Create a talloc context called "msgb". If \a pool_size is 0, create a named
+ * const as msgb talloc context. If \a pool_size is nonzero, create a talloc
+ * pool, possibly for faster msgb allocations (see talloc_pool()).
+ *  \param[in] root_ctx talloc context used as parent for the new "msgb" ctx.
+ *  \param[in] pool_size if nonzero, create a talloc pool of this size.
+ *  \returns the new msgb talloc context, e.g. for reporting
+ */
+void *msgb_talloc_ctx_init(void *root_ctx, unsigned int pool_size)
+{
+	if (!pool_size)
+		tall_msgb_ctx = talloc_size(root_ctx, 0);
+	else
+		tall_msgb_ctx = talloc_pool(root_ctx, pool_size);
+	talloc_set_name_const(tall_msgb_ctx, "msgb");
+	return tall_msgb_ctx;
+}
+
+/*! \brief Copy an msgb.
+ *
+ *  This function allocates a new msgb, copies the data buffer of msg,
+ *  and adjusts the pointers (incl l1h-l4h) accordingly. The cb part
+ *  is not copied.
+ *  \param[in] msg  The old msgb object
+ *  \param[in] name Human-readable name to be associated with msgb
+ */
+struct msgb *msgb_copy(const struct msgb *msg, const char *name)
+{
+	struct msgb *new_msg;
+
+	new_msg = msgb_alloc(msg->data_len, name);
+	if (!new_msg)
+		return NULL;
+
+	/* copy data */
+	memcpy(new_msg->_data, msg->_data, new_msg->data_len);
+
+	/* copy header */
+	new_msg->len = msg->len;
+	new_msg->data += msg->data - msg->_data;
+	new_msg->head += msg->head - msg->_data;
+	new_msg->tail += msg->tail - msg->_data;
+
+	if (msg->l1h)
+		new_msg->l1h = new_msg->_data + (msg->l1h - msg->_data);
+	if (msg->l2h)
+		new_msg->l2h = new_msg->_data + (msg->l2h - msg->_data);
+	if (msg->l3h)
+		new_msg->l3h = new_msg->_data + (msg->l3h - msg->_data);
+	if (msg->l4h)
+		new_msg->l4h = new_msg->_data + (msg->l4h - msg->_data);
+
+	return new_msg;
+}
+
+/*! \brief Resize an area within an msgb
+ *
+ *  This resizes a sub area of the msgb data and adjusts the pointers (incl
+ *  l1h-l4h) accordingly. The cb part is not updated. If the area is extended,
+ *  the contents of the extension is undefined. The complete sub area must be a
+ *  part of [data,tail].
+ *
+ *  \param[inout] msg       The msgb object
+ *  \param[in]    area      A pointer to the sub-area
+ *  \param[in]    old_size  The old size of the sub-area
+ *  \param[in]    new_size  The new size of the sub-area
+ *  \returns 0 on success, -1 if there is not enough space to extend the area
+ */
+int msgb_resize_area(struct msgb *msg, uint8_t *area,
+			    int old_size, int new_size)
+{
+	int rc;
+	uint8_t *post_start = area + old_size;
+	int pre_len = area - msg->data;
+	int post_len = msg->len - old_size - pre_len;
+	int delta_size = new_size - old_size;
+
+	if (old_size < 0 || new_size < 0)
+		MSGB_ABORT(msg, "Negative sizes are not allowed\n");
+	if (area < msg->data || post_start > msg->tail)
+		MSGB_ABORT(msg, "Sub area is not fully contained in the msg data\n");
+
+	if (delta_size == 0)
+		return 0;
+
+	if (delta_size > 0) {
+		rc = msgb_trim(msg, msg->len + delta_size);
+		if (rc < 0)
+			return rc;
+	}
+
+	memmove(area + new_size, area + old_size, post_len);
+
+	if (msg->l1h >= post_start)
+		msg->l1h += delta_size;
+	if (msg->l2h >= post_start)
+		msg->l2h += delta_size;
+	if (msg->l3h >= post_start)
+		msg->l3h += delta_size;
+	if (msg->l4h >= post_start)
+		msg->l4h += delta_size;
+
+	if (delta_size < 0)
+		msgb_trim(msg, msg->len + delta_size);
+
+	return 0;
+}
+
+#if 0
+/*! \brief Return a (static) buffer containing a hexdump of the msg
+ * \param[in] msg message buffer
+ * \returns a pointer to a static char array
+ */
+const char *msgb_hexdump(const struct msgb *msg)
+{
+	static char buf[4100];
+	int buf_offs = 0;
+	int nchars;
+	const unsigned char *start = msg->data;
+	const unsigned char *lxhs[4];
+	int i;
+
+	lxhs[0] = msg->l1h;
+	lxhs[1] = msg->l2h;
+	lxhs[2] = msg->l3h;
+	lxhs[3] = msg->l4h;
+
+	for (i = 0; i < ARRAY_SIZE(lxhs); i++) {
+		if (!lxhs[i])
+			continue;
+
+		if (lxhs[i] < msg->head)
+			continue;
+		if (lxhs[i] > msg->head + msg->data_len)
+			continue;
+		if (lxhs[i] > msg->tail)
+			continue;
+		if (lxhs[i] < msg->data || lxhs[i] > msg->tail) {
+			nchars = snprintf(buf + buf_offs, sizeof(buf) - buf_offs,
+					  "(L%d=data%+" PRIdPTR ") ",
+					  i+1, lxhs[i] - msg->data);
+			buf_offs += nchars;
+			continue;
+		}
+		if (lxhs[i] < start) {
+			nchars = snprintf(buf + buf_offs, sizeof(buf) - buf_offs,
+					  "(L%d%+" PRIdPTR ") ", i+1,
+					  start - lxhs[i]);
+			buf_offs += nchars;
+			continue;
+		}
+		nchars = snprintf(buf + buf_offs, sizeof(buf) - buf_offs,
+				  "%s[L%d]> ",
+				  osmo_hexdump(start, lxhs[i] - start),
+				  i+1);
+		if (nchars < 0 || nchars + buf_offs >= sizeof(buf))
+			return "ERROR";
+
+		buf_offs += nchars;
+		start = lxhs[i];
+	}
+	nchars = snprintf(buf + buf_offs, sizeof(buf) - buf_offs,
+			  "%s", osmo_hexdump(start, msg->tail - start));
+	if (nchars < 0 || nchars + buf_offs >= sizeof(buf))
+		return "ERROR";
+
+	buf_offs += nchars;
+
+	for (i = 0; i < ARRAY_SIZE(lxhs); i++) {
+		if (!lxhs[i])
+			continue;
+
+		if (lxhs[i] < msg->head || lxhs[i] > msg->head + msg->data_len) {
+			nchars = snprintf(buf + buf_offs, sizeof(buf) - buf_offs,
+					  "(L%d out of range) ", i+1);
+		} else if (lxhs[i] <= msg->data + msg->data_len &&
+			   lxhs[i] > msg->tail) {
+			nchars = snprintf(buf + buf_offs, sizeof(buf) - buf_offs,
+					  "(L%d=tail%+" PRIdPTR ") ",
+					  i+1, lxhs[i] - msg->tail);
+		} else
+			continue;
+
+		if (nchars < 0 || nchars + buf_offs >= sizeof(buf))
+			return "ERROR";
+		buf_offs += nchars;
+	}
+
+	return buf;
+}
+#endif
+
+/*! @} */

libosmocore/src/pseudo_talloc.c

@@ -0,0 +1,111 @@
+/* Memory allocation library
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/utils.h>
+#include "trace.h"
+
+/* TODO: this number should dynamically scale. We need at least one per IN/IRQ endpoint,
+ * as well as at least 3 for every OUT endpoint.  Plus some more depending on the application */
+#define NUM_RCTX_SMALL 20
+#define RCTX_SIZE_SMALL 348
+
+static uint8_t msgb_data[NUM_RCTX_SMALL][RCTX_SIZE_SMALL] __attribute__((aligned(sizeof(long))));
+static uint8_t msgb_inuse[NUM_RCTX_SMALL];
+
+void *_talloc_zero(const void *ctx, size_t size, const char *name)
+{
+	unsigned int i;
+	unsigned long x;
+
+	local_irq_save(x);
+	if (size > RCTX_SIZE_SMALL) {
+		local_irq_restore(x);
+		TRACE_ERROR("%s() request too large(%d > %d)\r\n", __func__, size, RCTX_SIZE_SMALL);
+		return NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(msgb_inuse); i++) {
+		if (!msgb_inuse[i]) {
+			uint8_t *out = msgb_data[i];
+			msgb_inuse[i] = 1;
+			memset(out, 0, size);
+			local_irq_restore(x);
+			return out;
+		}
+	}
+	local_irq_restore(x);
+	TRACE_ERROR("%s() out of memory!\r\n", __func__);
+	return NULL;
+}
+
+int _talloc_free(void *ptr, const char *location)
+{
+	unsigned int i;
+	unsigned long x;
+
+	local_irq_save(x);
+	for (i = 0; i < ARRAY_SIZE(msgb_inuse); i++) {
+		if (ptr == msgb_data[i]) {
+			if (!msgb_inuse[i]) {
+				TRACE_ERROR("%s: double_free by %s\r\n", __func__, location);
+				OSMO_ASSERT(0);
+			} else {
+				msgb_inuse[i] = 0;
+			}
+			local_irq_restore(x);
+			return 0;
+		}
+	}
+
+	local_irq_restore(x);
+	TRACE_ERROR("%s: invalid pointer %p from %s\r\n", __func__, ptr, location);
+	OSMO_ASSERT(0);
+	return -1;
+}
+
+void talloc_report(const void *ptr, FILE *f)
+{
+	unsigned int i;
+
+	fprintf(f, "talloc_report(): ");
+	for (i = 0; i < ARRAY_SIZE(msgb_inuse); i++) {
+		if (msgb_inuse[i])
+			fputc('X', f);
+		else
+			fputc('_', f);
+	}
+	fprintf(f, "\r\n");
+}
+
+void talloc_set_name_const(const void *ptr, const char *name)
+{
+	/* do nothing */
+}
+
+#if 0
+void *talloc_named_const(const void *context, size_t size, const char *name)
+{
+	if (size)
+		TRACE_ERROR("%s: called with size!=0 from %s\r\n", __func__, name);
+	return NULL;
+}
+
+void *talloc_pool(const void *context, size_t size)
+{
+}
+#endif
+

libosmocore/src/rbtree.c

@@ -0,0 +1,381 @@
+/*
+  Red Black Trees
+  (C) 1999  Andrea Arcangeli <andrea@suse.de>
+  (C) 2002  David Woodhouse <dwmw2@infradead.org>
+
+  SPDX-License-Identifier: GPL-2.0+
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  linux/lib/rbtree.c
+*/
+
+#include <osmocom/core/linuxrbtree.h>
+
+static void __rb_rotate_left(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *right = node->rb_right;
+	struct rb_node *parent = rb_parent(node);
+
+	if ((node->rb_right = right->rb_left))
+		rb_set_parent(right->rb_left, node);
+	right->rb_left = node;
+
+	rb_set_parent(right, parent);
+
+	if (parent)
+	{
+		if (node == parent->rb_left)
+			parent->rb_left = right;
+		else
+			parent->rb_right = right;
+	}
+	else
+		root->rb_node = right;
+	rb_set_parent(node, right);
+}
+
+static void __rb_rotate_right(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *left = node->rb_left;
+	struct rb_node *parent = rb_parent(node);
+
+	if ((node->rb_left = left->rb_right))
+		rb_set_parent(left->rb_right, node);
+	left->rb_right = node;
+
+	rb_set_parent(left, parent);
+
+	if (parent)
+	{
+		if (node == parent->rb_right)
+			parent->rb_right = left;
+		else
+			parent->rb_left = left;
+	}
+	else
+		root->rb_node = left;
+	rb_set_parent(node, left);
+}
+
+void rb_insert_color(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *parent, *gparent;
+
+	while ((parent = rb_parent(node)) && rb_is_red(parent))
+	{
+		gparent = rb_parent(parent);
+
+		if (parent == gparent->rb_left)
+		{
+			{
+				register struct rb_node *uncle = gparent->rb_right;
+				if (uncle && rb_is_red(uncle))
+				{
+					rb_set_black(uncle);
+					rb_set_black(parent);
+					rb_set_red(gparent);
+					node = gparent;
+					continue;
+				}
+			}
+
+			if (parent->rb_right == node)
+			{
+				register struct rb_node *tmp;
+				__rb_rotate_left(parent, root);
+				tmp = parent;
+				parent = node;
+				node = tmp;
+			}
+
+			rb_set_black(parent);
+			rb_set_red(gparent);
+			__rb_rotate_right(gparent, root);
+		} else {
+			{
+				register struct rb_node *uncle = gparent->rb_left;
+				if (uncle && rb_is_red(uncle))
+				{
+					rb_set_black(uncle);
+					rb_set_black(parent);
+					rb_set_red(gparent);
+					node = gparent;
+					continue;
+				}
+			}
+
+			if (parent->rb_left == node)
+			{
+				register struct rb_node *tmp;
+				__rb_rotate_right(parent, root);
+				tmp = parent;
+				parent = node;
+				node = tmp;
+			}
+
+			rb_set_black(parent);
+			rb_set_red(gparent);
+			__rb_rotate_left(gparent, root);
+		}
+	}
+
+	rb_set_black(root->rb_node);
+}
+
+static void __rb_erase_color(struct rb_node *node, struct rb_node *parent,
+			     struct rb_root *root)
+{
+	struct rb_node *other;
+
+	while ((!node || rb_is_black(node)) && node != root->rb_node)
+	{
+		if (parent->rb_left == node)
+		{
+			other = parent->rb_right;
+			if (rb_is_red(other))
+			{
+				rb_set_black(other);
+				rb_set_red(parent);
+				__rb_rotate_left(parent, root);
+				other = parent->rb_right;
+			}
+			if ((!other->rb_left || rb_is_black(other->rb_left)) &&
+			    (!other->rb_right || rb_is_black(other->rb_right)))
+			{
+				rb_set_red(other);
+				node = parent;
+				parent = rb_parent(node);
+			}
+			else
+			{
+				if (!other->rb_right || rb_is_black(other->rb_right))
+				{
+					rb_set_black(other->rb_left);
+					rb_set_red(other);
+					__rb_rotate_right(other, root);
+					other = parent->rb_right;
+				}
+				rb_set_color(other, rb_color(parent));
+				rb_set_black(parent);
+				rb_set_black(other->rb_right);
+				__rb_rotate_left(parent, root);
+				node = root->rb_node;
+				break;
+			}
+		}
+		else
+		{
+			other = parent->rb_left;
+			if (rb_is_red(other))
+			{
+				rb_set_black(other);
+				rb_set_red(parent);
+				__rb_rotate_right(parent, root);
+				other = parent->rb_left;
+			}
+			if ((!other->rb_left || rb_is_black(other->rb_left)) &&
+			    (!other->rb_right || rb_is_black(other->rb_right)))
+			{
+				rb_set_red(other);
+				node = parent;
+				parent = rb_parent(node);
+			}
+			else
+			{
+				if (!other->rb_left || rb_is_black(other->rb_left))
+				{
+					rb_set_black(other->rb_right);
+					rb_set_red(other);
+					__rb_rotate_left(other, root);
+					other = parent->rb_left;
+				}
+				rb_set_color(other, rb_color(parent));
+				rb_set_black(parent);
+				rb_set_black(other->rb_left);
+				__rb_rotate_right(parent, root);
+				node = root->rb_node;
+				break;
+			}
+		}
+	}
+	if (node)
+		rb_set_black(node);
+}
+
+void rb_erase(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *child, *parent;
+	int color;
+
+	if (!node->rb_left)
+		child = node->rb_right;
+	else if (!node->rb_right)
+		child = node->rb_left;
+	else
+	{
+		struct rb_node *old = node, *left;
+
+		node = node->rb_right;
+		while ((left = node->rb_left) != NULL)
+			node = left;
+
+		if (rb_parent(old)) {
+			if (rb_parent(old)->rb_left == old)
+				rb_parent(old)->rb_left = node;
+			else
+				rb_parent(old)->rb_right = node;
+		} else
+			root->rb_node = node;
+
+		child = node->rb_right;
+		parent = rb_parent(node);
+		color = rb_color(node);
+
+		if (parent == old) {
+			parent = node;
+		} else {
+			if (child)
+				rb_set_parent(child, parent);
+			parent->rb_left = child;
+
+			node->rb_right = old->rb_right;
+			rb_set_parent(old->rb_right, node);
+		}
+
+		node->rb_parent_color = old->rb_parent_color;
+		node->rb_left = old->rb_left;
+		rb_set_parent(old->rb_left, node);
+
+		goto color;
+	}
+
+	parent = rb_parent(node);
+	color = rb_color(node);
+
+	if (child)
+		rb_set_parent(child, parent);
+	if (parent)
+	{
+		if (parent->rb_left == node)
+			parent->rb_left = child;
+		else
+			parent->rb_right = child;
+	}
+	else
+		root->rb_node = child;
+
+ color:
+	if (color == RB_BLACK)
+		__rb_erase_color(child, parent, root);
+}
+
+/*
+ * This function returns the first node (in sort order) of the tree.
+ */
+struct rb_node *rb_first(const struct rb_root *root)
+{
+	struct rb_node	*n;
+
+	n = root->rb_node;
+	if (!n)
+		return NULL;
+	while (n->rb_left)
+		n = n->rb_left;
+	return n;
+}
+
+struct rb_node *rb_last(const struct rb_root *root)
+{
+	struct rb_node	*n;
+
+	n = root->rb_node;
+	if (!n)
+		return NULL;
+	while (n->rb_right)
+		n = n->rb_right;
+	return n;
+}
+
+struct rb_node *rb_next(const struct rb_node *node)
+{
+	struct rb_node *parent;
+
+	if (rb_parent(node) == node)
+		return NULL;
+
+	/* If we have a right-hand child, go down and then left as far
+	   as we can. */
+	if (node->rb_right) {
+		node = node->rb_right; 
+		while (node->rb_left)
+			node=node->rb_left;
+		return (struct rb_node *)node;
+	}
+
+	/* No right-hand children.  Everything down and left is
+	   smaller than us, so any 'next' node must be in the general
+	   direction of our parent. Go up the tree; any time the
+	   ancestor is a right-hand child of its parent, keep going
+	   up. First time it's a left-hand child of its parent, said
+	   parent is our 'next' node. */
+	while ((parent = rb_parent(node)) && node == parent->rb_right)
+		node = parent;
+
+	return parent;
+}
+
+struct rb_node *rb_prev(const struct rb_node *node)
+{
+	struct rb_node *parent;
+
+	if (rb_parent(node) == node)
+		return NULL;
+
+	/* If we have a left-hand child, go down and then right as far
+	   as we can. */
+	if (node->rb_left) {
+		node = node->rb_left; 
+		while (node->rb_right)
+			node=node->rb_right;
+		return (struct rb_node *)node;
+	}
+
+	/* No left-hand children. Go up till we find an ancestor which
+	   is a right-hand child of its parent */
+	while ((parent = rb_parent(node)) && node == parent->rb_left)
+		node = parent;
+
+	return parent;
+}
+
+void rb_replace_node(struct rb_node *victim, struct rb_node *new,
+		     struct rb_root *root)
+{
+	struct rb_node *parent = rb_parent(victim);
+
+	/* Set the surrounding nodes to point to the replacement */
+	if (parent) {
+		if (victim == parent->rb_left)
+			parent->rb_left = new;
+		else
+			parent->rb_right = new;
+	} else {
+		root->rb_node = new;
+	}
+	if (victim->rb_left)
+		rb_set_parent(victim->rb_left, new);
+	if (victim->rb_right)
+		rb_set_parent(victim->rb_right, new);
+
+	/* Copy the pointers/colour from the victim to the replacement */
+	*new = *victim;
+}

libosmocore/src/timer.c

@@ -0,0 +1,290 @@
+/*
+ * (C) 2008,2009 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2011 by Harald Welte <laforge@gnumonks.org>
+ * All Rights Reserved
+ *
+ * Authors: Holger Hans Peter Freyther <zecke@selfish.org>
+ *	    Harald Welte <laforge@gnumonks.org>
+ *	    Pablo Neira Ayuso <pablo@gnumonks.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+
+/*! \addtogroup timer
+ *  @{
+ */
+
+/*! \file timer.c
+ */
+
+#include <assert.h>
+#include <limits.h>
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/timer.h>
+
+/* These store the amount of time that we wait until next timer expires. */
+static struct osmo_timeval nearest;
+static struct osmo_timeval *nearest_p;
+
+static struct rb_root timer_root = RB_ROOT;
+
+static void __add_timer(struct osmo_timer_list *timer)
+{
+	struct rb_node **new = &(timer_root.rb_node);
+	struct rb_node *parent = NULL;
+
+	while (*new) {
+		struct osmo_timer_list *this;
+
+		this = container_of(*new, struct osmo_timer_list, node);
+
+		parent = *new;
+		if (timercmp(&timer->timeout, &this->timeout, <))
+			new = &((*new)->rb_left);
+		else
+			new = &((*new)->rb_right);
+        }
+
+        rb_link_node(&timer->node, parent, new);
+        rb_insert_color(&timer->node, &timer_root);
+}
+
+/*! set up timer callback and data
+ *  \param[in] timer the timer that should be added
+ *  \param[in] cb function to be called when timer expires
+ *  \param[in] data pointer to data that passed to the callback function
+ */
+void osmo_timer_setup(struct osmo_timer_list *timer, void (*cb)(void *data),
+		      void *data)
+{
+	timer->cb	= cb;
+	timer->data	= data;
+}
+
+/*! \brief add a new timer to the timer management
+ *  \param[in] timer the timer that should be added
+ */
+void osmo_timer_add(struct osmo_timer_list *timer)
+{
+	osmo_timer_del(timer);
+	timer->active = 1;
+	INIT_LLIST_HEAD(&timer->list);
+	__add_timer(timer);
+}
+
+/*! \brief schedule a timer at a given future relative time
+ *  \param[in] timer the to-be-added timer
+ *  \param[in] seconds number of seconds from now
+ *  \param[in] microseconds number of microseconds from now
+ *
+ * This function can be used to (re-)schedule a given timer at a
+ * specified number of seconds+microseconds in the future.  It will
+ * internally add it to the timer management data structures, thus
+ * osmo_timer_add() is automatically called.
+ */
+void
+osmo_timer_schedule(struct osmo_timer_list *timer, int seconds, int microseconds)
+{
+	struct osmo_timeval current_time;
+
+	osmo_gettimeofday(&current_time, NULL);
+#if 0
+	timer->timeout.tv_sec = seconds;
+	timer->timeout.tv_usec = microseconds;
+#else
+	timer->timeout.expires = (seconds*1000) + (microseconds/1000);
+#endif
+	timeradd(&timer->timeout, &current_time, &timer->timeout);
+	osmo_timer_add(timer);
+}
+
+/*! \brief delete a timer from timer management
+ *  \param[in] timer the to-be-deleted timer
+ *
+ * This function can be used to delete a previously added/scheduled
+ * timer from the timer management code.
+ */
+void osmo_timer_del(struct osmo_timer_list *timer)
+{
+	if (timer->active) {
+		timer->active = 0;
+		rb_erase(&timer->node, &timer_root);
+		/* make sure this is not already scheduled for removal. */
+		if (!llist_empty(&timer->list))
+			llist_del_init(&timer->list);
+	}
+}
+
+/*! \brief check if given timer is still pending
+ *  \param[in] timer the to-be-checked timer
+ *  \return 1 if pending, 0 otherwise
+ *
+ * This function can be used to determine whether a given timer
+ * has alredy expired (returns 0) or is still pending (returns 1)
+ */
+int osmo_timer_pending(struct osmo_timer_list *timer)
+{
+	return timer->active;
+}
+
+/*! \brief compute the remaining time of a timer
+ *  \param[in] timer the to-be-checked timer
+ *  \param[in] now the current time (NULL if not known)
+ *  \param[out] remaining remaining time until timer fires
+ *  \return 0 if timer has not expired yet, -1 if it has
+ *
+ *  This function can be used to determine the amount of time
+ *  remaining until the expiration of the timer.
+ */
+int osmo_timer_remaining(const struct osmo_timer_list *timer,
+			 const struct osmo_timeval *now,
+			 struct osmo_timeval *remaining)
+{
+	struct osmo_timeval current_time;
+
+	if (!now)
+		osmo_gettimeofday(&current_time, NULL);
+	else
+		current_time = *now;
+
+	timersub(&timer->timeout, &current_time, remaining);
+
+#if 0
+	if (remaining->tv_sec < 0)
+#else
+	if (remaining->expires < 0)
+#endif
+		return -1;
+
+	return 0;
+}
+
+/*! \brief Determine time between now and the nearest timer
+ *  \returns pointer to osmo_timeval of nearest timer, NULL if there is none
+ *
+ * if we have a nearest time return the delta between the current
+ * time and the time of the nearest timer.
+ * If the nearest timer timed out return NULL and then we will
+ * dispatch everything after the select
+ */
+struct osmo_timeval *osmo_timers_nearest(void)
+{
+	/* nearest_p is exactly what we need already: NULL if nothing is
+	 * waiting, {0,0} if we must dispatch immediately, and the correct
+	 * delay if we need to wait */
+	return nearest_p;
+}
+
+static void update_nearest(struct osmo_timeval *cand, struct osmo_timeval *current)
+{
+#if 0
+	if (cand->tv_sec != LONG_MAX) {
+#else
+	if (cand->expires != LONG_MAX) {
+#endif
+		if (timercmp(cand, current, >))
+			timersub(cand, current, &nearest);
+		else {
+			/* loop again inmediately */
+			timerclear(&nearest);
+		}
+		nearest_p = &nearest;
+	} else {
+		nearest_p = NULL;
+	}
+}
+
+/*! \brief Find the nearest time and update nearest_p */
+void osmo_timers_prepare(void)
+{
+	struct rb_node *node;
+	struct osmo_timeval current;
+
+	osmo_gettimeofday(&current, NULL);
+
+	node = rb_first(&timer_root);
+	if (node) {
+		struct osmo_timer_list *this;
+		this = container_of(node, struct osmo_timer_list, node);
+		update_nearest(&this->timeout, &current);
+	} else {
+		nearest_p = NULL;
+	}
+}
+
+/*! \brief fire all timers... and remove them */
+int osmo_timers_update(void)
+{
+	struct osmo_timeval current_time;
+	struct rb_node *node;
+	struct llist_head timer_eviction_list;
+	struct osmo_timer_list *this;
+	int work = 0;
+
+	osmo_gettimeofday(&current_time, NULL);
+
+	INIT_LLIST_HEAD(&timer_eviction_list);
+	for (node = rb_first(&timer_root); node; node = rb_next(node)) {
+		this = container_of(node, struct osmo_timer_list, node);
+
+		if (timercmp(&this->timeout, &current_time, >))
+			break;
+
+		llist_add(&this->list, &timer_eviction_list);
+	}
+
+	/*
+	 * The callbacks might mess with our list and in this case
+	 * even llist_for_each_entry_safe is not safe to use. To allow
+	 * osmo_timer_del to be called from within the callback we need
+	 * to restart the iteration for each element scheduled for removal.
+	 *
+	 * The problematic scenario is the following: Given two timers A
+	 * and B that have expired at the same time. Thus, they are both
+	 * in the eviction list in this order: A, then B. If we remove
+	 * timer B from the A's callback, we continue with B in the next
+	 * iteration step, leading to an access-after-release.
+	 */
+restart:
+	llist_for_each_entry(this, &timer_eviction_list, list) {
+		osmo_timer_del(this);
+		if (this->cb)
+			this->cb(this->data);
+		work = 1;
+		goto restart;
+	}
+
+	return work;
+}
+
+/*! \brief Check how many timers we have in the system
+ *  \returns number of \ref osmo_timer_list registered */
+int osmo_timers_check(void)
+{
+	struct rb_node *node;
+	int i = 0;
+
+	for (node = rb_first(&timer_root); node; node = rb_next(node)) {
+		i++;
+	}
+	return i;
+}
+
+extern volatile unsigned long jiffies;
+int osmo_gettimeofday(struct osmo_timeval *tv, struct timezone *tz)
+{
+	tv->expires = jiffies;
+	return 0;
+}
+
+/*! @} */

libosmocore/src/utils.c

@@ -0,0 +1,103 @@
+/*
+ * (C) 2011 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2011 by Sylvain Munaut <tnt@246tNt.com>
+ * (C) 2014 by Nils O. Selåsdal <noselasd@fiane.dyndns.org>
+ *
+ * All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+
+#include <stdbool.h>
+#include <string.h>
+#include <stdint.h>
+#include <errno.h>
+#include <stdio.h>
+#include <inttypes.h>
+#include <limits.h>
+
+#include <osmocom/core/utils.h>
+
+
+/*! \addtogroup utils
+ * @{
+ * various utility routines
+ *
+ * \file utils.c */
+
+static char namebuf[255];
+
+/*! get human-readable string for given value
+ *  \param[in] vs Array of value_string tuples
+ *  \param[in] val Value to be converted
+ *  \returns pointer to human-readable string
+ *
+ * If val is found in vs, the array's string entry is returned. Otherwise, an
+ * "unknown" string containing the actual value is composed in a static buffer
+ * that is reused across invocations.
+ */
+const char *get_value_string(const struct value_string *vs, uint32_t val)
+{
+	const char *str = get_value_string_or_null(vs, val);
+	if (str)
+		return str;
+
+	snprintf(namebuf, sizeof(namebuf), "unknown 0x%"PRIx32, val);
+	namebuf[sizeof(namebuf) - 1] = '\0';
+	return namebuf;
+}
+
+/*! get human-readable string or NULL for given value
+ *  \param[in] vs Array of value_string tuples
+ *  \param[in] val Value to be converted
+ *  \returns pointer to human-readable string or NULL if val is not found
+ */
+const char *get_value_string_or_null(const struct value_string *vs,
+				     uint32_t val)
+{
+	int i;
+
+	if (!vs)
+		return NULL;
+
+	for (i = 0;; i++) {
+		if (vs[i].value == 0 && vs[i].str == NULL)
+			break;
+		if (vs[i].value == val)
+			return vs[i].str;
+	}
+
+	return NULL;
+}
+
+/*! get numeric value for given human-readable string
+ *  \param[in] vs Array of value_string tuples
+ *  \param[in] str human-readable string
+ *  \returns numeric value (>0) or negative numer in case of error
+ */
+int get_string_value(const struct value_string *vs, const char *str)
+{
+	int i;
+
+	for (i = 0;; i++) {
+		if (vs[i].value == 0 && vs[i].str == NULL)
+			break;
+		if (!strcasecmp(vs[i].str, str))
+			return vs[i].value;
+	}
+	return -EINVAL;
+}
+
+/*! @} */

libsimtrace/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(src)

libsimtrace/include/libsimtrace/card_emu.h

@@ -0,0 +1,84 @@
+/* ISO7816-3 state machine for the card side
+ *
+ * (C) 2010-2023 by Harald Welte <hwelte@hmw-consulting.de>
+ * (C) 2018 by sysmocom -s.f.m.c. GmbH, Author: Kevin Redon <kredon@sysmocom.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#pragma once
+
+#include <stdint.h>
+
+#include <osmocom/core/linuxlist.h>
+#include <libsimtrace/usb_buffered_ep.h>
+
+struct card_handle;
+
+enum card_io {
+	CARD_IO_VCC,
+	CARD_IO_RST,
+	CARD_IO_CLK,
+};
+
+/** initialise card slot
+ *  @param[in] slot_num slot number (arbitrary number)
+ *  @param[in] uart_chan UART peripheral channel number
+ *  @param[in] in_bep USB IN buffered end point
+ *  @param[in] irq_bep USB INTerrupt buffered end point
+ *  @param[in] vcc_active initial VCC signal state (true = on)
+ *  @param[in] in_reset initial RST signal state (true = reset asserted)
+ *  @param[in] clocked initial CLK signat state (true = active)
+ *  @return main card handle reference
+ */
+struct card_handle *card_emu_init(uint8_t slot_num, uint8_t uart_chan,
+				  struct usb_buffered_ep *in_bep, struct usb_buffered_ep *irq_bep,
+				  bool vcc_active, bool in_reset, bool clocked);
+
+/* process a single byte received from the reader */
+void card_emu_process_rx_byte(struct card_handle *ch, uint8_t byte);
+
+/* transmit a single byte to the reader */
+int card_emu_tx_byte(struct card_handle *ch);
+
+/* hardware driver informs us that a card I/O signal has changed */
+void card_emu_io_statechg(struct card_handle *ch, enum card_io io, int active);
+
+/* User sets a new ATR to be returned during next card reset */
+int card_emu_set_atr(struct card_handle *ch, const uint8_t *atr, uint8_t len);
+
+struct llist_head *card_emu_get_uart_tx_queue(struct card_handle *ch);
+void card_emu_have_new_uart_tx(struct card_handle *ch);
+int card_emu_report_status(struct card_handle *ch, bool report_on_irq);
+
+void card_emu_wtime_half_expired(void *ch);
+void card_emu_wtime_expired(void *ch);
+
+/* user-provided low-level initialization for driver/board code */
+int card_emu_drv_init(struct card_handle *ch, uint8_t uart_chan);
+
+
+#define ENABLE_TX		0x01
+#define ENABLE_RX		0x02
+#define ENABLE_TX_TIMER_ONLY	0x03
+
+int card_emu_uart_update_fidi(uint8_t uart_chan, unsigned int fidi);
+void card_emu_uart_update_wt(uint8_t uart_chan, uint32_t wt);
+void card_emu_uart_reset_wt(uint8_t uart_chan);
+int card_emu_uart_tx(uint8_t uart_chan, uint8_t byte);
+void card_emu_uart_enable(uint8_t uart_chan, uint8_t rxtx);
+void card_emu_uart_wait_tx_idle(uint8_t uart_chan);
+void card_emu_uart_interrupt(uint8_t uart_chan);
+
+int card_emu_get_vcc(uint8_t uart_chan);
+
+struct cardemu_usb_msg_config;
+int card_emu_set_config(struct card_handle *ch, const struct cardemu_usb_msg_config *scfg,
+			unsigned int scfg_len);

libsimtrace/include/libsimtrace/iso7816_fidi.h

@@ -0,0 +1,26 @@
+/* ISO7816-3 Fi/Di tables + computation
+ *
+ * (C) 2010-2015 by Harald Welte <laforge@gnumonks.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#pragma once
+
+#include <stdint.h>
+
+/* Table 7 of ISO 7816-3:2006 */
+extern const uint16_t iso7816_3_fi_table[16];
+
+/* Table 8 from ISO 7816-3:2006 */
+extern const uint8_t iso7816_3_di_table[16];
+
+/* compute the F/D ratio based on F_index and D_index values */
+int iso7816_3_compute_fd_ratio(uint8_t f_index, uint8_t d_index);

libsimtrace/include/libsimtrace/simtrace_prot.h

@@ -0,0 +1,363 @@
+/* SIMtrace2 USB protocol
+ *
+ * (C) 2015-2022 by Harald Welte <hwelte@hmw-consulting.de>
+ * (C) 2018 by sysmocom -s.f.m.c. GmbH, Author: Kevin Redon <kredon@sysmocom.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/***********************************************************************
+ * COMMON HEADER
+ ***********************************************************************/
+
+enum simtrace_msg_class {
+	SIMTRACE_MSGC_GENERIC = 0,
+	/* Card Emulation / Forwarding */
+	SIMTRACE_MSGC_CARDEM,
+	/* Modem Control (if modem is attached next to device) */
+	SIMTRACE_MSGC_MODEM,
+	/* Reader/phone-car/SIM communication sniff */
+	SIMTRACE_MSGC_SNIFF,
+
+	/* first vendor-specific request */
+	_SIMTRACE_MGSC_VENDOR_FIRST = 127,
+};
+
+enum simtrace_msg_type_generic {
+	/* Generic Error Message */
+	SIMTRACE_CMD_DO_ERROR	= 0,
+	/* Request/Response for simtrace_board_info */
+	SIMTRACE_CMD_BD_BOARD_INFO,
+};
+
+/* SIMTRACE_MSGC_CARDEM */
+enum simtrace_msg_type_cardem {
+	/* TPDU Data to be transmitted to phone */
+	SIMTRACE_MSGT_DT_CEMU_TX_DATA = 1,
+	/* Set the ATR to be returned at phone-SIM reset */
+	SIMTRACE_MSGT_DT_CEMU_SET_ATR,
+	/* Get Statistics Request / Response */
+	SIMTRACE_MSGT_BD_CEMU_STATS,
+	/* Get Status Request / Response */
+	SIMTRACE_MSGT_BD_CEMU_STATUS,
+	/* Request / Confirm emulated card insert */
+	SIMTRACE_MSGT_DT_CEMU_CARDINSERT,
+	/* TPDU Data received from phomne */
+	SIMTRACE_MSGT_DO_CEMU_RX_DATA,
+	/* Indicate PTS request from phone */
+	SIMTRACE_MSGT_DO_CEMU_PTS,
+	/* Set configurable parameters */
+	SIMTRACE_MSGT_BD_CEMU_CONFIG,
+};
+
+/* SIMTRACE_MSGC_MODEM */
+enum simtrace_msg_type_modem {
+	/* Modem Control: Reset an attached modem */
+	SIMTRACE_MSGT_DT_MODEM_RESET = 1,
+	/* Modem Control: Select local / remote SIM */
+	SIMTRACE_MSGT_DT_MODEM_SIM_SELECT,
+	/* Modem Control: Status (WWAN LED, SIM Presence) */
+	SIMTRACE_MSGT_BD_MODEM_STATUS,
+};
+
+/* SIMTRACE_MSGC_SNIFF */
+enum simtrace_msg_type_sniff {
+	/* Status change (card inserted, reset, ...) */
+	SIMTRACE_MSGT_SNIFF_CHANGE = 0,
+	/* Fi/Di baudrate change */
+	SIMTRACE_MSGT_SNIFF_FIDI,
+	/* ATR data */
+	SIMTRACE_MSGT_SNIFF_ATR,
+	/* PPS (request or response) data */
+	SIMTRACE_MSGT_SNIFF_PPS,
+	/* TPDU data */
+	SIMTRACE_MSGT_SNIFF_TPDU,
+	/* Statistics */
+	SIMTRACE_MSGT_DO_SNIFF_STATS,
+};
+
+/* common message header */
+struct simtrace_msg_hdr {
+	uint8_t msg_class;	/* simtrace_msg_class */
+	uint8_t msg_type;	/* simtrace_msg_type_xxx */
+	uint8_t seq_nr;
+	uint8_t slot_nr;	/* SIM slot number */
+	uint16_t _reserved;
+	uint16_t msg_len;	/* length including header */
+	uint8_t payload[0];
+} __attribute__ ((packed));
+
+/***********************************************************************
+ * Capabilities
+ ***********************************************************************/
+
+/* generic capabilities */
+enum simtrace_capability_generic {
+	/* compatible with 5V SIM card interface */
+	SIMTRACE_CAP_VOLT_5V,
+	/* compatible with 3.3V SIM card interface */
+	SIMTRACE_CAP_VOLT_3V3,
+	/* compatible with 1.8V SIM card interface */
+	SIMTRACE_CAP_VOLT_1V8,
+	/* Has LED1 */
+	SIMTRACE_CAP_LED_1,
+	/* Has LED2 */
+	SIMTRACE_CAP_LED_2,
+	/* Has Single-Pole Dual-Throw (local/remote SIM) */
+	SIMTRACE_CAP_SPDT,
+	/* Has Bus-Switch (trace / MITM) */
+	SIMTRACE_CAP_BUS_SWITCH,
+	/* Can read VSIM via ADC */
+	SIMTRACE_CAP_VSIM_ADC,
+	/* Can read temperature via ADC */
+	SIMTRACE_CAP_TEMP_ADC,
+	/* Supports DFU for firmware update */
+	SIMTRACE_CAP_DFU,
+	/* Supports Ctrl EP command for erasing flash / return to SAM-BA */
+	SIMTRACE_CAP_ERASE_FLASH,
+	/* Can read the status of card insert contact */
+	SIMTRACE_CAP_READ_CARD_DET,
+	/* Can control the status of a simulated card insert */
+	SIMTRACE_CAP_ASSERT_CARD_DET,
+	/* Can toggle the hardware reset of an attached modem */
+	SIMTRACE_CAP_ASSERT_MODEM_RST,
+};
+
+/* vendor-specific capabilities of sysmocom devices */
+enum simtrace_capability_vendor {
+	/* Can erase a peer SAM3 controller */
+	SIMTRACE_CAP_SYSMO_QMOD_ERASE_PEER,
+	/* Can read/write an attached EEPROM */
+	SIMTRACE_CAP_SYSMO_QMOD_RW_EEPROM,
+	/* can reset an attached USB hub */
+	SIMTRACE_CAP_SYSMO_QMOD_RESET_HUB,
+};
+
+/* SIMTRACE_CMD_BD_BOARD_INFO */
+struct simtrace_board_info {
+	struct {
+		char manufacturer[32];
+		char model[32];
+		char version[32];
+	} hardware;
+	struct {
+		/* who provided this software? */
+		char provider[32];
+		/* name of software image */
+		char name[32];
+		/* (git) version at build time */
+		char version[32];
+		/* built on which machine? */
+		char buildhost[32];
+		/* CRC-32 over software image */
+		uint32_t crc;
+	} software;
+	struct {
+		/* Maximum baud rate supported */
+		uint32_t max_baud_rate;
+	} speed;
+	/* number of bytes of generic capability bit-mask */
+	uint8_t cap_generic_bytes;
+	/* number of bytes of vendor capability bit-mask */
+	uint8_t cap_vendor_bytes;
+	uint8_t data[0];
+	/* cap_generic + cap_vendor */
+} __attribute__ ((packed));
+
+/***********************************************************************
+ * CARD EMULATOR / FORWARDER
+ ***********************************************************************/
+
+/* indicates a TPDU header is present in this message */
+#define CEMU_DATA_F_TPDU_HDR	0x00000001
+/* indicates last part of transmission in this direction */
+#define CEMU_DATA_F_FINAL	0x00000002
+/* incdicates a PB is present and we should continue with TX */
+#define CEMU_DATA_F_PB_AND_TX	0x00000004
+/* incdicates a PB is present and we should continue with RX */
+#define CEMU_DATA_F_PB_AND_RX	0x00000008
+
+/* CEMU_USB_MSGT_DT_CARDINSERT */
+struct cardemu_usb_msg_cardinsert {
+	uint8_t card_insert;
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DT_SET_ATR */
+struct cardemu_usb_msg_set_atr {
+	uint8_t atr_len;
+	/* variable-length ATR data */
+	uint8_t atr[0];
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DT_TX_DATA */
+struct cardemu_usb_msg_tx_data {
+	uint32_t flags;
+	uint16_t data_len;
+	/* variable-length TPDU data */
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DO_RX_DATA */
+struct cardemu_usb_msg_rx_data {
+	uint32_t flags;
+	uint16_t data_len;
+	/* variable-length TPDU data */
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+#define CEMU_STATUS_F_VCC_PRESENT	0x00000001
+#define CEMU_STATUS_F_CLK_ACTIVE	0x00000002
+#define CEMU_STATUS_F_RCEMU_ACTIVE	0x00000004
+#define CEMU_STATUS_F_CARD_INSERT	0x00000008
+#define CEMU_STATUS_F_RESET_ACTIVE	0x00000010
+
+/* CEMU_USB_MSGT_DO_STATUS */
+struct cardemu_usb_msg_status {
+	uint32_t flags;
+	/* phone-applied target voltage in mV */
+	uint16_t voltage_mv;
+	/* F/D related information. Not actual Fn/Dn values but indexes into tables! */
+	union {
+		uint8_t F_index;	/* <! Index to ISO7816-3 Table 7 (F and f_max values) */
+		uint8_t fi;		/* <! old, wrong name for API compatibility */
+	};
+	union {
+		uint8_t D_index;	/* <! Index to ISO7816-3 Table 8 (D value) */
+		uint8_t di;		/* <! old, wrong name for API compatibility */
+	};
+	uint8_t wi;		/* <! Waiting Integer as defined in ISO7816-3 Section 10.2 */
+	uint32_t waiting_time;	/* <! Waiting Time in etu as defined in ISO7816-3 Section 8.1 */
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DO_PTS */
+struct cardemu_usb_msg_pts_info {
+	uint8_t pts_len;
+	/* PTS request as sent from reader */
+	uint8_t req[6];
+	/* PTS response as sent by card */
+	uint8_t resp[6];
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DO_ERROR */
+struct cardemu_usb_msg_error {
+	uint8_t severity;
+	uint8_t subsystem;
+	uint16_t code;
+	uint8_t msg_len;
+	/* human-readable error message */
+	uint8_t msg[0];
+} __attribute__ ((packed));
+
+/* enable/disable the generation of DO_STATUS on IRQ endpoint */
+#define CEMU_FEAT_F_STATUS_IRQ	0x00000001
+
+/* SIMTRACE_MSGT_BD_CEMU_CONFIG */
+struct cardemu_usb_msg_config {
+	/* bit-mask of CEMU_FEAT_F flags */
+	uint32_t features;
+	/* the selected slot number (if an external mux is present) */
+	uint8_t slot_mux_nr;
+} __attribute__ ((packed));
+
+/***********************************************************************
+ * MODEM CONTROL
+ ***********************************************************************/
+
+/* SIMTRACE_MSGT_DT_MODEM_RESET */
+struct st_modem_reset {
+	/* 0: de-assert reset, 1: assert reset, 2: pulse reset */
+	uint8_t asserted;
+	/* if above is '2', duration of pulse in ms */
+	uint16_t pulse_duration_msec;
+} __attribute__((packed));
+
+/* SIMTRACE_MSGT_DT_MODEM_SIM_SELECT */
+struct st_modem_sim_select {
+	/* remote (1), local (0) */
+	uint8_t remote_sim;
+} __attribute__((packed));
+
+/* SIMTRACE_MSGT_BD_MODEM_STATUS */
+#define ST_MDM_STS_BIT_WWAN_LED		(1 << 0)
+#define ST_MDM_STS_BIT_CARD_INSERTED	(1 << 1)
+struct st_modem_status {
+	/* bit-field of supported status bits */
+	uint8_t supported_mask;
+	/* bit-field of current status bits */
+	uint8_t status_mask;
+	/* bit-field of changed status bits */
+	uint8_t changed_mask;
+} __attribute__((packed));
+
+/***********************************************************************
+ * SNIFF
+ ***********************************************************************/
+
+/* SIMTRACE_MSGT_SNIFF_CHANGE flags */
+#define SNIFF_CHANGE_FLAG_CARD_INSERT (1<<0)
+#define SNIFF_CHANGE_FLAG_CARD_EJECT (1<<1)
+#define SNIFF_CHANGE_FLAG_RESET_ASSERT (1<<2)
+#define SNIFF_CHANGE_FLAG_RESET_DEASSERT (1<<3)
+#define SNIFF_CHANGE_FLAG_TIMEOUT_WT (1<<4)
+/* SIMTRACE_MSGT_SNIFF_ATR, SIMTRACE_MSGT_SNIFF_PPS, SIMTRACE_MSGT_SNIFF_TPDU flags */
+#define SNIFF_DATA_FLAG_ERROR_INCOMPLETE (1<<5)
+#define SNIFF_DATA_FLAG_ERROR_MALFORMED (1<<6)
+#define SNIFF_DATA_FLAG_ERROR_CHECKSUM (1<<7)
+#define SNIFF_DATA_FLAG_ERROR_OVERRUN (1<<8)
+#define SNIFF_DATA_FLAG_ERROR_FRAMING (1<<9)
+#define SNIFF_DATA_FLAG_ERROR_PARITY (1<<10)
+
+/* SIMTRACE_MSGT_SNIFF_CHANGE */
+struct sniff_change {
+	/* SIMTRACE_MSGT_SNIFF_CHANGE flags */
+	uint32_t flags;
+} __attribute__ ((packed));
+
+/* SIMTRACE_MSGT_SNIFF_FIDI */
+struct sniff_fidi {
+	/* Fi/Di values as encoded in TA1 */
+	uint8_t fidi;
+} __attribute__ ((packed));
+
+/* SIMTRACE_MSGT_SNIFF_ATR, SIMTRACE_MSGT_SNIFF_PPS, SIMTRACE_MSGT_SNIFF_TPDU */
+struct sniff_data {
+	/* data flags */
+	uint32_t flags;
+	/* data length */
+	uint16_t length;
+	/* data */
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+/* SIMTRACE_MSGT_DO_SNIFF_STATS */
+struct st_sniff_stats {
+	uint32_t flags;				/* RFU */
+	uint32_t num_bytes;			/* total lnumber of bytes received */
+	uint32_t num_tpdu;			/* total number of TPDUs received */
+	uint32_t num_atr;			/* total number of ATRs received */
+	uint32_t num_pps;			/* total number of PPS (req, resp) received */
+	uint32_t num_reset;			/* total number of resets */
+	struct {
+		uint32_t overruns;
+		uint32_t framing_errs;
+		uint32_t parity_errs;
+		uint32_t breaks;
+	} num_usart;
+	uint32_t num_waiting_time_exp;
+	uint32_t num_tpdu_overflows;		/* TPDU buffer overflows */
+	uint32_t num_csum_errors;		/* ATR + PPS checksum */
+	uint32_t num_ringbuf_overflows;		/* ISR->main ringbuffer overflows */
+	uint32_t num_tpdu_malformed;
+} __attribute__ ((packed));

libsimtrace/include/libsimtrace/usb_buffered_ep.h

@@ -0,0 +1,33 @@
+#pragma once
+#include <stdint.h>
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/msgb.h>
+
+#if 0
+#define LOGBEP(bep, fmt, args ...) \
+	printf("%s(%02x): " fmt, __func__, (bep)->ep, ## args)
+#else
+#define LOGBEP(bep, fmt, args ...)
+#endif
+
+/* buffered USB endpoint (with queue of msgb) */
+struct usb_buffered_ep {
+	/* endpoint address */
+	uint8_t ep;
+	/* currently any transfer in progress? */
+	struct msgb * msg_in_progress;
+	/* Tx queue (IN) / Rx queue (OUT) */
+	struct llist_head queue;
+	/* current length of queue */
+	unsigned int queue_len;
+};
+
+
+void bep_init(struct usb_buffered_ep *bep, uint8_t ep);
+struct msgb *bep_msgb_alloc(struct usb_buffered_ep *bep);
+void bep_msgb_free(struct msgb *msg);
+
+int bep_refill_to_host(struct usb_buffered_ep *bep, bool is_out_ep);
+struct msgb *bep_get_completed(struct usb_buffered_ep *bep);
+void bep_complete_in_progress(struct usb_buffered_ep *bep);
+int bep_enqueue_msgb(struct msgb *msg);

libsimtrace/src/CMakeLists.txt

@@ -0,0 +1,9 @@
+file(GLOB FILES *.c *.h)
+add_library(libsimtrace ${FILES})
+
+target_link_libraries(libsimtrace  PRIVATE
+                      libosmocore
+                      pico_stdlib
+                      cmsis_core)
+
+target_include_directories(libsimtrace PUBLIC ../include)

libsimtrace/src/iso7816_fidi.c

@@ -0,0 +1,59 @@
+/* ISO7816-3 Fi/Di tables + computation
+ *
+ * (C) 2010-2015 by Harald Welte <laforge@gnumonks.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <stdint.h>
+#include <errno.h>
+
+#include <osmocom/core/utils.h>
+#include <libsimtrace/iso7816_fidi.h>
+
+/* Table 7 of ISO 7816-3:2006 */
+const uint16_t iso7816_3_fi_table[] = {
+	372, 372, 558, 744, 1116, 1488, 1860, 0,
+	0, 512, 768, 1024, 1536, 2048, 0, 0
+};
+
+/* Table 8 from ISO 7816-3:2006 */
+const uint8_t iso7816_3_di_table[] = {
+	0, 1, 2, 4, 8, 16, 32, 64,
+	12, 20, 2, 4, 8, 16, 32, 64,
+};
+
+/* compute the F/D ratio based on Fi and Di values */
+int iso7816_3_compute_fd_ratio(uint8_t f_index, uint8_t d_index)
+{
+	uint16_t f, d;
+	int ret;
+
+	if (f_index >= ARRAY_SIZE(iso7816_3_fi_table) ||
+	    d_index >= ARRAY_SIZE(iso7816_3_di_table))
+		return -EINVAL;
+
+	f = iso7816_3_fi_table[f_index];
+	if (f == 0)
+		return -EINVAL;
+
+	d = iso7816_3_di_table[d_index];
+	if (d == 0)
+		return -EINVAL;
+
+	/* See table 7 of ISO 7816-3: From 1000 on we divide by 1/d,
+	 * which equals a multiplication by d */
+	if (d_index < 8)
+		ret = f / d;
+	else
+		ret = f * d;
+
+	return ret;
+}