X-Git-Url: http://pileus.org/git/?a=blobdiff_plain;f=hw2%2Fmain.cpp;h=2c0f2746e716f3064431fc1ee08901322fb5167e;hb=d634f081b7dceacd2b23046f75a8dc54c9f545ff;hp=aac88f2c0dccf3962b60f5656b42089b4507b082;hpb=d5a9226714d158f2ac458425eba2550fd02c5cbe;p=~andy%2Fcsm213a-hw

diff --git a/hw2/main.cpp b/hw2/main.cpp
index aac88f2..2c0f274 100644
--- a/hw2/main.cpp
+++ b/hw2/main.cpp
@@ -1,165 +1,740 @@
-#include "mbed.h"
-
-/**
- * Mode of operation:
- *   Devices 1 and 2 synchronize clocks using serial messages.
- *
- *   1. Each serial message timestamped using the hardware timer capture
- *      registers in both the sender and receiver.
- *   2. The sender transmits the send timestamp during the next time-sync
- *      message.
- *   3. The receiver then compares the senders timestamp with it's own
- *      timestamp for the corresponding messages and calculates an offset.
- *   4. The offset is used to compensate the receivers local clock.
- *
- *   Time synchronization is performed in both directions.
- */
-
-/***********************
- * Message Definitions *
- ***********************/
-
-#define MSG_HEADER 0x1234
-
-typedef enum {
-	MSG_ID_SYNC,       // Time synchronization
-	MSG_ID_EVENT,      // Event occurred
-} msgid_t;
-
-typedef struct {
-	uint32_t seconds;  // Seconds since 1970 (without leap seconds)
-	uint32_t nanosec;  // Nanoseconds since 'seconds'
-} ntime_t;
-
-typedef struct {
-	uint16_t header;   // Message Header
-	uint16_t mesgid;   // Message ID
-	uint16_t length;   // Body length
-	uint16_t cksum;    // Body checksum
-} header_t;
-
-typedef struct {
-	uint16_t seq;      // Current sequence counter
-	uint16_t prev;     // Sequence of previous message
-	ntime_t  time;     // Time of previous message
-} sync_msg_t;
-
-typedef struct {
-	uint16_t device;   // Device ID
-	uint16_t event;    // Event ID
-	ntime_t  time;     // Timestamp
-} event_msg_t;
-
-/*******************
- * Timer functions *
- *******************/
-
-/**
- * Generate time stamp for an async event:
- *   time:  drift compensated wall-clock time
- *   stamp: event timestamp from Timer/PWM Module
- */
-void time_stamp(ntime_t *time, uint32_t stamp)
-{
-	// todo
-}
-
-/**
- * Compensate the Real-Time-Clock oscillator for
- * temperature and drift errors. Called at 1Hz and
- * synchronous to the RTC 1Hz output.
- */
-void time_rtc_comp(void)
-{
-	// todo
-}
-
-/**
- * Synchronize the timer internal state with updates
- * from an external time sync message.
- *   ours: our internal timestamp for the event
- *   ref:  reference timestamp from the other device
- */
-void time_ext_sync(ntime_t *ours, ntime_t *ref)
-{
-	// todo
-}
-
-/************************
- * Serial I/O functions *
- ************************/
-
-/**
- * Output time sync message
- */
-void serial_send_sync(void)
-{
-}
-
-/**
- * Output external event received message
- *   event: id of the received event
- *   time:  compensated timestamp of the event
- */
-void serial_send_event(uint16_t event, ntime_t *time)
-{
-}
-
-/**
- * Process serial receive messages
- */
-void serial_receive(void)
-{
-}
-
-/********************
- * Data definitions *
- ********************/
-
-// LEDs
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-
-// UARTs         tx      rx
-Serial     uart0(USBTX,  USBRX);
-Serial     uart1(PTE0,   PTE1);
-Serial     uart2(PTE16,  PTE17);
-
-/********
- * Main *
- ********/
-
-void test_uart(void)
-{
-	char xmt[32] = "hello, world";
-	char rcv[32] = {};
-
-	printf("start\r\n");
-	for (int i = 0; xmt[i]; i++) {
-		uart1.putc(xmt[i]);
-		rcv[i] = uart2.getc();
-	}
-	printf("xmt: %s\r\n", xmt);
-	printf("rcv: %s\r\n", rcv);
-}
-
-void test_leds(void)
-{
-	led1 = 1; led2 = 0; wait(0.1);
-	led1 = 0; led2 = 1; wait(0.1);
-}
-
-int main(int argc, char **argv)
-{
-	uart0.baud(115200);
-	uart1.baud(115200);
-	uart2.baud(115200);
-
-	test_uart();
-	test_leds();
-
-	while (1) {
-		printf("tick\r\n");
-		test_leds();
-	}
-}
+#include "messages.h"
+
+#include "mbed.h"
+#include "serial_irq.h"
+#include "serial_dma.h"
+#include "timer_dma.h"
+
+/**
+ * Mode of operation:
+ *   Devices 1 and 2 synchronize clocks using serial messages.
+ *
+ *   1. Each serial message timestamped using the hardware timer capture
+ *      registers in both the sender and receiver.
+ *   2. The sender transmits the send timestamp during the next time-sync
+ *      message.
+ *   3. The receiver then compares the senders timestamp with it's own
+ *      timestamp for the corresponding messages and calculates an offset.
+ *   4. The offset is used to compensate the receivers local clock.
+ *
+ *   Time synchronization is performed in both directions.
+ */
+
+/*******************
+ * Timer functions *
+ *******************/
+
+#define NSEC_PER_SEC 1000000000ULL
+
+uint64_t time_last_local; // timestamp at last time sync
+uint64_t time_last_world; // offset at last time sync
+
+/**
+ * Generate time stamp for an async event:
+ *   time:  drift compensated wall-clock time
+ *   stamp: event timestamp from PIT Module
+ */
+uint64_t time_to_world(uint64_t local)
+{
+	uint64_t elapsed = local - time_last_local;
+	return time_last_world + elapsed;
+}
+
+uint64_t time_to_local(uint64_t world)
+{
+	uint64_t elapsed = world - time_last_world;
+	return time_last_local + elapsed;
+}
+
+/**
+ * Synchronize the timer internal state with updates
+ * from an external time sync message.
+ *   local: our internal timestamp for the event
+ *   world: reference timestamp from the other device
+ */
+void time_ext_init(uint64_t local, uint64_t world)
+{
+	sirq_printf("initialize clocks: %d -> %d\r\n",
+			(int)(local/NSEC_PER_SEC),
+			(int)(world/NSEC_PER_SEC));
+
+	time_last_local = local;
+	time_last_world = world;
+}
+
+/**
+ * Synchronize the timer internal state with updates
+ * from an external time sync message.
+ *   local: our internal timestamp for the event
+ *   world: reference timestamp from the other device
+ */
+void time_ext_sync(uint64_t local, uint64_t world)
+{
+	uint64_t guess = time_to_world(local);
+
+	time_last_local = local;
+	time_last_world = (guess/2) + (world/2);
+	//time_last_world = (guess * 3 / 4) + (world * 1 / 4);
+	//time_last_world =
+	//	(guess - (        guess / 2)) +
+	//	(world - (world - world / 2));
+	//time_last_world =
+	//	(guess - (guess - guess / 4)) +
+	//	(world - (        world / 4));
+
+	world = time_last_world;
+
+//#ifdef VERBOSE
+#if 0
+	uint64_t error = world > guess ? world - guess :
+	                 guess > world ? guess - world : 0;
+	int      ahead = guess > world;
+	sirq_printf("syncing clocks: %6d=%d.%04u -> %d.%04u (err: %s%ld.%09lu)\r\n",
+			(int)((local / NSEC_PER_SEC)),
+			(int)((guess / NSEC_PER_SEC)),
+			(int)((guess % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)),
+			(int)((world / NSEC_PER_SEC)),
+			(int)((world % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)),
+			ahead ? "-" : " ",
+			(int32_t )(error / (int64_t)NSEC_PER_SEC),
+			(uint32_t)(error % (int64_t)NSEC_PER_SEC));
+#endif
+//#endif
+}
+
+void time_printf(const char *label, uint64_t local)
+{
+	uint64_t world = time_to_world(local);
+	sirq_printf("%s -- %d.%09u -> %d.%09u\r\n",
+			label,
+			(int)(local / NSEC_PER_SEC),
+			(int)(local % NSEC_PER_SEC),
+			(int)(world / NSEC_PER_SEC),
+			(int)(world % NSEC_PER_SEC));
+}
+
+/*********************
+ * Signal generation *
+ *********************/
+
+// for 50 Mhz clock  50/1000 = 1/20  (PLL/2)
+
+// for 48 Mhz clock  48/1000 = 6/125 (FLL)
+// for 24 Mhz clock, 24/1000 = 3/125
+// for 12 Mhz clock, 12/1000 = 3/250
+// for  6 Mhz clock,  6/1000 = 3/500
+// for  3 Mhz clock,  3/1000 = 3/1000
+
+#define EMIT_PS 1
+
+//#if EMIT_PS == 0
+//#define EMIT_CLOCKS(nsec) ((uint16_t)((nsec)   / 20))
+//#define EMIT_NSEC(clocks) ((uint16_t)((clocks) * 20))
+
+#if EMIT_PS == 0
+#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 6 / 125))
+#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 125 / 6))
+#elif EMIT_PS == 1
+#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 125))
+#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 125 / 3))
+#elif EMIT_PS == 2
+#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 250))
+#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 250 / 3))
+#elif EMIT_PS == 3
+#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 500))
+#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 500 / 3))
+#elif EMIT_PS == 4
+#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 1000))
+#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 1000 / 3))
+#endif
+
+static uint32_t *emit_pcr    = 0; // transmit pin name
+
+static uint64_t  emit_start  = 0; // transmit start time (world time)
+static uint64_t  emit_period = 0; // transmit period
+static uint64_t  emit_due    = 0; // next transmit (world time)
+
+static uint32_t  emit_slack  = 0; // how far ahead we need to schedule, in us
+static uint32_t  emit_worst  = 0; // worst-case latency in task table
+
+void emit_init(int alt, PinName pin, PinMode mode)
+{
+	// Find pin
+	emit_pcr = (uint32_t*)(PORTA_BASE + pin);
+
+	// Enable clocks
+	SIM->SCGC6            |= SIM_SCGC6_TPM1_MASK;
+	SIM->SOPT2            |= SIM_SOPT2_TPMSRC(1);
+
+	// Reset PLL Source
+	//SIM->SOPT2 &= ~SIM_SOPT2_PLLFLLSEL_MASK;
+
+	// Debug print on SOPT2
+	// -- mbed may set PLLFLL when configuring UART0
+	// SOPT2: u0src=1 tpmsrc=1 USBSRC PLL/2 clkos=0 rtcos
+	sirq_printf("SOPT2: u0src=%d tpmsrc=%d %s %s clkos=%d %s\r\n",
+		(SIM->SOPT2 & SIM_SOPT2_UART0SRC_MASK)     >> SIM_SOPT2_UART0SRC_SHIFT,
+		(SIM->SOPT2 & SIM_SOPT2_TPMSRC_MASK)       >> SIM_SOPT2_TPMSRC_SHIFT,
+		(SIM->SOPT2 & SIM_SOPT2_UART0SRC_MASK)     ? "USBSRC" : "usbsrc",
+		(SIM->SOPT2 & SIM_SOPT2_PLLFLLSEL_MASK)    ? "PLL/2"  : "FLL",
+		(SIM->SOPT2 & SIM_SOPT2_CLKOUTSEL_MASK)    >> SIM_SOPT2_CLKOUTSEL_SHIFT,
+		(SIM->SOPT2 & SIM_SOPT2_RTCCLKOUTSEL_MASK) ? "RTCOS"  : "rtcos");
+
+	// Set pin mode
+	emit_pcr[0]            = PORT_PCR_ISF_MASK
+	                       | PORT_PCR_MUX(alt)
+	                       | mode;
+
+	// Setup Timer/PWM Module
+	TPM1->SC               = TPM_SC_TOF_MASK;
+	TPM1->CNT              = TPM_CNT_COUNT(0);
+	TPM1->MOD              = TPM_MOD_MOD(0xFFFF);
+
+	TPM1->CONTROLS[0].CnSC = TPM_CnSC_CHF_MASK    // clear flag
+	                       | TPM_CnSC_MSB_MASK    // set output highon match,
+	                       | TPM_CnSC_ELSB_MASK   // cleared on overflow
+	                       | TPM_CnSC_ELSA_MASK;  // ..
+
+	TPM1->STATUS           = TPM_STATUS_CH0F_MASK
+                               | TPM_STATUS_TOF_MASK;
+
+	TPM1->CONF             = TPM_CONF_CSOO_MASK;
+}
+
+void emit_enable(uint64_t start, uint64_t period)
+{
+	const int slack_clocks = 0x4000; // tune based on emit_worst
+
+	emit_start  = start;
+	emit_period = period;
+	emit_due    = start + period;
+
+	emit_slack  = EMIT_NSEC(slack_clocks);
+
+	time_printf("emit scheduled", emit_due);
+}
+
+void emit_schedule(uint64_t when)
+{
+	uint64_t local  = time_to_local(when) * 3 / 125;
+	uint32_t width  = EMIT_CLOCKS(10000);
+
+	// Disable timer
+	TPM1->SC               = TPM_SC_TOF_MASK;
+
+	__disable_irq();
+
+	uint64_t now    = ((uint64_t)~PIT->LTMR64H << 32)
+	                | ((uint64_t)~PIT->LTMR64L);
+	uint32_t delta  = local - now;
+	uint32_t start  = delta >> (EMIT_PS-1); // convert to clocks
+	uint32_t stop   = start + width;        // end time
+
+	// Set transmit time
+	TPM1->CONTROLS[0].CnV  = start;
+	TPM1->MOD              = TPM_MOD_MOD(stop);
+
+	// Start the timer
+	TPM1->SC               = TPM_SC_TOF_MASK
+	                       | TPM_SC_PS(EMIT_PS)
+	                       | TPM_SC_CMOD(1);
+
+	__enable_irq();
+
+	// Test
+	//int64_t  cnv = TPM1->CONTROLS[0].CnV;
+	//int64_t  mod = TPM1->MOD;
+	//int64_t  due = local - tdma_time();
+	//sirq_printf("%6d -- cnv=%04x mod=%04x due=%04x start=%04x\r\n",
+	//		(int)(cnv - EMIT_CLOCKS(due)),
+	//		(int)cnv, (int)mod,
+	//		(int)EMIT_CLOCKS(due), EMIT_CLOCKS(start));
+
+	// Clock testing
+	//uint32_t test_tpm0 =  TPM1->CNT;
+	//uint32_t test_pit0 = ~PIT->CHANNEL[0].CVAL;
+	//for (int i = 0; i < 100; i++)
+	//	asm("nop");
+	//uint32_t test_tpm1 =  TPM1->CNT;
+	//uint32_t test_pit1 = ~PIT->CHANNEL[0].CVAL;
+
+	//uint32_t test_tpm  = test_tpm1 - test_tpm0;
+	//uint32_t test_pit  = test_pit1 - test_pit0;
+	//sirq_printf("pit/tpm: %d - tpm=%08x/%08x=%d pit=%08x/%08x=%d\r\n",
+	//		test_tpm - test_pit,
+	//		test_tpm0, test_tpm1, test_tpm,
+	//		test_pit0, test_pit1, test_pit);
+
+	// Debug output
+	//time_printf("emitting event", when);
+}
+
+void emit_transmit(uint64_t local, uint64_t world)
+{
+	static uint64_t prev = 0;
+
+	// Record how how much time we have to reschedule
+  	if (prev && (local-prev) > emit_worst)
+  		emit_worst = (local-prev);
+	prev = local;
+
+	// Schedule task if needed
+	if (emit_due && emit_period &&
+	    world+emit_slack > emit_due) {
+		emit_schedule(emit_due);
+		emit_due += emit_period;
+	}
+}
+
+/************************
+ * Serial I/O functions *
+ ************************/
+
+typedef struct {
+	int      index;
+	int      state;
+	uint8_t  buffer[256];
+} parser_t;
+
+static uint32_t serial_device_id   = 0;
+
+const  uint64_t serial_sync_delay  = NSEC_PER_SEC / 100;
+static uint64_t serial_sync_due    = 0;
+
+static tdma_t  *serial_tdma_rcv    = NULL;
+static tdma_t  *serial_tdma_xmt    = NULL;
+
+/**
+ * Convert world to local time
+ */
+uint64_t serial_read_time(ntime_t time)
+{
+	return ((uint64_t)time.seconds) * NSEC_PER_SEC
+	     + ((uint64_t)time.nanosec);
+}
+
+ntime_t serial_write_time(uint64_t time)
+{
+	ntime_t buf = {};
+	buf.seconds = time / NSEC_PER_SEC;
+	buf.nanosec = time % NSEC_PER_SEC;
+	return buf;
+}
+
+int serial_time_stamp(tdma_t *port, uint64_t *local, uint64_t *world,
+		const char *msg)
+{
+	int valid = tdma_stamp(port, local);
+	*world = time_to_world(*local);
+
+	if (!valid)
+		sirq_printf("%s -- missing\r\n", msg);
+	//else
+	//	time_printf(msg, current);
+
+	return valid;
+}
+
+/**
+ * Output initialization message init message
+ */
+void serial_send_init(uint16_t device, uint64_t local)
+{
+}
+
+/**
+ * Output time sync message
+ */
+void serial_send_sync(sirq_t *port, uint64_t now)
+{
+	if (serial_sync_due == 0 || now < serial_sync_due)
+		return; // not ready
+
+	// Message data
+	header_t   head;
+	sync_msg_t body;
+
+	// Write header
+	head.header = MSG_HEADER;
+	head.msgid  = MSG_ID_SYNC;
+	head.length = sizeof(body);
+	head.cksum  = 0; // todo
+
+	tdma_stop(serial_tdma_rcv, 0);
+	tdma_start(serial_tdma_xmt);
+
+	sirq_write(port, &head, sizeof(head));
+
+	tdma_stop(serial_tdma_xmt, 100);
+	tdma_start(serial_tdma_rcv);
+
+	// Save transmit time
+	uint64_t local = 0, world = 0;
+	serial_time_stamp(serial_tdma_xmt, &local, &world,
+			"sync time transmit");
+
+	// Debug output
+	//sirq_printf("sync time transmit\r\n");
+	//time_printf("  local", local);
+	//time_printf("  world", world);
+
+	// Write body with updated time and send
+	body.time = serial_write_time(world);
+
+	sirq_write(port, &body, sizeof(body));
+
+	// Queue next transmit time
+	serial_sync_due  = 0;
+}
+
+/**
+ * Output external event received message
+ *   event: id of the received event
+ *   time:  compensated timestamp of the event
+ */
+void serial_send_event(sirq_t *port, uint16_t event, uint64_t local)
+{
+	//time_printf("event received", local);
+
+	// Convert timestamp
+	uint64_t world = time_to_world(local);
+	ntime_t  ltime = serial_write_time(local);
+	ntime_t  wtime = serial_write_time(world);
+
+	// Message data
+	header_t    head = {};
+	event_msg_t body = {};
+
+	// Transmit sync message
+	head.header = MSG_HEADER;
+	head.msgid  = MSG_ID_EVENT;
+	head.length = sizeof(body);
+	head.cksum  = 0; // todo
+
+	body.device = serial_device_id;
+	body.event  = event;
+	body.world  = wtime;
+	body.local  = ltime;
+
+	// Transmit message to BBB
+	sirq_write(port, &head, sizeof(head));
+	sirq_write(port, &body, sizeof(body));
+}
+
+/**
+ * Handle init message
+ */
+void serial_handle_init(init_msg_t *msg)
+{
+	sirq_printf("initialize: %s %s %s %s %s\r\n",
+		msg->valid & MSG_VALID_DEVICE ? "DEV"    : "dev",
+		msg->valid & MSG_VALID_START  ? "START"  : "start",
+		msg->valid & MSG_VALID_PERIOD ? "PERIOD" : "period",
+		msg->valid & MSG_VALID_WORLD  ? "WORLD"  : "world",
+		msg->valid & MSG_VALID_SYNC   ? "SYNC"   : "sync");
+	sirq_printf("  dev    -- %d\r\n", msg->device);
+	time_printf("  start ", serial_read_time(msg->start));
+	time_printf("  period", serial_read_time(msg->period));
+	time_printf("  world ", serial_read_time(msg->world));
+
+	if (msg->valid & MSG_VALID_DEVICE)
+		serial_device_id = msg->device;
+
+	if (msg->valid & MSG_VALID_START ||
+	    msg->valid & MSG_VALID_PERIOD) {
+		uint64_t start  = serial_read_time(msg->start);
+		uint64_t period = serial_read_time(msg->period);
+		emit_enable(start, period);
+	}
+
+	if (msg->valid & MSG_VALID_WORLD) {
+		uint64_t world = serial_read_time(msg->world);
+		uint64_t local = tdma_time();
+		time_ext_init(local, world);
+	}
+
+	if (msg->valid & MSG_VALID_SYNC)
+		serial_sync_due = tdma_time() + serial_sync_delay;
+}
+
+/**
+ * Handle sync message
+ */
+void serial_handle_sync(sync_msg_t *msg)
+{
+	// Read receive timestamp
+	uint64_t local = 0, world = 0;
+	serial_time_stamp(serial_tdma_rcv, &local, &world,
+			"sync time receive ");
+	tdma_stop(serial_tdma_rcv, 0);
+
+	// Lookup reference time from message
+	uint64_t reference = serial_read_time(msg->time);
+
+	// Debug output
+	//sirq_printf("sync time receive\r\n");
+	//time_printf("  local", local);
+	//time_printf("  world", world);
+	//time_printf("  ref  ", reference);
+
+	// Synchronize the clocks
+	time_ext_sync(local, reference);
+
+	// Queue transmit to other board
+	serial_sync_due   = tdma_time() + serial_sync_delay;
+}
+
+/**
+ * Handle event message
+ */
+void serial_handle_event(event_msg_t *msg)
+{
+}
+
+/**
+ * Deliver message
+ */
+void serial_deliver(int msgid, void *body)
+{
+	switch (msgid) {
+		case MSG_ID_INIT:
+			//sirq_printf("received init msg\r\n");
+			serial_handle_init((init_msg_t*)body);
+			break;
+		case MSG_ID_SYNC:
+			//sirq_printf("received sync msg\r\n");
+			serial_handle_sync((sync_msg_t*)body);
+			break;
+		case MSG_ID_EVENT:
+			//sirq_printf("received event msg\r\n");
+			serial_handle_event((event_msg_t*)body);
+			break;
+	}
+}
+
+/**
+ * Process serial receive messages
+ */
+void serial_receive(parser_t *parser, int byte)
+{
+	//sirq_printf("serial_receive - %02x\r\n", byte);
+
+	// Lookup pointers
+	header_t *head = (header_t*)parser->buffer;
+	void     *body = (void*)(head+1);
+	const int max_length = sizeof(parser->buffer)-sizeof(header_t);
+
+	// Process uart messages
+	parser->buffer[parser->index++] = byte;
+	switch (parser->state) {
+		case 0: // Search
+			if (parser->index == sizeof(uint16_t)) {
+				if (head->header == MSG_HEADER) {
+					parser->state = 1;
+				} else {
+					parser->buffer[0] = parser->buffer[1];
+					parser->index = 1;
+				}
+			}
+			break;
+		case 1: // Header
+			if (parser->index == sizeof(header_t)) {
+				if (head->length <= max_length &&
+				    head->msgid  <= MSG_MAX_ID) {
+					parser->state = 2;
+				} else {
+					parser->index = 0;
+					parser->state = 0;
+				}
+			}
+			break;
+		case 2: // Data
+			if (parser->index == (int)sizeof(header_t)+head->length) {
+				serial_deliver(head->msgid, body);
+				parser->index = 0;
+				parser->state = 0;
+			}
+			break;
+	}
+}
+
+/********************
+ * Data definitions *
+ ********************/
+
+// LEDs
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+// Message Parsers
+parser_t   parser_dbg;
+parser_t   parser_bbb;
+parser_t   parser_mbed;
+
+// Serial IRQ
+sirq_t    *sirq_dbg;
+sirq_t    *sirq_bbb;
+sirq_t    *sirq_mbed;
+
+// Timer DMA
+tdma_t    *tdma_evt;
+tdma_t    *tdma_rcv;
+tdma_t    *tdma_xmt;
+
+/*********
+ * Tasks *
+ *********/
+
+void task_serial(uint64_t local, uint64_t world)
+{
+	while (sirq_ready(sirq_dbg)) {
+		//sirq_printf("serial recv - dbg\r\n");
+		serial_receive(&parser_dbg,  sirq_getc(sirq_dbg));
+	}
+
+	while (sirq_ready(sirq_bbb)) {
+		//sirq_printf("serial recv - bbb\r\n");
+		serial_receive(&parser_bbb,  sirq_getc(sirq_bbb));
+	}
+
+	while (sirq_ready(sirq_mbed)) {
+		//sirq_printf("serial recv - mbed\r\n");
+		serial_receive(&parser_mbed, sirq_getc(sirq_mbed));
+	}
+}
+
+void task_events(uint64_t local, uint64_t world)
+{
+	uint64_t event = 0;
+
+#ifdef VERBOSE
+	if (tdma_stamp(tdma_evt, &event)) {
+		sirq_printf("event received - evt\r\n");
+	if (tdma_stamp(tdma_rcv, &event))
+		sirq_printf("event received - rcv\r\n");
+	if (tdma_stamp(tdma_xmt, &event))
+		sirq_printf("event received - xmt\r\n");
+#endif
+
+	if (tdma_stamp(tdma_evt, &event))
+		serial_send_event(sirq_bbb, 0, event);
+	tdma_stop(tdma_evt, 0);
+	tdma_start(tdma_evt);
+}
+
+void task_sync(uint64_t local, uint64_t world)
+{
+	serial_send_sync(sirq_mbed, local);
+}
+
+void task_leds(uint64_t local, uint64_t world)
+{
+	static uint32_t which = 0;
+	led1 = (which == 0);
+	led2 = (which == 1);
+	which ^= 1;
+}
+
+void task_emit(uint64_t local, uint64_t world)
+{
+	emit_transmit(local, world);
+}
+
+void task_debug(uint64_t local, uint64_t world)
+{
+	//tdma_debug(tdma_rcv);
+	//tdma_debug(tdma_xmt);
+
+	//sirq_debug(sirq_mbed);
+
+	//serial_send_event(sirq_bbb, 1, local);
+
+#ifdef VERBOSE
+	sirq_printf("background - %6u.%02u -> %u.%02u\r\n",
+			(uint32_t)(local / NSEC_PER_SEC),
+			(uint32_t)(local % NSEC_PER_SEC / 10000000),
+			(uint32_t)(world / NSEC_PER_SEC),
+			(uint32_t)(world % NSEC_PER_SEC / 10000000));
+#endif
+}
+
+/********
+ * Main *
+ ********/
+
+#define N_ELEM(x) (sizeof(x) / sizeof((x)[0]))
+
+extern void test_main(void);
+extern serial_t stdio_uart;
+
+static struct {
+	void (*task)(uint64_t, uint64_t);
+	uint64_t period;
+	uint64_t due;
+} tasks[] = {
+	{ task_serial, 0          }, // always
+	{ task_events, 0          }, // always -- testing
+	{ task_sync,   0          }, // always
+	{ task_emit,   0          }, // always
+	{ task_leds,   100000000  }, // 10hz
+	{ task_debug,  1000000000 }, // 1hz
+};
+
+void background(void)
+{
+	// Debugging
+	uint64_t local = tdma_time();
+	uint64_t world = time_to_world(local);
+
+	// Run the scheduler
+	for (unsigned i = 0; i < N_ELEM(tasks); i++) {
+		if (local >= tasks[i].due) {
+			tasks[i].task(local, world);
+			tasks[i].due += tasks[i].period;
+		}
+	}
+}
+
+int main(int argc, char **argv)
+{
+	tdma_init();
+
+	//pin = 1;
+
+	// Open serial ports
+	sirq_dbg   = sirq_open(SIRQ_UART0, USBTX, USBRX, 115200); // to pc
+	sirq_bbb   = sirq_open(SIRQ_UART1, PTE0,  PTE1,  115200); // to bbb
+	sirq_mbed  = sirq_open(SIRQ_UART2, PTD3,  PTD2,  115200); // to mbed
+
+	// Setup timers
+	tdma_evt   = tdma_open(TDMA_CHAN0, 3, PTC9,  PullDown); // async event
+
+	// mbed time sync
+	tdma_rcv   = tdma_open(TDMA_CHAN2, 3, PTD2,  PullUp);   // time sync rcv
+	tdma_xmt   = tdma_open(TDMA_CHAN3, 3, PTD3,  PullUp);   // time sync xmt
+
+	// host time sync
+	//tdma_rcv   = tdma_open(TDMA_CHAN2, 2, USBRX, PullUp); // time sync rcv
+	//tdma_xmt   = tdma_open(TDMA_CHAN3, 2, USBTX, PullUp); // time sync xmt
+
+	// start timers
+	tdma_start(tdma_evt);
+	tdma_start(tdma_rcv);
+	tdma_start(tdma_xmt);
+
+	// Serial timestamping
+	serial_tdma_rcv = tdma_rcv;
+	serial_tdma_xmt = tdma_xmt;
+
+	// Setup event generation
+	emit_init(3, PTE20, PullDown);
+
+	// Run background loop
+	while (true)
+		background();
+
+	// Run tests
+	//test_main();
+
+	return 0;
+}