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 int      comm_device_id   = 0;
  48 static int      comm_relay_mode  = 0;
  49
  50 const  uint64_t comm_sync_delay  = NSEC_PER_SEC / 100;
  51 static uint64_t comm_sync_due    = 0;
  52
  53 static sirq_t  *comm_sirq_dbg    = 0;
  54 static sirq_t  *comm_sirq_bbb    = 0;
  55 static sirq_t  *comm_sirq_mbed   = 0;
  56
  57 static tdma_t  *comm_tdma_rcv    = 0;
  58 static tdma_t  *comm_tdma_xmt    = 0;
  59
  60 /**
  61  * Convert world to local time
  62  */
  63 static uint64_t comm_read_time(ntime_t time)
  64 {
  65         return ((uint64_t)time.seconds) * NSEC_PER_SEC
  66              + ((uint64_t)time.nanosec);
  67 }
  68
  69 static ntime_t comm_write_time(uint64_t time)
  70 {
  71         ntime_t buf = {};
  72         buf.seconds = time / NSEC_PER_SEC;
  73         buf.nanosec = time % NSEC_PER_SEC;
  74         return buf;
  75 }
  76
  77 static int comm_time_stamp(tdma_t *port, uint64_t *local, uint64_t *world,
  78                 const char *msg)
  79 {
  80         int valid = tdma_stamp(port, local);
  81         *world = time_to_world(*local);
  82
  83         if (!valid)
  84                 sirq_printf("%s -- missing\r\n", msg);
  85         //else
  86         //      time_printf(msg, current);
  87
  88         return valid;
  89 }
  90
  91 /**
  92  * Initialization
  93  */
  94 void comm_init(sirq_t *dbg, sirq_t *bbb, sirq_t *mbed,
  95                tdma_t *rcv, tdma_t *xmt)
  96 {
  97         comm_sirq_dbg  = dbg;
  98         comm_sirq_bbb  = bbb;
  99         comm_sirq_mbed = mbed;
 100
 101         comm_tdma_rcv  = rcv;
 102         comm_tdma_xmt  = xmt;
 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         sirq_write(comm_sirq_mbed, &head, sizeof(head));
 124
 125         // Capture transmit time
 126         tdma_stop(comm_tdma_rcv, 0);
 127         tdma_start(comm_tdma_xmt);
 128
 129         sirq_transmit(comm_sirq_mbed);
 130
 131         tdma_stop(comm_tdma_xmt, 100);
 132         tdma_start(comm_tdma_rcv);
 133
 134         // Save transmit time
 135         uint64_t xmt_local = 0, xmt_world = 0;
 136         comm_time_stamp(comm_tdma_xmt, &xmt_local, &xmt_world,
 137                         "sync time transmit");
 138
 139         // Debug output
 140         //sirq_printf("sync time transmit\r\n");
 141         //time_printf("  local", xmt_local);
 142         //time_printf("  world", xmt_world);
 143
 144         // Write body with updated time and send
 145         body.time = comm_write_time(xmt_world);
 146
 147         sirq_write(comm_sirq_mbed, &body, sizeof(body));
 148
 149         sirq_transmit(comm_sirq_mbed);
 150
 151         // Queue next transmit time
 152         comm_sync_due  = 0;
 153 }
 154
 155 /**
 156  * Output external event received message
 157  *   event: id of the received event
 158  *   time:  compensated timestamp of the event
 159  */
 160 void comm_send_event(uint16_t event, uint64_t local)
 161 {
 162         //time_printf("event received", local);
 163
 164         // Convert timestamp
 165         uint64_t world = time_to_world(local);
 166         ntime_t  ltime = comm_write_time(local);
 167         ntime_t  wtime = comm_write_time(world);
 168
 169         // Message data
 170         header_t    head = {};
 171         event_msg_t body = {};
 172
 173         // Transmit sync message
 174         head.header = MSG_HEADER;
 175         head.msgid  = MSG_ID_EVENT;
 176         head.length = sizeof(body);
 177         head.cksum  = 0; // todo
 178
 179         body.device = comm_device_id;
 180         body.event  = event;
 181         body.world  = wtime;
 182         body.local  = ltime;
 183
 184         // Transmit message to BBB
 185         if (comm_device_id == 1) {
 186                 sirq_write(comm_sirq_bbb,  &head, sizeof(head));
 187                 sirq_write(comm_sirq_bbb,  &body, sizeof(body));
 188         } else if (comm_device_id > 1) {
 189                 sirq_write(comm_sirq_mbed, &head, sizeof(head));
 190                 sirq_write(comm_sirq_mbed, &body, sizeof(body));
 191         } else {
 192                 sirq_printf("no device id, skipping event\r\n");
 193         }
 194 }
 195
 196 /**
 197  * Handle init message
 198  */
 199 void comm_handle_init(header_t *head, init_msg_t *body)
 200 {
 201         // Relay initialization from bbb to mbed
 202         if (comm_device_id && body->device != comm_device_id) {
 203                 //sirq_printf("relaying init\r\n");
 204
 205                 sirq_write(comm_sirq_mbed, head, sizeof(*head));
 206                 sirq_write(comm_sirq_mbed, body, sizeof(*body));
 207
 208                 // Normally we transmit during the time sync but
 209                 // if we haven't started syncing yet, we need to
 210                 // push out the message now.
 211                 if (!comm_sync_due)
 212                         sirq_transmit(comm_sirq_mbed);
 213                 return;
 214         }
 215
 216         // Debug output
 217         sirq_printf("initialize: %s %s %s %s %s\r\n",
 218                 body->control & MSG_CTL_VALID_DEVICE ? "DEV"    : "dev",
 219                 body->control & MSG_CTL_VALID_START  ? "START"  : "start",
 220                 body->control & MSG_CTL_VALID_PERIOD ? "PERIOD" : "period",
 221                 body->control & MSG_CTL_VALID_WORLD  ? "WORLD"  : "world",
 222                 body->control & MSG_CTL_VALID_SYNC   ? "SYNC"   : "sync");
 223         sirq_printf("  dev    -- %d\r\n", body->device);
 224         time_printf("  start ", comm_read_time(body->start));
 225         time_printf("  period", comm_read_time(body->period));
 226         time_printf("  world ", comm_read_time(body->world));
 227
 228         // Validate message parts and initialize
 229         if (body->control & MSG_CTL_VALID_DEVICE)
 230                 comm_device_id = body->device;
 231
 232         if (body->control & MSG_CTL_VALID_START ||
 233             body->control & MSG_CTL_VALID_PERIOD) {
 234                 uint64_t start  = comm_read_time(body->start);
 235                 uint64_t period = comm_read_time(body->period);
 236                 emit_enable(start, period);
 237         }
 238
 239         if (body->control & MSG_CTL_VALID_WORLD) {
 240                 uint64_t world = comm_read_time(body->world);
 241                 uint64_t local = tdma_time();
 242                 time_ext_init(local, world);
 243         }
 244
 245         if (body->control & MSG_CTL_RELAY_MODE)
 246                 comm_relay_mode = 1;
 247         else
 248                 comm_relay_mode = 0;
 249
 250         if (body->control & MSG_CTL_VALID_SYNC)
 251                 comm_sync_due = tdma_time() + comm_sync_delay;
 252 }
 253
 254 /**
 255  * Handle sync message
 256  */
 257 void comm_handle_sync(header_t *head, sync_msg_t *body)
 258 {
 259         // Read receive timestamp
 260         uint64_t local = 0, world = 0;
 261         comm_time_stamp(comm_tdma_rcv, &local, &world,
 262                         "sync time receive ");
 263         tdma_stop(comm_tdma_rcv, 0);
 264
 265         // Lookup reference time from message
 266         uint64_t reference = comm_read_time(body->time);
 267
 268         // Debug output
 269         //sirq_printf("sync time receive\r\n");
 270         //time_printf("  local", local);
 271         //time_printf("  world", world);
 272         //time_printf("  ref  ", reference);
 273
 274         // Synchronize the clocks
 275         time_ext_sync(local, reference);
 276
 277         // Queue transmit to other board
 278         comm_sync_due   = tdma_time() + comm_sync_delay;
 279 }
 280
 281 /**
 282  * Handle event message
 283  */
 284 void comm_handle_event(header_t *head, event_msg_t *body)
 285 {
 286         // Relay events from other mbeds
 287         sirq_write(comm_sirq_bbb, head, sizeof(*head));
 288         sirq_write(comm_sirq_bbb, body, sizeof(*body));
 289 }