Improved timing code

emulator/devices/peripherals/bcm.c

@@ -18,27 +18,6 @@
 
 #include "bcm.h"
 
-#ifdef _MSC_VER
-#include <Windows.h>
-#else
-#include <time.h>
-#endif
-
-uint64_t getnano() {
-#ifdef _MSC_VER
-    static LARGE_INTEGER frequency;
-    if (frequency.QuadPart == 0) QueryPerformanceFrequency(&frequency);
-    LARGE_INTEGER now;
-    QueryPerformanceCounter(&now);
-    double x = (double)now.QuadPart / (double)frequency.QuadPart;
-    return (uint64_t)(x * 1000000000.0);
-#else
-    struct timespec now;
-    clock_gettime(CLOCK_MONOTONIC, &now);
-    return now.tv_sec * 1000000000 + now.tv_nsec;
-#endif
-}
-
 
 void handle_bcm (Emulator *emu) 
 {
@@ -190,14 +169,50 @@ void setup_bcm (Emulator *emu)
 //    return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) - ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec);
 //}
 
+double mclk_clock_nstime(Emulator *emu) {
+    Cpu *cpu = emu->cpu;
+    Bcm *bcm = cpu->bcm;
+    double nsec;
+
+    if (bcm->mclk_source == DCOCLK) {
+        nsec = (1.0/(bcm->dco_freq/bcm->mclk_div))*1000000000.0;
+    } else {
+        nsec = (1.0/1030000) * 1000000000.0;
+    }
+    return nsec;
+}
+
 void mclk_wait_cycles (Emulator *emu, uint64_t cycles)
 {
-    Cpu *cpu = emu->cpu;
-    Bcm *bcm = cpu->bcm;  
+//    Cpu *cpu = emu->cpu;
+//    Bcm *bcm = cpu->bcm;
+//
+//    uint64_t start = getnano();
+//
+////    uint64_t start, end;
+////    uint64_t i, elapsed_nsecs;
+//    double thing = 1000;
+//
+//    if (bcm->mclk_source == DCOCLK) {
+//        thing = (1.0/(bcm->dco_freq/bcm->mclk_div))*1000000000.0;
+//    }
+//    thing *= cycles;
+//
+//    if (last_nano > 0) {
+//        thing -= (double)(start - last_nano);
+//    }
+//    if (thing > 0) {
+////        struct timespec tv;
+////        tv.tv_sec = 0;
+////        tv.tv_nsec = (long)thing;
+////        nanosleep(&tv, &tv);
+//        usleep((long)(thing / 1000.0));
+//    }
+//
+//    last_nano = start;
+//    return;
+/*
 
-    uint64_t start, end;
-    uint64_t i, elapsed_nsecs;
-  
     for (i = 0;i < cycles;i++)
     {
         start = getnano();
@@ -223,42 +238,43 @@ void mclk_wait_cycles (Emulator *emu, uint64_t cycles)
             }
         }
     }
+*/
 }
 
 void smclk_wait_cycles (Emulator *emu, uint64_t cycles)
 {
-  Cpu *cpu = emu->cpu;
-  Bcm *bcm = cpu->bcm;  
-  
-  uint64_t start, end;
-  uint64_t i, elapsed_nsecs;
-  
-  for (i = 0;i < cycles;i++) {
-    start = getnano();
-    //    clock_gettime(CLOCK_MONOTONIC, &start);
-
-    while (true) {
-      end = getnano();
-//      clock_gettime(CLOCK_MONOTONIC, &end);
-      elapsed_nsecs = end - start;//nanosec_diff(&end, &start);
-
-      // Choose timing based on clock source
-      if (bcm->mclk_source == DCOCLK) {
-	//printf("div: %llu\n", 
-	//(long long unsigned)(1/(bcm->dco_freq/bcm->mclk_div)));
-
-	double thing = (1.0/(bcm->dco_freq/bcm->mclk_div))*1000000000.0;
-
-	if (elapsed_nsecs >= (uint64_t)thing) {
-	  break;
-	}    
-      }
-      else {
-	puts("Error, clock source");
-      }
-
-    }
-  }
+//  Cpu *cpu = emu->cpu;
+//  Bcm *bcm = cpu->bcm;
+//
+//  uint64_t start, end;
+//  uint64_t i, elapsed_nsecs;
+//
+//  for (i = 0;i < cycles;i++) {
+//    start = getnano();
+//    //    clock_gettime(CLOCK_MONOTONIC, &start);
+//
+//    while (true) {
+//      end = getnano();
+////      clock_gettime(CLOCK_MONOTONIC, &end);
+//      elapsed_nsecs = end - start;//nanosec_diff(&end, &start);
+//
+//      // Choose timing based on clock source
+//      if (bcm->mclk_source == DCOCLK) {
+//	//printf("div: %llu\n",
+//	//(long long unsigned)(1/(bcm->dco_freq/bcm->mclk_div)));
+//
+//	double thing = (1.0/(bcm->dco_freq/bcm->mclk_div))*1000000000.0;
+//
+//	if (elapsed_nsecs >= (uint64_t)thing) {
+//	  break;
+//	}
+//      }
+//      else {
+//	puts("Error, clock source");
+//      }
+//
+//    }
+//  }
   
 }
 /*

emulator/devices/peripherals/bcm.h

@@ -59,10 +59,12 @@ struct Bcm {
   uint64_t mclk_freq;
 };
 
-uint64_t nanosec_diff(struct timespec *timeA_p, struct timespec *timeB_p);
+//uint64_t nanosec_diff(struct timespec *timeA_p, struct timespec *timeB_p);
 void mclk_wait_cycles (Emulator *emu, uint64_t cycles);
 void smclk_wait_cycles (Emulator *emu, uint64_t cycles);
 void aclk_wait_cycles (Emulator *emu, uint64_t cycles);
+
+double mclk_clock_nstime(Emulator *emu);
 //void *DCO_source (void *data);
 void setup_bcm (Emulator *emu);
 void handle_bcm (Emulator *emu);

emulator/devices/peripherals/usci.c

@@ -16,9 +16,6 @@
   along with this program. If not, see <https://www.gnu.org/licenses/>.
 */
 
-#define TXIFG 0x02
-#define RXIFG 0x01
-
 #include "usci.h"
 
 /*
@@ -101,8 +98,7 @@ void open_pty (Emulator *emu)
 }
 */
 
-void handle_usci (Emulator *emu) 
-{
+void handle_usci (Emulator *emu) {
   Cpu *cpu = emu->cpu;
   Debugger *deb = emu->debugger;
   Usci *usci = cpu->usci;
@@ -127,22 +123,20 @@ void handle_usci (Emulator *emu)
       *usci->IFG2 &= ~TXIFG;
 
       if (c & 0xFF) {
-	if (deb->web_interface) {
-        print_serial(emu, (char*)&str[0]);
-        //write(sp, usci->UCA0TXBUF, 1);
-	}
-	else if (deb->console_interface) {
-        //write(sp, usci->UCA0TXBUF, 1);
-	}
-
-	*usci->UCA0TXBUF = 0;
+        if (deb->web_interface) {
+            print_serial(emu, (char*)&str[0]);
+            //write(sp, usci->UCA0TXBUF, 1);
+        }
+        //else if (deb->console_interface) {
+            //write(sp, usci->UCA0TXBUF, 1);
+        //}
+	    *usci->UCA0TXBUF = 0;
       }
 
       //*usci->IFG2 &= TXIFG;
       *usci->IFG2 |= TXIFG;
     }
   }
-
   return;
 }
 

emulator/devices/peripherals/usci.h

@@ -26,6 +26,9 @@
 #include "../cpu/registers.h"
 #include "../utilities.h"
 
+#define TXIFG 0x02
+#define RXIFG 0x01
+
 // USCI_Ax Control Register 0
 typedef struct Ctl0 
 {

emulator/python/py_functions.c

@@ -1,4 +1,26 @@
 #include "py_functions.h"
+
+#ifdef _MSC_VER
+#include <Windows.h>
+#else
+#include <time.h>
+#endif
+
+uint64_t getnano() {
+#ifdef _MSC_VER
+    static LARGE_INTEGER frequency;
+    if (frequency.QuadPart == 0) QueryPerformanceFrequency(&frequency);
+    LARGE_INTEGER now;
+    QueryPerformanceCounter(&now);
+    double x = (double)now.QuadPart / (double)frequency.QuadPart;
+    return (uint64_t)(x * 1000000000.0);
+#else
+    struct timespec now;
+    clock_gettime(CLOCK_MONOTONIC, &now);
+    return now.tv_sec * 1000000000 + now.tv_nsec;
+#endif
+}
+
 // This is an ugly solution but heh
 Emulator *emuInst = NULL;
 
@@ -124,6 +146,63 @@ void stop_emu() {
     print_console(emuInst, "Stopping emulator..\n");
 }
 
+
+void write_serial(uint8_t *data, int len) {
+    if(emuInst == NULL) return;
+    Usci *usci = emuInst->cpu->usci;
+//    int i = 0;
+//    uint8_t *bytes = data;
+
+    printf("len is %d\n", len);
+    for(int i=0; i < len; i++) {
+        usleep(333);
+        printf("waiting.. ");
+        while (*usci->IFG2 & RXIFG) {
+            usleep(333);
+            if(emuInst->debugger->quit) {
+                puts("debugger stopped");
+                return;
+            }
+        }
+//        uint8_t thing = *(bytes);
+        *usci->UCA0RXBUF = data[i];
+        *usci->IFG2 |= RXIFG;
+        printf("0x%04X in UCA0RXBUF\n", (uint8_t)*usci->UCA0RXBUF);
+        printf("waiting.. ");
+        while (*usci->IFG2 & RXIFG) {
+            usleep(333);
+            if(emuInst->debugger->quit) {
+                puts("debugger stopped");
+                return;
+            }
+        }
+        puts("done\n");
+    }
+
+
+//    while (true) {
+//        usleep(333);
+//        while (*usci->IFG2 & RXIFG);
+//        uint8_t thing = *(bytes);
+//
+////        if (thing == '\n') {
+////          thing = '\r';
+////        }
+//        *usci->UCA0RXBUF = thing;
+//        *usci->IFG2 |= RXIFG;
+//
+//        //printf("\n0x%04X in UCA0RXBUF\n", (uint8_t)*usci->UCA0RXBUF);
+//        //puts("waiting..");
+//        while (*usci->IFG2 & RXIFG);
+//        //puts("done");
+//        //*usci->IFG2 |= RXIFG;
+//        if (*usci->UCA0RXBUF == '\r' || *usci->UCA0RXBUF == '\n') break;
+//        ++bytes;
+//      }
+//      return NULL;
+//    }
+}
+
 void start_emu(char *file) {
     emuInst = (Emulator *) calloc( 1, sizeof(Emulator) );
     Cpu *cpu = NULL; Debugger *deb = NULL;
@@ -158,9 +237,16 @@ void start_emu(char *file) {
         disassemble(emuInst, cpu->pc, 1);
         //    update_register_display(emuInst);
 
+        uint16_t counter = 0;
+        uint64_t time_delta = 0;
+        uint64_t time_last = getnano();
+
         while (!deb->quit) {
             // Handle debugger when CPU is not running
             if (!cpu->running) {
+                counter = 0;
+                time_delta = 0;
+                time_last = getnano();
                 usleep(10000);
                 continue;
             }
@@ -177,8 +263,27 @@ void start_emu(char *file) {
             handle_port_1(emuInst);
             handle_usci(emuInst);
 
-            // Average of 4 cycles per instruction
-            mclk_wait_cycles(emuInst, 4);
+            counter++;
+            if(counter > 500) {
+                uint64_t time_now = getnano();
+
+                // Average of 4 cycles per instruction (actually around 4.88)
+                uint64_t cycles_time = (uint64_t)(mclk_clock_nstime(emuInst) * 4.883 * 500);
+                uint64_t delta = time_now - time_last;
+                if(time_last > time_now) delta = 0;
+                uint64_t sleep_time = cycles_time - delta;
+                if(delta > cycles_time) {
+                    time_delta += (delta - cycles_time);
+                } else if(time_delta > sleep_time) {
+                    time_delta -= sleep_time;
+                } else {
+                    sleep_time += time_delta;
+                    time_delta = 0;
+                    usleep(sleep_time/1000);
+                }
+                time_last = time_now;
+                counter = 0;
+            }
         }
     }
 

emulator/python/py_functions.h

@@ -31,5 +31,6 @@ PyObject *get_port1_regs();
 PyObject *get_bcm_regs();
 PyObject *get_timer_regs();
 PyObject *get_usci_regs();
+void write_serial(uint8_t *data, int len);
 
 #endif

emulator/python/py_interface.c

@@ -126,6 +126,18 @@ static PyObject *method_on_control(PyObject *self, PyObject *args) {
     return Py_None;
 }
 
+static PyObject *method_write_serial(PyObject *self, PyObject *args) {
+    char *cmd;
+    int len;
+    if(!PyArg_ParseTuple(args, "s#", &cmd, &len)) {
+        return NULL;
+    }
+    Py_BEGIN_ALLOW_THREADS
+    write_serial(cmd, len);
+    Py_END_ALLOW_THREADS
+    return Py_None;
+}
+
 
 static PyMethodDef RunMethods[] = {
     {"init", method_start, METH_VARARGS, "Initialise msp430 emulator"},
@@ -139,6 +151,7 @@ static PyMethodDef RunMethods[] = {
     {"on_control", method_on_control, METH_VARARGS, "Set emulator callback for control"},
     {"get_regs", method_get_regs, METH_VARARGS, "Get emulator registers"},
     {"set_regs", method_set_regs, METH_VARARGS, "Set emulator registers"},
+    {"write_serial", method_write_serial, METH_VARARGS, "Write to UART serial"},
     {NULL, NULL, 0, NULL}
 };