ARMv7 support.

git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@3572 c046a42c-6fe2-441c-8c8c-71466251a162

Changelog

@@ -17,6 +17,7 @@
   - MIPS mipssim pequdo machine (Thiemo Seufer)
   - Strace for Linux userland emulation (Stuart Anderson, Thayne Harbaugh)
   - OMAP310 MPU emulation plus Palm T|E machine (Andrzej Zaborowski)
+  - ARM v6, v7, NEON SIMD and SMP emulation (Paul Brook/CodeSourcery)
 
 version 0.9.0:
 

Makefile.target

@@ -493,7 +493,9 @@ ifeq ($(TARGET_BASE_ARCH), arm)
 VL_OBJS+= integratorcp.o versatilepb.o ps2.o smc91c111.o arm_pic.o arm_timer.o
 VL_OBJS+= arm_boot.o pl011.o pl031.o pl050.o pl080.o pl110.o pl181.o pl190.o
 VL_OBJS+= versatile_pci.o sd.o ptimer.o
-VL_OBJS+= arm_gic.o realview.o arm_sysctl.o
+VL_OBJS+= realview_gic.o realview.o arm_sysctl.o mpcore.o
+VL_OBJS+= armv7m.o armv7m_nvic.o stellaris.o i2c.o ssd0303.o pl022.o
+VL_OBJS+= ssd0323.o pl061.o
 VL_OBJS+= arm-semi.o
 VL_OBJS+= pxa2xx.o pxa2xx_pic.o pxa2xx_gpio.o pxa2xx_timer.o pxa2xx_dma.o
 VL_OBJS+= pxa2xx_lcd.o pxa2xx_mmci.o pxa2xx_pcmcia.o max111x.o max7310.o

cpu-exec.c

@@ -173,6 +173,7 @@ static inline TranslationBlock *tb_find_fast(void)
         flags |= (1 << 6);
     if (env->vfp.xregs[ARM_VFP_FPEXC] & (1 << 30))
         flags |= (1 << 7);
+    flags |= (env->condexec_bits << 8);
     cs_base = 0;
     pc = env->regs[15];
 #elif defined(TARGET_SPARC)
@@ -511,8 +512,18 @@ int cpu_exec(CPUState *env1)
                         env->exception_index = EXCP_FIQ;
                         do_interrupt(env);
                     }
+                    /* ARMv7-M interrupt return works by loading a magic value
+                       into the PC.  On real hardware the load causes the
+                       return to occur.  The qemu implementation performs the
+                       jump normally, then does the exception return when the
+                       CPU tries to execute code at the magic address.
+                       This will cause the magic PC value to be pushed to
+                       the stack if an interrupt occured at the wrong time.
+                       We avoid this by disabling interrupts when
+                       pc contains a magic address.  */
                     if (interrupt_request & CPU_INTERRUPT_HARD
-                        && !(env->uncached_cpsr & CPSR_I)) {
+                        && ((IS_M(env) && env->regs[15] < 0xfffffff0)
+                            || !(env->uncached_cpsr & CPSR_I))) {
                         env->exception_index = EXCP_IRQ;
                         do_interrupt(env);
                     }

fpu/softfloat-native.h

@@ -224,6 +224,11 @@ INLINE float32 float32_chs(float32 a)
     return -a;
 }
 
+INLINE float32 float32_scalbn(float32 a, int n)
+{
+    return scalbnf(a, n);
+}
+
 /*----------------------------------------------------------------------------
 | Software IEC/IEEE double-precision conversion routines.
 *----------------------------------------------------------------------------*/
@@ -311,6 +316,11 @@ INLINE float64 float64_chs(float64 a)
     return -a;
 }
 
+INLINE float64 float64_scalbn(float64 a, int n)
+{
+    return scalbn(a, n);
+}
+
 #ifdef FLOATX80
 
 /*----------------------------------------------------------------------------
@@ -391,4 +401,10 @@ INLINE floatx80 floatx80_chs(floatx80 a)
 {
     return -a;
 }
+
+INLINE floatx80 floatx80_scalbn(floatx80 a, int n)
+{
+    return scalbnl(a, n);
+}
+
 #endif

fpu/softfloat.c

@@ -5377,3 +5377,78 @@ int float ## s ## _compare_quiet( float ## s a, float ## s b STATUS_PARAM )  \
 
 COMPARE(32, 0xff)
 COMPARE(64, 0x7ff)
+
+/* Multiply A by 2 raised to the power N.  */
+float32 float32_scalbn( float32 a, int n STATUS_PARAM )
+{
+    flag aSign;
+    int16 aExp;
+    bits32 aSig;
+
+    aSig = extractFloat32Frac( a );
+    aExp = extractFloat32Exp( a );
+    aSign = extractFloat32Sign( a );
+
+    if ( aExp == 0xFF ) {
+        return a;
+    }
+    aExp += n;
+    return roundAndPackFloat32( aSign, aExp, aSig STATUS_VAR );
+}
+
+float64 float64_scalbn( float64 a, int n STATUS_PARAM )
+{
+    flag aSign;
+    int16 aExp;
+    bits64 aSig;
+
+    aSig = extractFloat64Frac( a );
+    aExp = extractFloat64Exp( a );
+    aSign = extractFloat64Sign( a );
+
+    if ( aExp == 0x7FF ) {
+        return a;
+    }
+    aExp += n;
+    return roundAndPackFloat64( aSign, aExp, aSig STATUS_VAR );
+}
+
+#ifdef FLOATX80
+floatx80 floatx80_scalbn( floatx80 a, int n STATUS_PARAM )
+{
+    flag aSign;
+    int16 aExp;
+    bits64 aSig;
+
+    aSig = extractFloatx80Frac( a );
+    aExp = extractFloatx80Exp( a );
+    aSign = extractFloatx80Sign( a );
+
+    if ( aExp == 0x7FF ) {
+        return a;
+    }
+    aExp += n;
+    return roundAndPackFloatx80( STATUS(floatx80_rounding_precision),
+                                 aSign, aExp, aSig, 0 STATUS_VAR );
+}
+#endif
+
+#ifdef FLOAT128
+float128 float128_scalbn( float128 a, int n STATUS_PARAM )
+{
+    flag aSign;
+    int32 aExp;
+    bits64 aSig0, aSig1;
+
+    aSig1 = extractFloat128Frac1( a );
+    aSig0 = extractFloat128Frac0( a );
+    aExp = extractFloat128Exp( a );
+    aSign = extractFloat128Sign( a );
+    if ( aExp == 0x7FFF ) {
+        return a;
+    }
+    aExp += n;
+    return roundAndPackFloat128( aSign, aExp, aSig0, aSig1, 0 STATUS_VAR );
+
+}
+#endif

fpu/softfloat.h

@@ -244,6 +244,7 @@ int float32_compare( float32, float32 STATUS_PARAM );
 int float32_compare_quiet( float32, float32 STATUS_PARAM );
 int float32_is_nan( float32 );
 int float32_is_signaling_nan( float32 );
+float32 float32_scalbn( float32, int STATUS_PARAM );
 
 INLINE float32 float32_abs(float32 a)
 {
@@ -295,6 +296,7 @@ int float64_compare( float64, float64 STATUS_PARAM );
 int float64_compare_quiet( float64, float64 STATUS_PARAM );
 int float64_is_nan( float64 a );
 int float64_is_signaling_nan( float64 );
+float64 float64_scalbn( float64, int STATUS_PARAM );
 
 INLINE float64 float64_abs(float64 a)
 {
@@ -339,6 +341,7 @@ int floatx80_le_quiet( floatx80, floatx80 STATUS_PARAM );
 int floatx80_lt_quiet( floatx80, floatx80 STATUS_PARAM );
 int floatx80_is_nan( floatx80 );
 int floatx80_is_signaling_nan( floatx80 );
+floatx80 floatx80_scalbn( floatx80, int STATUS_PARAM );
 
 INLINE floatx80 floatx80_abs(floatx80 a)
 {
@@ -387,6 +390,7 @@ int float128_le_quiet( float128, float128 STATUS_PARAM );
 int float128_lt_quiet( float128, float128 STATUS_PARAM );
 int float128_is_nan( float128 );
 int float128_is_signaling_nan( float128 );
+float128 float128_scalbn( float128, int STATUS_PARAM );
 
 INLINE float128 float128_abs(float128 a)
 {

hw/arm_boot.c

@@ -1,7 +1,7 @@
 /*
  * ARM kernel loader.
  *
- * Copyright (c) 2006 CodeSourcery.
+ * Copyright (c) 2006-2007 CodeSourcery.
  * Written by Paul Brook
  *
  * This code is licenced under the GPL.
@@ -24,6 +24,22 @@ static uint32_t bootloader[] = {
   0  /* Kernel entry point.  Set by integratorcp_init.  */
 };
 
+/* Entry point for secondary CPUs.  Enable interrupt controller and
+   Issue WFI until start address is written to system controller.  */
+static uint32_t smpboot[] = {
+  0xe3a00201, /* mov     r0, #0x10000000 */
+  0xe3800601, /* orr     r0, r0, #0x001000000 */
+  0xe3a01001, /* mov     r1, #1 */
+  0xe5801100, /* str     r1, [r0, #0x100] */
+  0xe3a00201, /* mov     r0, #0x10000000 */
+  0xe3800030, /* orr     r0, #0x30 */
+  0xe320f003, /* wfi */
+  0xe5901000, /* ldr     r1, [r0] */
+  0xe3110003, /* tst     r1, #3 */
+  0x1afffffb, /* bne     <wfi> */
+  0xe12fff11  /* bx      r1 */
+};
+
 static void main_cpu_reset(void *opaque)
 {
     CPUState *env = opaque;
@@ -33,6 +49,8 @@ static void main_cpu_reset(void *opaque)
         arm_load_kernel(env, env->ram_size, env->kernel_filename,
                         env->kernel_cmdline, env->initrd_filename,
                         env->board_id, env->loader_start);
+
+    /* TODO:  Reset secondary CPUs.  */
 }
 
 static void set_kernel_args(uint32_t ram_size, int initrd_size,
@@ -211,6 +229,8 @@ void arm_load_kernel(CPUState *env, int ram_size, const char *kernel_filename,
         bootloader[6] = entry;
         for (n = 0; n < sizeof(bootloader) / 4; n++)
             stl_raw(phys_ram_base + (n * 4), bootloader[n]);
+        for (n = 0; n < sizeof(smpboot) / 4; n++)
+            stl_raw(phys_ram_base + ram_size + (n * 4), smpboot[n]);
         if (old_param)
             set_kernel_args_old(ram_size, initrd_size,
                                 kernel_cmdline, loader_start);

hw/arm_sysctl.c

@@ -1,7 +1,7 @@
 /*
  * Status and system control registers for ARM RealView/Versatile boards.
  *
- * Copyright (c) 2006 CodeSourcery.
+ * Copyright (c) 2006-2007 CodeSourcery.
  * Written by Paul Brook
  *
  * This code is licenced under the GPL.
@@ -200,6 +200,9 @@ void arm_sysctl_init(uint32_t base, uint32_t sys_id)
         return;
     s->base = base;
     s->sys_id = sys_id;
+    /* The MPcore bootloader uses these flags to start secondary CPUs.
+       We don't use a bootloader, so do this here.  */
+    s->flags = 3;
     iomemtype = cpu_register_io_memory(0, arm_sysctl_readfn,
                                        arm_sysctl_writefn, s);
     cpu_register_physical_memory(base, 0x00001000, iomemtype);

hw/armv7m.c

@@ -0,0 +1,204 @@
+/*
+ * ARMV7M System emulation.
+ *
+ * Copyright (c) 2006-2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+#include "vl.h"
+
+/* Bitbanded IO.  Each word corresponds to a single bit.  */
+
+/* Get the byte address of the real memory for a bitband acess.  */
+static inline uint32_t bitband_addr(uint32_t addr)
+{
+    uint32_t res;
+
+    res = addr & 0xe0000000;
+    res |= (addr & 0x1ffffff) >> 5;
+    return res;
+
+}
+
+static uint32_t bitband_readb(void *opaque, target_phys_addr_t offset)
+{
+    uint8_t v;
+    cpu_physical_memory_read(bitband_addr(offset), &v, 1);
+    return (v & (1 << ((offset >> 2) & 7))) != 0;
+}
+
+static void bitband_writeb(void *opaque, target_phys_addr_t offset,
+                           uint32_t value)
+{
+    uint32_t addr;
+    uint8_t mask;
+    uint8_t v;
+    addr = bitband_addr(offset);
+    mask = (1 << ((offset >> 2) & 7));
+    cpu_physical_memory_read(addr, &v, 1);
+    if (value & 1)
+        v |= mask;
+    else
+        v &= ~mask;
+    cpu_physical_memory_write(addr, &v, 1);
+}
+
+static uint32_t bitband_readw(void *opaque, target_phys_addr_t offset)
+{
+    uint32_t addr;
+    uint16_t mask;
+    uint16_t v;
+    addr = bitband_addr(offset) & ~1;
+    mask = (1 << ((offset >> 2) & 15));
+    mask = tswap16(mask);
+    cpu_physical_memory_read(addr, (uint8_t *)&v, 2);
+    return (v & mask) != 0;
+}
+
+static void bitband_writew(void *opaque, target_phys_addr_t offset,
+                           uint32_t value)
+{
+    uint32_t addr;
+    uint16_t mask;
+    uint16_t v;
+    addr = bitband_addr(offset) & ~1;
+    mask = (1 << ((offset >> 2) & 15));
+    mask = tswap16(mask);
+    cpu_physical_memory_read(addr, (uint8_t *)&v, 2);
+    if (value & 1)
+        v |= mask;
+    else
+        v &= ~mask;
+    cpu_physical_memory_write(addr, (uint8_t *)&v, 2);
+}
+
+static uint32_t bitband_readl(void *opaque, target_phys_addr_t offset)
+{
+    uint32_t addr;
+    uint32_t mask;
+    uint32_t v;
+    addr = bitband_addr(offset) & ~3;
+    mask = (1 << ((offset >> 2) & 31));
+    mask = tswap32(mask);
+    cpu_physical_memory_read(addr, (uint8_t *)&v, 4);
+    return (v & mask) != 0;
+}
+
+static void bitband_writel(void *opaque, target_phys_addr_t offset,
+                           uint32_t value)
+{
+    uint32_t addr;
+    uint32_t mask;
+    uint32_t v;
+    addr = bitband_addr(offset) & ~3;
+    mask = (1 << ((offset >> 2) & 31));
+    mask = tswap32(mask);
+    cpu_physical_memory_read(addr, (uint8_t *)&v, 4);
+    if (value & 1)
+        v |= mask;
+    else
+        v &= ~mask;
+    cpu_physical_memory_write(addr, (uint8_t *)&v, 4);
+}
+
+static CPUReadMemoryFunc *bitband_readfn[] = {
+   bitband_readb,
+   bitband_readw,
+   bitband_readl
+};
+
+static CPUWriteMemoryFunc *bitband_writefn[] = {
+   bitband_writeb,
+   bitband_writew,
+   bitband_writel
+};
+
+static void armv7m_bitband_init(void)
+{
+    int iomemtype;
+
+    iomemtype = cpu_register_io_memory(0, bitband_readfn, bitband_writefn,
+                                       NULL);
+    cpu_register_physical_memory(0x22000000, 0x02000000, iomemtype);
+    cpu_register_physical_memory(0x42000000, 0x02000000, iomemtype);
+}
+
+/* Board init.  */
+/* Init CPU and memory for a v7-M based board.
+   flash_size and sram_size are in kb.
+   Returns the NVIC array.  */
+
+qemu_irq *armv7m_init(int flash_size, int sram_size,
+                      const char *kernel_filename, const char *cpu_model)
+{
+    CPUState *env;
+    qemu_irq *pic;
+    uint32_t pc;
+    int image_size;
+    uint64_t entry;
+    uint64_t lowaddr;
+
+    flash_size *= 1024;
+    sram_size *= 1024;
+
+    if (!cpu_model)
+	cpu_model = "cortex-m3";
+    env = cpu_init(cpu_model);
+    if (!env) {
+        fprintf(stderr, "Unable to find CPU definition\n");
+        exit(1);
+    }
+
+#if 0
+    /* > 32Mb SRAM gets complicated because it overlaps the bitband area.
+       We don't have proper commandline options, so allocate half of memory
+       as SRAM, up to a maximum of 32Mb, and the rest as code.  */
+    if (ram_size > (512 + 32) * 1024 * 1024)
+        ram_size = (512 + 32) * 1024 * 1024;
+    sram_size = (ram_size / 2) & TARGET_PAGE_MASK;
+    if (sram_size > 32 * 1024 * 1024)
+        sram_size = 32 * 1024 * 1024;
+    code_size = ram_size - sram_size;
+#endif
+
+    /* Flash programming is done via the SCU, so pretend it is ROM.  */
+    cpu_register_physical_memory(0, flash_size, IO_MEM_ROM);
+    cpu_register_physical_memory(0x20000000, sram_size,
+                                 flash_size + IO_MEM_RAM);
+    armv7m_bitband_init();
+
+    pic = armv7m_nvic_init(env);
+
+    image_size = load_elf(kernel_filename, 0, &entry, &lowaddr, NULL);
+    if (image_size < 0) {
+        image_size = load_image(kernel_filename, phys_ram_base);
+	lowaddr = 0;
+    }
+    if (image_size < 0) {
+        fprintf(stderr, "qemu: could not load kernel '%s'\n",
+                kernel_filename);
+        exit(1);
+    }
+
+    /* If the image was loaded at address zero then assume it is a
+       regular ROM image and perform the normal CPU reset sequence.
+       Otherwise jump directly to the entry point.  */
+    if (lowaddr == 0) {
+	env->regs[13] = tswap32(*(uint32_t *)phys_ram_base);
+	pc = tswap32(*(uint32_t *)(phys_ram_base + 4));
+    } else {
+	pc = entry;
+    }
+    env->thumb = pc & 1;
+    env->regs[15] = pc & ~1;
+
+    /* Hack to map an additional page of ram at the top of the address
+       space.  This stops qemu complaining about executing code outside RAM
+       when returning from an exception.  */
+    cpu_register_physical_memory(0xfffff000, 0x1000, IO_MEM_RAM + ram_size);
+
+    return pic;
+}
+

hw/armv7m_nvic.c

@@ -0,0 +1,381 @@
+/*
+ * ARM Nested Vectored Interrupt Controller
+ *
+ * Copyright (c) 2006-2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ *
+ * The ARMv7M System controller is fairly tightly tied in with the
+ * NVIC.  Much of that is also implemented here.
+ */
+
+#include "vl.h"
+#include "arm_pic.h"
+
+#define GIC_NIRQ 64
+#define NCPU 1
+#define NVIC 1
+
+/* Only a single "CPU" interface is present.  */
+static inline int
+gic_get_current_cpu(void)
+{
+    return 0;
+}
+
+static uint32_t nvic_readl(void *opaque, uint32_t offset);
+static void nvic_writel(void *opaque, uint32_t offset, uint32_t value);
+
+#include "arm_gic.c"
+
+typedef struct {
+    struct {
+        uint32_t control;
+        uint32_t reload;
+        int64_t tick;
+        QEMUTimer *timer;
+    } systick;
+    gic_state *gic;
+} nvic_state;
+
+/* qemu timers run at 1GHz.   We want something closer to 1MHz.  */
+#define SYSTICK_SCALE 1000ULL
+
+#define SYSTICK_ENABLE    (1 << 0)
+#define SYSTICK_TICKINT   (1 << 1)
+#define SYSTICK_CLKSOURCE (1 << 2)
+#define SYSTICK_COUNTFLAG (1 << 16)
+
+/* Conversion factor from qemu timer to SysTick frequencies.
+   QEMU uses a base of 1GHz, so these give 20MHz and 1MHz for core and
+   reference frequencies.  */
+
+static inline int64_t systick_scale(nvic_state *s)
+{
+    if (s->systick.control & SYSTICK_CLKSOURCE)
+        return 50;
+    else
+        return 1000;
+}
+
+static void systick_reload(nvic_state *s, int reset)
+{
+    if (reset)
+        s->systick.tick = qemu_get_clock(vm_clock);
+    s->systick.tick += (s->systick.reload + 1) * systick_scale(s);
+    qemu_mod_timer(s->systick.timer, s->systick.tick);
+}
+
+static void systick_timer_tick(void * opaque)
+{
+    nvic_state *s = (nvic_state *)opaque;
+    s->systick.control |= SYSTICK_COUNTFLAG;
+    if (s->systick.control & SYSTICK_TICKINT) {
+        /* Trigger the interrupt.  */
+        armv7m_nvic_set_pending(s, ARMV7M_EXCP_SYSTICK);
+    }
+    if (s->systick.reload == 0) {
+        s->systick.control &= ~SYSTICK_ENABLE;
+    } else {
+        systick_reload(s, 0);
+    }
+}
+
+/* The external routines use the hardware vector numbering, ie. the first
+   IRQ is #16.  The internal GIC routines use #32 as the first IRQ.  */
+void armv7m_nvic_set_pending(void *opaque, int irq)
+{
+    nvic_state *s = (nvic_state *)opaque;
+    if (irq >= 16)
+        irq += 16;
+    gic_set_pending_private(s->gic, 0, irq);
+}
+
+/* Make pending IRQ active.  */
+int armv7m_nvic_acknowledge_irq(void *opaque)
+{
+    nvic_state *s = (nvic_state *)opaque;
+    uint32_t irq;
+
+    irq = gic_acknowledge_irq(s->gic, 0);
+    if (irq == 1023)
+        cpu_abort(cpu_single_env, "Interrupt but no vector\n");
+    if (irq >= 32)
+        irq -= 16;
+    return irq;
+}
+
+void armv7m_nvic_complete_irq(void *opaque, int irq)
+{
+    nvic_state *s = (nvic_state *)opaque;
+    if (irq >= 16)
+        irq += 16;
+    gic_complete_irq(s->gic, 0, irq);
+}
+
+static uint32_t nvic_readl(void *opaque, uint32_t offset)
+{
+    nvic_state *s = (nvic_state *)opaque;
+    uint32_t val;
+    int irq;
+
+    switch (offset) {
+    case 4: /* Interrupt Control Type.  */
+        return (GIC_NIRQ / 32) - 1;
+    case 0x10: /* SysTick Control and Status.  */
+        val = s->systick.control;
+        s->systick.control &= ~SYSTICK_COUNTFLAG;
+        return val;
+    case 0x14: /* SysTick Reload Value.  */
+        return s->systick.reload;
+    case 0x18: /* SysTick Current Value.  */
+        {
+            int64_t t;
+            if ((s->systick.control & SYSTICK_ENABLE) == 0)
+                return 0;
+            t = qemu_get_clock(vm_clock);
+            if (t >= s->systick.tick)
+                return 0;
+            val = ((s->systick.tick - (t + 1)) / systick_scale(s)) + 1;
+            /* The interrupt in triggered when the timer reaches zero.
+               However the counter is not reloaded until the next clock
+               tick.  This is a hack to return zero during the first tick.  */
+            if (val > s->systick.reload)
+                val = 0;
+            return val;
+        }
+    case 0x1c: /* SysTick Calibration Value.  */
+        return 10000;
+    case 0xd00: /* CPUID Base.  */
+        return cpu_single_env->cp15.c0_cpuid;
+    case 0xd04: /* Interrypt Control State.  */
+        /* VECTACTIVE */
+        val = s->gic->running_irq[0];
+        if (val == 1023) {
+            val = 0;
+        } else if (val >= 32) {
+            val -= 16;
+        }
+        /* RETTOBASE */
+        if (s->gic->running_irq[0] == 1023
+                || s->gic->last_active[s->gic->running_irq[0]][0] == 1023) {
+            val |= (1 << 11);
+        }
+        /* VECTPENDING */
+        if (s->gic->current_pending[0] != 1023)
+            val |= (s->gic->current_pending[0] << 12);
+        /* ISRPENDING */
+        for (irq = 32; irq < GIC_NIRQ; irq++) {
+            if (s->gic->irq_state[irq].pending) {
+                val |= (1 << 22);
+                break;
+            }
+        }
+        /* PENDSTSET */
+        if (s->gic->irq_state[ARMV7M_EXCP_SYSTICK].pending)
+            val |= (1 << 26);
+        /* PENDSVSET */
+        if (s->gic->irq_state[ARMV7M_EXCP_PENDSV].pending)
+            val |= (1 << 28);
+        /* NMIPENDSET */
+        if (s->gic->irq_state[ARMV7M_EXCP_NMI].pending)
+            val |= (1 << 31);
+        return val;
+    case 0xd08: /* Vector Table Offset.  */
+        return cpu_single_env->v7m.vecbase;
+    case 0xd0c: /* Application Interrupt/Reset Control.  */
+        return 0xfa05000;
+    case 0xd10: /* System Control.  */
+        /* TODO: Implement SLEEPONEXIT.  */
+        return 0;
+    case 0xd14: /* Configuration Control.  */
+        /* TODO: Implement Configuration Control bits.  */
+        return 0;
+    case 0xd18: case 0xd1c: case 0xd20: /* System Handler Priority.  */
+        irq = offset - 0xd14;
+        val = 0;
+        val = s->gic->priority1[irq++][0];
+        val = s->gic->priority1[irq++][0] << 8;
+        val = s->gic->priority1[irq++][0] << 16;
+        val = s->gic->priority1[irq][0] << 24;
+        return val;
+    case 0xd24: /* System Handler Status.  */
+        val = 0;
+        if (s->gic->irq_state[ARMV7M_EXCP_MEM].active) val |= (1 << 0);
+        if (s->gic->irq_state[ARMV7M_EXCP_BUS].active) val |= (1 << 1);
+        if (s->gic->irq_state[ARMV7M_EXCP_USAGE].active) val |= (1 << 3);
+        if (s->gic->irq_state[ARMV7M_EXCP_SVC].active) val |= (1 << 7);
+        if (s->gic->irq_state[ARMV7M_EXCP_DEBUG].active) val |= (1 << 8);
+        if (s->gic->irq_state[ARMV7M_EXCP_PENDSV].active) val |= (1 << 10);
+        if (s->gic->irq_state[ARMV7M_EXCP_SYSTICK].active) val |= (1 << 11);
+        if (s->gic->irq_state[ARMV7M_EXCP_USAGE].pending) val |= (1 << 12);
+        if (s->gic->irq_state[ARMV7M_EXCP_MEM].pending) val |= (1 << 13);
+        if (s->gic->irq_state[ARMV7M_EXCP_BUS].pending) val |= (1 << 14);
+        if (s->gic->irq_state[ARMV7M_EXCP_SVC].pending) val |= (1 << 15);
+        if (s->gic->irq_state[ARMV7M_EXCP_MEM].enabled) val |= (1 << 16);
+        if (s->gic->irq_state[ARMV7M_EXCP_BUS].enabled) val |= (1 << 17);
+        if (s->gic->irq_state[ARMV7M_EXCP_USAGE].enabled) val |= (1 << 18);
+        return val;
+    case 0xd28: /* Configurable Fault Status.  */
+        /* TODO: Implement Fault Status.  */
+        cpu_abort(cpu_single_env,
+                  "Not implemented: Configurable Fault Status.");
+        return 0;
+    case 0xd2c: /* Hard Fault Status.  */
+    case 0xd30: /* Debug Fault Status.  */
+    case 0xd34: /* Mem Manage Address.  */
+    case 0xd38: /* Bus Fault Address.  */
+    case 0xd3c: /* Aux Fault Status.  */
+        /* TODO: Implement fault status registers.  */
+        goto bad_reg;
+    case 0xd40: /* PFR0.  */
+        return 0x00000030;
+    case 0xd44: /* PRF1.  */
+        return 0x00000200;
+    case 0xd48: /* DFR0.  */
+        return 0x00100000;
+    case 0xd4c: /* AFR0.  */
+        return 0x00000000;
+    case 0xd50: /* MMFR0.  */
+        return 0x00000030;
+    case 0xd54: /* MMFR1.  */
+        return 0x00000000;
+    case 0xd58: /* MMFR2.  */
+        return 0x00000000;
+    case 0xd5c: /* MMFR3.  */
+        return 0x00000000;
+    case 0xd60: /* ISAR0.  */
+        return 0x01141110;
+    case 0xd64: /* ISAR1.  */
+        return 0x02111000;
+    case 0xd68: /* ISAR2.  */
+        return 0x21112231;
+    case 0xd6c: /* ISAR3.  */
+        return 0x01111110;
+    case 0xd70: /* ISAR4.  */
+        return 0x01310102;
+    /* TODO: Implement debug registers.  */
+    default:
+    bad_reg:
+        cpu_abort(cpu_single_env, "NVIC: Bad read offset 0x%x\n", offset);
+    }
+}
+
+static void nvic_writel(void *opaque, uint32_t offset, uint32_t value)
+{
+    nvic_state *s = (nvic_state *)opaque;
+    uint32_t oldval;
+    switch (offset) {
+    case 0x10: /* SysTick Control and Status.  */
+        oldval = s->systick.control;
+        s->systick.control &= 0xfffffff8;
+        s->systick.control |= value & 7;
+        if ((oldval ^ value) & SYSTICK_ENABLE) {
+            int64_t now = qemu_get_clock(vm_clock);
+            if (value & SYSTICK_ENABLE) {
+                if (s->systick.tick) {
+                    s->systick.tick += now;
+                    qemu_mod_timer(s->systick.timer, s->systick.tick);
+                } else {
+                    systick_reload(s, 1);
+                }
+            } else {
+                qemu_del_timer(s->systick.timer);
+                s->systick.tick -= now;
+                if (s->systick.tick < 0)
+                  s->systick.tick = 0;
+            }
+        } else if ((oldval ^ value) & SYSTICK_CLKSOURCE) {
+            /* This is a hack. Force the timer to be reloaded
+               when the reference clock is changed.  */
+            systick_reload(s, 1);
+        }
+        break;
+    case 0x14: /* SysTick Reload Value.  */
+        s->systick.reload = value;
+        break;
+    case 0x18: /* SysTick Current Value.  Writes reload the timer.  */
+        systick_reload(s, 1);
+        s->systick.control &= ~SYSTICK_COUNTFLAG;
+        break;
+    case 0xd04: /* Interrupt Control State.  */
+        if (value & (1 << 31)) {
+            armv7m_nvic_set_pending(s, ARMV7M_EXCP_NMI);
+        }
+        if (value & (1 << 28)) {
+            armv7m_nvic_set_pending(s, ARMV7M_EXCP_PENDSV);
+        } else if (value & (1 << 27)) {
+            s->gic->irq_state[ARMV7M_EXCP_PENDSV].pending = 0;
+            gic_update(s->gic);
+        }
+        if (value & (1 << 26)) {
+            armv7m_nvic_set_pending(s, ARMV7M_EXCP_SYSTICK);
+        } else if (value & (1 << 25)) {
+            s->gic->irq_state[ARMV7M_EXCP_SYSTICK].pending = 0;
+            gic_update(s->gic);
+        }
+        break;
+    case 0xd08: /* Vector Table Offset.  */
+        cpu_single_env->v7m.vecbase = value & 0xffffff80;
+        break;
+    case 0xd0c: /* Application Interrupt/Reset Control.  */
+        if ((value >> 16) == 0x05fa) {
+            if (value & 2) {
+                cpu_abort(cpu_single_env, "VECTCLRACTIVE not implemented");
+            }
+            if (value & 5) {
+                cpu_abort(cpu_single_env, "System reset");
+            }
+        }
+        break;
+    case 0xd10: /* System Control.  */
+    case 0xd14: /* Configuration Control.  */
+        /* TODO: Implement control registers.  */
+        goto bad_reg;
+    case 0xd18: case 0xd1c: case 0xd20: /* System Handler Priority.  */
+        {
+            int irq;
+            irq = offset - 0xd14;
+            s->gic->priority1[irq++][0] = value & 0xff;
+            s->gic->priority1[irq++][0] = (value >> 8) & 0xff;
+            s->gic->priority1[irq++][0] = (value >> 16) & 0xff;
+            s->gic->priority1[irq][0] = (value >> 24) & 0xff;
+            gic_update(s->gic);
+        }
+        break;
+    case 0xd24: /* System Handler Control.  */
+        /* TODO: Real hardware allows you to set/clear the active bits
+           under some circumstances.  We don't implement this.  */
+        s->gic->irq_state[ARMV7M_EXCP_MEM].enabled = (value & (1 << 16)) != 0;
+        s->gic->irq_state[ARMV7M_EXCP_BUS].enabled = (value & (1 << 17)) != 0;
+        s->gic->irq_state[ARMV7M_EXCP_USAGE].enabled = (value & (1 << 18)) != 0;
+        break;
+    case 0xd28: /* Configurable Fault Status.  */
+    case 0xd2c: /* Hard Fault Status.  */
+    case 0xd30: /* Debug Fault Status.  */
+    case 0xd34: /* Mem Manage Address.  */
+    case 0xd38: /* Bus Fault Address.  */
+    case 0xd3c: /* Aux Fault Status.  */
+        goto bad_reg;
+    default:
+    bad_reg:
+        cpu_abort(cpu_single_env, "NVIC: Bad write offset 0x%x\n", offset);
+    }
+}
+
+qemu_irq *armv7m_nvic_init(CPUState *env)
+{
+    nvic_state *s;
+    qemu_irq *parent;
+
+    parent = arm_pic_init_cpu(env);
+    s = (nvic_state *)qemu_mallocz(sizeof(nvic_state));
+    s->gic = gic_init(0xe000e000, &parent[ARM_PIC_CPU_IRQ]);
+    s->gic->nvic = s;
+    s->systick.timer = qemu_new_timer(vm_clock, systick_timer_tick, s);
+    if (env->v7m.nvic)
+        cpu_abort(env, "CPU can only have one NVIC\n");
+    env->v7m.nvic = s;
+    return s->gic->in;
+}

hw/integratorcp.c

@@ -497,8 +497,8 @@ static void integratorcp_init(int ram_size, int vga_ram_size,
     icp_pic_init(0xca000000, pic[26], NULL);
     icp_pit_init(0x13000000, pic, 5);
     pl031_init(0x15000000, pic[8]);
-    pl011_init(0x16000000, pic[1], serial_hds[0]);
+    pl011_init(0x16000000, pic[1], serial_hds[0], PL011_ARM);
-    pl011_init(0x17000000, pic[2], serial_hds[1]);
+    pl011_init(0x17000000, pic[2], serial_hds[1], PL011_ARM);
     icp_control_init(0xcb000000);
     pl050_init(0x18000000, pic[3], 0);
     pl050_init(0x19000000, pic[4], 1);

hw/mpcore.c

@@ -0,0 +1,323 @@
+/*
+ * ARM MPCore internal peripheral emulation.
+ *
+ * Copyright (c) 2006-2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+#include "vl.h"
+
+#define MPCORE_PRIV_BASE  0x10100000
+#define NCPU 4
+/* ??? The MPCore TRM says the on-chip controller has 224 external IRQ lines
+   (+ 32 internal).  However my test chip only exposes/reports 32.
+   More importantly Linux falls over if more than 32 are present!  */
+#define GIC_NIRQ 64
+
+static inline int
+gic_get_current_cpu(void)
+{
+  return cpu_single_env->cpu_index;
+}
+
+#include "arm_gic.c"
+
+/* MPCore private memory region.  */
+
+typedef struct {
+    uint32_t count;
+    uint32_t load;
+    uint32_t control;
+    uint32_t status;
+    uint32_t old_status;
+    int64_t tick;
+    QEMUTimer *timer;
+    struct mpcore_priv_state *mpcore;
+    int id; /* Encodes both timer/watchdog and CPU.  */
+} mpcore_timer_state;
+
+typedef struct mpcore_priv_state {
+    gic_state *gic;
+    uint32_t scu_control;
+    mpcore_timer_state timer[8];
+} mpcore_priv_state;
+
+/* Per-CPU Timers.  */
+
+static inline void mpcore_timer_update_irq(mpcore_timer_state *s)
+{
+    if (s->status & ~s->old_status) {
+        gic_set_pending_private(s->mpcore->gic, s->id >> 1, 29 + (s->id & 1));
+    }
+    s->old_status = s->status;
+}
+
+/* Return conversion factor from mpcore timer ticks to qemu timer ticks.  */
+static inline uint32_t mpcore_timer_scale(mpcore_timer_state *s)
+{
+    return (((s->control >> 8) & 0xff) + 1) * 10;
+}
+
+static void mpcore_timer_reload(mpcore_timer_state *s, int restart)
+{
+    if (s->count == 0)
+        return;
+    if (restart)
+        s->tick = qemu_get_clock(vm_clock);
+    s->tick += (int64_t)s->count * mpcore_timer_scale(s);
+    qemu_mod_timer(s->timer, s->tick);
+}
+
+static void mpcore_timer_tick(void *opaque)
+{
+    mpcore_timer_state *s = (mpcore_timer_state *)opaque;
+    s->status = 1;
+    if (s->control & 2) {
+        s->count = s->load;
+        mpcore_timer_reload(s, 0);
+    } else {
+        s->count = 0;
+    }
+    mpcore_timer_update_irq(s);
+}
+
+static uint32_t mpcore_timer_read(mpcore_timer_state *s, int offset)
+{
+    int64_t val;
+    switch (offset) {
+    case 0: /* Load */
+        return s->load;
+        /* Fall through.  */
+    case 4: /* Counter.  */
+        if (((s->control & 1) == 0) || (s->count == 0))
+            return 0;
+        /* Slow and ugly, but hopefully won't happen too often.  */
+        val = s->tick - qemu_get_clock(vm_clock);
+        val /= mpcore_timer_scale(s);
+        if (val < 0)
+            val = 0;
+        return val;
+    case 8: /* Control.  */
+        return s->control;
+    case 12: /* Interrupt status.  */
+        return s->status;
+    }
+}
+
+static void mpcore_timer_write(mpcore_timer_state *s, int offset,
+                               uint32_t value)
+{
+    int64_t old;
+    switch (offset) {
+    case 0: /* Load */
+        s->load = value;
+        /* Fall through.  */
+    case 4: /* Counter.  */
+        if ((s->control & 1) && s->count) {
+            /* Cancel the previous timer.  */
+            qemu_del_timer(s->timer);
+        }
+        s->count = value;
+        if (s->control & 1) {
+            mpcore_timer_reload(s, 1);
+        }
+        break;
+    case 8: /* Control.  */
+        old = s->control;
+        s->control = value;
+        if (((old & 1) == 0) && (value & 1)) {
+            if (s->count == 0 && (s->control & 2))
+                s->count = s->load;
+            mpcore_timer_reload(s, 1);
+        }
+        break;
+    case 12: /* Interrupt status.  */
+        s->status &= ~value;
+        mpcore_timer_update_irq(s);
+        break;
+    }
+}
+
+static void mpcore_timer_init(mpcore_priv_state *mpcore,
+                              mpcore_timer_state *s, int id)
+{
+    s->id = id;
+    s->mpcore = mpcore;
+    s->timer = qemu_new_timer(vm_clock, mpcore_timer_tick, s);
+}
+
+
+/* Per-CPU private memory mapped IO.  */
+
+static uint32_t mpcore_priv_read(void *opaque, target_phys_addr_t offset)
+{
+    mpcore_priv_state *s = (mpcore_priv_state *)opaque;
+    int id;
+    offset &= 0xfff;
+    if (offset < 0x100) {
+        /* SCU */
+        switch (offset) {
+        case 0x00: /* Control.  */
+            return s->scu_control;
+        case 0x04: /* Configuration.  */
+            return 0xf3;
+        case 0x08: /* CPU status.  */
+            return 0;
+        case 0x0c: /* Invalidate all.  */
+            return 0;
+        default:
+            goto bad_reg;
+        }
+    } else if (offset < 0x600) {
+        /* Interrupt controller.  */
+        if (offset < 0x200) {
+            id = gic_get_current_cpu();
+        } else {
+            id = (offset - 0x200) >> 8;
+        }
+        return gic_cpu_read(s->gic, id, offset & 0xff);
+    } else if (offset < 0xb00) {
+        /* Timers.  */
+        if (offset < 0x700) {
+            id = gic_get_current_cpu();
+        } else {
+            id = (offset - 0x700) >> 8;
+        }
+        id <<= 1;
+        if (offset & 0x20)
+          id++;
+        return mpcore_timer_read(&s->timer[id], offset & 0xf);
+    }
+bad_reg:
+    cpu_abort(cpu_single_env, "mpcore_priv_read: Bad offset %x\n",
+              (int)offset);
+    return 0;
+}
+
+static void mpcore_priv_write(void *opaque, target_phys_addr_t offset,
+                          uint32_t value)
+{
+    mpcore_priv_state *s = (mpcore_priv_state *)opaque;
+    int id;
+    offset &= 0xfff;
+    if (offset < 0x100) {
+        /* SCU */
+        switch (offset) {
+        case 0: /* Control register.  */
+            s->scu_control = value & 1;
+            break;
+        case 0x0c: /* Invalidate all.  */
+            /* This is a no-op as cache is not emulated.  */
+            break;
+        default:
+            goto bad_reg;
+        }
+    } else if (offset < 0x600) {
+        /* Interrupt controller.  */
+        if (offset < 0x200) {
+            id = gic_get_current_cpu();
+        } else {
+            id = (offset - 0x200) >> 8;
+        }
+        gic_cpu_write(s->gic, id, offset & 0xff, value);
+    } else if (offset < 0xb00) {
+        /* Timers.  */
+        if (offset < 0x700) {
+            id = gic_get_current_cpu();
+        } else {
+            id = (offset - 0x700) >> 8;
+        }
+        id <<= 1;
+        if (offset & 0x20)
+          id++;
+        mpcore_timer_write(&s->timer[id], offset & 0xf, value);
+        return;
+    }
+    return;
+bad_reg:
+    cpu_abort(cpu_single_env, "mpcore_priv_read: Bad offset %x\n",
+              (int)offset);
+}
+
+static CPUReadMemoryFunc *mpcore_priv_readfn[] = {
+   mpcore_priv_read,
+   mpcore_priv_read,
+   mpcore_priv_read
+};
+
+static CPUWriteMemoryFunc *mpcore_priv_writefn[] = {
+   mpcore_priv_write,
+   mpcore_priv_write,
+   mpcore_priv_write
+};
+
+
+static qemu_irq *mpcore_priv_init(uint32_t base, qemu_irq *pic_irq)
+{
+    mpcore_priv_state *s;
+    int iomemtype;
+    int i;
+
+    s = (mpcore_priv_state *)qemu_mallocz(sizeof(mpcore_priv_state));
+    if (!s)
+        return NULL;
+    s->gic = gic_init(base, pic_irq);
+    if (!s->gic)
+        return NULL;
+    iomemtype = cpu_register_io_memory(0, mpcore_priv_readfn,
+                                       mpcore_priv_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    for (i = 0; i < 8; i++) {
+        mpcore_timer_init(s, &s->timer[i], i);
+    }
+    return s->gic->in;
+}
+
+/* Dummy PIC to route IRQ lines.  The baseboard has 4 independent IRQ
+   controllers.  The output of these, plus some of the raw input lines
+   are fed into a single SMP-aware interrupt controller on the CPU.  */
+typedef struct {
+    qemu_irq *cpuic;
+    qemu_irq *rvic[4];
+} mpcore_rirq_state;
+
+/* Map baseboard IRQs onto CPU IRQ lines.  */
+static const int mpcore_irq_map[32] = {
+    -1, -1, -1, -1,  1,  2, -1, -1,
+    -1, -1,  6, -1,  4,  5, -1, -1,
+    -1, 14, 15,  0,  7,  8, -1, -1,
+    -1, -1, -1, -1,  9,  3, -1, -1,
+};
+
+static void mpcore_rirq_set_irq(void *opaque, int irq, int level)
+{
+    mpcore_rirq_state *s = (mpcore_rirq_state *)opaque;
+    int i;
+
+    for (i = 0; i < 4; i++) {
+        qemu_set_irq(s->rvic[i][irq], level);
+    }
+    if (irq < 32) {
+        irq = mpcore_irq_map[irq];
+        if (irq >= 0) {
+            qemu_set_irq(s->cpuic[irq], level);
+        }
+    }
+}
+
+qemu_irq *mpcore_irq_init(qemu_irq *cpu_irq)
+{
+    mpcore_rirq_state *s;
+    int n;
+
+    /* ??? IRQ routing is hardcoded to "normal" mode.  */
+    s = qemu_mallocz(sizeof(mpcore_rirq_state));
+    s->cpuic = mpcore_priv_init(MPCORE_PRIV_BASE, cpu_irq);
+    for (n = 0; n < 4; n++) {
+        s->rvic[n] = realview_gic_init(0x10040000 + n * 0x10000,
+                                       s->cpuic[10 + n]);
+    }
+    return qemu_allocate_irqs(mpcore_rirq_set_irq, s, 64);
+}

hw/pl011.c

@@ -28,6 +28,7 @@ typedef struct {
     int read_trigger;
     CharDriverState *chr;
     qemu_irq irq;
+    enum pl011_type type;
 } pl011_state;
 
 #define PL011_INT_TX 0x20
@@ -38,8 +39,10 @@ typedef struct {
 #define PL011_FLAG_TXFF 0x20
 #define PL011_FLAG_RXFE 0x10
 
-static const unsigned char pl011_id[] =
+static const unsigned char pl011_id[2][8] = {
-{ 0x11, 0x10, 0x14, 0x00, 0x0d, 0xf0, 0x05, 0xb1 };
+  { 0x11, 0x10, 0x14, 0x00, 0x0d, 0xf0, 0x05, 0xb1 }, /* PL011_ARM */
+  { 0x11, 0x00, 0x18, 0x01, 0x0d, 0xf0, 0x05, 0xb1 }, /* PL011_LUMINARY */
+};
 
 static void pl011_update(pl011_state *s)
 {
@@ -56,7 +59,7 @@ static uint32_t pl011_read(void *opaque, target_phys_addr_t offset)
 
     offset -= s->base;
     if (offset >= 0xfe0 && offset < 0x1000) {
-        return pl011_id[(offset - 0xfe0) >> 2];
+        return pl011_id[s->type][(offset - 0xfe0) >> 2];
     }
     switch (offset >> 2) {
     case 0: /* UARTDR */
@@ -137,6 +140,9 @@ static void pl011_write(void *opaque, target_phys_addr_t offset,
     case 1: /* UARTCR */
         s->cr = value;
         break;
+    case 6: /* UARTFR */
+        /* Writes to Flag register are ignored.  */
+        break;
     case 8: /* UARTUARTILPR */
         s->ilpr = value;
         break;
@@ -224,7 +230,7 @@ static CPUWriteMemoryFunc *pl011_writefn[] = {
 };
 
 void pl011_init(uint32_t base, qemu_irq irq,
-                CharDriverState *chr)
+                CharDriverState *chr, enum pl011_type type)
 {
     int iomemtype;
     pl011_state *s;
@@ -235,6 +241,7 @@ void pl011_init(uint32_t base, qemu_irq irq,
     cpu_register_physical_memory(base, 0x00001000, iomemtype);
     s->base = base;
     s->irq = irq;
+    s->type = type;
     s->chr = chr;
     s->read_trigger = 1;
     s->ifl = 0x12;

hw/pl022.c

@@ -0,0 +1,264 @@
+/*
+ * Arm PrimeCell PL022 Synchronous Serial Port
+ *
+ * Copyright (c) 2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+#include "vl.h"
+
+//#define DEBUG_PL022 1
+
+#ifdef DEBUG_PL022
+#define DPRINTF(fmt, args...) \
+do { printf("pl022: " fmt , ##args); } while (0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "pl022: error: " fmt , ##args); exit(1);} while (0)
+#else
+#define DPRINTF(fmt, args...) do {} while(0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "pl022: error: " fmt , ##args);} while (0)
+#endif
+
+#define PL022_CR1_LBM 0x01
+#define PL022_CR1_SSE 0x02
+#define PL022_CR1_MS  0x04
+#define PL022_CR1_SDO 0x08
+
+#define PL022_SR_TFE  0x01
+#define PL022_SR_TNF  0x02
+#define PL022_SR_RNE  0x04
+#define PL022_SR_RFF  0x08
+#define PL022_SR_BSY  0x10
+
+#define PL022_INT_ROR 0x01
+#define PL022_INT_RT  0x04
+#define PL022_INT_RX  0x04
+#define PL022_INT_TX  0x08
+
+typedef struct {
+    uint32_t base;
+    uint32_t cr0;
+    uint32_t cr1;
+    uint32_t bitmask;
+    uint32_t sr;
+    uint32_t cpsr;
+    uint32_t is;
+    uint32_t im;
+    /* The FIFO head points to the next empty entry.  */
+    int tx_fifo_head;
+    int rx_fifo_head;
+    int tx_fifo_len;
+    int rx_fifo_len;
+    uint16_t tx_fifo[8];
+    uint16_t rx_fifo[8];
+    qemu_irq irq;
+    int (*xfer_cb)(void *, int);
+    void *opaque;
+} pl022_state;
+
+static const unsigned char pl022_id[8] =
+  { 0x22, 0x10, 0x04, 0x00, 0x0d, 0xf0, 0x05, 0xb1 };
+
+static void pl022_update(pl022_state *s)
+{
+    s->sr = 0;
+    if (s->tx_fifo_len == 0)
+        s->sr |= PL022_SR_TFE;
+    if (s->tx_fifo_len != 8)
+        s->sr |= PL022_SR_TNF;
+    if (s->rx_fifo_len != 0)
+        s->sr |= PL022_SR_RNE;
+    if (s->rx_fifo_len == 8)
+        s->sr |= PL022_SR_RFF;
+    if (s->tx_fifo_len)
+        s->sr |= PL022_SR_BSY;
+    s->is = 0;
+    if (s->rx_fifo_len >= 4)
+        s->is |= PL022_INT_RX;
+    if (s->tx_fifo_len <= 4)
+        s->is |= PL022_INT_TX;
+
+    qemu_set_irq(s->irq, (s->is & s->im) != 0);
+}
+
+static void pl022_xfer(pl022_state *s)
+{
+    int i;
+    int o;
+    int val;
+
+    if ((s->cr1 & PL022_CR1_SSE) == 0) {
+        pl022_update(s);
+        DPRINTF("Disabled\n");
+        return;
+    }
+
+    DPRINTF("Maybe xfer %d/%d\n", s->tx_fifo_len, s->rx_fifo_len);
+    i = (s->tx_fifo_head - s->tx_fifo_len) & 7;
+    o = s->rx_fifo_head;
+    /* ??? We do not emulate the line speed.
+       This may break some applications.  The are two problematic cases:
+        (a) A driver feeds data into the TX FIFO until it is full,
+         and only then drains the RX FIFO.  On real hardware the CPU can
+         feed data fast enough that the RX fifo never gets chance to overflow.
+        (b) A driver transmits data, deliberately allowing the RX FIFO to
+         overflow because it ignores the RX data anyway.
+
+       We choose to support (a) by stalling the transmit engine if it would
+       cause the RX FIFO to overflow.  In practice much transmit-only code
+       falls into (a) because it flushes the RX FIFO to determine when
+       the transfer has completed.  */
+    while (s->tx_fifo_len && s->rx_fifo_len < 8) {
+        DPRINTF("xfer\n");
+        val = s->tx_fifo[i];
+        if (s->cr1 & PL022_CR1_LBM) {
+            /* Loopback mode.  */
+        } else if (s->xfer_cb) {
+            val = s->xfer_cb(s->opaque, val);
+        } else {
+            val = 0;
+        }
+        s->rx_fifo[o] = val & s->bitmask;
+        i = (i + 1) & 7;
+        o = (o + 1) & 7;
+        s->tx_fifo_len--;
+        s->rx_fifo_len++;
+    }
+    s->rx_fifo_head = o;
+    pl022_update(s);
+}
+
+static uint32_t pl022_read(void *opaque, target_phys_addr_t offset)
+{
+    pl022_state *s = (pl022_state *)opaque;
+    int val;
+
+    offset -= s->base;
+    if (offset >= 0xfe0 && offset < 0x1000) {
+        return pl022_id[(offset - 0xfe0) >> 2];
+    }
+    switch (offset) {
+    case 0x00: /* CR0 */
+      return s->cr0;
+    case 0x04: /* CR1 */
+      return s->cr1;
+    case 0x08: /* DR */
+        if (s->rx_fifo_len) {
+            val = s->rx_fifo[(s->rx_fifo_head - s->rx_fifo_len) & 7];
+            DPRINTF("RX %02x\n", val);
+            s->rx_fifo_len--;
+            pl022_xfer(s);
+        } else {
+            val = 0;
+        }
+        return val;
+    case 0x0c: /* SR */
+        return s->sr;
+    case 0x10: /* CPSR */
+        return s->cpsr;
+    case 0x14: /* IMSC */
+        return s->im;
+    case 0x18: /* RIS */
+        return s->is;
+    case 0x1c: /* MIS */
+        return s->im & s->is;
+    case 0x20: /* DMACR */
+        /* Not implemented.  */
+        return 0;
+    default:
+        cpu_abort (cpu_single_env, "pl022_read: Bad offset %x\n",
+                   (int)offset);
+        return 0;
+    }
+}
+
+static void pl022_write(void *opaque, target_phys_addr_t offset,
+                        uint32_t value)
+{
+    pl022_state *s = (pl022_state *)opaque;
+
+    offset -= s->base;
+    switch (offset) {
+    case 0x00: /* CR0 */
+        s->cr0 = value;
+        /* Clock rate and format are ignored.  */
+        s->bitmask = (1 << ((value & 15) + 1)) - 1;
+        break;
+    case 0x04: /* CR1 */
+        s->cr1 = value;
+        if ((s->cr1 & (PL022_CR1_MS | PL022_CR1_SSE))
+                   == (PL022_CR1_MS | PL022_CR1_SSE)) {
+            BADF("SPI slave mode not implemented\n");
+        }
+        pl022_xfer(s);
+        break;
+    case 0x08: /* DR */
+        if (s->tx_fifo_len < 8) {
+            DPRINTF("TX %02x\n", value);
+            s->tx_fifo[s->tx_fifo_head] = value & s->bitmask;
+            s->tx_fifo_head = (s->tx_fifo_head + 1) & 7;
+            s->tx_fifo_len++;
+            pl022_xfer(s);
+        }
+        break;
+    case 0x10: /* CPSR */
+        /* Prescaler.  Ignored.  */
+        s->cpsr = value & 0xff;
+        break;
+    case 0x14: /* IMSC */
+        s->im = value;
+        pl022_update(s);
+        break;
+    case 0x20: /* DMACR */
+        if (value)
+            cpu_abort (cpu_single_env, "pl022: DMA not implemented\n");
+        break;
+    default:
+        cpu_abort (cpu_single_env, "pl022_write: Bad offset %x\n",
+                   (int)offset);
+    }
+}
+
+static void pl022_reset(pl022_state *s)
+{
+    s->rx_fifo_len = 0;
+    s->tx_fifo_len = 0;
+    s->im = 0;
+    s->is = PL022_INT_TX;
+    s->sr = PL022_SR_TFE | PL022_SR_TNF;
+}
+
+static CPUReadMemoryFunc *pl022_readfn[] = {
+   pl022_read,
+   pl022_read,
+   pl022_read
+};
+
+static CPUWriteMemoryFunc *pl022_writefn[] = {
+   pl022_write,
+   pl022_write,
+   pl022_write
+};
+
+void pl022_init(uint32_t base, qemu_irq irq, int (*xfer_cb)(void *, int),
+                void * opaque)
+{
+    int iomemtype;
+    pl022_state *s;
+
+    s = (pl022_state *)qemu_mallocz(sizeof(pl022_state));
+    iomemtype = cpu_register_io_memory(0, pl022_readfn,
+                                       pl022_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    s->base = base;
+    s->irq = irq;
+    s->xfer_cb = xfer_cb;
+    s->opaque = opaque;
+    pl022_reset(s);
+    /* ??? Save/restore.  */
+}
+
+

hw/pl061.c

@@ -0,0 +1,256 @@
+/*
+ * Arm PrimeCell PL061 General Purpose IO with additional
+ * Luminary Micro Stellaris bits.
+ *
+ * Copyright (c) 2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+#include "vl.h"
+
+//#define DEBUG_PL061 1
+
+#ifdef DEBUG_PL061
+#define DPRINTF(fmt, args...) \
+do { printf("pl061: " fmt , ##args); } while (0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "pl061: error: " fmt , ##args); exit(1);} while (0)
+#else
+#define DPRINTF(fmt, args...) do {} while(0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "pl061: error: " fmt , ##args);} while (0)
+#endif
+
+static const uint8_t pl061_id[12] =
+  { 0x00, 0x00, 0x00, 0x00, 0x61, 0x00, 0x18, 0x01, 0x0d, 0xf0, 0x05, 0xb1 };
+
+typedef struct {
+    uint32_t base;
+    int locked;
+    uint8_t data;
+    uint8_t old_data;
+    uint8_t dir;
+    uint8_t isense;
+    uint8_t ibe;
+    uint8_t iev;
+    uint8_t im;
+    uint8_t istate;
+    uint8_t afsel;
+    uint8_t dr2r;
+    uint8_t dr4r;
+    uint8_t dr8r;
+    uint8_t odr;
+    uint8_t pur;
+    uint8_t pdr;
+    uint8_t slr;
+    uint8_t den;
+    uint8_t cr;
+    qemu_irq irq;
+    qemu_irq out[8];
+} pl061_state;
+
+static void pl061_update(pl061_state *s)
+{
+    uint8_t changed;
+    uint8_t mask;
+    int i;
+
+    changed = s->old_data ^ s->data;
+    if (!changed)
+        return;
+
+    s->old_data = s->data;
+    for (i = 0; i < 8; i++) {
+        mask = 1 << i;
+        if ((changed & mask & s->dir) && s->out) {
+            DPRINTF("Set output %d = %d\n", i, (s->data & mask) != 0);
+            qemu_set_irq(s->out[i], (s->data & mask) != 0);
+        }
+    }
+
+    /* FIXME: Implement input interrupts.  */
+}
+
+static uint32_t pl061_read(void *opaque, target_phys_addr_t offset)
+{
+    pl061_state *s = (pl061_state *)opaque;
+
+    offset -= s->base;
+    if (offset >= 0xfd0 && offset < 0x1000) {
+        return pl061_id[(offset - 0xfd0) >> 2];
+    }
+    if (offset < 0x400) {
+        return s->data & (offset >> 2);
+    }
+    switch (offset) {
+    case 0x400: /* Direction */
+        return s->dir;
+    case 0x404: /* Interrupt sense */
+        return s->isense;
+    case 0x408: /* Interrupt both edges */
+        return s->ibe;
+    case 0x40c: /* Interupt event */
+        return s->iev;
+    case 0x410: /* Interrupt mask */
+        return s->im;
+    case 0x414: /* Raw interrupt status */
+        return s->istate;
+    case 0x418: /* Masked interrupt status */
+        return s->istate | s->im;
+    case 0x420: /* Alternate function select */
+        return s->afsel;
+    case 0x500: /* 2mA drive */
+        return s->dr2r;
+    case 0x504: /* 4mA drive */
+        return s->dr4r;
+    case 0x508: /* 8mA drive */
+        return s->dr8r;
+    case 0x50c: /* Open drain */
+        return s->odr;
+    case 0x510: /* Pull-up */
+        return s->pur;
+    case 0x514: /* Pull-down */
+        return s->pdr;
+    case 0x518: /* Slew rate control */
+        return s->slr;
+    case 0x51c: /* Digital enable */
+        return s->den;
+    case 0x520: /* Lock */
+        return s->locked;
+    case 0x524: /* Commit */
+        return s->cr;
+    default:
+        cpu_abort (cpu_single_env, "pl061_read: Bad offset %x\n",
+                   (int)offset);
+        return 0;
+    }
+}
+
+static void pl061_write(void *opaque, target_phys_addr_t offset,
+                        uint32_t value)
+{
+    pl061_state *s = (pl061_state *)opaque;
+    uint8_t mask;
+
+    offset -= s->base;
+    if (offset < 0x400) {
+        mask = (offset >> 2) & s->dir;
+        s->data = (s->data & ~mask) | (value & mask);
+        pl061_update(s);
+        return;
+    }
+    switch (offset) {
+    case 0x400: /* Direction */
+        s->dir = value;
+        break;
+    case 0x404: /* Interrupt sense */
+        s->isense = value;
+        break;
+    case 0x408: /* Interrupt both edges */
+        s->ibe = value;
+        break;
+    case 0x40c: /* Interupt event */
+        s->iev = value;
+        break;
+    case 0x410: /* Interrupt mask */
+        s->im = value;
+        break;
+    case 0x41c: /* Interrupt clear */
+        s->istate &= ~value;
+        break;
+    case 0x420: /* Alternate function select */
+        mask = s->cr;
+        s->afsel = (s->afsel & ~mask) | (value & mask);
+        break;
+    case 0x500: /* 2mA drive */
+        s->dr2r = value;
+        break;
+    case 0x504: /* 4mA drive */
+        s->dr4r = value;
+        break;
+    case 0x508: /* 8mA drive */
+        s->dr8r = value;
+        break;
+    case 0x50c: /* Open drain */
+        s->odr = value;
+        break;
+    case 0x510: /* Pull-up */
+        s->pur = value;
+        break;
+    case 0x514: /* Pull-down */
+        s->pdr = value;
+        break;
+    case 0x518: /* Slew rate control */
+        s->slr = value;
+        break;
+    case 0x51c: /* Digital enable */
+        s->den = value;
+        break;
+    case 0x520: /* Lock */
+        s->locked = (value != 0xacce551);
+        break;
+    case 0x524: /* Commit */
+        if (!s->locked)
+            s->cr = value;
+        break;
+    default:
+        cpu_abort (cpu_single_env, "pl061_write: Bad offset %x\n",
+                   (int)offset);
+    }
+    pl061_update(s);
+}
+
+static void pl061_reset(pl061_state *s)
+{
+  s->locked = 1;
+  s->cr = 0xff;
+}
+
+void pl061_set_irq(void * opaque, int irq, int level)
+{
+    pl061_state *s = (pl061_state *)opaque;
+    uint8_t mask;
+
+    mask = 1 << irq;
+    if ((s->dir & mask) == 0) {
+        s->data &= ~mask;
+        if (level)
+            s->data |= mask;
+        pl061_update(s);
+    }
+}
+
+static CPUReadMemoryFunc *pl061_readfn[] = {
+   pl061_read,
+   pl061_read,
+   pl061_read
+};
+
+static CPUWriteMemoryFunc *pl061_writefn[] = {
+   pl061_write,
+   pl061_write,
+   pl061_write
+};
+
+/* Returns an array of inputs.  */
+qemu_irq *pl061_init(uint32_t base, qemu_irq irq, qemu_irq **out)
+{
+    int iomemtype;
+    pl061_state *s;
+
+    s = (pl061_state *)qemu_mallocz(sizeof(pl061_state));
+    iomemtype = cpu_register_io_memory(0, pl061_readfn,
+                                       pl061_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    s->base = base;
+    s->irq = irq;
+    pl061_reset(s);
+    if (out)
+        *out = s->out;
+
+    /* ??? Save/restore.  */
+    return qemu_allocate_irqs(pl061_set_irq, s, 8);
+}
+

hw/pxa2xx.c

@@ -297,7 +297,7 @@ static void pxa2xx_clkpwr_write(void *opaque, int op2, int reg, int crm,
                     ARM_CPU_MODE_SVC | CPSR_A | CPSR_F | CPSR_I;
             s->env->cp15.c1_sys = 0;
             s->env->cp15.c1_coproc = 0;
-            s->env->cp15.c2_base = 0;
+            s->env->cp15.c2_base0 = 0;
             s->env->cp15.c3 = 0;
             s->pm_regs[PSSR >> 2] |= 0x8;	/* Set STS */
             s->pm_regs[RCSR >> 2] |= 0x8;	/* Set GPR */
@@ -2031,7 +2031,8 @@ struct pxa2xx_state_s *pxa270_init(unsigned int sdram_size,
         fprintf(stderr, "Unable to find CPU definition\n");
         exit(1);
     }
-    register_savevm("cpu", 0, 0, cpu_save, cpu_load, s->env);
+    register_savevm("cpu", 0, ARM_CPU_SAVE_VERSION, cpu_save, cpu_load,
+                    s->env);
 
     /* SDRAM & Internal Memory Storage */
     cpu_register_physical_memory(PXA2XX_SDRAM_BASE,
@@ -2145,7 +2146,8 @@ struct pxa2xx_state_s *pxa255_init(unsigned int sdram_size,
         fprintf(stderr, "Unable to find CPU definition\n");
         exit(1);
     }
-    register_savevm("cpu", 0, 0, cpu_save, cpu_load, s->env);
+    register_savevm("cpu", 0, ARM_CPU_SAVE_VERSION, cpu_save, cpu_load,
+                    s->env);
 
     /* SDRAM & Internal Memory Storage */
     cpu_register_physical_memory(PXA2XX_SDRAM_BASE, sdram_size,

hw/realview.c

@@ -25,13 +25,32 @@ static void realview_init(int ram_size, int vga_ram_size,
     NICInfo *nd;
     int n;
     int done_smc = 0;
+    qemu_irq cpu_irq[4];
+    int ncpu;
 
     if (!cpu_model)
         cpu_model = "arm926";
-    env = cpu_init(cpu_model);
+    /* FIXME: obey smp_cpus.  */
-    if (!env) {
+    if (strcmp(cpu_model, "arm11mpcore") == 0) {
-        fprintf(stderr, "Unable to find CPU definition\n");
+        ncpu = 4;
-        exit(1);
+    } else {
+        ncpu = 1;
+    }
+
+    for (n = 0; n < ncpu; n++) {
+        env = cpu_init(cpu_model);
+        if (!env) {
+            fprintf(stderr, "Unable to find CPU definition\n");
+            exit(1);
+        }
+        pic = arm_pic_init_cpu(env);
+        cpu_irq[n] = pic[ARM_PIC_CPU_IRQ];
+        if (n > 0) {
+            /* Set entry point for secondary CPUs.  This assumes we're using
+               the init code from arm_boot.c.  Real hardware resets all CPUs
+               the same.  */
+            env->regs[15] = 0x80000000;
+        }
     }
 
     /* ??? RAM shoud repeat to fill physical memory space.  */
@@ -39,18 +58,23 @@ static void realview_init(int ram_size, int vga_ram_size,
     cpu_register_physical_memory(0, ram_size, IO_MEM_RAM);
 
     arm_sysctl_init(0x10000000, 0xc1400400);
-    pic = arm_pic_init_cpu(env);
+
-    /* ??? The documentation says GIC1 is nFIQ and either GIC2 or GIC3
+    if (ncpu == 1) {
-       is nIRQ (there are inconsistencies).  However Linux 2.6.17 expects
+        /* ??? The documentation says GIC1 is nFIQ and either GIC2 or GIC3
-       GIC1 to be nIRQ and ignores all the others, so do that for now.  */
+           is nIRQ (there are inconsistencies).  However Linux 2.6.17 expects
-    pic = arm_gic_init(0x10040000, pic[ARM_PIC_CPU_IRQ]);
+           GIC1 to be nIRQ and ignores all the others, so do that for now.  */
+        pic = realview_gic_init(0x10040000, cpu_irq[0]);
+    } else {
+        pic = mpcore_irq_init(cpu_irq);
+    }
+
     pl050_init(0x10006000, pic[20], 0);
     pl050_init(0x10007000, pic[21], 1);
 
-    pl011_init(0x10009000, pic[12], serial_hds[0]);
+    pl011_init(0x10009000, pic[12], serial_hds[0], PL011_ARM);
-    pl011_init(0x1000a000, pic[13], serial_hds[1]);
+    pl011_init(0x1000a000, pic[13], serial_hds[1], PL011_ARM);
-    pl011_init(0x1000b000, pic[14], serial_hds[2]);
+    pl011_init(0x1000b000, pic[14], serial_hds[2], PL011_ARM);
-    pl011_init(0x1000c000, pic[15], serial_hds[3]);
+    pl011_init(0x1000c000, pic[15], serial_hds[3], PL011_ARM);
 
     /* DMA controller is optional, apparently.  */
     pl080_init(0x10030000, pic[24], 2);
@@ -114,10 +138,10 @@ static void realview_init(int ram_size, int vga_ram_size,
     /*  0x10019000 PCI controller config.  */
     /*  0x10020000 CLCD.  */
     /* 0x10030000 DMA Controller.  */
-    /* 0x10040000 GIC1 (FIQ1).  */
+    /* 0x10040000 GIC1.  */
-    /* 0x10050000 GIC2 (IRQ1).  */
+    /* 0x10050000 GIC2.  */
-    /*  0x10060000 GIC3 (FIQ2).  */
+    /* 0x10060000 GIC3.  */
-    /*  0x10070000 GIC4 (IRQ2).  */
+    /* 0x10070000 GIC4.  */
     /*  0x10080000 SMC.  */
     /*  0x40000000 NOR flash.  */
     /*  0x44000000 DoC flash.  */
@@ -137,8 +161,14 @@ static void realview_init(int ram_size, int vga_ram_size,
     /* 0x68000000 PCI mem 1.  */
     /* 0x6c000000 PCI mem 2.  */
 
-    arm_load_kernel(env, ram_size, kernel_filename, kernel_cmdline,
+    arm_load_kernel(first_cpu, ram_size, kernel_filename, kernel_cmdline,
                     initrd_filename, 0x33b, 0x0);
+
+    /* ??? Hack to map an additional page of ram for the secondary CPU
+       startup code.  I guess this works on real hardware because the
+       BootROM happens to be in ROM/flash or in memory that isn't clobbered
+       until after Linux boots the secondary CPUs.  */
+    cpu_register_physical_memory(0x80000000, 0x1000, IO_MEM_RAM + ram_size);
 }
 
 QEMUMachine realview_machine = {

hw/realview_gic.c

@@ -0,0 +1,64 @@
+/*
+ * ARM RealView Emulation Baseboard Interrupt Controller
+ *
+ * Copyright (c) 2006-2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+#include "vl.h"
+#include "arm_pic.h"
+
+#define GIC_NIRQ 96
+#define NCPU 1
+
+/* Only a single "CPU" interface is present.  */
+static inline int
+gic_get_current_cpu(void)
+{
+  return 0;
+}
+
+#include "arm_gic.c"
+
+static uint32_t realview_gic_cpu_read(void *opaque, target_phys_addr_t offset)
+{
+    gic_state *s = (gic_state *)opaque;
+    offset -= s->base;
+    return gic_cpu_read(s, gic_get_current_cpu(), offset);
+}
+
+static void realview_gic_cpu_write(void *opaque, target_phys_addr_t offset,
+                          uint32_t value)
+{
+    gic_state *s = (gic_state *)opaque;
+    offset -= s->base;
+    gic_cpu_write(s, gic_get_current_cpu(), offset, value);
+}
+
+static CPUReadMemoryFunc *realview_gic_cpu_readfn[] = {
+   realview_gic_cpu_read,
+   realview_gic_cpu_read,
+   realview_gic_cpu_read
+};
+
+static CPUWriteMemoryFunc *realview_gic_cpu_writefn[] = {
+   realview_gic_cpu_write,
+   realview_gic_cpu_write,
+   realview_gic_cpu_write
+};
+
+qemu_irq *realview_gic_init(uint32_t base, qemu_irq parent_irq)
+{
+    gic_state *s;
+    int iomemtype;
+
+    s = gic_init(base, &parent_irq);
+    if (!s)
+        return NULL;
+    iomemtype = cpu_register_io_memory(0, realview_gic_cpu_readfn,
+                                       realview_gic_cpu_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    return s->in;
+}

hw/ssd0303.c

@@ -0,0 +1,273 @@
+/*
+ * SSD0303 OLED controller with OSRAM Pictiva 96x16 display.
+ *
+ * Copyright (c) 2006-2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+/* The controller can support a variety of different displays, but we only
+   implement one.  Most of the commends relating to brightness and geometry
+   setup are ignored. */
+#include "vl.h"
+
+//#define DEBUG_SSD0303 1
+
+#ifdef DEBUG_SSD0303
+#define DPRINTF(fmt, args...) \
+do { printf("ssd0303: " fmt , ##args); } while (0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "ssd0303: error: " fmt , ##args); exit(1);} while (0)
+#else
+#define DPRINTF(fmt, args...) do {} while(0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "ssd0303: error: " fmt , ##args);} while (0)
+#endif
+
+/* Scaling factor for pixels.  */
+#define MAGNIFY 4
+
+enum ssd0303_mode
+{
+    SSD0303_IDLE,
+    SSD0303_DATA,
+    SSD0303_CMD
+};
+
+enum ssd0303_cmd {
+    SSD0303_CMD_NONE,
+    SSD0303_CMD_SKIP1
+};
+
+typedef struct {
+    i2c_slave i2c;
+    DisplayState *ds;
+    int row;
+    int col;
+    int start_line;
+    int mirror;
+    int flash;
+    int enabled;
+    int inverse;
+    int redraw;
+    enum ssd0303_mode mode;
+    enum ssd0303_cmd cmd_state;
+    uint8_t framebuffer[132*8];
+} ssd0303_state;
+
+static int ssd0303_recv(i2c_slave *i2c)
+{
+    BADF("Reads not implemented\n");
+    return -1;
+}
+
+static int ssd0303_send(i2c_slave *i2c, uint8_t data)
+{
+    ssd0303_state *s = (ssd0303_state *)i2c;
+    enum ssd0303_cmd old_cmd_state;
+    switch (s->mode) {
+    case SSD0303_IDLE:
+        DPRINTF("byte 0x%02x\n", data);
+        if (data == 0x80)
+            s->mode = SSD0303_CMD;
+        else if (data == 0x40)
+            s->mode = SSD0303_DATA;
+        else
+            BADF("Unexpected byte 0x%x\n", data);
+        break;
+    case SSD0303_DATA:
+        DPRINTF("data 0x%02x\n", data);
+        if (s->col < 132) {
+            s->framebuffer[s->col + s->row * 132] = data;
+            s->col++;
+            s->redraw = 1;
+        }
+        break;
+    case SSD0303_CMD:
+        old_cmd_state = s->cmd_state;
+        s->cmd_state = SSD0303_CMD_NONE;
+        switch (old_cmd_state) {
+        case SSD0303_CMD_NONE:
+            DPRINTF("cmd 0x%02x\n", data);
+            s->mode = SSD0303_IDLE;
+            switch (data) {
+            case 0x00 ... 0x0f: /* Set lower colum address.  */
+                s->col = (s->col & 0xf0) | (data & 0xf);
+                break;
+            case 0x10 ... 0x20: /* Set higher column address.  */
+                s->col = (s->col & 0x0f) | ((data & 0xf) << 4);
+                break;
+            case 0x40 ... 0x7f: /* Set start line.  */
+                s->start_line = 0;
+                break;
+            case 0x81: /* Set contrast (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xa0: /* Mirror off.  */
+                s->mirror = 0;
+                break;
+            case 0xa1: /* Mirror off.  */
+                s->mirror = 1;
+                break;
+            case 0xa4: /* Entire display off.  */
+                s->flash = 0;
+                break;
+            case 0xa5: /* Entire display on.  */
+                s->flash = 1;
+                break;
+            case 0xa6: /* Inverse off.  */
+                s->inverse = 0;
+                break;
+            case 0xa7: /* Inverse on.  */
+                s->inverse = 1;
+                break;
+            case 0xa8: /* Set multipled ratio (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xad: /* DC-DC power control.  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xae: /* Display off.  */
+                s->enabled = 0;
+                break;
+            case 0xaf: /* Display on.  */
+                s->enabled = 1;
+                break;
+            case 0xb0 ... 0xbf: /* Set Page address.  */
+                s->row = data & 7;
+                break;
+            case 0xc0 ... 0xc8: /* Set COM output direction (Ignored).  */
+                break;
+            case 0xd3: /* Set display offset (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xd5: /* Set display clock (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xd8: /* Set color and power mode (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xd9: /* Set pre-charge period (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xda: /* Set COM pin configuration (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xdb: /* Set VCOM dselect level (Ignored).  */
+                s->cmd_state = SSD0303_CMD_SKIP1;
+                break;
+            case 0xe3: /* no-op.  */
+                break;
+            default:
+                BADF("Unknown command: 0x%x\n", data);
+            }
+            break;
+        case SSD0303_CMD_SKIP1:
+            DPRINTF("skip 0x%02x\n", data);
+            break;
+        }
+        break;
+    }
+    return 0;
+}
+
+static void ssd0303_event(i2c_slave *i2c, enum i2c_event event)
+{
+    ssd0303_state *s = (ssd0303_state *)i2c;
+    switch (event) {
+    case I2C_FINISH:
+        s->mode = SSD0303_IDLE;
+        break;
+    case I2C_START_RECV:
+    case I2C_START_SEND:
+    case I2C_NACK:
+        /* Nothing to do.  */
+        break;
+    }
+}
+
+static void ssd0303_update_display(void *opaque)
+{
+    ssd0303_state *s = (ssd0303_state *)opaque;
+    uint8_t *dest;
+    uint8_t *src;
+    int x;
+    int y;
+    int line;
+    char *colors[2];
+    char colortab[MAGNIFY * 8];
+    int dest_width;
+    uint8_t mask;
+
+    if (s->redraw) {
+        switch (s->ds->depth) {
+        case 0:
+            return;
+        case 15:
+            dest_width = 2;
+            break;
+        case 16:
+            dest_width = 2;
+            break;
+        case 24:
+            dest_width = 3;
+            break;
+        case 32:
+            dest_width = 4;
+            break;
+        default:
+            BADF("Bad color depth\n");
+            return;
+        }
+        dest_width *= MAGNIFY;
+        memset(colortab, 0xff, dest_width);
+        memset(colortab + dest_width, 0, dest_width);
+        if (s->flash) {
+            colors[0] = colortab;
+            colors[1] = colortab;
+        } else if (s->inverse) {
+            colors[0] = colortab;
+            colors[1] = colortab + dest_width;
+        } else {
+            colors[0] = colortab + dest_width;
+            colors[1] = colortab;
+        }
+        dest = s->ds->data;
+        for (y = 0; y < 16; y++) {
+            line = (y + s->start_line) & 63;
+            src = s->framebuffer + 132 * (line >> 3) + 36;
+            mask = 1 << (line & 7);
+            for (x = 0; x < 96; x++) {
+                memcpy(dest, colors[(*src & mask) != 0], dest_width);
+                dest += dest_width;
+                src++;
+            }
+            for (x = 1; x < MAGNIFY; x++) {
+                memcpy(dest, dest - dest_width * 96, dest_width * 96);
+                dest += dest_width * 96;
+            }
+        }
+    }
+    dpy_update(s->ds, 0, 0, 96 * MAGNIFY, 16 * MAGNIFY);
+}
+
+static void ssd0303_invalidate_display(void * opaque)
+{
+    ssd0303_state *s = (ssd0303_state *)opaque;
+    s->redraw = 1;
+}
+
+void ssd0303_init(DisplayState *ds, i2c_bus *bus, int address)
+{
+    ssd0303_state *s;
+
+    s = (ssd0303_state *)i2c_slave_init(bus, address, sizeof(ssd0303_state));
+    s->ds = ds;
+    s->i2c.event = ssd0303_event;
+    s->i2c.recv = ssd0303_recv;
+    s->i2c.send = ssd0303_send;
+    graphic_console_init(ds, ssd0303_update_display, ssd0303_invalidate_display,
+                         NULL, s);
+    dpy_resize(s->ds, 96 * MAGNIFY, 16 * MAGNIFY);
+}