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  * Output time sync message
 106  */
 107 void comm_send_sync(uint64_t local)
 108 {
 109         if (comm_sync_due == 0 || local < comm_sync_due)
 110                 return; // not ready
 111
 112         // Message data
 113         header_t   head;
 114         sync_msg_t body;
 115
 116         // Write header
 117         head.header = MSG_HEADER;
 118         head.msgid  = MSG_ID_SYNC;
 119         head.length = sizeof(body);
 120         head.cksum  = 0; // todo
 121
 122         sirq_write(comm_sirq_mbed, &head, sizeof(head));
 123
 124         // Capture transmit time
 125         tdma_stop(comm_tdma_rcv, 0);
 126         tdma_start(comm_tdma_xmt);
 127
 128         sirq_transmit(comm_sirq_mbed);
 129
 130         tdma_stop(comm_tdma_xmt, 100);
 131         tdma_start(comm_tdma_rcv);
 132
 133         // Save transmit time
 134         uint64_t xmt_local = 0, xmt_world = 0;
 135         comm_time_stamp(comm_tdma_xmt, &xmt_local, &xmt_world,
 136                         "sync time transmit");
 137
 138         // Debug output
 139         //sirq_printf("sync time transmit\r\n");
 140         //time_printf("  local", xmt_local);
 141         //time_printf("  world", xmt_world);
 142
 143         // Write body with updated time and send
 144         body.time = comm_write_time(xmt_world);
 145
 146         sirq_write(comm_sirq_mbed, &body, sizeof(body));
 147
 148         sirq_transmit(comm_sirq_mbed);
 149
 150         // Queue next transmit time
 151         comm_sync_due  = 0;
 152 }
 153
 154 /**
 155  * Output external event received message
 156  *   event: id of the received event
 157  *   time:  compensated timestamp of the event
 158  */
 159 void comm_send_event(uint16_t event, uint64_t local)
 160 {
 161         //time_printf("event received", local);
 162
 163         // Convert timestamp
 164         uint64_t world = time_to_world(local);
 165         ntime_t  ltime = comm_write_time(local);
 166         ntime_t  wtime = comm_write_time(world);
 167
 168         // Message data
 169         header_t    head = {};
 170         event_msg_t body = {};
 171
 172         // Transmit sync message
 173         head.header = MSG_HEADER;
 174         head.msgid  = MSG_ID_EVENT;
 175         head.length = sizeof(body);
 176         head.cksum  = 0; // todo
 177
 178         body.device = comm_device_id;
 179         body.event  = event;
 180         body.world  = wtime;
 181         body.local  = ltime;
 182
 183         // Transmit message to BBB
 184         if (comm_device_id == 1) {
 185                 sirq_write(comm_sirq_bbb,  &head, sizeof(head));
 186                 sirq_write(comm_sirq_bbb,  &body, sizeof(body));
 187         } else if (comm_device_id > 1) {
 188                 sirq_write(comm_sirq_mbed, &head, sizeof(head));
 189                 sirq_write(comm_sirq_mbed, &body, sizeof(body));
 190         } else {
 191                 sirq_printf("no device id, skipping event\r\n");
 192         }
 193 }
 194
 195 /**
 196  * Handle init message
 197  */
 198 void comm_handle_init(header_t *head, init_msg_t *body)
 199 {
 200         // Relay initialization from bbb to mbed
 201         if (comm_device_id && body->device != comm_device_id) {
 202                 sirq_write(comm_sirq_bbb, &head, sizeof(head));
 203                 sirq_write(comm_sirq_bbb, &body, sizeof(body));
 204                 return;
 205         }
 206
 207         // Debug output
 208         sirq_printf("initialize: %s %s %s %s %s\r\n",
 209                 body->valid & MSG_VALID_DEVICE ? "DEV"    : "dev",
 210                 body->valid & MSG_VALID_START  ? "START"  : "start",
 211                 body->valid & MSG_VALID_PERIOD ? "PERIOD" : "period",
 212                 body->valid & MSG_VALID_WORLD  ? "WORLD"  : "world",
 213                 body->valid & MSG_VALID_SYNC   ? "SYNC"   : "sync");
 214         sirq_printf("  dev    -- %d\r\n", body->device);
 215         time_printf("  start ", comm_read_time(body->start));
 216         time_printf("  period", comm_read_time(body->period));
 217         time_printf("  world ", comm_read_time(body->world));
 218
 219         // Validate message parts and initialize
 220         if (body->valid & MSG_VALID_DEVICE)
 221                 comm_device_id = body->device;
 222
 223         if (body->valid & MSG_VALID_START ||
 224             body->valid & MSG_VALID_PERIOD) {
 225                 uint64_t start  = comm_read_time(body->start);
 226                 uint64_t period = comm_read_time(body->period);
 227                 emit_enable(start, period);
 228         }
 229
 230         if (body->valid & MSG_VALID_WORLD) {
 231                 uint64_t world = comm_read_time(body->world);
 232                 uint64_t local = tdma_time();
 233                 time_ext_init(local, world);
 234         }
 235
 236         if (body->valid & MSG_VALID_SYNC)
 237                 comm_sync_due = tdma_time() + comm_sync_delay;
 238 }
 239
 240 /**
 241  * Handle sync message
 242  */
 243 void comm_handle_sync(header_t *head, sync_msg_t *body)
 244 {
 245         // Read receive timestamp
 246         uint64_t local = 0, world = 0;
 247         comm_time_stamp(comm_tdma_rcv, &local, &world,
 248                         "sync time receive ");
 249         tdma_stop(comm_tdma_rcv, 0);
 250
 251         // Lookup reference time from message
 252         uint64_t reference = comm_read_time(body->time);
 253
 254         // Debug output
 255         //sirq_printf("sync time receive\r\n");
 256         //time_printf("  local", local);
 257         //time_printf("  world", world);
 258         //time_printf("  ref  ", reference);
 259
 260         // Synchronize the clocks
 261         time_ext_sync(local, reference);
 262
 263         // Queue transmit to other board
 264         comm_sync_due   = tdma_time() + comm_sync_delay;
 265 }
 266
 267 /**
 268  * Handle event message
 269  */
 270 void comm_handle_event(header_t *head, event_msg_t *body)
 271 {
 272         // Relay event from mbed to bbb
 273         if (comm_device_id == 1) {
 274                 sirq_write(comm_sirq_bbb, &head, sizeof(head));
 275                 sirq_write(comm_sirq_bbb, &body, sizeof(body));
 276         }
 277 }