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  * Communication functions *
  13  ***************************/
  14
  15 static uint32_t comm_device_id   = 0;
  16
  17 const  uint64_t comm_sync_delay  = NSEC_PER_SEC / 100;
  18 static uint64_t comm_sync_due    = 0;
  19
  20 static sirq_t  *comm_sirq_dbg    = 0;
  21 static sirq_t  *comm_sirq_bbb    = 0;
  22 static sirq_t  *comm_sirq_mbed   = 0;
  23
  24 static tdma_t  *comm_tdma_rcv    = 0;
  25 static tdma_t  *comm_tdma_xmt    = 0;
  26
  27 /**
  28  * Initialization
  29  */
  30 void comm_init(sirq_t *dbg, sirq_t *bbb, sirq_t *mbed,
  31                tdma_t *rcv, tdma_t *xmt)
  32 {
  33         comm_sirq_dbg  = dbg;
  34         comm_sirq_bbb  = bbb;
  35         comm_sirq_mbed = mbed;
  36
  37         comm_tdma_rcv  = rcv;
  38         comm_tdma_xmt  = xmt;
  39 }
  40
  41 /**
  42  * Convert world to local time
  43  */
  44 uint64_t comm_read_time(ntime_t time)
  45 {
  46         return ((uint64_t)time.seconds) * NSEC_PER_SEC
  47              + ((uint64_t)time.nanosec);
  48 }
  49
  50 ntime_t comm_write_time(uint64_t time)
  51 {
  52         ntime_t buf = {};
  53         buf.seconds = time / NSEC_PER_SEC;
  54         buf.nanosec = time % NSEC_PER_SEC;
  55         return buf;
  56 }
  57
  58 int comm_time_stamp(tdma_t *port, uint64_t *local, uint64_t *world,
  59                 const char *msg)
  60 {
  61         int valid = tdma_stamp(port, local);
  62         *world = time_to_world(*local);
  63
  64         if (!valid)
  65                 sirq_printf("%s -- missing\r\n", msg);
  66         //else
  67         //      time_printf(msg, current);
  68
  69         return valid;
  70 }
  71
  72 /**
  73  * Output initialization message init message
  74  */
  75 void comm_send_init(uint16_t device, uint64_t local)
  76 {
  77 }
  78
  79 /**
  80  * Output time sync message
  81  */
  82 void comm_send_sync(sirq_t *port, uint64_t now)
  83 {
  84         if (comm_sync_due == 0 || now < comm_sync_due)
  85                 return; // not ready
  86
  87         // Message data
  88         header_t   head;
  89         sync_msg_t body;
  90
  91         // Write header
  92         head.header = MSG_HEADER;
  93         head.msgid  = MSG_ID_SYNC;
  94         head.length = sizeof(body);
  95         head.cksum  = 0; // todo
  96
  97         tdma_stop(comm_tdma_rcv, 0);
  98         tdma_start(comm_tdma_xmt);
  99
 100         sirq_write(port, &head, sizeof(head));
 101
 102         tdma_stop(comm_tdma_xmt, 100);
 103         tdma_start(comm_tdma_rcv);
 104
 105         // Save transmit time
 106         uint64_t local = 0, world = 0;
 107         comm_time_stamp(comm_tdma_xmt, &local, &world,
 108                         "sync time transmit");
 109
 110         // Debug output
 111         //sirq_printf("sync time transmit\r\n");
 112         //time_printf("  local", local);
 113         //time_printf("  world", world);
 114
 115         // Write body with updated time and send
 116         body.time = comm_write_time(world);
 117
 118         sirq_write(port, &body, sizeof(body));
 119
 120         // Queue next transmit time
 121         comm_sync_due  = 0;
 122 }
 123
 124 /**
 125  * Output external event received message
 126  *   event: id of the received event
 127  *   time:  compensated timestamp of the event
 128  */
 129 void comm_send_event(sirq_t *port, uint16_t event, uint64_t local)
 130 {
 131         //time_printf("event received", local);
 132
 133         // Convert timestamp
 134         uint64_t world = time_to_world(local);
 135         ntime_t  ltime = comm_write_time(local);
 136         ntime_t  wtime = comm_write_time(world);
 137
 138         // Message data
 139         header_t    head = {};
 140         event_msg_t body = {};
 141
 142         // Transmit sync message
 143         head.header = MSG_HEADER;
 144         head.msgid  = MSG_ID_EVENT;
 145         head.length = sizeof(body);
 146         head.cksum  = 0; // todo
 147
 148         body.device = comm_device_id;
 149         body.event  = event;
 150         body.world  = wtime;
 151         body.local  = ltime;
 152
 153         // Transmit message to BBB
 154         sirq_write(port, &head, sizeof(head));
 155         sirq_write(port, &body, sizeof(body));
 156 }
 157
 158 /**
 159  * Handle init message
 160  */
 161 void comm_handle_init(int msgid, init_msg_t *body)
 162 {
 163         sirq_printf("initialize: %s %s %s %s %s\r\n",
 164                 body->valid & MSG_VALID_DEVICE ? "DEV"    : "dev",
 165                 body->valid & MSG_VALID_START  ? "START"  : "start",
 166                 body->valid & MSG_VALID_PERIOD ? "PERIOD" : "period",
 167                 body->valid & MSG_VALID_WORLD  ? "WORLD"  : "world",
 168                 body->valid & MSG_VALID_SYNC   ? "SYNC"   : "sync");
 169         sirq_printf("  dev    -- %d\r\n", body->device);
 170         time_printf("  start ", comm_read_time(body->start));
 171         time_printf("  period", comm_read_time(body->period));
 172         time_printf("  world ", comm_read_time(body->world));
 173
 174         if (body->valid & MSG_VALID_DEVICE)
 175                 comm_device_id = body->device;
 176
 177         if (body->valid & MSG_VALID_START ||
 178             body->valid & MSG_VALID_PERIOD) {
 179                 uint64_t start  = comm_read_time(body->start);
 180                 uint64_t period = comm_read_time(body->period);
 181                 emit_enable(start, period);
 182         }
 183
 184         if (body->valid & MSG_VALID_WORLD) {
 185                 uint64_t world = comm_read_time(body->world);
 186                 uint64_t local = tdma_time();
 187                 time_ext_init(local, world);
 188         }
 189
 190         if (body->valid & MSG_VALID_SYNC)
 191                 comm_sync_due = tdma_time() + comm_sync_delay;
 192 }
 193
 194 /**
 195  * Handle sync message
 196  */
 197 void comm_handle_sync(int msgid, sync_msg_t *body)
 198 {
 199         // Read receive timestamp
 200         uint64_t local = 0, world = 0;
 201         comm_time_stamp(comm_tdma_rcv, &local, &world,
 202                         "sync time receive ");
 203         tdma_stop(comm_tdma_rcv, 0);
 204
 205         // Lookup reference time from message
 206         uint64_t reference = comm_read_time(body->time);
 207
 208         // Debug output
 209         //sirq_printf("sync time receive\r\n");
 210         //time_printf("  local", local);
 211         //time_printf("  world", world);
 212         //time_printf("  ref  ", reference);
 213
 214         // Synchronize the clocks
 215         time_ext_sync(local, reference);
 216
 217         // Queue transmit to other board
 218         comm_sync_due   = tdma_time() + comm_sync_delay;
 219 }
 220
 221 /**
 222  * Handle event message
 223  */
 224 void comm_handle_event(header_t *head, event_msg_t *body)
 225 {
 226         // Relay event from mbed to bbb
 227         if (comm_device_id == 1) {
 228                 sirq_write(comm_sirq_bbb, &head, sizeof(head));
 229                 sirq_write(comm_sirq_bbb, &body, sizeof(body));
 230         }
 231 }