diff -r 4c7a54a43420 extras/mini-os/kernel.c
--- a/extras/mini-os/kernel.c	Tue Feb 17 12:30:29 2009 +0000
+++ b/extras/mini-os/kernel.c	Fri Feb 20 14:55:13 2009 +0000
@@ -75,15 +75,144 @@
     /* test_xenbus(); */
 }
 
+#if 0
+#define rdtscll(val) do { \
+     unsigned int a,d; \
+     asm volatile("rdtsc" : "=a" (a), "=d" (d)); \
+     (val) = ((unsigned long long)a) | (((unsigned long long)d)<<32); \
+} while(0)
+#endif
+
+
+void calibrate_cpms(long long *cpms)
+{
+    struct timeval tv;
+    long long ca, cb, ta, tb, t1, t2;
+
+retry:
+    /* Try to detect if we're scheduled out. */
+    do {
+        gettimeofday(&tv, NULL); t1 = tv.tv_sec * 1000000 + tv.tv_usec;
+        rdtscll(ca);
+        gettimeofday(&tv, NULL); t2 = tv.tv_sec * 1000000 + tv.tv_usec;
+    } while (t2 - t1 > 10);
+    ta = (t1+t2)/2;
+
+    msleep(100);
+
+    do {
+        gettimeofday(&tv, NULL); t1 = tv.tv_sec * 1000000 + tv.tv_usec;
+        rdtscll(cb);
+        gettimeofday(&tv, NULL); t2 = tv.tv_sec * 1000000 + tv.tv_usec;
+    } while (t2 - t1 > 10);
+    tb = (t1+t2)/2;
+
+    if ( tb-ta <= 0 || cb-ca <= 0)
+    {
+        printk("%s: ca %lld cb %lld ta %lld tb %lld, retry in 1s\n",
+               __func__, ca, cb, ta, tb);
+        msleep(1000);
+        goto retry;
+    }
+
+    *cpms = ((cb - ca) * 1000 )/ (tb - ta);
+
+    printk("cpms: %lld\n", *cpms);
+}
+
+#define CAL_COUNT 100000
+#define CHECK_MS 10
+void calibrate_ipms(long long cpms, long long *ipms)
+{
+    long long ca, cb, s[2], d;
+    int i, j, k;
+
+    k = CAL_COUNT;
+
+    do {
+        for ( j=0 ; j<2; j++)
+        {
+            rdtscll(ca);
+            for ( i=0 ; i < k; i++ )
+                ;
+            rdtscll(cb);
+            s[j] = CAL_COUNT*cpms/(cb-ca);
+        }
+        d = s[0] - s[1];
+        if ( d < 0 )
+            d = -d;
+    } while( d > 100 );
+
+    *ipms = (s[0]+s[1])/2;
+
+    printk("ipms: %lld\n", *ipms);
+
+    k = CHECK_MS * *ipms;
+
+    rdtscll(ca);
+    for ( i=0; i<k; i++);
+    rdtscll(cb);
+
+    printk(" check: %lld %lld\n", CHECK_MS * cpms, cb-ca);
+}
+
+#define QUANTUM_MS    1ULL  /* How long to run each "quantum" */
+#define PERIOD_MS     5ULL  /* How often "quanta" show up */
+#define UPDATE_MS  5000ULL  /* How often to report how well we're doing */
+
 static void periodic_thread(void *p)
 {
-    struct timeval tv;
+    long long now, next_quantum, next_update;
+    long long cpms, ipms;
+    unsigned received = 0, processed = 0;
+    int i, quantum_count;
+
     printk("Periodic thread started.\n");
+
+    /* Wait for timing code to come up */
+    msleep(50);
+
+    calibrate_cpms(&cpms);
+    calibrate_ipms(cpms, &ipms);
+
+    quantum_count = QUANTUM_MS * ipms;
+
+    rdtscll(now);
+
+    next_quantum = now;
+    next_update = now + UPDATE_MS * cpms;
+    
     for(;;)
     {
-        gettimeofday(&tv, NULL);
-        printk("T(s=%ld us=%ld)\n", tv.tv_sec, tv.tv_usec);
-        msleep(1000);
+        rdtscll(now);
+
+        do
+        {
+            received++;
+            next_quantum+=PERIOD_MS * cpms;
+        } while  ( next_quantum < now );
+
+        /* Now do one "quantum" of work */
+        for ( i=0; i<quantum_count; i++)
+            ;
+        processed++;
+
+        /* See if it's time to post an update */
+        rdtscll(now);
+        if ( now > next_update )
+        {
+            printk("%lu\n", processed*100/received);
+            processed = received = 0;
+            while ( now > next_update )
+                next_update += UPDATE_MS * cpms;
+        }
+            
+        /* If we haven't passed the next quantum, sleep. */
+        if ( now < next_quantum )
+        {
+            int msec = ((next_quantum - now)+(cpms-1)) / cpms;
+            msleep(msec);
+        }
     }
 }