world = time_last_world;
//#ifdef VERBOSE
-#if 0
+#if 1
uint64_t error = world > guess ? world - guess :
guess > world ? guess - world : 0;
int ahead = guess > world;
* Serial I/O functions *
************************/
-typedef struct {
- int index;
- int state;
- uint8_t buffer[256];
-} parser_t;
-
static uint32_t serial_device_id = 0;
const uint64_t serial_sync_delay = NSEC_PER_SEC / 100;
/**
* Handle init message
*/
-void serial_handle_init(init_msg_t *msg)
+void serial_handle_init(int msgid, init_msg_t *msg)
{
sirq_printf("initialize: %s %s %s %s %s\r\n",
msg->valid & MSG_VALID_DEVICE ? "DEV" : "dev",
/**
* Handle sync message
*/
-void serial_handle_sync(sync_msg_t *msg)
+void serial_handle_sync(int msgid, sync_msg_t *msg)
{
// Read receive timestamp
uint64_t local = 0, world = 0;
/**
* Handle event message
*/
-void serial_handle_event(event_msg_t *msg)
-{
-}
-
-/**
- * Deliver message
- */
-void serial_deliver(int msgid, void *body)
+void serial_handle_event(int msgid, event_msg_t *msg)
{
- switch (msgid) {
- case MSG_ID_INIT:
- //sirq_printf("received init msg\r\n");
- serial_handle_init((init_msg_t*)body);
- break;
- case MSG_ID_SYNC:
- //sirq_printf("received sync msg\r\n");
- serial_handle_sync((sync_msg_t*)body);
- break;
- case MSG_ID_EVENT:
- //sirq_printf("received event msg\r\n");
- serial_handle_event((event_msg_t*)body);
- break;
- }
-}
-
-/**
- * Process serial receive messages
- */
-void serial_receive(parser_t *parser, int byte)
-{
- //sirq_printf("serial_receive - %02x\r\n", byte);
-
- // Lookup pointers
- header_t *head = (header_t*)parser->buffer;
- void *body = (void*)(head+1);
- const int max_length = sizeof(parser->buffer)-sizeof(header_t);
-
- // Process uart messages
- parser->buffer[parser->index++] = byte;
- switch (parser->state) {
- case 0: // Search
- if (parser->index == sizeof(uint16_t)) {
- if (head->header == MSG_HEADER) {
- parser->state = 1;
- } else {
- parser->buffer[0] = parser->buffer[1];
- parser->index = 1;
- }
- }
- break;
- case 1: // Header
- if (parser->index == sizeof(header_t)) {
- if (head->length <= max_length &&
- head->msgid <= MSG_MAX_ID) {
- parser->state = 2;
- } else {
- parser->index = 0;
- parser->state = 0;
- }
- }
- break;
- case 2: // Data
- if (parser->index == (int)sizeof(header_t)+head->length) {
- serial_deliver(head->msgid, body);
- parser->index = 0;
- parser->state = 0;
- }
- break;
- }
}
/********************
{
while (sirq_ready(sirq_dbg)) {
//sirq_printf("serial recv - dbg\r\n");
- serial_receive(&parser_dbg, sirq_getc(sirq_dbg));
+ msg_receive(&parser_dbg, sirq_getc(sirq_dbg));
}
while (sirq_ready(sirq_bbb)) {
//sirq_printf("serial recv - bbb\r\n");
- serial_receive(&parser_bbb, sirq_getc(sirq_bbb));
+ msg_receive(&parser_bbb, sirq_getc(sirq_bbb));
}
while (sirq_ready(sirq_mbed)) {
//sirq_printf("serial recv - mbed\r\n");
- serial_receive(&parser_mbed, sirq_getc(sirq_mbed));
+ msg_receive(&parser_mbed, sirq_getc(sirq_mbed));
}
}
//sirq_debug(sirq_mbed);
- //serial_send_event(sirq_bbb, 1, local);
+ serial_send_event(sirq_bbb, 1, local);
#ifdef VERBOSE
sirq_printf("background - %6u.%02u -> %u.%02u\r\n",
{
tdma_init();
- //pin = 1;
-
// Open serial ports
sirq_dbg = sirq_open(SIRQ_UART0, USBTX, USBRX, 115200); // to pc
sirq_bbb = sirq_open(SIRQ_UART1, PTE0, PTE1, 115200); // to bbb
// Setup timers
tdma_evt = tdma_open(TDMA_CHAN0, 3, PTC9, PullDown); // async event
-
- // mbed time sync
tdma_rcv = tdma_open(TDMA_CHAN2, 3, PTD2, PullUp); // time sync rcv
tdma_xmt = tdma_open(TDMA_CHAN3, 3, PTD3, PullUp); // time sync xmt
- // host time sync
- //tdma_rcv = tdma_open(TDMA_CHAN2, 2, USBRX, PullUp); // time sync rcv
- //tdma_xmt = tdma_open(TDMA_CHAN3, 2, USBTX, PullUp); // time sync xmt
+ // Register messages
+ msg_register(&parser_dbg, MSG_ID_INIT, (handler_t)serial_handle_init);
+ msg_register(&parser_dbg, MSG_ID_SYNC, (handler_t)serial_handle_sync);
+ msg_register(&parser_dbg, MSG_ID_EVENT, (handler_t)serial_handle_event);
+ msg_register(&parser_bbb, MSG_ID_INIT, (handler_t)serial_handle_init);
+ msg_register(&parser_mbed, MSG_ID_SYNC, (handler_t)serial_handle_sync);
// start timers
tdma_start(tdma_evt);