commit 9ee6e8bb853bdea7ef6c645a1a07aa55fd206aba
Author: pbrook <pbrook@c046a42c-6fe2-441c-8c8c-71466251a162>
Date:   Sun Nov 11 00:04:49 2007 +0000

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

diff --git a/Changelog b/Changelog
index 1012400..75f39d6 100644
--- a/Changelog
+++ b/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:
 
diff --git a/Makefile.target b/Makefile.target
index ee91a57..25fed3f 100644
--- a/Makefile.target
+++ b/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
diff --git a/cpu-exec.c b/cpu-exec.c
index 4ba63fb..aa58cbb 100644
--- a/cpu-exec.c
+++ b/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);
                     }
diff --git a/fpu/softfloat-native.h b/fpu/softfloat-native.h
index 2977717..53bf681 100644
--- a/fpu/softfloat-native.h
+++ b/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
diff --git a/fpu/softfloat.c b/fpu/softfloat.c
index 6db6cf1..8ebb692 100644
--- a/fpu/softfloat.c
+++ b/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
diff --git a/fpu/softfloat.h b/fpu/softfloat.h
index f344d2e..f0261fb 100644
--- a/fpu/softfloat.h
+++ b/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)
 {
diff --git a/hw/arm_boot.c b/hw/arm_boot.c
index 7a99b41..8ef14ab 100644
--- a/hw/arm_boot.c
+++ b/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);
diff --git a/hw/arm_gic.c b/hw/arm_gic.c
index 8cd7182..774b79b 100644
--- a/hw/arm_gic.c
+++ b/hw/arm_gic.c
@@ -1,17 +1,15 @@
 /*
- * ARM AMBA Generic/Distributed Interrupt Controller
+ * ARM Generic/Distributed Interrupt Controller
  *
- * Copyright (c) 2006 CodeSourcery.
+ * Copyright (c) 2006-2007 CodeSourcery.
  * Written by Paul Brook
  *
  * This code is licenced under the GPL.
  */
 
-/* TODO: Some variants of this controller can handle multiple CPUs.
-   Currently only single CPU operation is implemented.  */
-
-#include "vl.h"
-#include "arm_pic.h"
+/* This file contains implementation code for the RealView EB interrupt
+   controller, MPCore distributed interrupt controller and ARMv7-M
+   Nested Vectored Interrupt Controller.  */
 
 //#define DEBUG_GIC
 
@@ -22,58 +20,84 @@ do { printf("arm_gic: " fmt , ##args); } while (0)
 #define DPRINTF(fmt, args...) do {} while(0)
 #endif
 
-/* Distributed interrupt controller.  */
-
+#ifdef NVIC
+static const uint8_t gic_id[] =
+{ 0x00, 0xb0, 0x1b, 0x00, 0x0d, 0xe0, 0x05, 0xb1 };
+#define GIC_DIST_OFFSET 0
+/* The NVIC has 16 internal vectors.  However these are not exposed
+   through the normal GIC interface.  */
+#define GIC_BASE_IRQ    32
+#else
 static const uint8_t gic_id[] =
 { 0x90, 0x13, 0x04, 0x00, 0x0d, 0xf0, 0x05, 0xb1 };
-
-#define GIC_NIRQ 96
+#define GIC_DIST_OFFSET 0x1000
+#define GIC_BASE_IRQ    0
+#endif
 
 typedef struct gic_irq_state
 {
+    /* ??? The documentation seems to imply the enable bits are global, even
+       for per-cpu interrupts.  This seems strange.  */
     unsigned enabled:1;
-    unsigned pending:1;
-    unsigned active:1;
+    unsigned pending:NCPU;
+    unsigned active:NCPU;
     unsigned level:1;
-    unsigned model:1; /* 0 = 1:N, 1 = N:N */
+    unsigned model:1; /* 0 = N:N, 1 = 1:N */
     unsigned trigger:1; /* nonzero = edge triggered.  */
 } gic_irq_state;
 
+#define ALL_CPU_MASK ((1 << NCPU) - 1)
+
 #define GIC_SET_ENABLED(irq) s->irq_state[irq].enabled = 1
 #define GIC_CLEAR_ENABLED(irq) s->irq_state[irq].enabled = 0
 #define GIC_TEST_ENABLED(irq) s->irq_state[irq].enabled
-#define GIC_SET_PENDING(irq) s->irq_state[irq].pending = 1
-#define GIC_CLEAR_PENDING(irq) s->irq_state[irq].pending = 0
-#define GIC_TEST_PENDING(irq) s->irq_state[irq].pending
-#define GIC_SET_ACTIVE(irq) s->irq_state[irq].active = 1
-#define GIC_CLEAR_ACTIVE(irq) s->irq_state[irq].active = 0
-#define GIC_TEST_ACTIVE(irq) s->irq_state[irq].active
+#define GIC_SET_PENDING(irq, cm) s->irq_state[irq].pending |= (cm)
+#define GIC_CLEAR_PENDING(irq, cm) s->irq_state[irq].pending &= ~(cm)
+#define GIC_TEST_PENDING(irq, cm) ((s->irq_state[irq].pending & (cm)) != 0)
+#define GIC_SET_ACTIVE(irq, cm) s->irq_state[irq].active |= (cm)
+#define GIC_CLEAR_ACTIVE(irq, cm) s->irq_state[irq].active &= ~(cm)
+#define GIC_TEST_ACTIVE(irq, cm) ((s->irq_state[irq].active & (cm)) != 0)
 #define GIC_SET_MODEL(irq) s->irq_state[irq].model = 1
 #define GIC_CLEAR_MODEL(irq) s->irq_state[irq].model = 0
 #define GIC_TEST_MODEL(irq) s->irq_state[irq].model
-#define GIC_SET_LEVEL(irq) s->irq_state[irq].level = 1
-#define GIC_CLEAR_LEVEL(irq) s->irq_state[irq].level = 0
-#define GIC_TEST_LEVEL(irq) s->irq_state[irq].level
+#define GIC_SET_LEVEL(irq, cm) s->irq_state[irq].level = (cm)
+#define GIC_CLEAR_LEVEL(irq, cm) s->irq_state[irq].level &= ~(cm)
+#define GIC_TEST_LEVEL(irq, cm) (s->irq_state[irq].level & (cm)) != 0
 #define GIC_SET_TRIGGER(irq) s->irq_state[irq].trigger = 1
 #define GIC_CLEAR_TRIGGER(irq) s->irq_state[irq].trigger = 0
 #define GIC_TEST_TRIGGER(irq) s->irq_state[irq].trigger
+#define GIC_GET_PRIORITY(irq, cpu) \
+  (((irq) < 32) ? s->priority1[irq][cpu] : s->priority2[(irq) - 32])
+#ifdef NVIC
+#define GIC_TARGET(irq) 1
+#else
+#define GIC_TARGET(irq) s->irq_target[irq]
+#endif
 
 typedef struct gic_state
 {
     uint32_t base;
-    qemu_irq parent_irq;
+    qemu_irq parent_irq[NCPU];
     int enabled;
-    int cpu_enabled;
+    int cpu_enabled[NCPU];
 
     gic_irq_state irq_state[GIC_NIRQ];
+#ifndef NVIC
     int irq_target[GIC_NIRQ];
-    int priority[GIC_NIRQ];
-    int last_active[GIC_NIRQ];
-
-    int priority_mask;
-    int running_irq;
-    int running_priority;
-    int current_pending;
+#endif
+    int priority1[32][NCPU];
+    int priority2[GIC_NIRQ - 32];
+    int last_active[GIC_NIRQ][NCPU];
+
+    int priority_mask[NCPU];
+    int running_irq[NCPU];
+    int running_priority[NCPU];
+    int current_pending[NCPU];
+
+    qemu_irq *in;
+#ifdef NVIC
+    void *nvic;
+#endif
 } gic_state;
 
 /* TODO: Many places that call this routine could be optimized.  */
@@ -83,112 +107,136 @@ static void gic_update(gic_state *s)
     int best_irq;
     int best_prio;
     int irq;
-
-    s->current_pending = 1023;
-    if (!s->enabled || !s->cpu_enabled) {
-        qemu_irq_lower(s->parent_irq);
-        return;
-    }
-    best_prio = 0x100;
-    best_irq = 1023;
-    for (irq = 0; irq < 96; irq++) {
-        if (GIC_TEST_ENABLED(irq) && GIC_TEST_PENDING(irq)) {
-            if (s->priority[irq] < best_prio) {
-                best_prio = s->priority[irq];
-                best_irq = irq;
+    int level;
+    int cpu;
+    int cm;
+
+    for (cpu = 0; cpu < NCPU; cpu++) {
+        cm = 1 << cpu;
+        s->current_pending[cpu] = 1023;
+        if (!s->enabled || !s->cpu_enabled[cpu]) {
+	    qemu_irq_lower(s->parent_irq[cpu]);
+            return;
+        }
+        best_prio = 0x100;
+        best_irq = 1023;
+        for (irq = 0; irq < GIC_NIRQ; irq++) {
+            if (GIC_TEST_ENABLED(irq) && GIC_TEST_PENDING(irq, cm)) {
+                if (GIC_GET_PRIORITY(irq, cpu) < best_prio) {
+                    best_prio = GIC_GET_PRIORITY(irq, cpu);
+                    best_irq = irq;
+                }
             }
         }
-    }
-    if (best_prio > s->priority_mask) {
-        qemu_irq_lower(s->parent_irq);
-    } else {
-        s->current_pending = best_irq;
-        if (best_prio < s->running_priority) {
-            DPRINTF("Raised pending IRQ %d\n", best_irq);
-            qemu_irq_raise(s->parent_irq);
+        level = 0;
+        if (best_prio <= s->priority_mask[cpu]) {
+            s->current_pending[cpu] = best_irq;
+            if (best_prio < s->running_priority[cpu]) {
+                DPRINTF("Raised pending IRQ %d\n", best_irq);
+                level = 1;
+            }
         }
+        qemu_set_irq(s->parent_irq[cpu], level);
     }
 }
 
+static void __attribute__((unused))
+gic_set_pending_private(gic_state *s, int cpu, int irq)
+{
+    int cm = 1 << cpu;
+
+    if (GIC_TEST_PENDING(irq, cm))
+        return;
+
+    DPRINTF("Set %d pending cpu %d\n", irq, cpu);
+    GIC_SET_PENDING(irq, cm);
+    gic_update(s);
+}
+
+/* Process a change in an external IRQ input.  */
 static void gic_set_irq(void *opaque, int irq, int level)
 {
     gic_state *s = (gic_state *)opaque;
     /* The first external input line is internal interrupt 32.  */
     irq += 32;
-    if (level == GIC_TEST_LEVEL(irq))
+    if (level == GIC_TEST_LEVEL(irq, ALL_CPU_MASK))
         return;
 
     if (level) {
-        GIC_SET_LEVEL(irq);
+        GIC_SET_LEVEL(irq, ALL_CPU_MASK);
         if (GIC_TEST_TRIGGER(irq) || GIC_TEST_ENABLED(irq)) {
-            DPRINTF("Set %d pending\n", irq);
-            GIC_SET_PENDING(irq);
+            DPRINTF("Set %d pending mask %x\n", irq, GIC_TARGET(irq));
+            GIC_SET_PENDING(irq, GIC_TARGET(irq));
         }
     } else {
-        GIC_CLEAR_LEVEL(irq);
+        GIC_CLEAR_LEVEL(irq, ALL_CPU_MASK);
     }
     gic_update(s);
 }
 
-static void gic_set_running_irq(gic_state *s, int irq)
+static void gic_set_running_irq(gic_state *s, int cpu, int irq)
 {
-    s->running_irq = irq;
-    if (irq == 1023)
-        s->running_priority = 0x100;
-    else
-        s->running_priority = s->priority[irq];
+    s->running_irq[cpu] = irq;
+    if (irq == 1023) {
+        s->running_priority[cpu] = 0x100;
+    } else {
+        s->running_priority[cpu] = GIC_GET_PRIORITY(irq, cpu);
+    }
     gic_update(s);
 }
 
-static uint32_t gic_acknowledge_irq(gic_state *s)
+static uint32_t gic_acknowledge_irq(gic_state *s, int cpu)
 {
     int new_irq;
-    new_irq = s->current_pending;
-    if (new_irq == 1023 || s->priority[new_irq] >= s->running_priority) {
+    int cm = 1 << cpu;
+    new_irq = s->current_pending[cpu];
+    if (new_irq == 1023
+            || GIC_GET_PRIORITY(new_irq, cpu) >= s->running_priority[cpu]) {
         DPRINTF("ACK no pending IRQ\n");
         return 1023;
     }
-    qemu_irq_lower(s->parent_irq);
-    s->last_active[new_irq] = s->running_irq;
-    /* For level triggered interrupts we clear the pending bit while
-       the interrupt is active.  */
-    GIC_CLEAR_PENDING(new_irq);
-    gic_set_running_irq(s, new_irq);
+    s->last_active[new_irq][cpu] = s->running_irq[cpu];
+    /* Clear pending flags for both level and edge triggered interrupts.
+       Level triggered IRQs will be reasserted once they become inactive.  */
+    GIC_CLEAR_PENDING(new_irq, GIC_TEST_MODEL(new_irq) ? ALL_CPU_MASK : cm);
+    gic_set_running_irq(s, cpu, new_irq);
     DPRINTF("ACK %d\n", new_irq);
     return new_irq;
 }
 
-static void gic_complete_irq(gic_state * s, int irq)
+static void gic_complete_irq(gic_state * s, int cpu, int irq)
 {
     int update = 0;
+    int cm = 1 << cpu;
     DPRINTF("EOI %d\n", irq);
-    if (s->running_irq == 1023)
+    if (s->running_irq[cpu] == 1023)
         return; /* No active IRQ.  */
     if (irq != 1023) {
         /* Mark level triggered interrupts as pending if they are still
            raised.  */
         if (!GIC_TEST_TRIGGER(irq) && GIC_TEST_ENABLED(irq)
-                && GIC_TEST_LEVEL(irq)) {
-            GIC_SET_PENDING(irq);
+                && GIC_TEST_LEVEL(irq, cm) && (GIC_TARGET(irq) & cm) != 0) {
+            DPRINTF("Set %d pending mask %x\n", irq, cm);
+            GIC_SET_PENDING(irq, cm);
             update = 1;
         }
     }
-    if (irq != s->running_irq) {
+    if (irq != s->running_irq[cpu]) {
         /* Complete an IRQ that is not currently running.  */
-        int tmp = s->running_irq;
-        while (s->last_active[tmp] != 1023) {
-            if (s->last_active[tmp] == irq) {
-                s->last_active[tmp] = s->last_active[irq];
+        int tmp = s->running_irq[cpu];
+        while (s->last_active[tmp][cpu] != 1023) {
+            if (s->last_active[tmp][cpu] == irq) {
+                s->last_active[tmp][cpu] = s->last_active[irq][cpu];
                 break;
             }
-            tmp = s->last_active[tmp];
+            tmp = s->last_active[tmp][cpu];
         }
         if (update) {
             gic_update(s);
         }
     } else {
         /* Complete the current running IRQ.  */
-        gic_set_running_irq(s, s->last_active[s->running_irq]);
+        gic_set_running_irq(s, cpu, s->last_active[s->running_irq[cpu]][cpu]);
     }
 }
 
@@ -198,15 +246,22 @@ static uint32_t gic_dist_readb(void *opaque, target_phys_addr_t offset)
     uint32_t res;
     int irq;
     int i;
+    int cpu;
+    int cm;
+    int mask;
 
-    offset -= s->base + 0x1000;
+    cpu = gic_get_current_cpu();
+    cm = 1 << cpu;
+    offset -= s->base + GIC_DIST_OFFSET;
     if (offset < 0x100) {
+#ifndef NVIC
         if (offset == 0)
             return s->enabled;
         if (offset == 4)
-            return (GIC_NIRQ / 32) - 1;
+            return ((GIC_NIRQ / 32) - 1) | ((NCPU - 1) << 5);
         if (offset < 0x08)
             return 0;
+#endif
         goto bad_reg;
     } else if (offset < 0x200) {
         /* Interrupt Set/Clear Enable.  */
@@ -214,6 +269,7 @@ static uint32_t gic_dist_readb(void *opaque, target_phys_addr_t offset)
             irq = (offset - 0x100) * 8;
         else
             irq = (offset - 0x180) * 8;
+        irq += GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
         res = 0;
@@ -228,40 +284,48 @@ static uint32_t gic_dist_readb(void *opaque, target_phys_addr_t offset)
             irq = (offset - 0x200) * 8;
         else
             irq = (offset - 0x280) * 8;
+        irq += GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
         res = 0;
+        mask = (irq < 32) ?  cm : ALL_CPU_MASK;
         for (i = 0; i < 8; i++) {
-            if (GIC_TEST_PENDING(irq + i)) {
+            if (GIC_TEST_PENDING(irq + i, mask)) {
                 res |= (1 << i);
             }
         }
     } else if (offset < 0x400) {
         /* Interrupt Active.  */
-        irq = (offset - 0x300) * 8;
+        irq = (offset - 0x300) * 8 + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
         res = 0;
+        mask = (irq < 32) ?  cm : ALL_CPU_MASK;
         for (i = 0; i < 8; i++) {
-            if (GIC_TEST_ACTIVE(irq + i)) {
+            if (GIC_TEST_ACTIVE(irq + i, mask)) {
                 res |= (1 << i);
             }
         }
     } else if (offset < 0x800) {
         /* Interrupt Priority.  */
-        irq = offset - 0x400;
+        irq = (offset - 0x400) + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
-        res = s->priority[irq];
+        res = GIC_GET_PRIORITY(irq, cpu);
+#ifndef NVIC
     } else if (offset < 0xc00) {
         /* Interrupt CPU Target.  */
-        irq = offset - 0x800;
+        irq = (offset - 0x800) + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
-        res = s->irq_target[irq];
+        if (irq >= 29 && irq <= 31) {
+            res = cm;
+        } else {
+            res = GIC_TARGET(irq);
+        }
     } else if (offset < 0xf00) {
         /* Interrupt Configuration.  */
-        irq = (offset - 0xc00) * 2;
+        irq = (offset - 0xc00) * 2 + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
         res = 0;
@@ -271,6 +335,7 @@ static uint32_t gic_dist_readb(void *opaque, target_phys_addr_t offset)
             if (GIC_TEST_TRIGGER(irq + i))
                 res |= (2 << (i * 2));
         }
+#endif
     } else if (offset < 0xfe0) {
         goto bad_reg;
     } else /* offset >= 0xfe0 */ {
@@ -282,7 +347,7 @@ static uint32_t gic_dist_readb(void *opaque, target_phys_addr_t offset)
     }
     return res;
 bad_reg:
-    cpu_abort (cpu_single_env, "gic_dist_readb: Bad offset %x\n", offset);
+    cpu_abort(cpu_single_env, "gic_dist_readb: Bad offset %x\n", (int)offset);
     return 0;
 }
 
@@ -297,6 +362,13 @@ static uint32_t gic_dist_readw(void *opaque, target_phys_addr_t offset)
 static uint32_t gic_dist_readl(void *opaque, target_phys_addr_t offset)
 {
     uint32_t val;
+#ifdef NVIC
+    gic_state *s = (gic_state *)opaque;
+    uint32_t addr;
+    addr = offset - s->base;
+    if (addr < 0x100 || addr > 0xd00)
+        return nvic_readl(s->nvic, addr);
+#endif
     val = gic_dist_readw(opaque, offset);
     val |= gic_dist_readw(opaque, offset + 2) << 16;
     return val;
@@ -308,9 +380,14 @@ static void gic_dist_writeb(void *opaque, target_phys_addr_t offset,
     gic_state *s = (gic_state *)opaque;
     int irq;
     int i;
+    int cpu;
 
-    offset -= s->base + 0x1000;
+    cpu = gic_get_current_cpu();
+    offset -= s->base + GIC_DIST_OFFSET;
     if (offset < 0x100) {
+#ifdef NVIC
+        goto bad_reg;
+#else
         if (offset == 0) {
             s->enabled = (value & 1);
             DPRINTF("Distribution %sabled\n", s->enabled ? "En" : "Dis");
@@ -319,27 +396,36 @@ static void gic_dist_writeb(void *opaque, target_phys_addr_t offset,
         } else {
             goto bad_reg;
         }
+#endif
     } else if (offset < 0x180) {
         /* Interrupt Set Enable.  */
-        irq = (offset - 0x100) * 8;
+        irq = (offset - 0x100) * 8 + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
+        if (irq < 16)
+          value = 0xff;
         for (i = 0; i < 8; i++) {
             if (value & (1 << i)) {
+                int mask = (irq < 32) ? (1 << cpu) : GIC_TARGET(irq);
                 if (!GIC_TEST_ENABLED(irq + i))
                     DPRINTF("Enabled IRQ %d\n", irq + i);
                 GIC_SET_ENABLED(irq + i);
                 /* If a raised level triggered IRQ enabled then mark
                    is as pending.  */
-                if (GIC_TEST_LEVEL(irq + i) && !GIC_TEST_TRIGGER(irq + i))
-                    GIC_SET_PENDING(irq + i);
+                if (GIC_TEST_LEVEL(irq + i, mask)
+                        && !GIC_TEST_TRIGGER(irq + i)) {
+                    DPRINTF("Set %d pending mask %x\n", irq + i, mask);
+                    GIC_SET_PENDING(irq + i, mask);
+                }
             }
         }
     } else if (offset < 0x200) {
         /* Interrupt Clear Enable.  */
-        irq = (offset - 0x180) * 8;
+        irq = (offset - 0x180) * 8 + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
+        if (irq < 16)
+          value = 0;
         for (i = 0; i < 8; i++) {
             if (value & (1 << i)) {
                 if (GIC_TEST_ENABLED(irq + i))
@@ -349,22 +435,28 @@ static void gic_dist_writeb(void *opaque, target_phys_addr_t offset,
         }
     } else if (offset < 0x280) {
         /* Interrupt Set Pending.  */
-        irq = (offset - 0x200) * 8;
+        irq = (offset - 0x200) * 8 + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
+        if (irq < 16)
+          irq = 0;
+
         for (i = 0; i < 8; i++) {
             if (value & (1 << i)) {
-                GIC_SET_PENDING(irq + i);
+                GIC_SET_PENDING(irq + i, GIC_TARGET(irq));
             }
         }
     } else if (offset < 0x300) {
         /* Interrupt Clear Pending.  */
-        irq = (offset - 0x280) * 8;
+        irq = (offset - 0x280) * 8 + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
         for (i = 0; i < 8; i++) {
+            /* ??? This currently clears the pending bit for all CPUs, even
+               for per-CPU interrupts.  It's unclear whether this is the
+               corect behavior.  */
             if (value & (1 << i)) {
-                GIC_CLEAR_PENDING(irq + i);
+                GIC_CLEAR_PENDING(irq + i, ALL_CPU_MASK);
             }
         }
     } else if (offset < 0x400) {
@@ -372,21 +464,32 @@ static void gic_dist_writeb(void *opaque, target_phys_addr_t offset,
         goto bad_reg;
     } else if (offset < 0x800) {
         /* Interrupt Priority.  */
-        irq = offset - 0x400;
+        irq = (offset - 0x400) + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
-        s->priority[irq] = value;
+        if (irq < 32) {
+            s->priority1[irq][cpu] = value;
+        } else {
+            s->priority2[irq - 32] = value;
+        }
+#ifndef NVIC
     } else if (offset < 0xc00) {
         /* Interrupt CPU Target.  */
-        irq = offset - 0x800;
+        irq = (offset - 0x800) + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
-        s->irq_target[irq] = value;
+        if (irq < 29)
+            value = 0;
+        else if (irq < 32)
+            value = ALL_CPU_MASK;
+        s->irq_target[irq] = value & ALL_CPU_MASK;
     } else if (offset < 0xf00) {
         /* Interrupt Configuration.  */
-        irq = (offset - 0xc00) * 4;
+        irq = (offset - 0xc00) * 4 + GIC_BASE_IRQ;
         if (irq >= GIC_NIRQ)
             goto bad_reg;
+        if (irq < 32)
+            value |= 0xaa;
         for (i = 0; i < 4; i++) {
             if (value & (1 << (i * 2))) {
                 GIC_SET_MODEL(irq + i);
@@ -399,25 +502,20 @@ static void gic_dist_writeb(void *opaque, target_phys_addr_t offset,
                 GIC_CLEAR_TRIGGER(irq + i);
             }
         }
+#endif
     } else {
-        /* 0xf00 is only handled for word writes.  */
+        /* 0xf00 is only handled for 32-bit writes.  */
         goto bad_reg;
     }
     gic_update(s);
     return;
 bad_reg:
-    cpu_abort (cpu_single_env, "gic_dist_writeb: Bad offset %x\n", offset);
+    cpu_abort(cpu_single_env, "gic_dist_writeb: Bad offset %x\n", (int)offset);
 }
 
 static void gic_dist_writew(void *opaque, target_phys_addr_t offset,
                             uint32_t value)
 {
-    gic_state *s = (gic_state *)opaque;
-    if (offset - s->base == 0xf00) {
-        GIC_SET_PENDING(value & 0x3ff);
-        gic_update(s);
-        return;
-    }
     gic_dist_writeb(opaque, offset, value & 0xff);
     gic_dist_writeb(opaque, offset + 1, value >> 8);
 }
@@ -425,6 +523,41 @@ static void gic_dist_writew(void *opaque, target_phys_addr_t offset,
 static void gic_dist_writel(void *opaque, target_phys_addr_t offset,
                             uint32_t value)
 {
+    gic_state *s = (gic_state *)opaque;
+#ifdef NVIC
+    uint32_t addr;
+    addr = offset - s->base;
+    if (addr < 0x100 || (addr > 0xd00 && addr != 0xf00)) {
+        nvic_writel(s->nvic, addr, value);
+        return;
+    }
+#endif
+    if (offset - s->base == GIC_DIST_OFFSET + 0xf00) {
+        int cpu;
+        int irq;
+        int mask;
+
+        cpu = gic_get_current_cpu();
+        irq = value & 0x3ff;
+        switch ((value >> 24) & 3) {
+        case 0:
+            mask = (value >> 16) & ALL_CPU_MASK;
+            break;
+        case 1:
+            mask = 1 << cpu;
+            break;
+        case 2:
+            mask = ALL_CPU_MASK ^ (1 << cpu);
+            break;
+        default:
+            DPRINTF("Bad Soft Int target filter\n");
+            mask = ALL_CPU_MASK;
+            break;
+        }
+        GIC_SET_PENDING(irq, mask);
+        gic_update(s);
+        return;
+    }
     gic_dist_writew(opaque, offset, value & 0xffff);
     gic_dist_writew(opaque, offset + 2, value >> 16);
 }
@@ -441,105 +574,100 @@ static CPUWriteMemoryFunc *gic_dist_writefn[] = {
    gic_dist_writel
 };
 
-static uint32_t gic_cpu_read(void *opaque, target_phys_addr_t offset)
+#ifndef NVIC
+static uint32_t gic_cpu_read(gic_state *s, int cpu, int offset)
 {
-    gic_state *s = (gic_state *)opaque;
-    offset -= s->base;
     switch (offset) {
     case 0x00: /* Control */
-        return s->cpu_enabled;
+        return s->cpu_enabled[cpu];
     case 0x04: /* Priority mask */
-        return s->priority_mask;
+        return s->priority_mask[cpu];
     case 0x08: /* Binary Point */
         /* ??? Not implemented.  */
         return 0;
     case 0x0c: /* Acknowledge */
-        return gic_acknowledge_irq(s);
+        return gic_acknowledge_irq(s, cpu);
     case 0x14: /* Runing Priority */
-        return s->running_priority;
+        return s->running_priority[cpu];
     case 0x18: /* Highest Pending Interrupt */
-        return s->current_pending;
+        return s->current_pending[cpu];
     default:
-        cpu_abort (cpu_single_env, "gic_cpu_read: Bad offset %x\n", offset);
+        cpu_abort(cpu_single_env, "gic_cpu_read: Bad offset %x\n",
+                  (int)offset);
         return 0;
     }
 }
 
-static void gic_cpu_write(void *opaque, target_phys_addr_t offset,
-                          uint32_t value)
+static void gic_cpu_write(gic_state *s, int cpu, int offset, uint32_t value)
 {
-    gic_state *s = (gic_state *)opaque;
-    offset -= s->base;
     switch (offset) {
     case 0x00: /* Control */
-        s->cpu_enabled = (value & 1);
+        s->cpu_enabled[cpu] = (value & 1);
         DPRINTF("CPU %sabled\n", s->cpu_enabled ? "En" : "Dis");
         break;
     case 0x04: /* Priority mask */
-        s->priority_mask = (value & 0x3ff);
+        s->priority_mask[cpu] = (value & 0xff);
         break;
     case 0x08: /* Binary Point */
         /* ??? Not implemented.  */
         break;
     case 0x10: /* End Of Interrupt */
-        return gic_complete_irq(s, value & 0x3ff);
+        return gic_complete_irq(s, cpu, value & 0x3ff);
     default:
-        cpu_abort (cpu_single_env, "gic_cpu_write: Bad offset %x\n", offset);
+        cpu_abort(cpu_single_env, "gic_cpu_write: Bad offset %x\n",
+                  (int)offset);
         return;
     }
     gic_update(s);
 }
-
-static CPUReadMemoryFunc *gic_cpu_readfn[] = {
-   gic_cpu_read,
-   gic_cpu_read,
-   gic_cpu_read
-};
-
-static CPUWriteMemoryFunc *gic_cpu_writefn[] = {
-   gic_cpu_write,
-   gic_cpu_write,
-   gic_cpu_write
-};
+#endif
 
 static void gic_reset(gic_state *s)
 {
     int i;
     memset(s->irq_state, 0, GIC_NIRQ * sizeof(gic_irq_state));
-    s->priority_mask = 0xf0;
-    s->current_pending = 1023;
-    s->running_irq = 1023;
-    s->running_priority = 0x100;
+    for (i = 0 ; i < NCPU; i++) {
+        s->priority_mask[i] = 0xf0;
+        s->current_pending[i] = 1023;
+        s->running_irq[i] = 1023;
+        s->running_priority[i] = 0x100;
+#ifdef NVIC
+        /* The NVIC doesn't have per-cpu interfaces, so enable by default.  */
+        s->cpu_enabled[i] = 1;
+#else
+        s->cpu_enabled[i] = 0;
+#endif
+    }
     for (i = 0; i < 15; i++) {
         GIC_SET_ENABLED(i);
         GIC_SET_TRIGGER(i);
     }
+#ifdef NVIC
+    /* The NVIC is always enabled.  */
+    s->enabled = 1;
+#else
     s->enabled = 0;
-    s->cpu_enabled = 0;
+#endif
 }
 
-qemu_irq *arm_gic_init(uint32_t base, qemu_irq parent_irq)
+static gic_state *gic_init(uint32_t base, qemu_irq *parent_irq)
 {
     gic_state *s;
-    qemu_irq *qi;
     int iomemtype;
+    int i;
 
     s = (gic_state *)qemu_mallocz(sizeof(gic_state));
     if (!s)
         return NULL;
-    qi = qemu_allocate_irqs(gic_set_irq, s, GIC_NIRQ);
-    s->parent_irq = parent_irq;
-    if (base != 0xffffffff) {
-        iomemtype = cpu_register_io_memory(0, gic_cpu_readfn,
-                                           gic_cpu_writefn, s);
-        cpu_register_physical_memory(base, 0x00001000, iomemtype);
-        iomemtype = cpu_register_io_memory(0, gic_dist_readfn,
-                                           gic_dist_writefn, s);
-        cpu_register_physical_memory(base + 0x1000, 0x00001000, iomemtype);
-        s->base = base;
-    } else {
-        s->base = 0;
+    s->in = qemu_allocate_irqs(gic_set_irq, s, GIC_NIRQ);
+    for (i = 0; i < NCPU; i++) {
+        s->parent_irq[i] = parent_irq[i];
     }
+    iomemtype = cpu_register_io_memory(0, gic_dist_readfn,
+                                       gic_dist_writefn, s);
+    cpu_register_physical_memory(base + GIC_DIST_OFFSET, 0x00001000,
+                                 iomemtype);
+    s->base = base;
     gic_reset(s);
-    return qi;
+    return s;
 }
diff --git a/hw/arm_sysctl.c b/hw/arm_sysctl.c
index 468a494..e3179e2 100644
--- a/hw/arm_sysctl.c
+++ b/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);
diff --git a/hw/armv7m.c b/hw/armv7m.c
new file mode 100644
index 0000000..f0a90e1
--- /dev/null
+++ b/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;
+}
+
diff --git a/hw/armv7m_nvic.c b/hw/armv7m_nvic.c
new file mode 100644
index 0000000..d304082
--- /dev/null
+++ b/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;
+}
diff --git a/hw/integratorcp.c b/hw/integratorcp.c
index 75315a8..31e7d7d 100644
--- a/hw/integratorcp.c
+++ b/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(0x17000000, pic[2], serial_hds[1]);
+    pl011_init(0x16000000, pic[1], serial_hds[0], PL011_ARM);
+    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);
diff --git a/hw/mpcore.c b/hw/mpcore.c
new file mode 100644
index 0000000..cc33208
--- /dev/null
+++ b/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);
+}
diff --git a/hw/pl011.c b/hw/pl011.c
index df33491..9037554 100644
--- a/hw/pl011.c
+++ b/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[] =
-{ 0x11, 0x10, 0x14, 0x00, 0x0d, 0xf0, 0x05, 0xb1 };
+static const unsigned char pl011_id[2][8] = {
+  { 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;
diff --git a/hw/pl022.c b/hw/pl022.c
new file mode 100644
index 0000000..d7c735b
--- /dev/null
+++ b/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.  */
+}
+
+
diff --git a/hw/pl061.c b/hw/pl061.c
new file mode 100644
index 0000000..fa5004a
--- /dev/null
+++ b/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);
+}
+
diff --git a/hw/pxa2xx.c b/hw/pxa2xx.c
index ebaff13..3c10839 100644
--- a/hw/pxa2xx.c
+++ b/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,
diff --git a/hw/realview.c b/hw/realview.c
index f97d6e6..e02deee 100644
--- a/hw/realview.c
+++ b/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);
-    if (!env) {
-        fprintf(stderr, "Unable to find CPU definition\n");
-        exit(1);
+    /* FIXME: obey smp_cpus.  */
+    if (strcmp(cpu_model, "arm11mpcore") == 0) {
+        ncpu = 4;
+    } 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
-       is nIRQ (there are inconsistencies).  However Linux 2.6.17 expects
-       GIC1 to be nIRQ and ignores all the others, so do that for now.  */
-    pic = arm_gic_init(0x10040000, pic[ARM_PIC_CPU_IRQ]);
+
+    if (ncpu == 1) {
+        /* ??? The documentation says GIC1 is nFIQ and either GIC2 or GIC3
+           is nIRQ (there are inconsistencies).  However Linux 2.6.17 expects
+           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(0x1000a000, pic[13], serial_hds[1]);
-    pl011_init(0x1000b000, pic[14], serial_hds[2]);
-    pl011_init(0x1000c000, pic[15], serial_hds[3]);
+    pl011_init(0x10009000, pic[12], serial_hds[0], PL011_ARM);
+    pl011_init(0x1000a000, pic[13], serial_hds[1], PL011_ARM);
+    pl011_init(0x1000b000, pic[14], serial_hds[2], PL011_ARM);
+    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).  */
-    /* 0x10050000 GIC2 (IRQ1).  */
-    /*  0x10060000 GIC3 (FIQ2).  */
-    /*  0x10070000 GIC4 (IRQ2).  */
+    /* 0x10040000 GIC1.  */
+    /* 0x10050000 GIC2.  */
+    /* 0x10060000 GIC3.  */
+    /* 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 = {
diff --git a/hw/realview_gic.c b/hw/realview_gic.c
new file mode 100644
index 0000000..cbc9614
--- /dev/null
+++ b/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;
+}
diff --git a/hw/ssd0303.c b/hw/ssd0303.c
new file mode 100644
index 0000000..138cfc7
--- /dev/null
+++ b/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);
+}
diff --git a/hw/ssd0323.c b/hw/ssd0323.c
new file mode 100644
index 0000000..67361bc
--- /dev/null
+++ b/hw/ssd0323.c
@@ -0,0 +1,267 @@
+/*
+ * SSD0323 OLED controller with OSRAM Pictiva 128x64 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_SSD0323 1
+
+#ifdef DEBUG_SSD0323
+#define DPRINTF(fmt, args...) \
+do { printf("ssd0323: " fmt , ##args); } while (0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "ssd0323: error: " fmt , ##args); exit(1);} while (0)
+#else
+#define DPRINTF(fmt, args...) do {} while(0)
+#define BADF(fmt, args...) \
+do { fprintf(stderr, "ssd0323: error: " fmt , ##args);} while (0)
+#endif
+
+/* Scaling factor for pixels.  */
+#define MAGNIFY 4
+
+enum ssd0323_mode
+{
+    SSD0323_CMD,
+    SSD0323_DATA
+};
+
+typedef struct {
+    DisplayState *ds;
+
+    int cmd_len;
+    int cmd;
+    int cmd_data[8];
+    int row;
+    int row_start;
+    int row_end;
+    int col;
+    int col_start;
+    int col_end;
+    int redraw;
+    enum ssd0323_mode mode;
+    uint8_t framebuffer[128 * 80 / 2];
+} ssd0323_state;
+
+int ssd0323_xfer_ssi(void *opaque, int data)
+{
+    ssd0323_state *s = (ssd0323_state *)opaque;
+    switch (s->mode) {
+    case SSD0323_DATA:
+        DPRINTF("data 0x%02x\n", data);
+        s->framebuffer[s->col + s->row * 64] = data;
+        s->col++;
+        if (s->col > s->col_end) {
+            s->row++;
+            s->col = s->col_start;
+        }
+        if (s->row > s->row_end) {
+            s->row = s->row_start;
+        }
+        s->redraw = 1;
+        break;
+    case SSD0323_CMD:
+        DPRINTF("cmd 0x%02x\n", data);
+        if (s->cmd_len == 0) {
+            s->cmd = data;
+        } else {
+            s->cmd_data[s->cmd_len - 1] = data;
+        }
+        s->cmd_len++;
+        switch (s->cmd) {
+#define DATA(x) if (s->cmd_len <= (x)) return 0
+        case 0x15: /* Set column.  */
+            DATA(2);
+            s->col_start = s->cmd_data[0] % 64;
+            s->col_end = s->cmd_data[1] % 64;
+            break;
+        case 0x75: /* Set row.  */
+            DATA(2);
+            s->row_start = s->cmd_data[0] % 80;
+            s->row_end = s->cmd_data[1] % 80;
+            break;
+        case 0x81: /* Set contrast */
+            DATA(1);
+            break;
+        case 0x84: case 0x85: case 0x86: /* Max current.  */
+            DATA(0);
+            break;
+        case 0xa0: /* Set remapping.  */
+            /* FIXME: Implement this.  */
+            DATA(1);
+            break;
+        case 0xa1: /* Set display start line.  */
+        case 0xa2: /* Set display offset.  */
+            /* FIXME: Implement these.  */
+            DATA(1);
+            break;
+        case 0xa4: /* Normal mode.  */
+        case 0xa5: /* All on.  */
+        case 0xa6: /* All off.  */
+        case 0xa7: /* Inverse.  */
+            /* FIXME: Implement these.  */
+            DATA(0);
+            break;
+        case 0xa8: /* Set multiplex ratio.  */
+        case 0xad: /* Set DC-DC converter.  */
+            DATA(1);
+            /* Ignored.  Don't care.  */
+            break;
+        case 0xae: /* Display off.  */
+        case 0xaf: /* Display on.  */
+            DATA(0);
+            /* TODO: Implement power control.  */
+            break;
+        case 0xb1: /* Set phase length.  */
+        case 0xb2: /* Set row period.  */
+        case 0xb3: /* Set clock rate.  */
+        case 0xbc: /* Set precharge.  */
+        case 0xbe: /* Set VCOMH.  */
+        case 0xbf: /* Set segment low.  */
+            DATA(1);
+            /* Ignored.  Don't care.  */
+            break;
+        case 0xb8: /* Set grey scale table.  */
+            /* FIXME: Implement this.  */
+            DATA(8);
+            break;
+        case 0xe3: /* NOP.  */
+            DATA(0);
+            break;
+        default:
+            BADF("Unknown command: 0x%x\n", data);
+        }
+        s->cmd_len = 0;
+        return 0;
+    }
+    return 0;
+}
+
+static void ssd0323_update_display(void *opaque)
+{
+    ssd0323_state *s = (ssd0323_state *)opaque;
+    uint8_t *dest;
+    uint8_t *src;
+    int x;
+    int y;
+    int i;
+    int line;
+    char *colors[16];
+    char colortab[MAGNIFY * 64];
+    char *p;
+    int dest_width;
+
+    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;
+        }
+        p = colortab;
+        for (i = 0; i < 16; i++) {
+            int n;
+            colors[i] = p;
+            switch (s->ds->depth) {
+            case 15:
+                n = i * 2 + (i >> 3);
+                p[0] = n | (n << 5);
+                p[1] = (n << 2) | (n >> 3);
+                break;
+            case 16:
+                n = i * 2 + (i >> 3);
+                p[0] = n | (n << 6) | ((n << 1) & 0x20);
+                p[1] = (n << 3) | (n >> 2);
+                break;
+            case 24:
+            case 32:
+                n = (i << 4) | i;
+                p[0] = p[1] = p[2] = n;
+                break;
+            default:
+                BADF("Bad color depth\n");
+                return;
+            }
+            p += dest_width;
+        }
+        dest = s->ds->data;
+        for (y = 0; y < 64; y++) {
+            line = y;
+            src = s->framebuffer + 64 * line;
+            for (x = 0; x < 64; x++) {
+                int val;
+                val = *src >> 4;
+                for (i = 0; i < MAGNIFY; i++) {
+                    memcpy(dest, colors[val], dest_width);
+                    dest += dest_width;
+                }
+                val = *src & 0xf;
+                for (i = 0; i < MAGNIFY; i++) {
+                    memcpy(dest, colors[val], dest_width);
+                    dest += dest_width;
+                }
+                src++;
+            }
+            for (i = 1; i < MAGNIFY; i++) {
+                memcpy(dest, dest - dest_width * MAGNIFY * 128,
+                       dest_width * 128 * MAGNIFY);
+                dest += dest_width * 128 * MAGNIFY;
+            }
+        }
+    }
+    dpy_update(s->ds, 0, 0, 128 * MAGNIFY, 64 * MAGNIFY);
+}
+
+static void ssd0323_invalidate_display(void * opaque)
+{
+    ssd0323_state *s = (ssd0323_state *)opaque;
+    s->redraw = 1;
+}
+
+/* Command/data input.  */
+static void ssd0323_cd(void *opaque, int n, int level)
+{
+    ssd0323_state *s = (ssd0323_state *)opaque;
+    DPRINTF("%s mode\n", level ? "Data" : "Command");
+    s->mode = level ? SSD0323_DATA : SSD0323_CMD;
+}
+
+void *ssd0323_init(DisplayState *ds, qemu_irq *cmd_p)
+{
+    ssd0323_state *s;
+    qemu_irq *cmd;
+
+    s = (ssd0323_state *)qemu_mallocz(sizeof(ssd0323_state));
+    s->ds = ds;
+    graphic_console_init(ds, ssd0323_update_display, ssd0323_invalidate_display,
+                         NULL, s);
+    dpy_resize(s->ds, 128 * MAGNIFY, 64 * MAGNIFY);
+    s->col_end = 63;
+    s->row_end = 79;
+
+    cmd = qemu_allocate_irqs(ssd0323_cd, s, 1);
+    *cmd_p = *cmd;
+
+    return s;
+}
diff --git a/hw/stellaris.c b/hw/stellaris.c
new file mode 100644
index 0000000..62f2c03
--- /dev/null
+++ b/hw/stellaris.c
@@ -0,0 +1,1101 @@
+/*
+ * Luminary Micro Stellaris preipherals
+ *
+ * Copyright (c) 2006 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+#include "vl.h"
+#include "arm_pic.h"
+
+typedef const struct {
+    const char *name;
+    uint32_t did0;
+    uint32_t did1;
+    uint32_t dc0;
+    uint32_t dc1;
+    uint32_t dc2;
+    uint32_t dc3;
+    uint32_t dc4;
+    enum {OLED_I2C, OLED_SSI} oled;
+} stellaris_board_info;
+
+/* General purpose timer module.  */
+
+/* Multiplication factor to convert from GPTM timer ticks to qemu timer
+   ticks.  */
+static int stellaris_clock_scale;
+
+typedef struct gptm_state {
+    uint32_t config;
+    uint32_t mode[2];
+    uint32_t control;
+    uint32_t state;
+    uint32_t mask;
+    uint32_t load[2];
+    uint32_t match[2];
+    uint32_t prescale[2];
+    uint32_t match_prescale[2];
+    uint32_t rtc;
+    int64_t tick[2];
+    struct gptm_state *opaque[2];
+    uint32_t base;
+    QEMUTimer *timer[2];
+    /* The timers have an alternate output used to trigger the ADC.  */
+    qemu_irq trigger;
+    qemu_irq irq;
+} gptm_state;
+
+static void gptm_update_irq(gptm_state *s)
+{
+    int level;
+    level = (s->state & s->mask) != 0;
+    qemu_set_irq(s->irq, level);
+}
+
+static void gptm_stop(gptm_state *s, int n)
+{
+    qemu_del_timer(s->timer[n]);
+}
+
+static void gptm_reload(gptm_state *s, int n, int reset)
+{
+    int64_t tick;
+    if (reset)
+        tick = qemu_get_clock(vm_clock);
+    else
+        tick = s->tick[n];
+
+    if (s->config == 0) {
+        /* 32-bit CountDown.  */
+        uint32_t count;
+        count = s->load[0] | (s->load[1] << 16);
+        tick += (int64_t)count * stellaris_clock_scale;
+    } else if (s->config == 1) {
+        /* 32-bit RTC.  1Hz tick.  */
+        tick += ticks_per_sec;
+    } else if (s->mode[n] == 0xa) {
+        /* PWM mode.  Not implemented.  */
+    } else {
+        cpu_abort(cpu_single_env, "TODO: 16-bit timer mode 0x%x\n",
+                  s->mode[n]);
+    }
+    s->tick[n] = tick;
+    qemu_mod_timer(s->timer[n], tick);
+}
+
+static void gptm_tick(void *opaque)
+{
+    gptm_state **p = (gptm_state **)opaque;
+    gptm_state *s;
+    int n;
+
+    s = *p;
+    n = p - s->opaque;
+    if (s->config == 0) {
+        s->state |= 1;
+        if ((s->control & 0x20)) {
+            /* Output trigger.  */
+	    qemu_irq_raise(s->trigger);
+	    qemu_irq_lower(s->trigger);
+        }
+        if (s->mode[0] & 1) {
+            /* One-shot.  */
+            s->control &= ~1;
+        } else {
+            /* Periodic.  */
+            gptm_reload(s, 0, 0);
+        }
+    } else if (s->config == 1) {
+        /* RTC.  */
+        uint32_t match;
+        s->rtc++;
+        match = s->match[0] | (s->match[1] << 16);
+        if (s->rtc > match)
+            s->rtc = 0;
+        if (s->rtc == 0) {
+            s->state |= 8;
+        }
+        gptm_reload(s, 0, 0);
+    } else if (s->mode[n] == 0xa) {
+        /* PWM mode.  Not implemented.  */
+    } else {
+        cpu_abort(cpu_single_env, "TODO: 16-bit timer mode 0x%x\n",
+                  s->mode[n]);
+    }
+    gptm_update_irq(s);
+}
+
+static uint32_t gptm_read(void *opaque, target_phys_addr_t offset)
+{
+    gptm_state *s = (gptm_state *)opaque;
+
+    offset -= s->base;
+    switch (offset) {
+    case 0x00: /* CFG */
+        return s->config;
+    case 0x04: /* TAMR */
+        return s->mode[0];
+    case 0x08: /* TBMR */
+        return s->mode[1];
+    case 0x0c: /* CTL */
+        return s->control;
+    case 0x18: /* IMR */
+        return s->mask;
+    case 0x1c: /* RIS */
+        return s->state;
+    case 0x20: /* MIS */
+        return s->state & s->mask;
+    case 0x24: /* CR */
+        return 0;
+    case 0x28: /* TAILR */
+        return s->load[0] | ((s->config < 4) ? (s->load[1] << 16) : 0);
+    case 0x2c: /* TBILR */
+        return s->load[1];
+    case 0x30: /* TAMARCHR */
+        return s->match[0] | ((s->config < 4) ? (s->match[1] << 16) : 0);
+    case 0x34: /* TBMATCHR */
+        return s->match[1];
+    case 0x38: /* TAPR */
+        return s->prescale[0];
+    case 0x3c: /* TBPR */
+        return s->prescale[1];
+    case 0x40: /* TAPMR */
+        return s->match_prescale[0];
+    case 0x44: /* TBPMR */
+        return s->match_prescale[1];
+    case 0x48: /* TAR */
+        if (s->control == 1)
+            return s->rtc;
+    case 0x4c: /* TBR */
+        cpu_abort(cpu_single_env, "TODO: Timer value read\n");
+    default:
+        cpu_abort(cpu_single_env, "gptm_read: Bad offset 0x%x\n", (int)offset);
+        return 0;
+    }
+}
+
+static void gptm_write(void *opaque, target_phys_addr_t offset, uint32_t value)
+{
+    gptm_state *s = (gptm_state *)opaque;
+    uint32_t oldval;
+
+    offset -= s->base;
+    /* The timers should be disabled before changing the configuration.
+       We take advantage of this and defer everything until the timer
+       is enabled.  */
+    switch (offset) {
+    case 0x00: /* CFG */
+        s->config = value;
+        break;
+    case 0x04: /* TAMR */
+        s->mode[0] = value;
+        break;
+    case 0x08: /* TBMR */
+        s->mode[1] = value;
+        break;
+    case 0x0c: /* CTL */
+        oldval = s->control;
+        s->control = value;
+        /* TODO: Implement pause.  */
+        if ((oldval ^ value) & 1) {
+            if (value & 1) {
+                gptm_reload(s, 0, 1);
+            } else {
+                gptm_stop(s, 0);
+            }
+        }
+        if (((oldval ^ value) & 0x100) && s->config >= 4) {
+            if (value & 0x100) {
+                gptm_reload(s, 1, 1);
+            } else {
+                gptm_stop(s, 1);
+            }
+        }
+        break;
+    case 0x18: /* IMR */
+        s->mask = value & 0x77;
+        gptm_update_irq(s);
+        break;
+    case 0x24: /* CR */
+        s->state &= ~value;
+        break;
+    case 0x28: /* TAILR */
+        s->load[0] = value & 0xffff;
+        if (s->config < 4) {
+            s->load[1] = value >> 16;
+        }
+        break;
+    case 0x2c: /* TBILR */
+        s->load[1] = value & 0xffff;
+        break;
+    case 0x30: /* TAMARCHR */
+        s->match[0] = value & 0xffff;
+        if (s->config < 4) {
+            s->match[1] = value >> 16;
+        }
+        break;
+    case 0x34: /* TBMATCHR */
+        s->match[1] = value >> 16;
+        break;
+    case 0x38: /* TAPR */
+        s->prescale[0] = value;
+        break;
+    case 0x3c: /* TBPR */
+        s->prescale[1] = value;
+        break;
+    case 0x40: /* TAPMR */
+        s->match_prescale[0] = value;
+        break;
+    case 0x44: /* TBPMR */
+        s->match_prescale[0] = value;
+        break;
+    default:
+        cpu_abort(cpu_single_env, "gptm_write: Bad offset 0x%x\n", (int)offset);
+    }
+    gptm_update_irq(s);
+}
+
+static CPUReadMemoryFunc *gptm_readfn[] = {
+   gptm_read,
+   gptm_read,
+   gptm_read
+};
+
+static CPUWriteMemoryFunc *gptm_writefn[] = {
+   gptm_write,
+   gptm_write,
+   gptm_write
+};
+
+static void stellaris_gptm_init(uint32_t base, qemu_irq irq, qemu_irq trigger)
+{
+    int iomemtype;
+    gptm_state *s;
+
+    s = (gptm_state *)qemu_mallocz(sizeof(gptm_state));
+    s->base = base;
+    s->irq = irq;
+    s->trigger = trigger;
+    s->opaque[0] = s->opaque[1] = s;
+
+    iomemtype = cpu_register_io_memory(0, gptm_readfn,
+                                       gptm_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    s->timer[0] = qemu_new_timer(vm_clock, gptm_tick, &s->opaque[0]);
+    s->timer[1] = qemu_new_timer(vm_clock, gptm_tick, &s->opaque[1]);
+    /* ??? Save/restore.  */
+}
+
+
+/* System controller.  */
+
+typedef struct {
+    uint32_t base;
+    uint32_t pborctl;
+    uint32_t ldopctl;
+    uint32_t int_status;
+    uint32_t int_mask;
+    uint32_t resc;
+    uint32_t rcc;
+    uint32_t rcgc[3];
+    uint32_t scgc[3];
+    uint32_t dcgc[3];
+    uint32_t clkvclr;
+    uint32_t ldoarst;
+    qemu_irq irq;
+    stellaris_board_info *board;
+} ssys_state;
+
+static void ssys_update(ssys_state *s)
+{
+  qemu_set_irq(s->irq, (s->int_status & s->int_mask) != 0);
+}
+
+static uint32_t pllcfg_sandstorm[16] = {
+    0x31c0, /* 1 Mhz */
+    0x1ae0, /* 1.8432 Mhz */
+    0x18c0, /* 2 Mhz */
+    0xd573, /* 2.4576 Mhz */
+    0x37a6, /* 3.57954 Mhz */
+    0x1ae2, /* 3.6864 Mhz */
+    0x0c40, /* 4 Mhz */
+    0x98bc, /* 4.906 Mhz */
+    0x935b, /* 4.9152 Mhz */
+    0x09c0, /* 5 Mhz */
+    0x4dee, /* 5.12 Mhz */
+    0x0c41, /* 6 Mhz */
+    0x75db, /* 6.144 Mhz */
+    0x1ae6, /* 7.3728 Mhz */
+    0x0600, /* 8 Mhz */
+    0x585b /* 8.192 Mhz */
+};
+
+static uint32_t pllcfg_fury[16] = {
+    0x3200, /* 1 Mhz */
+    0x1b20, /* 1.8432 Mhz */
+    0x1900, /* 2 Mhz */
+    0xf42b, /* 2.4576 Mhz */
+    0x37e3, /* 3.57954 Mhz */
+    0x1b21, /* 3.6864 Mhz */
+    0x0c80, /* 4 Mhz */
+    0x98ee, /* 4.906 Mhz */
+    0xd5b4, /* 4.9152 Mhz */
+    0x0a00, /* 5 Mhz */
+    0x4e27, /* 5.12 Mhz */
+    0x1902, /* 6 Mhz */
+    0xec1c, /* 6.144 Mhz */
+    0x1b23, /* 7.3728 Mhz */
+    0x0640, /* 8 Mhz */
+    0xb11c /* 8.192 Mhz */
+};
+
+static uint32_t ssys_read(void *opaque, target_phys_addr_t offset)
+{
+    ssys_state *s = (ssys_state *)opaque;
+
+    offset -= s->base;
+    switch (offset) {
+    case 0x000: /* DID0 */
+        return s->board->did0;
+    case 0x004: /* DID1 */
+        return s->board->did1;
+    case 0x008: /* DC0 */
+        return s->board->dc0;
+    case 0x010: /* DC1 */
+        return s->board->dc1;
+    case 0x014: /* DC2 */
+        return s->board->dc2;
+    case 0x018: /* DC3 */
+        return s->board->dc3;
+    case 0x01c: /* DC4 */
+        return s->board->dc4;
+    case 0x030: /* PBORCTL */
+        return s->pborctl;
+    case 0x034: /* LDOPCTL */
+        return s->ldopctl;
+    case 0x040: /* SRCR0 */
+        return 0;
+    case 0x044: /* SRCR1 */
+        return 0;
+    case 0x048: /* SRCR2 */
+        return 0;
+    case 0x050: /* RIS */
+        return s->int_status;
+    case 0x054: /* IMC */
+        return s->int_mask;
+    case 0x058: /* MISC */
+        return s->int_status & s->int_mask;
+    case 0x05c: /* RESC */
+        return s->resc;
+    case 0x060: /* RCC */
+        return s->rcc;
+    case 0x064: /* PLLCFG */
+        {
+            int xtal;
+            xtal = (s->rcc >> 6) & 0xf;
+            if (s->board->did0 & (1 << 16)) {
+                return pllcfg_fury[xtal];
+            } else {
+                return pllcfg_sandstorm[xtal];
+            }
+        }
+    case 0x100: /* RCGC0 */
+        return s->rcgc[0];
+    case 0x104: /* RCGC1 */
+        return s->rcgc[1];
+    case 0x108: /* RCGC2 */
+        return s->rcgc[2];
+    case 0x110: /* SCGC0 */
+        return s->scgc[0];
+    case 0x114: /* SCGC1 */
+        return s->scgc[1];
+    case 0x118: /* SCGC2 */
+        return s->scgc[2];
+    case 0x120: /* DCGC0 */
+        return s->dcgc[0];
+    case 0x124: /* DCGC1 */
+        return s->dcgc[1];
+    case 0x128: /* DCGC2 */
+        return s->dcgc[2];
+    case 0x150: /* CLKVCLR */
+        return s->clkvclr;
+    case 0x160: /* LDOARST */
+        return s->ldoarst;
+    default:
+        cpu_abort(cpu_single_env, "gptm_read: Bad offset 0x%x\n", (int)offset);
+        return 0;
+    }
+}
+
+static void ssys_write(void *opaque, target_phys_addr_t offset, uint32_t value)
+{
+    ssys_state *s = (ssys_state *)opaque;
+
+    offset -= s->base;
+    switch (offset) {
+    case 0x030: /* PBORCTL */
+        s->pborctl = value & 0xffff;
+        break;
+    case 0x034: /* LDOPCTL */
+        s->ldopctl = value & 0x1f;
+        break;
+    case 0x040: /* SRCR0 */
+    case 0x044: /* SRCR1 */
+    case 0x048: /* SRCR2 */
+        fprintf(stderr, "Peripheral reset not implemented\n");
+        break;
+    case 0x054: /* IMC */
+        s->int_mask = value & 0x7f;
+        break;
+    case 0x058: /* MISC */
+        s->int_status &= ~value;
+        break;
+    case 0x05c: /* RESC */
+        s->resc = value & 0x3f;
+        break;
+    case 0x060: /* RCC */
+        if ((s->rcc & (1 << 13)) != 0 && (value & (1 << 13)) == 0) {
+            /* PLL enable.  */
+            s->int_status |= (1 << 6);
+        }
+        s->rcc = value;
+        stellaris_clock_scale = 5 * (((s->rcc >> 23) & 0xf) + 1);
+        break;
+    case 0x100: /* RCGC0 */
+        s->rcgc[0] = value;
+        break;
+    case 0x104: /* RCGC1 */
+        s->rcgc[1] = value;
+        break;
+    case 0x108: /* RCGC2 */
+        s->rcgc[2] = value;
+        break;
+    case 0x110: /* SCGC0 */
+        s->scgc[0] = value;
+        break;
+    case 0x114: /* SCGC1 */
+        s->scgc[1] = value;
+        break;
+    case 0x118: /* SCGC2 */
+        s->scgc[2] = value;
+        break;
+    case 0x120: /* DCGC0 */
+        s->dcgc[0] = value;
+        break;
+    case 0x124: /* DCGC1 */
+        s->dcgc[1] = value;
+        break;
+    case 0x128: /* DCGC2 */
+        s->dcgc[2] = value;
+        break;
+    case 0x150: /* CLKVCLR */
+        s->clkvclr = value;
+        break;
+    case 0x160: /* LDOARST */
+        s->ldoarst = value;
+        break;
+    default:
+        cpu_abort(cpu_single_env, "gptm_write: Bad offset 0x%x\n", (int)offset);
+    }
+    ssys_update(s);
+}
+
+static CPUReadMemoryFunc *ssys_readfn[] = {
+   ssys_read,
+   ssys_read,
+   ssys_read
+};
+
+static CPUWriteMemoryFunc *ssys_writefn[] = {
+   ssys_write,
+   ssys_write,
+   ssys_write
+};
+
+void ssys_reset(void *opaque)
+{
+    ssys_state *s = (ssys_state *)opaque;
+
+    s->pborctl = 0x7ffd;
+    s->rcc = 0x078e3ac0;
+    s->rcgc[0] = 1;
+    s->scgc[0] = 1;
+    s->dcgc[0] = 1;
+}
+
+static void stellaris_sys_init(uint32_t base, qemu_irq irq,
+                               stellaris_board_info * board)
+{
+    int iomemtype;
+    ssys_state *s;
+
+    s = (ssys_state *)qemu_mallocz(sizeof(ssys_state));
+    s->base = base;
+    s->irq = irq;
+    s->board = board;
+
+    iomemtype = cpu_register_io_memory(0, ssys_readfn,
+                                       ssys_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    ssys_reset(s);
+    /* ??? Save/restore.  */
+}
+
+
+/* I2C controller.  */
+
+typedef struct {
+    i2c_bus *bus;
+    qemu_irq irq;
+    uint32_t base;
+    uint32_t msa;
+    uint32_t mcs;
+    uint32_t mdr;
+    uint32_t mtpr;
+    uint32_t mimr;
+    uint32_t mris;
+    uint32_t mcr;
+} stellaris_i2c_state;
+
+#define STELLARIS_I2C_MCS_BUSY    0x01
+#define STELLARIS_I2C_MCS_ERROR   0x02
+#define STELLARIS_I2C_MCS_ADRACK  0x04
+#define STELLARIS_I2C_MCS_DATACK  0x08
+#define STELLARIS_I2C_MCS_ARBLST  0x10
+#define STELLARIS_I2C_MCS_IDLE    0x20
+#define STELLARIS_I2C_MCS_BUSBSY  0x40
+
+static uint32_t stellaris_i2c_read(void *opaque, target_phys_addr_t offset)
+{
+    stellaris_i2c_state *s = (stellaris_i2c_state *)opaque;
+
+    offset -= s->base;
+    switch (offset) {
+    case 0x00: /* MSA */
+        return s->msa;
+    case 0x04: /* MCS */
+        /* We don't emulate timing, so the controller is never busy.  */
+        return s->mcs | STELLARIS_I2C_MCS_IDLE;
+    case 0x08: /* MDR */
+        return s->mdr;
+    case 0x0c: /* MTPR */
+        return s->mtpr;
+    case 0x10: /* MIMR */
+        return s->mimr;
+    case 0x14: /* MRIS */
+        return s->mris;
+    case 0x18: /* MMIS */
+        return s->mris & s->mimr;
+    case 0x20: /* MCR */
+        return s->mcr;
+    default:
+        cpu_abort(cpu_single_env, "strllaris_i2c_read: Bad offset 0x%x\n",
+                  (int)offset);
+        return 0;
+    }
+}
+
+static void stellaris_i2c_update(stellaris_i2c_state *s)
+{
+    int level;
+
+    level = (s->mris & s->mimr) != 0;
+    qemu_set_irq(s->irq, level);
+}
+
+static void stellaris_i2c_write(void *opaque, target_phys_addr_t offset,
+                                uint32_t value)
+{
+    stellaris_i2c_state *s = (stellaris_i2c_state *)opaque;
+
+    offset -= s->base;
+    switch (offset) {
+    case 0x00: /* MSA */
+        s->msa = value & 0xff;
+        break;
+    case 0x04: /* MCS */
+        if ((s->mcr & 0x10) == 0) {
+            /* Disabled.  Do nothing.  */
+            break;
+        }
+        /* Grab the bus if this is starting a transfer.  */
+        if ((value & 2) && (s->mcs & STELLARIS_I2C_MCS_BUSBSY) == 0) {
+            if (i2c_start_transfer(s->bus, s->msa >> 1, s->msa & 1)) {
+                s->mcs |= STELLARIS_I2C_MCS_ARBLST;
+            } else {
+                s->mcs &= ~STELLARIS_I2C_MCS_ARBLST;
+                s->mcs |= STELLARIS_I2C_MCS_BUSBSY;
+            }
+        }
+        /* If we don't have the bus then indicate an error.  */
+        if (!i2c_bus_busy(s->bus)
+                || (s->mcs & STELLARIS_I2C_MCS_BUSBSY) == 0) {
+            s->mcs |= STELLARIS_I2C_MCS_ERROR;
+            break;
+        }
+        s->mcs &= ~STELLARIS_I2C_MCS_ERROR;
+        if (value & 1) {
+            /* Transfer a byte.  */
+            /* TODO: Handle errors.  */
+            if (s->msa & 1) {
+                /* Recv */
+                s->mdr = i2c_recv(s->bus) & 0xff;
+            } else {
+                /* Send */
+                i2c_send(s->bus, s->mdr);
+            }
+            /* Raise an interrupt.  */
+            s->mris |= 1;
+        }
+        if (value & 4) {
+            /* Finish transfer.  */
+            i2c_end_transfer(s->bus);
+            s->mcs &= ~STELLARIS_I2C_MCS_BUSBSY;
+        }
+        break;
+    case 0x08: /* MDR */
+        s->mdr = value & 0xff;
+        break;
+    case 0x0c: /* MTPR */
+        s->mtpr = value & 0xff;
+        break;
+    case 0x10: /* MIMR */
+        s->mimr = 1;
+        break;
+    case 0x1c: /* MICR */
+        s->mris &= ~value;
+        break;
+    case 0x20: /* MCR */
+        if (value & 1)
+            cpu_abort(cpu_single_env,
+                      "stellaris_i2c_write: Loopback not implemented\n");
+        if (value & 0x20)
+            cpu_abort(cpu_single_env,
+                      "stellaris_i2c_write: Slave mode not implemented\n");
+        s->mcr = value & 0x31;
+        break;
+    default:
+        cpu_abort(cpu_single_env, "stellaris_i2c_write: Bad offset 0x%x\n",
+                  (int)offset);
+    }
+    stellaris_i2c_update(s);
+}
+
+static void stellaris_i2c_reset(stellaris_i2c_state *s)
+{
+    if (s->mcs & STELLARIS_I2C_MCS_BUSBSY)
+        i2c_end_transfer(s->bus);
+
+    s->msa = 0;
+    s->mcs = 0;
+    s->mdr = 0;
+    s->mtpr = 1;
+    s->mimr = 0;
+    s->mris = 0;
+    s->mcr = 0;
+    stellaris_i2c_update(s);
+}
+
+static CPUReadMemoryFunc *stellaris_i2c_readfn[] = {
+   stellaris_i2c_read,
+   stellaris_i2c_read,
+   stellaris_i2c_read
+};
+
+static CPUWriteMemoryFunc *stellaris_i2c_writefn[] = {
+   stellaris_i2c_write,
+   stellaris_i2c_write,
+   stellaris_i2c_write
+};
+
+static void stellaris_i2c_init(uint32_t base, qemu_irq irq, i2c_bus *bus)
+{
+    stellaris_i2c_state *s;
+    int iomemtype;
+
+    s = (stellaris_i2c_state *)qemu_mallocz(sizeof(stellaris_i2c_state));
+    s->base = base;
+    s->irq = irq;
+    s->bus = bus;
+
+    iomemtype = cpu_register_io_memory(0, stellaris_i2c_readfn,
+                                       stellaris_i2c_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    /* ??? For now we only implement the master interface.  */
+    stellaris_i2c_reset(s);
+}
+
+/* Analogue to Digital Converter.  This is only partially implemented,
+   enough for applications that use a combined ADC and timer tick.  */
+
+#define STELLARIS_ADC_EM_CONTROLLER 0
+#define STELLARIS_ADC_EM_COMP       1
+#define STELLARIS_ADC_EM_EXTERNAL   4
+#define STELLARIS_ADC_EM_TIMER      5
+#define STELLARIS_ADC_EM_PWM0       6
+#define STELLARIS_ADC_EM_PWM1       7
+#define STELLARIS_ADC_EM_PWM2       8
+
+#define STELLARIS_ADC_FIFO_EMPTY    0x0100
+#define STELLARIS_ADC_FIFO_FULL     0x1000
+
+typedef struct
+{
+    uint32_t base;
+    uint32_t actss;
+    uint32_t ris;
+    uint32_t im;
+    uint32_t emux;
+    uint32_t ostat;
+    uint32_t ustat;
+    uint32_t sspri;
+    uint32_t sac;
+    struct {
+        uint32_t state;
+        uint32_t data[16];
+    } fifo[4];
+    uint32_t ssmux[4];
+    uint32_t ssctl[4];
+    qemu_irq irq;
+} stellaris_adc_state;
+
+static uint32_t stellaris_adc_fifo_read(stellaris_adc_state *s, int n)
+{
+    int tail;
+
+    tail = s->fifo[n].state & 0xf;
+    if (s->fifo[n].state & STELLARIS_ADC_FIFO_EMPTY) {
+        s->ustat |= 1 << n;
+    } else {
+        s->fifo[n].state = (s->fifo[n].state & ~0xf) | ((tail + 1) & 0xf);
+        s->fifo[n].state &= ~STELLARIS_ADC_FIFO_FULL;
+        if (tail + 1 == ((s->fifo[n].state >> 4) & 0xf))
+            s->fifo[n].state |= STELLARIS_ADC_FIFO_EMPTY;
+    }
+    return s->fifo[n].data[tail];
+}
+
+static void stellaris_adc_fifo_write(stellaris_adc_state *s, int n,
+                                     uint32_t value)
+{
+    int head;
+
+    head = (s->fifo[n].state >> 4) & 0xf;
+    if (s->fifo[n].state & STELLARIS_ADC_FIFO_FULL) {
+        s->ostat |= 1 << n;
+        return;
+    }
+    s->fifo[n].data[head] = value;
+    head = (head + 1) & 0xf;
+    s->fifo[n].state &= ~STELLARIS_ADC_FIFO_EMPTY;
+    s->fifo[n].state = (s->fifo[n].state & ~0xf0) | (head << 4);
+    if ((s->fifo[n].state & 0xf) == head)
+        s->fifo[n].state |= STELLARIS_ADC_FIFO_FULL;
+}
+
+static void stellaris_adc_update(stellaris_adc_state *s)
+{
+    int level;
+
+    level = (s->ris & s->im) != 0;
+    qemu_set_irq(s->irq, level);
+}
+
+static void stellaris_adc_trigger(void *opaque, int irq, int level)
+{
+    stellaris_adc_state *s = (stellaris_adc_state *)opaque;
+    /* Some applications use the ADC as a random number source, so introduce
+       some variation into the signal.  */
+    static uint32_t noise = 0;
+
+    if ((s->actss & 1) == 0) {
+        return;
+    }
+
+    noise = noise * 314159 + 1;
+    /* ??? actual inputs not implemented.  Return an arbitrary value.  */
+    stellaris_adc_fifo_write(s, 0, 0x200 + ((noise >> 16) & 7));
+    s->ris |= 1;
+    stellaris_adc_update(s);
+}
+
+static void stellaris_adc_reset(stellaris_adc_state *s)
+{
+    int n;
+
+    for (n = 0; n < 4; n++) {
+        s->ssmux[n] = 0;
+        s->ssctl[n] = 0;
+        s->fifo[n].state = STELLARIS_ADC_FIFO_EMPTY;
+    }
+}
+
+static uint32_t stellaris_adc_read(void *opaque, target_phys_addr_t offset)
+{
+    stellaris_adc_state *s = (stellaris_adc_state *)opaque;
+
+    /* TODO: Implement this.  */
+    offset -= s->base;
+    if (offset >= 0x40 && offset < 0xc0) {
+        int n;
+        n = (offset - 0x40) >> 5;
+        switch (offset & 0x1f) {
+        case 0x00: /* SSMUX */
+            return s->ssmux[n];
+        case 0x04: /* SSCTL */
+            return s->ssctl[n];
+        case 0x08: /* SSFIFO */
+            return stellaris_adc_fifo_read(s, n);
+        case 0x0c: /* SSFSTAT */
+            return s->fifo[n].state;
+        default:
+            break;
+        }
+    }
+    switch (offset) {
+    case 0x00: /* ACTSS */
+        return s->actss;
+    case 0x04: /* RIS */
+        return s->ris;
+    case 0x08: /* IM */
+        return s->im;
+    case 0x0c: /* ISC */
+        return s->ris & s->im;
+    case 0x10: /* OSTAT */
+        return s->ostat;
+    case 0x14: /* EMUX */
+        return s->emux;
+    case 0x18: /* USTAT */
+        return s->ustat;
+    case 0x20: /* SSPRI */
+        return s->sspri;
+    case 0x30: /* SAC */
+        return s->sac;
+    default:
+        cpu_abort(cpu_single_env, "strllaris_adc_read: Bad offset 0x%x\n",
+                  (int)offset);
+        return 0;
+    }
+}
+
+static void stellaris_adc_write(void *opaque, target_phys_addr_t offset,
+                                uint32_t value)
+{
+    stellaris_adc_state *s = (stellaris_adc_state *)opaque;
+
+    /* TODO: Implement this.  */
+    offset -= s->base;
+    if (offset >= 0x40 && offset < 0xc0) {
+        int n;
+        n = (offset - 0x40) >> 5;
+        switch (offset & 0x1f) {
+        case 0x00: /* SSMUX */
+            s->ssmux[n] = value & 0x33333333;
+            return;
+        case 0x04: /* SSCTL */
+            if (value != 6) {
+                cpu_abort(cpu_single_env, "ADC: Unimplemented sequence %x\n",
+                          value);
+            }
+            s->ssctl[n] = value;
+            return;
+        default:
+            break;
+        }
+    }
+    switch (offset) {
+    case 0x00: /* ACTSS */
+        s->actss = value & 0xf;
+        if (value & 0xe) {
+            cpu_abort(cpu_single_env,
+                      "Not implemented:  ADC sequencers 1-3\n");
+        }
+        break;
+    case 0x08: /* IM */
+        s->im = value;
+        break;
+    case 0x0c: /* ISC */
+        s->ris &= ~value;
+        break;
+    case 0x10: /* OSTAT */
+        s->ostat &= ~value;
+        break;
+    case 0x14: /* EMUX */
+        s->emux = value;
+        break;
+    case 0x18: /* USTAT */
+        s->ustat &= ~value;
+        break;
+    case 0x20: /* SSPRI */
+        s->sspri = value;
+        break;
+    case 0x28: /* PSSI */
+        cpu_abort(cpu_single_env, "Not implemented:  ADC sample initiate\n");
+        break;
+    case 0x30: /* SAC */
+        s->sac = value;
+        break;
+    default:
+        cpu_abort(cpu_single_env, "stellaris_adc_write: Bad offset 0x%x\n",
+                  (int)offset);
+    }
+    stellaris_adc_update(s);
+}
+
+static CPUReadMemoryFunc *stellaris_adc_readfn[] = {
+   stellaris_adc_read,
+   stellaris_adc_read,
+   stellaris_adc_read
+};
+
+static CPUWriteMemoryFunc *stellaris_adc_writefn[] = {
+   stellaris_adc_write,
+   stellaris_adc_write,
+   stellaris_adc_write
+};
+
+static qemu_irq stellaris_adc_init(uint32_t base, qemu_irq irq)
+{
+    stellaris_adc_state *s;
+    int iomemtype;
+    qemu_irq *qi;
+
+    s = (stellaris_adc_state *)qemu_mallocz(sizeof(stellaris_adc_state));
+    s->base = base;
+    s->irq = irq;
+
+    iomemtype = cpu_register_io_memory(0, stellaris_adc_readfn,
+                                       stellaris_adc_writefn, s);
+    cpu_register_physical_memory(base, 0x00001000, iomemtype);
+    stellaris_adc_reset(s);
+    qi = qemu_allocate_irqs(stellaris_adc_trigger, s, 1);
+    return qi[0];
+}
+
+/* Board init.  */
+static stellaris_board_info stellaris_boards[] = {
+  { "LM3S811EVB",
+    0,
+    0x0032000e,
+    0x001f001f, /* dc0 */
+    0x001132bf,
+    0x01071013,
+    0x3f0f01ff,
+    0x0000001f,
+    OLED_I2C
+  },
+  { "LM3S6965EVB",
+    0x10010002,
+    0x1073402e,
+    0x00ff007f, /* dc0 */
+    0x001133ff,
+    0x030f5317,
+    0x0f0f87ff,
+    0x5000007f,
+    OLED_SSI
+  }
+};
+
+static void stellaris_init(const char *kernel_filename, const char *cpu_model,
+                           DisplayState *ds, stellaris_board_info *board)
+{
+    static const int uart_irq[] = {5, 6, 33, 34};
+    static const int timer_irq[] = {19, 21, 23, 35};
+    static const uint32_t gpio_addr[7] =
+      { 0x40004000, 0x40005000, 0x40006000, 0x40007000,
+        0x40024000, 0x40025000, 0x40026000};
+    static const int gpio_irq[7] = {0, 1, 2, 3, 4, 30, 31};
+
+    qemu_irq *pic;
+    qemu_irq *gpio_in[5];
+    qemu_irq *gpio_out[5];
+    qemu_irq adc;
+    int sram_size;
+    int flash_size;
+    i2c_bus *i2c;
+    int i;
+
+    flash_size = ((board->dc0 & 0xffff) + 1) << 1;
+    sram_size = (board->dc0 >> 18) + 1;
+    pic = armv7m_init(flash_size, sram_size, kernel_filename, cpu_model);
+
+    if (board->dc1 & (1 << 16)) {
+        adc = stellaris_adc_init(0x40038000, pic[14]);
+    } else {
+        adc = NULL;
+    }
+    for (i = 0; i < 4; i++) {
+        if (board->dc2 & (0x10000 << i)) {
+            stellaris_gptm_init(0x40030000 + i * 0x1000,
+                                pic[timer_irq[i]], adc);
+        }
+    }
+
+    stellaris_sys_init(0x400fe000, pic[28], board);
+
+    for (i = 0; i < 7; i++) {
+        if (board->dc4 & (1 << i)) {
+            gpio_in[i] = pl061_init(gpio_addr[i], pic[gpio_irq[i]],
+                                    &gpio_out[i]);
+        }
+    }
+
+    if (board->dc2 & (1 << 12)) {
+        i2c = i2c_init_bus();
+        stellaris_i2c_init(0x40020000, pic[8], i2c);
+        if (board->oled == OLED_I2C) {
+            ssd0303_init(ds, i2c, 0x3d);
+        }
+    }
+
+    for (i = 0; i < 4; i++) {
+        if (board->dc2 & (1 << i)) {
+            pl011_init(0x4000c000 + i * 0x1000, pic[uart_irq[i]],
+                       serial_hds[i], PL011_LUMINARY);
+        }
+    }
+    if (board->dc2 & (1 << 4)) {
+        if (board->oled == OLED_SSI) {
+            void * oled;
+            /* FIXME: Implement chip select for OLED/MMC.  */
+            oled = ssd0323_init(ds, &gpio_out[2][7]);
+            pl022_init(0x40008000, pic[7], ssd0323_xfer_ssi, oled);
+        } else {
+            pl022_init(0x40008000, pic[7], NULL, NULL);
+        }
+    }
+}
+
+/* FIXME: Figure out how to generate these from stellaris_boards.  */
+static void lm3s811evb_init(int ram_size, int vga_ram_size,
+                     const char *boot_device, DisplayState *ds,
+                     const char **fd_filename, int snapshot,
+                     const char *kernel_filename, const char *kernel_cmdline,
+                     const char *initrd_filename, const char *cpu_model)
+{
+    stellaris_init(kernel_filename, cpu_model, ds, &stellaris_boards[0]);
+}
+
+static void lm3s6965evb_init(int ram_size, int vga_ram_size,
+                     const char *boot_device, DisplayState *ds,
+                     const char **fd_filename, int snapshot,
+                     const char *kernel_filename, const char *kernel_cmdline,
+                     const char *initrd_filename, const char *cpu_model)
+{
+    stellaris_init(kernel_filename, cpu_model, ds, &stellaris_boards[1]);
+}
+
+QEMUMachine lm3s811evb_machine = {
+    "lm3s811evb",
+    "Stellaris LM3S811EVB",
+    lm3s811evb_init,
+};
+
+QEMUMachine lm3s6965evb_machine = {
+    "lm3s6965evb",
+    "Stellaris LM3S6965EVB",
+    lm3s6965evb_init,
+};
diff --git a/hw/versatilepb.c b/hw/versatilepb.c
index fc27c46..4e8e76e 100644
--- a/hw/versatilepb.c
+++ b/hw/versatilepb.c
@@ -208,10 +208,10 @@ static void versatile_init(int ram_size, int vga_ram_size,
         }
     }
 
-    pl011_init(0x101f1000, pic[12], serial_hds[0]);
-    pl011_init(0x101f2000, pic[13], serial_hds[1]);
-    pl011_init(0x101f3000, pic[14], serial_hds[2]);
-    pl011_init(0x10009000, sic[6], serial_hds[3]);
+    pl011_init(0x101f1000, pic[12], serial_hds[0], PL011_ARM);
+    pl011_init(0x101f2000, pic[13], serial_hds[1], PL011_ARM);
+    pl011_init(0x101f3000, pic[14], serial_hds[2], PL011_ARM);
+    pl011_init(0x10009000, sic[6], serial_hds[3], PL011_ARM);
 
     pl080_init(0x10130000, pic[17], 8);
     sp804_init(0x101e2000, pic[4]);
diff --git a/qemu-doc.texi b/qemu-doc.texi
index 3645ba2..740d303 100644
--- a/qemu-doc.texi
+++ b/qemu-doc.texi
@@ -77,10 +77,12 @@ For system emulation, the following hardware targets are supported:
 @item Sun4m (32-bit Sparc processor)
 @item Sun4u (64-bit Sparc processor, in progress)
 @item Malta board (32-bit MIPS processor)
-@item ARM Integrator/CP (ARM926E, 1026E or 946E processor)
-@item ARM Versatile baseboard (ARM926E)
-@item ARM RealView Emulation baseboard (ARM926EJ-S)
+@item ARM Integrator/CP (ARM)
+@item ARM Versatile baseboard (ARM)
+@item ARM RealView Emulation baseboard (ARM)
 @item Spitz, Akita, Borzoi and Terrier PDAs (PXA270 processor)
+@item Luminary Micro LM3S811EVB (ARM Cortex-M3)
+@item Luminary Micro LM3S6965EVB (ARM Cortex-M3)
 @item Freescale MCF5208EVB (ColdFire V2).
 @item Arnewsh MCF5206 evaluation board (ColdFire V2).
 @item Palm Tungsten|E PDA (OMAP310 processor)
@@ -2117,7 +2119,7 @@ devices:
 
 @itemize @minus
 @item
-ARM926E, ARM1026E or ARM946E CPU
+ARM926E, ARM1026E, ARM946E, ARM1136 or Cortex-A8 CPU
 @item
 Two PL011 UARTs
 @item
@@ -2134,7 +2136,7 @@ The ARM Versatile baseboard is emulated with the following devices:
 
 @itemize @minus
 @item
-ARM926E CPU
+ARM926E, ARM1136 or Cortex-A8 CPU
 @item
 PL190 Vectored Interrupt Controller
 @item
@@ -2163,7 +2165,7 @@ The ARM RealView Emulation baseboard is emulated with the following devices:
 
 @itemize @minus
 @item
-ARM926E CPU
+ARM926E, ARM1136, ARM11MPCORE(x4) or Cortex-A8 CPU
 @item
 ARM AMBA Generic/Distributed Interrupt Controller
 @item
@@ -2237,6 +2239,34 @@ Secure Digital card connected to OMAP MMC/SD host
 Three on-chip UARTs
 @end itemize
 
+The Luminary Micro Stellaris LM3S811EVB emulation includes the following
+devices:
+
+@itemize @minus
+@item
+Cortex-M3 CPU core.
+@item
+64k Flash and 8k SRAM.
+@item
+Timers, UARTs, ADC and I@math{^2}C interface.
+@item
+OSRAM Pictiva 96x16 OLED with SSD0303 controller on I@math{^2}C bus.
+@end itemize
+
+The Luminary Micro Stellaris LM3S6965EVB emulation includes the following
+devices:
+
+@itemize @minus
+@item
+Cortex-M3 CPU core.
+@item
+256k Flash and 64k SRAM.
+@item
+Timers, UARTs, ADC, I@math{^2}C and SSI interfaces.
+@item
+OSRAM Pictiva 128x64 OLED with SSD0323 controller connected via SSI.
+@end itemize
+
 A Linux 2.6 test image is available on the QEMU web site. More
 information is available in the QEMU mailing-list archive.
 
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index fb22b90..59b9696 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -37,6 +37,18 @@
 #define EXCP_IRQ             5
 #define EXCP_FIQ             6
 #define EXCP_BKPT            7
+#define EXCP_EXCEPTION_EXIT  8   /* Return from v7M exception.  */
+
+#define ARMV7M_EXCP_RESET   1
+#define ARMV7M_EXCP_NMI     2
+#define ARMV7M_EXCP_HARD    3
+#define ARMV7M_EXCP_MEM     4
+#define ARMV7M_EXCP_BUS     5
+#define ARMV7M_EXCP_USAGE   6
+#define ARMV7M_EXCP_SVC     11
+#define ARMV7M_EXCP_DEBUG   12
+#define ARMV7M_EXCP_PENDSV  14
+#define ARMV7M_EXCP_SYSTICK 15
 
 typedef void ARMWriteCPFunc(void *opaque, int cp_info,
                             int srcreg, int operand, uint32_t value);
@@ -76,17 +88,22 @@ typedef struct CPUARMState {
     uint32_t VF; /* V is the bit 31. All other bits are undefined */
     uint32_t NZF; /* N is bit 31. Z is computed from NZF */
     uint32_t QF; /* 0 or 1 */
-
-    int thumb; /* 0 = arm mode, 1 = thumb mode */
+    uint32_t GE; /* cpsr[19:16] */
+    int thumb; /* cprs[5]. 0 = arm mode, 1 = thumb mode. */
+    uint32_t condexec_bits; /* IT bits.  cpsr[15:10,26:25].  */
 
     /* System control coprocessor (cp15) */
     struct {
         uint32_t c0_cpuid;
         uint32_t c0_cachetype;
+        uint32_t c0_c1[8]; /* Feature registers.  */
+        uint32_t c0_c2[8]; /* Instruction set registers.  */
         uint32_t c1_sys; /* System control register.  */
         uint32_t c1_coproc; /* Coprocessor access register.  */
         uint32_t c1_xscaleauxcr; /* XScale auxiliary control register.  */
-        uint32_t c2_base; /* MMU translation table base.  */
+        uint32_t c2_base0; /* MMU translation table base 0.  */
+        uint32_t c2_base1; /* MMU translation table base 1.  */
+        uint32_t c2_mask; /* MMU translation table base mask.  */
         uint32_t c2_data; /* MPU data cachable bits.  */
         uint32_t c2_insn; /* MPU instruction cachable bits.  */
         uint32_t c3; /* MMU domain access control register
@@ -100,6 +117,9 @@ typedef struct CPUARMState {
         uint32_t c9_data;
         uint32_t c13_fcse; /* FCSE PID.  */
         uint32_t c13_context; /* Context ID.  */
+        uint32_t c13_tls1; /* User RW Thread register.  */
+        uint32_t c13_tls2; /* User RO Thread register.  */
+        uint32_t c13_tls3; /* Privileged Thread register.  */
         uint32_t c15_cpar; /* XScale Coprocessor Access Register */
         uint32_t c15_ticonfig; /* TI925T configuration byte.  */
         uint32_t c15_i_max; /* Maximum D-cache dirty line index.  */
@@ -107,6 +127,17 @@ typedef struct CPUARMState {
         uint32_t c15_threadid; /* TI debugger thread-ID.  */
     } cp15;
 
+    struct {
+        uint32_t other_sp;
+        uint32_t vecbase;
+        uint32_t basepri;
+        uint32_t control;
+        int current_sp;
+        int exception;
+        int pending_exception;
+        void *nvic;
+    } v7m;
+
     /* Coprocessor IO used by peripherals */
     struct {
         ARMReadCPFunc *cp_read;
@@ -117,6 +148,10 @@ typedef struct CPUARMState {
     /* Internal CPU feature flags.  */
     uint32_t features;
 
+    /* Callback for vectored interrupt controller.  */
+    int (*get_irq_vector)(struct CPUARMState *);
+    void *irq_opaque;
+
     /* exception/interrupt handling */
     jmp_buf jmp_env;
     int exception_index;
@@ -126,7 +161,7 @@ typedef struct CPUARMState {
 
     /* VFP coprocessor state.  */
     struct {
-        float64 regs[16];
+        float64 regs[32];
 
         uint32_t xregs[16];
         /* We store these fpcsr fields separately for convenience.  */
@@ -136,9 +171,16 @@ typedef struct CPUARMState {
         /* Temporary variables if we don't have spare fp regs.  */
         float32 tmp0s, tmp1s;
         float64 tmp0d, tmp1d;
+        /* scratch space when Tn are not sufficient.  */
+        uint32_t scratch[8];
 
         float_status fp_status;
     } vfp;
+#if defined(CONFIG_USER_ONLY)
+    struct mmon_state *mmon_entry;
+#else
+    uint32_t mmon_addr;
+#endif
 
     /* iwMMXt coprocessor state.  */
     struct {
@@ -169,6 +211,7 @@ int cpu_arm_exec(CPUARMState *s);
 void cpu_arm_close(CPUARMState *s);
 void do_interrupt(CPUARMState *);
 void switch_mode(CPUARMState *, int);
+uint32_t do_arm_semihosting(CPUARMState *env);
 
 /* you can call this signal handler from your SIGBUS and SIGSEGV
    signal handlers to inform the virtual CPU of exceptions. non zero
@@ -176,6 +219,9 @@ void switch_mode(CPUARMState *, int);
 int cpu_arm_signal_handler(int host_signum, void *pinfo,
                            void *puc);
 
+void cpu_lock(void);
+void cpu_unlock(void);
+
 #define CPSR_M (0x1f)
 #define CPSR_T (1 << 5)
 #define CPSR_F (1 << 6)
@@ -183,13 +229,24 @@ int cpu_arm_signal_handler(int host_signum, void *pinfo,
 #define CPSR_A (1 << 8)
 #define CPSR_E (1 << 9)
 #define CPSR_IT_2_7 (0xfc00)
-/* Bits 20-23 reserved.  */
+#define CPSR_GE (0xf << 16)
+#define CPSR_RESERVED (0xf << 20)
 #define CPSR_J (1 << 24)
 #define CPSR_IT_0_1 (3 << 25)
 #define CPSR_Q (1 << 27)
-#define CPSR_NZCV (0xf << 28)
+#define CPSR_V (1 << 28)
+#define CPSR_C (1 << 29)
+#define CPSR_Z (1 << 30)
+#define CPSR_N (1 << 31)
+#define CPSR_NZCV (CPSR_N | CPSR_Z | CPSR_C | CPSR_V)
+
+#define CPSR_IT (CPSR_IT_0_1 | CPSR_IT_2_7)
+#define CACHED_CPSR_BITS (CPSR_T | CPSR_GE | CPSR_IT | CPSR_Q | CPSR_NZCV)
+/* Bits writable in user mode.  */
+#define CPSR_USER (CPSR_NZCV | CPSR_Q | CPSR_GE)
+/* Execution state bits.  MRS read as zero, MSR writes ignored.  */
+#define CPSR_EXEC (CPSR_T | CPSR_IT | CPSR_J)
 
-#define CACHED_CPSR_BITS (CPSR_T | CPSR_Q | CPSR_NZCV)
 /* Return the current CPSR value.  */
 static inline uint32_t cpsr_read(CPUARMState *env)
 {
@@ -197,7 +254,21 @@ static inline uint32_t cpsr_read(CPUARMState *env)
     ZF = (env->NZF == 0);
     return env->uncached_cpsr | (env->NZF & 0x80000000) | (ZF << 30) |
         (env->CF << 29) | ((env->VF & 0x80000000) >> 3) | (env->QF << 27)
-        | (env->thumb << 5);
+        | (env->thumb << 5) | ((env->condexec_bits & 3) << 25)
+        | ((env->condexec_bits & 0xfc) << 8)
+        | (env->GE << 16);
+}
+
+/* Return the current xPSR value.  */
+static inline uint32_t xpsr_read(CPUARMState *env)
+{
+    int ZF;
+    ZF = (env->NZF == 0);
+    return (env->NZF & 0x80000000) | (ZF << 30)
+        | (env->CF << 29) | ((env->VF & 0x80000000) >> 3) | (env->QF << 27)
+        | (env->thumb << 24) | ((env->condexec_bits & 3) << 25)
+        | ((env->condexec_bits & 0xfc) << 8)
+        | env->v7m.exception;
 }
 
 /* Set the CPSR.  Note that some bits of mask must be all-set or all-clear.  */
@@ -213,6 +284,17 @@ static inline void cpsr_write(CPUARMState *env, uint32_t val, uint32_t mask)
         env->QF = ((val & CPSR_Q) != 0);
     if (mask & CPSR_T)
         env->thumb = ((val & CPSR_T) != 0);
+    if (mask & CPSR_IT_0_1) {
+        env->condexec_bits &= ~3;
+        env->condexec_bits |= (val >> 25) & 3;
+    }
+    if (mask & CPSR_IT_2_7) {
+        env->condexec_bits &= 3;
+        env->condexec_bits |= (val >> 8) & 0xfc;
+    }
+    if (mask & CPSR_GE) {
+        env->GE = (val >> 16) & 0xf;
+    }
 
     if ((env->uncached_cpsr ^ val) & mask & CPSR_M) {
         switch_mode(env, val & CPSR_M);
@@ -221,6 +303,32 @@ static inline void cpsr_write(CPUARMState *env, uint32_t val, uint32_t mask)
     env->uncached_cpsr = (env->uncached_cpsr & ~mask) | (val & mask);
 }
 
+/* Set the xPSR.  Note that some bits of mask must be all-set or all-clear.  */
+static inline void xpsr_write(CPUARMState *env, uint32_t val, uint32_t mask)
+{
+    /* NOTE: N = 1 and Z = 1 cannot be stored currently */
+    if (mask & CPSR_NZCV) {
+        env->NZF = (val & 0xc0000000) ^ 0x40000000;
+        env->CF = (val >> 29) & 1;
+        env->VF = (val << 3) & 0x80000000;
+    }
+    if (mask & CPSR_Q)
+        env->QF = ((val & CPSR_Q) != 0);
+    if (mask & (1 << 24))
+        env->thumb = ((val & (1 << 24)) != 0);
+    if (mask & CPSR_IT_0_1) {
+        env->condexec_bits &= ~3;
+        env->condexec_bits |= (val >> 25) & 3;
+    }
+    if (mask & CPSR_IT_2_7) {
+        env->condexec_bits &= 3;
+        env->condexec_bits |= (val >> 8) & 0xfc;
+    }
+    if (mask & 0x1ff) {
+        env->v7m.exception = val & 0x1ff;
+    }
+}
+
 enum arm_cpu_mode {
   ARM_CPU_MODE_USR = 0x10,
   ARM_CPU_MODE_FIQ = 0x11,
@@ -234,6 +342,8 @@ enum arm_cpu_mode {
 /* VFP system registers.  */
 #define ARM_VFP_FPSID   0
 #define ARM_VFP_FPSCR   1
+#define ARM_VFP_MVFR1   6
+#define ARM_VFP_MVFR0   7
 #define ARM_VFP_FPEXC   8
 #define ARM_VFP_FPINST  9
 #define ARM_VFP_FPINST2 10
@@ -253,7 +363,15 @@ enum arm_features {
     ARM_FEATURE_AUXCR,  /* ARM1026 Auxiliary control register.  */
     ARM_FEATURE_XSCALE, /* Intel XScale extensions.  */
     ARM_FEATURE_IWMMXT, /* Intel iwMMXt extension.  */
+    ARM_FEATURE_V6,
+    ARM_FEATURE_V6K,
+    ARM_FEATURE_V7,
+    ARM_FEATURE_THUMB2,
     ARM_FEATURE_MPU,    /* Only has Memory Protection Unit, not full MMU.  */
+    ARM_FEATURE_VFP3,
+    ARM_FEATURE_NEON,
+    ARM_FEATURE_DIV,
+    ARM_FEATURE_M, /* Microcontroller profile.  */
     ARM_FEATURE_OMAPCP  /* OMAP specific CP15 ops handling.  */
 };
 
@@ -264,27 +382,44 @@ static inline int arm_feature(CPUARMState *env, int feature)
 
 void arm_cpu_list(FILE *f, int (*cpu_fprintf)(FILE *f, const char *fmt, ...));
 
+/* Interface between CPU and Interrupt controller.  */
+void armv7m_nvic_set_pending(void *opaque, int irq);
+int armv7m_nvic_acknowledge_irq(void *opaque);
+void armv7m_nvic_complete_irq(void *opaque, int irq);
+
 void cpu_arm_set_cp_io(CPUARMState *env, int cpnum,
                        ARMReadCPFunc *cp_read, ARMWriteCPFunc *cp_write,
                        void *opaque);
 
-#define ARM_CPUID_ARM1026   0x4106a262
-#define ARM_CPUID_ARM926    0x41069265
-#define ARM_CPUID_ARM946    0x41059461
-#define ARM_CPUID_TI915T    0x54029152
-#define ARM_CPUID_TI925T    0x54029252
-#define ARM_CPUID_PXA250    0x69052100
-#define ARM_CPUID_PXA255    0x69052d00
-#define ARM_CPUID_PXA260    0x69052903
-#define ARM_CPUID_PXA261    0x69052d05
-#define ARM_CPUID_PXA262    0x69052d06
-#define ARM_CPUID_PXA270    0x69054110
-#define ARM_CPUID_PXA270_A0 0x69054110
-#define ARM_CPUID_PXA270_A1 0x69054111
-#define ARM_CPUID_PXA270_B0 0x69054112
-#define ARM_CPUID_PXA270_B1 0x69054113
-#define ARM_CPUID_PXA270_C0 0x69054114
-#define ARM_CPUID_PXA270_C5 0x69054117
+/* Does the core conform to the the "MicroController" profile. e.g. Cortex-M3.
+   Note the M in older cores (eg. ARM7TDMI) stands for Multiply. These are
+   conventional cores (ie. Application or Realtime profile).  */
+
+#define IS_M(env) arm_feature(env, ARM_FEATURE_M)
+#define ARM_CPUID(env) (env->cp15.c0_cpuid)
+
+#define ARM_CPUID_ARM1026     0x4106a262
+#define ARM_CPUID_ARM926      0x41069265
+#define ARM_CPUID_ARM946      0x41059461
+#define ARM_CPUID_TI915T      0x54029152
+#define ARM_CPUID_TI925T      0x54029252
+#define ARM_CPUID_PXA250      0x69052100
+#define ARM_CPUID_PXA255      0x69052d00
+#define ARM_CPUID_PXA260      0x69052903
+#define ARM_CPUID_PXA261      0x69052d05
+#define ARM_CPUID_PXA262      0x69052d06
+#define ARM_CPUID_PXA270      0x69054110
+#define ARM_CPUID_PXA270_A0   0x69054110
+#define ARM_CPUID_PXA270_A1   0x69054111
+#define ARM_CPUID_PXA270_B0   0x69054112
+#define ARM_CPUID_PXA270_B1   0x69054113
+#define ARM_CPUID_PXA270_C0   0x69054114
+#define ARM_CPUID_PXA270_C5   0x69054117
+#define ARM_CPUID_ARM1136     0x4117b363
+#define ARM_CPUID_ARM11MPCORE 0x410fb022
+#define ARM_CPUID_CORTEXA8    0x410fc080
+#define ARM_CPUID_CORTEXM3    0x410fc231
+#define ARM_CPUID_ANY         0xffffffff
 
 #if defined(CONFIG_USER_ONLY)
 #define TARGET_PAGE_BITS 12
@@ -302,6 +437,8 @@ void cpu_arm_set_cp_io(CPUARMState *env, int cpnum,
 #define cpu_signal_handler cpu_arm_signal_handler
 #define cpu_list arm_cpu_list
 
+#define ARM_CPU_SAVE_VERSION 1
+
 /* MMU modes definitions */
 #define MMU_MODE0_SUFFIX _kernel
 #define MMU_MODE1_SUFFIX _user
diff --git a/target-arm/exec.h b/target-arm/exec.h
index 0c53b8d..cb20206 100644
--- a/target-arm/exec.h
+++ b/target-arm/exec.h
@@ -68,12 +68,18 @@ static inline int cpu_halted(CPUState *env) {
 
 /* In op_helper.c */
 
-void cpu_lock(void);
-void cpu_unlock(void);
 void helper_set_cp(CPUState *, uint32_t, uint32_t);
 uint32_t helper_get_cp(CPUState *, uint32_t);
 void helper_set_cp15(CPUState *, uint32_t, uint32_t);
 uint32_t helper_get_cp15(CPUState *, uint32_t);
+void helper_set_r13_banked(CPUState *env, int mode, uint32_t val);
+uint32_t helper_get_r13_banked(CPUState *env, int mode);
+uint32_t helper_v7m_mrs(CPUState *env, int reg);
+void helper_v7m_msr(CPUState *env, int reg, uint32_t val);
+
+void helper_mark_exclusive(CPUARMState *, uint32_t addr);
+int helper_test_exclusive(CPUARMState *, uint32_t addr);
+void helper_clrex(CPUARMState *env);
 
 void cpu_loop_exit(void);
 
@@ -91,4 +97,11 @@ void do_vfp_cmpes(void);
 void do_vfp_cmped(void);
 void do_vfp_set_fpscr(void);
 void do_vfp_get_fpscr(void);
-
+float32 helper_recps_f32(float32, float32);
+float32 helper_rsqrts_f32(float32, float32);
+uint32_t helper_recpe_u32(uint32_t);
+uint32_t helper_rsqrte_u32(uint32_t);
+float32 helper_recpe_f32(float32);
+float32 helper_rsqrte_f32(float32);
+void helper_neon_tbl(int rn, int maxindex);
+uint32_t helper_neon_mul_p8(uint32_t op1, uint32_t op2);
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 06eac66..6975256 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -4,6 +4,25 @@
 
 #include "cpu.h"
 #include "exec-all.h"
+#include "gdbstub.h"
+
+static uint32_t cortexa8_cp15_c0_c1[8] =
+{ 0x1031, 0x11, 0x400, 0, 0x31100003, 0x20000000, 0x01202000, 0x11 };
+
+static uint32_t cortexa8_cp15_c0_c2[8] =
+{ 0x00101111, 0x12112111, 0x21232031, 0x11112131, 0x00111142, 0, 0, 0 };
+
+static uint32_t mpcore_cp15_c0_c1[8] =
+{ 0x111, 0x1, 0, 0x2, 0x01100103, 0x10020302, 0x01222000, 0 };
+
+static uint32_t mpcore_cp15_c0_c2[8] =
+{ 0x00100011, 0x12002111, 0x11221011, 0x01102131, 0x141, 0, 0, 0 };
+
+static uint32_t arm1136_cp15_c0_c1[8] =
+{ 0x111, 0x1, 0x2, 0x3, 0x01130003, 0x10030302, 0x01222110, 0 };
+
+static uint32_t arm1136_cp15_c0_c2[8] =
+{ 0x00140011, 0x12002111, 0x11231111, 0x01102131, 0x141, 0, 0, 0 };
 
 static uint32_t cpu_arm_find_by_name(const char *name);
 
@@ -34,6 +53,62 @@ static void cpu_reset_model_id(CPUARMState *env, uint32_t id)
         env->cp15.c0_cachetype = 0x1dd20d2;
         env->cp15.c1_sys = 0x00090078;
         break;
+    case ARM_CPUID_ARM1136:
+        set_feature(env, ARM_FEATURE_V6);
+        set_feature(env, ARM_FEATURE_VFP);
+        set_feature(env, ARM_FEATURE_AUXCR);
+        env->vfp.xregs[ARM_VFP_FPSID] = 0x410120b4;
+        env->vfp.xregs[ARM_VFP_MVFR0] = 0x11111111;
+        env->vfp.xregs[ARM_VFP_MVFR1] = 0x00000000;
+        memcpy(env->cp15.c0_c1, arm1136_cp15_c0_c1, 8 * sizeof(uint32_t));
+        memcpy(env->cp15.c0_c1, arm1136_cp15_c0_c2, 8 * sizeof(uint32_t));
+        env->cp15.c0_cachetype = 0x1dd20d2;
+        break;
+    case ARM_CPUID_ARM11MPCORE:
+        set_feature(env, ARM_FEATURE_V6);
+        set_feature(env, ARM_FEATURE_V6K);
+        set_feature(env, ARM_FEATURE_VFP);
+        set_feature(env, ARM_FEATURE_AUXCR);
+        env->vfp.xregs[ARM_VFP_FPSID] = 0x410120b4;
+        env->vfp.xregs[ARM_VFP_MVFR0] = 0x11111111;
+        env->vfp.xregs[ARM_VFP_MVFR1] = 0x00000000;
+        memcpy(env->cp15.c0_c1, mpcore_cp15_c0_c1, 8 * sizeof(uint32_t));
+        memcpy(env->cp15.c0_c1, mpcore_cp15_c0_c2, 8 * sizeof(uint32_t));
+        env->cp15.c0_cachetype = 0x1dd20d2;
+        break;
+    case ARM_CPUID_CORTEXA8:
+        set_feature(env, ARM_FEATURE_V6);
+        set_feature(env, ARM_FEATURE_V6K);
+        set_feature(env, ARM_FEATURE_V7);
+        set_feature(env, ARM_FEATURE_AUXCR);
+        set_feature(env, ARM_FEATURE_THUMB2);
+        set_feature(env, ARM_FEATURE_VFP);
+        set_feature(env, ARM_FEATURE_VFP3);
+        set_feature(env, ARM_FEATURE_NEON);
+        env->vfp.xregs[ARM_VFP_FPSID] = 0x410330c0;
+        env->vfp.xregs[ARM_VFP_MVFR0] = 0x11110222;
+        env->vfp.xregs[ARM_VFP_MVFR1] = 0x00011100;
+        memcpy(env->cp15.c0_c1, cortexa8_cp15_c0_c1, 8 * sizeof(uint32_t));
+        memcpy(env->cp15.c0_c1, cortexa8_cp15_c0_c2, 8 * sizeof(uint32_t));
+        env->cp15.c0_cachetype = 0x1dd20d2;
+        break;
+    case ARM_CPUID_CORTEXM3:
+        set_feature(env, ARM_FEATURE_V6);
+        set_feature(env, ARM_FEATURE_THUMB2);
+        set_feature(env, ARM_FEATURE_V7);
+        set_feature(env, ARM_FEATURE_M);
+        set_feature(env, ARM_FEATURE_DIV);
+        break;
+    case ARM_CPUID_ANY: /* For userspace emulation.  */
+        set_feature(env, ARM_FEATURE_V6);
+        set_feature(env, ARM_FEATURE_V6K);
+        set_feature(env, ARM_FEATURE_V7);
+        set_feature(env, ARM_FEATURE_THUMB2);
+        set_feature(env, ARM_FEATURE_VFP);
+        set_feature(env, ARM_FEATURE_VFP3);
+        set_feature(env, ARM_FEATURE_NEON);
+        set_feature(env, ARM_FEATURE_DIV);
+        break;
     case ARM_CPUID_TI915T:
     case ARM_CPUID_TI925T:
         set_feature(env, ARM_FEATURE_OMAPCP);
@@ -85,6 +160,10 @@ void cpu_reset(CPUARMState *env)
 #else
     /* SVC mode with interrupts disabled.  */
     env->uncached_cpsr = ARM_CPU_MODE_SVC | CPSR_A | CPSR_F | CPSR_I;
+    /* On ARMv7-M the CPSR_I is the value of the PRIMASK register, and is
+       clear at reset.  */
+    if (IS_M(env))
+        env->uncached_cpsr &= ~CPSR_I;
     env->vfp.xregs[ARM_VFP_FPEXC] = 0;
 #endif
     env->regs[15] = 0;
@@ -117,6 +196,10 @@ static const struct arm_cpu_t arm_cpu_names[] = {
     { ARM_CPUID_ARM926, "arm926"},
     { ARM_CPUID_ARM946, "arm946"},
     { ARM_CPUID_ARM1026, "arm1026"},
+    { ARM_CPUID_ARM1136, "arm1136"},
+    { ARM_CPUID_ARM11MPCORE, "arm11mpcore"},
+    { ARM_CPUID_CORTEXM3, "cortex-m3"},
+    { ARM_CPUID_CORTEXA8, "cortex-a8"},
     { ARM_CPUID_TI925T, "ti925t" },
     { ARM_CPUID_PXA250, "pxa250" },
     { ARM_CPUID_PXA255, "pxa255" },
@@ -130,6 +213,7 @@ static const struct arm_cpu_t arm_cpu_names[] = {
     { ARM_CPUID_PXA270_B1, "pxa270-b1" },
     { ARM_CPUID_PXA270_C0, "pxa270-c0" },
     { ARM_CPUID_PXA270_C5, "pxa270-c5" },
+    { ARM_CPUID_ANY, "any"},
     { 0, NULL}
 };
 
@@ -164,6 +248,30 @@ void cpu_arm_close(CPUARMState *env)
     free(env);
 }
 
+/* Polynomial multiplication is like integer multiplcation except the
+   partial products are XORed, not added.  */
+uint32_t helper_neon_mul_p8(uint32_t op1, uint32_t op2)
+{
+    uint32_t mask;
+    uint32_t result;
+    result = 0;
+    while (op1) {
+        mask = 0;
+        if (op1 & 1)
+            mask |= 0xff;
+        if (op1 & (1 << 8))
+            mask |= (0xff << 8);
+        if (op1 & (1 << 16))
+            mask |= (0xff << 16);
+        if (op1 & (1 << 24))
+            mask |= (0xff << 24);
+        result ^= op2 & mask;
+        op1 = (op1 >> 1) & 0x7f7f7f7f;
+        op2 = (op2 << 1) & 0xfefefefe;
+    }
+    return result;
+}
+
 #if defined(CONFIG_USER_ONLY)
 
 void do_interrupt (CPUState *env)
@@ -171,6 +279,16 @@ void do_interrupt (CPUState *env)
     env->exception_index = -1;
 }
 
+/* Structure used to record exclusive memory locations.  */
+typedef struct mmon_state {
+    struct mmon_state *next;
+    CPUARMState *cpu_env;
+    uint32_t addr;
+} mmon_state;
+
+/* Chain of current locks.  */
+static mmon_state* mmon_head = NULL;
+
 int cpu_arm_handle_mmu_fault (CPUState *env, target_ulong address, int rw,
                               int mmu_idx, int is_softmmu)
 {
@@ -184,6 +302,64 @@ int cpu_arm_handle_mmu_fault (CPUState *env, target_ulong address, int rw,
     return 1;
 }
 
+static void allocate_mmon_state(CPUState *env)
+{
+    env->mmon_entry = malloc(sizeof (mmon_state));
+    if (!env->mmon_entry)
+        abort();
+    memset (env->mmon_entry, 0, sizeof (mmon_state));
+    env->mmon_entry->cpu_env = env;
+    mmon_head = env->mmon_entry;
+}
+
+/* Flush any monitor locks for the specified address.  */
+static void flush_mmon(uint32_t addr)
+{
+    mmon_state *mon;
+
+    for (mon = mmon_head; mon; mon = mon->next)
+      {
+        if (mon->addr != addr)
+          continue;
+
+        mon->addr = 0;
+        break;
+      }
+}
+
+/* Mark an address for exclusive access.  */
+void helper_mark_exclusive(CPUState *env, uint32_t addr)
+{
+    if (!env->mmon_entry)
+        allocate_mmon_state(env);
+    /* Clear any previous locks.  */
+    flush_mmon(addr);
+    env->mmon_entry->addr = addr;
+}
+
+/* Test if an exclusive address is still exclusive.  Returns zero
+   if the address is still exclusive.   */
+int helper_test_exclusive(CPUState *env, uint32_t addr)
+{
+    int res;
+
+    if (!env->mmon_entry)
+        return 1;
+    if (env->mmon_entry->addr == addr)
+        res = 0;
+    else
+        res = 1;
+    flush_mmon(addr);
+    return res;
+}
+
+void helper_clrex(CPUState *env)
+{
+    if (!(env->mmon_entry && env->mmon_entry->addr))
+        return;
+    flush_mmon(env->mmon_entry->addr);
+}
+
 target_phys_addr_t cpu_get_phys_page_debug(CPUState *env, target_ulong addr)
 {
     return addr;
@@ -215,12 +391,35 @@ uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
     return 0;
 }
 
+/* These should probably raise undefined insn exceptions.  */
+void helper_v7m_msr(CPUState *env, int reg, uint32_t val)
+{
+    cpu_abort(env, "v7m_mrs %d\n", reg);
+}
+
+uint32_t helper_v7m_mrs(CPUState *env, int reg)
+{
+    cpu_abort(env, "v7m_mrs %d\n", reg);
+    return 0;
+}
+
 void switch_mode(CPUState *env, int mode)
 {
     if (mode != ARM_CPU_MODE_USR)
         cpu_abort(env, "Tried to switch out of user mode\n");
 }
 
+void helper_set_r13_banked(CPUState *env, int mode, uint32_t val)
+{
+    cpu_abort(env, "banked r13 write\n");
+}
+
+uint32_t helper_get_r13_banked(CPUState *env, int mode)
+{
+    cpu_abort(env, "banked r13 read\n");
+    return 0;
+}
+
 #else
 
 extern int semihosting_enabled;
@@ -275,6 +474,129 @@ void switch_mode(CPUState *env, int mode)
     env->spsr = env->banked_spsr[i];
 }
 
+static void v7m_push(CPUARMState *env, uint32_t val)
+{
+    env->regs[13] -= 4;
+    stl_phys(env->regs[13], val);
+}
+
+static uint32_t v7m_pop(CPUARMState *env)
+{
+    uint32_t val;
+    val = ldl_phys(env->regs[13]);
+    env->regs[13] += 4;
+    return val;
+}
+
+/* Switch to V7M main or process stack pointer.  */
+static void switch_v7m_sp(CPUARMState *env, int process)
+{
+    uint32_t tmp;
+    if (env->v7m.current_sp != process) {
+        tmp = env->v7m.other_sp;
+        env->v7m.other_sp = env->regs[13];
+        env->regs[13] = tmp;
+        env->v7m.current_sp = process;
+    }
+}
+
+static void do_v7m_exception_exit(CPUARMState *env)
+{
+    uint32_t type;
+    uint32_t xpsr;
+
+    type = env->regs[15];
+    if (env->v7m.exception != 0)
+        armv7m_nvic_complete_irq(env->v7m.nvic, env->v7m.exception);
+
+    /* Switch to the target stack.  */
+    switch_v7m_sp(env, (type & 4) != 0);
+    /* Pop registers.  */
+    env->regs[0] = v7m_pop(env);
+    env->regs[1] = v7m_pop(env);
+    env->regs[2] = v7m_pop(env);
+    env->regs[3] = v7m_pop(env);
+    env->regs[12] = v7m_pop(env);
+    env->regs[14] = v7m_pop(env);
+    env->regs[15] = v7m_pop(env);
+    xpsr = v7m_pop(env);
+    xpsr_write(env, xpsr, 0xfffffdff);
+    /* Undo stack alignment.  */
+    if (xpsr & 0x200)
+        env->regs[13] |= 4;
+    /* ??? The exception return type specifies Thread/Handler mode.  However
+       this is also implied by the xPSR value. Not sure what to do
+       if there is a mismatch.  */
+    /* ??? Likewise for mismatches between the CONTROL register and the stack
+       pointer.  */
+}
+
+void do_interrupt_v7m(CPUARMState *env)
+{
+    uint32_t xpsr = xpsr_read(env);
+    uint32_t lr;
+    uint32_t addr;
+
+    lr = 0xfffffff1;
+    if (env->v7m.current_sp)
+        lr |= 4;
+    if (env->v7m.exception == 0)
+        lr |= 8;
+
+    /* For exceptions we just mark as pending on the NVIC, and let that
+       handle it.  */
+    /* TODO: Need to escalate if the current priority is higher than the
+       one we're raising.  */
+    switch (env->exception_index) {
+    case EXCP_UDEF:
+        armv7m_nvic_set_pending(env->v7m.nvic, ARMV7M_EXCP_USAGE);
+        return;
+    case EXCP_SWI:
+        env->regs[15] += 2;
+        armv7m_nvic_set_pending(env->v7m.nvic, ARMV7M_EXCP_SVC);
+        return;
+    case EXCP_PREFETCH_ABORT:
+    case EXCP_DATA_ABORT:
+        armv7m_nvic_set_pending(env->v7m.nvic, ARMV7M_EXCP_MEM);
+        return;
+    case EXCP_BKPT:
+        armv7m_nvic_set_pending(env->v7m.nvic, ARMV7M_EXCP_DEBUG);
+        return;
+    case EXCP_IRQ:
+        env->v7m.exception = armv7m_nvic_acknowledge_irq(env->v7m.nvic);
+        break;
+    case EXCP_EXCEPTION_EXIT:
+        do_v7m_exception_exit(env);
+        return;
+    default:
+        cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);
+        return; /* Never happens.  Keep compiler happy.  */
+    }
+
+    /* Align stack pointer.  */
+    /* ??? Should only do this if Configuration Control Register
+       STACKALIGN bit is set.  */
+    if (env->regs[13] & 4) {
+        env->regs[13] += 4;
+        xpsr |= 0x200;
+    }
+    /* Switch to the hander mode.  */
+    v7m_push(env, xpsr);
+    v7m_push(env, env->regs[15]);
+    v7m_push(env, env->regs[14]);
+    v7m_push(env, env->regs[12]);
+    v7m_push(env, env->regs[3]);
+    v7m_push(env, env->regs[2]);
+    v7m_push(env, env->regs[1]);
+    v7m_push(env, env->regs[0]);
+    switch_v7m_sp(env, 0);
+    env->uncached_cpsr &= ~CPSR_IT;
+    env->regs[14] = lr;
+    addr = ldl_phys(env->v7m.vecbase + env->v7m.exception * 4);
+    env->regs[15] = addr & 0xfffffffe;
+    env->thumb = addr & 1;
+}
+
 /* Handle a CPU exception.  */
 void do_interrupt(CPUARMState *env)
 {
@@ -283,6 +605,10 @@ void do_interrupt(CPUARMState *env)
     int new_mode;
     uint32_t offset;
 
+    if (IS_M(env)) {
+        do_interrupt_v7m(env);
+        return;
+    }
     /* TODO: Vectored interrupt controller.  */
     switch (env->exception_index) {
     case EXCP_UDEF:
@@ -317,8 +643,19 @@ void do_interrupt(CPUARMState *env)
         /* The PC already points to the next instructon.  */
         offset = 0;
         break;
-    case EXCP_PREFETCH_ABORT:
     case EXCP_BKPT:
+        /* See if this is a semihosting syscall.  */
+        if (env->thumb) {
+            mask = lduw_code(env->regs[15]) & 0xff;
+            if (mask == 0xab
+                  && (env->uncached_cpsr & CPSR_M) != ARM_CPU_MODE_USR) {
+                env->regs[15] += 2;
+                env->regs[0] = do_arm_semihosting(env);
+                return;
+            }
+        }
+        /* Fall through to prefetch abort.  */
+    case EXCP_PREFETCH_ABORT:
         new_mode = ARM_CPU_MODE_ABT;
         addr = 0x0c;
         mask = CPSR_A | CPSR_I;
@@ -354,6 +691,8 @@ void do_interrupt(CPUARMState *env)
     }
     switch_mode (env, new_mode);
     env->spsr = cpsr_read(env);
+    /* Clear IT bits.  */
+    env->condexec_bits = 0;
     /* Switch to the new mode, and switch to Arm mode.  */
     /* ??? Thumb interrupt handlers not implemented.  */
     env->uncached_cpsr = (env->uncached_cpsr & ~CPSR_M) | new_mode;
@@ -370,9 +709,16 @@ void do_interrupt(CPUARMState *env)
 static inline int check_ap(CPUState *env, int ap, int domain, int access_type,
                            int is_user)
 {
+  int prot_ro;
+
   if (domain == 3)
     return PAGE_READ | PAGE_WRITE;
 
+  if (access_type == 1)
+      prot_ro = 0;
+  else
+      prot_ro = PAGE_READ;
+
   switch (ap) {
   case 0:
       if (access_type == 1)
@@ -389,18 +735,24 @@ static inline int check_ap(CPUState *env, int ap, int domain, int access_type,
       return is_user ? 0 : PAGE_READ | PAGE_WRITE;
   case 2:
       if (is_user)
-          return (access_type == 1) ? 0 : PAGE_READ;
+          return prot_ro;
       else
           return PAGE_READ | PAGE_WRITE;
   case 3:
       return PAGE_READ | PAGE_WRITE;
+  case 4: case 7: /* Reserved.  */
+      return 0;
+  case 5:
+      return is_user ? 0 : prot_ro;
+  case 6:
+      return prot_ro;
   default:
       abort();
   }
 }
 
-static int get_phys_addr(CPUState *env, uint32_t address, int access_type,
-                         int is_user, uint32_t *phys_ptr, int *prot)
+static int get_phys_addr_v5(CPUState *env, uint32_t address, int access_type,
+			    int is_user, uint32_t *phys_ptr, int *prot)
 {
     int code;
     uint32_t table;
@@ -410,145 +762,259 @@ static int get_phys_addr(CPUState *env, uint32_t address, int access_type,
     int domain;
     uint32_t phys_addr;
 
-    /* Fast Context Switch Extension.  */
-    if (address < 0x02000000)
-        address += env->cp15.c13_fcse;
-
-    if ((env->cp15.c1_sys & 1) == 0) {
-        /* MMU/MPU disabled.  */
-        *phys_ptr = address;
-        *prot = PAGE_READ | PAGE_WRITE;
-    } else if (arm_feature(env, ARM_FEATURE_MPU)) {
-        int n;
-        uint32_t mask;
-        uint32_t base;
-
-        *phys_ptr = address;
-        for (n = 7; n >= 0; n--) {
-            base = env->cp15.c6_region[n];
-            if ((base & 1) == 0)
-                continue;
-            mask = 1 << ((base >> 1) & 0x1f);
-            /* Keep this shift separate from the above to avoid an
-               (undefined) << 32.  */
-            mask = (mask << 1) - 1;
-            if (((base ^ address) & ~mask) == 0)
-                break;
-        }
-        if (n < 0)
-            return 2;
-
-        if (access_type == 2) {
-            mask = env->cp15.c5_insn;
-        } else {
-            mask = env->cp15.c5_data;
-        }
-        mask = (mask >> (n * 4)) & 0xf;
-        switch (mask) {
-        case 0:
-            return 1;
-        case 1:
-            if (is_user)
-              return 1;
-            *prot = PAGE_READ | PAGE_WRITE;
-            break;
-        case 2:
-            *prot = PAGE_READ;
-            if (!is_user)
-                *prot |= PAGE_WRITE;
+    /* Pagetable walk.  */
+    /* Lookup l1 descriptor.  */
+    if (address & env->cp15.c2_mask)
+        table = env->cp15.c2_base1;
+    else
+        table = env->cp15.c2_base0;
+    table = (table & 0xffffc000) | ((address >> 18) & 0x3ffc);
+    desc = ldl_phys(table);
+    type = (desc & 3);
+    domain = (env->cp15.c3 >> ((desc >> 4) & 0x1e)) & 3;
+    if (type == 0) {
+        /* Secton translation fault.  */
+        code = 5;
+        goto do_fault;
+    }
+    if (domain == 0 || domain == 2) {
+        if (type == 2)
+            code = 9; /* Section domain fault.  */
+        else
+            code = 11; /* Page domain fault.  */
+        goto do_fault;
+    }
+    if (type == 2) {
+        /* 1Mb section.  */
+        phys_addr = (desc & 0xfff00000) | (address & 0x000fffff);
+        ap = (desc >> 10) & 3;
+        code = 13;
+    } else {
+        /* Lookup l2 entry.  */
+	if (type == 1) {
+	    /* Coarse pagetable.  */
+	    table = (desc & 0xfffffc00) | ((address >> 10) & 0x3fc);
+	} else {
+	    /* Fine pagetable.  */
+	    table = (desc & 0xfffff000) | ((address >> 8) & 0xffc);
+	}
+        desc = ldl_phys(table);
+        switch (desc & 3) {
+        case 0: /* Page translation fault.  */
+            code = 7;
+            goto do_fault;
+        case 1: /* 64k page.  */
+            phys_addr = (desc & 0xffff0000) | (address & 0xffff);
+            ap = (desc >> (4 + ((address >> 13) & 6))) & 3;
             break;
-        case 3:
-            *prot = PAGE_READ | PAGE_WRITE;
+        case 2: /* 4k page.  */
+            phys_addr = (desc & 0xfffff000) | (address & 0xfff);
+            ap = (desc >> (4 + ((address >> 13) & 6))) & 3;
             break;
-        case 5:
-            if (is_user)
-                return 1;
-            *prot = PAGE_READ;
-            break;
-        case 6:
-            *prot = PAGE_READ;
+        case 3: /* 1k page.  */
+	    if (type == 1) {
+		if (arm_feature(env, ARM_FEATURE_XSCALE)) {
+		    phys_addr = (desc & 0xfffff000) | (address & 0xfff);
+		} else {
+		    /* Page translation fault.  */
+		    code = 7;
+		    goto do_fault;
+		}
+	    } else {
+		phys_addr = (desc & 0xfffffc00) | (address & 0x3ff);
+	    }
+            ap = (desc >> 4) & 3;
             break;
         default:
-            /* Bad permission.  */
-            return 1;
+            /* Never happens, but compiler isn't smart enough to tell.  */
+            abort();
         }
+        code = 15;
+    }
+    *prot = check_ap(env, ap, domain, access_type, is_user);
+    if (!*prot) {
+        /* Access permission fault.  */
+        goto do_fault;
+    }
+    *phys_ptr = phys_addr;
+    return 0;
+do_fault:
+    return code | (domain << 4);
+}
+
+static int get_phys_addr_v6(CPUState *env, uint32_t address, int access_type,
+			    int is_user, uint32_t *phys_ptr, int *prot)
+{
+    int code;
+    uint32_t table;
+    uint32_t desc;
+    uint32_t xn;
+    int type;
+    int ap;
+    int domain;
+    uint32_t phys_addr;
+
+    /* Pagetable walk.  */
+    /* Lookup l1 descriptor.  */
+    if (address & env->cp15.c2_mask)
+        table = env->cp15.c2_base1;
+    else
+        table = env->cp15.c2_base0;
+    table = (table & 0xffffc000) | ((address >> 18) & 0x3ffc);
+    desc = ldl_phys(table);
+    type = (desc & 3);
+    if (type == 0) {
+        /* Secton translation fault.  */
+        code = 5;
+        domain = 0;
+        goto do_fault;
+    } else if (type == 2 && (desc & (1 << 18))) {
+        /* Supersection.  */
+        domain = 0;
     } else {
-        /* Pagetable walk.  */
-        /* Lookup l1 descriptor.  */
-        table = (env->cp15.c2_base & 0xffffc000) | ((address >> 18) & 0x3ffc);
-        desc = ldl_phys(table);
-        type = (desc & 3);
-        domain = (env->cp15.c3 >> ((desc >> 4) & 0x1e)) & 3;
-        if (type == 0) {
-            /* Secton translation fault.  */
-            code = 5;
-            goto do_fault;
-        }
-        if (domain == 0 || domain == 2) {
-            if (type == 2)
-                code = 9; /* Section domain fault.  */
-            else
-                code = 11; /* Page domain fault.  */
-            goto do_fault;
-        }
-        if (type == 2) {
-            /* 1Mb section.  */
-            phys_addr = (desc & 0xfff00000) | (address & 0x000fffff);
-            ap = (desc >> 10) & 3;
-            code = 13;
+        /* Section or page.  */
+        domain = (desc >> 4) & 0x1e;
+    }
+    domain = (env->cp15.c3 >> domain) & 3;
+    if (domain == 0 || domain == 2) {
+        if (type == 2)
+            code = 9; /* Section domain fault.  */
+        else
+            code = 11; /* Page domain fault.  */
+        goto do_fault;
+    }
+    if (type == 2) {
+        if (desc & (1 << 18)) {
+            /* Supersection.  */
+            phys_addr = (desc & 0xff000000) | (address & 0x00ffffff);
         } else {
-            /* Lookup l2 entry.  */
-            if (type == 1) {
-                /* Coarse pagetable.  */
-                table = (desc & 0xfffffc00) | ((address >> 10) & 0x3fc);
-            } else {
-                /* Fine pagetable.  */
-                table = (desc & 0xfffff000) | ((address >> 8) & 0xffc);
-            }
-            desc = ldl_phys(table);
-            switch (desc & 3) {
-            case 0: /* Page translation fault.  */
-                code = 7;
-                goto do_fault;
-            case 1: /* 64k page.  */
-                phys_addr = (desc & 0xffff0000) | (address & 0xffff);
-                ap = (desc >> (4 + ((address >> 13) & 6))) & 3;
-                break;
-            case 2: /* 4k page.  */
-                phys_addr = (desc & 0xfffff000) | (address & 0xfff);
-                ap = (desc >> (4 + ((address >> 13) & 6))) & 3;
-                break;
-            case 3: /* 1k page.  */
-                if (type == 1) {
-                    if (arm_feature(env, ARM_FEATURE_XSCALE))
-                        phys_addr = (desc & 0xfffff000) | (address & 0xfff);
-                    else {
-                        /* Page translation fault.  */
-                        code = 7;
-                        goto do_fault;
-                    }
-                } else
-                    phys_addr = (desc & 0xfffffc00) | (address & 0x3ff);
-                ap = (desc >> 4) & 3;
-                break;
-            default:
-                /* Never happens, but compiler isn't smart enough to tell.  */
-                abort();
-            }
-            code = 15;
+            /* Section.  */
+            phys_addr = (desc & 0xfff00000) | (address & 0x000fffff);
         }
-        *prot = check_ap(env, ap, domain, access_type, is_user);
-        if (!*prot) {
-            /* Access permission fault.  */
+        ap = ((desc >> 10) & 3) | ((desc >> 13) & 4);
+        xn = desc & (1 << 4);
+        code = 13;
+    } else {
+        /* Lookup l2 entry.  */
+        table = (desc & 0xfffffc00) | ((address >> 10) & 0x3fc);
+        desc = ldl_phys(table);
+        ap = ((desc >> 4) & 3) | ((desc >> 7) & 4);
+        switch (desc & 3) {
+        case 0: /* Page translation fault.  */
+            code = 7;
             goto do_fault;
+        case 1: /* 64k page.  */
+            phys_addr = (desc & 0xffff0000) | (address & 0xffff);
+            xn = desc & (1 << 15);
+            break;
+        case 2: case 3: /* 4k page.  */
+            phys_addr = (desc & 0xfffff000) | (address & 0xfff);
+            xn = desc & 1;
+            break;
+        default:
+            /* Never happens, but compiler isn't smart enough to tell.  */
+            abort();
         }
-        *phys_ptr = phys_addr;
+        code = 15;
+    }
+    if (xn && access_type == 2)
+        goto do_fault;
+
+    *prot = check_ap(env, ap, domain, access_type, is_user);
+    if (!*prot) {
+        /* Access permission fault.  */
+        goto do_fault;
     }
+    *phys_ptr = phys_addr;
     return 0;
 do_fault:
     return code | (domain << 4);
 }
 
+static int get_phys_addr_mpu(CPUState *env, uint32_t address, int access_type,
+			     int is_user, uint32_t *phys_ptr, int *prot)
+{
+    int n;
+    uint32_t mask;
+    uint32_t base;
+
+    *phys_ptr = address;
+    for (n = 7; n >= 0; n--) {
+	base = env->cp15.c6_region[n];
+	if ((base & 1) == 0)
+	    continue;
+	mask = 1 << ((base >> 1) & 0x1f);
+	/* Keep this shift separate from the above to avoid an
+	   (undefined) << 32.  */
+	mask = (mask << 1) - 1;
+	if (((base ^ address) & ~mask) == 0)
+	    break;
+    }
+    if (n < 0)
+	return 2;
+
+    if (access_type == 2) {
+	mask = env->cp15.c5_insn;
+    } else {
+	mask = env->cp15.c5_data;
+    }
+    mask = (mask >> (n * 4)) & 0xf;
+    switch (mask) {
+    case 0:
+	return 1;
+    case 1:
+	if (is_user)
+	  return 1;
+	*prot = PAGE_READ | PAGE_WRITE;
+	break;
+    case 2:
+	*prot = PAGE_READ;
+	if (!is_user)
+	    *prot |= PAGE_WRITE;
+	break;
+    case 3:
+	*prot = PAGE_READ | PAGE_WRITE;
+	break;
+    case 5:
+	if (is_user)
+	    return 1;
+	*prot = PAGE_READ;
+	break;
+    case 6:
+	*prot = PAGE_READ;
+	break;
+    default:
+	/* Bad permission.  */
+	return 1;
+    }
+    return 0;
+}
+
+static inline int get_phys_addr(CPUState *env, uint32_t address,
+                                int access_type, int is_user,
+                                uint32_t *phys_ptr, int *prot)
+{
+    /* Fast Context Switch Extension.  */
+    if (address < 0x02000000)
+        address += env->cp15.c13_fcse;
+
+    if ((env->cp15.c1_sys & 1) == 0) {
+        /* MMU/MPU disabled.  */
+        *phys_ptr = address;
+        *prot = PAGE_READ | PAGE_WRITE;
+        return 0;
+    } else if (arm_feature(env, ARM_FEATURE_MPU)) {
+	return get_phys_addr_mpu(env, address, access_type, is_user, phys_ptr,
+				 prot);
+    } else if (env->cp15.c1_sys & (1 << 23)) {
+        return get_phys_addr_v6(env, address, access_type, is_user, phys_ptr,
+                                prot);
+    } else {
+        return get_phys_addr_v5(env, address, access_type, is_user, phys_ptr,
+                                prot);
+    }
+}
+
 int cpu_arm_handle_mmu_fault (CPUState *env, target_ulong address,
                               int access_type, int mmu_idx, int is_softmmu)
 {
@@ -572,6 +1038,8 @@ int cpu_arm_handle_mmu_fault (CPUState *env, target_ulong address,
         env->exception_index = EXCP_PREFETCH_ABORT;
     } else {
         env->cp15.c5_data = ret;
+        if (access_type == 1 && arm_feature(env, ARM_FEATURE_V6))
+            env->cp15.c5_data |= (1 << 11);
         env->cp15.c6_data = address;
         env->exception_index = EXCP_DATA_ABORT;
     }
@@ -592,6 +1060,24 @@ target_phys_addr_t cpu_get_phys_page_debug(CPUState *env, target_ulong addr)
     return phys_addr;
 }
 
+/* Not really implemented.  Need to figure out a sane way of doing this.
+   Maybe add generic watchpoint support and use that.  */
+
+void helper_mark_exclusive(CPUState *env, uint32_t addr)
+{
+    env->mmon_addr = addr;
+}
+
+int helper_test_exclusive(CPUState *env, uint32_t addr)
+{
+    return (env->mmon_addr != addr);
+}
+
+void helper_clrex(CPUState *env)
+{
+    env->mmon_addr = -1;
+}
+
 void helper_set_cp(CPUState *env, uint32_t insn, uint32_t val)
 {
     int cp_num = (insn >> 8) & 0xf;
@@ -649,13 +1135,20 @@ static uint32_t extended_mpu_ap_bits(uint32_t val)
 
 void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
 {
-    uint32_t op2;
-    uint32_t crm;
+    int op1;
+    int op2;
+    int crm;
 
+    op1 = (insn >> 21) & 7;
     op2 = (insn >> 5) & 7;
     crm = insn & 0xf;
     switch ((insn >> 16) & 0xf) {
-    case 0: /* ID codes.  */
+    case 0:
+        if (((insn >> 21) & 7) == 2) {
+            /* ??? Select cache level.  Ignore.  */
+            return;
+        }
+        /* ID codes.  */
         if (arm_feature(env, ARM_FEATURE_XSCALE))
             break;
         if (arm_feature(env, ARM_FEATURE_OMAPCP))
@@ -672,12 +1165,13 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
             /* This may enable/disable the MMU, so do a TLB flush.  */
             tlb_flush(env, 1);
             break;
-        case 1:
+        case 1: /* Auxiliary cotrol register.  */
             if (arm_feature(env, ARM_FEATURE_XSCALE)) {
                 env->cp15.c1_xscaleauxcr = val;
                 break;
             }
-            goto bad_reg;
+            /* Not implemented.  */
+            break;
         case 2:
             if (arm_feature(env, ARM_FEATURE_XSCALE))
                 goto bad_reg;
@@ -702,7 +1196,19 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
                 goto bad_reg;
             }
         } else {
-            env->cp15.c2_base = val;
+	    switch (op2) {
+	    case 0:
+		env->cp15.c2_base0 = val;
+		break;
+	    case 1:
+		env->cp15.c2_base1 = val;
+		break;
+	    case 2:
+		env->cp15.c2_mask = ~(((uint32_t)0xffffffffu) >> val);
+		break;
+	    default:
+		goto bad_reg;
+	    }
         }
         break;
     case 3: /* MMU Domain access control / MPU write buffer control.  */
@@ -751,7 +1257,8 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
             case 0:
                 env->cp15.c6_data = val;
                 break;
-            case 1:
+            case 1: /* ??? This is WFAR on armv6 */
+            case 2:
                 env->cp15.c6_insn = val;
                 break;
             default:
@@ -763,6 +1270,7 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
         env->cp15.c15_i_max = 0x000;
         env->cp15.c15_i_min = 0xff0;
         /* No cache, so nothing to do.  */
+        /* ??? MPCore has VA to PA translation functions.  */
         break;
     case 8: /* MMU TLB control.  */
         switch (op2) {
@@ -783,6 +1291,13 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
             tlb_flush(env, 1);
 #endif
             break;
+        case 2: /* Invalidate on ASID.  */
+            tlb_flush(env, val == 0);
+            break;
+        case 3: /* Invalidate single entry on MVA.  */
+            /* ??? This is like case 1, but ignores ASID.  */
+            tlb_flush(env, 1);
+            break;
         default:
             goto bad_reg;
         }
@@ -792,17 +1307,26 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
             break;
         switch (crm) {
         case 0: /* Cache lockdown.  */
-            switch (op2) {
-            case 0:
-                env->cp15.c9_data = val;
-                break;
-            case 1:
-                env->cp15.c9_insn = val;
-                break;
-            default:
-                goto bad_reg;
-            }
-            break;
+	    switch (op1) {
+	    case 0: /* L1 cache.  */
+		switch (op2) {
+		case 0:
+		    env->cp15.c9_data = val;
+		    break;
+		case 1:
+		    env->cp15.c9_insn = val;
+		    break;
+		default:
+		    goto bad_reg;
+		}
+		break;
+	    case 1: /* L2 cache.  */
+		/* Ignore writes to L2 lockdown/auxiliary registers.  */
+		break;
+	    default:
+		goto bad_reg;
+	    }
+	    break;
         case 1: /* TCM memory region registers.  */
             /* Not implemented.  */
             goto bad_reg;
@@ -832,6 +1356,15 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
               tlb_flush(env, 0);
             env->cp15.c13_context = val;
             break;
+        case 2:
+            env->cp15.c13_tls1 = val;
+            break;
+        case 3:
+            env->cp15.c13_tls2 = val;
+            break;
+        case 4:
+            env->cp15.c13_tls3 = val;
+            break;
         default:
             goto bad_reg;
         }
@@ -880,27 +1413,64 @@ void helper_set_cp15(CPUState *env, uint32_t insn, uint32_t val)
     return;
 bad_reg:
     /* ??? For debugging only.  Should raise illegal instruction exception.  */
-    cpu_abort(env, "Unimplemented cp15 register write\n");
+    cpu_abort(env, "Unimplemented cp15 register write (c%d, c%d, {%d, %d})\n",
+              (insn >> 16) & 0xf, crm, op1, op2);
 }
 
 uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
 {
-    uint32_t op2;
-    uint32_t crm;
+    int op1;
+    int op2;
+    int crm;
 
+    op1 = (insn >> 21) & 7;
     op2 = (insn >> 5) & 7;
     crm = insn & 0xf;
     switch ((insn >> 16) & 0xf) {
     case 0: /* ID codes.  */
-        switch (op2) {
-        default: /* Device ID.  */
-            return env->cp15.c0_cpuid;
-        case 1: /* Cache Type.  */
-            return env->cp15.c0_cachetype;
-        case 2: /* TCM status.  */
+        switch (op1) {
+        case 0:
+            switch (crm) {
+            case 0:
+                switch (op2) {
+                case 0: /* Device ID.  */
+                    return env->cp15.c0_cpuid;
+                case 1: /* Cache Type.  */
+		    return env->cp15.c0_cachetype;
+                case 2: /* TCM status.  */
+                    return 0;
+                case 3: /* TLB type register.  */
+                    return 0; /* No lockable TLB entries.  */
+                case 5: /* CPU ID */
+                    return env->cpu_index;
+                default:
+                    goto bad_reg;
+                }
+            case 1:
+                if (!arm_feature(env, ARM_FEATURE_V6))
+                    goto bad_reg;
+                return env->cp15.c0_c1[op2];
+            case 2:
+                if (!arm_feature(env, ARM_FEATURE_V6))
+                    goto bad_reg;
+                return env->cp15.c0_c2[op2];
+            case 3: case 4: case 5: case 6: case 7:
+                return 0;
+            default:
+                goto bad_reg;
+            }
+        case 1:
+            /* These registers aren't documented on arm11 cores.  However
+               Linux looks at them anyway.  */
+            if (!arm_feature(env, ARM_FEATURE_V6))
+                goto bad_reg;
+            if (crm != 0)
+                goto bad_reg;
             if (arm_feature(env, ARM_FEATURE_XSCALE))
                 goto bad_reg;
             return 0;
+        default:
+            goto bad_reg;
         }
     case 1: /* System configuration.  */
         if (arm_feature(env, ARM_FEATURE_OMAPCP))
@@ -909,11 +1479,22 @@ uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
         case 0: /* Control register.  */
             return env->cp15.c1_sys;
         case 1: /* Auxiliary control register.  */
-            if (arm_feature(env, ARM_FEATURE_AUXCR))
-                return 1;
             if (arm_feature(env, ARM_FEATURE_XSCALE))
                 return env->cp15.c1_xscaleauxcr;
-            goto bad_reg;
+            if (!arm_feature(env, ARM_FEATURE_AUXCR))
+                goto bad_reg;
+            switch (ARM_CPUID(env)) {
+            case ARM_CPUID_ARM1026:
+                return 1;
+            case ARM_CPUID_ARM1136:
+                return 7;
+            case ARM_CPUID_ARM11MPCORE:
+                return 1;
+            case ARM_CPUID_CORTEXA8:
+                return 0;
+            default:
+                goto bad_reg;
+            }
         case 2: /* Coprocessor access register.  */
             if (arm_feature(env, ARM_FEATURE_XSCALE))
                 goto bad_reg;
@@ -934,8 +1515,27 @@ uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
                 goto bad_reg;
             }
         } else {
-            return env->cp15.c2_base;
-        }
+	    switch (op2) {
+	    case 0:
+		return env->cp15.c2_base0;
+	    case 1:
+		return env->cp15.c2_base1;
+	    case 2:
+		{
+		    int n;
+		    uint32_t mask;
+		    n = 0;
+		    mask = env->cp15.c2_mask;
+		    while (mask) {
+			n++;
+			mask <<= 1;
+		    }
+		    return n;
+		}
+	    default:
+		goto bad_reg;
+	    }
+	}
     case 3: /* MMU Domain access control / MPU write buffer control.  */
         return env->cp15.c3;
     case 4: /* Reserved.  */
@@ -963,26 +1563,37 @@ uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
         default:
             goto bad_reg;
         }
-    case 6: /* MMU Fault address / MPU base/size.  */
+    case 6: /* MMU Fault address.  */
         if (arm_feature(env, ARM_FEATURE_MPU)) {
-            int n;
-            n = (insn & 0xf);
-            if (n >= 8)
+            if (crm >= 8)
                 goto bad_reg;
-            return env->cp15.c6_region[n];
+            return env->cp15.c6_region[crm];
         } else {
             if (arm_feature(env, ARM_FEATURE_OMAPCP))
                 op2 = 0;
-            switch (op2) {
-            case 0:
-                return env->cp15.c6_data;
-            case 1:
-                /* Arm9 doesn't have an IFAR, but implementing it anyway
-                   shouldn't do any harm.  */
-                return env->cp15.c6_insn;
-            default:
-                goto bad_reg;
-            }
+	    switch (op2) {
+	    case 0:
+		return env->cp15.c6_data;
+	    case 1:
+		if (arm_feature(env, ARM_FEATURE_V6)) {
+		    /* Watchpoint Fault Adrress.  */
+		    return 0; /* Not implemented.  */
+		} else {
+		    /* Instruction Fault Adrress.  */
+		    /* Arm9 doesn't have an IFAR, but implementing it anyway
+		       shouldn't do any harm.  */
+		    return env->cp15.c6_insn;
+		}
+	    case 2:
+		if (arm_feature(env, ARM_FEATURE_V6)) {
+		    /* Instruction Fault Adrress.  */
+		    return env->cp15.c6_insn;
+		} else {
+		    goto bad_reg;
+		}
+	    default:
+		goto bad_reg;
+	    }
         }
     case 7: /* Cache control.  */
         /* ??? This is for test, clean and invaidate operations that set the
@@ -993,13 +1604,23 @@ uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
     case 8: /* MMU TLB control.  */
         goto bad_reg;
     case 9: /* Cache lockdown.  */
-        if (arm_feature(env, ARM_FEATURE_OMAPCP))
+        switch (op1) {
+        case 0: /* L1 cache.  */
+	    if (arm_feature(env, ARM_FEATURE_OMAPCP))
+		return 0;
+            switch (op2) {
+            case 0:
+                return env->cp15.c9_data;
+            case 1:
+                return env->cp15.c9_insn;
+            default:
+                goto bad_reg;
+            }
+        case 1: /* L2 cache */
+            if (crm != 0)
+                goto bad_reg;
+            /* L2 Lockdown and Auxiliary control.  */
             return 0;
-        switch (op2) {
-        case 0:
-            return env->cp15.c9_data;
-        case 1:
-            return env->cp15.c9_insn;
         default:
             goto bad_reg;
         }
@@ -1015,6 +1636,12 @@ uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
             return env->cp15.c13_fcse;
         case 1:
             return env->cp15.c13_context;
+        case 2:
+            return env->cp15.c13_tls1;
+        case 3:
+            return env->cp15.c13_tls2;
+        case 4:
+            return env->cp15.c13_tls3;
         default:
             goto bad_reg;
         }
@@ -1048,10 +1675,125 @@ uint32_t helper_get_cp15(CPUState *env, uint32_t insn)
     }
 bad_reg:
     /* ??? For debugging only.  Should raise illegal instruction exception.  */
-    cpu_abort(env, "Unimplemented cp15 register read\n");
+    cpu_abort(env, "Unimplemented cp15 register read (c%d, c%d, {%d, %d})\n",
+              (insn >> 16) & 0xf, crm, op1, op2);
     return 0;
 }
 
+void helper_set_r13_banked(CPUState *env, int mode, uint32_t val)
+{
+    env->banked_r13[bank_number(mode)] = val;
+}
+
+uint32_t helper_get_r13_banked(CPUState *env, int mode)
+{
+    return env->banked_r13[bank_number(mode)];
+}
+
+uint32_t helper_v7m_mrs(CPUState *env, int reg)
+{
+    switch (reg) {
+    case 0: /* APSR */
+        return xpsr_read(env) & 0xf8000000;
+    case 1: /* IAPSR */
+        return xpsr_read(env) & 0xf80001ff;
+    case 2: /* EAPSR */
+        return xpsr_read(env) & 0xff00fc00;
+    case 3: /* xPSR */
+        return xpsr_read(env) & 0xff00fdff;
+    case 5: /* IPSR */
+        return xpsr_read(env) & 0x000001ff;
+    case 6: /* EPSR */
+        return xpsr_read(env) & 0x0700fc00;
+    case 7: /* IEPSR */
+        return xpsr_read(env) & 0x0700edff;
+    case 8: /* MSP */
+        return env->v7m.current_sp ? env->v7m.other_sp : env->regs[13];
+    case 9: /* PSP */
+        return env->v7m.current_sp ? env->regs[13] : env->v7m.other_sp;
+    case 16: /* PRIMASK */
+        return (env->uncached_cpsr & CPSR_I) != 0;
+    case 17: /* FAULTMASK */
+        return (env->uncached_cpsr & CPSR_F) != 0;
+    case 18: /* BASEPRI */
+    case 19: /* BASEPRI_MAX */
+        return env->v7m.basepri;
+    case 20: /* CONTROL */
+        return env->v7m.control;
+    default:
+        /* ??? For debugging only.  */
+        cpu_abort(env, "Unimplemented system register read (%d)\n", reg);
+        return 0;
+    }
+}
+
+void helper_v7m_msr(CPUState *env, int reg, uint32_t val)
+{
+    switch (reg) {
+    case 0: /* APSR */
+        xpsr_write(env, val, 0xf8000000);
+        break;
+    case 1: /* IAPSR */
+        xpsr_write(env, val, 0xf8000000);
+        break;
+    case 2: /* EAPSR */
+        xpsr_write(env, val, 0xfe00fc00);
+        break;
+    case 3: /* xPSR */
+        xpsr_write(env, val, 0xfe00fc00);
+        break;
+    case 5: /* IPSR */
+        /* IPSR bits are readonly.  */
+        break;
+    case 6: /* EPSR */
+        xpsr_write(env, val, 0x0600fc00);
+        break;
+    case 7: /* IEPSR */
+        xpsr_write(env, val, 0x0600fc00);
+        break;
+    case 8: /* MSP */
+        if (env->v7m.current_sp)
+            env->v7m.other_sp = val;
+        else
+            env->regs[13] = val;
+        break;
+    case 9: /* PSP */
+        if (env->v7m.current_sp)
+            env->regs[13] = val;
+        else
+            env->v7m.other_sp = val;
+        break;
+    case 16: /* PRIMASK */
+        if (val & 1)
+            env->uncached_cpsr |= CPSR_I;
+        else
+            env->uncached_cpsr &= ~CPSR_I;
+        break;
+    case 17: /* FAULTMASK */
+        if (val & 1)
+            env->uncached_cpsr |= CPSR_F;
+        else
+            env->uncached_cpsr &= ~CPSR_F;
+        break;
+    case 18: /* BASEPRI */
+        env->v7m.basepri = val & 0xff;
+        break;
+    case 19: /* BASEPRI_MAX */
+        val &= 0xff;
+        if (val != 0 && (val < env->v7m.basepri || env->v7m.basepri == 0))
+            env->v7m.basepri = val;
+        break;
+    case 20: /* CONTROL */
+        env->v7m.control = val & 3;
+        switch_v7m_sp(env, (val & 2) != 0);
+        break;
+    default:
+        /* ??? For debugging only.  */
+        cpu_abort(env, "Unimplemented system register write (%d)\n", reg);
+        return;
+    }
+}
+
 void cpu_arm_set_cp_io(CPUARMState *env, int cpnum,
                 ARMReadCPFunc *cp_read, ARMWriteCPFunc *cp_write,
                 void *opaque)
diff --git a/target-arm/op.c b/target-arm/op.c
index e8a536c..216944a 100644
--- a/target-arm/op.c
+++ b/target-arm/op.c
@@ -2,7 +2,7 @@
  *  ARM micro operations
  *
  *  Copyright (c) 2003 Fabrice Bellard
- *  Copyright (c) 2005 CodeSourcery, LLC
+ *  Copyright (c) 2005-2007 CodeSourcery, LLC
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
@@ -101,11 +101,6 @@ void OPPROTO op_movl_T0_im(void)
     T0 = PARAM1;
 }
 
-void OPPROTO op_movl_T0_T1(void)
-{
-    T0 = T1;
-}
-
 void OPPROTO op_movl_T1_im(void)
 {
     T1 = PARAM1;
@@ -236,6 +231,11 @@ void OPPROTO op_bicl_T0_T1(void)
     T0 &= ~T1;
 }
 
+void OPPROTO op_notl_T0(void)
+{
+    T0 = ~T0;
+}
+
 void OPPROTO op_notl_T1(void)
 {
     T1 = ~T1;
@@ -351,6 +351,19 @@ void OPPROTO op_test_le(void)
     FORCE_RET();
 }
 
+void OPPROTO op_test_T0(void)
+{
+    if (T0)
+        GOTO_LABEL_PARAM(1);
+    FORCE_RET();
+}
+void OPPROTO op_testn_T0(void)
+{
+    if (!T0)
+        GOTO_LABEL_PARAM(1);
+    FORCE_RET();
+}
+
 void OPPROTO op_goto_tb0(void)
 {
     GOTO_TB(op_goto_tb0, PARAM1, 0);
@@ -368,7 +381,8 @@ void OPPROTO op_exit_tb(void)
 
 void OPPROTO op_movl_T0_cpsr(void)
 {
-    T0 = cpsr_read(env);
+    /* Execution state bits always read as zero.  */
+    T0 = cpsr_read(env) & ~CPSR_EXEC;
     FORCE_RET();
 }
 
@@ -438,6 +452,28 @@ void OPPROTO op_addq_lo_T0_T1(void)
     T0 = res;
 }
 
+/* Dual 16-bit accumulate.  */
+void OPPROTO op_addq_T0_T1_dual(void)
+{
+  uint64_t res;
+  res = ((uint64_t)(env->regs[PARAM2]) << 32) | (env->regs[PARAM1]);
+  res += (int32_t)T0;
+  res += (int32_t)T1;
+  env->regs[PARAM1] = (uint32_t)res;
+  env->regs[PARAM2] = res >> 32;
+}
+
+/* Dual 16-bit subtract accumulate.  */
+void OPPROTO op_subq_T0_T1_dual(void)
+{
+  uint64_t res;
+  res = ((uint64_t)(env->regs[PARAM2]) << 32) | (env->regs[PARAM1]);
+  res += (int32_t)T0;
+  res -= (int32_t)T1;
+  env->regs[PARAM1] = (uint32_t)res;
+  env->regs[PARAM2] = res >> 32;
+}
+
 void OPPROTO op_logicq_cc(void)
 {
     env->NZF = (T1 & 0x80000000) | ((T0 | T1) != 0);
@@ -455,8 +491,21 @@ void OPPROTO op_logicq_cc(void)
 #include "op_mem.h"
 #endif
 
+void OPPROTO op_clrex(void)
+{
+    cpu_lock();
+    helper_clrex(env);
+    cpu_unlock();
+}
+
 /* shifts */
 
+/* Used by NEON.  */
+void OPPROTO op_shll_T0_im(void)
+{
+    T1 = T1 << PARAM1;
+}
+
 /* T1 based */
 
 void OPPROTO op_shll_T1_im(void)
@@ -813,8 +862,39 @@ void OPPROTO op_double_T1_saturate(void)
   FORCE_RET();
 }
 
-/* thumb shift by immediate */
-void OPPROTO op_shll_T0_im_thumb(void)
+/* Unsigned saturating arithmetic for NEON.  */
+void OPPROTO op_addl_T0_T1_usaturate(void)
+{
+  uint32_t res;
+
+  res = T0 + T1;
+  if (res < T0) {
+      env->QF = 1;
+      T0 = 0xffffffff;
+  } else {
+      T0 = res;
+  }
+
+  FORCE_RET();
+}
+
+void OPPROTO op_subl_T0_T1_usaturate(void)
+{
+  uint32_t res;
+
+  res = T0 - T1;
+  if (res > T0) {
+      env->QF = 1;
+      T0 = 0;
+  } else {
+      T0 = res;
+  }
+
+  FORCE_RET();
+}
+
+/* Thumb shift by immediate */
+void OPPROTO op_shll_T0_im_thumb_cc(void)
 {
     int shift;
     shift = PARAM1;
@@ -826,7 +906,13 @@ void OPPROTO op_shll_T0_im_thumb(void)
     FORCE_RET();
 }
 
-void OPPROTO op_shrl_T0_im_thumb(void)
+void OPPROTO op_shll_T0_im_thumb(void)
+{
+    T0 = T0 << PARAM1;
+    FORCE_RET();
+}
+
+void OPPROTO op_shrl_T0_im_thumb_cc(void)
 {
     int shift;
 
@@ -842,7 +928,20 @@ void OPPROTO op_shrl_T0_im_thumb(void)
     FORCE_RET();
 }
 
-void OPPROTO op_sarl_T0_im_thumb(void)
+void OPPROTO op_shrl_T0_im_thumb(void)
+{
+    int shift;
+
+    shift = PARAM1;
+    if (shift == 0) {
+	T0 = 0;
+    } else {
+	T0 = T0 >> shift;
+    }
+    FORCE_RET();
+}
+
+void OPPROTO op_sarl_T0_im_thumb_cc(void)
 {
     int shift;
 
@@ -858,6 +957,19 @@ void OPPROTO op_sarl_T0_im_thumb(void)
     FORCE_RET();
 }
 
+void OPPROTO op_sarl_T0_im_thumb(void)
+{
+    int shift;
+
+    shift = PARAM1;
+    if (shift == 0) {
+	env->CF = T0 & 1;
+    } else {
+	T0 = ((int32_t)T0) >> shift;
+    }
+    FORCE_RET();
+}
+
 /* exceptions */
 
 void OPPROTO op_swi(void)
@@ -891,6 +1003,12 @@ void OPPROTO op_bkpt(void)
     cpu_loop_exit();
 }
 
+void OPPROTO op_exception_exit(void)
+{
+    env->exception_index = EXCP_EXCEPTION_EXIT;
+    cpu_loop_exit();
+}
+
 /* VFP support.  We follow the convention used for VFP instrunctions:
    Single precition routines have a "s" suffix, double precision a
    "d" suffix.  */
@@ -982,6 +1100,28 @@ static inline uint32_t vfp_stoi(float32 s)
     return v.i;
 }
 
+static inline float64 vfp_itod(uint64_t i)
+{
+    union {
+        uint64_t i;
+        float64 d;
+    } v;
+
+    v.i = i;
+    return v.d;
+}
+
+static inline uint64_t vfp_dtoi(float64 d)
+{
+    union {
+        uint64_t i;
+        float64 d;
+    } v;
+
+    v.d = d;
+    return v.i;
+}
+
 /* Integer to float conversion.  */
 VFP_OP(uito, s)
 {
@@ -1056,6 +1196,32 @@ VFP_OP(fcvts, d)
     FT0s = float64_to_float32(FT0d, &env->vfp.fp_status);
 }
 
+/* VFP3 fixed point conversion.  */
+#define VFP_CONV_FIX(name, p, ftype, itype, sign) \
+VFP_OP(name##to, p) \
+{ \
+    ftype tmp; \
+    tmp = sign##int32_to_##ftype ((itype)vfp_##p##toi(FT0##p), \
+                                  &env->vfp.fp_status); \
+    FT0##p = ftype##_scalbn(tmp, PARAM1, &env->vfp.fp_status); \
+} \
+VFP_OP(to##name, p) \
+{ \
+    ftype tmp; \
+    tmp = ftype##_scalbn(FT0##p, PARAM1, &env->vfp.fp_status); \
+    FT0##p = vfp_ito##p((itype)ftype##_to_##sign##int32_round_to_zero(tmp, \
+            &env->vfp.fp_status)); \
+}
+
+VFP_CONV_FIX(sh, d, float64, int16, )
+VFP_CONV_FIX(sl, d, float64, int32, )
+VFP_CONV_FIX(uh, d, float64, uint16, u)
+VFP_CONV_FIX(ul, d, float64, uint32, u)
+VFP_CONV_FIX(sh, s, float32, int16, )
+VFP_CONV_FIX(sl, s, float32, int32, )
+VFP_CONV_FIX(uh, s, float32, uint16, u)
+VFP_CONV_FIX(ul, s, float32, uint32, u)
+
 /* Get and Put values from registers.  */
 VFP_OP(getreg_F0, d)
 {
@@ -1142,6 +1308,20 @@ void OPPROTO op_vfp_mdrr(void)
     FT0d = u.d;
 }
 
+/* Load immediate.  PARAM1 is the 32 most significant bits of the value.  */
+void OPPROTO op_vfp_fconstd(void)
+{
+    CPU_DoubleU u;
+    u.l.upper = PARAM1;
+    u.l.lower = 0;
+    FT0d = u.d;
+}
+
+void OPPROTO op_vfp_fconsts(void)
+{
+    FT0s = vfp_itos(PARAM1);
+}
+
 /* Copy the most significant bit of T0 to all bits of T1.  */
 void OPPROTO op_signbit_T1_T0(void)
 {
@@ -1204,9 +1384,9 @@ void OPPROTO op_movl_user_T0(void)
     FORCE_RET();
 }
 
-void OPPROTO op_movl_T2_T0(void)
+void OPPROTO op_movl_T0_T1(void)
 {
-    T2 = T0;
+    T0 = T1;
 }
 
 void OPPROTO op_movl_T0_T2(void)
@@ -1214,5 +1394,530 @@ void OPPROTO op_movl_T0_T2(void)
     T0 = T2;
 }
 
+void OPPROTO op_movl_T1_T0(void)
+{
+    T1 = T0;
+}
+
+void OPPROTO op_movl_T1_T2(void)
+{
+    T1 = T2;
+}
+
+void OPPROTO op_movl_T2_T0(void)
+{
+    T2 = T0;
+}
+
+/* ARMv6 Media instructions.  */
+
+/* Note that signed overflow is undefined in C.  The following routines are
+   careful to use unsigned types where modulo arithmetic is required.
+   Failure to do so _will_ break on newer gcc.  */
+
+/* Signed saturating arithmetic.  */
+
+/* Perform 16-bit signed satruating addition.  */
+static inline uint16_t add16_sat(uint16_t a, uint16_t b)
+{
+    uint16_t res;
+
+    res = a + b;
+    if (((res ^ a) & 0x8000) && !((a ^ b) & 0x8000)) {
+        if (a & 0x8000)
+            res = 0x8000;
+        else
+            res = 0x7fff;
+    }
+    return res;
+}
+
+/* Perform 8-bit signed satruating addition.  */
+static inline uint8_t add8_sat(uint8_t a, uint8_t b)
+{
+    uint8_t res;
+
+    res = a + b;
+    if (((res ^ a) & 0x80) && !((a ^ b) & 0x80)) {
+        if (a & 0x80)
+            res = 0x80;
+        else
+            res = 0x7f;
+    }
+    return res;
+}
+
+/* Perform 16-bit signed satruating subtraction.  */
+static inline uint16_t sub16_sat(uint16_t a, uint16_t b)
+{
+    uint16_t res;
+
+    res = a - b;
+    if (((res ^ a) & 0x8000) && ((a ^ b) & 0x8000)) {
+        if (a & 0x8000)
+            res = 0x8000;
+        else
+            res = 0x7fff;
+    }
+    return res;
+}
+
+/* Perform 8-bit signed satruating subtraction.  */
+static inline uint8_t sub8_sat(uint8_t a, uint8_t b)
+{
+    uint8_t res;
+
+    res = a - b;
+    if (((res ^ a) & 0x80) && ((a ^ b) & 0x80)) {
+        if (a & 0x80)
+            res = 0x80;
+        else
+            res = 0x7f;
+    }
+    return res;
+}
+
+#define ADD16(a, b, n) RESULT(add16_sat(a, b), n, 16);
+#define SUB16(a, b, n) RESULT(sub16_sat(a, b), n, 16);
+#define ADD8(a, b, n)  RESULT(add8_sat(a, b), n, 8);
+#define SUB8(a, b, n)  RESULT(sub8_sat(a, b), n, 8);
+#define PFX q
+
+#include "op_addsub.h"
+
+/* Unsigned saturating arithmetic.  */
+static inline uint16_t add16_usat(uint16_t a, uint8_t b)
+{
+    uint16_t res;
+    res = a + b;
+    if (res < a)
+        res = 0xffff;
+    return res;
+}
+
+static inline uint16_t sub16_usat(uint16_t a, uint8_t b)
+{
+    if (a < b)
+        return a - b;
+    else
+        return 0;
+}
+
+static inline uint8_t add8_usat(uint8_t a, uint8_t b)
+{
+    uint8_t res;
+    res = a + b;
+    if (res < a)
+        res = 0xff;
+    return res;
+}
+
+static inline uint8_t sub8_usat(uint8_t a, uint8_t b)
+{
+    if (a < b)
+        return a - b;
+    else
+        return 0;
+}
+
+#define ADD16(a, b, n) RESULT(add16_usat(a, b), n, 16);
+#define SUB16(a, b, n) RESULT(sub16_usat(a, b), n, 16);
+#define ADD8(a, b, n)  RESULT(add8_usat(a, b), n, 8);
+#define SUB8(a, b, n)  RESULT(sub8_usat(a, b), n, 8);
+#define PFX uq
+
+#include "op_addsub.h"
+
+/* Signed modulo arithmetic.  */
+#define SARITH16(a, b, n, op) do { \
+    int32_t sum; \
+    sum = (int16_t)((uint16_t)(a) op (uint16_t)(b)); \
+    RESULT(sum, n, 16); \
+    if (sum >= 0) \
+        ge |= 3 << (n * 2); \
+    } while(0)
+
+#define SARITH8(a, b, n, op) do { \
+    int32_t sum; \
+    sum = (int8_t)((uint8_t)(a) op (uint8_t)(b)); \
+    RESULT(sum, n, 8); \
+    if (sum >= 0) \
+        ge |= 1 << n; \
+    } while(0)
+
+
+#define ADD16(a, b, n) SARITH16(a, b, n, +)
+#define SUB16(a, b, n) SARITH16(a, b, n, -)
+#define ADD8(a, b, n)  SARITH8(a, b, n, +)
+#define SUB8(a, b, n)  SARITH8(a, b, n, -)
+#define PFX s
+#define ARITH_GE
+
+#include "op_addsub.h"
+
+/* Unsigned modulo arithmetic.  */
+#define ADD16(a, b, n) do { \
+    uint32_t sum; \
+    sum = (uint32_t)(uint16_t)(a) + (uint32_t)(uint16_t)(b); \
+    RESULT(sum, n, 16); \
+    if ((sum >> 16) == 0) \
+        ge |= 3 << (n * 2); \
+    } while(0)
+
+#define ADD8(a, b, n) do { \
+    uint32_t sum; \
+    sum = (uint32_t)(uint8_t)(a) + (uint32_t)(uint8_t)(b); \
+    RESULT(sum, n, 8); \
+    if ((sum >> 8) == 0) \
+        ge |= 3 << (n * 2); \
+    } while(0)
+
+#define SUB16(a, b, n) do { \
+    uint32_t sum; \
+    sum = (uint32_t)(uint16_t)(a) - (uint32_t)(uint16_t)(b); \
+    RESULT(sum, n, 16); \
+    if ((sum >> 16) == 0) \
+        ge |= 3 << (n * 2); \
+    } while(0)
+
+#define SUB8(a, b, n) do { \
+    uint32_t sum; \
+    sum = (uint32_t)(uint8_t)(a) - (uint32_t)(uint8_t)(b); \
+    RESULT(sum, n, 8); \
+    if ((sum >> 8) == 0) \
+        ge |= 3 << (n * 2); \
+    } while(0)
+
+#define PFX u
+#define ARITH_GE
+
+#include "op_addsub.h"
+
+/* Halved signed arithmetic.  */
+#define ADD16(a, b, n) \
+  RESULT(((int32_t)(int16_t)(a) + (int32_t)(int16_t)(b)) >> 1, n, 16)
+#define SUB16(a, b, n) \
+  RESULT(((int32_t)(int16_t)(a) - (int32_t)(int16_t)(b)) >> 1, n, 16)
+#define ADD8(a, b, n) \
+  RESULT(((int32_t)(int8_t)(a) + (int32_t)(int8_t)(b)) >> 1, n, 8)
+#define SUB8(a, b, n) \
+  RESULT(((int32_t)(int8_t)(a) - (int32_t)(int8_t)(b)) >> 1, n, 8)
+#define PFX sh
+
+#include "op_addsub.h"
+
+/* Halved unsigned arithmetic.  */
+#define ADD16(a, b, n) \
+  RESULT(((uint32_t)(uint16_t)(a) + (uint32_t)(uint16_t)(b)) >> 1, n, 16)
+#define SUB16(a, b, n) \
+  RESULT(((uint32_t)(uint16_t)(a) - (uint32_t)(uint16_t)(b)) >> 1, n, 16)
+#define ADD8(a, b, n) \
+  RESULT(((uint32_t)(uint8_t)(a) + (uint32_t)(uint8_t)(b)) >> 1, n, 8)
+#define SUB8(a, b, n) \
+  RESULT(((uint32_t)(uint8_t)(a) - (uint32_t)(uint8_t)(b)) >> 1, n, 8)
+#define PFX uh
+
+#include "op_addsub.h"
+
+void OPPROTO op_pkhtb_T0_T1(void)
+{
+    T0 = (T0 & 0xffff0000) | (T1 & 0xffff);
+}
+
+void OPPROTO op_pkhbt_T0_T1(void)
+{
+    T0 = (T0 & 0xffff) | (T1 & 0xffff0000);
+}
+void OPPROTO op_rev_T0(void)
+{
+    T0 =  ((T0 & 0xff000000) >> 24)
+        | ((T0 & 0x00ff0000) >> 8)
+        | ((T0 & 0x0000ff00) << 8)
+        | ((T0 & 0x000000ff) << 24);
+}
+
+void OPPROTO op_revh_T0(void)
+{
+    T0 = (T0 >> 16) | (T0 << 16);
+}
+
+void OPPROTO op_rev16_T0(void)
+{
+    T0 =  ((T0 & 0xff000000) >> 8)
+        | ((T0 & 0x00ff0000) << 8)
+        | ((T0 & 0x0000ff00) >> 8)
+        | ((T0 & 0x000000ff) << 8);
+}
+
+void OPPROTO op_revsh_T0(void)
+{
+    T0 = (int16_t)(  ((T0 & 0x0000ff00) >> 8)
+                   | ((T0 & 0x000000ff) << 8));
+}
+
+void OPPROTO op_rbit_T0(void)
+{
+    T0 =  ((T0 & 0xff000000) >> 24)
+        | ((T0 & 0x00ff0000) >> 8)
+        | ((T0 & 0x0000ff00) << 8)
+        | ((T0 & 0x000000ff) << 24);
+    T0 =  ((T0 & 0xf0f0f0f0) >> 4)
+        | ((T0 & 0x0f0f0f0f) << 4);
+    T0 =  ((T0 & 0x88888888) >> 3)
+        | ((T0 & 0x44444444) >> 1)
+        | ((T0 & 0x22222222) << 1)
+        | ((T0 & 0x11111111) << 3);
+}
+
+/* Swap low and high halfwords.  */
+void OPPROTO op_swap_half_T1(void)
+{
+    T1 = (T1 >> 16) | (T1 << 16);
+    FORCE_RET();
+}
+
+/* Dual 16-bit signed multiply.  */
+void OPPROTO op_mul_dual_T0_T1(void)
+{
+    int32_t low;
+    int32_t high;
+    low = (int32_t)(int16_t)T0 * (int32_t)(int16_t)T1;
+    high = (((int32_t)T0) >> 16) * (((int32_t)T1) >> 16);
+    T0 = low;
+    T1 = high;
+}
+
+void OPPROTO op_sel_T0_T1(void)
+{
+    uint32_t mask;
+    uint32_t flags;
+
+    flags = env->GE;
+    mask = 0;
+    if (flags & 1)
+        mask |= 0xff;
+    if (flags & 2)
+        mask |= 0xff00;
+    if (flags & 4)
+        mask |= 0xff0000;
+    if (flags & 8)
+        mask |= 0xff000000;
+    T0 = (T0 & mask) | (T1 & ~mask);
+    FORCE_RET();
+}
+
+void OPPROTO op_roundqd_T0_T1(void)
+{
+    T0 = T1 + ((uint32_t)T0 >> 31);
+}
+
+/* Signed saturation.  */
+static inline uint32_t do_ssat(int32_t val, int shift)
+{
+    int32_t top;
+    uint32_t mask;
+
+    shift = PARAM1;
+    top = val >> shift;
+    mask = (1u << shift) - 1;
+    if (top > 0) {
+        env->QF = 1;
+        return mask;
+    } else if (top < -1) {
+        env->QF = 1;
+        return ~mask;
+    }
+    return val;
+}
+
+/* Unsigned saturation.  */
+static inline uint32_t do_usat(int32_t val, int shift)
+{
+    uint32_t max;
+
+    shift = PARAM1;
+    max = (1u << shift) - 1;
+    if (val < 0) {
+        env->QF = 1;
+        return 0;
+    } else if (val > max) {
+        env->QF = 1;
+        return max;
+    }
+    return val;
+}
+
+/* Signed saturate.  */
+void OPPROTO op_ssat_T1(void)
+{
+    T0 = do_ssat(T0, PARAM1);
+    FORCE_RET();
+}
+
+/* Dual halfword signed saturate.  */
+void OPPROTO op_ssat16_T1(void)
+{
+    uint32_t res;
+
+    res = (uint16_t)do_ssat((int16_t)T0, PARAM1);
+    res |= do_ssat(((int32_t)T0) >> 16, PARAM1) << 16;
+    T0 = res;
+    FORCE_RET();
+}
+
+/* Unsigned saturate.  */
+void OPPROTO op_usat_T1(void)
+{
+    T0 = do_usat(T0, PARAM1);
+    FORCE_RET();
+}
+
+/* Dual halfword unsigned saturate.  */
+void OPPROTO op_usat16_T1(void)
+{
+    uint32_t res;
+
+    res = (uint16_t)do_usat((int16_t)T0, PARAM1);
+    res |= do_usat(((int32_t)T0) >> 16, PARAM1) << 16;
+    T0 = res;
+    FORCE_RET();
+}
+
+/* Dual 16-bit add.  */
+void OPPROTO op_add16_T1_T2(void)
+{
+    uint32_t mask;
+    mask = (T0 & T1) & 0x8000;
+    T0 ^= ~0x8000;
+    T1 ^= ~0x8000;
+    T0 = (T0 + T1) ^ mask;
+}
+
+static inline uint8_t do_usad(uint8_t a, uint8_t b)
+{
+    if (a > b)
+        return a - b;
+    else
+        return b - a;
+}
+
+/* Unsigned sum of absolute byte differences.  */
+void OPPROTO op_usad8_T0_T1(void)
+{
+    uint32_t sum;
+    sum = do_usad(T0, T1);
+    sum += do_usad(T0 >> 8, T1 >> 8);
+    sum += do_usad(T0 >> 16, T1 >>16);
+    sum += do_usad(T0 >> 24, T1 >> 24);
+    T0 = sum;
+}
+
+/* Thumb-2 instructions.  */
+
+/* Insert T1 into T0.  Result goes in T1.  */
+void OPPROTO op_bfi_T1_T0(void)
+{
+    int shift = PARAM1;
+    uint32_t mask = PARAM2;
+    uint32_t bits;
+
+    bits = (T1 << shift) & mask;
+    T1 = (T0 & ~mask) | bits;
+}
+
+/* Unsigned bitfield extract.  */
+void OPPROTO op_ubfx_T1(void)
+{
+    uint32_t shift = PARAM1;
+    uint32_t mask = PARAM2;
+
+    T1 >>= shift;
+    T1 &= mask;
+}
+
+/* Signed bitfield extract.  */
+void OPPROTO op_sbfx_T1(void)
+{
+    uint32_t shift = PARAM1;
+    uint32_t width = PARAM2;
+    int32_t val;
+
+    val = T1 << (32 - (shift + width));
+    T1 = val >> (32 - width);
+}
+
+void OPPROTO op_movtop_T0_im(void)
+{
+    T0 = (T0 & 0xffff) | PARAM1;
+}
+
+/* Used by table branch instructions.  */
+void OPPROTO op_jmp_T0_im(void)
+{
+    env->regs[15] = PARAM1 + (T0 << 1);
+}
+
+void OPPROTO op_set_condexec(void)
+{
+    env->condexec_bits = PARAM1;
+}
+
+void OPPROTO op_sdivl_T0_T1(void)
+{
+  int32_t num;
+  int32_t den;
+  num = T0;
+  den = T1;
+  if (den == 0)
+    T0 = 0;
+  else
+    T0 = num / den;
+  FORCE_RET();
+}
+
+void OPPROTO op_udivl_T0_T1(void)
+{
+  uint32_t num;
+  uint32_t den;
+  num = T0;
+  den = T1;
+  if (den == 0)
+    T0 = 0;
+  else
+    T0 = num / den;
+  FORCE_RET();
+}
+
+void OPPROTO op_movl_T1_r13_banked(void)
+{
+    T1 = helper_get_r13_banked(env, PARAM1);
+}
+
+void OPPROTO op_movl_r13_T1_banked(void)
+{
+    helper_set_r13_banked(env, PARAM1, T1);
+}
+
+void OPPROTO op_v7m_mrs_T0(void)
+{
+    T0 = helper_v7m_mrs(env, PARAM1);
+}
+
+void OPPROTO op_v7m_msr_T0(void)
+{
+    helper_v7m_msr(env, PARAM1, T0);
+}
+
+void OPPROTO op_movl_T0_sp(void)
+{
+    if (PARAM1 == env->v7m.current_sp)
+        T0 = env->regs[13];
+    else
+        T0 = env->v7m.other_sp;
+    FORCE_RET();
+}
+
+#include "op_neon.h"
+
 /* iwMMXt support */
 #include "op_iwmmxt.c"
diff --git a/target-arm/op_addsub.h b/target-arm/op_addsub.h
new file mode 100644
index 0000000..d15360d
--- /dev/null
+++ b/target-arm/op_addsub.h
@@ -0,0 +1,106 @@
+/*
+ * ARMv6 integer SIMD operations.
+ *
+ * Copyright (c) 2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+
+#ifdef ARITH_GE
+#define DECLARE_GE uint32_t ge = 0
+#define SET_GE env->GE = ge
+#else
+#define DECLARE_GE do{}while(0)
+#define SET_GE do{}while(0)
+#endif
+
+#define RESULT(val, n, width) \
+    res |= ((uint32_t)(glue(glue(uint,width),_t))(val)) << (n * width)
+
+void OPPROTO glue(glue(op_,PFX),add16_T0_T1)(void)
+{
+    uint32_t res = 0;
+    DECLARE_GE;
+
+    ADD16(T0, T1, 0);
+    ADD16(T0 >> 16, T1 >> 16, 1);
+    SET_GE;
+    T0 = res;
+    FORCE_RET();
+}
+
+void OPPROTO glue(glue(op_,PFX),add8_T0_T1)(void)
+{
+    uint32_t res = 0;
+    DECLARE_GE;
+
+    ADD8(T0, T1, 0);
+    ADD8(T0 >> 8, T1 >> 8, 1);
+    ADD8(T0 >> 16, T1 >> 16, 2);
+    ADD8(T0 >> 24, T1 >> 24, 3);
+    SET_GE;
+    T0 = res;
+    FORCE_RET();
+}
+
+void OPPROTO glue(glue(op_,PFX),sub16_T0_T1)(void)
+{
+    uint32_t res = 0;
+    DECLARE_GE;
+
+    SUB16(T0, T1, 0);
+    SUB16(T0 >> 16, T1 >> 16, 1);
+    SET_GE;
+    T0 = res;
+    FORCE_RET();
+}
+
+void OPPROTO glue(glue(op_,PFX),sub8_T0_T1)(void)
+{
+    uint32_t res = 0;
+    DECLARE_GE;
+
+    SUB8(T0, T1, 0);
+    SUB8(T0 >> 8, T1 >> 8, 1);
+    SUB8(T0 >> 16, T1 >> 16, 2);
+    SUB8(T0 >> 24, T1 >> 24, 3);
+    SET_GE;
+    T0 = res;
+    FORCE_RET();
+}
+
+void OPPROTO glue(glue(op_,PFX),subaddx_T0_T1)(void)
+{
+    uint32_t res = 0;
+    DECLARE_GE;
+
+    ADD16(T0, T1, 0);
+    SUB16(T0 >> 16, T1 >> 16, 1);
+    SET_GE;
+    T0 = res;
+    FORCE_RET();
+}
+
+void OPPROTO glue(glue(op_,PFX),addsubx_T0_T1)(void)
+{
+    uint32_t res = 0;
+    DECLARE_GE;
+
+    SUB16(T0, T1, 0);
+    ADD16(T0 >> 16, T1 >> 16, 1);
+    SET_GE;
+    T0 = res;
+    FORCE_RET();
+}
+
+#undef DECLARE_GE
+#undef SET_GE
+#undef RESULT
+
+#undef ARITH_GE
+#undef PFX
+#undef ADD16
+#undef SUB16
+#undef ADD8
+#undef SUB8
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index 6e14a4d..a9bd95b 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -1,7 +1,7 @@
 /*
  *  ARM helper routines
  *
- *  Copyright (c) 2005 CodeSourcery, LLC
+ *  Copyright (c) 2005-2007 CodeSourcery, LLC
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
@@ -175,6 +175,81 @@ void do_vfp_get_fpscr(void)
     T0 |= vfp_exceptbits_from_host(i);
 }
 
+float32 helper_recps_f32(float32 a, float32 b)
+{
+    float_status *s = &env->vfp.fp_status;
+    float32 two = int32_to_float32(2, s);
+    return float32_sub(two, float32_mul(a, b, s), s);
+}
+
+float32 helper_rsqrts_f32(float32 a, float32 b)
+{
+    float_status *s = &env->vfp.fp_status;
+    float32 three = int32_to_float32(3, s);
+    return float32_sub(three, float32_mul(a, b, s), s);
+}
+
+/* TODO: The architecture specifies the value that the estimate functions
+   should return.  We return the exact reciprocal/root instead.  */
+float32 helper_recpe_f32(float32 a)
+{
+    float_status *s = &env->vfp.fp_status;
+    float32 one = int32_to_float32(1, s);
+    return float32_div(one, a, s);
+}
+
+float32 helper_rsqrte_f32(float32 a)
+{
+    float_status *s = &env->vfp.fp_status;
+    float32 one = int32_to_float32(1, s);
+    return float32_div(one, float32_sqrt(a, s), s);
+}
+
+uint32_t helper_recpe_u32(uint32_t a)
+{
+    float_status *s = &env->vfp.fp_status;
+    float32 tmp;
+    tmp = int32_to_float32(a, s);
+    tmp = float32_scalbn(tmp, -32, s);
+    tmp = helper_recpe_f32(tmp);
+    tmp = float32_scalbn(tmp, 31, s);
+    return float32_to_int32(tmp, s);
+}
+
+uint32_t helper_rsqrte_u32(uint32_t a)
+{
+    float_status *s = &env->vfp.fp_status;
+    float32 tmp;
+    tmp = int32_to_float32(a, s);
+    tmp = float32_scalbn(tmp, -32, s);
+    tmp = helper_rsqrte_f32(tmp);
+    tmp = float32_scalbn(tmp, 31, s);
+    return float32_to_int32(tmp, s);
+}
+
+void helper_neon_tbl(int rn, int maxindex)
+{
+    uint32_t val;
+    uint32_t mask;
+    uint32_t tmp;
+    int index;
+    int shift;
+    uint64_t *table;
+    table = (uint64_t *)&env->vfp.regs[rn];
+    val = 0;
+    mask = 0;
+    for (shift = 0; shift < 32; shift += 8) {
+        index = (T1 >> shift) & 0xff;
+        if (index <= maxindex) {
+            tmp = (table[index >> 3] >> (index & 7)) & 0xff;
+            val |= tmp << shift;
+        } else {
+            val |= T0 & (0xff << shift);
+        }
+    }
+    T0 = val;
+}
+
 #if !defined(CONFIG_USER_ONLY)
 
 #define MMUSUFFIX _mmu
@@ -227,5 +302,4 @@ void tlb_fill (target_ulong addr, int is_write, int mmu_idx, void *retaddr)
     }
     env = saved_env;
 }
-
 #endif
diff --git a/target-arm/op_mem.h b/target-arm/op_mem.h
index 6bccb06..b12b63c 100644
--- a/target-arm/op_mem.h
+++ b/target-arm/op_mem.h
@@ -1,5 +1,6 @@
 /* ARM memory operations.  */
 
+void helper_ld(uint32_t);
 /* Load from address T1 into T0.  */
 #define MEM_LD_OP(name) \
 void OPPROTO glue(op_ld##name,MEMSUFFIX)(void) \
@@ -49,6 +50,64 @@ MEM_SWP_OP(l, l)
 
 #undef MEM_SWP_OP
 
+/* Load-locked, store exclusive.  */
+#define EXCLUSIVE_OP(suffix, ldsuffix) \
+void OPPROTO glue(op_ld##suffix##ex,MEMSUFFIX)(void) \
+{ \
+    cpu_lock(); \
+    helper_mark_exclusive(env, T1); \
+    T0 = glue(ld##ldsuffix,MEMSUFFIX)(T1); \
+    cpu_unlock(); \
+    FORCE_RET(); \
+} \
+ \
+void OPPROTO glue(op_st##suffix##ex,MEMSUFFIX)(void) \
+{ \
+    int failed; \
+    cpu_lock(); \
+    failed = helper_test_exclusive(env, T1); \
+    /* ??? Is it safe to hold the cpu lock over a store?  */ \
+    if (!failed) { \
+        glue(st##suffix,MEMSUFFIX)(T1, T0); \
+    } \
+    T0 = failed; \
+    cpu_unlock(); \
+    FORCE_RET(); \
+}
+
+EXCLUSIVE_OP(b, ub)
+EXCLUSIVE_OP(w, uw)
+EXCLUSIVE_OP(l, l)
+
+#undef EXCLUSIVE_OP
+
+/* Load exclusive T0:T1 from address T1.  */
+void OPPROTO glue(op_ldqex,MEMSUFFIX)(void)
+{
+    cpu_lock();
+    helper_mark_exclusive(env, T1);
+    T0 = glue(ldl,MEMSUFFIX)(T1);
+    T1 = glue(ldl,MEMSUFFIX)((T1 + 4));
+    cpu_unlock();
+    FORCE_RET();
+}
+
+/* Store exclusive T0:T2 to address T1.  */
+void OPPROTO glue(op_stqex,MEMSUFFIX)(void)
+{
+    int failed;
+    cpu_lock();
+    failed = helper_test_exclusive(env, T1);
+    /* ??? Is it safe to hold the cpu lock over a store?  */
+    if (!failed) {
+        glue(stl,MEMSUFFIX)(T1, T0);
+        glue(stl,MEMSUFFIX)((T1 + 4), T2);
+    }
+    T0 = failed;
+    cpu_unlock();
+    FORCE_RET();
+}
+
 /* Floating point load/store.  Address is in T1 */
 #define VFP_MEM_OP(p, w) \
 void OPPROTO glue(op_vfp_ld##p,MEMSUFFIX)(void) \
diff --git a/target-arm/op_neon.h b/target-arm/op_neon.h
new file mode 100644
index 0000000..232375e
--- /dev/null
+++ b/target-arm/op_neon.h
@@ -0,0 +1,1754 @@
+/*
+ * ARM NEON vector operations.
+ *
+ * Copyright (c) 2007 CodeSourcery.
+ * Written by Paul Brook
+ *
+ * This code is licenced under the GPL.
+ */
+/* Note that for NEON an "l" prefix means it is a wide operation, unlike
+   scalar arm ops where it means a word size operation.  */
+
+/* ??? NEON ops should probably have their own float status.  */
+#define NFS &env->vfp.fp_status
+#define NEON_OP(name) void OPPROTO op_neon_##name (void)
+
+NEON_OP(getreg_T0)
+{
+    T0 = *(uint32_t *)((char *) env + PARAM1);
+}
+
+NEON_OP(getreg_T1)
+{
+    T1 = *(uint32_t *)((char *) env + PARAM1);
+}
+
+NEON_OP(getreg_T2)
+{
+    T2 = *(uint32_t *)((char *) env + PARAM1);
+}
+
+NEON_OP(setreg_T0)
+{
+    *(uint32_t *)((char *) env + PARAM1) = T0;
+}
+
+NEON_OP(setreg_T1)
+{
+    *(uint32_t *)((char *) env + PARAM1) = T1;
+}
+
+NEON_OP(setreg_T2)
+{
+    *(uint32_t *)((char *) env + PARAM1) = T2;
+}
+
+#define NEON_TYPE1(name, type) \
+typedef struct \
+{ \
+    type v1; \
+} neon_##name;
+#ifdef WORDS_BIGENDIAN
+#define NEON_TYPE2(name, type) \
+typedef struct \
+{ \
+    type v2; \
+    type v1; \
+} neon_##name;
+#define NEON_TYPE4(name, type) \
+typedef struct \
+{ \
+    type v4; \
+    type v3; \
+    type v2; \
+    type v1; \
+} neon_##name;
+#else
+#define NEON_TYPE2(name, type) \
+typedef struct \
+{ \
+    type v1; \
+    type v2; \
+} neon_##name;
+#define NEON_TYPE4(name, type) \
+typedef struct \
+{ \
+    type v1; \
+    type v2; \
+    type v3; \
+    type v4; \
+} neon_##name;
+#endif
+
+NEON_TYPE4(s8, int8_t)
+NEON_TYPE4(u8, uint8_t)
+NEON_TYPE2(s16, int16_t)
+NEON_TYPE2(u16, uint16_t)
+NEON_TYPE1(s32, int32_t)
+NEON_TYPE1(u32, uint32_t)
+#undef NEON_TYPE4
+#undef NEON_TYPE2
+#undef NEON_TYPE1
+
+/* Copy from a uint32_t to a vector structure type.  */
+#define NEON_UNPACK(vtype, dest, val) do { \
+    union { \
+        vtype v; \
+        uint32_t i; \
+    } conv_u; \
+    conv_u.i = (val); \
+    dest = conv_u.v; \
+    } while(0)
+
+/* Copy from a vector structure type to a uint32_t.  */
+#define NEON_PACK(vtype, dest, val) do { \
+    union { \
+        vtype v; \
+        uint32_t i; \
+    } conv_u; \
+    conv_u.v = (val); \
+    dest = conv_u.i; \
+    } while(0)
+
+#define NEON_DO1 \
+    NEON_FN(vdest.v1, vsrc1.v1, vsrc2.v1);
+#define NEON_DO2 \
+    NEON_FN(vdest.v1, vsrc1.v1, vsrc2.v1); \
+    NEON_FN(vdest.v2, vsrc1.v2, vsrc2.v2);
+#define NEON_DO4 \
+    NEON_FN(vdest.v1, vsrc1.v1, vsrc2.v1); \
+    NEON_FN(vdest.v2, vsrc1.v2, vsrc2.v2); \
+    NEON_FN(vdest.v3, vsrc1.v3, vsrc2.v3); \
+    NEON_FN(vdest.v4, vsrc1.v4, vsrc2.v4);
+
+#define NEON_VOP(name, vtype, n) \
+NEON_OP(name) \
+{ \
+    vtype vsrc1; \
+    vtype vsrc2; \
+    vtype vdest; \
+    NEON_UNPACK(vtype, vsrc1, T0); \
+    NEON_UNPACK(vtype, vsrc2, T1); \
+    NEON_DO##n; \
+    NEON_PACK(vtype, T0, vdest); \
+    FORCE_RET(); \
+}
+
+#define NEON_VOP1(name, vtype, n) \
+NEON_OP(name) \
+{ \
+    vtype vsrc1; \
+    vtype vdest; \
+    NEON_UNPACK(vtype, vsrc1, T0); \
+    NEON_DO##n; \
+    NEON_PACK(vtype, T0, vdest); \
+    FORCE_RET(); \
+}
+
+/* Pairwise operations.  */
+/* For 32-bit elements each segment only contains a single element, so
+   the elementwise and pairwise operations are the same.  */
+#define NEON_PDO2 \
+    NEON_FN(vdest.v1, vsrc1.v1, vsrc1.v2); \
+    NEON_FN(vdest.v2, vsrc2.v1, vsrc2.v2);
+#define NEON_PDO4 \
+    NEON_FN(vdest.v1, vsrc1.v1, vsrc1.v2); \
+    NEON_FN(vdest.v2, vsrc1.v3, vsrc1.v4); \
+    NEON_FN(vdest.v3, vsrc2.v1, vsrc2.v2); \
+    NEON_FN(vdest.v4, vsrc2.v3, vsrc2.v4); \
+
+#define NEON_POP(name, vtype, n) \
+NEON_OP(name) \
+{ \
+    vtype vsrc1; \
+    vtype vsrc2; \
+    vtype vdest; \
+    NEON_UNPACK(vtype, vsrc1, T0); \
+    NEON_UNPACK(vtype, vsrc2, T1); \
+    NEON_PDO##n; \
+    NEON_PACK(vtype, T0, vdest); \
+    FORCE_RET(); \
+}
+
+#define NEON_FN(dest, src1, src2) dest = (src1 + src2) >> 1
+NEON_VOP(hadd_s8, neon_s8, 4)
+NEON_VOP(hadd_u8, neon_u8, 4)
+NEON_VOP(hadd_s16, neon_s16, 2)
+NEON_VOP(hadd_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(hadd_s32)
+{
+    int32_t src1 = T0;
+    int32_t src2 = T1;
+    int32_t dest;
+
+    dest = (src1 >> 1) + (src2 >> 1);
+    if (src1 & src2 & 1)
+        dest++;
+    T0 = dest;
+    FORCE_RET();
+}
+
+NEON_OP(hadd_u32)
+{
+    uint32_t src1 = T0;
+    uint32_t src2 = T1;
+    uint32_t dest;
+
+    dest = (src1 >> 1) + (src2 >> 1);
+    if (src1 & src2 & 1)
+        dest++;
+    T0 = dest;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) dest = (src1 + src2 + 1) >> 1
+NEON_VOP(rhadd_s8, neon_s8, 4)
+NEON_VOP(rhadd_u8, neon_u8, 4)
+NEON_VOP(rhadd_s16, neon_s16, 2)
+NEON_VOP(rhadd_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(rhadd_s32)
+{
+    int32_t src1 = T0;
+    int32_t src2 = T1;
+    int32_t dest;
+
+    dest = (src1 >> 1) + (src2 >> 1);
+    if ((src1 | src2) & 1)
+        dest++;
+    T0 = dest;
+    FORCE_RET();
+}
+
+NEON_OP(rhadd_u32)
+{
+    uint32_t src1 = T0;
+    uint32_t src2 = T1;
+    uint32_t dest;
+
+    dest = (src1 >> 1) + (src2 >> 1);
+    if ((src1 | src2) & 1)
+        dest++;
+    T0 = dest;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) dest = (src1 - src2) >> 1
+NEON_VOP(hsub_s8, neon_s8, 4)
+NEON_VOP(hsub_u8, neon_u8, 4)
+NEON_VOP(hsub_s16, neon_s16, 2)
+NEON_VOP(hsub_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(hsub_s32)
+{
+    int32_t src1 = T0;
+    int32_t src2 = T1;
+    int32_t dest;
+
+    dest = (src1 >> 1) - (src2 >> 1);
+    if ((~src1) & src2 & 1)
+        dest--;
+    T0 = dest;
+    FORCE_RET();
+}
+
+NEON_OP(hsub_u32)
+{
+    uint32_t src1 = T0;
+    uint32_t src2 = T1;
+    uint32_t dest;
+
+    dest = (src1 >> 1) - (src2 >> 1);
+    if ((~src1) & src2 & 1)
+        dest--;
+    T0 = dest;
+    FORCE_RET();
+}
+
+/* ??? bsl, bif and bit are all the same op, just with the oparands in a
+   differnet order.  It's currently easier to have 3 differnt ops than
+   rearange the operands.  */
+
+/* Bitwise Select.  */
+NEON_OP(bsl)
+{
+    T0 = (T0 & T2) | (T1 & ~T2);
+}
+
+/* Bitwise Insert If True.  */
+NEON_OP(bit)
+{
+    T0 = (T0 & T1) | (T2 & ~T1);
+}
+
+/* Bitwise Insert If False.  */
+NEON_OP(bif)
+{
+    T0 = (T2 & T1) | (T0 & ~T1);
+}
+
+#define NEON_USAT(dest, src1, src2, type) do { \
+    uint32_t tmp = (uint32_t)src1 + (uint32_t)src2; \
+    if (tmp != (type)tmp) { \
+        env->QF = 1; \
+        dest = ~0; \
+    } else { \
+        dest = tmp; \
+    }} while(0)
+#define NEON_FN(dest, src1, src2) NEON_USAT(dest, src1, src2, uint8_t)
+NEON_VOP(qadd_u8, neon_u8, 4)
+#undef NEON_FN
+#define NEON_FN(dest, src1, src2) NEON_USAT(dest, src1, src2, uint16_t)
+NEON_VOP(qadd_u16, neon_u16, 2)
+#undef NEON_FN
+#undef NEON_USAT
+
+#define NEON_SSAT(dest, src1, src2, type) do { \
+    int32_t tmp = (uint32_t)src1 + (uint32_t)src2; \
+    if (tmp != (type)tmp) { \
+        env->QF = 1; \
+        if (src2 > 0) { \
+            tmp = (1 << (sizeof(type) * 8 - 1)) - 1; \
+        } else { \
+            tmp = 1 << (sizeof(type) * 8 - 1); \
+        } \
+    } \
+    dest = tmp; \
+    } while(0)
+#define NEON_FN(dest, src1, src2) NEON_SSAT(dest, src1, src2, int8_t)
+NEON_VOP(qadd_s8, neon_s8, 4)
+#undef NEON_FN
+#define NEON_FN(dest, src1, src2) NEON_SSAT(dest, src1, src2, int16_t)
+NEON_VOP(qadd_s16, neon_s16, 2)
+#undef NEON_FN
+#undef NEON_SSAT
+
+#define NEON_USAT(dest, src1, src2, type) do { \
+    uint32_t tmp = (uint32_t)src1 - (uint32_t)src2; \
+    if (tmp != (type)tmp) { \
+        env->QF = 1; \
+        dest = 0; \
+    } else { \
+        dest = tmp; \
+    }} while(0)
+#define NEON_FN(dest, src1, src2) NEON_USAT(dest, src1, src2, uint8_t)
+NEON_VOP(qsub_u8, neon_u8, 4)
+#undef NEON_FN
+#define NEON_FN(dest, src1, src2) NEON_USAT(dest, src1, src2, uint16_t)
+NEON_VOP(qsub_u16, neon_u16, 2)
+#undef NEON_FN
+#undef NEON_USAT
+
+#define NEON_SSAT(dest, src1, src2, type) do { \
+    int32_t tmp = (uint32_t)src1 - (uint32_t)src2; \
+    if (tmp != (type)tmp) { \
+        env->QF = 1; \
+        if (src2 < 0) { \
+            tmp = (1 << (sizeof(type) * 8 - 1)) - 1; \
+        } else { \
+            tmp = 1 << (sizeof(type) * 8 - 1); \
+        } \
+    } \
+    dest = tmp; \
+    } while(0)
+#define NEON_FN(dest, src1, src2) NEON_SSAT(dest, src1, src2, int8_t)
+NEON_VOP(qsub_s8, neon_s8, 4)
+#undef NEON_FN
+#define NEON_FN(dest, src1, src2) NEON_SSAT(dest, src1, src2, int16_t)
+NEON_VOP(qsub_s16, neon_s16, 2)
+#undef NEON_FN
+#undef NEON_SSAT
+
+#define NEON_FN(dest, src1, src2) dest = (src1 > src2) ? ~0 : 0
+NEON_VOP(cgt_s8, neon_s8, 4)
+NEON_VOP(cgt_u8, neon_u8, 4)
+NEON_VOP(cgt_s16, neon_s16, 2)
+NEON_VOP(cgt_u16, neon_u16, 2)
+NEON_VOP(cgt_s32, neon_s32, 1)
+NEON_VOP(cgt_u32, neon_u32, 1)
+#undef NEON_FN
+
+#define NEON_FN(dest, src1, src2) dest = (src1 >= src2) ? ~0 : 0
+NEON_VOP(cge_s8, neon_s8, 4)
+NEON_VOP(cge_u8, neon_u8, 4)
+NEON_VOP(cge_s16, neon_s16, 2)
+NEON_VOP(cge_u16, neon_u16, 2)
+NEON_VOP(cge_s32, neon_s32, 1)
+NEON_VOP(cge_u32, neon_u32, 1)
+#undef NEON_FN
+
+#define NEON_FN(dest, src1, src2) do { \
+    int8_t tmp; \
+    tmp = (int8_t)src2; \
+    if (tmp < 0) { \
+        dest = src1 >> -tmp; \
+    } else { \
+        dest = src1 << tmp; \
+    }} while (0)
+NEON_VOP(shl_s8, neon_s8, 4)
+NEON_VOP(shl_u8, neon_u8, 4)
+NEON_VOP(shl_s16, neon_s16, 2)
+NEON_VOP(shl_u16, neon_u16, 2)
+NEON_VOP(shl_s32, neon_s32, 1)
+NEON_VOP(shl_u32, neon_u32, 1)
+#undef NEON_FN
+
+NEON_OP(shl_u64)
+{
+    int8_t shift = T2;
+    uint64_t val = T0 | ((uint64_t)T1 << 32);
+    if (shift < 0) {
+        val >>= -shift;
+    } else {
+        val <<= shift;
+    }
+    T0 = val;
+    T1 = val >> 32;
+    FORCE_RET();
+}
+
+NEON_OP(shl_s64)
+{
+    int8_t shift = T2;
+    int64_t val = T0 | ((uint64_t)T1 << 32);
+    if (shift < 0) {
+        val >>= -shift;
+    } else {
+        val <<= shift;
+    }
+    T0 = val;
+    T1 = val >> 32;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) do { \
+    int8_t tmp; \
+    tmp = (int8_t)src1; \
+    if (tmp < 0) { \
+        dest = (src2 + (1 << (-1 - tmp))) >> -tmp; \
+    } else { \
+        dest = src2 << tmp; \
+    }} while (0)
+
+NEON_VOP(rshl_s8, neon_s8, 4)
+NEON_VOP(rshl_u8, neon_u8, 4)
+NEON_VOP(rshl_s16, neon_s16, 2)
+NEON_VOP(rshl_u16, neon_u16, 2)
+NEON_VOP(rshl_s32, neon_s32, 1)
+NEON_VOP(rshl_u32, neon_u32, 1)
+#undef NEON_FN
+
+NEON_OP(rshl_u64)
+{
+    int8_t shift = T2;
+    uint64_t val = T0 | ((uint64_t)T1 << 32);
+    if (shift < 0) {
+        val = (val + ((uint64_t)1 << (-1 - shift))) >> -shift;
+        val >>= -shift;
+    } else {
+        val <<= shift;
+    }
+    T0 = val;
+    T1 = val >> 32;
+    FORCE_RET();
+}
+
+NEON_OP(rshl_s64)
+{
+    int8_t shift = T2;
+    int64_t val = T0 | ((uint64_t)T1 << 32);
+    if (shift < 0) {
+        val = (val + ((int64_t)1 << (-1 - shift))) >> -shift;
+    } else {
+        val <<= shift;
+    }
+    T0 = val;
+    T1 = val >> 32;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) do { \
+    int8_t tmp; \
+    tmp = (int8_t)src1; \
+    if (tmp < 0) { \
+        dest = src2 >> -tmp; \
+    } else { \
+        dest = src2 << tmp; \
+        if ((dest >> tmp) != src2) { \
+            env->QF = 1; \
+            dest = ~0; \
+        } \
+    }} while (0)
+NEON_VOP(qshl_s8, neon_s8, 4)
+NEON_VOP(qshl_s16, neon_s16, 2)
+NEON_VOP(qshl_s32, neon_s32, 1)
+#undef NEON_FN
+
+NEON_OP(qshl_s64)
+{
+    int8_t shift = T2;
+    int64_t val = T0 | ((uint64_t)T1 << 32);
+    if (shift < 0) {
+        val >>= -shift;
+    } else {
+        int64_t tmp = val;
+        val <<= shift;
+        if ((val >> shift) != tmp) {
+            env->QF = 1;
+            val = (tmp >> 63) ^ 0x7fffffffffffffffULL;
+        }
+    }
+    T0 = val;
+    T1 = val >> 32;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) do { \
+    int8_t tmp; \
+    tmp = (int8_t)src1; \
+    if (tmp < 0) { \
+        dest = src2 >> -tmp; \
+    } else { \
+        dest = src2 << tmp; \
+        if ((dest >> tmp) != src2) { \
+            env->QF = 1; \
+            dest = src2 >> 31; \
+        } \
+    }} while (0)
+NEON_VOP(qshl_u8, neon_u8, 4)
+NEON_VOP(qshl_u16, neon_u16, 2)
+NEON_VOP(qshl_u32, neon_u32, 1)
+#undef NEON_FN
+
+NEON_OP(qshl_u64)
+{
+    int8_t shift = T2;
+    uint64_t val = T0 | ((uint64_t)T1 << 32);
+    if (shift < 0) {
+        val >>= -shift;
+    } else {
+        uint64_t tmp = val;
+        val <<= shift;
+        if ((val >> shift) != tmp) {
+            env->QF = 1;
+            val = ~(uint64_t)0;
+        }
+    }
+    T0 = val;
+    T1 = val >> 32;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) do { \
+    int8_t tmp; \
+    tmp = (int8_t)src1; \
+    if (tmp < 0) { \
+        dest = (src2 + (1 << (-1 - tmp))) >> -tmp; \
+    } else { \
+        dest = src2 << tmp; \
+        if ((dest >> tmp) != src2) { \
+            dest = ~0; \
+        } \
+    }} while (0)
+NEON_VOP(qrshl_s8, neon_s8, 4)
+NEON_VOP(qrshl_s16, neon_s16, 2)
+NEON_VOP(qrshl_s32, neon_s32, 1)
+#undef NEON_FN
+
+#define NEON_FN(dest, src1, src2) do { \
+    int8_t tmp; \
+    tmp = (int8_t)src1; \
+    if (tmp < 0) { \
+        dest = (src2 + (1 << (-1 - tmp))) >> -tmp; \
+    } else { \
+        dest = src2 << tmp; \
+        if ((dest >> tmp) != src2) { \
+            env->QF = 1; \
+            dest = src2 >> 31; \
+        } \
+    }} while (0)
+NEON_VOP(qrshl_u8, neon_u8, 4)
+NEON_VOP(qrshl_u16, neon_u16, 2)
+NEON_VOP(qrshl_u32, neon_u32, 1)
+#undef NEON_FN
+
+#define NEON_FN(dest, src1, src2) dest = (src1 > src2) ? src1 : src2
+NEON_VOP(max_s8, neon_s8, 4)
+NEON_VOP(max_u8, neon_u8, 4)
+NEON_VOP(max_s16, neon_s16, 2)
+NEON_VOP(max_u16, neon_u16, 2)
+NEON_VOP(max_s32, neon_s32, 1)
+NEON_VOP(max_u32, neon_u32, 1)
+NEON_POP(pmax_s8, neon_s8, 4)
+NEON_POP(pmax_u8, neon_u8, 4)
+NEON_POP(pmax_s16, neon_s16, 2)
+NEON_POP(pmax_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(max_f32)
+{
+    float32 f0 = vfp_itos(T0);
+    float32 f1 = vfp_itos(T1);
+    T0 = (float32_compare_quiet(f0, f1, NFS) == 1) ? T0 : T1;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) dest = (src1 < src2) ? src1 : src2
+NEON_VOP(min_s8, neon_s8, 4)
+NEON_VOP(min_u8, neon_u8, 4)
+NEON_VOP(min_s16, neon_s16, 2)
+NEON_VOP(min_u16, neon_u16, 2)
+NEON_VOP(min_s32, neon_s32, 1)
+NEON_VOP(min_u32, neon_u32, 1)
+NEON_POP(pmin_s8, neon_s8, 4)
+NEON_POP(pmin_u8, neon_u8, 4)
+NEON_POP(pmin_s16, neon_s16, 2)
+NEON_POP(pmin_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(min_f32)
+{
+    float32 f0 = vfp_itos(T0);
+    float32 f1 = vfp_itos(T1);
+    T0 = (float32_compare_quiet(f0, f1, NFS) == -1) ? T0 : T1;
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) \
+    dest = (src1 > src2) ? (src1 - src2) : (src2 - src1)
+NEON_VOP(abd_s8, neon_s8, 4)
+NEON_VOP(abd_u8, neon_u8, 4)
+NEON_VOP(abd_s16, neon_s16, 2)
+NEON_VOP(abd_u16, neon_u16, 2)
+NEON_VOP(abd_s32, neon_s32, 1)
+NEON_VOP(abd_u32, neon_u32, 1)
+#undef NEON_FN
+
+NEON_OP(abd_f32)
+{
+    float32 f0 = vfp_itos(T0);
+    float32 f1 = vfp_itos(T1);
+    T0 = vfp_stoi((float32_compare_quiet(f0, f1, NFS) == 1)
+                  ? float32_sub(f0, f1, NFS)
+                  : float32_sub(f1, f0, NFS));
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) dest = src1 + src2
+NEON_VOP(add_u8, neon_u8, 4)
+NEON_VOP(add_u16, neon_u16, 2)
+NEON_POP(padd_u8, neon_u8, 4)
+NEON_POP(padd_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(add_f32)
+{
+    T0 = vfp_stoi(float32_add(vfp_itos(T0), vfp_itos(T1), NFS));
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) dest = src1 - src2
+NEON_VOP(sub_u8, neon_u8, 4)
+NEON_VOP(sub_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(sub_f32)
+{
+    T0 = vfp_stoi(float32_sub(vfp_itos(T0), vfp_itos(T1), NFS));
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) dest = src2 - src1
+NEON_VOP(rsb_u8, neon_u8, 4)
+NEON_VOP(rsb_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(rsb_f32)
+{
+    T0 = vfp_stoi(float32_sub(vfp_itos(T1), vfp_itos(T0), NFS));
+    FORCE_RET();
+}
+
+#define NEON_FN(dest, src1, src2) dest = src1 * src2
+NEON_VOP(mul_u8, neon_u8, 4)
+NEON_VOP(mul_u16, neon_u16, 2)
+#undef NEON_FN
+
+NEON_OP(mul_f32)
+{
+    T0 = vfp_stoi(float32_mul(vfp_itos(T0), vfp_itos(T1), NFS));
+    FORCE_RET();
+}
+
+NEON_OP(mul_p8)
+{
+    T0 = helper_neon_mul_p8(T0, T1);
+}
+
+#define NEON_FN(dest, src1, src2) dest = (src1 & src2) ? -1 : 0
+NEON_VOP(tst_u8, neon_u8, 4)
+NEON_VOP(tst_u16, neon_u16, 2)
+NEON_VOP(tst_u32, neon_u32, 1)
+#undef NEON_FN
+
+#define NEON_FN(dest, src1, src2) dest = (src1 == src2) ? -1 : 0
+NEON_VOP(ceq_u8, neon_u8, 4)
+NEON_VOP(ceq_u16, neon_u16, 2)
+NEON_VOP(ceq_u32, neon_u32, 1)
+#undef NEON_FN
+
+#define NEON_QDMULH16(dest, src1, src2, round) do { \
+    uint32_t tmp = (int32_t)(int16_t) src1 * (int16_t) src2; \
+    if ((tmp ^ (tmp << 1)) & SIGNBIT) { \
+        env->QF = 1; \
+        tmp = (tmp >> 31) ^ ~SIGNBIT; \
+    } \
+    tmp <<= 1; \
+    if (round) { \
+        int32_t old = tmp; \
+        tmp += 1 << 15; \
+        if ((int32_t)tmp < old) { \
+            env->QF = 1; \
+            tmp = SIGNBIT - 1; \
+        } \
+    } \
+    dest = tmp >> 16; \
+    } while(0)
+#define NEON_FN(dest, src1, src2) NEON_QDMULH16(dest, src1, src2, 0)
+NEON_VOP(qdmulh_s16, neon_s16, 2)
+#undef NEON_FN
+#define NEON_FN(dest, src1, src2) NEON_QDMULH16(dest, src1, src2, 1)
+NEON_VOP(qrdmulh_s16, neon_s16, 2)
+#undef NEON_FN
+#undef NEON_QDMULH16
+
+#define SIGNBIT64 ((uint64_t)1 << 63)
+#define NEON_QDMULH32(dest, src1, src2, round) do { \
+    uint64_t tmp = (int64_t)(int32_t) src1 * (int32_t) src2; \
+    if ((tmp ^ (tmp << 1)) & SIGNBIT64) { \
+        env->QF = 1; \
+        tmp = (tmp >> 63) ^ ~SIGNBIT64; \
+    } else { \
+        tmp <<= 1; \
+    } \
+    if (round) { \
+        int64_t old = tmp; \
+        tmp += (int64_t)1 << 31; \
+        if ((int64_t)tmp < old) { \
+            env->QF = 1; \
+            tmp = SIGNBIT64 - 1; \
+        } \
+    } \
+    dest = tmp >> 32; \
+    } while(0)
+#define NEON_FN(dest, src1, src2) NEON_QDMULH32(dest, src1, src2, 0)
+NEON_VOP(qdmulh_s32, neon_s32, 1)
+#undef NEON_FN
+#define NEON_FN(dest, src1, src2) NEON_QDMULH32(dest, src1, src2, 1)
+NEON_VOP(qrdmulh_s32, neon_s32, 1)
+#undef NEON_FN
+#undef NEON_QDMULH32
+
+NEON_OP(recps_f32)
+{
+    T0 = vfp_stoi(helper_recps_f32(vfp_itos(T0), vfp_itos(T1)));
+    FORCE_RET();
+}
+
+NEON_OP(rsqrts_f32)
+{
+    T0 = vfp_stoi(helper_rsqrts_f32(vfp_itos(T0), vfp_itos(T1)));
+    FORCE_RET();
+}
+
+/* Floating point comparisons produce an integer result.  */
+#define NEON_VOP_FCMP(name, cmp) \
+NEON_OP(name) \
+{ \
+    if (float32_compare_quiet(vfp_itos(T0), vfp_itos(T1), NFS) cmp 0) \
+        T0 = -1; \
+    else \
+        T0 = 0; \
+    FORCE_RET(); \
+}
+
+NEON_VOP_FCMP(ceq_f32, ==)
+NEON_VOP_FCMP(cge_f32, >=)
+NEON_VOP_FCMP(cgt_f32, >)
+
+NEON_OP(acge_f32)
+{
+    float32 f0 = float32_abs(vfp_itos(T0));
+    float32 f1 = float32_abs(vfp_itos(T1));
+    T0 = (float32_compare_quiet(f0, f1,NFS) >= 0) ? -1 : 0;
+    FORCE_RET();
+}
+
+NEON_OP(acgt_f32)
+{
+    float32 f0 = float32_abs(vfp_itos(T0));
+    float32 f1 = float32_abs(vfp_itos(T1));
+    T0 = (float32_compare_quiet(f0, f1, NFS) > 0) ? -1 : 0;
+    FORCE_RET();
+}
+
+/* Narrowing instructions.  The named type is the destination type.  */
+NEON_OP(narrow_u8)
+{
+    T0 = (T0 & 0xff) | ((T0 >> 8) & 0xff00)
+         | ((T1 << 16) & 0xff0000) | (T1 << 24);
+    FORCE_RET();
+}
+
+NEON_OP(narrow_sat_u8)
+{
+    neon_u16 src;
+    neon_u8 dest;
+#define SAT8(d, s) \
+    if (s > 0xff) { \
+        d = 0xff; \
+        env->QF = 1; \
+    } else  { \
+        d = s; \
+    }
+
+    NEON_UNPACK(neon_u16, src, T0);
+    SAT8(dest.v1, src.v1);
+    SAT8(dest.v2, src.v2);
+    NEON_UNPACK(neon_u16, src, T1);
+    SAT8(dest.v3, src.v1);
+    SAT8(dest.v4, src.v2);
+    NEON_PACK(neon_u8, T0, dest);
+    FORCE_RET();
+#undef SAT8
+}
+
+NEON_OP(narrow_sat_s8)
+{
+    neon_s16 src;
+    neon_s8 dest;
+#define SAT8(d, s) \
+    if (s != (uint8_t)s) { \
+        d = (s >> 15) ^ 0x7f; \
+        env->QF = 1; \
+    } else  { \
+        d = s; \
+    }
+
+    NEON_UNPACK(neon_s16, src, T0);
+    SAT8(dest.v1, src.v1);
+    SAT8(dest.v2, src.v2);
+    NEON_UNPACK(neon_s16, src, T1);
+    SAT8(dest.v3, src.v1);
+    SAT8(dest.v4, src.v2);
+    NEON_PACK(neon_s8, T0, dest);
+    FORCE_RET();
+#undef SAT8
+}
+
+NEON_OP(narrow_u16)
+{
+    T0 = (T0 & 0xffff) | (T1 << 16);
+}
+
+NEON_OP(narrow_sat_u16)
+{
+    if (T0 > 0xffff) {
+        T0 = 0xffff;
+        env->QF = 1;
+    }
+    if (T1 > 0xffff) {
+        T1 = 0xffff;
+        env->QF = 1;
+    }
+    T0 |= T1 << 16;
+    FORCE_RET();
+}
+
+NEON_OP(narrow_sat_s16)
+{
+    if ((int32_t)T0 != (int16_t)T0) {
+        T0 = ((int32_t)T0 >> 31) ^ 0x7fff;
+        env->QF = 1;
+    }
+    if ((int32_t)T1 != (int16_t) T1) {
+        T1 = ((int32_t)T1 >> 31) ^ 0x7fff;
+        env->QF = 1;
+    }
+    T0 = (uint16_t)T0 | (T1 << 16);
+    FORCE_RET();
+}
+
+NEON_OP(narrow_sat_u32)
+{
+    if (T1) {
+        T0 = 0xffffffffu;
+        env->QF = 1;
+    }
+    FORCE_RET();
+}
+
+NEON_OP(narrow_sat_s32)
+{
+    int32_t sign = (int32_t)T1 >> 31;
+
+    if ((int32_t)T1 != sign) {
+        T0 = sign ^ 0x7fffffff;
+        env->QF = 1;
+    }
+    FORCE_RET();
+}
+
+/* Narrowing instructions.  Named type is the narrow type.  */
+NEON_OP(narrow_high_u8)
+{
+    T0 = ((T0 >> 8) & 0xff) | ((T0 >> 16) & 0xff00)
+        | ((T1 << 8) & 0xff0000) | (T1 & 0xff000000);
+    FORCE_RET();
+}
+
+NEON_OP(narrow_high_u16)
+{
+    T0 = (T0 >> 16) | (T1 & 0xffff0000);
+    FORCE_RET();
+}
+
+NEON_OP(narrow_high_round_u8)
+{
+    T0 = (((T0 + 0x80) >> 8) & 0xff) | (((T0 + 0x800000) >> 16) & 0xff00)
+        | (((T1 + 0x80) << 8) & 0xff0000) | ((T1 + 0x800000) & 0xff000000);
+    FORCE_RET();
+}
+
+NEON_OP(narrow_high_round_u16)
+{
+    T0 = ((T0 + 0x8000) >> 16) | ((T1 + 0x8000) & 0xffff0000);
+    FORCE_RET();
+}
+
+NEON_OP(narrow_high_round_u32)
+{
+    if (T0 >= 0x80000000u)
+        T0 = T1 + 1;
+    else
+        T0 = T1;
+    FORCE_RET();
+}
+
+/* Widening instructions.  Named type is source type.  */
+NEON_OP(widen_s8)
+{
+    uint32_t src;
+
+    src = T0;
+    T0 = (uint16_t)(int8_t)src | ((int8_t)(src >> 8) << 16);
+    T1 = (uint16_t)(int8_t)(src >> 16) | ((int8_t)(src >> 24) << 16);
+}
+
+NEON_OP(widen_u8)
+{
+    T1 = ((T0 >> 8) & 0xff0000) | ((T0 >> 16) & 0xff);
+    T0 = ((T0 << 8) & 0xff0000) | (T0 & 0xff);
+}
+
+NEON_OP(widen_s16)
+{
+    int32_t src;
+
+    src = T0;
+    T0 = (int16_t)src;
+    T1 = src >> 16;
+}
+
+NEON_OP(widen_u16)
+{
+    T1 = T0 >> 16;
+    T0 &= 0xffff;
+}
+
+NEON_OP(widen_s32)
+{
+    T1 = (int32_t)T0 >> 31;
+    FORCE_RET();
+}
+
+NEON_OP(widen_high_u8)
+{
+    T1 = (T0 & 0xff000000) | ((T0 >> 8) & 0xff00);
+    T0 = ((T0 << 16) & 0xff000000) | ((T0 << 8) & 0xff00);
+}
+
+NEON_OP(widen_high_u16)
+{
+    T1 = T0 & 0xffff0000;
+    T0 <<= 16;
+}
+
+/* Long operations.  The type is the wide type.  */
+NEON_OP(shll_u16)
+{
+    int shift = PARAM1;
+    uint32_t mask;
+
+    mask = 0xffff >> (16 - shift);
+    mask |= mask << 16;
+    mask = ~mask;
+
+    T0 = (T0 << shift) & mask;
+    T1 = (T1 << shift) & mask;
+    FORCE_RET();
+}
+
+NEON_OP(shll_u64)
+{
+    int shift = PARAM1;
+
+    T1 <<= shift;
+    T1 |= T0 >> (32 - shift);
+    T0 <<= shift;
+    FORCE_RET();
+}
+
+NEON_OP(addl_u16)
+{
+    uint32_t tmp;
+    uint32_t high;
+
+    tmp = env->vfp.scratch[0];
+    high = (T0 >> 16) + (tmp >> 16);
+    T0 = (uint16_t)(T0 + tmp);
+    T0 |= (high << 16);
+    tmp = env->vfp.scratch[1];
+    high = (T1 >> 16) + (tmp >> 16);
+    T1 = (uint16_t)(T1 + tmp);
+    T1 |= (high << 16);
+    FORCE_RET();
+}
+
+NEON_OP(addl_u32)
+{
+    T0 += env->vfp.scratch[0];
+    T1 += env->vfp.scratch[1];
+    FORCE_RET();
+}
+
+NEON_OP(addl_u64)
+{
+    uint64_t tmp;
+    tmp = T0 | ((uint64_t)T1 << 32);
+    tmp += env->vfp.scratch[0];
+    tmp += (uint64_t)env->vfp.scratch[1] << 32;
+    T0 = tmp;
+    T1 = tmp >> 32;
+    FORCE_RET();
+}
+
+NEON_OP(subl_u16)
+{
+    uint32_t tmp;
+    uint32_t high;
+
+    tmp = env->vfp.scratch[0];
+    high = (T0 >> 16) - (tmp >> 16);
+    T0 = (uint16_t)(T0 - tmp);
+    T0 |= (high << 16);
+    tmp = env->vfp.scratch[1];
+    high = (T1 >> 16) - (tmp >> 16);
+    T1 = (uint16_t)(T1 - tmp);
+    T1 |= (high << 16);
+    FORCE_RET();
+}
+
+NEON_OP(subl_u32)
+{
+    T0 -= env->vfp.scratch[0];
+    T1 -= env->vfp.scratch[1];
+    FORCE_RET();
+}
+
+NEON_OP(subl_u64)
+{
+    uint64_t tmp;
+    tmp = T0 | ((uint64_t)T1 << 32);
+    tmp -= env->vfp.scratch[0];
+    tmp -= (uint64_t)env->vfp.scratch[1] << 32;
+    T0 = tmp;
+    T1 = tmp >> 32;
+    FORCE_RET();
+}
+
+#define DO_ABD(dest, x, y, type) do { \
+    type tmp_x = x; \
+    type tmp_y = y; \
+    dest = ((tmp_x > tmp_y) ? tmp_x - tmp_y : tmp_y - tmp_x); \
+    } while(0)
+
+NEON_OP(abdl_u16)
+{
+    uint32_t tmp;
+    uint32_t low;
+    uint32_t high;
+
+    DO_ABD(low, T0, T1, uint8_t);
+    DO_ABD(tmp, T0 >> 8, T1 >> 8, uint8_t);
+    low |= tmp << 16;
+    DO_ABD(high, T0 >> 16, T1 >> 16, uint8_t);
+    DO_ABD(tmp, T0 >> 24, T1 >> 24, uint8_t);
+    high |= tmp << 16;
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(abdl_s16)
+{
+    uint32_t tmp;
+    uint32_t low;
+    uint32_t high;
+
+    DO_ABD(low, T0, T1, int8_t);
+    DO_ABD(tmp, T0 >> 8, T1 >> 8, int8_t);
+    low |= tmp << 16;
+    DO_ABD(high, T0 >> 16, T1 >> 16, int8_t);
+    DO_ABD(tmp, T0 >> 24, T1 >> 24, int8_t);
+    high |= tmp << 16;
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(abdl_u32)
+{
+    uint32_t low;
+    uint32_t high;
+
+    DO_ABD(low, T0, T1, uint16_t);
+    DO_ABD(high, T0 >> 16, T1 >> 16, uint16_t);
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(abdl_s32)
+{
+    uint32_t low;
+    uint32_t high;
+
+    DO_ABD(low, T0, T1, int16_t);
+    DO_ABD(high, T0 >> 16, T1 >> 16, int16_t);
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(abdl_u64)
+{
+    DO_ABD(T0, T0, T1, uint32_t);
+    T1 = 0;
+}
+
+NEON_OP(abdl_s64)
+{
+    DO_ABD(T0, T0, T1, int32_t);
+    T1 = 0;
+}
+#undef DO_ABD
+
+/* Widening multiple. Named type is the source type.  */
+#define DO_MULL(dest, x, y, type1, type2) do { \
+    type1 tmp_x = x; \
+    type1 tmp_y = y; \
+    dest = (type2)((type2)tmp_x * (type2)tmp_y); \
+    } while(0)
+
+NEON_OP(mull_u8)
+{
+    uint32_t tmp;
+    uint32_t low;
+    uint32_t high;
+
+    DO_MULL(low, T0, T1, uint8_t, uint16_t);
+    DO_MULL(tmp, T0 >> 8, T1 >> 8, uint8_t, uint16_t);
+    low |= tmp << 16;
+    DO_MULL(high, T0 >> 16, T1 >> 16, uint8_t, uint16_t);
+    DO_MULL(tmp, T0 >> 24, T1 >> 24, uint8_t, uint16_t);
+    high |= tmp << 16;
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(mull_s8)
+{
+    uint32_t tmp;
+    uint32_t low;
+    uint32_t high;
+
+    DO_MULL(low, T0, T1, int8_t, uint16_t);
+    DO_MULL(tmp, T0 >> 8, T1 >> 8, int8_t, uint16_t);
+    low |= tmp << 16;
+    DO_MULL(high, T0 >> 16, T1 >> 16, int8_t, uint16_t);
+    DO_MULL(tmp, T0 >> 24, T1 >> 24, int8_t, uint16_t);
+    high |= tmp << 16;
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(mull_u16)
+{
+    uint32_t low;
+    uint32_t high;
+
+    DO_MULL(low, T0, T1, uint16_t, uint32_t);
+    DO_MULL(high, T0 >> 16, T1 >> 16, uint16_t, uint32_t);
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(mull_s16)
+{
+    uint32_t low;
+    uint32_t high;
+
+    DO_MULL(low, T0, T1, int16_t, uint32_t);
+    DO_MULL(high, T0 >> 16, T1 >> 16, int16_t, uint32_t);
+    T0 = low;
+    T1 = high;
+    FORCE_RET();
+}
+
+NEON_OP(addl_saturate_s32)
+{
+    uint32_t tmp;
+    uint32_t res;
+
+    tmp = env->vfp.scratch[0];
+    res = T0 + tmp;
+    if (((res ^ T0) & SIGNBIT) && !((T0 ^ tmp) & SIGNBIT)) {
+        env->QF = 1;
+        T0 = (T0 >> 31) ^ 0x7fffffff;
+    } else {
+      T0 = res;
+    }
+    tmp = env->vfp.scratch[1];
+    res = T1 + tmp;
+    if (((res ^ T1) & SIGNBIT) && !((T1 ^ tmp) & SIGNBIT)) {
+        env->QF = 1;
+        T1 = (T1 >> 31) ^ 0x7fffffff;
+    } else {
+      T1 = res;
+    }
+    FORCE_RET();
+}
+
+NEON_OP(addl_saturate_s64)
+{
+    uint64_t src1;
+    uint64_t src2;
+    uint64_t res;
+
+    src1 = T0 + ((uint64_t)T1 << 32);
+    src2 = env->vfp.scratch[0] + ((uint64_t)env->vfp.scratch[1] << 32);
+    res = src1 + src2;
+    if (((res ^ src1) & SIGNBIT64) && !((src1 ^ src2) & SIGNBIT64)) {
+        env->QF = 1;
+        T0 = ~(int64_t)src1 >> 63;
+        T1 = T0 ^ 0x80000000;
+    } else {
+      T0 = res;
+      T1 = res >> 32;
+    }
+    FORCE_RET();
+}
+
+NEON_OP(addl_saturate_u64)
+{
+    uint64_t src1;
+    uint64_t src2;
+    uint64_t res;
+
+    src1 = T0 + ((uint64_t)T1 << 32);
+    src2 = env->vfp.scratch[0] + ((uint64_t)env->vfp.scratch[1] << 32);
+    res = src1 + src2;
+    if (res < src1) {
+        env->QF = 1;
+        T0 = 0xffffffff;
+        T1 = 0xffffffff;
+    } else {
+      T0 = res;
+      T1 = res >> 32;
+    }
+    FORCE_RET();
+}
+
+NEON_OP(subl_saturate_s64)
+{
+    uint64_t src1;
+    uint64_t src2;
+    uint64_t res;
+
+    src1 = T0 + ((uint64_t)T1 << 32);
+    src2 = env->vfp.scratch[0] + ((uint64_t)env->vfp.scratch[1] << 32);
+    res = src1 - src2;
+    if (((res ^ src1) & SIGNBIT64) && ((src1 ^ src2) & SIGNBIT64)) {
+        env->QF = 1;
+        T0 = ~(int64_t)src1 >> 63;
+        T1 = T0 ^ 0x80000000;
+    } else {
+      T0 = res;
+      T1 = res >> 32;
+    }
+    FORCE_RET();
+}
+
+NEON_OP(subl_saturate_u64)
+{
+    uint64_t src1;
+    uint64_t src2;
+    uint64_t res;
+
+    src1 = T0 + ((uint64_t)T1 << 32);
+    src2 = env->vfp.scratch[0] + ((uint64_t)env->vfp.scratch[1] << 32);
+    if (src1 < src2) {
+        env->QF = 1;
+        T0 = 0;
+        T1 = 0;
+    } else {
+      res = src1 - src2;
+      T0 = res;
+      T1 = res >> 32;
+    }
+    FORCE_RET();
+}
+
+NEON_OP(negl_u16)
+{
+    uint32_t tmp;
+    tmp = T0 >> 16;
+    tmp = -tmp;
+    T0 = (-T0 & 0xffff) | (tmp << 16);
+    tmp = T1 >> 16;
+    tmp = -tmp;
+    T1 = (-T1 & 0xffff) | (tmp << 16);
+    FORCE_RET();
+}
+
+NEON_OP(negl_u32)
+{
+    T0 = -T0;
+    T1 = -T1;
+    FORCE_RET();
+}
+
+NEON_OP(negl_u64)
+{
+    uint64_t val;
+
+    val = T0 | ((uint64_t)T1 << 32);
+    val = -val;
+    T0 = val;
+    T1 = val >> 32;
+    FORCE_RET();
+}
+
+/* Scalar operations.  */
+NEON_OP(dup_low16)
+{
+    T0 = (T0 & 0xffff) | (T0 << 16);
+    FORCE_RET();
+}
+
+NEON_OP(dup_high16)
+{
+    T0 = (T0 >> 16) | (T0 & 0xffff0000);
+    FORCE_RET();
+}
+
+/* Helper for VEXT */
+NEON_OP(extract)
+{
+    int shift = PARAM1;
+    T0 = (T0 >> shift) | (T1 << (32 - shift));
+    FORCE_RET();
+}
+
+/* Pairwise add long.  Named type is source type.  */
+NEON_OP(paddl_s8)
+{
+    int8_t src1;
+    int8_t src2;
+    uint16_t result;
+    src1 = T0 >> 24;
+    src2 = T0 >> 16;
+    result = (uint16_t)src1 + src2;
+    src1 = T0 >> 8;
+    src2 = T0;
+    T0 = (uint16_t)((uint16_t)src1 + src2) | ((uint32_t)result << 16);
+    FORCE_RET();
+}
+
+NEON_OP(paddl_u8)
+{
+    uint8_t src1;
+    uint8_t src2;
+    uint16_t result;
+    src1 = T0 >> 24;
+    src2 = T0 >> 16;
+    result = (uint16_t)src1 + src2;
+    src1 = T0 >> 8;
+    src2 = T0;
+    T0 = (uint16_t)((uint16_t)src1 + src2) | ((uint32_t)result << 16);
+    FORCE_RET();
+}
+
+NEON_OP(paddl_s16)
+{
+    T0 = (uint32_t)(int16_t)T0 + (uint32_t)(int16_t)(T0 >> 16);
+    FORCE_RET();
+}
+
+NEON_OP(paddl_u16)
+{
+    T0 = (uint32_t)(uint16_t)T0 + (uint32_t)(uint16_t)(T0 >> 16);
+    FORCE_RET();
+}
+
+NEON_OP(paddl_s32)
+{
+    int64_t tmp;
+    tmp = (int64_t)(int32_t)T0 + (int64_t)(int32_t)T1;
+    T0 = tmp;
+    T1 = tmp >> 32;
+    FORCE_RET();
+}
+
+NEON_OP(paddl_u32)
+{
+    uint64_t tmp;
+    tmp = (uint64_t)T0 + (uint64_t)T1;
+    T0 = tmp;
+    T1 = tmp >> 32;
+    FORCE_RET();
+}
+
+/* Count Leading Sign/Zero Bits.  */
+static inline int do_clz8(uint8_t x)
+{
+    int n;
+    for (n = 8; x; n--)
+        x >>= 1;
+    return n;
+}
+
+static inline int do_clz16(uint16_t x)
+{
+    int n;
+    for (n = 16; x; n--)
+        x >>= 1;
+    return n;
+}
+
+NEON_OP(clz_u8)
+{
+    uint32_t result;
+    uint32_t tmp;
+
+    tmp = T0;
+    result = do_clz8(tmp);
+    result |= do_clz8(tmp >> 8) << 8;
+    result |= do_clz8(tmp >> 16) << 16;
+    result |= do_clz8(tmp >> 24) << 24;
+    T0 = result;
+    FORCE_RET();
+}
+
+NEON_OP(clz_u16)
+{
+    uint32_t result;
+    uint32_t tmp;
+    tmp = T0;
+    result = do_clz16(tmp);
+    result |= do_clz16(tmp >> 16) << 16;
+    T0 = result;
+    FORCE_RET();
+}
+
+NEON_OP(cls_s8)
+{
+    uint32_t result;
+    int8_t tmp;
+    tmp = T0;
+    result = do_clz8((tmp < 0) ? ~tmp : tmp) - 1;
+    tmp = T0 >> 8;
+    result |= (do_clz8((tmp < 0) ? ~tmp : tmp) - 1) << 8;
+    tmp = T0 >> 16;
+    result |= (do_clz8((tmp < 0) ? ~tmp : tmp) - 1) << 16;
+    tmp = T0 >> 24;
+    result |= (do_clz8((tmp < 0) ? ~tmp : tmp) - 1) << 24;
+    T0 = result;
+    FORCE_RET();
+}
+
+NEON_OP(cls_s16)
+{
+    uint32_t result;
+    int16_t tmp;
+    tmp = T0;
+    result = do_clz16((tmp < 0) ? ~tmp : tmp) - 1;
+    tmp = T0 >> 16;
+    result |= (do_clz16((tmp < 0) ? ~tmp : tmp) - 1) << 16;
+    T0 = result;
+    FORCE_RET();
+}
+
+NEON_OP(cls_s32)
+{
+    int count;
+    if ((int32_t)T0 < 0)
+        T0 = ~T0;
+    for (count = 32; T0 > 0; count--)
+        T0 = T0 >> 1;
+    T0 = count - 1;
+    FORCE_RET();
+}
+
+/* Bit count.  */
+NEON_OP(cnt_u8)
+{
+    T0 = (T0 & 0x55555555) + ((T0 >>  1) & 0x55555555);
+    T0 = (T0 & 0x33333333) + ((T0 >>  2) & 0x33333333);
+    T0 = (T0 & 0x0f0f0f0f) + ((T0 >>  4) & 0x0f0f0f0f);
+    FORCE_RET();
+}
+
+/* Saturnating negation.  */
+/* ??? Make these use NEON_VOP1 */
+#define DO_QABS8(x) do { \
+    if (x == (int8_t)0x80) { \
+        x = 0x7f; \
+        env->QF = 1; \
+    } else if (x < 0) { \
+        x = -x; \
+    }} while (0)
+NEON_OP(qabs_s8)
+{
+    neon_s8 vec;
+    NEON_UNPACK(neon_s8, vec, T0);
+    DO_QABS8(vec.v1);
+    DO_QABS8(vec.v2);
+    DO_QABS8(vec.v3);
+    DO_QABS8(vec.v4);
+    NEON_PACK(neon_s8, T0, vec);
+    FORCE_RET();
+}
+#undef DO_QABS8
+
+#define DO_QNEG8(x) do { \
+    if (x == (int8_t)0x80) { \
+        x = 0x7f; \
+        env->QF = 1; \
+    } else { \
+        x = -x; \
+    }} while (0)
+NEON_OP(qneg_s8)
+{
+    neon_s8 vec;
+    NEON_UNPACK(neon_s8, vec, T0);
+    DO_QNEG8(vec.v1);
+    DO_QNEG8(vec.v2);
+    DO_QNEG8(vec.v3);
+    DO_QNEG8(vec.v4);
+    NEON_PACK(neon_s8, T0, vec);
+    FORCE_RET();
+}
+#undef DO_QNEG8
+
+#define DO_QABS16(x) do { \
+    if (x == (int16_t)0x8000) { \
+        x = 0x7fff; \
+        env->QF = 1; \
+    } else if (x < 0) { \
+        x = -x; \
+    }} while (0)
+NEON_OP(qabs_s16)
+{
+    neon_s16 vec;
+    NEON_UNPACK(neon_s16, vec, T0);
+    DO_QABS16(vec.v1);
+    DO_QABS16(vec.v2);
+    NEON_PACK(neon_s16, T0, vec);
+    FORCE_RET();
+}
+#undef DO_QABS16
+
+#define DO_QNEG16(x) do { \
+    if (x == (int16_t)0x8000) { \
+        x = 0x7fff; \
+        env->QF = 1; \
+    } else { \
+        x = -x; \
+    }} while (0)
+NEON_OP(qneg_s16)
+{
+    neon_s16 vec;
+    NEON_UNPACK(neon_s16, vec, T0);
+    DO_QNEG16(vec.v1);
+    DO_QNEG16(vec.v2);
+    NEON_PACK(neon_s16, T0, vec);
+    FORCE_RET();
+}
+#undef DO_QNEG16
+
+NEON_OP(qabs_s32)
+{
+    if (T0 == 0x80000000) {
+        T0 = 0x7fffffff;
+        env->QF = 1;
+    } else if ((int32_t)T0 < 0) {
+        T0 = -T0;
+    }
+    FORCE_RET();
+}
+
+NEON_OP(qneg_s32)
+{
+    if (T0 == 0x80000000) {
+        T0 = 0x7fffffff;
+        env->QF = 1;
+    } else {
+        T0 = -T0;
+    }
+    FORCE_RET();
+}
+
+/* Unary opperations */
+#define NEON_FN(dest, src, dummy) dest = (src < 0) ? -src : src
+NEON_VOP1(abs_s8, neon_s8, 4)
+NEON_VOP1(abs_s16, neon_s16, 2)
+NEON_OP(abs_s32)
+{
+    if ((int32_t)T0 < 0)
+        T0 = -T0;
+    FORCE_RET();
+}
+#undef NEON_FN
+
+/* Transpose.  Argument order is rather strange to avoid special casing
+   the tranlation code.
+   On input T0 = rm, T1 = rd.  On output T0 = rd, T1 = rm  */
+NEON_OP(trn_u8)
+{
+    uint32_t rd;
+    uint32_t rm;
+    rd = ((T0 & 0x00ff00ff) << 8) | (T1 & 0x00ff00ff);
+    rm = ((T1 & 0xff00ff00) >> 8) | (T0 & 0xff00ff00);
+    T0 = rd;
+    T1 = rm;
+    FORCE_RET();
+}
+
+NEON_OP(trn_u16)
+{
+    uint32_t rd;
+    uint32_t rm;
+    rd = (T0 << 16) | (T1 & 0xffff);
+    rm = (T1 >> 16) | (T0 & 0xffff0000);
+    T0 = rd;
+    T1 = rm;
+    FORCE_RET();
+}
+
+/* Worker routines for zip and unzip.  */
+NEON_OP(unzip_u8)
+{
+    uint32_t rd;
+    uint32_t rm;
+    rd = (T0 & 0xff) | ((T0 >> 8) & 0xff00)
+         | ((T1 << 16) & 0xff0000) | ((T1 << 8) & 0xff000000);
+    rm = ((T0 >> 8) & 0xff) | ((T0 >> 16) & 0xff00)
+         | ((T1 << 8) & 0xff0000) | (T1 & 0xff000000);
+    T0 = rd;
+    T1 = rm;
+    FORCE_RET();
+}
+
+NEON_OP(zip_u8)
+{
+    uint32_t rd;
+    uint32_t rm;
+    rd = (T0 & 0xff) | ((T1 << 8) & 0xff00)
+         | ((T0 << 16) & 0xff0000) | ((T1 << 24) & 0xff000000);
+    rm = ((T0 >> 16) & 0xff) | ((T1 >> 8) & 0xff00)
+         | ((T0 >> 8) & 0xff0000) | (T1 & 0xff000000);
+    T0 = rd;
+    T1 = rm;
+    FORCE_RET();
+}
+
+NEON_OP(zip_u16)
+{
+    uint32_t tmp;
+
+    tmp = (T0 & 0xffff) | (T1 << 16);
+    T1 = (T1 & 0xffff0000) | (T0 >> 16);
+    T0 = tmp;
+    FORCE_RET();
+}
+
+/* Reciprocal/root estimate.  */
+NEON_OP(recpe_u32)
+{
+    T0 = helper_recpe_u32(T0);
+}
+
+NEON_OP(rsqrte_u32)
+{
+    T0 = helper_rsqrte_u32(T0);
+}
+
+NEON_OP(recpe_f32)
+{
+    FT0s = helper_recpe_f32(FT0s);
+}
+
+NEON_OP(rsqrte_f32)
+{
+    FT0s = helper_rsqrte_f32(FT0s);
+}
+
+/* Table lookup.  This accessed the register file directly.  */
+NEON_OP(tbl)
+{
+    helper_neon_tbl(PARAM1, PARAM2);
+}
+
+NEON_OP(dup_u8)
+{
+    T0 = (T0 >> PARAM1) & 0xff;
+    T0 |= T0 << 8;
+    T0 |= T0 << 16;
+    FORCE_RET();
+}
+
+/* Helpers for element load/store.  */
+NEON_OP(insert_elt)
+{
+    int shift = PARAM1;
+    uint32_t mask = PARAM2;
+    T2 = (T2 & mask) | (T0 << shift);
+    FORCE_RET();
+}
+
+NEON_OP(extract_elt)
+{
+    int shift = PARAM1;
+    uint32_t mask = PARAM2;
+    T0 = (T2 & mask) >> shift;
+    FORCE_RET();
+}
diff --git a/target-arm/translate.c b/target-arm/translate.c
index bb01f2f..9366bf1 100644
--- a/target-arm/translate.c
+++ b/target-arm/translate.c
@@ -2,7 +2,7 @@
  *  ARM translation
  *
  *  Copyright (c) 2003 Fabrice Bellard
- *  Copyright (c) 2005 CodeSourcery, LLC
+ *  Copyright (c) 2005-2007 CodeSourcery
  *  Copyright (c) 2007 OpenedHand, Ltd.
  *
  * This library is free software; you can redistribute it and/or
@@ -29,9 +29,11 @@
 #include "exec-all.h"
 #include "disas.h"
 
-#define ENABLE_ARCH_5J  0
-#define ENABLE_ARCH_6   1
-#define ENABLE_ARCH_6T2 1
+#define ENABLE_ARCH_5J    0
+#define ENABLE_ARCH_6     arm_feature(env, ARM_FEATURE_V6)
+#define ENABLE_ARCH_6K   arm_feature(env, ARM_FEATURE_V6K)
+#define ENABLE_ARCH_6T2   arm_feature(env, ARM_FEATURE_THUMB2)
+#define ENABLE_ARCH_7     arm_feature(env, ARM_FEATURE_V7)
 
 #define ARCH(x) if (!ENABLE_ARCH_##x) goto illegal_op;
 
@@ -43,6 +45,9 @@ typedef struct DisasContext {
     int condjmp;
     /* The label that will be jumped to when the instruction is skipped.  */
     int condlabel;
+    /* Thumb-2 condtional execution bits.  */
+    int condexec_mask;
+    int condexec_cond;
     struct TranslationBlock *tb;
     int singlestep_enabled;
     int thumb;
@@ -58,7 +63,10 @@ typedef struct DisasContext {
 #define IS_USER(s) (s->user)
 #endif
 
-#define DISAS_JUMP_NEXT 4
+/* These instructions trap after executing, so defer them until after the
+   conditional executions state has been updated.  */
+#define DISAS_WFI 4
+#define DISAS_SWI 5
 
 #ifdef USE_DIRECT_JUMP
 #define TBPARAM(x)
@@ -81,6 +89,51 @@ enum {
 
 #include "gen-op.h"
 
+#define PAS_OP(pfx) {  \
+    gen_op_ ## pfx ## add16_T0_T1, \
+    gen_op_ ## pfx ## addsubx_T0_T1, \
+    gen_op_ ## pfx ## subaddx_T0_T1, \
+    gen_op_ ## pfx ## sub16_T0_T1, \
+    gen_op_ ## pfx ## add8_T0_T1, \
+    NULL, \
+    NULL, \
+    gen_op_ ## pfx ## sub8_T0_T1 }
+
+static GenOpFunc *gen_arm_parallel_addsub[8][8] = {
+    {},
+    PAS_OP(s),
+    PAS_OP(q),
+    PAS_OP(sh),
+    {},
+    PAS_OP(u),
+    PAS_OP(uq),
+    PAS_OP(uh),
+};
+#undef PAS_OP
+
+/* For unknown reasons Arm and Thumb-2 use arbitrarily diffenet encodings.  */
+#define PAS_OP(pfx) {  \
+    gen_op_ ## pfx ## add8_T0_T1, \
+    gen_op_ ## pfx ## add16_T0_T1, \
+    gen_op_ ## pfx ## addsubx_T0_T1, \
+    NULL, \
+    gen_op_ ## pfx ## sub8_T0_T1, \
+    gen_op_ ## pfx ## sub16_T0_T1, \
+    gen_op_ ## pfx ## subaddx_T0_T1, \
+    NULL }
+
+static GenOpFunc *gen_thumb2_parallel_addsub[8][8] = {
+    PAS_OP(s),
+    PAS_OP(q),
+    PAS_OP(sh),
+    {},
+    PAS_OP(u),
+    PAS_OP(uq),
+    PAS_OP(uh),
+    {}
+};
+#undef PAS_OP
+
 static GenOpFunc1 *gen_test_cc[14] = {
     gen_op_test_eq,
     gen_op_test_ne,
@@ -275,6 +328,12 @@ static GenOpFunc1 *gen_op_movl_TN_im[3] = {
     gen_op_movl_T2_im,
 };
 
+static GenOpFunc1 *gen_shift_T0_im_thumb_cc[3] = {
+    gen_op_shll_T0_im_thumb_cc,
+    gen_op_shrl_T0_im_thumb_cc,
+    gen_op_sarl_T0_im_thumb_cc,
+};
+
 static GenOpFunc1 *gen_shift_T0_im_thumb[3] = {
     gen_op_shll_T0_im_thumb,
     gen_op_shrl_T0_im_thumb,
@@ -421,6 +480,15 @@ static inline void gen_vfp_##name(int dp) \
         gen_op_vfp_##name##s();           \
 }
 
+#define VFP_OP1(name)                               \
+static inline void gen_vfp_##name(int dp, int arg)  \
+{                                                   \
+    if (dp)                                         \
+        gen_op_vfp_##name##d(arg);                  \
+    else                                            \
+        gen_op_vfp_##name##s(arg);                  \
+}
+
 VFP_OP(add)
 VFP_OP(sub)
 VFP_OP(mul)
@@ -437,9 +505,25 @@ VFP_OP(toui)
 VFP_OP(touiz)
 VFP_OP(tosi)
 VFP_OP(tosiz)
+VFP_OP1(tosh)
+VFP_OP1(tosl)
+VFP_OP1(touh)
+VFP_OP1(toul)
+VFP_OP1(shto)
+VFP_OP1(slto)
+VFP_OP1(uhto)
+VFP_OP1(ulto)
 
 #undef VFP_OP
 
+static inline void gen_vfp_fconst(int dp, uint32_t val)
+{
+    if (dp)
+        gen_op_vfp_fconstd(val);
+    else
+        gen_op_vfp_fconsts(val);
+}
+
 static inline void gen_vfp_ld(DisasContext *s, int dp)
 {
     if (dp)
@@ -469,6 +553,20 @@ vfp_reg_offset (int dp, int reg)
           + offsetof(CPU_DoubleU, l.lower);
     }
 }
+
+/* Return the offset of a 32-bit piece of a NEON register.
+   zero is the least significant end of the register.  */
+static inline long
+neon_reg_offset (int reg, int n)
+{
+    int sreg;
+    sreg = reg * 2 + n;
+    return vfp_reg_offset(0, sreg);
+}
+
+#define NEON_GET_REG(T, reg, n) gen_op_neon_getreg_##T(neon_reg_offset(reg, n))
+#define NEON_SET_REG(T, reg, n) gen_op_neon_setreg_##T(neon_reg_offset(reg, n))
+
 static inline void gen_mov_F0_vreg(int dp, int reg)
 {
     if (dp)
@@ -1582,14 +1680,49 @@ static int disas_cp_insn(CPUState *env, DisasContext *s, uint32_t insn)
     return 0;
 }
 
+static int cp15_user_ok(uint32_t insn)
+{
+    int cpn = (insn >> 16) & 0xf;
+    int cpm = insn & 0xf;
+    int op = ((insn >> 5) & 7) | ((insn >> 18) & 0x38);
+
+    if (cpn == 13 && cpm == 0) {
+        /* TLS register.  */
+        if (op == 2 || (op == 3 && (insn & ARM_CP_RW_BIT)))
+            return 1;
+    }
+    if (cpn == 7) {
+        /* ISB, DSB, DMB.  */
+        if ((cpm == 5 && op == 4)
+                || (cpm == 10 && (op == 4 || op == 5)))
+            return 1;
+    }
+    return 0;
+}
+
 /* Disassemble system coprocessor (cp15) instruction.  Return nonzero if
    instruction is not defined.  */
 static int disas_cp15_insn(CPUState *env, DisasContext *s, uint32_t insn)
 {
     uint32_t rd;
 
-    /* ??? Some cp15 registers are accessible from userspace.  */
-    if (IS_USER(s)) {
+    /* M profile cores use memory mapped registers instead of cp15.  */
+    if (arm_feature(env, ARM_FEATURE_M))
+	return 1;
+
+    if ((insn & (1 << 25)) == 0) {
+        if (insn & (1 << 20)) {
+            /* mrrc */
+            return 1;
+        }
+        /* mcrr.  Used for block cache operations, so implement as no-op.  */
+        return 0;
+    }
+    if ((insn & (1 << 4)) == 0) {
+        /* cdp */
+        return 1;
+    }
+    if (IS_USER(s) && !cp15_user_ok(insn)) {
         return 1;
     }
     if ((insn & 0x0fff0fff) == 0x0e070f90
@@ -1597,8 +1730,7 @@ static int disas_cp15_insn(CPUState *env, DisasContext *s, uint32_t insn)
         /* Wait for interrupt.  */
         gen_op_movl_T0_im((long)s->pc);
         gen_op_movl_reg_TN[0][15]();
-        gen_op_wfi();
-        s->is_jmp = DISAS_JUMP;
+        s->is_jmp = DISAS_WFI;
         return 0;
     }
     rd = (insn >> 12) & 0xf;
@@ -1620,6 +1752,32 @@ static int disas_cp15_insn(CPUState *env, DisasContext *s, uint32_t insn)
     return 0;
 }
 
+#define VFP_REG_SHR(x, n) (((n) > 0) ? (x) >> (n) : (x) << -(n))
+#define VFP_SREG(insn, bigbit, smallbit) \
+  ((VFP_REG_SHR(insn, bigbit - 1) & 0x1e) | (((insn) >> (smallbit)) & 1))
+#define VFP_DREG(reg, insn, bigbit, smallbit) do { \
+    if (arm_feature(env, ARM_FEATURE_VFP3)) { \
+        reg = (((insn) >> (bigbit)) & 0x0f) \
+              | (((insn) >> ((smallbit) - 4)) & 0x10); \
+    } else { \
+        if (insn & (1 << (smallbit))) \
+            return 1; \
+        reg = ((insn) >> (bigbit)) & 0x0f; \
+    }} while (0)
+
+#define VFP_SREG_D(insn) VFP_SREG(insn, 12, 22)
+#define VFP_DREG_D(reg, insn) VFP_DREG(reg, insn, 12, 22)
+#define VFP_SREG_N(insn) VFP_SREG(insn, 16,  7)
+#define VFP_DREG_N(reg, insn) VFP_DREG(reg, insn, 16,  7)
+#define VFP_SREG_M(insn) VFP_SREG(insn,  0,  5)
+#define VFP_DREG_M(reg, insn) VFP_DREG(reg, insn,  0,  5)
+
+static inline int
+vfp_enabled(CPUState * env)
+{
+    return ((env->vfp.xregs[ARM_VFP_FPEXC] & (1 << 30)) != 0);
+}
+
 /* Disassemble a VFP instruction.  Returns nonzero if an error occured
    (ie. an undefined instruction).  */
 static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
@@ -1630,12 +1788,13 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
     if (!arm_feature(env, ARM_FEATURE_VFP))
         return 1;
 
-    if ((env->vfp.xregs[ARM_VFP_FPEXC] & (1 << 30)) == 0) {
-        /* VFP disabled.  Only allow fmxr/fmrx to/from fpexc and fpsid.  */
+    if (!vfp_enabled(env)) {
+        /* VFP disabled.  Only allow fmxr/fmrx to/from some control regs.  */
         if ((insn & 0x0fe00fff) != 0x0ee00a10)
             return 1;
         rn = (insn >> 16) & 0xf;
-        if (rn != 0 && rn != 8)
+        if (rn != ARM_VFP_FPSID && rn != ARM_VFP_FPEXC
+            && rn != ARM_VFP_MVFR1 && rn != ARM_VFP_MVFR0)
             return 1;
     }
     dp = ((insn & 0xf00) == 0xb00);
@@ -1643,44 +1802,129 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
     case 0xe:
         if (insn & (1 << 4)) {
             /* single register transfer */
-            if ((insn & 0x6f) != 0x00)
-                return 1;
             rd = (insn >> 12) & 0xf;
             if (dp) {
-                if (insn & 0x80)
+                int size;
+                int pass;
+
+                VFP_DREG_N(rn, insn);
+                if (insn & 0xf)
                     return 1;
-                rn = (insn >> 16) & 0xf;
-                /* Get the existing value even for arm->vfp moves because
-                   we only set half the register.  */
-                gen_mov_F0_vreg(1, rn);
-                gen_op_vfp_mrrd();
+                if (insn & 0x00c00060
+                    && !arm_feature(env, ARM_FEATURE_NEON))
+                    return 1;
+
+                pass = (insn >> 21) & 1;
+                if (insn & (1 << 22)) {
+                    size = 0;
+                    offset = ((insn >> 5) & 3) * 8;
+                } else if (insn & (1 << 5)) {
+                    size = 1;
+                    offset = (insn & (1 << 6)) ? 16 : 0;
+                } else {
+                    size = 2;
+                    offset = 0;
+                }
                 if (insn & ARM_CP_RW_BIT) {
                     /* vfp->arm */
-                    if (insn & (1 << 21))
-                        gen_movl_reg_T1(s, rd);
-                    else
-                        gen_movl_reg_T0(s, rd);
+                    switch (size) {
+                    case 0:
+                        NEON_GET_REG(T1, rn, pass);
+                        if (offset)
+                            gen_op_shrl_T1_im(offset);
+                        if (insn & (1 << 23))
+                            gen_op_uxtb_T1();
+                        else
+                            gen_op_sxtb_T1();
+                        break;
+                    case 1:
+                        NEON_GET_REG(T1, rn, pass);
+                        if (insn & (1 << 23)) {
+                            if (offset) {
+                                gen_op_shrl_T1_im(16);
+                            } else {
+                                gen_op_uxth_T1();
+                            }
+                        } else {
+                            if (offset) {
+                                gen_op_sarl_T1_im(16);
+                            } else {
+                                gen_op_sxth_T1();
+                            }
+                        }
+                        break;
+                    case 2:
+                        NEON_GET_REG(T1, rn, pass);
+                        break;
+                    }
+                    gen_movl_reg_T1(s, rd);
                 } else {
                     /* arm->vfp */
-                    if (insn & (1 << 21))
-                        gen_movl_T1_reg(s, rd);
-                    else
-                        gen_movl_T0_reg(s, rd);
-                    gen_op_vfp_mdrr();
-                    gen_mov_vreg_F0(dp, rn);
+                    gen_movl_T0_reg(s, rd);
+                    if (insn & (1 << 23)) {
+                        /* VDUP */
+                        if (size == 0) {
+                            gen_op_neon_dup_u8(0);
+                        } else if (size == 1) {
+                            gen_op_neon_dup_low16();
+                        }
+                        NEON_SET_REG(T0, rn, 0);
+                        NEON_SET_REG(T0, rn, 1);
+                    } else {
+                        /* VMOV */
+                        switch (size) {
+                        case 0:
+                            NEON_GET_REG(T2, rn, pass);
+                            gen_op_movl_T1_im(0xff);
+                            gen_op_andl_T0_T1();
+                            gen_op_neon_insert_elt(offset, ~(0xff << offset));
+                            NEON_SET_REG(T2, rn, pass);
+                            break;
+                        case 1:
+                            NEON_GET_REG(T2, rn, pass);
+                            gen_op_movl_T1_im(0xffff);
+                            gen_op_andl_T0_T1();
+                            bank_mask = offset ? 0xffff : 0xffff0000;
+                            gen_op_neon_insert_elt(offset, bank_mask);
+                            NEON_SET_REG(T2, rn, pass);
+                            break;
+                        case 2:
+                            NEON_SET_REG(T0, rn, pass);
+                            break;
+                        }
+                    }
                 }
-            } else {
-                rn = ((insn >> 15) & 0x1e) | ((insn >> 7) & 1);
+            } else { /* !dp */
+                if ((insn & 0x6f) != 0x00)
+                    return 1;
+                rn = VFP_SREG_N(insn);
                 if (insn & ARM_CP_RW_BIT) {
                     /* vfp->arm */
                     if (insn & (1 << 21)) {
                         /* system register */
                         rn >>= 1;
+
                         switch (rn) {
                         case ARM_VFP_FPSID:
+                            /* VFP2 allows access for FSID from userspace.
+                               VFP3 restricts all id registers to privileged
+                               accesses.  */
+                            if (IS_USER(s)
+                                && arm_feature(env, ARM_FEATURE_VFP3))
+                                return 1;
+                            gen_op_vfp_movl_T0_xreg(rn);
+                            break;
                         case ARM_VFP_FPEXC:
+                            if (IS_USER(s))
+                                return 1;
+                            gen_op_vfp_movl_T0_xreg(rn);
+                            break;
                         case ARM_VFP_FPINST:
                         case ARM_VFP_FPINST2:
+                            /* Not present in VFP3.  */
+                            if (IS_USER(s)
+                                || arm_feature(env, ARM_FEATURE_VFP3))
+                                return 1;
                             gen_op_vfp_movl_T0_xreg(rn);
                             break;
                         case ARM_VFP_FPSCR:
@@ -1689,6 +1933,13 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
 			    else
 				gen_op_vfp_movl_T0_fpscr();
                             break;
+                        case ARM_VFP_MVFR0:
+                        case ARM_VFP_MVFR1:
+                            if (IS_USER(s)
+                                || !arm_feature(env, ARM_FEATURE_VFP3))
+                                return 1;
+                            gen_op_vfp_movl_T0_xreg(rn);
+                            break;
                         default:
                             return 1;
                         }
@@ -1709,6 +1960,8 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
                         /* system register */
                         switch (rn) {
                         case ARM_VFP_FPSID:
+                        case ARM_VFP_MVFR0:
+                        case ARM_VFP_MVFR1:
                             /* Writes are ignored.  */
                             break;
                         case ARM_VFP_FPSCR:
@@ -1716,6 +1969,8 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
                             gen_lookup_tb(s);
                             break;
                         case ARM_VFP_FPEXC:
+                            if (IS_USER(s))
+                                return 1;
                             gen_op_vfp_movl_xreg_T0(rn);
                             gen_lookup_tb(s);
                             break;
@@ -1742,38 +1997,31 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
                     rn = ((insn >> 15) & 0x1e) | ((insn >> 7) & 1);
                 } else {
                     /* rn is register number */
-                    if (insn & (1 << 7))
-                        return 1;
-                    rn = (insn >> 16) & 0xf;
+                    VFP_DREG_N(rn, insn);
                 }
 
                 if (op == 15 && (rn == 15 || rn > 17)) {
                     /* Integer or single precision destination.  */
-                    rd = ((insn >> 11) & 0x1e) | ((insn >> 22) & 1);
+                    rd = VFP_SREG_D(insn);
                 } else {
-                    if (insn & (1 << 22))
-                        return 1;
-                    rd = (insn >> 12) & 0xf;
+                    VFP_DREG_D(rd, insn);
                 }
 
                 if (op == 15 && (rn == 16 || rn == 17)) {
                     /* Integer source.  */
                     rm = ((insn << 1) & 0x1e) | ((insn >> 5) & 1);
                 } else {
-                    if (insn & (1 << 5))
-                        return 1;
-                    rm = insn & 0xf;
+                    VFP_DREG_M(rm, insn);
                 }
             } else {
-                rn = ((insn >> 15) & 0x1e) | ((insn >> 7) & 1);
+                rn = VFP_SREG_N(insn);
                 if (op == 15 && rn == 15) {
                     /* Double precision destination.  */
-                    if (insn & (1 << 22))
-                        return 1;
-                    rd = (insn >> 12) & 0xf;
-                } else
-                    rd = ((insn >> 11) & 0x1e) | ((insn >> 22) & 1);
-                rm = ((insn << 1) & 0x1e) | ((insn >> 5) & 1);
+                    VFP_DREG_D(rd, insn);
+                } else {
+                    rd = VFP_SREG_D(insn);
+                }
+                rm = VFP_SREG_M(insn);
             }
 
             veclen = env->vfp.vec_len;
@@ -1831,9 +2079,17 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
                     gen_mov_F0_vreg(dp, rd);
                     gen_vfp_F1_ld0(dp);
                     break;
+                case 20:
+                case 21:
+                case 22:
+                case 23:
+                    /* Source and destination the same.  */
+                    gen_mov_F0_vreg(dp, rd);
+                    break;
                 default:
                     /* One source operand.  */
                     gen_mov_F0_vreg(dp, rm);
+                    break;
                 }
             } else {
                 /* Two source operands.  */
@@ -1882,6 +2138,27 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
                 case 8: /* div: fn / fm */
                     gen_vfp_div(dp);
                     break;
+                case 14: /* fconst */
+                    if (!arm_feature(env, ARM_FEATURE_VFP3))
+                      return 1;
+
+                    n = (insn << 12) & 0x80000000;
+                    i = ((insn >> 12) & 0x70) | (insn & 0xf);
+                    if (dp) {
+                        if (i & 0x40)
+                            i |= 0x3f80;
+                        else
+                            i |= 0x4000;
+                        n |= i << 16;
+                    } else {
+                        if (i & 0x40)
+                            i |= 0x780;
+                        else
+                            i |= 0x800;
+                        n |= i << 19;
+                    }
+                    gen_vfp_fconst(dp, n);
+                    break;
                 case 15: /* extension space */
                     switch (rn) {
                     case 0: /* cpy */
@@ -1921,6 +2198,26 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
                     case 17: /* fsito */
                         gen_vfp_sito(dp);
                         break;
+                    case 20: /* fshto */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_shto(dp, rm);
+                        break;
+                    case 21: /* fslto */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_slto(dp, rm);
+                        break;
+                    case 22: /* fuhto */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_uhto(dp, rm);
+                        break;
+                    case 23: /* fulto */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_ulto(dp, rm);
+                        break;
                     case 24: /* ftoui */
                         gen_vfp_toui(dp);
                         break;
@@ -1933,6 +2230,26 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
                     case 27: /* ftosiz */
                         gen_vfp_tosiz(dp);
                         break;
+                    case 28: /* ftosh */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_tosh(dp, rm);
+                        break;
+                    case 29: /* ftosl */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_tosl(dp, rm);
+                        break;
+                    case 30: /* ftouh */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_touh(dp, rm);
+                        break;
+                    case 31: /* ftoul */
+                        if (!arm_feature(env, ARM_FEATURE_VFP3))
+                          return 1;
+                        gen_vfp_toul(dp, rm);
+                        break;
                     default: /* undefined */
                         printf ("rn:%d\n", rn);
                         return 1;
@@ -1994,16 +2311,15 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
         break;
     case 0xc:
     case 0xd:
-        if (dp && (insn & (1 << 22))) {
+        if (dp && (insn & 0x03e00000) == 0x00400000) {
             /* two-register transfer */
             rn = (insn >> 16) & 0xf;
             rd = (insn >> 12) & 0xf;
             if (dp) {
-                if (insn & (1 << 5))
-                    return 1;
-                rm = insn & 0xf;
-            } else
-                rm = ((insn << 1) & 0x1e) | ((insn >> 5) & 1);
+                VFP_DREG_M(rm, insn);
+            } else {
+                rm = VFP_SREG_M(insn);
+            }
 
             if (insn & ARM_CP_RW_BIT) {
                 /* vfp->arm */
@@ -2040,10 +2356,14 @@ static int disas_vfp_insn(CPUState * env, DisasContext *s, uint32_t insn)
             /* Load/store */
             rn = (insn >> 16) & 0xf;
             if (dp)
-                rd = (insn >> 12) & 0xf;
+                VFP_DREG_D(rd, insn);
             else
-                rd = ((insn >> 11) & 0x1e) | ((insn >> 22) & 1);
-            gen_movl_T1_reg(s, rn);
+                rd = VFP_SREG_D(insn);
+            if (s->thumb && rn == 15) {
+                gen_op_movl_T1_im(s->pc & ~2);
+            } else {
+                gen_movl_T1_reg(s, rn);
+            }
             if ((insn & 0x01200000) == 0x01000000) {
                 /* Single load/store */
                 offset = (insn & 0xff) << 2;
@@ -2156,8 +2476,9 @@ static inline void gen_mulxy(int x, int y)
 }
 
 /* Return the mask of PSR bits set by a MSR instruction.  */
-static uint32_t msr_mask(DisasContext *s, int flags, int spsr) {
+static uint32_t msr_mask(CPUState *env, DisasContext *s, int flags, int spsr) {
     uint32_t mask;
+    uint32_t reserved;
 
     mask = 0;
     if (flags & (1 << 0))
@@ -2168,14 +2489,19 @@ static uint32_t msr_mask(DisasContext *s, int flags, int spsr) {
         mask |= 0xff0000;
     if (flags & (1 << 3))
         mask |= 0xff000000;
+
     /* Mask out undefined bits.  */
-    mask &= 0xf90f03ff;
-    /* Mask out state bits.  */
+    mask &= ~CPSR_RESERVED;
+    if (!arm_feature(env, ARM_FEATURE_V6))
+        reserved &= ~(CPSR_E | CPSR_GE);
+    if (!arm_feature(env, ARM_FEATURE_THUMB2))
+        reserved &= ~CPSR_IT;
+    /* Mask out execution state bits.  */
     if (!spsr)
-        mask &= ~0x01000020;
+        reserved &= ~CPSR_EXEC;
     /* Mask out privileged bits.  */
     if (IS_USER(s))
-        mask &= 0xf80f0200;
+        mask &= CPSR_USER;
     return mask;
 }
 
@@ -2194,6 +2520,7 @@ static int gen_set_psr_T0(DisasContext *s, uint32_t mask, int spsr)
     return 0;
 }
 
+/* Generate an old-style exception return.  */
 static void gen_exception_return(DisasContext *s)
 {
     gen_op_movl_reg_TN[0][15]();
@@ -2202,775 +2529,4309 @@ static void gen_exception_return(DisasContext *s)
     s->is_jmp = DISAS_UPDATE;
 }
 
-static void disas_arm_insn(CPUState * env, DisasContext *s)
+/* Generate a v6 exception return.  */
+static void gen_rfe(DisasContext *s)
 {
-    unsigned int cond, insn, val, op1, i, shift, rm, rs, rn, rd, sh;
+    gen_op_movl_cpsr_T0(0xffffffff);
+    gen_op_movl_T0_T2();
+    gen_op_movl_reg_TN[0][15]();
+    s->is_jmp = DISAS_UPDATE;
+}
 
-    insn = ldl_code(s->pc);
-    s->pc += 4;
+static inline void
+gen_set_condexec (DisasContext *s)
+{
+    if (s->condexec_mask) {
+        gen_op_set_condexec((s->condexec_cond << 4) | (s->condexec_mask >> 1));
+    }
+}
 
-    cond = insn >> 28;
-    if (cond == 0xf){
-        /* Unconditional instructions.  */
-        if ((insn & 0x0d70f000) == 0x0550f000)
-            return; /* PLD */
-        else if ((insn & 0x0e000000) == 0x0a000000) {
-            /* branch link and change to thumb (blx <offset>) */
-            int32_t offset;
+static void gen_nop_hint(DisasContext *s, int val)
+{
+    switch (val) {
+    case 3: /* wfi */
+        gen_op_movl_T0_im((long)s->pc);
+        gen_op_movl_reg_TN[0][15]();
+        s->is_jmp = DISAS_WFI;
+        break;
+    case 2: /* wfe */
+    case 4: /* sev */
+        /* TODO: Implement SEV and WFE.  May help SMP performance.  */
+    default: /* nop */
+        break;
+    }
+}
 
-            val = (uint32_t)s->pc;
-            gen_op_movl_T0_im(val);
-            gen_movl_reg_T0(s, 14);
-            /* Sign-extend the 24-bit offset */
-            offset = (((int32_t)insn) << 8) >> 8;
-            /* offset * 4 + bit24 * 2 + (thumb bit) */
-            val += (offset << 2) | ((insn >> 23) & 2) | 1;
-            /* pipeline offset */
-            val += 4;
-            gen_op_movl_T0_im(val);
-            gen_bx(s);
-            return;
-        } else if ((insn & 0x0e000f00) == 0x0c000100) {
-            if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
-                /* iWMMXt register transfer.  */
-                if (env->cp15.c15_cpar & (1 << 1))
-                    if (!disas_iwmmxt_insn(env, s, insn))
-                        return;
-            }
-        } else if ((insn & 0x0fe00000) == 0x0c400000) {
-            /* Coprocessor double register transfer.  */
-        } else if ((insn & 0x0f000010) == 0x0e000010) {
-            /* Additional coprocessor register transfer.  */
-        } else if ((insn & 0x0ff10010) == 0x01000000) {
-            /* cps (privileged) */
-        } else if ((insn & 0x0ffffdff) == 0x01010000) {
-            /* setend */
-            if (insn & (1 << 9)) {
-                /* BE8 mode not implemented.  */
-                goto illegal_op;
-            }
-            return;
-        }
-        goto illegal_op;
+/* Neon shift by constant.  The actual ops are the same as used for variable
+   shifts.  [OP][U][SIZE]  */
+static GenOpFunc *gen_neon_shift_im[8][2][4] = {
+    { /* 0 */ /* VSHR */
+      {
+        gen_op_neon_shl_u8,
+        gen_op_neon_shl_u16,
+        gen_op_neon_shl_u32,
+        gen_op_neon_shl_u64
+      }, {
+        gen_op_neon_shl_s8,
+        gen_op_neon_shl_s16,
+        gen_op_neon_shl_s32,
+        gen_op_neon_shl_s64
+      }
+    }, { /* 1 */ /* VSRA */
+      {
+        gen_op_neon_shl_u8,
+        gen_op_neon_shl_u16,
+        gen_op_neon_shl_u32,
+        gen_op_neon_shl_u64
+      }, {
+        gen_op_neon_shl_s8,
+        gen_op_neon_shl_s16,
+        gen_op_neon_shl_s32,
+        gen_op_neon_shl_s64
+      }
+    }, { /* 2 */ /* VRSHR */
+      {
+        gen_op_neon_rshl_u8,
+        gen_op_neon_rshl_u16,
+        gen_op_neon_rshl_u32,
+        gen_op_neon_rshl_u64
+      }, {
+        gen_op_neon_rshl_s8,
+        gen_op_neon_rshl_s16,
+        gen_op_neon_rshl_s32,
+        gen_op_neon_rshl_s64
+      }
+    }, { /* 3 */ /* VRSRA */
+      {
+        gen_op_neon_rshl_u8,
+        gen_op_neon_rshl_u16,
+        gen_op_neon_rshl_u32,
+        gen_op_neon_rshl_u64
+      }, {
+        gen_op_neon_rshl_s8,
+        gen_op_neon_rshl_s16,
+        gen_op_neon_rshl_s32,
+        gen_op_neon_rshl_s64
+      }
+    }, { /* 4 */
+      {
+        NULL, NULL, NULL, NULL
+      }, { /* VSRI */
+        gen_op_neon_shl_u8,
+        gen_op_neon_shl_u16,
+        gen_op_neon_shl_u32,
+        gen_op_neon_shl_u64,
+      }
+    }, { /* 5 */
+      { /* VSHL */
+        gen_op_neon_shl_u8,
+        gen_op_neon_shl_u16,
+        gen_op_neon_shl_u32,
+        gen_op_neon_shl_u64,
+      }, { /* VSLI */
+        gen_op_neon_shl_u8,
+        gen_op_neon_shl_u16,
+        gen_op_neon_shl_u32,
+        gen_op_neon_shl_u64,
+      }
+    }, { /* 6 */ /* VQSHL */
+      {
+        gen_op_neon_qshl_u8,
+        gen_op_neon_qshl_u16,
+        gen_op_neon_qshl_u32,
+        gen_op_neon_qshl_u64
+      }, {
+        gen_op_neon_qshl_s8,
+        gen_op_neon_qshl_s16,
+        gen_op_neon_qshl_s32,
+        gen_op_neon_qshl_s64
+      }
+    }, { /* 7 */ /* VQSHLU */
+      {
+        gen_op_neon_qshl_u8,
+        gen_op_neon_qshl_u16,
+        gen_op_neon_qshl_u32,
+        gen_op_neon_qshl_u64
+      }, {
+        gen_op_neon_qshl_u8,
+        gen_op_neon_qshl_u16,
+        gen_op_neon_qshl_u32,
+        gen_op_neon_qshl_u64
+      }
     }
-    if (cond != 0xe) {
-        /* if not always execute, we generate a conditional jump to
-           next instruction */
-        s->condlabel = gen_new_label();
-        gen_test_cc[cond ^ 1](s->condlabel);
-        s->condjmp = 1;
-        //gen_test_cc[cond ^ 1]((long)s->tb, (long)s->pc);
-        //s->is_jmp = DISAS_JUMP_NEXT;
+};
+
+/* [R][U][size - 1] */
+static GenOpFunc *gen_neon_shift_im_narrow[2][2][3] = {
+    {
+      {
+        gen_op_neon_shl_u16,
+        gen_op_neon_shl_u32,
+        gen_op_neon_shl_u64
+      }, {
+        gen_op_neon_shl_s16,
+        gen_op_neon_shl_s32,
+        gen_op_neon_shl_s64
+      }
+    }, {
+      {
+        gen_op_neon_rshl_u16,
+        gen_op_neon_rshl_u32,
+        gen_op_neon_rshl_u64
+      }, {
+        gen_op_neon_rshl_s16,
+        gen_op_neon_rshl_s32,
+        gen_op_neon_rshl_s64
+      }
     }
-    if ((insn & 0x0f900000) == 0x03000000) {
-        if ((insn & 0x0fb0f000) != 0x0320f000)
-            goto illegal_op;
-        /* CPSR = immediate */
-        val = insn & 0xff;
-        shift = ((insn >> 8) & 0xf) * 2;
-        if (shift)
-            val = (val >> shift) | (val << (32 - shift));
-        gen_op_movl_T0_im(val);
-        i = ((insn & (1 << 22)) != 0);
-        if (gen_set_psr_T0(s, msr_mask(s, (insn >> 16) & 0xf, i), i))
-            goto illegal_op;
-    } else if ((insn & 0x0f900000) == 0x01000000
-               && (insn & 0x00000090) != 0x00000090) {
-        /* miscellaneous instructions */
-        op1 = (insn >> 21) & 3;
-        sh = (insn >> 4) & 0xf;
-        rm = insn & 0xf;
-        switch (sh) {
-        case 0x0: /* move program status register */
-            if (op1 & 1) {
-                /* PSR = reg */
-                gen_movl_T0_reg(s, rm);
-                i = ((op1 & 2) != 0);
-                if (gen_set_psr_T0(s, msr_mask(s, (insn >> 16) & 0xf, i), i))
-                    goto illegal_op;
-            } else {
-                /* reg = PSR */
-                rd = (insn >> 12) & 0xf;
-                if (op1 & 2) {
-                    if (IS_USER(s))
-                        goto illegal_op;
-                    gen_op_movl_T0_spsr();
-                } else {
-                    gen_op_movl_T0_cpsr();
-                }
-                gen_movl_reg_T0(s, rd);
-            }
-            break;
-        case 0x1:
-            if (op1 == 1) {
-                /* branch/exchange thumb (bx).  */
-                gen_movl_T0_reg(s, rm);
-                gen_bx(s);
-            } else if (op1 == 3) {
-                /* clz */
-                rd = (insn >> 12) & 0xf;
-                gen_movl_T0_reg(s, rm);
-                gen_op_clz_T0();
-                gen_movl_reg_T0(s, rd);
-            } else {
-                goto illegal_op;
-            }
-            break;
-        case 0x2:
-            if (op1 == 1) {
-                ARCH(5J); /* bxj */
-                /* Trivial implementation equivalent to bx.  */
-                gen_movl_T0_reg(s, rm);
-                gen_bx(s);
-            } else {
-                goto illegal_op;
-            }
-            break;
-        case 0x3:
-            if (op1 != 1)
-              goto illegal_op;
+};
 
-            /* branch link/exchange thumb (blx) */
-            val = (uint32_t)s->pc;
-            gen_op_movl_T1_im(val);
-            gen_movl_T0_reg(s, rm);
-            gen_movl_reg_T1(s, 14);
-            gen_bx(s);
-            break;
-        case 0x5: /* saturating add/subtract */
-            rd = (insn >> 12) & 0xf;
-            rn = (insn >> 16) & 0xf;
-            gen_movl_T0_reg(s, rm);
+static inline void
+gen_op_neon_narrow_u32 ()
+{
+    /* No-op.  */
+}
+
+static GenOpFunc *gen_neon_narrow[3] = {
+    gen_op_neon_narrow_u8,
+    gen_op_neon_narrow_u16,
+    gen_op_neon_narrow_u32
+};
+
+static GenOpFunc *gen_neon_narrow_satu[3] = {
+    gen_op_neon_narrow_sat_u8,
+    gen_op_neon_narrow_sat_u16,
+    gen_op_neon_narrow_sat_u32
+};
+
+static GenOpFunc *gen_neon_narrow_sats[3] = {
+    gen_op_neon_narrow_sat_s8,
+    gen_op_neon_narrow_sat_s16,
+    gen_op_neon_narrow_sat_s32
+};
+
+static inline int gen_neon_add(int size)
+{
+    switch (size) {
+    case 0: gen_op_neon_add_u8(); break;
+    case 1: gen_op_neon_add_u16(); break;
+    case 2: gen_op_addl_T0_T1(); break;
+    default: return 1;
+    }
+    return 0;
+}
+
+/* 32-bit pairwise ops end up the same as the elementsise versions.  */
+#define gen_op_neon_pmax_s32  gen_op_neon_max_s32
+#define gen_op_neon_pmax_u32  gen_op_neon_max_u32
+#define gen_op_neon_pmin_s32  gen_op_neon_min_s32
+#define gen_op_neon_pmin_u32  gen_op_neon_min_u32
+
+#define GEN_NEON_INTEGER_OP(name) do { \
+    switch ((size << 1) | u) { \
+    case 0: gen_op_neon_##name##_s8(); break; \
+    case 1: gen_op_neon_##name##_u8(); break; \
+    case 2: gen_op_neon_##name##_s16(); break; \
+    case 3: gen_op_neon_##name##_u16(); break; \
+    case 4: gen_op_neon_##name##_s32(); break; \
+    case 5: gen_op_neon_##name##_u32(); break; \
+    default: return 1; \
+    }} while (0)
+
+static inline void
+gen_neon_movl_scratch_T0(int scratch)
+{
+  uint32_t offset;
+
+  offset = offsetof(CPUARMState, vfp.scratch[scratch]);
+  gen_op_neon_setreg_T0(offset);
+}
+
+static inline void
+gen_neon_movl_scratch_T1(int scratch)
+{
+  uint32_t offset;
+
+  offset = offsetof(CPUARMState, vfp.scratch[scratch]);
+  gen_op_neon_setreg_T1(offset);
+}
+
+static inline void
+gen_neon_movl_T0_scratch(int scratch)
+{
+  uint32_t offset;
+
+  offset = offsetof(CPUARMState, vfp.scratch[scratch]);
+  gen_op_neon_getreg_T0(offset);
+}
+
+static inline void
+gen_neon_movl_T1_scratch(int scratch)
+{
+  uint32_t offset;
+
+  offset = offsetof(CPUARMState, vfp.scratch[scratch]);
+  gen_op_neon_getreg_T1(offset);
+}
+
+static inline void gen_op_neon_widen_u32(void)
+{
+    gen_op_movl_T1_im(0);
+}
+
+static inline void gen_neon_get_scalar(int size, int reg)
+{
+    if (size == 1) {
+        NEON_GET_REG(T0, reg >> 1, reg & 1);
+    } else {
+        NEON_GET_REG(T0, reg >> 2, (reg >> 1) & 1);
+        if (reg & 1)
+            gen_op_neon_dup_low16();
+        else
+            gen_op_neon_dup_high16();
+    }
+}
+
+static void gen_neon_unzip(int reg, int q, int tmp, int size)
+{
+    int n;
+
+    for (n = 0; n < q + 1; n += 2) {
+        NEON_GET_REG(T0, reg, n);
+        NEON_GET_REG(T0, reg, n + n);
+        switch (size) {
+        case 0: gen_op_neon_unzip_u8(); break;
+        case 1: gen_op_neon_zip_u16(); break; /* zip and unzip are the same.  */
+        case 2: /* no-op */; break;
+        default: abort();
+        }
+        gen_neon_movl_scratch_T0(tmp + n);
+        gen_neon_movl_scratch_T1(tmp + n + 1);
+    }
+}
+
+static struct {
+    int nregs;
+    int interleave;
+    int spacing;
+} neon_ls_element_type[11] = {
+    {4, 4, 1},
+    {4, 4, 2},
+    {4, 1, 1},
+    {4, 2, 1},
+    {3, 3, 1},
+    {3, 3, 2},
+    {3, 1, 1},
+    {1, 1, 1},
+    {2, 2, 1},
+    {2, 2, 2},
+    {2, 1, 1}
+};
+
+/* Translate a NEON load/store element instruction.  Return nonzero if the
+   instruction is invalid.  */
+static int disas_neon_ls_insn(CPUState * env, DisasContext *s, uint32_t insn)
+{
+    int rd, rn, rm;
+    int op;
+    int nregs;
+    int interleave;
+    int stride;
+    int size;
+    int reg;
+    int pass;
+    int load;
+    int shift;
+    uint32_t mask;
+    int n;
+
+    if (!vfp_enabled(env))
+      return 1;
+    VFP_DREG_D(rd, insn);
+    rn = (insn >> 16) & 0xf;
+    rm = insn & 0xf;
+    load = (insn & (1 << 21)) != 0;
+    if ((insn & (1 << 23)) == 0) {
+        /* Load store all elements.  */
+        op = (insn >> 8) & 0xf;
+        size = (insn >> 6) & 3;
+        if (op > 10 || size == 3)
+            return 1;
+        nregs = neon_ls_element_type[op].nregs;
+        interleave = neon_ls_element_type[op].interleave;
+        gen_movl_T1_reg(s, rn);
+        stride = (1 << size) * interleave;
+        for (reg = 0; reg < nregs; reg++) {
+            if (interleave > 2 || (interleave == 2 && nregs == 2)) {
+                gen_movl_T1_reg(s, rn);
+                gen_op_addl_T1_im((1 << size) * reg);
+            } else if (interleave == 2 && nregs == 4 && reg == 2) {
+                gen_movl_T1_reg(s, rn);
+                gen_op_addl_T1_im(1 << size);
+            }
+            for (pass = 0; pass < 2; pass++) {
+                if (size == 2) {
+                    if (load) {
+                        gen_ldst(ldl, s);
+                        NEON_SET_REG(T0, rd, pass);
+                    } else {
+                        NEON_GET_REG(T0, rd, pass);
+                        gen_ldst(stl, s);
+                    }
+                    gen_op_addl_T1_im(stride);
+                } else if (size == 1) {
+                    if (load) {
+                        gen_ldst(lduw, s);
+                        gen_op_addl_T1_im(stride);
+                        gen_op_movl_T2_T0();
+                        gen_ldst(lduw, s);
+                        gen_op_addl_T1_im(stride);
+                        gen_op_neon_insert_elt(16, 0xffff);
+                        NEON_SET_REG(T2, rd, pass);
+                    } else {
+                        NEON_GET_REG(T2, rd, pass);
+                        gen_op_movl_T0_T2();
+                        gen_ldst(stw, s);
+                        gen_op_addl_T1_im(stride);
+                        gen_op_neon_extract_elt(16, 0xffff0000);
+                        gen_ldst(stw, s);
+                        gen_op_addl_T1_im(stride);
+                    }
+                } else /* size == 0 */ {
+                    if (load) {
+                        mask = 0xff;
+                        for (n = 0; n < 4; n++) {
+                            gen_ldst(ldub, s);
+                            gen_op_addl_T1_im(stride);
+                            if (n == 0) {
+                                gen_op_movl_T2_T0();
+                            } else {
+                                gen_op_neon_insert_elt(n * 8, ~mask);
+                            }
+                            mask <<= 8;
+                        }
+                        NEON_SET_REG(T2, rd, pass);
+                    } else {
+                        NEON_GET_REG(T2, rd, pass);
+                        mask = 0xff;
+                        for (n = 0; n < 4; n++) {
+                            if (n == 0) {
+                                gen_op_movl_T0_T2();
+                            } else {
+                                gen_op_neon_extract_elt(n * 8, mask);
+                            }
+                            gen_ldst(stb, s);
+                            gen_op_addl_T1_im(stride);
+                            mask <<= 8;
+                        }
+                    }
+                }
+            }
+            rd += neon_ls_element_type[op].spacing;
+        }
+        stride = nregs * 8;
+    } else {
+        size = (insn >> 10) & 3;
+        if (size == 3) {
+            /* Load single element to all lanes.  */
+            if (!load)
+                return 1;
+            size = (insn >> 6) & 3;
+            nregs = ((insn >> 8) & 3) + 1;
+            stride = (insn & (1 << 5)) ? 2 : 1;
             gen_movl_T1_reg(s, rn);
-            if (op1 & 2)
-                gen_op_double_T1_saturate();
-            if (op1 & 1)
-                gen_op_subl_T0_T1_saturate();
-            else
-                gen_op_addl_T0_T1_saturate();
-            gen_movl_reg_T0(s, rd);
-            break;
-        case 7: /* bkpt */
-            gen_op_movl_T0_im((long)s->pc - 4);
-            gen_op_movl_reg_TN[0][15]();
-            gen_op_bkpt();
-            s->is_jmp = DISAS_JUMP;
-            break;
-        case 0x8: /* signed multiply */
-        case 0xa:
-        case 0xc:
-        case 0xe:
-            rs = (insn >> 8) & 0xf;
-            rn = (insn >> 12) & 0xf;
-            rd = (insn >> 16) & 0xf;
-            if (op1 == 1) {
-                /* (32 * 16) >> 16 */
-                gen_movl_T0_reg(s, rm);
-                gen_movl_T1_reg(s, rs);
-                if (sh & 4)
-                    gen_op_sarl_T1_im(16);
-                else
-                    gen_op_sxth_T1();
-                gen_op_imulw_T0_T1();
-                if ((sh & 2) == 0) {
-                    gen_movl_T1_reg(s, rn);
-                    gen_op_addl_T0_T1_setq();
+            for (reg = 0; reg < nregs; reg++) {
+                switch (size) {
+                case 0:
+                    gen_ldst(ldub, s);
+                    gen_op_neon_dup_u8(0);
+                    break;
+                case 1:
+                    gen_ldst(lduw, s);
+                    gen_op_neon_dup_low16();
+                    break;
+                case 2:
+                    gen_ldst(ldl, s);
+                    break;
+                case 3:
+                    return 1;
                 }
-                gen_movl_reg_T0(s, rd);
-            } else {
-                /* 16 * 16 */
-                gen_movl_T0_reg(s, rm);
-                gen_movl_T1_reg(s, rs);
-                gen_mulxy(sh & 2, sh & 4);
-                if (op1 == 2) {
-                    gen_op_signbit_T1_T0();
-                    gen_op_addq_T0_T1(rn, rd);
-                    gen_movl_reg_T0(s, rn);
-                    gen_movl_reg_T1(s, rd);
-                } else {
-                    if (op1 == 0) {
-                        gen_movl_T1_reg(s, rn);
-                        gen_op_addl_T0_T1_setq();
+                gen_op_addl_T1_im(1 << size);
+                NEON_SET_REG(T0, rd, 0);
+                NEON_SET_REG(T0, rd, 1);
+                rd += stride;
+            }
+            stride = (1 << size) * nregs;
+        } else {
+            /* Single element.  */
+            pass = (insn >> 7) & 1;
+            switch (size) {
+            case 0:
+                shift = ((insn >> 5) & 3) * 8;
+                mask = 0xff << shift;
+                stride = 1;
+                break;
+            case 1:
+                shift = ((insn >> 6) & 1) * 16;
+                mask = shift ? 0xffff0000 : 0xffff;
+                stride = (insn & (1 << 5)) ? 2 : 1;
+                break;
+            case 2:
+                shift = 0;
+                mask = 0xffffffff;
+                stride = (insn & (1 << 6)) ? 2 : 1;
+                break;
+            default:
+                abort();
+            }
+            nregs = ((insn >> 8) & 3) + 1;
+            gen_movl_T1_reg(s, rn);
+            for (reg = 0; reg < nregs; reg++) {
+                if (load) {
+                    if (size != 2) {
+                        NEON_GET_REG(T2, rd, pass);
+                    }
+                    switch (size) {
+                    case 0:
+                        gen_ldst(ldub, s);
+                        break;
+                    case 1:
+                        gen_ldst(lduw, s);
+                        break;
+                    case 2:
+                        gen_ldst(ldl, s);
+                        NEON_SET_REG(T0, rd, pass);
+                        break;
+                    }
+                    if (size != 2) {
+                        gen_op_neon_insert_elt(shift, ~mask);
+                        NEON_SET_REG(T0, rd, pass);
+                    }
+                } else { /* Store */
+                    if (size == 2) {
+                        NEON_GET_REG(T0, rd, pass);
+                    } else {
+                        NEON_GET_REG(T2, rd, pass);
+                        gen_op_neon_extract_elt(shift, mask);
+                    }
+                    switch (size) {
+                    case 0:
+                        gen_ldst(stb, s);
+                        break;
+                    case 1:
+                        gen_ldst(stw, s);
+                        break;
+                    case 2:
+                        gen_ldst(stl, s);
+                        break;
                     }
-                    gen_movl_reg_T0(s, rd);
                 }
+                rd += stride;
+                gen_op_addl_T1_im(1 << size);
             }
-            break;
-        default:
-            goto illegal_op;
+            stride = nregs * (1 << size);
         }
-    } else if (((insn & 0x0e000000) == 0 &&
-                (insn & 0x00000090) != 0x90) ||
-               ((insn & 0x0e000000) == (1 << 25))) {
-        int set_cc, logic_cc, shiftop;
+    }
+    if (rm != 15) {
+        gen_movl_T1_reg(s, rn);
+        if (rm == 13) {
+            gen_op_addl_T1_im(stride);
+        } else {
+            gen_movl_T2_reg(s, rm);
+            gen_op_addl_T1_T2();
+        }
+        gen_movl_reg_T1(s, rn);
+    }
+    return 0;
+}
 
-        op1 = (insn >> 21) & 0xf;
-        set_cc = (insn >> 20) & 1;
-        logic_cc = table_logic_cc[op1] & set_cc;
+/* Translate a NEON data processing instruction.  Return nonzero if the
+   instruction is invalid.
+   In general we process vectors in 32-bit chunks.  This means we can reuse
+   some of the scalar ops, and hopefully the code generated for 32-bit
+   hosts won't be too awful.  The downside is that the few 64-bit operations
+   (mainly shifts) get complicated.  */
 
-        /* data processing instruction */
-        if (insn & (1 << 25)) {
-            /* immediate operand */
-            val = insn & 0xff;
-            shift = ((insn >> 8) & 0xf) * 2;
-            if (shift)
-                val = (val >> shift) | (val << (32 - shift));
-            gen_op_movl_T1_im(val);
-            if (logic_cc && shift)
-                gen_op_mov_CF_T1();
-        } else {
-            /* register */
-            rm = (insn) & 0xf;
-            gen_movl_T1_reg(s, rm);
-            shiftop = (insn >> 5) & 3;
-            if (!(insn & (1 << 4))) {
-                shift = (insn >> 7) & 0x1f;
-                if (shift != 0) {
-                    if (logic_cc) {
-                        gen_shift_T1_im_cc[shiftop](shift);
+static int disas_neon_data_insn(CPUState * env, DisasContext *s, uint32_t insn)
+{
+    int op;
+    int q;
+    int rd, rn, rm;
+    int size;
+    int shift;
+    int pass;
+    int count;
+    int pairwise;
+    int u;
+    int n;
+    uint32_t imm;
+
+    if (!vfp_enabled(env))
+      return 1;
+    q = (insn & (1 << 6)) != 0;
+    u = (insn >> 24) & 1;
+    VFP_DREG_D(rd, insn);
+    VFP_DREG_N(rn, insn);
+    VFP_DREG_M(rm, insn);
+    size = (insn >> 20) & 3;
+    if ((insn & (1 << 23)) == 0) {
+        /* Three register same length.  */
+        op = ((insn >> 7) & 0x1e) | ((insn >> 4) & 1);
+        if (size == 3 && (op == 1 || op == 5 || op == 16)) {
+            for (pass = 0; pass < (q ? 2 : 1); pass++) {
+                NEON_GET_REG(T0, rm, pass * 2);
+                NEON_GET_REG(T1, rm, pass * 2 + 1);
+                gen_neon_movl_scratch_T0(0);
+                gen_neon_movl_scratch_T1(1);
+                NEON_GET_REG(T0, rn, pass * 2);
+                NEON_GET_REG(T1, rn, pass * 2 + 1);
+                switch (op) {
+                case 1: /* VQADD */
+                    if (u) {
+                        gen_op_neon_addl_saturate_u64();
                     } else {
-                        gen_shift_T1_im[shiftop](shift);
+                        gen_op_neon_addl_saturate_s64();
                     }
-                } else if (shiftop != 0) {
-                    if (logic_cc) {
-                        gen_shift_T1_0_cc[shiftop]();
+                    break;
+                case 5: /* VQSUB */
+                    if (u) {
+                        gen_op_neon_subl_saturate_u64();
                     } else {
-                        gen_shift_T1_0[shiftop]();
+                        gen_op_neon_subl_saturate_s64();
                     }
+                    break;
+                case 16:
+                    if (u) {
+                        gen_op_neon_subl_u64();
+                    } else {
+                        gen_op_neon_addl_u64();
+                    }
+                    break;
+                default:
+                    abort();
                 }
-            } else {
-                rs = (insn >> 8) & 0xf;
-                gen_movl_T0_reg(s, rs);
-                if (logic_cc) {
-                    gen_shift_T1_T0_cc[shiftop]();
-                } else {
-                    gen_shift_T1_T0[shiftop]();
-                }
+                NEON_SET_REG(T0, rd, pass * 2);
+                NEON_SET_REG(T1, rd, pass * 2 + 1);
             }
+            return 0;
         }
-        if (op1 != 0x0f && op1 != 0x0d) {
-            rn = (insn >> 16) & 0xf;
-            gen_movl_T0_reg(s, rn);
-        }
-        rd = (insn >> 12) & 0xf;
-        switch(op1) {
-        case 0x00:
-            gen_op_andl_T0_T1();
-            gen_movl_reg_T0(s, rd);
-            if (logic_cc)
-                gen_op_logic_T0_cc();
+        switch (op) {
+        case 8: /* VSHL */
+        case 9: /* VQSHL */
+        case 10: /* VRSHL */
+        case 11: /* VQSHL */
+            /* Shift operations have Rn and Rm reversed.  */
+            {
+                int tmp;
+                tmp = rn;
+                rn = rm;
+                rm = tmp;
+                pairwise = 0;
+            }
             break;
-        case 0x01:
-            gen_op_xorl_T0_T1();
-            gen_movl_reg_T0(s, rd);
-            if (logic_cc)
-                gen_op_logic_T0_cc();
+        case 20: /* VPMAX */
+        case 21: /* VPMIN */
+        case 23: /* VPADD */
+            pairwise = 1;
             break;
-        case 0x02:
-            if (set_cc && rd == 15) {
-                /* SUBS r15, ... is used for exception return.  */
-                if (IS_USER(s))
-                    goto illegal_op;
-                gen_op_subl_T0_T1_cc();
-                gen_exception_return(s);
-            } else {
-                if (set_cc)
-                    gen_op_subl_T0_T1_cc();
-                else
-                    gen_op_subl_T0_T1();
-                gen_movl_reg_T0(s, rd);
-            }
+        case 26: /* VPADD (float) */
+            pairwise = (u && size < 2);
             break;
-        case 0x03:
-            if (set_cc)
-                gen_op_rsbl_T0_T1_cc();
-            else
-                gen_op_rsbl_T0_T1();
-            gen_movl_reg_T0(s, rd);
-            break;
-        case 0x04:
-            if (set_cc)
-                gen_op_addl_T0_T1_cc();
-            else
-                gen_op_addl_T0_T1();
-            gen_movl_reg_T0(s, rd);
+        case 30: /* VPMIN/VPMAX (float) */
+            pairwise = u;
             break;
-        case 0x05:
-            if (set_cc)
-                gen_op_adcl_T0_T1_cc();
-            else
-                gen_op_adcl_T0_T1();
-            gen_movl_reg_T0(s, rd);
+        default:
+            pairwise = 0;
             break;
-        case 0x06:
-            if (set_cc)
-                gen_op_sbcl_T0_T1_cc();
+        }
+        for (pass = 0; pass < (q ? 4 : 2); pass++) {
+
+        if (pairwise) {
+            /* Pairwise.  */
+            if (q)
+                n = (pass & 1) * 2;
             else
-                gen_op_sbcl_T0_T1();
-            gen_movl_reg_T0(s, rd);
+                n = 0;
+            if (pass < q + 1) {
+                NEON_GET_REG(T0, rn, n);
+                NEON_GET_REG(T1, rn, n + 1);
+            } else {
+                NEON_GET_REG(T0, rm, n);
+                NEON_GET_REG(T1, rm, n + 1);
+            }
+        } else {
+            /* Elementwise.  */
+            NEON_GET_REG(T0, rn, pass);
+            NEON_GET_REG(T1, rm, pass);
+        }
+        switch (op) {
+        case 0: /* VHADD */
+            GEN_NEON_INTEGER_OP(hadd);
+            break;
+        case 1: /* VQADD */
+            switch (size << 1| u) {
+            case 0: gen_op_neon_qadd_s8(); break;
+            case 1: gen_op_neon_qadd_u8(); break;
+            case 2: gen_op_neon_qadd_s16(); break;
+            case 3: gen_op_neon_qadd_u16(); break;
+            case 4: gen_op_addl_T0_T1_saturate(); break;
+            case 5: gen_op_addl_T0_T1_usaturate(); break;
+            default: abort();
+            }
             break;
-        case 0x07:
-            if (set_cc)
-                gen_op_rscl_T0_T1_cc();
-            else
-                gen_op_rscl_T0_T1();
-            gen_movl_reg_T0(s, rd);
+        case 2: /* VRHADD */
+            GEN_NEON_INTEGER_OP(rhadd);
             break;
-        case 0x08:
-            if (set_cc) {
+        case 3: /* Logic ops.  */
+            switch ((u << 2) | size) {
+            case 0: /* VAND */
                 gen_op_andl_T0_T1();
-                gen_op_logic_T0_cc();
+                break;
+            case 1: /* BIC */
+                gen_op_bicl_T0_T1();
+                break;
+            case 2: /* VORR */
+                gen_op_orl_T0_T1();
+                break;
+            case 3: /* VORN */
+                gen_op_notl_T1();
+                gen_op_orl_T0_T1();
+                break;
+            case 4: /* VEOR */
+                gen_op_xorl_T0_T1();
+                break;
+            case 5: /* VBSL */
+                NEON_GET_REG(T2, rd, pass);
+                gen_op_neon_bsl();
+                break;
+            case 6: /* VBIT */
+                NEON_GET_REG(T2, rd, pass);
+                gen_op_neon_bit();
+                break;
+            case 7: /* VBIF */
+                NEON_GET_REG(T2, rd, pass);
+                gen_op_neon_bif();
+                break;
             }
             break;
-        case 0x09:
-            if (set_cc) {
-                gen_op_xorl_T0_T1();
-                gen_op_logic_T0_cc();
+        case 4: /* VHSUB */
+            GEN_NEON_INTEGER_OP(hsub);
+            break;
+        case 5: /* VQSUB */
+            switch ((size << 1) | u) {
+            case 0: gen_op_neon_qsub_s8(); break;
+            case 1: gen_op_neon_qsub_u8(); break;
+            case 2: gen_op_neon_qsub_s16(); break;
+            case 3: gen_op_neon_qsub_u16(); break;
+            case 4: gen_op_subl_T0_T1_saturate(); break;
+            case 5: gen_op_subl_T0_T1_usaturate(); break;
+            default: abort();
             }
             break;
-        case 0x0a:
-            if (set_cc) {
-                gen_op_subl_T0_T1_cc();
+        case 6: /* VCGT */
+            GEN_NEON_INTEGER_OP(cgt);
+            break;
+        case 7: /* VCGE */
+            GEN_NEON_INTEGER_OP(cge);
+            break;
+        case 8: /* VSHL */
+            switch ((size << 1) | u) {
+            case 0: gen_op_neon_shl_s8(); break;
+            case 1: gen_op_neon_shl_u8(); break;
+            case 2: gen_op_neon_shl_s16(); break;
+            case 3: gen_op_neon_shl_u16(); break;
+            case 4: gen_op_neon_shl_s32(); break;
+            case 5: gen_op_neon_shl_u32(); break;
+#if 0
+            /* ??? Implementing these is tricky because the vector ops work
+               on 32-bit pieces.  */
+            case 6: gen_op_neon_shl_s64(); break;
+            case 7: gen_op_neon_shl_u64(); break;
+#else
+            case 6: case 7: cpu_abort(env, "VSHL.64 not implemented");
+#endif
             }
             break;
-        case 0x0b:
-            if (set_cc) {
-                gen_op_addl_T0_T1_cc();
+        case 9: /* VQSHL */
+            switch ((size << 1) | u) {
+            case 0: gen_op_neon_qshl_s8(); break;
+            case 1: gen_op_neon_qshl_u8(); break;
+            case 2: gen_op_neon_qshl_s16(); break;
+            case 3: gen_op_neon_qshl_u16(); break;
+            case 4: gen_op_neon_qshl_s32(); break;
+            case 5: gen_op_neon_qshl_u32(); break;
+#if 0
+            /* ??? Implementing these is tricky because the vector ops work
+               on 32-bit pieces.  */
+            case 6: gen_op_neon_qshl_s64(); break;
+            case 7: gen_op_neon_qshl_u64(); break;
+#else
+            case 6: case 7: cpu_abort(env, "VQSHL.64 not implemented");
+#endif
             }
             break;
-        case 0x0c:
-            gen_op_orl_T0_T1();
-            gen_movl_reg_T0(s, rd);
-            if (logic_cc)
-                gen_op_logic_T0_cc();
+        case 10: /* VRSHL */
+            switch ((size << 1) | u) {
+            case 0: gen_op_neon_rshl_s8(); break;
+            case 1: gen_op_neon_rshl_u8(); break;
+            case 2: gen_op_neon_rshl_s16(); break;
+            case 3: gen_op_neon_rshl_u16(); break;
+            case 4: gen_op_neon_rshl_s32(); break;
+            case 5: gen_op_neon_rshl_u32(); break;
+#if 0
+            /* ??? Implementing these is tricky because the vector ops work
+               on 32-bit pieces.  */
+            case 6: gen_op_neon_rshl_s64(); break;
+            case 7: gen_op_neon_rshl_u64(); break;
+#else
+            case 6: case 7: cpu_abort(env, "VRSHL.64 not implemented");
+#endif
+            }
             break;
-        case 0x0d:
-            if (logic_cc && rd == 15) {
-                /* MOVS r15, ... is used for exception return.  */
-                if (IS_USER(s))
-                    goto illegal_op;
-                gen_op_movl_T0_T1();
-                gen_exception_return(s);
+        case 11: /* VQRSHL */
+            switch ((size << 1) | u) {
+            case 0: gen_op_neon_qrshl_s8(); break;
+            case 1: gen_op_neon_qrshl_u8(); break;
+            case 2: gen_op_neon_qrshl_s16(); break;
+            case 3: gen_op_neon_qrshl_u16(); break;
+            case 4: gen_op_neon_qrshl_s32(); break;
+            case 5: gen_op_neon_qrshl_u32(); break;
+#if 0
+            /* ??? Implementing these is tricky because the vector ops work
+               on 32-bit pieces.  */
+            case 6: gen_op_neon_qrshl_s64(); break;
+            case 7: gen_op_neon_qrshl_u64(); break;
+#else
+            case 6: case 7: cpu_abort(env, "VQRSHL.64 not implemented");
+#endif
+            }
+            break;
+        case 12: /* VMAX */
+            GEN_NEON_INTEGER_OP(max);
+            break;
+        case 13: /* VMIN */
+            GEN_NEON_INTEGER_OP(min);
+            break;
+        case 14: /* VABD */
+            GEN_NEON_INTEGER_OP(abd);
+            break;
+        case 15: /* VABA */
+            GEN_NEON_INTEGER_OP(abd);
+            NEON_GET_REG(T1, rd, pass);
+            gen_neon_add(size);
+            break;
+        case 16:
+            if (!u) { /* VADD */
+                if (gen_neon_add(size))
+                    return 1;
+            } else { /* VSUB */
+                switch (size) {
+                case 0: gen_op_neon_sub_u8(); break;
+                case 1: gen_op_neon_sub_u16(); break;
+                case 2: gen_op_subl_T0_T1(); break;
+                default: return 1;
+                }
+            }
+            break;
+        case 17:
+            if (!u) { /* VTST */
+                switch (size) {
+                case 0: gen_op_neon_tst_u8(); break;
+                case 1: gen_op_neon_tst_u16(); break;
+                case 2: gen_op_neon_tst_u32(); break;
+                default: return 1;
+                }
+            } else { /* VCEQ */
+                switch (size) {
+                case 0: gen_op_neon_ceq_u8(); break;
+                case 1: gen_op_neon_ceq_u16(); break;
+                case 2: gen_op_neon_ceq_u32(); break;
+                default: return 1;
+                }
+            }
+            break;
+        case 18: /* Multiply.  */
+            switch (size) {
+            case 0: gen_op_neon_mul_u8(); break;
+            case 1: gen_op_neon_mul_u16(); break;
+            case 2: gen_op_mul_T0_T1(); break;
+            default: return 1;
+            }
+            NEON_GET_REG(T1, rd, pass);
+            if (u) { /* VMLS */
+                switch (size) {
+                case 0: gen_op_neon_rsb_u8(); break;
+                case 1: gen_op_neon_rsb_u16(); break;
+                case 2: gen_op_rsbl_T0_T1(); break;
+                default: return 1;
+                }
+            } else { /* VMLA */
+                gen_neon_add(size);
+            }
+            break;
+        case 19: /* VMUL */
+            if (u) { /* polynomial */
+                gen_op_neon_mul_p8();
+            } else { /* Integer */
+                switch (size) {
+                case 0: gen_op_neon_mul_u8(); break;
+                case 1: gen_op_neon_mul_u16(); break;
+                case 2: gen_op_mul_T0_T1(); break;
+                default: return 1;
+                }
+            }
+            break;
+        case 20: /* VPMAX */
+            GEN_NEON_INTEGER_OP(pmax);
+            break;
+        case 21: /* VPMIN */
+            GEN_NEON_INTEGER_OP(pmin);
+            break;
+        case 22: /* Hultiply high.  */
+            if (!u) { /* VQDMULH */
+                switch (size) {
+                case 1: gen_op_neon_qdmulh_s16(); break;
+                case 2: gen_op_neon_qdmulh_s32(); break;
+                default: return 1;
+                }
+            } else { /* VQRDHMUL */
+                switch (size) {
+                case 1: gen_op_neon_qrdmulh_s16(); break;
+                case 2: gen_op_neon_qrdmulh_s32(); break;
+                default: return 1;
+                }
+            }
+            break;
+        case 23: /* VPADD */
+            if (u)
+                return 1;
+            switch (size) {
+            case 0: gen_op_neon_padd_u8(); break;
+            case 1: gen_op_neon_padd_u16(); break;
+            case 2: gen_op_addl_T0_T1(); break;
+            default: return 1;
+            }
+            break;
+        case 26: /* Floating point arithnetic.  */
+            switch ((u << 2) | size) {
+            case 0: /* VADD */
+                gen_op_neon_add_f32();
+                break;
+            case 2: /* VSUB */
+                gen_op_neon_sub_f32();
+                break;
+            case 4: /* VPADD */
+                gen_op_neon_add_f32();
+                break;
+            case 6: /* VABD */
+                gen_op_neon_abd_f32();
+                break;
+            default:
+                return 1;
+            }
+            break;
+        case 27: /* Float multiply.  */
+            gen_op_neon_mul_f32();
+            if (!u) {
+                NEON_GET_REG(T1, rd, pass);
+                if (size == 0) {
+                    gen_op_neon_add_f32();
+                } else {
+                    gen_op_neon_rsb_f32();
+                }
+            }
+            break;
+        case 28: /* Float compare.  */
+            if (!u) {
+                gen_op_neon_ceq_f32();
             } else {
-                gen_movl_reg_T1(s, rd);
-                if (logic_cc)
-                    gen_op_logic_T1_cc();
+                if (size == 0)
+                    gen_op_neon_cge_f32();
+                else
+                    gen_op_neon_cgt_f32();
             }
             break;
-        case 0x0e:
-            gen_op_bicl_T0_T1();
-            gen_movl_reg_T0(s, rd);
-            if (logic_cc)
-                gen_op_logic_T0_cc();
+        case 29: /* Float compare absolute.  */
+            if (!u)
+                return 1;
+            if (size == 0)
+                gen_op_neon_acge_f32();
+            else
+                gen_op_neon_acgt_f32();
             break;
-        default:
-        case 0x0f:
-            gen_op_notl_T1();
-            gen_movl_reg_T1(s, rd);
-            if (logic_cc)
-                gen_op_logic_T1_cc();
+        case 30: /* Float min/max.  */
+            if (size == 0)
+                gen_op_neon_max_f32();
+            else
+                gen_op_neon_min_f32();
+            break;
+        case 31:
+            if (size == 0)
+                gen_op_neon_recps_f32();
+            else
+                gen_op_neon_rsqrts_f32();
             break;
+        default:
+            abort();
         }
-    } else {
-        /* other instructions */
-        op1 = (insn >> 24) & 0xf;
-        switch(op1) {
-        case 0x0:
-        case 0x1:
-            /* multiplies, extra load/stores */
-            sh = (insn >> 5) & 3;
-            if (sh == 0) {
-                if (op1 == 0x0) {
-                    rd = (insn >> 16) & 0xf;
-                    rn = (insn >> 12) & 0xf;
-                    rs = (insn >> 8) & 0xf;
-                    rm = (insn) & 0xf;
-                    if (((insn >> 22) & 3) == 0) {
-                        /* 32 bit mul */
-                        gen_movl_T0_reg(s, rs);
-                        gen_movl_T1_reg(s, rm);
-                        gen_op_mul_T0_T1();
-                        if (insn & (1 << 21)) {
-                            gen_movl_T1_reg(s, rn);
-                            gen_op_addl_T0_T1();
-                        }
-                        if (insn & (1 << 20))
-                            gen_op_logic_T0_cc();
-                        gen_movl_reg_T0(s, rd);
+        /* Save the result.  For elementwise operations we can put it
+           straight into the destination register.  For pairwise operations
+           we have to be careful to avoid clobbering the source operands.  */
+        if (pairwise && rd == rm) {
+            gen_neon_movl_scratch_T0(pass);
+        } else {
+            NEON_SET_REG(T0, rd, pass);
+        }
+
+        } /* for pass */
+        if (pairwise && rd == rm) {
+            for (pass = 0; pass < (q ? 4 : 2); pass++) {
+                gen_neon_movl_T0_scratch(pass);
+                NEON_SET_REG(T0, rd, pass);
+            }
+        }
+    } else if (insn & (1 << 4)) {
+        if ((insn & 0x00380080) != 0) {
+            /* Two registers and shift.  */
+            op = (insn >> 8) & 0xf;
+            if (insn & (1 << 7)) {
+                /* 64-bit shift.   */
+                size = 3;
+            } else {
+                size = 2;
+                while ((insn & (1 << (size + 19))) == 0)
+                    size--;
+            }
+            shift = (insn >> 16) & ((1 << (3 + size)) - 1);
+            /* To avoid excessive dumplication of ops we implement shift
+               by immediate using the variable shift operations.  */
+            if (op < 8) {
+                /* Shift by immediate:
+                   VSHR, VSRA, VRSHR, VRSRA, VSRI, VSHL, VQSHL, VQSHLU.  */
+                /* Right shifts are encoded as N - shift, where N is the
+                   element size in bits.  */
+                if (op <= 4)
+                    shift = shift - (1 << (size + 3));
+                else
+                    shift++;
+                if (size == 3) {
+                    count = q + 1;
+                } else {
+                    count = q ? 4: 2;
+                }
+                switch (size) {
+                case 0:
+                    imm = (uint8_t) shift;
+                    imm |= imm << 8;
+                    imm |= imm << 16;
+                    break;
+                case 1:
+                    imm = (uint16_t) shift;
+                    imm |= imm << 16;
+                    break;
+                case 2:
+                case 3:
+                    imm = shift;
+                    break;
+                default:
+                    abort();
+                }
+
+                for (pass = 0; pass < count; pass++) {
+                    if (size < 3) {
+                        /* Operands in T0 and T1.  */
+                        gen_op_movl_T1_im(imm);
+                        NEON_GET_REG(T0, rm, pass);
                     } else {
-                        /* 64 bit mul */
-                        gen_movl_T0_reg(s, rs);
-                        gen_movl_T1_reg(s, rm);
-                        if (insn & (1 << 22))
-                            gen_op_imull_T0_T1();
-                        else
-                            gen_op_mull_T0_T1();
-                        if (insn & (1 << 21)) /* mult accumulate */
-                            gen_op_addq_T0_T1(rn, rd);
-                        if (!(insn & (1 << 23))) { /* double accumulate */
-                            ARCH(6);
-                            gen_op_addq_lo_T0_T1(rn);
-                            gen_op_addq_lo_T0_T1(rd);
+                        /* Operands in {T0, T1} and env->vfp.scratch.  */
+                        gen_op_movl_T0_im(imm);
+                        gen_neon_movl_scratch_T0(0);
+                        gen_op_movl_T0_im((int32_t)imm >> 31);
+                        gen_neon_movl_scratch_T0(1);
+                        NEON_GET_REG(T0, rm, pass * 2);
+                        NEON_GET_REG(T1, rm, pass * 2 + 1);
+                    }
+
+                    if (gen_neon_shift_im[op][u][size] == NULL)
+                        return 1;
+                    gen_neon_shift_im[op][u][size]();
+
+                    if (op == 1 || op == 3) {
+                        /* Accumulate.  */
+                        if (size == 3) {
+                            gen_neon_movl_scratch_T0(0);
+                            gen_neon_movl_scratch_T1(1);
+                            NEON_GET_REG(T0, rd, pass * 2);
+                            NEON_GET_REG(T1, rd, pass * 2 + 1);
+                            gen_op_neon_addl_u64();
+                        } else {
+                            NEON_GET_REG(T1, rd, pass);
+                            gen_neon_add(size);
                         }
-                        if (insn & (1 << 20))
-                            gen_op_logicq_cc();
-                        gen_movl_reg_T0(s, rn);
-                        gen_movl_reg_T1(s, rd);
+                    } else if (op == 4 || (op == 5 && u)) {
+                        /* Insert */
+                        if (size == 3) {
+                            cpu_abort(env, "VS[LR]I.64 not implemented");
+                        }
+                        switch (size) {
+                        case 0:
+                            if (op == 4)
+                                imm = 0xff >> -shift;
+                            else
+                                imm = (uint8_t)(0xff << shift);
+                            imm |= imm << 8;
+                            imm |= imm << 16;
+                            break;
+                        case 1:
+                            if (op == 4)
+                                imm = 0xffff >> -shift;
+                            else
+                                imm = (uint16_t)(0xffff << shift);
+                            imm |= imm << 16;
+                            break;
+                        case 2:
+                            if (op == 4)
+                                imm = 0xffffffffu >> -shift;
+                            else
+                                imm = 0xffffffffu << shift;
+                            break;
+                        default:
+                            abort();
+                        }
+                        NEON_GET_REG(T1, rd, pass);
+                        gen_op_movl_T2_im(imm);
+                        gen_op_neon_bsl();
                     }
+                    if (size == 3) {
+                        NEON_SET_REG(T0, rd, pass * 2);
+                        NEON_SET_REG(T1, rd, pass * 2 + 1);
+                    } else {
+                        NEON_SET_REG(T0, rd, pass);
+                    }
+                } /* for pass */
+            } else if (op < 10) {
+                /* Shift by immedaiate and narrow:
+                   VSHRN, VRSHRN, VQSHRN, VQRSHRN.  */
+                shift = shift - (1 << (size + 3));
+                size++;
+                if (size == 3) {
+                    count = q + 1;
                 } else {
-                    rn = (insn >> 16) & 0xf;
-                    rd = (insn >> 12) & 0xf;
-                    if (insn & (1 << 23)) {
-                        /* load/store exclusive */
-                        goto illegal_op;
+                    count = q ? 4: 2;
+                }
+                switch (size) {
+                case 1:
+                    imm = (uint16_t) shift;
+                    imm |= imm << 16;
+                    break;
+                case 2:
+                case 3:
+                    imm = shift;
+                    break;
+                default:
+                    abort();
+                }
+
+                /* Processing MSB first means we need to do less shuffling at
+                   the end.  */
+                for (pass =  count - 1; pass >= 0; pass--) {
+                    /* Avoid clobbering the second operand before it has been
+                       written.  */
+                    n = pass;
+                    if (rd == rm)
+                        n ^= (count - 1);
+                    else
+                        n = pass;
+
+                    if (size < 3) {
+                        /* Operands in T0 and T1.  */
+                        gen_op_movl_T1_im(imm);
+                        NEON_GET_REG(T0, rm, n);
                     } else {
-                        /* SWP instruction */
-                        rm = (insn) & 0xf;
+                        /* Operands in {T0, T1} and env->vfp.scratch.  */
+                        gen_op_movl_T0_im(imm);
+                        gen_neon_movl_scratch_T0(0);
+                        gen_op_movl_T0_im((int32_t)imm >> 31);
+                        gen_neon_movl_scratch_T0(1);
+                        NEON_GET_REG(T0, rm, n * 2);
+                        NEON_GET_REG(T0, rm, n * 2 + 1);
+                    }
 
-                        gen_movl_T0_reg(s, rm);
-                        gen_movl_T1_reg(s, rn);
-                        if (insn & (1 << 22)) {
-                            gen_ldst(swpb, s);
+                    gen_neon_shift_im_narrow[q][u][size - 1]();
+
+                    if (size < 3 && (pass & 1) == 0) {
+                        gen_neon_movl_scratch_T0(0);
+                    } else {
+                        uint32_t offset;
+
+                        if (size < 3)
+                            gen_neon_movl_T1_scratch(0);
+
+                        if (op == 8 && !u) {
+                            gen_neon_narrow[size - 1]();
                         } else {
-                            gen_ldst(swpl, s);
+                            if (op == 8)
+                                gen_neon_narrow_sats[size - 2]();
+                            else
+                                gen_neon_narrow_satu[size - 1]();
                         }
-                        gen_movl_reg_T0(s, rd);
+                        if (size == 3)
+                            offset = neon_reg_offset(rd, n);
+                        else
+                            offset = neon_reg_offset(rd, n >> 1);
+                        gen_op_neon_setreg_T0(offset);
+                    }
+                } /* for pass */
+            } else if (op == 10) {
+                /* VSHLL */
+                if (q)
+                    return 1;
+                for (pass = 0; pass < 2; pass++) {
+                    /* Avoid clobbering the input operand.  */
+                    if (rd == rm)
+                        n = 1 - pass;
+                    else
+                        n = pass;
+
+                    NEON_GET_REG(T0, rm, n);
+                    GEN_NEON_INTEGER_OP(widen);
+                    if (shift != 0) {
+                        /* The shift is less than the width of the source
+                           type, so in some cases we can just
+                           shift the whole register.  */
+                        if (size == 1 || (size == 0 && u)) {
+                            gen_op_shll_T0_im(shift);
+                            gen_op_shll_T1_im(shift);
+                        } else {
+                            switch (size) {
+                            case 0: gen_op_neon_shll_u16(shift); break;
+                            case 2: gen_op_neon_shll_u64(shift); break;
+                            default: abort();
+                            }
+                        }
+                    }
+                    NEON_SET_REG(T0, rd, n * 2);
+                    NEON_SET_REG(T1, rd, n * 2 + 1);
+                }
+            } else if (op == 15 || op == 16) {
+                /* VCVT fixed-point.  */
+                for (pass = 0; pass < (q ? 4 : 2); pass++) {
+                    gen_op_vfp_getreg_F0s(neon_reg_offset(rm, pass));
+                    if (op & 1) {
+                        if (u)
+                            gen_op_vfp_ultos(shift);
+                        else
+                            gen_op_vfp_sltos(shift);
+                    } else {
+                        if (u)
+                            gen_op_vfp_touls(shift);
+                        else
+                            gen_op_vfp_tosls(shift);
                     }
+                    gen_op_vfp_setreg_F0s(neon_reg_offset(rd, pass));
                 }
             } else {
-                int address_offset;
-                int load;
-                /* Misc load/store */
-                rn = (insn >> 16) & 0xf;
+                return 1;
+            }
+        } else { /* (insn & 0x00380080) == 0 */
+            int invert;
+
+            op = (insn >> 8) & 0xf;
+            /* One register and immediate.  */
+            imm = (u << 7) | ((insn >> 12) & 0x70) | (insn & 0xf);
+            invert = (insn & (1 << 5)) != 0;
+            switch (op) {
+            case 0: case 1:
+                /* no-op */
+                break;
+            case 2: case 3:
+                imm <<= 8;
+                break;
+            case 4: case 5:
+                imm <<= 16;
+                break;
+            case 6: case 7:
+                imm <<= 24;
+                break;
+            case 8: case 9:
+                imm |= imm << 16;
+                break;
+            case 10: case 11:
+                imm = (imm << 8) | (imm << 24);
+                break;
+            case 12:
+                imm = (imm < 8) | 0xff;
+                break;
+            case 13:
+                imm = (imm << 16) | 0xffff;
+                break;
+            case 14:
+                imm |= (imm << 8) | (imm << 16) | (imm << 24);
+                if (invert)
+                    imm = ~imm;
+                break;
+            case 15:
+                imm = ((imm & 0x80) << 24) | ((imm & 0x3f) << 19)
+                      | ((imm & 0x40) ? (0x1f << 25) : (1 << 30));
+                break;
+            }
+            if (invert)
+                imm = ~imm;
+
+            if (op != 14 || !invert)
+                gen_op_movl_T1_im(imm);
+
+            for (pass = 0; pass < (q ? 4 : 2); pass++) {
+                if (op & 1 && op < 12) {
+                    NEON_GET_REG(T0, rd, pass);
+                    if (invert) {
+                        /* The immediate value has already been inverted, so
+                           BIC becomes AND.  */
+                        gen_op_andl_T0_T1();
+                    } else {
+                        gen_op_orl_T0_T1();
+                    }
+                    NEON_SET_REG(T0, rd, pass);
+                } else {
+                    if (op == 14 && invert) {
+                        uint32_t tmp;
+                        tmp = 0;
+                        for (n = 0; n < 4; n++) {
+                            if (imm & (1 << (n + (pass & 1) * 4)))
+                                tmp |= 0xff << (n * 8);
+                        }
+                        gen_op_movl_T1_im(tmp);
+                    }
+                    /* VMOV, VMVN.  */
+                    NEON_SET_REG(T1, rd, pass);
+                }
+            }
+        }
+    } else { /* (insn & 0x00800010 == 0x00800010) */
+        if (size != 3) {
+            op = (insn >> 8) & 0xf;
+            if ((insn & (1 << 6)) == 0) {
+                /* Three registers of different lengths.  */
+                int src1_wide;
+                int src2_wide;
+                int prewiden;
+                /* prewiden, src1_wide, src2_wide */
+                static const int neon_3reg_wide[16][3] = {
+                    {1, 0, 0}, /* VADDL */
+                    {1, 1, 0}, /* VADDW */
+                    {1, 0, 0}, /* VSUBL */
+                    {1, 1, 0}, /* VSUBW */
+                    {0, 1, 1}, /* VADDHN */
+                    {0, 0, 0}, /* VABAL */
+                    {0, 1, 1}, /* VSUBHN */
+                    {0, 0, 0}, /* VABDL */
+                    {0, 0, 0}, /* VMLAL */
+                    {0, 0, 0}, /* VQDMLAL */
+                    {0, 0, 0}, /* VMLSL */
+                    {0, 0, 0}, /* VQDMLSL */
+                    {0, 0, 0}, /* Integer VMULL */
+                    {0, 0, 0}, /* VQDMULL */
+                    {0, 0, 0}  /* Polynomial VMULL */
+                };
+
+                prewiden = neon_3reg_wide[op][0];
+                src1_wide = neon_3reg_wide[op][1];
+                src2_wide = neon_3reg_wide[op][2];
+
+                /* Avoid overlapping operands.  Wide source operands are
+                   always aligned so will never overlap with wide
+                   destinations in problematic ways.  */
+                if (rd == rm) {
+                    NEON_GET_REG(T2, rm, 1);
+                } else if (rd == rn) {
+                    NEON_GET_REG(T2, rn, 1);
+                }
+                for (pass = 0; pass < 2; pass++) {
+                    /* Load the second operand into env->vfp.scratch.
+                       Also widen narrow operands.  */
+                    if (pass == 1 && rd == rm) {
+                        if (prewiden) {
+                            gen_op_movl_T0_T2();
+                        } else {
+                            gen_op_movl_T1_T2();
+                        }
+                    } else {
+                        if (src2_wide) {
+                            NEON_GET_REG(T0, rm, pass * 2);
+                            NEON_GET_REG(T1, rm, pass * 2 + 1);
+                        } else {
+                            if (prewiden) {
+                                NEON_GET_REG(T0, rm, pass);
+                            } else {
+                                NEON_GET_REG(T1, rm, pass);
+                            }
+                        }
+                    }
+                    if (prewiden && !src2_wide) {
+                        GEN_NEON_INTEGER_OP(widen);
+                    }
+                    if (prewiden || src2_wide) {
+                        gen_neon_movl_scratch_T0(0);
+                        gen_neon_movl_scratch_T1(1);
+                    }
+
+                    /* Load the first operand.  */
+                    if (pass == 1 && rd == rn) {
+                        gen_op_movl_T0_T2();
+                    } else {
+                        if (src1_wide) {
+                            NEON_GET_REG(T0, rn, pass * 2);
+                            NEON_GET_REG(T1, rn, pass * 2 + 1);
+                        } else {
+                            NEON_GET_REG(T0, rn, pass);
+                        }
+                    }
+                    if (prewiden && !src1_wide) {
+                        GEN_NEON_INTEGER_OP(widen);
+                    }
+                    switch (op) {
+                    case 0: case 1: case 4: /* VADDL, VADDW, VADDHN, VRADDHN */
+                        switch (size) {
+                        case 0: gen_op_neon_addl_u16(); break;
+                        case 1: gen_op_neon_addl_u32(); break;
+                        case 2: gen_op_neon_addl_u64(); break;
+                        default: abort();
+                        }
+                        break;
+                    case 2: case 3: case 6: /* VSUBL, VSUBW, VSUBHL, VRSUBHL */
+                        switch (size) {
+                        case 0: gen_op_neon_subl_u16(); break;
+                        case 1: gen_op_neon_subl_u32(); break;
+                        case 2: gen_op_neon_subl_u64(); break;
+                        default: abort();
+                        }
+                        break;
+                    case 5: case 7: /* VABAL, VABDL */
+                        switch ((size << 1) | u) {
+                        case 0: gen_op_neon_abdl_s16(); break;
+                        case 1: gen_op_neon_abdl_u16(); break;
+                        case 2: gen_op_neon_abdl_s32(); break;
+                        case 3: gen_op_neon_abdl_u32(); break;
+                        case 4: gen_op_neon_abdl_s64(); break;
+                        case 5: gen_op_neon_abdl_u64(); break;
+                        default: abort();
+                        }
+                        break;
+                    case 8: case 9: case 10: case 11: case 12: case 13:
+                        /* VMLAL, VQDMLAL, VMLSL, VQDMLSL, VMULL, VQDMULL */
+                        switch ((size << 1) | u) {
+                        case 0: gen_op_neon_mull_s8(); break;
+                        case 1: gen_op_neon_mull_u8(); break;
+                        case 2: gen_op_neon_mull_s16(); break;
+                        case 3: gen_op_neon_mull_u16(); break;
+                        case 4: gen_op_imull_T0_T1(); break;
+                        case 5: gen_op_mull_T0_T1(); break;
+                        default: abort();
+                        }
+                        break;
+                    case 14: /* Polynomial VMULL */
+                        cpu_abort(env, "Polynomial VMULL not implemented");
+
+                    default: /* 15 is RESERVED.  */
+                        return 1;
+                    }
+                    if (op == 5 || op == 13 || (op >= 8 && op <= 11)) {
+                        /* Accumulate.  */
+                        if (op == 10 || op == 11) {
+                            switch (size) {
+                            case 0: gen_op_neon_negl_u16(); break;
+                            case 1: gen_op_neon_negl_u32(); break;
+                            case 2: gen_op_neon_negl_u64(); break;
+                            default: abort();
+                            }
+                        }
+
+                        gen_neon_movl_scratch_T0(0);
+                        gen_neon_movl_scratch_T1(1);
+
+                        if (op != 13) {
+                            NEON_GET_REG(T0, rd, pass * 2);
+                            NEON_GET_REG(T1, rd, pass * 2 + 1);
+                        }
+
+                        switch (op) {
+                        case 5: case 8: case 10: /* VABAL, VMLAL, VMLSL */
+                            switch (size) {
+                            case 0: gen_op_neon_addl_u16(); break;
+                            case 1: gen_op_neon_addl_u32(); break;
+                            case 2: gen_op_neon_addl_u64(); break;
+                            default: abort();
+                            }
+                            break;
+                        case 9: case 11: /* VQDMLAL, VQDMLSL */
+                            switch (size) {
+                            case 1: gen_op_neon_addl_saturate_s32(); break;
+                            case 2: gen_op_neon_addl_saturate_s64(); break;
+                            default: abort();
+                            }
+                            /* Fall through.  */
+                        case 13: /* VQDMULL */
+                            switch (size) {
+                            case 1: gen_op_neon_addl_saturate_s32(); break;
+                            case 2: gen_op_neon_addl_saturate_s64(); break;
+                            default: abort();
+                            }
+                            break;
+                        default:
+                            abort();
+                        }
+                        NEON_SET_REG(T0, rd, pass * 2);
+                        NEON_SET_REG(T1, rd, pass * 2 + 1);
+                    } else if (op == 4 || op == 6) {
+                        /* Narrowing operation.  */
+                        if (u) {
+                            switch (size) {
+                            case 0: gen_op_neon_narrow_high_u8(); break;
+                            case 1: gen_op_neon_narrow_high_u16(); break;
+                            case 2: gen_op_movl_T0_T1(); break;
+                            default: abort();
+                            }
+                        } else {
+                            switch (size) {
+                            case 0: gen_op_neon_narrow_high_round_u8(); break;
+                            case 1: gen_op_neon_narrow_high_round_u16(); break;
+                            case 2: gen_op_neon_narrow_high_round_u32(); break;
+                            default: abort();
+                            }
+                        }
+                        NEON_SET_REG(T0, rd, pass);
+                    } else {
+                        /* Write back the result.  */
+                        NEON_SET_REG(T0, rd, pass * 2);
+                        NEON_SET_REG(T1, rd, pass * 2 + 1);
+                    }
+                }
+            } else {
+                /* Two registers and a scalar.  */
+                switch (op) {
+                case 0: /* Integer VMLA scalar */
+                case 1: /* Float VMLA scalar */
+                case 4: /* Integer VMLS scalar */
+                case 5: /* Floating point VMLS scalar */
+                case 8: /* Integer VMUL scalar */
+                case 9: /* Floating point VMUL scalar */
+                case 12: /* VQDMULH scalar */
+                case 13: /* VQRDMULH scalar */
+                    gen_neon_get_scalar(size, rm);
+                    gen_op_movl_T2_T0();
+                    for (pass = 0; pass < (u ? 4 : 2); pass++) {
+                        if (pass != 0)
+                            gen_op_movl_T0_T2();
+                        NEON_GET_REG(T1, rn, pass);
+                        if (op == 12) {
+                            if (size == 1) {
+                                gen_op_neon_qdmulh_s16();
+                            } else {
+                                gen_op_neon_qdmulh_s32();
+                            }
+                        } else if (op == 13) {
+                            if (size == 1) {
+                                gen_op_neon_qrdmulh_s16();
+                            } else {
+                                gen_op_neon_qrdmulh_s32();
+                            }
+                        } else if (op & 1) {
+                            gen_op_neon_mul_f32();
+                        } else {
+                            switch (size) {
+                            case 0: gen_op_neon_mul_u8(); break;
+                            case 1: gen_op_neon_mul_u16(); break;
+                            case 2: gen_op_mul_T0_T1(); break;
+                            default: return 1;
+                            }
+                        }
+                        if (op < 8) {
+                            /* Accumulate.  */
+                            NEON_GET_REG(T1, rd, pass);
+                            switch (op) {
+                            case 0:
+                                gen_neon_add(size);
+                                break;
+                            case 1:
+                                gen_op_neon_add_f32();
+                                break;
+                            case 4:
+                                switch (size) {
+                                case 0: gen_op_neon_rsb_u8(); break;
+                                case 1: gen_op_neon_rsb_u16(); break;
+                                case 2: gen_op_rsbl_T0_T1(); break;
+                                default: return 1;
+                                }
+                                break;
+                            case 5:
+                                gen_op_neon_rsb_f32();
+                                break;
+                            default:
+                                abort();
+                            }
+                        }
+                        NEON_SET_REG(T0, rd, pass);
+                    }
+                    break;
+                case 2: /* VMLAL sclar */
+                case 3: /* VQDMLAL scalar */
+                case 6: /* VMLSL scalar */
+                case 7: /* VQDMLSL scalar */
+                case 10: /* VMULL scalar */
+                case 11: /* VQDMULL scalar */
+                    if (rd == rn) {
+                        /* Save overlapping operands before they are
+                           clobbered.  */
+                        NEON_GET_REG(T0, rn, 1);
+                        gen_neon_movl_scratch_T0(2);
+                    }
+                    gen_neon_get_scalar(size, rm);
+                    gen_op_movl_T2_T0();
+                    for (pass = 0; pass < 2; pass++) {
+                        if (pass != 0) {
+                            gen_op_movl_T0_T2();
+                        }
+                        if (pass != 0 && rd == rn) {
+                            gen_neon_movl_T1_scratch(2);
+                        } else {
+                            NEON_GET_REG(T1, rn, pass);
+                        }
+                        switch ((size << 1) | u) {
+                        case 0: gen_op_neon_mull_s8(); break;
+                        case 1: gen_op_neon_mull_u8(); break;
+                        case 2: gen_op_neon_mull_s16(); break;
+                        case 3: gen_op_neon_mull_u16(); break;
+                        case 4: gen_op_imull_T0_T1(); break;
+                        case 5: gen_op_mull_T0_T1(); break;
+                        default: abort();
+                        }
+                        if (op == 6 || op == 7) {
+                            switch (size) {
+                            case 0: gen_op_neon_negl_u16(); break;
+                            case 1: gen_op_neon_negl_u32(); break;
+                            case 2: gen_op_neon_negl_u64(); break;
+                            default: abort();
+                            }
+                        }
+                        gen_neon_movl_scratch_T0(0);
+                        gen_neon_movl_scratch_T1(1);
+                        NEON_GET_REG(T0, rd, pass * 2);
+                        NEON_GET_REG(T1, rd, pass * 2 + 1);
+                        switch (op) {
+                        case 2: case 6:
+                            switch (size) {
+                            case 0: gen_op_neon_addl_u16(); break;
+                            case 1: gen_op_neon_addl_u32(); break;
+                            case 2: gen_op_neon_addl_u64(); break;
+                            default: abort();
+                            }
+                            break;
+                        case 3: case 7:
+                            switch (size) {
+                            case 1:
+                                gen_op_neon_addl_saturate_s32();
+                                gen_op_neon_addl_saturate_s32();
+                                break;
+                            case 2:
+                                gen_op_neon_addl_saturate_s64();
+                                gen_op_neon_addl_saturate_s64();
+                                break;
+                            default: abort();
+                            }
+                            break;
+                        case 10:
+                            /* no-op */
+                            break;
+                        case 11:
+                            switch (size) {
+                            case 1: gen_op_neon_addl_saturate_s32(); break;
+                            case 2: gen_op_neon_addl_saturate_s64(); break;
+                            default: abort();
+                            }
+                            break;
+                        default:
+                            abort();
+                        }
+                        NEON_SET_REG(T0, rd, pass * 2);
+                        NEON_SET_REG(T1, rd, pass * 2 + 1);
+                    }
+                    break;
+                default: /* 14 and 15 are RESERVED */
+                    return 1;
+                }
+            }
+        } else { /* size == 3 */
+            if (!u) {
+                /* Extract.  */
+                int reg;
+                imm = (insn >> 8) & 0xf;
+                reg = rn;
+                count = q ? 4 : 2;
+                n = imm >> 2;
+                NEON_GET_REG(T0, reg, n);
+                for (pass = 0; pass < count; pass++) {
+                    n++;
+                    if (n > count) {
+                        reg = rm;
+                        n -= count;
+                    }
+                    if (imm & 3) {
+                        NEON_GET_REG(T1, reg, n);
+                        gen_op_neon_extract((insn << 3) & 0x1f);
+                    }
+                    /* ??? This is broken if rd and rm overlap */
+                    NEON_SET_REG(T0, rd, pass);
+                    if (imm & 3) {
+                        gen_op_movl_T0_T1();
+                    } else {
+                        NEON_GET_REG(T0, reg, n);
+                    }
+                }
+            } else if ((insn & (1 << 11)) == 0) {
+                /* Two register misc.  */
+                op = ((insn >> 12) & 0x30) | ((insn >> 7) & 0xf);
+                size = (insn >> 18) & 3;
+                switch (op) {
+                case 0: /* VREV64 */
+                    if (size == 3)
+                        return 1;
+                    for (pass = 0; pass < (q ? 2 : 1); pass++) {
+                        NEON_GET_REG(T0, rm, pass * 2);
+                        NEON_GET_REG(T1, rm, pass * 2 + 1);
+                        switch (size) {
+                        case 0: gen_op_rev_T0(); break;
+                        case 1: gen_op_revh_T0(); break;
+                        case 2: /* no-op */ break;
+                        default: abort();
+                        }
+                        NEON_SET_REG(T0, rd, pass * 2 + 1);
+                        if (size == 2) {
+                            NEON_SET_REG(T1, rd, pass * 2);
+                        } else {
+                            gen_op_movl_T0_T1();
+                            switch (size) {
+                            case 0: gen_op_rev_T0(); break;
+                            case 1: gen_op_revh_T0(); break;
+                            default: abort();
+                            }
+                            NEON_SET_REG(T0, rd, pass * 2);
+                        }
+                    }
+                    break;
+                case 4: case 5: /* VPADDL */
+                case 12: case 13: /* VPADAL */
+                    if (size < 2)
+                        goto elementwise;
+                    if (size == 3)
+                        return 1;
+                    for (pass = 0; pass < (q ? 2 : 1); pass++) {
+                        NEON_GET_REG(T0, rm, pass * 2);
+                        NEON_GET_REG(T1, rm, pass * 2 + 1);
+                        if (op & 1)
+                            gen_op_neon_paddl_u32();
+                        else
+                            gen_op_neon_paddl_s32();
+                        if (op >= 12) {
+                            /* Accumulate.  */
+                            gen_neon_movl_scratch_T0(0);
+                            gen_neon_movl_scratch_T1(1);
+
+                            NEON_GET_REG(T0, rd, pass * 2);
+                            NEON_GET_REG(T1, rd, pass * 2 + 1);
+                            gen_op_neon_addl_u64();
+                        }
+                        NEON_SET_REG(T0, rd, pass * 2);
+                        NEON_SET_REG(T1, rd, pass * 2 + 1);
+                    }
+                    break;
+                case 33: /* VTRN */
+                    if (size == 2) {
+                        for (n = 0; n < (q ? 4 : 2); n += 2) {
+                            NEON_GET_REG(T0, rm, n);
+                            NEON_GET_REG(T1, rd, n + 1);
+                            NEON_SET_REG(T1, rm, n);
+                            NEON_SET_REG(T0, rd, n + 1);
+                        }
+                    } else {
+                        goto elementwise;
+                    }
+                    break;
+                case 34: /* VUZP */
+                    /* Reg  Before       After
+                       Rd   A3 A2 A1 A0  B2 B0 A2 A0
+                       Rm   B3 B2 B1 B0  B3 B1 A3 A1
+                     */
+                    if (size == 3)
+                        return 1;
+                    gen_neon_unzip(rd, q, 0, size);
+                    gen_neon_unzip(rm, q, 4, size);
+                    if (q) {
+                        static int unzip_order_q[8] =
+                            {0, 2, 4, 6, 1, 3, 5, 7};
+                        for (n = 0; n < 8; n++) {
+                            int reg = (n < 4) ? rd : rm;
+                            gen_neon_movl_T0_scratch(unzip_order_q[n]);
+                            NEON_SET_REG(T0, reg, n % 4);
+                        }
+                    } else {
+                        static int unzip_order[4] =
+                            {0, 4, 1, 5};
+                        for (n = 0; n < 4; n++) {
+                            int reg = (n < 2) ? rd : rm;
+                            gen_neon_movl_T0_scratch(unzip_order[n]);
+                            NEON_SET_REG(T0, reg, n % 2);
+                        }
+                    }
+                    break;
+                case 35: /* VZIP */
+                    /* Reg  Before       After
+                       Rd   A3 A2 A1 A0  B1 A1 B0 A0
+                       Rm   B3 B2 B1 B0  B3 A3 B2 A2
+                     */
+                    if (size == 3)
+                        return 1;
+                    count = (q ? 4 : 2);
+                    for (n = 0; n < count; n++) {
+                        NEON_GET_REG(T0, rd, n);
+                        NEON_GET_REG(T1, rd, n);
+                        switch (size) {
+                        case 0: gen_op_neon_zip_u8(); break;
+                        case 1: gen_op_neon_zip_u16(); break;
+                        case 2: /* no-op */; break;
+                        default: abort();
+                        }
+                        gen_neon_movl_scratch_T0(n * 2);
+                        gen_neon_movl_scratch_T1(n * 2 + 1);
+                    }
+                    for (n = 0; n < count * 2; n++) {
+                        int reg = (n < count) ? rd : rm;
+                        gen_neon_movl_T0_scratch(n);
+                        NEON_SET_REG(T0, reg, n % count);
+                    }
+                    break;
+                case 36: case 37: /* VMOVN, VQMOVUN, VQMOVN */
+                    for (pass = 0; pass < 2; pass++) {
+                        if (rd == rm + 1) {
+                            n = 1 - pass;
+                        } else {
+                            n = pass;
+                        }
+                        NEON_GET_REG(T0, rm, n * 2);
+                        NEON_GET_REG(T1, rm, n * 2 + 1);
+                        if (op == 36 && q == 0) {
+                            switch (size) {
+                            case 0: gen_op_neon_narrow_u8(); break;
+                            case 1: gen_op_neon_narrow_u16(); break;
+                            case 2: /* no-op */ break;
+                            default: return 1;
+                            }
+                        } else if (q) {
+                            switch (size) {
+                            case 0: gen_op_neon_narrow_sat_u8(); break;
+                            case 1: gen_op_neon_narrow_sat_u16(); break;
+                            case 2: gen_op_neon_narrow_sat_u32(); break;
+                            default: return 1;
+                            }
+                        } else {
+                            switch (size) {
+                            case 0: gen_op_neon_narrow_sat_s8(); break;
+                            case 1: gen_op_neon_narrow_sat_s16(); break;
+                            case 2: gen_op_neon_narrow_sat_s32(); break;
+                            default: return 1;
+                            }
+                        }
+                        NEON_SET_REG(T0, rd, n);
+                    }
+                    break;
+                case 38: /* VSHLL */
+                    if (q)
+                        return 1;
+                    if (rm == rd) {
+                        NEON_GET_REG(T2, rm, 1);
+                    }
+                    for (pass = 0; pass < 2; pass++) {
+                        if (pass == 1 && rm == rd) {
+                            gen_op_movl_T0_T2();
+                        } else {
+                            NEON_GET_REG(T0, rm, pass);
+                        }
+                        switch (size) {
+                        case 0: gen_op_neon_widen_high_u8(); break;
+                        case 1: gen_op_neon_widen_high_u16(); break;
+                        case 2:
+                            gen_op_movl_T1_T0();
+                            gen_op_movl_T0_im(0);
+                            break;
+                        default: return 1;
+                        }
+                        NEON_SET_REG(T0, rd, pass * 2);
+                        NEON_SET_REG(T1, rd, pass * 2 + 1);
+                    }
+                    break;
+                default:
+                elementwise:
+                    for (pass = 0; pass < (q ? 4 : 2); pass++) {
+                        if (op == 30 || op == 31 || op >= 58) {
+                            gen_op_vfp_getreg_F0s(neon_reg_offset(rm, pass));
+                        } else {
+                            NEON_GET_REG(T0, rm, pass);
+                        }
+                        switch (op) {
+                        case 1: /* VREV32 */
+                            switch (size) {
+                            case 0: gen_op_rev_T0(); break;
+                            case 1: gen_op_revh_T0(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 2: /* VREV16 */
+                            if (size != 0)
+                                return 1;
+                            gen_op_rev16_T0();
+                            break;
+                        case 4: case 5: /* VPADDL */
+                        case 12: case 13: /* VPADAL */
+                            switch ((size << 1) | (op & 1)) {
+                            case 0: gen_op_neon_paddl_s8(); break;
+                            case 1: gen_op_neon_paddl_u8(); break;
+                            case 2: gen_op_neon_paddl_s16(); break;
+                            case 3: gen_op_neon_paddl_u16(); break;
+                            default: abort();
+                            }
+                            if (op >= 12) {
+                                /* Accumulate */
+                                NEON_GET_REG(T1, rd, pass);
+                                switch (size) {
+                                case 0: gen_op_neon_add_u16(); break;
+                                case 1: gen_op_addl_T0_T1(); break;
+                                default: abort();
+                                }
+                            }
+                            break;
+                        case 8: /* CLS */
+                            switch (size) {
+                            case 0: gen_op_neon_cls_s8(); break;
+                            case 1: gen_op_neon_cls_s16(); break;
+                            case 2: gen_op_neon_cls_s32(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 9: /* CLZ */
+                            switch (size) {
+                            case 0: gen_op_neon_clz_u8(); break;
+                            case 1: gen_op_neon_clz_u16(); break;
+                            case 2: gen_op_clz_T0(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 10: /* CNT */
+                            if (size != 0)
+                                return 1;
+                            gen_op_neon_cnt_u8();
+                            break;
+                        case 11: /* VNOT */
+                            if (size != 0)
+                                return 1;
+                            gen_op_notl_T0();
+                            break;
+                        case 14: /* VQABS */
+                            switch (size) {
+                            case 0: gen_op_neon_qabs_s8(); break;
+                            case 1: gen_op_neon_qabs_s16(); break;
+                            case 2: gen_op_neon_qabs_s32(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 15: /* VQNEG */
+                            switch (size) {
+                            case 0: gen_op_neon_qneg_s8(); break;
+                            case 1: gen_op_neon_qneg_s16(); break;
+                            case 2: gen_op_neon_qneg_s32(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 16: case 19: /* VCGT #0, VCLE #0 */
+                            gen_op_movl_T1_im(0);
+                            switch(size) {
+                            case 0: gen_op_neon_cgt_s8(); break;
+                            case 1: gen_op_neon_cgt_s16(); break;
+                            case 2: gen_op_neon_cgt_s32(); break;
+                            default: return 1;
+                            }
+                            if (op == 19)
+                                gen_op_notl_T0();
+                            break;
+                        case 17: case 20: /* VCGE #0, VCLT #0 */
+                            gen_op_movl_T1_im(0);
+                            switch(size) {
+                            case 0: gen_op_neon_cge_s8(); break;
+                            case 1: gen_op_neon_cge_s16(); break;
+                            case 2: gen_op_neon_cge_s32(); break;
+                            default: return 1;
+                            }
+                            if (op == 20)
+                                gen_op_notl_T0();
+                            break;
+                        case 18: /* VCEQ #0 */
+                            gen_op_movl_T1_im(0);
+                            switch(size) {
+                            case 0: gen_op_neon_ceq_u8(); break;
+                            case 1: gen_op_neon_ceq_u16(); break;
+                            case 2: gen_op_neon_ceq_u32(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 22: /* VABS */
+                            switch(size) {
+                            case 0: gen_op_neon_abs_s8(); break;
+                            case 1: gen_op_neon_abs_s16(); break;
+                            case 2: gen_op_neon_abs_s32(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 23: /* VNEG */
+                            gen_op_movl_T1_im(0);
+                            switch(size) {
+                            case 0: gen_op_neon_rsb_u8(); break;
+                            case 1: gen_op_neon_rsb_u16(); break;
+                            case 2: gen_op_rsbl_T0_T1(); break;
+                            default: return 1;
+                            }
+                            break;
+                        case 24: case 27: /* Float VCGT #0, Float VCLE #0 */
+                            gen_op_movl_T1_im(0);
+                            gen_op_neon_cgt_f32();
+                            if (op == 27)
+                                gen_op_notl_T0();
+                            break;
+                        case 25: case 28: /* Float VCGE #0, Float VCLT #0 */
+                            gen_op_movl_T1_im(0);
+                            gen_op_neon_cge_f32();
+                            if (op == 28)
+                                gen_op_notl_T0();
+                            break;
+                        case 26: /* Float VCEQ #0 */
+                            gen_op_movl_T1_im(0);
+                            gen_op_neon_ceq_f32();
+                            break;
+                        case 30: /* Float VABS */
+                            gen_op_vfp_abss();
+                            break;
+                        case 31: /* Float VNEG */
+                            gen_op_vfp_negs();
+                            break;
+                        case 32: /* VSWP */
+                            NEON_GET_REG(T1, rd, pass);
+                            NEON_SET_REG(T1, rm, pass);
+                            break;
+                        case 33: /* VTRN */
+                            NEON_GET_REG(T1, rd, pass);
+                            switch (size) {
+                            case 0: gen_op_neon_trn_u8(); break;
+                            case 1: gen_op_neon_trn_u16(); break;
+                            case 2: abort();
+                            default: return 1;
+                            }
+                            NEON_SET_REG(T1, rm, pass);
+                            break;
+                        case 56: /* Integer VRECPE */
+                            gen_op_neon_recpe_u32();
+                            break;
+                        case 57: /* Integer VRSQRTE */
+                            gen_op_neon_rsqrte_u32();
+                            break;
+                        case 58: /* Float VRECPE */
+                            gen_op_neon_recpe_f32();
+                            break;
+                        case 59: /* Float VRSQRTE */
+                            gen_op_neon_rsqrte_f32();
+                            break;
+                        case 60: /* VCVT.F32.S32 */
+                            gen_op_vfp_tosizs();
+                            break;
+                        case 61: /* VCVT.F32.U32 */
+                            gen_op_vfp_touizs();
+                            break;
+                        case 62: /* VCVT.S32.F32 */
+                            gen_op_vfp_sitos();
+                            break;
+                        case 63: /* VCVT.U32.F32 */
+                            gen_op_vfp_uitos();
+                            break;
+                        default:
+                            /* Reserved: 21, 29, 39-56 */
+                            return 1;
+                        }
+                        if (op == 30 || op == 31 || op >= 58) {
+                            gen_op_vfp_setreg_F0s(neon_reg_offset(rm, pass));
+                        } else {
+                            NEON_SET_REG(T0, rd, pass);
+                        }
+                    }
+                    break;
+                }
+            } else if ((insn & (1 << 10)) == 0) {
+                /* VTBL, VTBX.  */
+                n = (insn >> 5) & 0x18;
+                NEON_GET_REG(T1, rm, 0);
+                if (insn & (1 << 6)) {
+                    NEON_GET_REG(T0, rd, 0);
+                } else {
+                    gen_op_movl_T0_im(0);
+                }
+                gen_op_neon_tbl(rn, n);
+                gen_op_movl_T2_T0();
+                NEON_GET_REG(T1, rm, 1);
+                if (insn & (1 << 6)) {
+                    NEON_GET_REG(T0, rd, 0);
+                } else {
+                    gen_op_movl_T0_im(0);
+                }
+                gen_op_neon_tbl(rn, n);
+                NEON_SET_REG(T2, rd, 0);
+                NEON_SET_REG(T0, rd, 1);
+            } else if ((insn & 0x380) == 0) {
+                /* VDUP */
+                if (insn & (1 << 19)) {
+                    NEON_SET_REG(T0, rm, 1);
+                } else {
+                    NEON_SET_REG(T0, rm, 0);
+                }
+                if (insn & (1 << 16)) {
+                    gen_op_neon_dup_u8(((insn >> 17) & 3) * 8);
+                } else if (insn & (1 << 17)) {
+                    if ((insn >> 18) & 1)
+                        gen_op_neon_dup_high16();
+                    else
+                        gen_op_neon_dup_low16();
+                }
+                for (pass = 0; pass < (q ? 4 : 2); pass++) {
+                    NEON_SET_REG(T0, rd, pass);
+                }
+            } else {
+                return 1;
+            }
+        }
+    }
+    return 0;
+}
+
+static int disas_coproc_insn(CPUState * env, DisasContext *s, uint32_t insn)
+{
+    int cpnum;
+
+    cpnum = (insn >> 8) & 0xf;
+    if (arm_feature(env, ARM_FEATURE_XSCALE)
+	    && ((env->cp15.c15_cpar ^ 0x3fff) & (1 << cpnum)))
+	return 1;
+
+    switch (cpnum) {
+      case 0:
+      case 1:
+	if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
+	    return disas_iwmmxt_insn(env, s, insn);
+	} else if (arm_feature(env, ARM_FEATURE_XSCALE)) {
+	    return disas_dsp_insn(env, s, insn);
+	}
+	return 1;
+    case 10:
+    case 11:
+	return disas_vfp_insn (env, s, insn);
+    case 15:
+	return disas_cp15_insn (env, s, insn);
+    default:
+	/* Unknown coprocessor.  See if the board has hooked it.  */
+	return disas_cp_insn (env, s, insn);
+    }
+}
+
+static void disas_arm_insn(CPUState * env, DisasContext *s)
+{
+    unsigned int cond, insn, val, op1, i, shift, rm, rs, rn, rd, sh;
+
+    insn = ldl_code(s->pc);
+    s->pc += 4;
+
+    /* M variants do not implement ARM mode.  */
+    if (IS_M(env))
+        goto illegal_op;
+    cond = insn >> 28;
+    if (cond == 0xf){
+        /* Unconditional instructions.  */
+        if (((insn >> 25) & 7) == 1) {
+            /* NEON Data processing.  */
+            if (!arm_feature(env, ARM_FEATURE_NEON))
+                goto illegal_op;
+
+            if (disas_neon_data_insn(env, s, insn))
+                goto illegal_op;
+            return;
+        }
+        if ((insn & 0x0f100000) == 0x04000000) {
+            /* NEON load/store.  */
+            if (!arm_feature(env, ARM_FEATURE_NEON))
+                goto illegal_op;
+
+            if (disas_neon_ls_insn(env, s, insn))
+                goto illegal_op;
+            return;
+        }
+        if ((insn & 0x0d70f000) == 0x0550f000)
+            return; /* PLD */
+        else if ((insn & 0x0ffffdff) == 0x01010000) {
+            ARCH(6);
+            /* setend */
+            if (insn & (1 << 9)) {
+                /* BE8 mode not implemented.  */
+                goto illegal_op;
+            }
+            return;
+        } else if ((insn & 0x0fffff00) == 0x057ff000) {
+            switch ((insn >> 4) & 0xf) {
+            case 1: /* clrex */
+                ARCH(6K);
+                gen_op_clrex();
+                return;
+            case 4: /* dsb */
+            case 5: /* dmb */
+            case 6: /* isb */
+                ARCH(7);
+                /* We don't emulate caches so these are a no-op.  */
+                return;
+            default:
+                goto illegal_op;
+            }
+        } else if ((insn & 0x0e5fffe0) == 0x084d0500) {
+            /* srs */
+            uint32_t offset;
+            if (IS_USER(s))
+                goto illegal_op;
+            ARCH(6);
+            op1 = (insn & 0x1f);
+            if (op1 == (env->uncached_cpsr & CPSR_M)) {
+                gen_movl_T1_reg(s, 13);
+            } else {
+                gen_op_movl_T1_r13_banked(op1);
+            }
+            i = (insn >> 23) & 3;
+            switch (i) {
+            case 0: offset = -4; break; /* DA */
+            case 1: offset = -8; break; /* DB */
+            case 2: offset = 0; break; /* IA */
+            case 3: offset = 4; break; /* IB */
+            default: abort();
+            }
+            if (offset)
+                gen_op_addl_T1_im(offset);
+            gen_movl_T0_reg(s, 14);
+            gen_ldst(stl, s);
+            gen_op_movl_T0_cpsr();
+            gen_op_addl_T1_im(4);
+            gen_ldst(stl, s);
+            if (insn & (1 << 21)) {
+                /* Base writeback.  */
+                switch (i) {
+                case 0: offset = -8; break;
+                case 1: offset = -4; break;
+                case 2: offset = 4; break;
+                case 3: offset = 0; break;
+                default: abort();
+                }
+                if (offset)
+                    gen_op_addl_T1_im(offset);
+                if (op1 == (env->uncached_cpsr & CPSR_M)) {
+                    gen_movl_reg_T1(s, 13);
+                } else {
+                    gen_op_movl_r13_T1_banked(op1);
+                }
+            }
+        } else if ((insn & 0x0e5fffe0) == 0x081d0a00) {
+            /* rfe */
+            uint32_t offset;
+            if (IS_USER(s))
+                goto illegal_op;
+            ARCH(6);
+            rn = (insn >> 16) & 0xf;
+            gen_movl_T1_reg(s, rn);
+            i = (insn >> 23) & 3;
+            switch (i) {
+            case 0: offset = 0; break; /* DA */
+            case 1: offset = -4; break; /* DB */
+            case 2: offset = 4; break; /* IA */
+            case 3: offset = 8; break; /* IB */
+            default: abort();
+            }
+            if (offset)
+                gen_op_addl_T1_im(offset);
+            /* Load CPSR into T2 and PC into T0.  */
+            gen_ldst(ldl, s);
+            gen_op_movl_T2_T0();
+            gen_op_addl_T1_im(-4);
+            gen_ldst(ldl, s);
+            if (insn & (1 << 21)) {
+                /* Base writeback.  */
+                switch (i) {
+                case 0: offset = -4; break;
+                case 1: offset = 0; break;
+                case 2: offset = 8; break;
+                case 3: offset = 4; break;
+                default: abort();
+                }
+                if (offset)
+                    gen_op_addl_T1_im(offset);
+                gen_movl_reg_T1(s, rn);
+            }
+            gen_rfe(s);
+        } else if ((insn & 0x0e000000) == 0x0a000000) {
+            /* branch link and change to thumb (blx <offset>) */
+            int32_t offset;
+
+            val = (uint32_t)s->pc;
+            gen_op_movl_T0_im(val);
+            gen_movl_reg_T0(s, 14);
+            /* Sign-extend the 24-bit offset */
+            offset = (((int32_t)insn) << 8) >> 8;
+            /* offset * 4 + bit24 * 2 + (thumb bit) */
+            val += (offset << 2) | ((insn >> 23) & 2) | 1;
+            /* pipeline offset */
+            val += 4;
+            gen_op_movl_T0_im(val);
+            gen_bx(s);
+            return;
+        } else if ((insn & 0x0e000f00) == 0x0c000100) {
+            if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
+                /* iWMMXt register transfer.  */
+                if (env->cp15.c15_cpar & (1 << 1))
+                    if (!disas_iwmmxt_insn(env, s, insn))
+                        return;
+            }
+        } else if ((insn & 0x0fe00000) == 0x0c400000) {
+            /* Coprocessor double register transfer.  */
+        } else if ((insn & 0x0f000010) == 0x0e000010) {
+            /* Additional coprocessor register transfer.  */
+        } else if ((insn & 0x0ff10010) == 0x01000000) {
+            uint32_t mask;
+            uint32_t val;
+            /* cps (privileged) */
+            if (IS_USER(s))
+                return;
+            mask = val = 0;
+            if (insn & (1 << 19)) {
+                if (insn & (1 << 8))
+                    mask |= CPSR_A;
+                if (insn & (1 << 7))
+                    mask |= CPSR_I;
+                if (insn & (1 << 6))
+                    mask |= CPSR_F;
+                if (insn & (1 << 18))
+                    val |= mask;
+            }
+            if (insn & (1 << 14)) {
+                mask |= CPSR_M;
+                val |= (insn & 0x1f);
+            }
+            if (mask) {
+                gen_op_movl_T0_im(val);
+                gen_set_psr_T0(s, mask, 0);
+            }
+            return;
+        }
+        goto illegal_op;
+    }
+    if (cond != 0xe) {
+        /* if not always execute, we generate a conditional jump to
+           next instruction */
+        s->condlabel = gen_new_label();
+        gen_test_cc[cond ^ 1](s->condlabel);
+        s->condjmp = 1;
+    }
+    if ((insn & 0x0f900000) == 0x03000000) {
+        if ((insn & (1 << 21)) == 0) {
+            ARCH(6T2);
+            rd = (insn >> 12) & 0xf;
+            val = ((insn >> 4) & 0xf000) | (insn & 0xfff);
+            if ((insn & (1 << 22)) == 0) {
+                /* MOVW */
+                gen_op_movl_T0_im(val);
+            } else {
+                /* MOVT */
+                gen_movl_T0_reg(s, rd);
+                gen_op_movl_T1_im(0xffff);
+                gen_op_andl_T0_T1();
+                gen_op_movl_T1_im(val << 16);
+                gen_op_orl_T0_T1();
+            }
+            gen_movl_reg_T0(s, rd);
+        } else {
+            if (((insn >> 12) & 0xf) != 0xf)
+                goto illegal_op;
+            if (((insn >> 16) & 0xf) == 0) {
+                gen_nop_hint(s, insn & 0xff);
+            } else {
+                /* CPSR = immediate */
+                val = insn & 0xff;
+                shift = ((insn >> 8) & 0xf) * 2;
+                if (shift)
+                    val = (val >> shift) | (val << (32 - shift));
+                gen_op_movl_T0_im(val);
+                i = ((insn & (1 << 22)) != 0);
+                if (gen_set_psr_T0(s, msr_mask(env, s, (insn >> 16) & 0xf, i), i))
+                    goto illegal_op;
+            }
+        }
+    } else if ((insn & 0x0f900000) == 0x01000000
+               && (insn & 0x00000090) != 0x00000090) {
+        /* miscellaneous instructions */
+        op1 = (insn >> 21) & 3;
+        sh = (insn >> 4) & 0xf;
+        rm = insn & 0xf;
+        switch (sh) {
+        case 0x0: /* move program status register */
+            if (op1 & 1) {
+                /* PSR = reg */
+                gen_movl_T0_reg(s, rm);
+                i = ((op1 & 2) != 0);
+                if (gen_set_psr_T0(s, msr_mask(env, s, (insn >> 16) & 0xf, i), i))
+                    goto illegal_op;
+            } else {
+                /* reg = PSR */
+                rd = (insn >> 12) & 0xf;
+                if (op1 & 2) {
+                    if (IS_USER(s))
+                        goto illegal_op;
+                    gen_op_movl_T0_spsr();
+                } else {
+                    gen_op_movl_T0_cpsr();
+                }
+                gen_movl_reg_T0(s, rd);
+            }
+            break;
+        case 0x1:
+            if (op1 == 1) {
+                /* branch/exchange thumb (bx).  */
+                gen_movl_T0_reg(s, rm);
+                gen_bx(s);
+            } else if (op1 == 3) {
+                /* clz */
+                rd = (insn >> 12) & 0xf;
+                gen_movl_T0_reg(s, rm);
+                gen_op_clz_T0();
+                gen_movl_reg_T0(s, rd);
+            } else {
+                goto illegal_op;
+            }
+            break;
+        case 0x2:
+            if (op1 == 1) {
+                ARCH(5J); /* bxj */
+                /* Trivial implementation equivalent to bx.  */
+                gen_movl_T0_reg(s, rm);
+                gen_bx(s);
+            } else {
+                goto illegal_op;
+            }
+            break;
+        case 0x3:
+            if (op1 != 1)
+              goto illegal_op;
+
+            /* branch link/exchange thumb (blx) */
+            val = (uint32_t)s->pc;
+            gen_op_movl_T1_im(val);
+            gen_movl_T0_reg(s, rm);
+            gen_movl_reg_T1(s, 14);
+            gen_bx(s);
+            break;
+        case 0x5: /* saturating add/subtract */
+            rd = (insn >> 12) & 0xf;
+            rn = (insn >> 16) & 0xf;
+            gen_movl_T0_reg(s, rm);
+            gen_movl_T1_reg(s, rn);
+            if (op1 & 2)
+                gen_op_double_T1_saturate();
+            if (op1 & 1)
+                gen_op_subl_T0_T1_saturate();
+            else
+                gen_op_addl_T0_T1_saturate();
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 7: /* bkpt */
+            gen_set_condexec(s);
+            gen_op_movl_T0_im((long)s->pc - 4);
+            gen_op_movl_reg_TN[0][15]();
+            gen_op_bkpt();
+            s->is_jmp = DISAS_JUMP;
+            break;
+        case 0x8: /* signed multiply */
+        case 0xa:
+        case 0xc:
+        case 0xe:
+            rs = (insn >> 8) & 0xf;
+            rn = (insn >> 12) & 0xf;
+            rd = (insn >> 16) & 0xf;
+            if (op1 == 1) {
+                /* (32 * 16) >> 16 */
+                gen_movl_T0_reg(s, rm);
+                gen_movl_T1_reg(s, rs);
+                if (sh & 4)
+                    gen_op_sarl_T1_im(16);
+                else
+                    gen_op_sxth_T1();
+                gen_op_imulw_T0_T1();
+                if ((sh & 2) == 0) {
+                    gen_movl_T1_reg(s, rn);
+                    gen_op_addl_T0_T1_setq();
+                }
+                gen_movl_reg_T0(s, rd);
+            } else {
+                /* 16 * 16 */
+                gen_movl_T0_reg(s, rm);
+                gen_movl_T1_reg(s, rs);
+                gen_mulxy(sh & 2, sh & 4);
+                if (op1 == 2) {
+                    gen_op_signbit_T1_T0();
+                    gen_op_addq_T0_T1(rn, rd);
+                    gen_movl_reg_T0(s, rn);
+                    gen_movl_reg_T1(s, rd);
+                } else {
+                    if (op1 == 0) {
+                        gen_movl_T1_reg(s, rn);
+                        gen_op_addl_T0_T1_setq();
+                    }
+                    gen_movl_reg_T0(s, rd);
+                }
+            }
+            break;
+        default:
+            goto illegal_op;
+        }
+    } else if (((insn & 0x0e000000) == 0 &&
+                (insn & 0x00000090) != 0x90) ||
+               ((insn & 0x0e000000) == (1 << 25))) {
+        int set_cc, logic_cc, shiftop;
+
+        op1 = (insn >> 21) & 0xf;
+        set_cc = (insn >> 20) & 1;
+        logic_cc = table_logic_cc[op1] & set_cc;
+
+        /* data processing instruction */
+        if (insn & (1 << 25)) {
+            /* immediate operand */
+            val = insn & 0xff;
+            shift = ((insn >> 8) & 0xf) * 2;
+            if (shift)
+                val = (val >> shift) | (val << (32 - shift));
+            gen_op_movl_T1_im(val);
+            if (logic_cc && shift)
+                gen_op_mov_CF_T1();
+        } else {
+            /* register */
+            rm = (insn) & 0xf;
+            gen_movl_T1_reg(s, rm);
+            shiftop = (insn >> 5) & 3;
+            if (!(insn & (1 << 4))) {
+                shift = (insn >> 7) & 0x1f;
+                if (shift != 0) {
+                    if (logic_cc) {
+                        gen_shift_T1_im_cc[shiftop](shift);
+                    } else {
+                        gen_shift_T1_im[shiftop](shift);
+                    }
+                } else if (shiftop != 0) {
+                    if (logic_cc) {
+                        gen_shift_T1_0_cc[shiftop]();
+                    } else {
+                        gen_shift_T1_0[shiftop]();
+                    }
+                }
+            } else {
+                rs = (insn >> 8) & 0xf;
+                gen_movl_T0_reg(s, rs);
+                if (logic_cc) {
+                    gen_shift_T1_T0_cc[shiftop]();
+                } else {
+                    gen_shift_T1_T0[shiftop]();
+                }
+            }
+        }
+        if (op1 != 0x0f && op1 != 0x0d) {
+            rn = (insn >> 16) & 0xf;
+            gen_movl_T0_reg(s, rn);
+        }
+        rd = (insn >> 12) & 0xf;
+        switch(op1) {
+        case 0x00:
+            gen_op_andl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            if (logic_cc)
+                gen_op_logic_T0_cc();
+            break;
+        case 0x01:
+            gen_op_xorl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            if (logic_cc)
+                gen_op_logic_T0_cc();
+            break;
+        case 0x02:
+            if (set_cc && rd == 15) {
+                /* SUBS r15, ... is used for exception return.  */
+                if (IS_USER(s))
+                    goto illegal_op;
+                gen_op_subl_T0_T1_cc();
+                gen_exception_return(s);
+            } else {
+                if (set_cc)
+                    gen_op_subl_T0_T1_cc();
+                else
+                    gen_op_subl_T0_T1();
+                gen_movl_reg_T0(s, rd);
+            }
+            break;
+        case 0x03:
+            if (set_cc)
+                gen_op_rsbl_T0_T1_cc();
+            else
+                gen_op_rsbl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 0x04:
+            if (set_cc)
+                gen_op_addl_T0_T1_cc();
+            else
+                gen_op_addl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 0x05:
+            if (set_cc)
+                gen_op_adcl_T0_T1_cc();
+            else
+                gen_op_adcl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 0x06:
+            if (set_cc)
+                gen_op_sbcl_T0_T1_cc();
+            else
+                gen_op_sbcl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 0x07:
+            if (set_cc)
+                gen_op_rscl_T0_T1_cc();
+            else
+                gen_op_rscl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 0x08:
+            if (set_cc) {
+                gen_op_andl_T0_T1();
+                gen_op_logic_T0_cc();
+            }
+            break;
+        case 0x09:
+            if (set_cc) {
+                gen_op_xorl_T0_T1();
+                gen_op_logic_T0_cc();
+            }
+            break;
+        case 0x0a:
+            if (set_cc) {
+                gen_op_subl_T0_T1_cc();
+            }
+            break;
+        case 0x0b:
+            if (set_cc) {
+                gen_op_addl_T0_T1_cc();
+            }
+            break;
+        case 0x0c:
+            gen_op_orl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            if (logic_cc)
+                gen_op_logic_T0_cc();
+            break;
+        case 0x0d:
+            if (logic_cc && rd == 15) {
+                /* MOVS r15, ... is used for exception return.  */
+                if (IS_USER(s))
+                    goto illegal_op;
+                gen_op_movl_T0_T1();
+                gen_exception_return(s);
+            } else {
+                gen_movl_reg_T1(s, rd);
+                if (logic_cc)
+                    gen_op_logic_T1_cc();
+            }
+            break;
+        case 0x0e:
+            gen_op_bicl_T0_T1();
+            gen_movl_reg_T0(s, rd);
+            if (logic_cc)
+                gen_op_logic_T0_cc();
+            break;
+        default:
+        case 0x0f:
+            gen_op_notl_T1();
+            gen_movl_reg_T1(s, rd);
+            if (logic_cc)
+                gen_op_logic_T1_cc();
+            break;
+        }
+    } else {
+        /* other instructions */
+        op1 = (insn >> 24) & 0xf;
+        switch(op1) {
+        case 0x0:
+        case 0x1:
+            /* multiplies, extra load/stores */
+            sh = (insn >> 5) & 3;
+            if (sh == 0) {
+                if (op1 == 0x0) {
+                    rd = (insn >> 16) & 0xf;
+                    rn = (insn >> 12) & 0xf;
+                    rs = (insn >> 8) & 0xf;
+                    rm = (insn) & 0xf;
+                    op1 = (insn >> 20) & 0xf;
+                    switch (op1) {
+                    case 0: case 1: case 2: case 3: case 6:
+                        /* 32 bit mul */
+                        gen_movl_T0_reg(s, rs);
+                        gen_movl_T1_reg(s, rm);
+                        gen_op_mul_T0_T1();
+                        if (insn & (1 << 22)) {
+                            /* Subtract (mls) */
+                            ARCH(6T2);
+                            gen_movl_T1_reg(s, rn);
+                            gen_op_rsbl_T0_T1();
+                        } else if (insn & (1 << 21)) {
+                            /* Add */
+                            gen_movl_T1_reg(s, rn);
+                            gen_op_addl_T0_T1();
+                        }
+                        if (insn & (1 << 20))
+                            gen_op_logic_T0_cc();
+                        gen_movl_reg_T0(s, rd);
+                        break;
+                    default:
+                        /* 64 bit mul */
+                        gen_movl_T0_reg(s, rs);
+                        gen_movl_T1_reg(s, rm);
+                        if (insn & (1 << 22))
+                            gen_op_imull_T0_T1();
+                        else
+                            gen_op_mull_T0_T1();
+                        if (insn & (1 << 21)) /* mult accumulate */
+                            gen_op_addq_T0_T1(rn, rd);
+                        if (!(insn & (1 << 23))) { /* double accumulate */
+                            ARCH(6);
+                            gen_op_addq_lo_T0_T1(rn);
+                            gen_op_addq_lo_T0_T1(rd);
+                        }
+                        if (insn & (1 << 20))
+                            gen_op_logicq_cc();
+                        gen_movl_reg_T0(s, rn);
+                        gen_movl_reg_T1(s, rd);
+                        break;
+                    }
+                } else {
+                    rn = (insn >> 16) & 0xf;
+                    rd = (insn >> 12) & 0xf;
+                    if (insn & (1 << 23)) {
+                        /* load/store exclusive */
+                        gen_movl_T1_reg(s, rn);
+                        if (insn & (1 << 20)) {
+                            gen_ldst(ldlex, s);
+                        } else {
+                            rm = insn & 0xf;
+                            gen_movl_T0_reg(s, rm);
+                            gen_ldst(stlex, s);
+                        }
+                        gen_movl_reg_T0(s, rd);
+                    } else {
+                        /* SWP instruction */
+                        rm = (insn) & 0xf;
+
+                        gen_movl_T0_reg(s, rm);
+                        gen_movl_T1_reg(s, rn);
+                        if (insn & (1 << 22)) {
+                            gen_ldst(swpb, s);
+                        } else {
+                            gen_ldst(swpl, s);
+                        }
+                        gen_movl_reg_T0(s, rd);
+                    }
+                }
+            } else {
+                int address_offset;
+                int load;
+                /* Misc load/store */
+                rn = (insn >> 16) & 0xf;
+                rd = (insn >> 12) & 0xf;
+                gen_movl_T1_reg(s, rn);
+                if (insn & (1 << 24))
+                    gen_add_datah_offset(s, insn, 0);
+                address_offset = 0;
+                if (insn & (1 << 20)) {
+                    /* load */
+                    switch(sh) {
+                    case 1:
+                        gen_ldst(lduw, s);
+                        break;
+                    case 2:
+                        gen_ldst(ldsb, s);
+                        break;
+                    default:
+                    case 3:
+                        gen_ldst(ldsw, s);
+                        break;
+                    }
+                    load = 1;
+                } else if (sh & 2) {
+                    /* doubleword */
+                    if (sh & 1) {
+                        /* store */
+                        gen_movl_T0_reg(s, rd);
+                        gen_ldst(stl, s);
+                        gen_op_addl_T1_im(4);
+                        gen_movl_T0_reg(s, rd + 1);
+                        gen_ldst(stl, s);
+                        load = 0;
+                    } else {
+                        /* load */
+                        gen_ldst(ldl, s);
+                        gen_movl_reg_T0(s, rd);
+                        gen_op_addl_T1_im(4);
+                        gen_ldst(ldl, s);
+                        rd++;
+                        load = 1;
+                    }
+                    address_offset = -4;
+                } else {
+                    /* store */
+                    gen_movl_T0_reg(s, rd);
+                    gen_ldst(stw, s);
+                    load = 0;
+                }
+                /* Perform base writeback before the loaded value to
+                   ensure correct behavior with overlapping index registers.
+                   ldrd with base writeback is is undefined if the
+                   destination and index registers overlap.  */
+                if (!(insn & (1 << 24))) {
+                    gen_add_datah_offset(s, insn, address_offset);
+                    gen_movl_reg_T1(s, rn);
+                } else if (insn & (1 << 21)) {
+                    if (address_offset)
+                        gen_op_addl_T1_im(address_offset);
+                    gen_movl_reg_T1(s, rn);
+                }
+                if (load) {
+                    /* Complete the load.  */
+                    gen_movl_reg_T0(s, rd);
+                }
+            }
+            break;
+        case 0x4:
+        case 0x5:
+            goto do_ldst;
+        case 0x6:
+        case 0x7:
+            if (insn & (1 << 4)) {
+                ARCH(6);
+                /* Armv6 Media instructions.  */
+                rm = insn & 0xf;
+                rn = (insn >> 16) & 0xf;
                 rd = (insn >> 12) & 0xf;
+                rs = (insn >> 8) & 0xf;
+                switch ((insn >> 23) & 3) {
+                case 0: /* Parallel add/subtract.  */
+                    op1 = (insn >> 20) & 7;
+                    gen_movl_T0_reg(s, rn);
+                    gen_movl_T1_reg(s, rm);
+                    sh = (insn >> 5) & 7;
+                    if ((op1 & 3) == 0 || sh == 5 || sh == 6)
+                        goto illegal_op;
+                    gen_arm_parallel_addsub[op1][sh]();
+                    gen_movl_reg_T0(s, rd);
+                    break;
+                case 1:
+                    if ((insn & 0x00700020) == 0) {
+                        /* Hafword pack.  */
+                        gen_movl_T0_reg(s, rn);
+                        gen_movl_T1_reg(s, rm);
+                        shift = (insn >> 7) & 0x1f;
+                        if (shift)
+                            gen_op_shll_T1_im(shift);
+                        if (insn & (1 << 6))
+                            gen_op_pkhtb_T0_T1();
+                        else
+                            gen_op_pkhbt_T0_T1();
+                        gen_movl_reg_T0(s, rd);
+                    } else if ((insn & 0x00200020) == 0x00200000) {
+                        /* [us]sat */
+                        gen_movl_T1_reg(s, rm);
+                        shift = (insn >> 7) & 0x1f;
+                        if (insn & (1 << 6)) {
+                            if (shift == 0)
+                                shift = 31;
+                            gen_op_sarl_T1_im(shift);
+                        } else {
+                            gen_op_shll_T1_im(shift);
+                        }
+                        sh = (insn >> 16) & 0x1f;
+                        if (sh != 0) {
+                            if (insn & (1 << 22))
+                                gen_op_usat_T1(sh);
+                            else
+                                gen_op_ssat_T1(sh);
+                        }
+                        gen_movl_T1_reg(s, rd);
+                    } else if ((insn & 0x00300fe0) == 0x00200f20) {
+                        /* [us]sat16 */
+                        gen_movl_T1_reg(s, rm);
+                        sh = (insn >> 16) & 0x1f;
+                        if (sh != 0) {
+                            if (insn & (1 << 22))
+                                gen_op_usat16_T1(sh);
+                            else
+                                gen_op_ssat16_T1(sh);
+                        }
+                        gen_movl_T1_reg(s, rd);
+                    } else if ((insn & 0x00700fe0) == 0x00000fa0) {
+                        /* Select bytes.  */
+                        gen_movl_T0_reg(s, rn);
+                        gen_movl_T1_reg(s, rm);
+                        gen_op_sel_T0_T1();
+                        gen_movl_reg_T0(s, rd);
+                    } else if ((insn & 0x000003e0) == 0x00000060) {
+                        gen_movl_T1_reg(s, rm);
+                        shift = (insn >> 10) & 3;
+                        /* ??? In many cases it's not neccessary to do a
+                           rotate, a shift is sufficient.  */
+                        if (shift != 0)
+                            gen_op_rorl_T1_im(shift * 8);
+                        op1 = (insn >> 20) & 7;
+                        switch (op1) {
+                        case 0: gen_op_sxtb16_T1(); break;
+                        case 2: gen_op_sxtb_T1();   break;
+                        case 3: gen_op_sxth_T1();   break;
+                        case 4: gen_op_uxtb16_T1(); break;
+                        case 6: gen_op_uxtb_T1();   break;
+                        case 7: gen_op_uxth_T1();   break;
+                        default: goto illegal_op;
+                        }
+                        if (rn != 15) {
+                            gen_movl_T2_reg(s, rn);
+                            if ((op1 & 3) == 0) {
+                                gen_op_add16_T1_T2();
+                            } else {
+                                gen_op_addl_T1_T2();
+                            }
+                        }
+                        gen_movl_reg_T1(s, rd);
+                    } else if ((insn & 0x003f0f60) == 0x003f0f20) {
+                        /* rev */
+                        gen_movl_T0_reg(s, rm);
+                        if (insn & (1 << 22)) {
+                            if (insn & (1 << 7)) {
+                                gen_op_revsh_T0();
+                            } else {
+                                ARCH(6T2);
+                                gen_op_rbit_T0();
+                            }
+                        } else {
+                            if (insn & (1 << 7))
+                                gen_op_rev16_T0();
+                            else
+                                gen_op_rev_T0();
+                        }
+                        gen_movl_reg_T0(s, rd);
+                    } else {
+                        goto illegal_op;
+                    }
+                    break;
+                case 2: /* Multiplies (Type 3).  */
+                    gen_movl_T0_reg(s, rm);
+                    gen_movl_T1_reg(s, rs);
+                    if (insn & (1 << 20)) {
+                        /* Signed multiply most significant [accumulate].  */
+                        gen_op_imull_T0_T1();
+                        if (insn & (1 << 5))
+                            gen_op_roundqd_T0_T1();
+                        else
+                            gen_op_movl_T0_T1();
+                        if (rn != 15) {
+                            gen_movl_T1_reg(s, rn);
+                            if (insn & (1 << 6)) {
+                                gen_op_addl_T0_T1();
+                            } else {
+                                gen_op_rsbl_T0_T1();
+                            }
+                        }
+                        gen_movl_reg_T0(s, rd);
+                    } else {
+                        if (insn & (1 << 5))
+                            gen_op_swap_half_T1();
+                        gen_op_mul_dual_T0_T1();
+                        if (insn & (1 << 22)) {
+                            if (insn & (1 << 6)) {
+                                /* smlald */
+                                gen_op_addq_T0_T1_dual(rn, rd);
+                            } else {
+                                /* smlsld */
+                                gen_op_subq_T0_T1_dual(rn, rd);
+                            }
+                        } else {
+                            /* This addition cannot overflow.  */
+                            if (insn & (1 << 6)) {
+                                /* sm[ul]sd */
+                                gen_op_subl_T0_T1();
+                            } else {
+                                /* sm[ul]ad */
+                                gen_op_addl_T0_T1();
+                            }
+                            if (rn != 15)
+                              {
+                                gen_movl_T1_reg(s, rn);
+                                gen_op_addl_T0_T1_setq();
+                              }
+                            gen_movl_reg_T0(s, rd);
+                        }
+                    }
+                    break;
+                case 3:
+                    op1 = ((insn >> 17) & 0x38) | ((insn >> 5) & 7);
+                    switch (op1) {
+                    case 0: /* Unsigned sum of absolute differences.  */
+                            goto illegal_op;
+                        gen_movl_T0_reg(s, rm);
+                        gen_movl_T1_reg(s, rs);
+                        gen_op_usad8_T0_T1();
+                        if (rn != 15) {
+                            gen_movl_T1_reg(s, rn);
+                            gen_op_addl_T0_T1();
+                        }
+                        gen_movl_reg_T0(s, rd);
+                        break;
+                    case 0x20: case 0x24: case 0x28: case 0x2c:
+                        /* Bitfield insert/clear.  */
+                        ARCH(6T2);
+                        shift = (insn >> 7) & 0x1f;
+                        i = (insn >> 16) & 0x1f;
+                        i = i + 1 - shift;
+                        if (rm == 15) {
+                            gen_op_movl_T1_im(0);
+                        } else {
+                            gen_movl_T1_reg(s, rm);
+                        }
+                        if (i != 32) {
+                            gen_movl_T0_reg(s, rd);
+                            gen_op_bfi_T1_T0(shift, ((1u << i) - 1) << shift);
+                        }
+                        gen_movl_reg_T1(s, rd);
+                        break;
+                    case 0x12: case 0x16: case 0x1a: case 0x1e: /* sbfx */
+                    case 0x32: case 0x36: case 0x3a: case 0x3e: /* ubfx */
+                        gen_movl_T1_reg(s, rm);
+                        shift = (insn >> 7) & 0x1f;
+                        i = ((insn >> 16) & 0x1f) + 1;
+                        if (shift + i > 32)
+                            goto illegal_op;
+                        if (i < 32) {
+                            if (op1 & 0x20) {
+                                gen_op_ubfx_T1(shift, (1u << i) - 1);
+                            } else {
+                                gen_op_sbfx_T1(shift, i);
+                            }
+                        }
+                        gen_movl_reg_T1(s, rd);
+                        break;
+                    default:
+                        goto illegal_op;
+                    }
+                    break;
+                }
+                break;
+            }
+        do_ldst:
+            /* Check for undefined extension instructions
+             * per the ARM Bible IE:
+             * xxxx 0111 1111 xxxx  xxxx xxxx 1111 xxxx
+             */
+            sh = (0xf << 20) | (0xf << 4);
+            if (op1 == 0x7 && ((insn & sh) == sh))
+            {
+                goto illegal_op;
+            }
+            /* load/store byte/word */
+            rn = (insn >> 16) & 0xf;
+            rd = (insn >> 12) & 0xf;
+            gen_movl_T1_reg(s, rn);
+            i = (IS_USER(s) || (insn & 0x01200000) == 0x00200000);
+            if (insn & (1 << 24))
+                gen_add_data_offset(s, insn);
+            if (insn & (1 << 20)) {
+                /* load */
+                s->is_mem = 1;
+#if defined(CONFIG_USER_ONLY)
+                if (insn & (1 << 22))
+                    gen_op_ldub_raw();
+                else
+                    gen_op_ldl_raw();
+#else
+                if (insn & (1 << 22)) {
+                    if (i)
+                        gen_op_ldub_user();
+                    else
+                        gen_op_ldub_kernel();
+                } else {
+                    if (i)
+                        gen_op_ldl_user();
+                    else
+                        gen_op_ldl_kernel();
+                }
+#endif
+            } else {
+                /* store */
+                gen_movl_T0_reg(s, rd);
+#if defined(CONFIG_USER_ONLY)
+                if (insn & (1 << 22))
+                    gen_op_stb_raw();
+                else
+                    gen_op_stl_raw();
+#else
+                if (insn & (1 << 22)) {
+                    if (i)
+                        gen_op_stb_user();
+                    else
+                        gen_op_stb_kernel();
+                } else {
+                    if (i)
+                        gen_op_stl_user();
+                    else
+                        gen_op_stl_kernel();
+                }
+#endif
+            }
+            if (!(insn & (1 << 24))) {
+                gen_add_data_offset(s, insn);
+                gen_movl_reg_T1(s, rn);
+            } else if (insn & (1 << 21))
+                gen_movl_reg_T1(s, rn); {
+            }
+            if (insn & (1 << 20)) {
+                /* Complete the load.  */
+                if (rd == 15)
+                    gen_bx(s);
+                else
+                    gen_movl_reg_T0(s, rd);
+            }
+            break;
+        case 0x08:
+        case 0x09:
+            {
+                int j, n, user, loaded_base;
+                /* load/store multiple words */
+                /* XXX: store correct base if write back */
+                user = 0;
+                if (insn & (1 << 22)) {
+                    if (IS_USER(s))
+                        goto illegal_op; /* only usable in supervisor mode */
+
+                    if ((insn & (1 << 15)) == 0)
+                        user = 1;
+                }
+                rn = (insn >> 16) & 0xf;
+                gen_movl_T1_reg(s, rn);
+
+                /* compute total size */
+                loaded_base = 0;
+                n = 0;
+                for(i=0;i<16;i++) {
+                    if (insn & (1 << i))
+                        n++;
+                }
+                /* XXX: test invalid n == 0 case ? */
+                if (insn & (1 << 23)) {
+                    if (insn & (1 << 24)) {
+                        /* pre increment */
+                        gen_op_addl_T1_im(4);
+                    } else {
+                        /* post increment */
+                    }
+                } else {
+                    if (insn & (1 << 24)) {
+                        /* pre decrement */
+                        gen_op_addl_T1_im(-(n * 4));
+                    } else {
+                        /* post decrement */
+                        if (n != 1)
+                            gen_op_addl_T1_im(-((n - 1) * 4));
+                    }
+                }
+                j = 0;
+                for(i=0;i<16;i++) {
+                    if (insn & (1 << i)) {
+                        if (insn & (1 << 20)) {
+                            /* load */
+                            gen_ldst(ldl, s);
+                            if (i == 15) {
+                                gen_bx(s);
+                            } else if (user) {
+                                gen_op_movl_user_T0(i);
+                            } else if (i == rn) {
+                                gen_op_movl_T2_T0();
+                                loaded_base = 1;
+                            } else {
+                                gen_movl_reg_T0(s, i);
+                            }
+                        } else {
+                            /* store */
+                            if (i == 15) {
+                                /* special case: r15 = PC + 8 */
+                                val = (long)s->pc + 4;
+                                gen_op_movl_TN_im[0](val);
+                            } else if (user) {
+                                gen_op_movl_T0_user(i);
+                            } else {
+                                gen_movl_T0_reg(s, i);
+                            }
+                            gen_ldst(stl, s);
+                        }
+                        j++;
+                        /* no need to add after the last transfer */
+                        if (j != n)
+                            gen_op_addl_T1_im(4);
+                    }
+                }
+                if (insn & (1 << 21)) {
+                    /* write back */
+                    if (insn & (1 << 23)) {
+                        if (insn & (1 << 24)) {
+                            /* pre increment */
+                        } else {
+                            /* post increment */
+                            gen_op_addl_T1_im(4);
+                        }
+                    } else {
+                        if (insn & (1 << 24)) {
+                            /* pre decrement */
+                            if (n != 1)
+                                gen_op_addl_T1_im(-((n - 1) * 4));
+                        } else {
+                            /* post decrement */
+                            gen_op_addl_T1_im(-(n * 4));
+                        }
+                    }
+                    gen_movl_reg_T1(s, rn);
+                }
+                if (loaded_base) {
+                    gen_op_movl_T0_T2();
+                    gen_movl_reg_T0(s, rn);
+                }
+                if ((insn & (1 << 22)) && !user) {
+                    /* Restore CPSR from SPSR.  */
+                    gen_op_movl_T0_spsr();
+                    gen_op_movl_cpsr_T0(0xffffffff);
+                    s->is_jmp = DISAS_UPDATE;
+                }
+            }
+            break;
+        case 0xa:
+        case 0xb:
+            {
+                int32_t offset;
+
+                /* branch (and link) */
+                val = (int32_t)s->pc;
+                if (insn & (1 << 24)) {
+                    gen_op_movl_T0_im(val);
+                    gen_op_movl_reg_TN[0][14]();
+                }
+                offset = (((int32_t)insn << 8) >> 8);
+                val += (offset << 2) + 4;
+                gen_jmp(s, val);
+            }
+            break;
+        case 0xc:
+        case 0xd:
+        case 0xe:
+            /* Coprocessor.  */
+            if (disas_coproc_insn(env, s, insn))
+                goto illegal_op;
+            break;
+        case 0xf:
+            /* swi */
+            gen_op_movl_T0_im((long)s->pc);
+            gen_op_movl_reg_TN[0][15]();
+            s->is_jmp = DISAS_SWI;
+            break;
+        default:
+        illegal_op:
+            gen_set_condexec(s);
+            gen_op_movl_T0_im((long)s->pc - 4);
+            gen_op_movl_reg_TN[0][15]();
+            gen_op_undef_insn();
+            s->is_jmp = DISAS_JUMP;
+            break;
+        }
+    }
+}
+
+/* Return true if this is a Thumb-2 logical op.  */
+static int
+thumb2_logic_op(int op)
+{
+    return (op < 8);
+}
+
+/* Generate code for a Thumb-2 data processing operation.  If CONDS is nonzero
+   then set condition code flags based on the result of the operation.
+   If SHIFTER_OUT is nonzero then set the carry flag for logical operations
+   to the high bit of T1.
+   Returns zero if the opcode is valid.  */
+
+static int
+gen_thumb2_data_op(DisasContext *s, int op, int conds, uint32_t shifter_out)
+{
+    int logic_cc;
+
+    logic_cc = 0;
+    switch (op) {
+    case 0: /* and */
+        gen_op_andl_T0_T1();
+        logic_cc = conds;
+        break;
+    case 1: /* bic */
+        gen_op_bicl_T0_T1();
+        logic_cc = conds;
+        break;
+    case 2: /* orr */
+        gen_op_orl_T0_T1();
+        logic_cc = conds;
+        break;
+    case 3: /* orn */
+        gen_op_notl_T1();
+        gen_op_orl_T0_T1();
+        logic_cc = conds;
+        break;
+    case 4: /* eor */
+        gen_op_xorl_T0_T1();
+        logic_cc = conds;
+        break;
+    case 8: /* add */
+        if (conds)
+            gen_op_addl_T0_T1_cc();
+        else
+            gen_op_addl_T0_T1();
+        break;
+    case 10: /* adc */
+        if (conds)
+            gen_op_adcl_T0_T1_cc();
+        else
+            gen_op_adcl_T0_T1();
+        break;
+    case 11: /* sbc */
+        if (conds)
+            gen_op_sbcl_T0_T1_cc();
+        else
+            gen_op_sbcl_T0_T1();
+        break;
+    case 13: /* sub */
+        if (conds)
+            gen_op_subl_T0_T1_cc();
+        else
+            gen_op_subl_T0_T1();
+        break;
+    case 14: /* rsb */
+        if (conds)
+            gen_op_rsbl_T0_T1_cc();
+        else
+            gen_op_rsbl_T0_T1();
+        break;
+    default: /* 5, 6, 7, 9, 12, 15. */
+        return 1;
+    }
+    if (logic_cc) {
+        gen_op_logic_T0_cc();
+        if (shifter_out)
+            gen_op_mov_CF_T1();
+    }
+    return 0;
+}
+
+/* Translate a 32-bit thumb instruction.  Returns nonzero if the instruction
+   is not legal.  */
+static int disas_thumb2_insn(CPUState *env, DisasContext *s, uint16_t insn_hw1)
+{
+    uint32_t insn, imm, shift, offset, addr;
+    uint32_t rd, rn, rm, rs;
+    int op;
+    int shiftop;
+    int conds;
+    int logic_cc;
+
+    if (!(arm_feature(env, ARM_FEATURE_THUMB2)
+          || arm_feature (env, ARM_FEATURE_M))) {
+        /* Thumb-1 cores may need to tread bl and blx as a pair of
+           16-bit instructions to get correct prefetch abort behavior.  */
+        insn = insn_hw1;
+        if ((insn & (1 << 12)) == 0) {
+            /* Second half of blx.  */
+            offset = ((insn & 0x7ff) << 1);
+            gen_movl_T0_reg(s, 14);
+            gen_op_movl_T1_im(offset);
+            gen_op_addl_T0_T1();
+            gen_op_movl_T1_im(0xfffffffc);
+            gen_op_andl_T0_T1();
+
+            addr = (uint32_t)s->pc;
+            gen_op_movl_T1_im(addr | 1);
+            gen_movl_reg_T1(s, 14);
+            gen_bx(s);
+            return 0;
+        }
+        if (insn & (1 << 11)) {
+            /* Second half of bl.  */
+            offset = ((insn & 0x7ff) << 1) | 1;
+            gen_movl_T0_reg(s, 14);
+            gen_op_movl_T1_im(offset);
+            gen_op_addl_T0_T1();
+
+            addr = (uint32_t)s->pc;
+            gen_op_movl_T1_im(addr | 1);
+            gen_movl_reg_T1(s, 14);
+            gen_bx(s);
+            return 0;
+        }
+        if ((s->pc & ~TARGET_PAGE_MASK) == 0) {
+            /* Instruction spans a page boundary.  Implement it as two
+               16-bit instructions in case the second half causes an
+               prefetch abort.  */
+            offset = ((int32_t)insn << 21) >> 9;
+            addr = s->pc + 2 + offset;
+            gen_op_movl_T0_im(addr);
+            gen_movl_reg_T0(s, 14);
+            return 0;
+        }
+        /* Fall through to 32-bit decode.  */
+    }
+
+    insn = lduw_code(s->pc);
+    s->pc += 2;
+    insn |= (uint32_t)insn_hw1 << 16;
+
+    if ((insn & 0xf800e800) != 0xf000e800) {
+        ARCH(6T2);
+    }
+
+    rn = (insn >> 16) & 0xf;
+    rs = (insn >> 12) & 0xf;
+    rd = (insn >> 8) & 0xf;
+    rm = insn & 0xf;
+    switch ((insn >> 25) & 0xf) {
+    case 0: case 1: case 2: case 3:
+        /* 16-bit instructions.  Should never happen.  */
+        abort();
+    case 4:
+        if (insn & (1 << 22)) {
+            /* Other load/store, table branch.  */
+            if (insn & 0x01200000) {
+                /* Load/store doubleword.  */
+                if (rn == 15) {
+                    gen_op_movl_T1_im(s->pc & ~3);
+                } else {
+                    gen_movl_T1_reg(s, rn);
+                }
+                offset = (insn & 0xff) * 4;
+                if ((insn & (1 << 23)) == 0)
+                    offset = -offset;
+                if (insn & (1 << 24)) {
+                    gen_op_addl_T1_im(offset);
+                    offset = 0;
+                }
+                if (insn & (1 << 20)) {
+                    /* ldrd */
+                    gen_ldst(ldl, s);
+                    gen_movl_reg_T0(s, rs);
+                    gen_op_addl_T1_im(4);
+                    gen_ldst(ldl, s);
+                    gen_movl_reg_T0(s, rd);
+                } else {
+                    /* strd */
+                    gen_movl_T0_reg(s, rs);
+                    gen_ldst(stl, s);
+                    gen_op_addl_T1_im(4);
+                    gen_movl_T0_reg(s, rd);
+                    gen_ldst(stl, s);
+                }
+                if (insn & (1 << 21)) {
+                    /* Base writeback.  */
+                    if (rn == 15)
+                        goto illegal_op;
+                    gen_op_addl_T1_im(offset - 4);
+                    gen_movl_reg_T1(s, rn);
+                }
+            } else if ((insn & (1 << 23)) == 0) {
+                /* Load/store exclusive word.  */
+                gen_movl_T0_reg(s, rd);
                 gen_movl_T1_reg(s, rn);
-                if (insn & (1 << 24))
-                    gen_add_datah_offset(s, insn, 0);
-                address_offset = 0;
                 if (insn & (1 << 20)) {
-                    /* load */
-                    switch(sh) {
+                    gen_ldst(ldlex, s);
+                } else {
+                    gen_ldst(stlex, s);
+                }
+                gen_movl_reg_T0(s, rd);
+            } else if ((insn & (1 << 6)) == 0) {
+                /* Table Branch.  */
+                if (rn == 15) {
+                    gen_op_movl_T1_im(s->pc);
+                } else {
+                    gen_movl_T1_reg(s, rn);
+                }
+                gen_movl_T2_reg(s, rm);
+                gen_op_addl_T1_T2();
+                if (insn & (1 << 4)) {
+                    /* tbh */
+                    gen_op_addl_T1_T2();
+                    gen_ldst(lduw, s);
+                } else { /* tbb */
+                    gen_ldst(ldub, s);
+                }
+                gen_op_jmp_T0_im(s->pc);
+                s->is_jmp = DISAS_JUMP;
+            } else {
+                /* Load/store exclusive byte/halfword/doubleword.  */
+                op = (insn >> 4) & 0x3;
+                gen_movl_T1_reg(s, rn);
+                if (insn & (1 << 20)) {
+                    switch (op) {
+                    case 0:
+                        gen_ldst(ldbex, s);
+                        break;
                     case 1:
-                        gen_ldst(lduw, s);
+                        gen_ldst(ldwex, s);
                         break;
-                    case 2:
-                        gen_ldst(ldsb, s);
+                    case 3:
+                        gen_ldst(ldqex, s);
+                        gen_movl_reg_T1(s, rd);
                         break;
                     default:
+                        goto illegal_op;
+                    }
+                    gen_movl_reg_T0(s, rs);
+                } else {
+                    gen_movl_T0_reg(s, rs);
+                    switch (op) {
+                    case 0:
+                        gen_ldst(stbex, s);
+                        break;
+                    case 1:
+                        gen_ldst(stwex, s);
+                        break;
                     case 3:
-                        gen_ldst(ldsw, s);
+                        gen_movl_T2_reg(s, rd);
+                        gen_ldst(stqex, s);
                         break;
+                    default:
+                        goto illegal_op;
                     }
-                    load = 1;
-                } else if (sh & 2) {
-                    /* doubleword */
-                    if (sh & 1) {
-                        /* store */
-                        gen_movl_T0_reg(s, rd);
-                        gen_ldst(stl, s);
+                    gen_movl_reg_T0(s, rm);
+                }
+            }
+        } else {
+            /* Load/store multiple, RFE, SRS.  */
+            if (((insn >> 23) & 1) == ((insn >> 24) & 1)) {
+                /* Not available in user mode.  */
+                if (!IS_USER(s))
+                    goto illegal_op;
+                if (insn & (1 << 20)) {
+                    /* rfe */
+                    gen_movl_T1_reg(s, rn);
+                    if (insn & (1 << 24)) {
                         gen_op_addl_T1_im(4);
-                        gen_movl_T0_reg(s, rd + 1);
+                    } else {
+                        gen_op_addl_T1_im(-4);
+                    }
+                    /* Load CPSR into T2 and PC into T0.  */
+                    gen_ldst(ldl, s);
+                    gen_op_movl_T2_T0();
+                    gen_op_addl_T1_im(-4);
+                    gen_ldst(ldl, s);
+                    if (insn & (1 << 21)) {
+                        /* Base writeback.  */
+                        if (insn & (1 << 24))
+                            gen_op_addl_T1_im(8);
+                        gen_movl_reg_T1(s, rn);
+                    }
+                    gen_rfe(s);
+                } else {
+                    /* srs */
+                    op = (insn & 0x1f);
+                    if (op == (env->uncached_cpsr & CPSR_M)) {
+                        gen_movl_T1_reg(s, 13);
+                    } else {
+                        gen_op_movl_T1_r13_banked(op);
+                    }
+                    if ((insn & (1 << 24)) == 0) {
+                        gen_op_addl_T1_im(-8);
+                    }
+                    gen_movl_T0_reg(s, 14);
+                    gen_ldst(stl, s);
+                    gen_op_movl_T0_cpsr();
+                    gen_op_addl_T1_im(4);
+                    gen_ldst(stl, s);
+                    if (insn & (1 << 21)) {
+                        if ((insn & (1 << 24)) == 0) {
+                            gen_op_addl_T1_im(-4);
+                        } else {
+                            gen_op_addl_T1_im(4);
+                        }
+                        if (op == (env->uncached_cpsr & CPSR_M)) {
+                            gen_movl_reg_T1(s, 13);
+                        } else {
+                            gen_op_movl_r13_T1_banked(op);
+                        }
+                    }
+                }
+            } else {
+                int i;
+                /* Load/store multiple.  */
+                gen_movl_T1_reg(s, rn);
+                offset = 0;
+                for (i = 0; i < 16; i++) {
+                    if (insn & (1 << i))
+                        offset += 4;
+                }
+                if (insn & (1 << 24)) {
+                    gen_op_addl_T1_im(-offset);
+                }
+
+                for (i = 0; i < 16; i++) {
+                    if ((insn & (1 << i)) == 0)
+                        continue;
+                    if (insn & (1 << 20)) {
+                        /* Load.  */
+                        gen_ldst(ldl, s);
+                        if (i == 15) {
+                            gen_bx(s);
+                        } else {
+                            gen_movl_reg_T0(s, i);
+                        }
+                    } else {
+                        /* Store.  */
+                        gen_movl_T0_reg(s, i);
                         gen_ldst(stl, s);
-                        load = 0;
+                    }
+                    gen_op_addl_T1_im(4);
+                }
+                if (insn & (1 << 21)) {
+                    /* Base register writeback.  */
+                    if (insn & (1 << 24)) {
+                        gen_op_addl_T1_im(-offset);
+                    }
+                    /* Fault if writeback register is in register list.  */
+                    if (insn & (1 << rn))
+                        goto illegal_op;
+                    gen_movl_reg_T1(s, rn);
+                }
+            }
+        }
+        break;
+    case 5: /* Data processing register constant shift.  */
+        if (rn == 15)
+            gen_op_movl_T0_im(0);
+        else
+            gen_movl_T0_reg(s, rn);
+        gen_movl_T1_reg(s, rm);
+        op = (insn >> 21) & 0xf;
+        shiftop = (insn >> 4) & 3;
+        shift = ((insn >> 6) & 3) | ((insn >> 10) & 0x1c);
+        conds = (insn & (1 << 20)) != 0;
+        logic_cc = (conds && thumb2_logic_op(op));
+        if (shift != 0) {
+            if (logic_cc) {
+                gen_shift_T1_im_cc[shiftop](shift);
+            } else {
+                gen_shift_T1_im[shiftop](shift);
+            }
+        } else if (shiftop != 0) {
+            if (logic_cc) {
+                gen_shift_T1_0_cc[shiftop]();
+            } else {
+                gen_shift_T1_0[shiftop]();
+            }
+        }
+        if (gen_thumb2_data_op(s, op, conds, 0))
+            goto illegal_op;
+        if (rd != 15)
+            gen_movl_reg_T0(s, rd);
+        break;
+    case 13: /* Misc data processing.  */
+        op = ((insn >> 22) & 6) | ((insn >> 7) & 1);
+        if (op < 4 && (insn & 0xf000) != 0xf000)
+            goto illegal_op;
+        switch (op) {
+        case 0: /* Register controlled shift.  */
+            gen_movl_T0_reg(s, rm);
+            gen_movl_T1_reg(s, rn);
+            if ((insn & 0x70) != 0)
+                goto illegal_op;
+            op = (insn >> 21) & 3;
+            if (insn & (1 << 20)) {
+                gen_shift_T1_T0_cc[op]();
+                gen_op_logic_T1_cc();
+            } else {
+                gen_shift_T1_T0[op]();
+            }
+            gen_movl_reg_T1(s, rd);
+            break;
+        case 1: /* Sign/zero extend.  */
+            gen_movl_T1_reg(s, rm);
+            shift = (insn >> 4) & 3;
+            /* ??? In many cases it's not neccessary to do a
+               rotate, a shift is sufficient.  */
+            if (shift != 0)
+                gen_op_rorl_T1_im(shift * 8);
+            op = (insn >> 20) & 7;
+            switch (op) {
+            case 0: gen_op_sxth_T1();   break;
+            case 1: gen_op_uxth_T1();   break;
+            case 2: gen_op_sxtb16_T1(); break;
+            case 3: gen_op_uxtb16_T1(); break;
+            case 4: gen_op_sxtb_T1();   break;
+            case 5: gen_op_uxtb_T1();   break;
+            default: goto illegal_op;
+            }
+            if (rn != 15) {
+                gen_movl_T2_reg(s, rn);
+                if ((op >> 1) == 1) {
+                    gen_op_add16_T1_T2();
+                } else {
+                    gen_op_addl_T1_T2();
+                }
+            }
+            gen_movl_reg_T1(s, rd);
+            break;
+        case 2: /* SIMD add/subtract.  */
+            op = (insn >> 20) & 7;
+            shift = (insn >> 4) & 7;
+            if ((op & 3) == 3 || (shift & 3) == 3)
+                goto illegal_op;
+            gen_movl_T0_reg(s, rn);
+            gen_movl_T1_reg(s, rm);
+            gen_thumb2_parallel_addsub[op][shift]();
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 3: /* Other data processing.  */
+            op = ((insn >> 17) & 0x38) | ((insn >> 4) & 7);
+            if (op < 4) {
+                /* Saturating add/subtract.  */
+                gen_movl_T0_reg(s, rm);
+                gen_movl_T1_reg(s, rn);
+                if (op & 2)
+                    gen_op_double_T1_saturate();
+                if (op & 1)
+                    gen_op_subl_T0_T1_saturate();
+                else
+                    gen_op_addl_T0_T1_saturate();
+            } else {
+                gen_movl_T0_reg(s, rn);
+                switch (op) {
+                case 0x0a: /* rbit */
+                    gen_op_rbit_T0();
+                    break;
+                case 0x08: /* rev */
+                    gen_op_rev_T0();
+                    break;
+                case 0x09: /* rev16 */
+                    gen_op_rev16_T0();
+                    break;
+                case 0x0b: /* revsh */
+                    gen_op_revsh_T0();
+                    break;
+                case 0x10: /* sel */
+                    gen_movl_T1_reg(s, rm);
+                    gen_op_sel_T0_T1();
+                    break;
+                case 0x18: /* clz */
+                    gen_op_clz_T0();
+                    break;
+                default:
+                    goto illegal_op;
+                }
+            }
+            gen_movl_reg_T0(s, rd);
+            break;
+        case 4: case 5: /* 32-bit multiply.  Sum of absolute differences.  */
+            op = (insn >> 4) & 0xf;
+            gen_movl_T0_reg(s, rn);
+            gen_movl_T1_reg(s, rm);
+            switch ((insn >> 20) & 7) {
+            case 0: /* 32 x 32 -> 32 */
+                gen_op_mul_T0_T1();
+                if (rs != 15) {
+                    gen_movl_T1_reg(s, rs);
+                    if (op)
+                        gen_op_rsbl_T0_T1();
+                    else
+                        gen_op_addl_T0_T1();
+                }
+                gen_movl_reg_T0(s, rd);
+                break;
+            case 1: /* 16 x 16 -> 32 */
+                gen_mulxy(op & 2, op & 1);
+                if (rs != 15) {
+                    gen_movl_T1_reg(s, rs);
+                    gen_op_addl_T0_T1_setq();
+                }
+                gen_movl_reg_T0(s, rd);
+                break;
+            case 2: /* Dual multiply add.  */
+            case 4: /* Dual multiply subtract.  */
+                if (op)
+                    gen_op_swap_half_T1();
+                gen_op_mul_dual_T0_T1();
+                /* This addition cannot overflow.  */
+                if (insn & (1 << 22)) {
+                    gen_op_subl_T0_T1();
+                } else {
+                    gen_op_addl_T0_T1();
+                }
+                if (rs != 15)
+                  {
+                    gen_movl_T1_reg(s, rs);
+                    gen_op_addl_T0_T1_setq();
+                  }
+                gen_movl_reg_T0(s, rd);
+                break;
+            case 3: /* 32 * 16 -> 32msb */
+                if (op)
+                    gen_op_sarl_T1_im(16);
+                else
+                    gen_op_sxth_T1();
+                gen_op_imulw_T0_T1();
+                if (rs != 15)
+                  {
+                    gen_movl_T1_reg(s, rs);
+                    gen_op_addl_T0_T1_setq();
+                  }
+                gen_movl_reg_T0(s, rd);
+                break;
+            case 5: case 6: /* 32 * 32 -> 32msb */
+                gen_op_imull_T0_T1();
+                if (insn & (1 << 5))
+                    gen_op_roundqd_T0_T1();
+                else
+                    gen_op_movl_T0_T1();
+                if (rs != 15) {
+                    gen_movl_T1_reg(s, rs);
+                    if (insn & (1 << 21)) {
+                        gen_op_addl_T0_T1();
                     } else {
-                        /* load */
-                        gen_ldst(ldl, s);
-                        gen_movl_reg_T0(s, rd);
-                        gen_op_addl_T1_im(4);
-                        gen_ldst(ldl, s);
-                        rd++;
-                        load = 1;
+                        gen_op_rsbl_T0_T1();
                     }
-                    address_offset = -4;
-                } else {
-                    /* store */
-                    gen_movl_T0_reg(s, rd);
-                    gen_ldst(stw, s);
-                    load = 0;
-                }
-                /* Perform base writeback before the loaded value to
-                   ensure correct behavior with overlapping index registers.
-                   ldrd with base writeback is is undefined if the
-                   destination and index registers overlap.  */
-                if (!(insn & (1 << 24))) {
-                    gen_add_datah_offset(s, insn, address_offset);
-                    gen_movl_reg_T1(s, rn);
-                } else if (insn & (1 << 21)) {
-                    if (address_offset)
-                        gen_op_addl_T1_im(address_offset);
-                    gen_movl_reg_T1(s, rn);
                 }
-                if (load) {
-                    /* Complete the load.  */
-                    gen_movl_reg_T0(s, rd);
+                gen_movl_reg_T0(s, rd);
+                break;
+            case 7: /* Unsigned sum of absolute differences.  */
+                gen_op_usad8_T0_T1();
+                if (rs != 15) {
+                    gen_movl_T1_reg(s, rs);
+                    gen_op_addl_T0_T1();
                 }
+                gen_movl_reg_T0(s, rd);
+                break;
             }
             break;
-        case 0x4:
-        case 0x5:
-        case 0x6:
-        case 0x7:
-            /* Check for undefined extension instructions
-             * per the ARM Bible IE:
-             * xxxx 0111 1111 xxxx  xxxx xxxx 1111 xxxx
-             */
-            sh = (0xf << 20) | (0xf << 4);
-            if (op1 == 0x7 && ((insn & sh) == sh))
-            {
-                goto illegal_op;
-            }
-            /* load/store byte/word */
-            rn = (insn >> 16) & 0xf;
-            rd = (insn >> 12) & 0xf;
-            gen_movl_T1_reg(s, rn);
-            i = (IS_USER(s) || (insn & 0x01200000) == 0x00200000);
-            if (insn & (1 << 24))
-                gen_add_data_offset(s, insn);
-            if (insn & (1 << 20)) {
-                /* load */
-                s->is_mem = 1;
-#if defined(CONFIG_USER_ONLY)
-                if (insn & (1 << 22))
-                    gen_op_ldub_raw();
+        case 6: case 7: /* 64-bit multiply, Divide.  */
+            op = ((insn >> 4) & 0xf) | ((insn >> 16) & 0x70);
+            gen_movl_T0_reg(s, rn);
+            gen_movl_T1_reg(s, rm);
+            if ((op & 0x50) == 0x10) {
+                /* sdiv, udiv */
+                if (!arm_feature(env, ARM_FEATURE_DIV))
+                    goto illegal_op;
+                if (op & 0x20)
+                    gen_op_udivl_T0_T1();
                 else
-                    gen_op_ldl_raw();
-#else
-                if (insn & (1 << 22)) {
-                    if (i)
-                        gen_op_ldub_user();
-                    else
-                        gen_op_ldub_kernel();
+                    gen_op_sdivl_T0_T1();
+                gen_movl_reg_T0(s, rd);
+            } else if ((op & 0xe) == 0xc) {
+                /* Dual multiply accumulate long.  */
+                if (op & 1)
+                    gen_op_swap_half_T1();
+                gen_op_mul_dual_T0_T1();
+                if (op & 0x10) {
+                    gen_op_subl_T0_T1();
                 } else {
-                    if (i)
-                        gen_op_ldl_user();
-                    else
-                        gen_op_ldl_kernel();
+                    gen_op_addl_T0_T1();
                 }
-#endif
+                gen_op_signbit_T1_T0();
+                gen_op_addq_T0_T1(rs, rd);
+                gen_movl_reg_T0(s, rs);
+                gen_movl_reg_T1(s, rd);
             } else {
-                /* store */
-                gen_movl_T0_reg(s, rd);
-#if defined(CONFIG_USER_ONLY)
-                if (insn & (1 << 22))
-                    gen_op_stb_raw();
-                else
-                    gen_op_stl_raw();
-#else
-                if (insn & (1 << 22)) {
-                    if (i)
-                        gen_op_stb_user();
-                    else
-                        gen_op_stb_kernel();
+                if (op & 0x20) {
+                    /* Unsigned 64-bit multiply  */
+                    gen_op_mull_T0_T1();
                 } else {
-                    if (i)
-                        gen_op_stl_user();
-                    else
-                        gen_op_stl_kernel();
+                    if (op & 8) {
+                        /* smlalxy */
+                        gen_mulxy(op & 2, op & 1);
+                        gen_op_signbit_T1_T0();
+                    } else {
+                        /* Signed 64-bit multiply  */
+                        gen_op_imull_T0_T1();
+                    }
                 }
-#endif
-            }
-            if (!(insn & (1 << 24))) {
-                gen_add_data_offset(s, insn);
-                gen_movl_reg_T1(s, rn);
-            } else if (insn & (1 << 21))
-                gen_movl_reg_T1(s, rn); {
-            }
-            if (insn & (1 << 20)) {
-                /* Complete the load.  */
-                if (rd == 15)
-                    gen_bx(s);
-                else
-                    gen_movl_reg_T0(s, rd);
+                if (op & 4) {
+                    /* umaal */
+                    gen_op_addq_lo_T0_T1(rs);
+                    gen_op_addq_lo_T0_T1(rd);
+                } else if (op & 0x40) {
+                    /* 64-bit accumulate.  */
+                    gen_op_addq_T0_T1(rs, rd);
+                }
+                gen_movl_reg_T0(s, rs);
+                gen_movl_reg_T1(s, rd);
             }
             break;
-        case 0x08:
-        case 0x09:
-            {
-                int j, n, user, loaded_base;
-                /* load/store multiple words */
-                /* XXX: store correct base if write back */
-                user = 0;
-                if (insn & (1 << 22)) {
-                    if (IS_USER(s))
-                        goto illegal_op; /* only usable in supervisor mode */
-
-                    if ((insn & (1 << 15)) == 0)
-                        user = 1;
+        }
+        break;
+    case 6: case 7: case 14: case 15:
+        /* Coprocessor.  */
+        if (((insn >> 24) & 3) == 3) {
+            /* Translate into the equivalent ARM encoding.  */
+            insn = (insn & 0xe2ffffff) | ((insn & (1 << 28)) >> 4);
+            if (disas_neon_data_insn(env, s, insn))
+                goto illegal_op;
+        } else {
+            if (insn & (1 << 28))
+                goto illegal_op;
+            if (disas_coproc_insn (env, s, insn))
+                goto illegal_op;
+        }
+        break;
+    case 8: case 9: case 10: case 11:
+        if (insn & (1 << 15)) {
+            /* Branches, misc control.  */
+            if (insn & 0x5000) {
+                /* Unconditional branch.  */
+                /* signextend(hw1[10:0]) -> offset[:12].  */
+                offset = ((int32_t)insn << 5) >> 9 & ~(int32_t)0xfff;
+                /* hw1[10:0] -> offset[11:1].  */
+                offset |= (insn & 0x7ff) << 1;
+                /* (~hw2[13, 11] ^ offset[24]) -> offset[23,22]
+                   offset[24:22] already have the same value because of the
+                   sign extension above.  */
+                offset ^= ((~insn) & (1 << 13)) << 10;
+                offset ^= ((~insn) & (1 << 11)) << 11;
+
+                addr = s->pc;
+                if (insn & (1 << 14)) {
+                    /* Branch and link.  */
+                    gen_op_movl_T1_im(addr | 1);
+                    gen_movl_reg_T1(s, 14);
                 }
-                rn = (insn >> 16) & 0xf;
-                gen_movl_T1_reg(s, rn);
 
-                /* compute total size */
-                loaded_base = 0;
-                n = 0;
-                for(i=0;i<16;i++) {
-                    if (insn & (1 << i))
-                        n++;
+                addr += offset;
+                if (insn & (1 << 12)) {
+                    /* b/bl */
+                    gen_jmp(s, addr);
+                } else {
+                    /* blx */
+                    addr &= ~(uint32_t)2;
+                    gen_op_movl_T0_im(addr);
+                    gen_bx(s);
                 }
-                /* XXX: test invalid n == 0 case ? */
-                if (insn & (1 << 23)) {
-                    if (insn & (1 << 24)) {
-                        /* pre increment */
-                        gen_op_addl_T1_im(4);
-                    } else {
-                        /* post increment */
-                    }
+            } else if (((insn >> 23) & 7) == 7) {
+                /* Misc control */
+                if (insn & (1 << 13))
+                    goto illegal_op;
+
+                if (insn & (1 << 26)) {
+                    /* Secure monitor call (v6Z) */
+                    goto illegal_op; /* not implemented.  */
                 } else {
-                    if (insn & (1 << 24)) {
-                        /* pre decrement */
-                        gen_op_addl_T1_im(-(n * 4));
-                    } else {
-                        /* post decrement */
-                        if (n != 1)
-                            gen_op_addl_T1_im(-((n - 1) * 4));
+                    op = (insn >> 20) & 7;
+                    switch (op) {
+                    case 0: /* msr cpsr.  */
+                        if (IS_M(env)) {
+                            gen_op_v7m_msr_T0(insn & 0xff);
+                            gen_movl_reg_T0(s, rn);
+                            gen_lookup_tb(s);
+                            break;
+                        }
+                        /* fall through */
+                    case 1: /* msr spsr.  */
+                        if (IS_M(env))
+                            goto illegal_op;
+                        gen_movl_T0_reg(s, rn);
+                        if (gen_set_psr_T0(s,
+                              msr_mask(env, s, (insn >> 8) & 0xf, op == 1),
+                              op == 1))
+                            goto illegal_op;
+                        break;
+                    case 2: /* cps, nop-hint.  */
+                        if (((insn >> 8) & 7) == 0) {
+                            gen_nop_hint(s, insn & 0xff);
+                        }
+                        /* Implemented as NOP in user mode.  */
+                        if (IS_USER(s))
+                            break;
+                        offset = 0;
+                        imm = 0;
+                        if (insn & (1 << 10)) {
+                            if (insn & (1 << 7))
+                                offset |= CPSR_A;
+                            if (insn & (1 << 6))
+                                offset |= CPSR_I;
+                            if (insn & (1 << 5))
+                                offset |= CPSR_F;
+                            if (insn & (1 << 9))
+                                imm = CPSR_A | CPSR_I | CPSR_F;
+                        }
+                        if (insn & (1 << 8)) {
+                            offset |= 0x1f;
+                            imm |= (insn & 0x1f);
+                        }
+                        if (offset) {
+                            gen_op_movl_T0_im(imm);
+                            gen_set_psr_T0(s, offset, 0);
+                        }
+                        break;
+                    case 3: /* Special control operations.  */
+                        op = (insn >> 4) & 0xf;
+                        switch (op) {
+                        case 2: /* clrex */
+                            gen_op_clrex();
+                            break;
+                        case 4: /* dsb */
+                        case 5: /* dmb */
+                        case 6: /* isb */
+                            /* These execute as NOPs.  */
+                            ARCH(7);
+                            break;
+                        default:
+                            goto illegal_op;
+                        }
+                        break;
+                    case 4: /* bxj */
+                        /* Trivial implementation equivalent to bx.  */
+                        gen_movl_T0_reg(s, rn);
+                        gen_bx(s);
+                        break;
+                    case 5: /* Exception return.  */
+                        /* Unpredictable in user mode.  */
+                        goto illegal_op;
+                    case 6: /* mrs cpsr.  */
+                        if (IS_M(env)) {
+                            gen_op_v7m_mrs_T0(insn & 0xff);
+                        } else {
+                            gen_op_movl_T0_cpsr();
+                        }
+                        gen_movl_reg_T0(s, rd);
+                        break;
+                    case 7: /* mrs spsr.  */
+                        /* Not accessible in user mode.  */
+                        if (IS_USER(s) || IS_M(env))
+                            goto illegal_op;
+                        gen_op_movl_T0_spsr();
+                        gen_movl_reg_T0(s, rd);
+                        break;
                     }
                 }
-                j = 0;
-                for(i=0;i<16;i++) {
-                    if (insn & (1 << i)) {
-                        if (insn & (1 << 20)) {
-                            /* load */
-                            gen_ldst(ldl, s);
-                            if (i == 15) {
-                                gen_bx(s);
-                            } else if (user) {
-                                gen_op_movl_user_T0(i);
-                            } else if (i == rn) {
-                                gen_op_movl_T2_T0();
-                                loaded_base = 1;
-                            } else {
-                                gen_movl_reg_T0(s, i);
-                            }
+            } else {
+                /* Conditional branch.  */
+                op = (insn >> 22) & 0xf;
+                /* Generate a conditional jump to next instruction.  */
+                s->condlabel = gen_new_label();
+                gen_test_cc[op ^ 1](s->condlabel);
+                s->condjmp = 1;
+
+                /* offset[11:1] = insn[10:0] */
+                offset = (insn & 0x7ff) << 1;
+                /* offset[17:12] = insn[21:16].  */
+                offset |= (insn & 0x003f0000) >> 4;
+                /* offset[31:20] = insn[26].  */
+                offset |= ((int32_t)((insn << 5) & 0x80000000)) >> 11;
+                /* offset[18] = insn[13].  */
+                offset |= (insn & (1 << 13)) << 5;
+                /* offset[19] = insn[11].  */
+                offset |= (insn & (1 << 11)) << 8;
+
+                /* jump to the offset */
+                addr = s->pc + offset;
+                gen_jmp(s, addr);
+            }
+        } else {
+            /* Data processing immediate.  */
+            if (insn & (1 << 25)) {
+                if (insn & (1 << 24)) {
+                    if (insn & (1 << 20))
+                        goto illegal_op;
+                    /* Bitfield/Saturate.  */
+                    op = (insn >> 21) & 7;
+                    imm = insn & 0x1f;
+                    shift = ((insn >> 6) & 3) | ((insn >> 10) & 0x1c);
+                    if (rn == 15)
+                        gen_op_movl_T1_im(0);
+                    else
+                        gen_movl_T1_reg(s, rn);
+                    switch (op) {
+                    case 2: /* Signed bitfield extract.  */
+                        imm++;
+                        if (shift + imm > 32)
+                            goto illegal_op;
+                        if (imm < 32)
+                            gen_op_sbfx_T1(shift, imm);
+                        break;
+                    case 6: /* Unsigned bitfield extract.  */
+                        imm++;
+                        if (shift + imm > 32)
+                            goto illegal_op;
+                        if (imm < 32)
+                            gen_op_ubfx_T1(shift, (1u << imm) - 1);
+                        break;
+                    case 3: /* Bitfield insert/clear.  */
+                        if (imm < shift)
+                            goto illegal_op;
+                        imm = imm + 1 - shift;
+                        if (imm != 32) {
+                            gen_movl_T0_reg(s, rd);
+                            gen_op_bfi_T1_T0(shift, ((1u << imm) - 1) << shift);
+                        }
+                        break;
+                    case 7:
+                        goto illegal_op;
+                    default: /* Saturate.  */
+                        gen_movl_T1_reg(s, rn);
+                        if (shift) {
+                            if (op & 1)
+                                gen_op_sarl_T1_im(shift);
+                            else
+                                gen_op_shll_T1_im(shift);
+                        }
+                        if (op & 4) {
+                            /* Unsigned.  */
+                            gen_op_ssat_T1(imm);
+                            if ((op & 1) && shift == 0)
+                                gen_op_usat16_T1(imm);
+                            else
+                                gen_op_usat_T1(imm);
                         } else {
-                            /* store */
-                            if (i == 15) {
-                                /* special case: r15 = PC + 8 */
-                                val = (long)s->pc + 4;
-                                gen_op_movl_TN_im[0](val);
-                            } else if (user) {
-                                gen_op_movl_T0_user(i);
-                            } else {
-                                gen_movl_T0_reg(s, i);
-                            }
-                            gen_ldst(stl, s);
+                            /* Signed.  */
+                            gen_op_ssat_T1(imm);
+                            if ((op & 1) && shift == 0)
+                                gen_op_ssat16_T1(imm);
+                            else
+                                gen_op_ssat_T1(imm);
                         }
-                        j++;
-                        /* no need to add after the last transfer */
-                        if (j != n)
-                            gen_op_addl_T1_im(4);
+                        break;
                     }
-                }
-                if (insn & (1 << 21)) {
-                    /* write back */
-                    if (insn & (1 << 23)) {
-                        if (insn & (1 << 24)) {
-                            /* pre increment */
+                    gen_movl_reg_T1(s, rd);
+                } else {
+                    imm = ((insn & 0x04000000) >> 15)
+                          | ((insn & 0x7000) >> 4) | (insn & 0xff);
+                    if (insn & (1 << 22)) {
+                        /* 16-bit immediate.  */
+                        imm |= (insn >> 4) & 0xf000;
+                        if (insn & (1 << 23)) {
+                            /* movt */
+                            gen_movl_T0_reg(s, rd);
+                            gen_op_movtop_T0_im(imm << 16);
                         } else {
-                            /* post increment */
-                            gen_op_addl_T1_im(4);
+                            /* movw */
+                            gen_op_movl_T0_im(imm);
                         }
                     } else {
-                        if (insn & (1 << 24)) {
-                            /* pre decrement */
-                            if (n != 1)
-                                gen_op_addl_T1_im(-((n - 1) * 4));
+                        /* Add/sub 12-bit immediate.  */
+                        if (rn == 15) {
+                            addr = s->pc & ~(uint32_t)3;
+                            if (insn & (1 << 23))
+                                addr -= imm;
+                            else
+                                addr += imm;
+                            gen_op_movl_T0_im(addr);
                         } else {
-                            /* post decrement */
-                            gen_op_addl_T1_im(-(n * 4));
+                            gen_movl_T0_reg(s, rn);
+                            gen_op_movl_T1_im(imm);
+                            if (insn & (1 << 23))
+                                gen_op_subl_T0_T1();
+                            else
+                                gen_op_addl_T0_T1();
                         }
-                    }
-                    gen_movl_reg_T1(s, rn);
-                }
-                if (loaded_base) {
-                    gen_op_movl_T0_T2();
-                    gen_movl_reg_T0(s, rn);
+                    }
+                    gen_movl_reg_T0(s, rd);
                 }
-                if ((insn & (1 << 22)) && !user) {
-                    /* Restore CPSR from SPSR.  */
-                    gen_op_movl_T0_spsr();
-                    gen_op_movl_cpsr_T0(0xffffffff);
-                    s->is_jmp = DISAS_UPDATE;
+            } else {
+                int shifter_out = 0;
+                /* modified 12-bit immediate.  */
+                shift = ((insn & 0x04000000) >> 23) | ((insn & 0x7000) >> 12);
+                imm = (insn & 0xff);
+                switch (shift) {
+                case 0: /* XY */
+                    /* Nothing to do.  */
+                    break;
+                case 1: /* 00XY00XY */
+                    imm |= imm << 16;
+                    break;
+                case 2: /* XY00XY00 */
+                    imm |= imm << 16;
+                    imm <<= 8;
+                    break;
+                case 3: /* XYXYXYXY */
+                    imm |= imm << 16;
+                    imm |= imm << 8;
+                    break;
+                default: /* Rotated constant.  */
+                    shift = (shift << 1) | (imm >> 7);
+                    imm |= 0x80;
+                    imm = imm << (32 - shift);
+                    shifter_out = 1;
+                    break;
                 }
-            }
-            break;
-        case 0xa:
-        case 0xb:
-            {
-                int32_t offset;
-
-                /* branch (and link) */
-                val = (int32_t)s->pc;
-                if (insn & (1 << 24)) {
-                    gen_op_movl_T0_im(val);
-                    gen_op_movl_reg_TN[0][14]();
+                gen_op_movl_T1_im(imm);
+                rn = (insn >> 16) & 0xf;
+                if (rn == 15)
+                    gen_op_movl_T0_im(0);
+                else
+                    gen_movl_T0_reg(s, rn);
+                op = (insn >> 21) & 0xf;
+                if (gen_thumb2_data_op(s, op, (insn & (1 << 20)) != 0,
+                                       shifter_out))
+                    goto illegal_op;
+                rd = (insn >> 8) & 0xf;
+                if (rd != 15) {
+                    gen_movl_reg_T0(s, rd);
                 }
-                offset = (((int32_t)insn << 8) >> 8);
-                val += (offset << 2) + 4;
-                gen_jmp(s, val);
             }
-            break;
-        case 0xc:
-        case 0xd:
-        case 0xe:
-            /* Coprocessor.  */
-            op1 = (insn >> 8) & 0xf;
-            if (arm_feature(env, ARM_FEATURE_XSCALE) &&
-                    ((env->cp15.c15_cpar ^ 0x3fff) & (1 << op1)))
+        }
+        break;
+    case 12: /* Load/store single data item.  */
+        {
+        int postinc = 0;
+        int writeback = 0;
+        if ((insn & 0x01100000) == 0x01000000) {
+            if (disas_neon_ls_insn(env, s, insn))
                 goto illegal_op;
-            switch (op1) {
-            case 0 ... 1:
-                if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
-                    if (disas_iwmmxt_insn(env, s, insn))
-                        goto illegal_op;
-                } else if (arm_feature(env, ARM_FEATURE_XSCALE)) {
-                    if (disas_dsp_insn(env, s, insn))
+            break;
+        }
+        if (rn == 15) {
+            /* PC relative.  */
+            /* s->pc has already been incremented by 4.  */
+            imm = s->pc & 0xfffffffc;
+            if (insn & (1 << 23))
+                imm += insn & 0xfff;
+            else
+                imm -= insn & 0xfff;
+            gen_op_movl_T1_im(imm);
+        } else {
+            gen_movl_T1_reg(s, rn);
+            if (insn & (1 << 23)) {
+                /* Positive offset.  */
+                imm = insn & 0xfff;
+                gen_op_addl_T1_im(imm);
+            } else {
+                op = (insn >> 8) & 7;
+                imm = insn & 0xff;
+                switch (op) {
+                case 0: case 8: /* Shifted Register.  */
+                    shift = (insn >> 4) & 0xf;
+                    if (shift > 3)
                         goto illegal_op;
-                } else
-                    goto illegal_op;
-                break;
-            case 2 ... 9:
-            case 12 ... 14:
-                if (disas_cp_insn (env, s, insn))
-                    goto illegal_op;
-                break;
-            case 10:
-            case 11:
-                if (disas_vfp_insn (env, s, insn))
+                    gen_movl_T2_reg(s, rm);
+                    if (shift)
+                        gen_op_shll_T2_im(shift);
+                    gen_op_addl_T1_T2();
+                    break;
+                case 4: /* Negative offset.  */
+                    gen_op_addl_T1_im(-imm);
+                    break;
+                case 6: /* User privilege.  */
+                    gen_op_addl_T1_im(imm);
+                    break;
+                case 1: /* Post-decrement.  */
+                    imm = -imm;
+                    /* Fall through.  */
+                case 3: /* Post-increment.  */
+                    gen_op_movl_T2_im(imm);
+                    postinc = 1;
+                    writeback = 1;
+                    break;
+                case 5: /* Pre-decrement.  */
+                    imm = -imm;
+                    /* Fall through.  */
+                case 7: /* Pre-increment.  */
+                    gen_op_addl_T1_im(imm);
+                    writeback = 1;
+                    break;
+                default:
                     goto illegal_op;
-                break;
-            case 15:
-                if (disas_cp15_insn (env, s, insn))
+                }
+            }
+        }
+        op = ((insn >> 21) & 3) | ((insn >> 22) & 4);
+        if (insn & (1 << 20)) {
+            /* Load.  */
+            if (rs == 15 && op != 2) {
+                if (op & 2)
                     goto illegal_op;
-                break;
-            default:
-                /* unknown coprocessor.  */
+                /* Memory hint.  Implemented as NOP.  */
+            } else {
+                switch (op) {
+                case 0: gen_ldst(ldub, s); break;
+                case 4: gen_ldst(ldsb, s); break;
+                case 1: gen_ldst(lduw, s); break;
+                case 5: gen_ldst(ldsw, s); break;
+                case 2: gen_ldst(ldl, s); break;
+                default: goto illegal_op;
+                }
+                if (rs == 15) {
+                    gen_bx(s);
+                } else {
+                    gen_movl_reg_T0(s, rs);
+                }
+            }
+        } else {
+            /* Store.  */
+            if (rs == 15)
                 goto illegal_op;
+            gen_movl_T0_reg(s, rs);
+            switch (op) {
+            case 0: gen_ldst(stb, s); break;
+            case 1: gen_ldst(stw, s); break;
+            case 2: gen_ldst(stl, s); break;
+            default: goto illegal_op;
             }
-            break;
-        case 0xf:
-            /* swi */
-            gen_op_movl_T0_im((long)s->pc);
-            gen_op_movl_reg_TN[0][15]();
-            gen_op_swi();
-            s->is_jmp = DISAS_JUMP;
-            break;
-        default:
-        illegal_op:
-            gen_op_movl_T0_im((long)s->pc - 4);
-            gen_op_movl_reg_TN[0][15]();
-            gen_op_undef_insn();
-            s->is_jmp = DISAS_JUMP;
-            break;
         }
+        if (postinc)
+            gen_op_addl_T1_im(imm);
+        if (writeback)
+            gen_movl_reg_T1(s, rn);
+        }
+        break;
+    default:
+        goto illegal_op;
     }
+    return 0;
+illegal_op:
+    return 1;
 }
 
-static void disas_thumb_insn(DisasContext *s)
+static void disas_thumb_insn(CPUState *env, DisasContext *s)
 {
     uint32_t val, insn, op, rm, rn, rd, shift, cond;
     int32_t offset;
     int i;
 
+    if (s->condexec_mask) {
+        cond = s->condexec_cond;
+        s->condlabel = gen_new_label();
+        gen_test_cc[cond ^ 1](s->condlabel);
+        s->condjmp = 1;
+    }
+
     insn = lduw_code(s->pc);
     s->pc += 2;
 
@@ -2990,17 +6851,27 @@ static void disas_thumb_insn(DisasContext *s)
                 rm = (insn >> 6) & 7;
                 gen_movl_T1_reg(s, rm);
             }
-            if (insn & (1 << 9))
-                gen_op_subl_T0_T1_cc();
-            else
-                gen_op_addl_T0_T1_cc();
+            if (insn & (1 << 9)) {
+                if (s->condexec_mask)
+                    gen_op_subl_T0_T1();
+                else
+                    gen_op_subl_T0_T1_cc();
+            } else {
+                if (s->condexec_mask)
+                    gen_op_addl_T0_T1();
+                else
+                    gen_op_addl_T0_T1_cc();
+            }
             gen_movl_reg_T0(s, rd);
         } else {
             /* shift immediate */
             rm = (insn >> 3) & 7;
             shift = (insn >> 6) & 0x1f;
             gen_movl_T0_reg(s, rm);
-            gen_shift_T0_im_thumb[op](shift);
+            if (s->condexec_mask)
+                gen_shift_T0_im_thumb[op](shift);
+            else
+                gen_shift_T0_im_thumb_cc[op](shift);
             gen_movl_reg_T0(s, rd);
         }
         break;
@@ -3016,16 +6887,23 @@ static void disas_thumb_insn(DisasContext *s)
         }
         switch (op) {
         case 0: /* mov */
-            gen_op_logic_T0_cc();
+            if (!s->condexec_mask)
+                gen_op_logic_T0_cc();
             break;
         case 1: /* cmp */
             gen_op_subl_T0_T1_cc();
             break;
         case 2: /* add */
-            gen_op_addl_T0_T1_cc();
+            if (s->condexec_mask)
+                gen_op_addl_T0_T1();
+            else
+                gen_op_addl_T0_T1_cc();
             break;
         case 3: /* sub */
-            gen_op_subl_T0_T1_cc();
+            if (s->condexec_mask)
+                gen_op_subl_T0_T1();
+            else
+                gen_op_subl_T0_T1_cc();
             break;
         }
         if (op != 1)
@@ -3099,33 +6977,57 @@ static void disas_thumb_insn(DisasContext *s)
         switch (op) {
         case 0x0: /* and */
             gen_op_andl_T0_T1();
-            gen_op_logic_T0_cc();
+            if (!s->condexec_mask)
+                gen_op_logic_T0_cc();
             break;
         case 0x1: /* eor */
             gen_op_xorl_T0_T1();
-            gen_op_logic_T0_cc();
+            if (!s->condexec_mask)
+                gen_op_logic_T0_cc();
             break;
         case 0x2: /* lsl */
-            gen_op_shll_T1_T0_cc();
-            gen_op_logic_T1_cc();
+            if (s->condexec_mask) {
+                gen_op_shll_T1_T0();
+            } else {
+                gen_op_shll_T1_T0_cc();
+                gen_op_logic_T1_cc();
+            }
             break;
         case 0x3: /* lsr */
-            gen_op_shrl_T1_T0_cc();
-            gen_op_logic_T1_cc();
+            if (s->condexec_mask) {
+                gen_op_shrl_T1_T0();
+            } else {
+                gen_op_shrl_T1_T0_cc();
+                gen_op_logic_T1_cc();
+            }
             break;
         case 0x4: /* asr */
-            gen_op_sarl_T1_T0_cc();
-            gen_op_logic_T1_cc();
+            if (s->condexec_mask) {
+                gen_op_sarl_T1_T0();
+            } else {
+                gen_op_sarl_T1_T0_cc();
+                gen_op_logic_T1_cc();
+            }
             break;
         case 0x5: /* adc */
-            gen_op_adcl_T0_T1_cc();
+            if (s->condexec_mask)
+                gen_op_adcl_T0_T1();
+            else
+                gen_op_adcl_T0_T1_cc();
             break;
         case 0x6: /* sbc */
-            gen_op_sbcl_T0_T1_cc();
+            if (s->condexec_mask)
+                gen_op_sbcl_T0_T1();
+            else
+                gen_op_sbcl_T0_T1_cc();
             break;
         case 0x7: /* ror */
-            gen_op_rorl_T1_T0_cc();
-            gen_op_logic_T1_cc();
+            if (s->condexec_mask) {
+                gen_op_rorl_T1_T0();
+            } else {
+                gen_op_rorl_T1_T0_cc();
+                gen_op_logic_T1_cc();
+            }
             break;
         case 0x8: /* tst */
             gen_op_andl_T0_T1();
@@ -3133,7 +7035,10 @@ static void disas_thumb_insn(DisasContext *s)
             rd = 16;
             break;
         case 0x9: /* neg */
-            gen_op_subl_T0_T1_cc();
+            if (s->condexec_mask)
+                gen_op_subl_T0_T1();
+            else
+                gen_op_subl_T0_T1_cc();
             break;
         case 0xa: /* cmp */
             gen_op_subl_T0_T1_cc();
@@ -3145,19 +7050,23 @@ static void disas_thumb_insn(DisasContext *s)
             break;
         case 0xc: /* orr */
             gen_op_orl_T0_T1();
-            gen_op_logic_T0_cc();
+            if (!s->condexec_mask)
+                gen_op_logic_T0_cc();
             break;
         case 0xd: /* mul */
             gen_op_mull_T0_T1();
-            gen_op_logic_T0_cc();
+            if (!s->condexec_mask)
+                gen_op_logic_T0_cc();
             break;
         case 0xe: /* bic */
             gen_op_bicl_T0_T1();
-            gen_op_logic_T0_cc();
+            if (!s->condexec_mask)
+                gen_op_logic_T0_cc();
             break;
         case 0xf: /* mvn */
             gen_op_notl_T1();
-            gen_op_logic_T1_cc();
+            if (!s->condexec_mask)
+                gen_op_logic_T1_cc();
             val = 1;
             rm = rd;
             break;
@@ -3323,6 +7232,19 @@ static void disas_thumb_insn(DisasContext *s)
             gen_movl_reg_T1(s, 13);
             break;
 
+        case 2: /* sign/zero extend.  */
+            ARCH(6);
+            rd = insn & 7;
+            rm = (insn >> 3) & 7;
+            gen_movl_T1_reg(s, rm);
+            switch ((insn >> 6) & 3) {
+            case 0: gen_op_sxth_T1(); break;
+            case 1: gen_op_sxtb_T1(); break;
+            case 2: gen_op_uxth_T1(); break;
+            case 3: gen_op_uxtb_T1(); break;
+            }
+            gen_movl_reg_T1(s, rd);
+            break;
         case 4: case 5: case 0xc: case 0xd:
             /* push/pop */
             gen_movl_T1_reg(s, 13);
@@ -3378,13 +7300,82 @@ static void disas_thumb_insn(DisasContext *s)
                 gen_bx(s);
             break;
 
+        case 1: case 3: case 9: case 11: /* czb */
+            rm = insn & 7;
+            gen_movl_T0_reg(s, rm);
+            s->condlabel = gen_new_label();
+            s->condjmp = 1;
+            if (insn & (1 << 11))
+                gen_op_testn_T0(s->condlabel);
+            else
+                gen_op_test_T0(s->condlabel);
+
+            offset = ((insn & 0xf8) >> 2) | (insn & 0x200) >> 3;
+            val = (uint32_t)s->pc + 2;
+            val += offset;
+            gen_jmp(s, val);
+            break;
+
+        case 15: /* IT, nop-hint.  */
+            if ((insn & 0xf) == 0) {
+                gen_nop_hint(s, (insn >> 4) & 0xf);
+                break;
+            }
+            /* If Then.  */
+            s->condexec_cond = (insn >> 4) & 0xe;
+            s->condexec_mask = insn & 0x1f;
+            /* No actual code generated for this insn, just setup state.  */
+            break;
+
         case 0xe: /* bkpt */
+            gen_set_condexec(s);
             gen_op_movl_T0_im((long)s->pc - 2);
             gen_op_movl_reg_TN[0][15]();
             gen_op_bkpt();
             s->is_jmp = DISAS_JUMP;
             break;
 
+        case 0xa: /* rev */
+            ARCH(6);
+            rn = (insn >> 3) & 0x7;
+            rd = insn & 0x7;
+            gen_movl_T0_reg(s, rn);
+            switch ((insn >> 6) & 3) {
+            case 0: gen_op_rev_T0(); break;
+            case 1: gen_op_rev16_T0(); break;
+            case 3: gen_op_revsh_T0(); break;
+            default: goto illegal_op;
+            }
+            gen_movl_reg_T0(s, rd);
+            break;
+
+        case 6: /* cps */
+            ARCH(6);
+            if (IS_USER(s))
+                break;
+            if (IS_M(env)) {
+                val = (insn & (1 << 4)) != 0;
+                gen_op_movl_T0_im(val);
+                /* PRIMASK */
+                if (insn & 1)
+                    gen_op_v7m_msr_T0(16);
+                /* FAULTMASK */
+                if (insn & 2)
+                    gen_op_v7m_msr_T0(17);
+
+                gen_lookup_tb(s);
+            } else {
+                if (insn & (1 << 4))
+                    shift = CPSR_A | CPSR_I | CPSR_F;
+                else
+                    shift = 0;
+
+                val = ((insn & 7) << 6) & shift;
+                gen_op_movl_T0_im(val);
+                gen_set_psr_T0(s, shift, 0);
+            }
+            break;
+
         default:
             goto undef;
         }
@@ -3423,19 +7414,17 @@ static void disas_thumb_insn(DisasContext *s)
 
         if (cond == 0xf) {
             /* swi */
+            gen_set_condexec(s);
             gen_op_movl_T0_im((long)s->pc | 1);
             /* Don't set r15.  */
             gen_op_movl_reg_TN[0][15]();
-            gen_op_swi();
-            s->is_jmp = DISAS_JUMP;
+            s->is_jmp = DISAS_SWI;
             break;
         }
         /* generate a conditional jump to next instruction */
         s->condlabel = gen_new_label();
         gen_test_cc[cond ^ 1](s->condlabel);
         s->condjmp = 1;
-        //gen_test_cc[cond ^ 1]((long)s->tb, (long)s->pc);
-        //s->is_jmp = DISAS_JUMP_NEXT;
         gen_movl_T1_reg(s, 15);
 
         /* jump to the offset */
@@ -3446,22 +7435,12 @@ static void disas_thumb_insn(DisasContext *s)
         break;
 
     case 14:
-        /* unconditional branch */
         if (insn & (1 << 11)) {
-            /* Second half of blx.  */
-            offset = ((insn & 0x7ff) << 1);
-            gen_movl_T0_reg(s, 14);
-            gen_op_movl_T1_im(offset);
-            gen_op_addl_T0_T1();
-            gen_op_movl_T1_im(0xfffffffc);
-            gen_op_andl_T0_T1();
-
-            val = (uint32_t)s->pc;
-            gen_op_movl_T1_im(val | 1);
-            gen_movl_reg_T1(s, 14);
-            gen_bx(s);
+            if (disas_thumb2_insn(env, s, insn))
+              goto undef32;
             break;
         }
+        /* unconditional branch */
         val = (uint32_t)s->pc;
         offset = ((int32_t)insn << 21) >> 21;
         val += (offset << 1) + 2;
@@ -3469,51 +7448,21 @@ static void disas_thumb_insn(DisasContext *s)
         break;
 
     case 15:
-        /* branch and link [and switch to arm] */
-        if ((s->pc & ~TARGET_PAGE_MASK) == 0) {
-            /* Instruction spans a page boundary.  Implement it as two
-               16-bit instructions in case the second half causes an
-               prefetch abort.  */
-            offset = ((int32_t)insn << 21) >> 9;
-            val = s->pc + 2 + offset;
-            gen_op_movl_T0_im(val);
-            gen_movl_reg_T0(s, 14);
-            break;
-        }
-        if (insn & (1 << 11)) {
-            /* Second half of bl.  */
-            offset = ((insn & 0x7ff) << 1) | 1;
-            gen_movl_T0_reg(s, 14);
-            gen_op_movl_T1_im(offset);
-            gen_op_addl_T0_T1();
-
-            val = (uint32_t)s->pc;
-            gen_op_movl_T1_im(val | 1);
-            gen_movl_reg_T1(s, 14);
-            gen_bx(s);
-            break;
-        }
-        offset = ((int32_t)insn << 21) >> 10;
-        insn = lduw_code(s->pc);
-        offset |= insn & 0x7ff;
-
-        val = (uint32_t)s->pc + 2;
-        gen_op_movl_T1_im(val | 1);
-        gen_movl_reg_T1(s, 14);
-
-        val += offset << 1;
-        if (insn & (1 << 12)) {
-            /* bl */
-            gen_jmp(s, val);
-        } else {
-            /* blx */
-            val &= ~(uint32_t)2;
-            gen_op_movl_T0_im(val);
-            gen_bx(s);
-        }
+        if (disas_thumb2_insn(env, s, insn))
+          goto undef32;
+        break;
     }
     return;
+undef32:
+    gen_set_condexec(s);
+    gen_op_movl_T0_im((long)s->pc - 4);
+    gen_op_movl_reg_TN[0][15]();
+    gen_op_undef_insn();
+    s->is_jmp = DISAS_JUMP;
+    return;
+illegal_op:
 undef:
+    gen_set_condexec(s);
     gen_op_movl_T0_im((long)s->pc - 2);
     gen_op_movl_reg_TN[0][15]();
     gen_op_undef_insn();
@@ -3547,21 +7496,44 @@ static inline int gen_intermediate_code_internal(CPUState *env,
     dc->singlestep_enabled = env->singlestep_enabled;
     dc->condjmp = 0;
     dc->thumb = env->thumb;
+    dc->condexec_mask = (env->condexec_bits & 0xf) << 1;
+    dc->condexec_cond = env->condexec_bits >> 4;
     dc->is_mem = 0;
 #if !defined(CONFIG_USER_ONLY)
-    dc->user = (env->uncached_cpsr & 0x1f) == ARM_CPU_MODE_USR;
+    if (IS_M(env)) {
+        dc->user = ((env->v7m.exception == 0) && (env->v7m.control & 1));
+    } else {
+        dc->user = (env->uncached_cpsr & 0x1f) == ARM_CPU_MODE_USR;
+    }
 #endif
     next_page_start = (pc_start & TARGET_PAGE_MASK) + TARGET_PAGE_SIZE;
     nb_gen_labels = 0;
     lj = -1;
+    /* Reset the conditional execution bits immediately. This avoids
+       complications trying to do it at the end of the block.  */
+    if (env->condexec_bits)
+      gen_op_set_condexec(0);
     do {
+#ifndef CONFIG_USER_ONLY
+        if (dc->pc >= 0xfffffff0 && IS_M(env)) {
+            /* We always get here via a jump, so know we are not in a
+               conditional execution block.  */
+            gen_op_exception_exit();
+        }
+#endif
+
         if (env->nb_breakpoints > 0) {
             for(j = 0; j < env->nb_breakpoints; j++) {
                 if (env->breakpoints[j] == dc->pc) {
+                    gen_set_condexec(dc);
                     gen_op_movl_T0_im((long)dc->pc);
                     gen_op_movl_reg_TN[0][15]();
                     gen_op_debug();
                     dc->is_jmp = DISAS_JUMP;
+                    /* Advance PC so that clearing the breakpoint will
+                       invalidate this TB.  */
+                    dc->pc += 2;
+                    goto done_generating;
                     break;
                 }
             }
@@ -3577,10 +7549,19 @@ static inline int gen_intermediate_code_internal(CPUState *env,
             gen_opc_instr_start[lj] = 1;
         }
 
-        if (env->thumb)
-          disas_thumb_insn(dc);
-        else
-          disas_arm_insn(env, dc);
+        if (env->thumb) {
+            disas_thumb_insn(env, dc);
+            if (dc->condexec_mask) {
+                dc->condexec_cond = (dc->condexec_cond & 0xe)
+                                   | ((dc->condexec_mask >> 4) & 1);
+                dc->condexec_mask = (dc->condexec_mask << 1) & 0x1f;
+                if (dc->condexec_mask == 0) {
+                    dc->condexec_cond = 0;
+                }
+            }
+        } else {
+            disas_arm_insn(env, dc);
+        }
 
         if (dc->condjmp && !dc->is_jmp) {
             gen_set_label(dc->condlabel);
@@ -3599,13 +7580,19 @@ static inline int gen_intermediate_code_internal(CPUState *env,
     } while (!dc->is_jmp && gen_opc_ptr < gen_opc_end &&
              !env->singlestep_enabled &&
              dc->pc < next_page_start);
+
     /* At this stage dc->condjmp will only be set when the skipped
-     * instruction was a conditional branch, and the PC has already been
-     * written.  */
+       instruction was a conditional branch or trap, and the PC has
+       already been written.  */
     if (__builtin_expect(env->singlestep_enabled, 0)) {
         /* Make sure the pc is updated, and raise a debug exception.  */
         if (dc->condjmp) {
-            gen_op_debug();
+            gen_set_condexec(dc);
+            if (dc->is_jmp == DISAS_SWI) {
+                gen_op_swi();
+            } else {
+                gen_op_debug();
+            }
             gen_set_label(dc->condlabel);
         }
         if (dc->condjmp || !dc->is_jmp) {
@@ -3613,8 +7600,24 @@ static inline int gen_intermediate_code_internal(CPUState *env,
             gen_op_movl_reg_TN[0][15]();
             dc->condjmp = 0;
         }
-        gen_op_debug();
+        gen_set_condexec(dc);
+        if (dc->is_jmp == DISAS_SWI && !dc->condjmp) {
+            gen_op_swi();
+        } else {
+            /* FIXME: Single stepping a WFI insn will not halt
+               the CPU.  */
+            gen_op_debug();
+        }
     } else {
+        /* While branches must always occur at the end of an IT block,
+           there are a few other things that can cause us to terminate
+           the TB in the middel of an IT block:
+            - Exception generating instructions (bkpt, swi, undefined).
+            - Page boundaries.
+            - Hardware watchpoints.
+           Hardware breakpoints have already been handled and skip this code.
+         */
+        gen_set_condexec(dc);
         switch(dc->is_jmp) {
         case DISAS_NEXT:
             gen_goto_tb(dc, 1, dc->pc);
@@ -3629,13 +7632,21 @@ static inline int gen_intermediate_code_internal(CPUState *env,
         case DISAS_TB_JUMP:
             /* nothing more to generate */
             break;
+        case DISAS_WFI:
+            gen_op_wfi();
+            break;
+        case DISAS_SWI:
+            gen_op_swi();
+            break;
         }
         if (dc->condjmp) {
             gen_set_label(dc->condlabel);
+            gen_set_condexec(dc);
             gen_goto_tb(dc, 1, dc->pc);
             dc->condjmp = 0;
         }
     }
+done_generating:
     *gen_opc_ptr = INDEX_op_end;
 
 #ifdef DEBUG_DISAS
@@ -3676,6 +7687,7 @@ static const char *cpu_mode_names[16] = {
   "usr", "fiq", "irq", "svc", "???", "???", "???", "abt",
   "???", "???", "???", "und", "???", "???", "???", "sys"
 };
+
 void cpu_dump_state(CPUState *env, FILE *f,
                     int (*cpu_fprintf)(FILE *f, const char *fmt, ...),
                     int flags)
diff --git a/vl.c b/vl.c
index d1ae8ca..7ffc904 100644
--- a/vl.c
+++ b/vl.c
@@ -6126,7 +6126,9 @@ void cpu_save(QEMUFile *f, void *opaque)
     qemu_put_be32(f, env->cp15.c1_sys);
     qemu_put_be32(f, env->cp15.c1_coproc);
     qemu_put_be32(f, env->cp15.c1_xscaleauxcr);
-    qemu_put_be32(f, env->cp15.c2_base);
+    qemu_put_be32(f, env->cp15.c2_base0);
+    qemu_put_be32(f, env->cp15.c2_base1);
+    qemu_put_be32(f, env->cp15.c2_mask);
     qemu_put_be32(f, env->cp15.c2_data);
     qemu_put_be32(f, env->cp15.c2_insn);
     qemu_put_be32(f, env->cp15.c3);
@@ -6141,6 +6143,9 @@ void cpu_save(QEMUFile *f, void *opaque)
     qemu_put_be32(f, env->cp15.c9_data);
     qemu_put_be32(f, env->cp15.c13_fcse);
     qemu_put_be32(f, env->cp15.c13_context);
+    qemu_put_be32(f, env->cp15.c13_tls1);
+    qemu_put_be32(f, env->cp15.c13_tls2);
+    qemu_put_be32(f, env->cp15.c13_tls3);
     qemu_put_be32(f, env->cp15.c15_cpar);
 
     qemu_put_be32(f, env->features);
@@ -6159,6 +6164,15 @@ void cpu_save(QEMUFile *f, void *opaque)
         /* TODO: Should use proper FPSCR access functions.  */
         qemu_put_be32(f, env->vfp.vec_len);
         qemu_put_be32(f, env->vfp.vec_stride);
+
+        if (arm_feature(env, ARM_FEATURE_VFP3)) {
+            for (i = 16;  i < 32; i++) {
+                CPU_DoubleU u;
+                u.d = env->vfp.regs[i];
+                qemu_put_be32(f, u.l.upper);
+                qemu_put_be32(f, u.l.lower);
+            }
+        }
     }
 
     if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
@@ -6169,6 +6183,15 @@ void cpu_save(QEMUFile *f, void *opaque)
             qemu_put_be32(f, env->iwmmxt.cregs[i]);
         }
     }
+
+    if (arm_feature(env, ARM_FEATURE_M)) {
+        qemu_put_be32(f, env->v7m.other_sp);
+        qemu_put_be32(f, env->v7m.vecbase);
+        qemu_put_be32(f, env->v7m.basepri);
+        qemu_put_be32(f, env->v7m.control);
+        qemu_put_be32(f, env->v7m.current_sp);
+        qemu_put_be32(f, env->v7m.exception);
+    }
 }
 
 int cpu_load(QEMUFile *f, void *opaque, int version_id)
@@ -6176,7 +6199,7 @@ int cpu_load(QEMUFile *f, void *opaque, int version_id)
     CPUARMState *env = (CPUARMState *)opaque;
     int i;
 
-    if (version_id != 0)
+    if (version_id != ARM_CPU_SAVE_VERSION)
         return -EINVAL;
 
     for (i = 0; i < 16; i++) {
@@ -6198,7 +6221,9 @@ int cpu_load(QEMUFile *f, void *opaque, int version_id)
     env->cp15.c1_sys = qemu_get_be32(f);
     env->cp15.c1_coproc = qemu_get_be32(f);
     env->cp15.c1_xscaleauxcr = qemu_get_be32(f);
-    env->cp15.c2_base = qemu_get_be32(f);
+    env->cp15.c2_base0 = qemu_get_be32(f);
+    env->cp15.c2_base1 = qemu_get_be32(f);
+    env->cp15.c2_mask = qemu_get_be32(f);
     env->cp15.c2_data = qemu_get_be32(f);
     env->cp15.c2_insn = qemu_get_be32(f);
     env->cp15.c3 = qemu_get_be32(f);
@@ -6213,6 +6238,9 @@ int cpu_load(QEMUFile *f, void *opaque, int version_id)
     env->cp15.c9_data = qemu_get_be32(f);
     env->cp15.c13_fcse = qemu_get_be32(f);
     env->cp15.c13_context = qemu_get_be32(f);
+    env->cp15.c13_tls1 = qemu_get_be32(f);
+    env->cp15.c13_tls2 = qemu_get_be32(f);
+    env->cp15.c13_tls3 = qemu_get_be32(f);
     env->cp15.c15_cpar = qemu_get_be32(f);
 
     env->features = qemu_get_be32(f);
@@ -6231,6 +6259,15 @@ int cpu_load(QEMUFile *f, void *opaque, int version_id)
         /* TODO: Should use proper FPSCR access functions.  */
         env->vfp.vec_len = qemu_get_be32(f);
         env->vfp.vec_stride = qemu_get_be32(f);
+
+        if (arm_feature(env, ARM_FEATURE_VFP3)) {
+            for (i = 0;  i < 16; i++) {
+                CPU_DoubleU u;
+                u.l.upper = qemu_get_be32(f);
+                u.l.lower = qemu_get_be32(f);
+                env->vfp.regs[i] = u.d;
+            }
+        }
     }
 
     if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
@@ -6242,6 +6279,15 @@ int cpu_load(QEMUFile *f, void *opaque, int version_id)
         }
     }
 
+    if (arm_feature(env, ARM_FEATURE_M)) {
+        env->v7m.other_sp = qemu_get_be32(f);
+        env->v7m.vecbase = qemu_get_be32(f);
+        env->v7m.basepri = qemu_get_be32(f);
+        env->v7m.control = qemu_get_be32(f);
+        env->v7m.current_sp = qemu_get_be32(f);
+        env->v7m.exception = qemu_get_be32(f);
+    }
+
     return 0;
 }
 
@@ -7392,6 +7438,8 @@ void register_machines(void)
     qemu_register_machine(&borzoipda_machine);
     qemu_register_machine(&terrierpda_machine);
     qemu_register_machine(&palmte_machine);
+    qemu_register_machine(&lm3s811evb_machine);
+    qemu_register_machine(&lm3s6965evb_machine);
 #elif defined(TARGET_SH4)
     qemu_register_machine(&shix_machine);
     qemu_register_machine(&r2d_machine);
diff --git a/vl.h b/vl.h
index 047fd3b..c4b4f77 100644
--- a/vl.h
+++ b/vl.h
@@ -1482,6 +1482,14 @@ extern QEMUMachine terrierpda_machine;
 /* palm.c */
 extern QEMUMachine palmte_machine;
 
+/* armv7m.c */
+qemu_irq *armv7m_init(int flash_size, int sram_size,
+                      const char *kernel_filename, const char *cpu_model);
+
+/* stellaris.c */
+extern QEMUMachine lm3s811evb_machine;
+extern QEMUMachine lm3s6965evb_machine;
+
 /* ps2.c */
 void *ps2_kbd_init(void (*update_irq)(void *, int), void *update_arg);
 void *ps2_mouse_init(void (*update_irq)(void *, int), void *update_arg);
@@ -1502,11 +1510,24 @@ void pl031_init(uint32_t base, qemu_irq irq);
 void *pl110_init(DisplayState *ds, uint32_t base, qemu_irq irq, int);
 
 /* pl011.c */
-void pl011_init(uint32_t base, qemu_irq irq, CharDriverState *chr);
+enum pl011_type {
+    PL011_ARM,
+    PL011_LUMINARY
+};
+
+void pl011_init(uint32_t base, qemu_irq irq, CharDriverState *chr,
+                enum pl011_type type);
+
+/* pl022.c */
+void pl022_init(uint32_t base, qemu_irq irq, int (*xfer_cb)(void *, int),
+                void *opaque);
 
 /* pl050.c */
 void pl050_init(uint32_t base, qemu_irq irq, int is_mouse);
 
+/* pl061.c */
+qemu_irq *pl061_init(uint32_t base, qemu_irq irq, qemu_irq **out);
+
 /* pl080.c */
 void *pl080_init(uint32_t base, qemu_irq irq, int nchannels);
 
@@ -1524,8 +1545,11 @@ void icp_pit_init(uint32_t base, qemu_irq *pic, int irq);
 /* arm_sysctl.c */
 void arm_sysctl_init(uint32_t base, uint32_t sys_id);
 
-/* arm_gic.c */
-qemu_irq *arm_gic_init(uint32_t base, qemu_irq parent_irq);
+/* realview_gic.c */
+qemu_irq *realview_gic_init(uint32_t base, qemu_irq parent_irq);
+
+/* mpcore.c */
+extern qemu_irq *mpcore_irq_init(qemu_irq *cpu_irq);
 
 /* arm_boot.c */
 
@@ -1533,6 +1557,16 @@ void arm_load_kernel(CPUState *env, int ram_size, const char *kernel_filename,
                      const char *kernel_cmdline, const char *initrd_filename,
                      int board_id, target_phys_addr_t loader_start);
 
+/* armv7m_nvic.c */
+qemu_irq *armv7m_nvic_init(CPUState *env);
+
+/* ssd0303.c */
+void ssd0303_init(DisplayState *ds, i2c_bus *bus, int address);
+
+/* ssd0323.c */
+int ssd0323_xfer_ssi(void *opaque, int data);
+void *ssd0323_init(DisplayState *ds, qemu_irq *cmd_p);
+
 /* sh7750.c */
 struct SH7750State;
 
