[RFC,2/7] cadence ttc: first revision

diff --git a/Makefile.target b/Makefile.target
index 824f6eb..44ba41b 100644
--- a/Makefile.target
+++ b/Makefile.target
@@ -335,6 +335,7 @@  obj-arm-y = integratorcp.o versatilepb.o arm_pic.o arm_timer.o
 obj-arm-y += arm_boot.o pl011.o pl031.o pl050.o pl080.o pl110.o pl181.o pl190.o
 obj-arm-y += versatile_pci.o
 obj-arm-y += cadence_uart.o
+obj-arm-y += cadence_ttc.o
 obj-arm-y += realview_gic.o realview.o arm_sysctl.o arm11mpcore.o a9mpcore.o
 obj-arm-y += arm_l2x0.o
 obj-arm-y += arm_mptimer.o
diff --git a/hw/cadence_ttc.c b/hw/cadence_ttc.c
new file mode 100644
index 0000000..2fe1696
--- /dev/null
+++ b/hw/cadence_ttc.c
@@ -0,0 +1,545 @@ 
+/*
+ * Xilinx Zynq cadence TTC model
+ *
+ * Copyright (c) 2011 Xilinx Inc.
+ * Written By Haibing Ma
+ *            M. Habib
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the Free
+ * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA
+ * 02139, USA.
+ */
+
+#include "sysbus.h"
+#include "qemu-timer.h"
+#include "ptimer.h"
+
+#ifdef CADENCE_TTC_ERR_DEBUG
+#define qemu_debug(...) \
+    fprintf(stderr,  ": %s: ", __func__); \
+    fprintf(stderr, ## __VA_ARGS__); \
+    fflush(stderr);
+#else
+    #define qemu_debug(...)
+#endif
+
+#define COUNTER_INTR_IV     0x00000001
+#define COUNTER_INTR_M1     0x00000002
+#define COUNTER_INTR_M2     0x00000004
+#define COUNTER_INTR_M3     0x00000008
+#define COUNTER_INTR_OV     0x00000010
+#define COUNTER_INTR_EV     0x00000020
+
+#define COUNTER_CTRL_DIS    0x00000001
+#define COUNTER_CTRL_INT    0x00000002
+#define COUNTER_CTRL_DEC    0x00000004
+#define COUNTER_CTRL_MATCH  0x00000008
+#define COUNTER_CTRL_RST    0x00000010
+
+#define CLOCK_CTRL_PS_EN    0x00000001
+#define CLOCK_CTRL_PS_V     0x0000001e
+
+typedef struct {
+    ptimer_state *timer;
+    uint32_t reg_clock;
+    uint32_t reg_count;
+    uint16_t reg_interval;
+    uint16_t reg_match[3];
+    uint32_t reg_intr;
+    uint32_t reg_intr_en;
+    uint32_t reg_event_ctrl;
+    uint32_t reg_event;
+    uint32_t control;
+    uint32_t limit;
+    uint32_t next_event[5];
+    uint16_t event_seq;
+    uint16_t event_total;
+
+    int freq;
+    qemu_irq irq;
+
+} cadence_timer_state;
+
+typedef struct {
+    SysBusDevice busdev;
+    MemoryRegion iomem;
+    cadence_timer_state *timer[3];
+} cadence_ttc_state;
+
+static void cadence_timer_update(cadence_timer_state *s)
+{
+    uint32_t flags;
+
+    flags = s->reg_intr & s->reg_intr_en;
+
+    qemu_set_irq(s->irq, flags != 0);
+}
+
+static cadence_timer_state *cadence_timer_from_addr (void *opaque,
+                                        target_phys_addr_t offset)
+{
+    unsigned int index;
+    cadence_ttc_state *s = (cadence_ttc_state *)opaque;
+
+    index = (offset >> 2) % 3;
+
+    return (s->timer[index]);
+}
+
+static void cadence_timer_recalibrate(cadence_timer_state *s)
+{
+    uint32_t limit;
+
+    if (s->reg_count & COUNTER_CTRL_INT) {
+        /* interval */
+        limit = s->reg_interval;
+    } else {
+        /* Free running.  */
+        limit = 0xffff;
+    }
+    if (limit == 0)
+        limit = 1;
+
+    ptimer_set_limit(s->timer, limit, 1);
+}
+
+static void cadence_timer_fix_events(cadence_timer_state *s)
+{
+    uint16_t tmp = 0;
+
+    if (s->event_total <2)
+        return;
+
+    if (s->event_total <3)
+    {
+        if (s->next_event[0] < s->next_event[1])
+            return;
+        else {
+            tmp = s->next_event[0];
+            s->next_event[0] = s->next_event[1];
+            s->next_event[1] = tmp;
+            return;
+        }
+    }
+
+    if (s->next_event[0] > s->next_event[1]) {
+        tmp = s->next_event[0];
+        s->next_event[0] = s->next_event[1];
+        s->next_event[1] = tmp;
+    }
+
+    if (s->next_event[2] > s->next_event[3]) {
+        tmp = s->next_event[2];
+        s->next_event[2] = s->next_event[3];
+        s->next_event[3] = tmp;
+    }
+
+    if (s->next_event[1] < s->next_event[2]){
+        return;
+    }
+
+    // 0 < 1  2 < 3  but (1 > 2)
+    if (s->next_event[0] < s->next_event[2]) { // 0 < 2
+        if (s->next_event[1] < s->next_event[3]) { // 1 < 3
+            tmp = s->next_event[1];
+            s->next_event[1] = s->next_event[2];
+            s->next_event[2] = tmp;
+        }
+        else // 1 > 3
+        {
+            tmp = s->next_event[1];
+            s->next_event[1] = s->next_event[2];
+            s->next_event[2] = s->next_event[3];
+            s->next_event[3] = tmp;
+        }
+        return;
+    }
+    else { //  0 < 1  2 < 3  but 0 > 2
+        if (s->next_event[1] < s->next_event[3]) { // 1 < 3
+            tmp = s->next_event[0];
+            s->next_event[0] = s->next_event[2];
+            s->next_event[2] = s->next_event[1];
+            s->next_event[1] = tmp;
+        }
+        else
+        {   // 1 > 3
+            if (s->next_event[0] > s->next_event[3]) { // 0 > 3
+                tmp = s->next_event[0];
+                s->next_event[0] = s->next_event[2];
+                s->next_event[2] = tmp;
+                tmp = s->next_event[1];
+                s->next_event[1] = s->next_event[3];
+                s->next_event[3] = tmp;
+            }
+            else
+            {   // 0 < 3
+                tmp = s->next_event[0];
+                s->next_event[0] = s->next_event[2];
+                s->next_event[2] = s->next_event[3];
+                s->next_event[3] = s->next_event[1];
+                s->next_event[1] = tmp;
+            }
+        }
+    }
+}
+
+static void cadence_timer_setup_events(cadence_timer_state *s)
+{
+    uint16_t tmp = 0;
+
+    s->event_total = 4;
+
+    if (s->reg_count & COUNTER_CTRL_INT) {
+        s->next_event[tmp++] = s->reg_interval;
+    }
+    else
+    {
+        s->next_event[tmp++] = 0xffff;
+    }
+
+    if (s->next_event[0] != s->reg_match[0])
+    {
+        s->next_event[tmp++] = s->reg_match[0];
+    }
+    else {
+        s->event_total--;
+    }
+
+    if ((s->reg_match[0] == s->reg_match[1]) ||
+                        (s->next_event[0] == s->reg_match[1]))
+    {
+        s->event_total--;
+    }
+    else {
+        s->next_event[tmp++] = s->reg_match[1];
+    }
+
+    if ((s->reg_match[0] == s->reg_match[2]) ||
+            (s->reg_match[1] == s->reg_match[2]) ||
+            (s->next_event[0] != s->reg_match[2]))
+    {
+        s->event_total--;
+    }
+    else {
+        s->next_event[tmp++] = s->reg_match[1];
+    }
+
+    cadence_timer_fix_events(s);
+}
+
+static uint32_t cadence_counter_value(cadence_timer_state *s)
+{
+    uint32_t r;
+
+    r = ptimer_get_count(s->timer);
+
+    if (s->reg_count & COUNTER_CTRL_DEC) {
+        return r;
+    } else {
+        if (s->reg_count & COUNTER_CTRL_INT)
+            return s->reg_interval - r;
+        else
+            return 0xffff - r;
+    }
+}
+
+static void cadence_counter_clock(cadence_timer_state *s , uint32_t value)
+{
+    int freq;
+
+    s->reg_clock = value & 0x3f;
+    if (s->reg_clock & CLOCK_CTRL_PS_EN) {
+        freq = s->freq;
+        freq >>= ((value & CLOCK_CTRL_PS_V) >> 1) + 1;
+        ptimer_set_freq(s->timer, freq);
+    }
+}
+
+static void cadence_counter_control(cadence_timer_state *s , uint32_t value)
+{
+    s->reg_count = value & 0x3f;
+    if (value & COUNTER_CTRL_RST) {
+        ptimer_stop(s->timer);
+        cadence_timer_recalibrate(s);
+        s->reg_count &= ~COUNTER_CTRL_RST;
+    }
+    if (value & COUNTER_CTRL_DIS) {
+        ptimer_stop(s->timer);
+    } else {
+        cadence_timer_recalibrate(s);
+        ptimer_run(s->timer, 0);
+    }
+    if (value & COUNTER_CTRL_MATCH) {
+        cadence_timer_setup_events(s);
+    }
+}
+
+static void cadence_timer_next_event(cadence_timer_state *s)
+{
+    uint32_t limit = 0;
+
+    if (s->event_seq < s->event_total) {
+        limit = s->next_event[s->event_seq++];
+    } else {
+        s->event_seq = 0;
+    }
+
+    ptimer_set_limit(s->timer, limit, 1);
+}
+
+/*********************************************************
+ * Read Timer registers
+ *
+ *********************************************************/
+
+static uint32_t cadence_ttc_read_imp(void *opaque, target_phys_addr_t offset)
+{
+    cadence_timer_state *s = cadence_timer_from_addr(opaque, offset);
+    uint32_t value;
+
+    switch (offset) {
+    case 0x00: /* clock control */
+    case 0x04:
+    case 0x08:
+        return s->reg_clock;
+
+    case 0x0c: /* counter control */
+    case 0x10:
+    case 0x14:
+        return s->reg_count;
+
+    case 0x18: /* counter value */
+    case 0x1c:
+    case 0x20:
+        return cadence_counter_value(s);
+
+    case 0x24: /* reg_interval counter */
+    case 0x28:
+    case 0x2c:
+        return s->reg_interval;
+
+    case 0x30: /* match 1 counter */
+    case 0x34:
+    case 0x38:
+        return s->reg_match[0];
+
+    case 0x3c: /* match 2 counter */
+    case 0x40:
+    case 0x44:
+        return s->reg_match[1];
+
+    case 0x48: /* match 3 counter */
+    case 0x4c:
+    case 0x50:
+        return s->reg_match[2];
+
+    case 0x54: /* interrupt register */
+    case 0x58:
+    case 0x5c:
+        /* cleared after read */
+        value = s->reg_intr;
+        s->reg_intr = 0;
+        return value;
+
+    case 0x60: /* interrupt enable */
+    case 0x64:
+    case 0x68:
+        return s->reg_intr_en;
+
+    case 0x6c:
+    case 0x70:
+    case 0x74:
+        return s->reg_event_ctrl;
+
+    case 0x78:
+    case 0x7c:
+    case 0x80:
+        return s->reg_event;
+
+    default:
+        return 0;
+    }
+}
+
+static uint64_t cadence_ttc_read(void *opaque, target_phys_addr_t offset,
+    unsigned size)
+{
+    uint32_t ret = cadence_ttc_read_imp(opaque, offset);
+    qemu_debug("addr: %08x data: %08x\n", offset, ret);
+    return ret;
+}
+
+/*********************************************************
+ * Write Timer registers
+ *
+ *********************************************************/
+
+static void cadence_ttc_write(void *opaque, target_phys_addr_t offset,
+        uint64_t value, unsigned size)
+{
+    cadence_timer_state *s = cadence_timer_from_addr(opaque, offset);
+
+    qemu_debug("addr: %08x data %08x\n", offset, (unsigned)value);
+
+    switch (offset) {
+    case 0x00: /* clock control */
+    case 0x04:
+    case 0x08:
+        cadence_counter_clock (s, value);
+        break;
+
+    case 0x0c: /* conter control */
+    case 0x10:
+    case 0x14:
+        cadence_counter_control (s, value);
+        break;
+
+    case 0x24: /* interval register */
+    case 0x28:
+    case 0x2c:
+        s->reg_interval = value & 0xffff;
+        break;
+
+    case 0x30: /* match register */
+    case 0x34:
+    case 0x38:
+        s->reg_match[0] = value & 0xffff;
+
+    case 0x3c: /* match register */
+    case 0x40:
+    case 0x44:
+        s->reg_match[1] = value & 0xffff;
+
+    case 0x48: /* match register */
+    case 0x4c:
+    case 0x50:
+        s->reg_match[2] = value & 0xffff;
+        break;
+
+    case 0x54: /* interrupt register */
+    case 0x58:
+    case 0x5c:
+        s->reg_intr &= (~value & 0xfff);
+        break;
+
+    case 0x60: /* interrupt enable */
+    case 0x64:
+    case 0x68:
+        s->reg_intr_en = value & 0x3f;
+        break;
+
+    case 0x6c: /* event control */
+    case 0x70:
+    case 0x74:
+        s->reg_event_ctrl = value & 0x07;
+        break;
+
+    default:
+        return;
+    }
+
+    cadence_timer_update(s);
+}
+
+static const MemoryRegionOps cadence_ttc_ops = {
+    .read = cadence_ttc_read,
+    .write = cadence_ttc_write,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+};
+
+/*********************************************************
+ * update time counter
+ *
+ *********************************************************/
+static void cadence_timer_tick(void *opaque)
+{
+    cadence_timer_state *s = (cadence_timer_state *)opaque;
+
+    if (s->reg_count & COUNTER_CTRL_MATCH) {
+        cadence_timer_next_event(s);
+    }
+
+    else
+    {
+        if (s->reg_count & COUNTER_CTRL_INT)
+        {
+            s->reg_intr |= COUNTER_INTR_IV;
+        }
+        else
+        {
+            s->reg_intr |= COUNTER_INTR_OV;
+        }
+    }
+
+    cadence_timer_update(s);
+}
+
+/*********************************************************
+ * Initialiaze each cadence TTC counter with its features
+ *
+ *********************************************************/
+
+static cadence_timer_state *cadence_timer_init(uint32_t freq)
+{
+    cadence_timer_state *s;
+    QEMUBH *bh;
+
+    s = (cadence_timer_state *)g_malloc0(sizeof(cadence_timer_state));
+    s->freq = freq;
+    s->reg_count = 0x21;
+
+    bh = qemu_bh_new(cadence_timer_tick, s);
+    s->timer = ptimer_init(bh);
+    ptimer_set_freq(s->timer, freq);
+
+    return s;
+}
+
+/*************************************************
+ * Initialiaze cadence TTC device on reset state
+ *
+ *************************************************/
+
+static int cadence_ttc_init(SysBusDevice *dev)
+{
+    cadence_ttc_state *s = FROM_SYSBUS(cadence_ttc_state, dev);
+
+    s->timer[0] = cadence_timer_init(2500000);
+    s->timer[1] = cadence_timer_init(2500000);
+    s->timer[2] = cadence_timer_init(2500000);
+
+    sysbus_init_irq(dev, &s->timer[0]->irq);
+    sysbus_init_irq(dev, &s->timer[1]->irq);
+    sysbus_init_irq(dev, &s->timer[2]->irq);
+
+    memory_region_init_io(&s->iomem, &cadence_ttc_ops, s, "timer", 0x1000);
+    sysbus_init_mmio(dev, &s->iomem);
+
+    return 0;
+}
+
+/********************************************
+ * Register cadence TTC device
+ *
+ *******************************************/
+static SysBusDeviceInfo ttc_info = {
+    .init = cadence_ttc_init,
+    .qdev.name  = "cadence_ttc",
+    .qdev.size  = sizeof(cadence_ttc_state),
+    .qdev.props = (Property[]) {
+        DEFINE_PROP_END_OF_LIST(),
+    }
+};
+
+static void cadence_ttc_register_devices(void)
+{
+    sysbus_register_withprop(&ttc_info);
+}
+
+device_init(cadence_ttc_register_devices)