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_relay_mode) {
 186                 sirq_write(comm_sirq_mbed, &head, sizeof(head));
 187                 sirq_write(comm_sirq_mbed, &body, sizeof(body));
 188         } else {
 189                 sirq_write(comm_sirq_bbb,  &head, sizeof(head));
 190                 sirq_write(comm_sirq_bbb,  &body, sizeof(body));
 191         }
 192 }
 193
 194 /**
 195  * Handle init message
 196  */
 197 void comm_handle_init(header_t *head, init_msg_t *body)
 198 {
 199         // Relay initialization from bbb to mbed
 200         if (comm_device_id && body->device != comm_device_id) {
 201                 //sirq_printf("relaying init\r\n");
 202
 203                 sirq_write(comm_sirq_mbed, head, sizeof(*head));
 204                 sirq_write(comm_sirq_mbed, body, sizeof(*body));
 205
 206                 // Normally we transmit during the time sync but
 207                 // if we haven't started syncing yet, we need to
 208                 // push out the message now.
 209                 if (!comm_sync_due)
 210                         sirq_transmit(comm_sirq_mbed);
 211                 return;
 212         }
 213
 214         // Debug output
 215         sirq_printf("initialize: %s %s %s %s %s\r\n",
 216                 body->control & MSG_CTL_VALID_DEVICE ? "DEV"    : "dev",
 217                 body->control & MSG_CTL_VALID_START  ? "START"  : "start",
 218                 body->control & MSG_CTL_VALID_PERIOD ? "PERIOD" : "period",
 219                 body->control & MSG_CTL_VALID_WORLD  ? "WORLD"  : "world",
 220                 body->control & MSG_CTL_VALID_SYNC   ? "SYNC"   : "sync");
 221         sirq_printf("  dev    -- %d\r\n", body->device);
 222         time_printf("  start ", comm_read_time(body->start));
 223         time_printf("  period", comm_read_time(body->period));
 224         time_printf("  world ", comm_read_time(body->world));
 225
 226         // Validate message parts and initialize
 227         if (body->control & MSG_CTL_VALID_DEVICE)
 228                 comm_device_id = body->device;
 229
 230         if (body->control & MSG_CTL_VALID_START)
 231                 emit_set_start(comm_read_time(body->start));
 232
 233         if (body->control & MSG_CTL_VALID_PERIOD)
 234                 emit_set_period(comm_read_time(body->period));
 235
 236         if (body->control & MSG_CTL_VALID_WORLD)
 237                 time_ext_init(tdma_time(), comm_read_time(body->world));
 238
 239         if (body->control & MSG_CTL_RELAY_MODE)
 240                 comm_relay_mode = 1;
 241         else
 242                 comm_relay_mode = 0;
 243
 244         if (body->control & MSG_CTL_VALID_SYNC)
 245                 comm_sync_due = tdma_time() + comm_sync_delay;
 246 }
 247
 248 /**
 249  * Handle sync message
 250  */
 251 void comm_handle_sync(header_t *head, sync_msg_t *body)
 252 {
 253         // Read receive timestamp
 254         uint64_t local = 0, world = 0;
 255         comm_time_stamp(comm_tdma_rcv, &local, &world,
 256                         "sync time receive ");
 257         tdma_stop(comm_tdma_rcv, 0);
 258
 259         // Lookup reference time from message
 260         uint64_t reference = comm_read_time(body->time);
 261
 262         // Debug output
 263         //sirq_printf("sync time receive\r\n");
 264         //time_printf("  local", local);
 265         //time_printf("  world", world);
 266         //time_printf("  ref  ", reference);
 267
 268         // Synchronize the clocks
 269         time_ext_sync(local, reference);
 270
 271         // Queue transmit to other board
 272         comm_sync_due   = tdma_time() + comm_sync_delay;
 273 }
 274
 275 /**
 276  * Handle event message
 277  */
 278 void comm_handle_event(header_t *head, event_msg_t *body)
 279 {
 280         // Relay events from other mbeds
 281         sirq_write(comm_sirq_bbb, head, sizeof(*head));
 282         sirq_write(comm_sirq_bbb, body, sizeof(*body));
 283 }