Pileus Git - ~andy/csm213a-hw/blob - hw2/main_comm.c

   1 #include <stdint.h>
   2
   3 #include "messages.h"
   4
   5 #include "serial_irq.h"
   6 #include "timer_dma.h"
   7
   8 #include "main_time.h"
   9 #include "main_emit.h"
  10
  11 /**
  12  * Communcation overview:
  13  *
  14  *     Initialization:
  15  *        bbb --init1-->  mbed1
  16  *        bbb --init2-->  mbed1 --init2---> mbed2
  17  *
  18  *     Time sync:
  19  *        bbb             mbed1 ---sync---> mbed2
  20  *        bbb             mbed1 <--sync---- mbed2
  21  *
  22  *     Event Receive:
  23  *        bbb <--event1-- mbed1             mbed2
  24  *        bbb <--event2-- mbed1 <--event2-- mbed2
  25  *
  26  * Initialization:
  27  *     Each mbed is initialized by the BBB by receiving an initialization
  28  *     message.  The Device ID must be non-zero, and is saved for future
  29  *     messages. If the device is already initialized and a recevied Device ID
  30  *     does not match the configured Device ID, the messages is relayed to the
  31  *     second mbed.
  32  *
  33  * Event receive:
  34  *     When receiving events, an event message is sent from the mbed to the
  35  *     bbb. If the mbed receiving the event is not Device 1, the message is
  36  *     sent to mbed1 instead of the bbb.
  37  *
  38  * Debug ports:
  39  *     1. Init messages may be received from the host instead of the bbb
  40  *     2. Event messages may be sent to the host in addition to the bbb
  41  */
  42
  43 /***************************
  44  * Communication functions *
  45  ***************************/
  46
  47 static uint32_t comm_device_id   = 0;
  48
  49 const  uint64_t comm_sync_delay  = NSEC_PER_SEC / 100;
  50 static uint64_t comm_sync_due    = 0;
  51
  52 static sirq_t  *comm_sirq_dbg    = 0;
  53 static sirq_t  *comm_sirq_bbb    = 0;
  54 static sirq_t  *comm_sirq_mbed   = 0;
  55
  56 static tdma_t  *comm_tdma_rcv    = 0;
  57 static tdma_t  *comm_tdma_xmt    = 0;
  58
  59 /**
  60  * Convert world to local time
  61  */
  62 static uint64_t comm_read_time(ntime_t time)
  63 {
  64         return ((uint64_t)time.seconds) * NSEC_PER_SEC
  65              + ((uint64_t)time.nanosec);
  66 }
  67
  68 static ntime_t comm_write_time(uint64_t time)
  69 {
  70         ntime_t buf = {};
  71         buf.seconds = time / NSEC_PER_SEC;
  72         buf.nanosec = time % NSEC_PER_SEC;
  73         return buf;
  74 }
  75
  76 static int comm_time_stamp(tdma_t *port, uint64_t *local, uint64_t *world,
  77                 const char *msg)
  78 {
  79         int valid = tdma_stamp(port, local);
  80         *world = time_to_world(*local);
  81
  82         if (!valid)
  83                 sirq_printf("%s -- missing\r\n", msg);
  84         //else
  85         //      time_printf(msg, current);
  86
  87         return valid;
  88 }
  89
  90 /**
  91  * Initialization
  92  */
  93 void comm_init(sirq_t *dbg, sirq_t *bbb, sirq_t *mbed,
  94                tdma_t *rcv, tdma_t *xmt)
  95 {
  96         comm_sirq_dbg  = dbg;
  97         comm_sirq_bbb  = bbb;
  98         comm_sirq_mbed = mbed;
  99
 100         comm_tdma_rcv  = rcv;
 101         comm_tdma_xmt  = xmt;
 102 }
 103
 104
 105 /**
 106  * Output time sync message
 107  */
 108 void comm_send_sync(uint64_t local)
 109 {
 110         if (comm_sync_due == 0 || local < comm_sync_due)
 111                 return; // not ready
 112
 113         // Message data
 114         header_t   head;
 115         sync_msg_t body;
 116
 117         // Write header
 118         head.header = MSG_HEADER;
 119         head.msgid  = MSG_ID_SYNC;
 120         head.length = sizeof(body);
 121         head.cksum  = 0; // todo
 122
 123         tdma_stop(comm_tdma_rcv, 0);
 124         tdma_start(comm_tdma_xmt);
 125
 126         sirq_write(comm_sirq_mbed, &head, sizeof(head));
 127
 128         tdma_stop(comm_tdma_xmt, 100);
 129         tdma_start(comm_tdma_rcv);
 130
 131         // Save transmit time
 132         uint64_t xmt_local = 0, xmt_world = 0;
 133         comm_time_stamp(comm_tdma_xmt, &xmt_local, &xmt_world,
 134                         "sync time transmit");
 135
 136         // Debug output
 137         //sirq_printf("sync time transmit\r\n");
 138         //time_printf("  local", xmt_local);
 139         //time_printf("  world", xmt_world);
 140
 141         // Write body with updated time and send
 142         body.time = comm_write_time(xmt_world);
 143
 144         sirq_write(comm_sirq_mbed, &body, sizeof(body));
 145
 146         // Queue next transmit time
 147         comm_sync_due  = 0;
 148 }
 149
 150 /**
 151  * Output external event received message
 152  *   event: id of the received event
 153  *   time:  compensated timestamp of the event
 154  */
 155 void comm_send_event(uint16_t event, uint64_t local)
 156 {
 157         //time_printf("event received", local);
 158
 159         // Convert timestamp
 160         uint64_t world = time_to_world(local);
 161         ntime_t  ltime = comm_write_time(local);
 162         ntime_t  wtime = comm_write_time(world);
 163
 164         // Message data
 165         header_t    head = {};
 166         event_msg_t body = {};
 167
 168         // Transmit sync message
 169         head.header = MSG_HEADER;
 170         head.msgid  = MSG_ID_EVENT;
 171         head.length = sizeof(body);
 172         head.cksum  = 0; // todo
 173
 174         body.device = comm_device_id;
 175         body.event  = event;
 176         body.world  = wtime;
 177         body.local  = ltime;
 178
 179         // Transmit message to BBB
 180         if (comm_device_id == 1) {
 181                 sirq_write(comm_sirq_bbb,  &head, sizeof(head));
 182                 sirq_write(comm_sirq_bbb,  &body, sizeof(body));
 183         } else {
 184                 sirq_write(comm_sirq_mbed, &head, sizeof(head));
 185                 sirq_write(comm_sirq_mbed, &body, sizeof(body));
 186         }
 187 }
 188
 189 /**
 190  * Handle init message
 191  */
 192 void comm_handle_init(header_t *head, init_msg_t *body)
 193 {
 194         // Relay initialization from bbb to mbed
 195         if (comm_device_id && body->device != comm_device_id) {
 196                 sirq_write(comm_sirq_bbb, &head, sizeof(head));
 197                 sirq_write(comm_sirq_bbb, &body, sizeof(body));
 198                 return;
 199         }
 200
 201         // Debug output
 202         sirq_printf("initialize: %s %s %s %s %s\r\n",
 203                 body->valid & MSG_VALID_DEVICE ? "DEV"    : "dev",
 204                 body->valid & MSG_VALID_START  ? "START"  : "start",
 205                 body->valid & MSG_VALID_PERIOD ? "PERIOD" : "period",
 206                 body->valid & MSG_VALID_WORLD  ? "WORLD"  : "world",
 207                 body->valid & MSG_VALID_SYNC   ? "SYNC"   : "sync");
 208         sirq_printf("  dev    -- %d\r\n", body->device);
 209         time_printf("  start ", comm_read_time(body->start));
 210         time_printf("  period", comm_read_time(body->period));
 211         time_printf("  world ", comm_read_time(body->world));
 212
 213         // Validate message parts and initialize
 214         if (body->valid & MSG_VALID_DEVICE)
 215                 comm_device_id = body->device;
 216
 217         if (body->valid & MSG_VALID_START ||
 218             body->valid & MSG_VALID_PERIOD) {
 219                 uint64_t start  = comm_read_time(body->start);
 220                 uint64_t period = comm_read_time(body->period);
 221                 emit_enable(start, period);
 222         }
 223
 224         if (body->valid & MSG_VALID_WORLD) {
 225                 uint64_t world = comm_read_time(body->world);
 226                 uint64_t local = tdma_time();
 227                 time_ext_init(local, world);
 228         }
 229
 230         if (body->valid & MSG_VALID_SYNC)
 231                 comm_sync_due = tdma_time() + comm_sync_delay;
 232 }
 233
 234 /**
 235  * Handle sync message
 236  */
 237 void comm_handle_sync(header_t *head, sync_msg_t *body)
 238 {
 239         // Read receive timestamp
 240         uint64_t local = 0, world = 0;
 241         comm_time_stamp(comm_tdma_rcv, &local, &world,
 242                         "sync time receive ");
 243         tdma_stop(comm_tdma_rcv, 0);
 244
 245         // Lookup reference time from message
 246         uint64_t reference = comm_read_time(body->time);
 247
 248         // Debug output
 249         //sirq_printf("sync time receive\r\n");
 250         //time_printf("  local", local);
 251         //time_printf("  world", world);
 252         //time_printf("  ref  ", reference);
 253
 254         // Synchronize the clocks
 255         time_ext_sync(local, reference);
 256
 257         // Queue transmit to other board
 258         comm_sync_due   = tdma_time() + comm_sync_delay;
 259 }
 260
 261 /**
 262  * Handle event message
 263  */
 264 void comm_handle_event(header_t *head, event_msg_t *body)
 265 {
 266         // Relay event from mbed to bbb
 267         if (comm_device_id == 1) {
 268                 sirq_write(comm_sirq_bbb, &head, sizeof(head));
 269                 sirq_write(comm_sirq_bbb, &body, sizeof(body));
 270         }
 271 }