--- /dev/null
+#include <stdint.h>
+
+#include "messages.h"
+
+#include "serial_irq.h"
+#include "timer_dma.h"
+
+#include "main_time.h"
+#include "main_emit.h"
+
+/***************************
+ * Communication functions *
+ ***************************/
+
+static uint32_t comm_device_id = 0;
+
+const uint64_t comm_sync_delay = NSEC_PER_SEC / 100;
+static uint64_t comm_sync_due = 0;
+
+static sirq_t *comm_sirq_dbg = 0;
+static sirq_t *comm_sirq_bbb = 0;
+static sirq_t *comm_sirq_mbed = 0;
+
+static tdma_t *comm_tdma_rcv = 0;
+static tdma_t *comm_tdma_xmt = 0;
+
+/**
+ * Initialization
+ */
+void comm_init(sirq_t *dbg, sirq_t *bbb, sirq_t *mbed,
+ tdma_t *rcv, tdma_t *xmt)
+{
+ comm_sirq_dbg = dbg;
+ comm_sirq_bbb = bbb;
+ comm_sirq_mbed = mbed;
+
+ comm_tdma_rcv = rcv;
+ comm_tdma_xmt = xmt;
+}
+
+/**
+ * Convert world to local time
+ */
+uint64_t comm_read_time(ntime_t time)
+{
+ return ((uint64_t)time.seconds) * NSEC_PER_SEC
+ + ((uint64_t)time.nanosec);
+}
+
+ntime_t comm_write_time(uint64_t time)
+{
+ ntime_t buf = {};
+ buf.seconds = time / NSEC_PER_SEC;
+ buf.nanosec = time % NSEC_PER_SEC;
+ return buf;
+}
+
+int comm_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 comm_send_init(uint16_t device, uint64_t local)
+{
+}
+
+/**
+ * Output time sync message
+ */
+void comm_send_sync(sirq_t *port, uint64_t now)
+{
+ if (comm_sync_due == 0 || now < comm_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(comm_tdma_rcv, 0);
+ tdma_start(comm_tdma_xmt);
+
+ sirq_write(port, &head, sizeof(head));
+
+ tdma_stop(comm_tdma_xmt, 100);
+ tdma_start(comm_tdma_rcv);
+
+ // Save transmit time
+ uint64_t local = 0, world = 0;
+ comm_time_stamp(comm_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 = comm_write_time(world);
+
+ sirq_write(port, &body, sizeof(body));
+
+ // Queue next transmit time
+ comm_sync_due = 0;
+}
+
+/**
+ * Output external event received message
+ * event: id of the received event
+ * time: compensated timestamp of the event
+ */
+void comm_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 = comm_write_time(local);
+ ntime_t wtime = comm_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 = comm_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 comm_handle_init(int msgid, init_msg_t *body)
+{
+ sirq_printf("initialize: %s %s %s %s %s\r\n",
+ body->valid & MSG_VALID_DEVICE ? "DEV" : "dev",
+ body->valid & MSG_VALID_START ? "START" : "start",
+ body->valid & MSG_VALID_PERIOD ? "PERIOD" : "period",
+ body->valid & MSG_VALID_WORLD ? "WORLD" : "world",
+ body->valid & MSG_VALID_SYNC ? "SYNC" : "sync");
+ sirq_printf(" dev -- %d\r\n", body->device);
+ time_printf(" start ", comm_read_time(body->start));
+ time_printf(" period", comm_read_time(body->period));
+ time_printf(" world ", comm_read_time(body->world));
+
+ if (body->valid & MSG_VALID_DEVICE)
+ comm_device_id = body->device;
+
+ if (body->valid & MSG_VALID_START ||
+ body->valid & MSG_VALID_PERIOD) {
+ uint64_t start = comm_read_time(body->start);
+ uint64_t period = comm_read_time(body->period);
+ emit_enable(start, period);
+ }
+
+ if (body->valid & MSG_VALID_WORLD) {
+ uint64_t world = comm_read_time(body->world);
+ uint64_t local = tdma_time();
+ time_ext_init(local, world);
+ }
+
+ if (body->valid & MSG_VALID_SYNC)
+ comm_sync_due = tdma_time() + comm_sync_delay;
+}
+
+/**
+ * Handle sync message
+ */
+void comm_handle_sync(int msgid, sync_msg_t *body)
+{
+ // Read receive timestamp
+ uint64_t local = 0, world = 0;
+ comm_time_stamp(comm_tdma_rcv, &local, &world,
+ "sync time receive ");
+ tdma_stop(comm_tdma_rcv, 0);
+
+ // Lookup reference time from message
+ uint64_t reference = comm_read_time(body->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
+ comm_sync_due = tdma_time() + comm_sync_delay;
+}
+
+/**
+ * Handle event message
+ */
+void comm_handle_event(header_t *head, event_msg_t *body)
+{
+ // Relay event from mbed to bbb
+ if (comm_device_id == 1) {
+ sirq_write(comm_sirq_bbb, &head, sizeof(head));
+ sirq_write(comm_sirq_bbb, &body, sizeof(body));
+ }
+}
projects / ~andy / csm213a-hw / blobdiff