sparc32 slaveio_timer user timer fixes

diff -p -u -r1.17 slavio_timer.c
@@ -54,16 +54,24 @@ typedef struct SLAVIO_TIMERState {
     ptimer_state *timer;
     uint32_t count, counthigh, reached;
     uint64_t limit;
-    int stopped;
-    int mode; // 0 = processor, 1 = user, 2 = system
+    // processor only
+    int running;
+    struct SLAVIO_TIMERState *master;
+    int slave_index;
+    // system only
     struct SLAVIO_TIMERState *slave[MAX_CPUS];
     uint32_t slave_mode;
 } SLAVIO_TIMERState;
 
 #define TIMER_MAXADDR 0x1f
-#define TIMER_SIZE (TIMER_MAXADDR + 1)
+#define SYS_TIMER_SIZE 0x14
 #define CPU_TIMER_SIZE 0x10
 
+static int slavio_timer_is_user(SLAVIO_TIMERState *s)
+{
+    return s->master && (s->master->slave_mode & (1 << s->slave_index));
+}
+
 // Update count, set irq, update expire_time
 // Convert from ptimer countdown units
 static void slavio_timer_get_out(SLAVIO_TIMERState *s)

@@ -84,9 +92,10 @@ static void slavio_timer_irq(void *opaqu
 
     slavio_timer_get_out(s);
     DPRINTF("callback: count %x%08x\n", s->counthigh, s->count);
-    s->reached = 0x80000000;
-    if (s->mode != 1)
+    if (!slavio_timer_is_user(s)) {
+        s->reached = 0x80000000;
         qemu_irq_raise(s->irq);
+    }
 }
 
 static uint32_t slavio_timer_mem_readl(void *opaque, target_phys_addr_t addr)

@@ -99,35 +108,39 @@ static uint32_t slavio_timer_mem_readl(v
     case 0:
         // read limit (system counter mode) or read most signifying
         // part of counter (user mode)
-        if (s->mode != 1) {
+        if (slavio_timer_is_user(s)) {
+            // read user timer MSW
+            slavio_timer_get_out(s);
+            ret = s->counthigh;
+        } else {
+            // read limit
             // clear irq
             qemu_irq_lower(s->irq);
             s->reached = 0;
             ret = s->limit & 0x7fffffff;
         }
-        else {
-            slavio_timer_get_out(s);
-            ret = s->counthigh & 0x7fffffff;
-        }
         break;
     case 1:
         // read counter and reached bit (system mode) or read lsbits
         // of counter (user mode)
         slavio_timer_get_out(s);
-        if (s->mode != 1)
-            ret = (s->count & 0x7fffffff) | s->reached;
-        else
-            ret = s->count;
+        if (slavio_timer_is_user(s)) // read user timer LSW
+            ret = s->count & 0xffffffe00;
+        else // read limit
+            ret = (s->count & 0x7ffffe00) | s->reached;
         break;
     case 3:
+        // only available in processor counter/timer
         // read start/stop status
-        ret = s->stopped;
+        ret = s->running;
         break;
     case 4:
+        // only available in system counter
         // read user/system mode
         ret = s->slave_mode;
         break;
     default:
+        DPRINTF("invalid read address " TARGET_FMT_plx "\n", addr);
         ret = 0;
         break;
     }

@@ -146,20 +159,31 @@ static void slavio_timer_mem_writel(void
     saddr = (addr & TIMER_MAXADDR) >> 2;
     switch (saddr) {
     case 0:
-        if (s->mode == 1) {
-            // set user counter limit MSW, reset counter
+        if (slavio_timer_is_user(s)) {
+            // set user counter MSW, reset counter
             qemu_irq_lower(s->irq);
-            s->limit &= 0xfffffe00ULL;
-            s->limit |= (uint64_t)val << 32;
+            s->limit = 0x7ffffffffffffe00ULL;
+            DPRINTF("processor %d user timer reset\n", s->slave_index);
+            ptimer_set_limit(s->timer, s->limit >> 9, 1);
+        } else {
+            // set limit, reset counter
+            qemu_irq_lower(s->irq);
+            s->limit = val & 0x7ffffe00ULL;
             if (!s->limit)
-                s->limit = 0x7ffffffffffffe00ULL;
+                s->limit = 0x7ffffe00ULL;
             ptimer_set_limit(s->timer, s->limit >> 9, 1);
-            break;
         }
-        // set limit, reset counter
-        reload = 1;
-        qemu_irq_lower(s->irq);
-        // fall through
+        break;
+    case 1:
+        if (slavio_timer_is_user(s)) {
+            // set user counter LSW, reset counter
+            qemu_irq_lower(s->irq);
+            s->limit = 0x7ffffffffffffe00ULL;
+            DPRINTF("processor %d user timer reset\n", s->slave_index);
+            ptimer_set_limit(s->timer, s->limit >> 9, 1);
+        } else
+            DPRINTF("not user timer\n");
+        break;
     case 2:
         // set limit without resetting counter
         s->limit = val & 0x7ffffe00ULL;

@@ -167,52 +191,42 @@ static void slavio_timer_mem_writel(void
             s->limit = 0x7ffffe00ULL;
         ptimer_set_limit(s->timer, s->limit >> 9, reload);
         break;
-    case 1:
-        // set user counter limit LSW, reset counter
-        if (s->mode == 1) {
-            qemu_irq_lower(s->irq);
-            s->limit &= 0x7fffffff00000000ULL;
-            s->limit |= val & 0xfffffe00ULL;
-            if (!s->limit)
-                s->limit = 0x7ffffffffffffe00ULL;
-            ptimer_set_limit(s->timer, s->limit >> 9, 1);
-        }
-        break;
     case 3:
-        // start/stop user counter
-        if (s->mode == 1) {
-            if (val & 1) {
-                ptimer_stop(s->timer);
-                s->stopped = 1;
-            }
-            else {
+        if (slavio_timer_is_user(s)) {
+            // start/stop user counter
+            if ((val & 1) && !s->running) {
+                DPRINTF("processor %d user timer started\n", s->slave_index);
                 ptimer_run(s->timer, 0);
-                s->stopped = 0;
+                s->running = 1;
+            } else if (!(val & 1) && s->running) {
+                DPRINTF("processor %d user timer stopped\n", s->slave_index);
+                ptimer_stop(s->timer);
+                s->running = 0;
             }
         }
         break;
     case 4:
-        // bit 0: user (1) or system (0) counter mode
-        {
+        if (s->master == NULL) {
             unsigned int i;
-
+                                                                                
             for (i = 0; i < MAX_CPUS; i++) {
                 if (val & (1 << i)) {
                     qemu_irq_lower(s->slave[i]->irq);
                     s->slave[i]->limit = -1ULL;
-                    s->slave[i]->mode = 1;
-                } else {
-                    s->slave[i]->mode = 0;
                 }
-                ptimer_stop(s->slave[i]->timer);
-                ptimer_set_limit(s->slave[i]->timer, s->slave[i]->limit >> 9,
-                                 1);
-                ptimer_run(s->slave[i]->timer, 0);
+                if ((val & (1 << i)) != (s->slave_mode & (1 << i))) {
+                    ptimer_stop(s->slave[i]->timer);
+                    ptimer_set_limit(s->slave[i]->timer, s->slave[i]->limit >> 9, 1);
+                    DPRINTF("processor %d timer changed\n", s->slave[i]->slave_index);
+                    ptimer_run(s->slave[i]->timer, 0);
+                }
             }
             s->slave_mode = val & ((1 << MAX_CPUS) - 1);
-        }
+        } else
+            DPRINTF("not system timer\n");
         break;
     default:
+        DPRINTF("invalid write address " TARGET_FMT_plx "\n", addr);
         break;
     }
 }

@@ -238,8 +252,8 @@ static void slavio_timer_save(QEMUFile *
     qemu_put_be32s(f, &s->counthigh);
     qemu_put_be32(f, 0); // Was irq
     qemu_put_be32s(f, &s->reached);
-    qemu_put_be32s(f, &s->stopped);
-    qemu_put_be32s(f, &s->mode);
+    qemu_put_be32s(f, &s->running);
+    qemu_put_be32s(f, 0); // Was mode
     qemu_put_ptimer(f, s->timer);
 }
 

@@ -256,8 +270,8 @@ static int slavio_timer_load(QEMUFile *f
     qemu_get_be32s(f, &s->counthigh);
     qemu_get_be32s(f, &tmp); // Was irq
     qemu_get_be32s(f, &s->reached);
-    qemu_get_be32s(f, &s->stopped);
-    qemu_get_be32s(f, &s->mode);
+    qemu_get_be32s(f, &s->running);
+    qemu_get_be32s(f, &tmp); // Was mode
     qemu_get_ptimer(f, s->timer);
 
     return 0;

@@ -267,18 +281,22 @@ static void slavio_timer_reset(void *opa
 {
     SLAVIO_TIMERState *s = opaque;
 
-    s->limit = 0x7ffffe00ULL;
+    if (slavio_timer_is_user(s))
+        s->limit = 0x7ffffffffffffe00ULL;
+    else
+        s->limit = 0x7ffffe00ULL;
     s->count = 0;
     s->reached = 0;
-    s->mode &= 2;
     ptimer_set_limit(s->timer, s->limit >> 9, 1);
     ptimer_run(s->timer, 0);
-    s->stopped = 1;
+    s->running = 1;
     qemu_irq_lower(s->irq);
 }
 
 static SLAVIO_TIMERState *slavio_timer_init(target_phys_addr_t addr,
-                                            qemu_irq irq, int mode)
+                                            qemu_irq irq,
+                                            SLAVIO_TIMERState *master,
+                                            int slave_index)
 {
     int slavio_timer_io_memory;
     SLAVIO_TIMERState *s;

@@ -288,18 +306,18 @@ static SLAVIO_TIMERState *slavio_timer_i
     if (!s)
         return s;
     s->irq = irq;
-    s->mode = mode;
+    s->master = master;
+    s->slave_index = slave_index;
     bh = qemu_bh_new(slavio_timer_irq, s);
     s->timer = ptimer_init(bh);
     ptimer_set_period(s->timer, 500ULL);
 
     slavio_timer_io_memory = cpu_register_io_memory(0, slavio_timer_mem_read,
                                                     slavio_timer_mem_write, s);
-    if (mode < 2)
+    if (master)
         cpu_register_physical_memory(addr, CPU_TIMER_SIZE, slavio_timer_io_memory);
     else
-        cpu_register_physical_memory(addr, TIMER_SIZE,
-                                     slavio_timer_io_memory);
+        cpu_register_physical_memory(addr, SYS_TIMER_SIZE, slavio_timer_io_memory);
     register_savevm("slavio_timer", addr, 2, slavio_timer_save, slavio_timer_load, s);
     qemu_register_reset(slavio_timer_reset, s);
     slavio_timer_reset(s);

@@ -313,11 +331,11 @@ void slavio_timer_init_all(target_phys_a
     SLAVIO_TIMERState *master;
     unsigned int i;
 
-    master = slavio_timer_init(base + 0x10000ULL, master_irq, 2);
+    master = slavio_timer_init(base + 0x10000ULL, master_irq, NULL, 0);
 
     for (i = 0; i < MAX_CPUS; i++) {
         master->slave[i] = slavio_timer_init(base + (target_phys_addr_t)
                                              (i * TARGET_PAGE_SIZE),
-                                             cpu_irqs[i], 0);
+                                             cpu_irqs[i], master, i);
     }
 }