Pileus Git - ~andy/csm213a-hw/blob - hw2/main.cpp

   1 #include "messages.h"
   2
   3 #include "mbed.h"
   4 #include "serial_irq.h"
   5 #include "serial_dma.h"
   6 #include "timer_dma.h"
   7
   8 /**
   9  * Mode of operation:
  10  *   Devices 1 and 2 synchronize clocks using serial messages.
  11  *
  12  *   1. Each serial message timestamped using the hardware timer capture
  13  *      registers in both the sender and receiver.
  14  *   2. The sender transmits the send timestamp during the next time-sync
  15  *      message.
  16  *   3. The receiver then compares the senders timestamp with it's own
  17  *      timestamp for the corresponding messages and calculates an offset.
  18  *   4. The offset is used to compensate the receivers local clock.
  19  *
  20  *   Time synchronization is performed in both directions.
  21  */
  22
  23 /****************
  24  * Testing vars *
  25  ****************/
  26
  27 uint32_t test_xmt_enab  = 0;
  28 uint64_t test_xmt_time0 = 0;
  29 uint64_t test_xmt_time1 = 0;
  30
  31 uint32_t test_rcv_enab  = 0;
  32 uint64_t test_rcv_time  = 0;
  33
  34 /*******************
  35  * Timer functions *
  36  *******************/
  37
  38 #define NSEC_PER_SEC 1000000000ULL
  39
  40 uint64_t time_last_local; // timestamp at last time sync
  41 uint64_t time_last_world; // offset at last time sync
  42
  43 /**
  44  * Generate time stamp for an async event:
  45  *   local: drift compensated wall-clock time
  46  *   world: nanoseconds in world time world
  47  *   valid: local timestamp at valid valid
  48  */
  49 //uint64_t time_to_local(uint64_t world, uint64_t valid)
  50 //{
  51 //      uint64_t now =
  52 //      local =  + (stamp);
  53 //}
  54
  55 /**
  56  * Generate time stamp for an async event:
  57  *   time:  drift compensated wall-clock time
  58  *   stamp: event timestamp from PIT Module
  59  */
  60 uint64_t time_to_world(uint64_t local)
  61 {
  62         uint64_t elapsed = local - time_last_local;
  63         return time_last_world + elapsed;
  64 }
  65
  66 /**
  67  * Compensate the Real-Time-Clock oscillator for
  68  * temperature and drift errors. Called at 1Hz and
  69  * synchronous to the RTC 1Hz output.
  70  */
  71 void time_rtc_comp(void)
  72 {
  73         // todo
  74 }
  75
  76 /**
  77  * Synchronize the timer internal state with updates
  78  * from an external time sync message.
  79  *   local: our internal timestamp for the event
  80  *   world: reference timestamp from the other device
  81  */
  82 void time_ext_init(uint64_t local, uint64_t world)
  83 {
  84         sirq_printf("initialize clocks: %d -> %d\r\n",
  85                         (int)(local/NSEC_PER_SEC),
  86                         (int)(world/NSEC_PER_SEC));
  87
  88         time_last_local = local;
  89         time_last_world = world;
  90 }
  91
  92 /**
  93  * Synchronize the timer internal state with updates
  94  * from an external time sync message.
  95  *   local: our internal timestamp for the event
  96  *   world: reference timestamp from the other device
  97  */
  98 void time_ext_sync(uint64_t local, uint64_t world)
  99 {
 100         uint64_t guess = time_to_world(local);
 101
 102         time_last_local = local;
 103         time_last_world = (guess/2) + (world/2);
 104         //time_last_world = (guess * 3 / 4) + (world * 1 / 4);
 105         //time_last_world =
 106         //      (guess - (        guess / 2)) +
 107         //      (world - (world - world / 2));
 108         //time_last_world =
 109         //      (guess - (guess - guess / 4)) +
 110         //      (world - (        world / 4));
 111
 112         world = time_last_world;
 113
 114 //#ifdef VERBOSE
 115 #if 0
 116         uint64_t error = world > guess ? world - guess :
 117                          guess > world ? guess - world : 0;
 118         int      ahead = guess > world;
 119         sirq_printf("syncing clocks: %6d=%d.%04u -> %d.%04u (err: %s%ld.%09lu)\r\n",
 120                         (int)((local / NSEC_PER_SEC)),
 121                         (int)((guess / NSEC_PER_SEC)),
 122                         (int)((guess % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)),
 123                         (int)((world / NSEC_PER_SEC)),
 124                         (int)((world % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)),
 125                         ahead ? "-" : " ",
 126                         (int32_t )(error / (int64_t)NSEC_PER_SEC),
 127                         (uint32_t)(error % (int64_t)NSEC_PER_SEC));
 128 #endif
 129 //#endif
 130
 131         // .000000284
 132         // .000000253
 133         // .000000264
 134         // .000000451
 135         // .000000284
 136         // .000000267
 137         // .000000223
 138         // .000000326
 139
 140 }
 141
 142 void time_printf(const char *label, uint64_t local)
 143 {
 144         uint64_t world = time_to_world(local);
 145         sirq_printf("%s -- %d.%09u -> %d.%09u\r\n",
 146                         label,
 147                         (int)(local / NSEC_PER_SEC),
 148                         (int)(local % NSEC_PER_SEC),
 149                         (int)(world / NSEC_PER_SEC),
 150                         (int)(world % NSEC_PER_SEC));
 151 }
 152
 153 /*********************
 154  * Signal generation *
 155  *********************/
 156
 157 static uint32_t *emit_pcr    = 0; // transmit pin name
 158
 159 static uint64_t  emit_start  = 0; // transmit start time
 160 static uint64_t  emit_period = 0; // transmit period
 161 static uint64_t  emit_due    = 0; // next transmit time
 162
 163 static uint32_t  emit_slack  = 0; // how far ahead we need to schedule
 164 static uint32_t  emit_worst  = 0; // worst-case latency in task table
 165
 166 void emit_init(int alt, PinName pin, PinMode mode)
 167 {
 168         // Find pin
 169         emit_pcr = (uint32_t*)(PORTA_BASE + pin);
 170
 171         // Enable clocks
 172         SIM->SCGC6            |= SIM_SCGC6_TPM0_MASK;
 173
 174         SIM->SOPT2            |= SIM_SOPT2_TPMSRC(1);
 175         SIM->SOPT4            |= SIM_SOPT4_TPM1CLKSEL_MASK;
 176
 177         // Set pin mode
 178         emit_pcr[0]            = PORT_PCR_ISF_MASK
 179                                | PORT_PCR_MUX(alt)
 180                                | mode;
 181
 182         // Setup Timer/PWM Module
 183         TPM0->SC               = TPM_SC_TOF_MASK
 184                                | TPM_SC_PS(1);        // 24 MHz clock ?
 185         TPM0->CNT              = TPM_CNT_COUNT(0);
 186         TPM0->MOD              = TPM_MOD_MOD(0xFFFF);
 187
 188         TPM0->CONTROLS[0].CnSC = TPM_CnSC_CHF_MASK    // clear flag
 189                                | TPM_CnSC_MSB_MASK    // pulse output on match
 190                                | TPM_CnSC_MSA_MASK    // ..
 191                                | TPM_CnSC_ELSA_MASK;  // pulse high
 192
 193         TPM0->CONTROLS[0].CnV  = 0xFFFF;              // time delay
 194
 195         TPM0->STATUS           = TPM_STATUS_CH0F_MASK
 196                                | TPM_STATUS_TOF_MASK;
 197
 198         TPM0->CONF             = TPM_CONF_CSOO_MASK;
 199 }
 200
 201 void emit_enable(uint64_t start, uint64_t period)
 202 {
 203         emit_start  = start;
 204         emit_period = period;
 205         emit_due    = start + period;
 206
 207         emit_slack  = 10000; // tune based on emit_worst
 208
 209         time_printf("scheuled emit - ", emit_due);
 210 }
 211
 212 void emit_schedule(uint64_t when)
 213 {
 214         uint64_t now   = tdma_time();
 215         uint16_t delay = (uint16_t)(when-now);
 216
 217         // Clear pending flags
 218         //emit_pcr[0]           |= PORT_PCR_ISF_MASK
 219
 220         // Disable timer
 221         TPM0->SC               = TPM_SC_TOF_MASK;
 222
 223         // Set transmit time
 224         TPM0->CNT              = TPM_CNT_COUNT(0);
 225         TPM0->CONTROLS[0].CnV  = delay;
 226
 227         // Start the timer
 228         TPM0->SC               = TPM_SC_TOF_MASK
 229                                | TPM_SC_CMOD(1);
 230 }
 231
 232 void emit_transmit(void)
 233 {
 234         static uint64_t prev;
 235
 236         // Get a fresh timestamp
 237         uint64_t world = tdma_time();
 238
 239         // Record how how much time we have to reschedule
 240         if (prev && world - prev > emit_worst)
 241                 emit_worst = prev;
 242
 243         // Schedule task if needed
 244         if (world+emit_slack > emit_due)
 245                 emit_schedule(emit_due);
 246 }
 247
 248 /************************
 249  * Serial I/O functions *
 250  ************************/
 251
 252 typedef struct {
 253         int      index;
 254         int      state;
 255         uint8_t  buffer[256];
 256 } parser_t;
 257
 258 static uint32_t serial_device_id   = 0;
 259
 260 const  uint64_t serial_sync_delay  = NSEC_PER_SEC / 100; // 1hz
 261 static uint64_t serial_sync_due    = 0;
 262
 263 static tdma_t  *serial_tdma_rcv    = NULL;
 264 static tdma_t  *serial_tdma_xmt    = NULL;
 265
 266 static uint64_t serial_prev_local  = 0;
 267 static uint64_t serial_prev_seq    = 0;
 268
 269 static uint64_t serial_xmt_local   = 0;
 270 static uint64_t serial_xmt_seq     = 0;
 271
 272 /**
 273  * Convert world to local time
 274  */
 275 uint64_t serial_read_time(ntime_t time)
 276 {
 277         return ((uint64_t)time.seconds) * NSEC_PER_SEC
 278              + ((uint64_t)time.nanosec);
 279 }
 280
 281 ntime_t serial_write_time(uint64_t time)
 282 {
 283         ntime_t buf = {};
 284         buf.seconds = time / NSEC_PER_SEC;
 285         buf.nanosec = time % NSEC_PER_SEC;
 286         return buf;
 287 }
 288
 289 /**
 290  * Output initialization message init message
 291  */
 292 void serial_send_init(uint16_t device, uint64_t local)
 293 {
 294 }
 295
 296 /**
 297  * Output time sync message
 298  */
 299 void serial_send_sync(sirq_t *port, uint64_t now)
 300 {
 301         if (serial_sync_due == 0 || now < serial_sync_due)
 302                 return; // not ready
 303
 304         //sirq_printf("sending sync\r\n");
 305
 306         // Calculate world time
 307         uint64_t local = 0;
 308         uint64_t world = time_to_world(serial_xmt_local);
 309
 310         // Message data
 311         header_t   head;
 312         sync_msg_t body;
 313
 314         // Transmit sync message
 315         head.header = MSG_HEADER;
 316         head.msgid  = MSG_ID_SYNC;
 317         head.length = sizeof(body);
 318         head.cksum  = 0; // todo
 319
 320         body.seq          = serial_xmt_seq;
 321         body.time.seconds = world / NSEC_PER_SEC;
 322         body.time.nanosec = world % NSEC_PER_SEC;
 323
 324         tdma_stop(serial_tdma_rcv);
 325
 326         test_xmt_enab  = 1;
 327         test_xmt_time0 = 0;
 328         test_xmt_time1 = 0;
 329
 330         tdma_start(serial_tdma_xmt);
 331         sirq_write(port, &head, sizeof(head));
 332         sirq_write(port, &body, sizeof(body));
 333         tdma_stop(serial_tdma_xmt);
 334
 335         // save transmit time
 336         //local = test_xmt_time1;
 337         int valid = tdma_stamp(serial_tdma_xmt, &local);
 338         if (!valid) {
 339                 sirq_printf("sync transmit time -- missed\r\n");
 340         } else {
 341                 //time_printf("sync transmit time ", local);
 342                 //time_printf("sync transmit test0", test_xmt_time0);
 343                 //time_printf("sync transmit test1", test_xmt_time1);
 344         }
 345
 346         tdma_start(serial_tdma_rcv);
 347
 348         serial_xmt_seq  += 1;
 349         serial_sync_due  = 0;
 350         serial_xmt_local = local;
 351 }
 352
 353 /**
 354  * Output external event received message
 355  *   event: id of the received event
 356  *   time:  compensated timestamp of the event
 357  */
 358 void serial_send_event(uint16_t event, uint64_t local)
 359 {
 360 //      uint64_t world = time_to_world(local);
 361
 362         // Message data
 363 #if 0
 364         header_t    head;
 365         event_msg_t body;
 366
 367         ntime_t time = {};
 368         time.seconds = (uint32_t)(world / NSEC_PER_SEC);
 369         time.nanosec = (uint32_t)(world % NSEC_PER_SEC);
 370
 371         sirq_printf("event received - %08x:%08x - %u.%09u\r\n",
 372                 (uint32_t)(local >> 32), (uint32_t)local,
 373                 time.seconds, time.nanosec);
 374
 375         // Transmit sync message
 376         head.header = MSG_HEADER;
 377         head.msgid  = MSG_ID_SYNC;
 378         head.length = sizeof(body);
 379         head.cksum  = 0; // todo
 380
 381         body.seq          = serial_xmt_seq;
 382         body.time.seconds = world / NSEC_PER_SEC;
 383         body.time.nanosec = world % NSEC_PER_SEC;
 384
 385         tdma_stop(serial_tdma_rcv);
 386
 387         test_xmt_enab  = 1;
 388         test_xmt_time0 = 0;
 389         test_xmt_time1 = 0;
 390
 391         tdma_start(serial_tdma_xmt);
 392         sirq_write(port, &head, sizeof(head));
 393         sirq_write(port, &body, sizeof(body));
 394         tdma_stop(serial_tdma_xmt);
 395 #endif
 396 }
 397
 398 /**
 399  * Handle init message
 400  */
 401 void serial_handle_init(init_msg_t *msg)
 402 {
 403         if (msg->valid & MSG_VALID_DEVICE)
 404                 serial_device_id = msg->device;
 405
 406         if ((msg->valid & MSG_VALID_START) ||
 407             (msg->valid & MSG_VALID_PERIOD)) {
 408                 uint64_t start  = serial_read_time(msg->start);
 409                 uint64_t period = serial_read_time(msg->period);
 410                 emit_enable(start, period);
 411         }
 412
 413         if (msg->valid & MSG_VALID_WORLD) {
 414                 uint64_t world = serial_read_time(msg->world);
 415                 uint64_t local = tdma_time();
 416                 time_ext_init(local, world);
 417         }
 418 }
 419
 420 /**
 421  * Handle sync message
 422  */
 423 void serial_handle_sync(sync_msg_t *msg)
 424 {
 425         // Read receive timestamp for next time sync message
 426         uint64_t current = 0;
 427         int valid = tdma_stamp(serial_tdma_rcv, &current);
 428         if (!valid)
 429                 sirq_printf("sync receive time  -- missing\r\n");
 430         //else
 431         //      time_printf("sync receive time ", current);
 432         tdma_stop(serial_tdma_rcv);
 433
 434         // Lookup times
 435         uint64_t world = ((uint64_t)msg->time.seconds) * NSEC_PER_SEC
 436                        + ((uint64_t)msg->time.nanosec);
 437
 438         // Valid times timestamp
 439         if (serial_prev_seq == (msg->seq-1)) {
 440                 uint64_t local = serial_prev_local;
 441                 time_ext_sync(local, world);
 442         }
 443
 444         // Queue transmit to other board
 445         serial_sync_due   = tdma_time() + serial_sync_delay;
 446
 447         // Update states
 448         serial_prev_local = current;
 449         serial_prev_seq   = msg->seq;
 450 }
 451
 452 /**
 453  * Handle event message
 454  */
 455 void serial_handle_event(event_msg_t *msg)
 456 {
 457 }
 458
 459 /**
 460  * Deliver message
 461  */
 462 void serial_deliver(int msgid, void *body)
 463 {
 464         switch (msgid) {
 465                 case MSG_ID_INIT:
 466                         //sirq_printf("received init msg\r\n");
 467                         serial_handle_init((init_msg_t*)body);
 468                         break;
 469                 case MSG_ID_SYNC:
 470                         //sirq_printf("received sync msg\r\n");
 471                         serial_handle_sync((sync_msg_t*)body);
 472                         break;
 473                 case MSG_ID_EVENT:
 474                         //sirq_printf("received event msg\r\n");
 475                         serial_handle_event((event_msg_t*)body);
 476                         break;
 477         }
 478 }
 479
 480 /**
 481  * Process serial receive messages
 482  */
 483 void serial_receive(parser_t *parser, int byte)
 484 {
 485         //sirq_printf("serial_receive - %02x\r\n", byte);
 486
 487         // Lookup pointers
 488         header_t *head = (header_t*)parser->buffer;
 489         void     *body = (void*)(head+1);
 490         const int max_length = sizeof(parser->buffer)-sizeof(header_t);
 491
 492         // Process uart messages
 493         parser->buffer[parser->index++] = byte;
 494         switch (parser->state) {
 495                 case 0: // Search
 496                         if (parser->index == sizeof(uint16_t)) {
 497                                 if (head->header == MSG_HEADER) {
 498                                         parser->state = 1;
 499                                 } else {
 500                                         parser->buffer[0] = parser->buffer[1];
 501                                         parser->index = 1;
 502                                 }
 503                         }
 504                         break;
 505                 case 1: // Header
 506                         if (parser->index == sizeof(header_t)) {
 507                                 if (head->length <= max_length &&
 508                                     head->msgid  <= MSG_MAX_ID) {
 509                                         parser->state = 2;
 510                                 } else {
 511                                         parser->index = 0;
 512                                         parser->state = 0;
 513                                 }
 514                         }
 515                         break;
 516                 case 2: // Data
 517                         if (parser->index == (int)sizeof(header_t)+head->length) {
 518                                 serial_deliver(head->msgid, body);
 519                                 parser->index = 0;
 520                                 parser->state = 0;
 521                         }
 522                         break;
 523         }
 524 }
 525
 526 /********************
 527  * Data definitions *
 528  ********************/
 529
 530 // LEDs
 531 DigitalOut led1(LED1);
 532 DigitalOut led2(LED2);
 533
 534 // Message Parsers
 535 parser_t   parser_dbg;
 536 parser_t   parser_bbb;
 537 parser_t   parser_mbed;
 538
 539 // Serial IRQ
 540 sirq_t    *sirq_dbg;
 541 sirq_t    *sirq_bbb;
 542 sirq_t    *sirq_mbed;
 543
 544 // Timer DMA
 545 tdma_t    *tdma_evt;
 546 tdma_t    *tdma_rcv;
 547 tdma_t    *tdma_xmt;
 548
 549 /*********
 550  * Tasks *
 551  *********/
 552
 553 void task_serial(uint64_t local, uint64_t world)
 554 {
 555         while (sirq_ready(sirq_dbg)) {
 556                 //sirq_printf("serial recv - dbg\r\n");
 557                 serial_receive(&parser_dbg,  sirq_getc(sirq_dbg));
 558         }
 559
 560         while (sirq_ready(sirq_bbb)) {
 561                 //sirq_printf("serial recv - bbb\r\n");
 562                 serial_receive(&parser_bbb,  sirq_getc(sirq_bbb));
 563         }
 564
 565         while (sirq_ready(sirq_mbed)) {
 566                 //sirq_printf("serial recv - mbed\r\n");
 567                 serial_receive(&parser_mbed, sirq_getc(sirq_mbed));
 568         }
 569 }
 570
 571 void task_events(uint64_t local, uint64_t world)
 572 {
 573         uint64_t event = 0;
 574
 575 #ifdef VERBOSE
 576         if (tdma_stamp(tdma_evt, &event)) {
 577                 sirq_printf("event received - evt\r\n");
 578         if (tdma_stamp(tdma_rcv, &event))
 579                 sirq_printf("event received - rcv\r\n");
 580         if (tdma_stamp(tdma_xmt, &event))
 581                 sirq_printf("event received - xmt\r\n");
 582 #endif
 583
 584         if (tdma_stamp(tdma_evt, &event))
 585                 serial_send_event(0, event);
 586         tdma_stop(tdma_evt);
 587         tdma_start(tdma_evt);
 588 }
 589
 590 void task_sync(uint64_t local, uint64_t world)
 591 {
 592         serial_send_sync(sirq_mbed, local);
 593 }
 594
 595 void task_leds(uint64_t local, uint64_t world)
 596 {
 597         static uint32_t which = 0;
 598         led1 = (which == 0);
 599         led2 = (which == 1);
 600         which ^= 1;
 601 }
 602
 603 void task_emit(uint64_t local, uint64_t world)
 604 {
 605         emit_transmit();
 606 }
 607
 608 void task_debug(uint64_t local, uint64_t world)
 609 {
 610         //tdma_debug(tdma_rcv);
 611         //tdma_debug(tdma_xmt);
 612
 613         //sirq_debug(sirq_mbed);
 614
 615 #ifdef VERBOSE
 616         sirq_printf("background - %6u.%02u -> %u.%02u\r\n",
 617                         (uint32_t)(local / NSEC_PER_SEC),
 618                         (uint32_t)(local % NSEC_PER_SEC / 10000000),
 619                         (uint32_t)(world / NSEC_PER_SEC),
 620                         (uint32_t)(world % NSEC_PER_SEC / 10000000));
 621 #endif
 622 }
 623
 624 /********
 625  * Main *
 626  ********/
 627
 628 #define N_ELEM(x) (sizeof(x) / sizeof((x)[0]))
 629
 630 extern void test_main(void);
 631 extern serial_t stdio_uart;
 632
 633 static struct {
 634         void (*task)(uint64_t, uint64_t);
 635         uint64_t period;
 636         uint64_t due;
 637 } tasks[] = {
 638         { task_serial, 0          }, // always
 639         { task_events, 1000000000 }, // always -- testing
 640         { task_sync,   0          }, // always
 641         { task_emit,   0          }, // always
 642         { task_leds,   100000000  }, // 10hz
 643         { task_debug,  1000000000 }, // 1hz
 644 };
 645
 646 void background(void)
 647 {
 648         // Debugging
 649         uint64_t local = tdma_time();
 650         uint64_t world = time_to_world(local);
 651
 652         // Run the scheduler
 653         for (unsigned i = 0; i < N_ELEM(tasks); i++) {
 654                 if (local >= tasks[i].due) {
 655                         tasks[i].task(local, world);
 656                         tasks[i].due += tasks[i].period;
 657                 }
 658         }
 659 }
 660
 661 int main(int argc, char **argv)
 662 {
 663         tdma_init();
 664
 665         // Open serial ports
 666         sirq_dbg   = sirq_open(SIRQ_UART0, USBTX, USBRX, 115200); // to pc
 667         sirq_bbb   = sirq_open(SIRQ_UART1, PTE0,  PTE1,  115200); // to bbb
 668         sirq_mbed  = sirq_open(SIRQ_UART2, PTD3,  PTD2,  115200); // to mbed
 669
 670         // Setup timers
 671         tdma_evt   = tdma_open(TDMA_CHAN0, 3, PTC9,  PullUp); // async event
 672
 673         // mbed time sync
 674         tdma_rcv   = tdma_open(TDMA_CHAN2, 3, PTD2,  PullUp);   // time sync rcv
 675         tdma_xmt   = tdma_open(TDMA_CHAN3, 3, PTD3,  PullUp);   // time sync xmt
 676
 677         // host time sync
 678         //tdma_rcv   = tdma_open(TDMA_CHAN2, 2, USBRX, PullUp); // time sync rcv
 679         //tdma_xmt   = tdma_open(TDMA_CHAN3, 2, USBTX, PullUp); // time sync xmt
 680
 681         // start timers
 682         tdma_start(tdma_evt);
 683         tdma_start(tdma_rcv);
 684         tdma_start(tdma_xmt);
 685
 686         // Serial timestamping
 687         serial_tdma_rcv = tdma_rcv;
 688         serial_tdma_xmt = tdma_xmt;
 689
 690         // Test clocks
 691         //MCG->C1    = 0x05; // was 0x1A
 692         //MCG->C2    = 0x2C; // was 0x24
 693         //MCG->C3    = 0x91; // was 0x91
 694         //MCG->C4    = 0x10; // was 0x10
 695         //MCG->C5    = 0x01; // was 0x01
 696         //MCG->C6    = 0x40; // was 0x40
 697         //MCG->S     = 0x6E; // was 0x6E
 698         //MCG->SC    = 0x02; // was 0x02
 699         //MCG->ATCVH = 0x00; // was 0x00
 700         //MCG->ATCVL = 0x00; // was 0x00
 701         //MCG->C7    = 0x00; // was 0x00
 702         //MCG->C8    = 0x80; // was 0x80
 703         //MCG->C9    = 0x00; // was 0x00
 704         //MCG->C10   = 0x00; // was 0x00
 705
 706         //sirq_printf("MGC - C1    %02hx\r\n", MCG->C1);     // 1A
 707         //sirq_printf("MGC - C2    %02hx\r\n", MCG->C2);     // 24
 708         //sirq_printf("MGC - C3    %02hx\r\n", MCG->C3);     // 91
 709         //sirq_printf("MGC - C4    %02hx\r\n", MCG->C4);     // 10
 710         //sirq_printf("MGC - C5    %02hx\r\n", MCG->C5);     // 01
 711         //sirq_printf("MGC - C6    %02hx\r\n", MCG->C6);     // 40
 712         //sirq_printf("MGC - S     %02hx\r\n", MCG->S);      // 6E
 713         //sirq_printf("MGC - SC    %02hx\r\n", MCG->SC);     // 02
 714         //sirq_printf("MGC - ATCVH %02hx\r\n", MCG->ATCVH);  // 00
 715         //sirq_printf("MGC - ATCVL %02hx\r\n", MCG->ATCVL);  // 00
 716         //sirq_printf("MGC - C7    %02hx\r\n", MCG->C7);     // 00
 717         //sirq_printf("MGC - C8    %02hx\r\n", MCG->C8);     // 80
 718         //sirq_printf("MGC - C9    %02hx\r\n", MCG->C9);     // 00
 719         //sirq_printf("MGC - C10   %02hx\r\n", MCG->C10);    // 00
 720
 721         // Run background loop
 722         printf("hello");
 723         while (true)
 724                 background();
 725
 726         // Run tests
 727         //test_main();
 728
 729         return 0;
 730 }