diff --git a/hw/grlib.h b/hw/grlib.h
index e1c4137..2a6e05d 100644
--- a/hw/grlib.h
+++ b/hw/grlib.h
@@ -34,38 +34,34 @@
 
 /* IRQMP */
 
-typedef void (*set_pil_in_fn) (void *opaque, uint32_t pil_in);
-
-void grlib_irqmp_set_irq(void *opaque, int irq, int level);
-
-void grlib_irqmp_ack(DeviceState *dev, int intno);
+void grlib_irqmp_ack(int cpu, DeviceState *dev, int intno);
 
 static inline
 DeviceState *grlib_irqmp_create(target_phys_addr_t   base,
-                                CPUSPARCState            *env,
-                                qemu_irq           **cpu_irqs,
-                                uint32_t             nr_irqs,
-                                set_pil_in_fn        set_pil_in)
+                                qemu_irq           **cpu_irqs)
 {
     DeviceState *dev;
+    SysBusDevice *s;
+    CPUSPARCState *env;
 
     assert(cpu_irqs != NULL);
 
     dev = qdev_create(NULL, "grlib,irqmp");
-    qdev_prop_set_ptr(dev, "set_pil_in", set_pil_in);
-    qdev_prop_set_ptr(dev, "set_pil_in_opaque", env);
 
     if (qdev_init(dev)) {
         return NULL;
     }
 
-    env->irq_manager = dev;
+    s = sysbus_from_qdev(dev);
+
+    for (env = first_cpu; env; env = env->next_cpu) {
+        env->irq_manager = dev;
+        env->qemu_irq_ack = leon3_irq_manager;
+        sysbus_connect_irq(s, env->cpu_index, cpu_irqs[env->cpu_index][0]);
+    }
 
     sysbus_mmio_map(sysbus_from_qdev(dev), 0, base);
 
-    *cpu_irqs = qemu_allocate_irqs(grlib_irqmp_set_irq,
-                                   dev,
-                                   nr_irqs);
 
     return dev;
 }
diff --git a/hw/grlib_irqmp.c b/hw/grlib_irqmp.c
index 0f6e65c..74ab255 100644
--- a/hw/grlib_irqmp.c
+++ b/hw/grlib_irqmp.c
@@ -26,12 +26,14 @@
 
 #include "sysbus.h"
 #include "cpu.h"
+#include "sysemu.h"
 
 #include "grlib.h"
 
 #include "trace.h"
 
 #define IRQMP_MAX_CPU 16
+#define IRQMP_MAX_PILS 16
 #define IRQMP_REG_SIZE 256      /* Size of memory mapped registers */
 
 /* Memory mapped register offsets */
@@ -45,14 +47,15 @@
 #define FORCE_OFFSET     0x80
 #define EXTENDED_OFFSET  0xC0
 
+/* Extended interrupt number (not supported yet) */
+#define EXTENDED_IRQ     0x00
+
 typedef struct IRQMPState IRQMPState;
 
 typedef struct IRQMP {
     SysBusDevice busdev;
     MemoryRegion iomem;
-
-    void *set_pil_in;
-    void *set_pil_in_opaque;
+    qemu_irq cpu_irqs[IRQMP_MAX_CPU];
 
     IRQMPState *state;
 } IRQMP;
@@ -60,7 +63,6 @@ typedef struct IRQMP {
 struct IRQMPState {
     uint32_t level;
     uint32_t pending;
-    uint32_t clear;
     uint32_t broadcast;
 
     uint32_t mask[IRQMP_MAX_CPU];
@@ -70,37 +72,43 @@ struct IRQMPState {
     IRQMP    *parent;
 };
 
-static void grlib_irqmp_check_irqs(IRQMPState *state)
+
+static unsigned int grlib_irqmp_irq_prioritize(uint32_t request)
 {
-    uint32_t      pend   = 0;
-    uint32_t      level0 = 0;
-    uint32_t      level1 = 0;
-    set_pil_in_fn set_pil_in;
+    unsigned int level;
 
-    assert(state != NULL);
-    assert(state->parent != NULL);
+    /* Interrupt 15 has highest priority */
+    for (level = IRQMP_MAX_PILS - 1; level > 0; level--) {
+        if (request & (1 << level)) {
+            return level;
+        }
+    }
 
-    /* IRQ for CPU 0 (no SMP support) */
-    pend = (state->pending | state->force[0])
-        & state->mask[0];
+    return 0;
+}
 
-    level0 = pend & ~state->level;
-    level1 = pend &  state->level;
+static void grlib_irqmp_check_irqs(IRQMPState *state)
+{
+    unsigned int level, i;
+    uint32_t request;
 
-    trace_grlib_irqmp_check_irqs(state->pending, state->force[0],
-                                 state->mask[0], level1, level0);
+    for (i = 0; i < smp_cpus; i++) {
+        request = (state->pending | state->force[i]) & state->mask[i];
 
-    set_pil_in = (set_pil_in_fn)state->parent->set_pil_in;
+        /* Interrupts with level 1 have higher priority */
+        level = grlib_irqmp_irq_prioritize(request & state->level);
+        if (level == 0) {
+            level = grlib_irqmp_irq_prioritize(request & ~state->level);
+        }
 
-    /* Trigger level1 interrupt first and level0 if there is no level1 */
-    if (level1 != 0) {
-        set_pil_in(state->parent->set_pil_in_opaque, level1);
-    } else {
-        set_pil_in(state->parent->set_pil_in_opaque, level0);
+        trace_grlib_irqmp_check_irqs(state->pending, state->force[i],
+                                     state->mask[i], level);
+
+        qemu_set_irq(state->parent->cpu_irqs[i], level);
     }
 }
 
-void grlib_irqmp_ack(DeviceState *dev, int intno)
+void grlib_irqmp_ack(int cpu, DeviceState *dev, int intno)
 {
     SysBusDevice *sdev;
     IRQMP        *irqmp;
@@ -121,20 +129,26 @@ void grlib_irqmp_ack(DeviceState *dev, int intno)
     intno &= 15;
     mask = 1 << intno;
 
-    trace_grlib_irqmp_ack(intno);
+    trace_grlib_irqmp_ack(cpu, intno);
 
     /* Clear registers */
-    state->pending  &= ~mask;
-    state->force[0] &= ~mask; /* Only CPU 0 (No SMP support) */
+    if (state->force[cpu] & mask) {
+        /* Clear force bit if set */
+        state->force[cpu] &= ~mask;
+    } else {
+        /* Otherwise clear pending bit */
+        state->pending &= ~mask;
+    }
 
     grlib_irqmp_check_irqs(state);
 }
 
-void grlib_irqmp_set_irq(void *opaque, int irq, int level)
+static void grlib_irqmp_set_irq(void *opaque, int irq, int level)
 {
     IRQMP      *irqmp;
     IRQMPState *s;
     int         i = 0;
+    uint32_t    mask = 1 << irq;
 
     assert(opaque != NULL);
 
@@ -149,13 +163,13 @@ void grlib_irqmp_set_irq(void *opaque, int irq, int level)
     if (level) {
         trace_grlib_irqmp_set_irq(irq);
 
-        if (s->broadcast & 1 << irq) {
+        if ((smp_cpus > 1) && (s->broadcast & mask)) {
             /* Broadcasted IRQ */
             for (i = 0; i < IRQMP_MAX_CPU; i++) {
-                s->force[i] |= 1 << irq;
+                s->force[i] |= mask;
             }
         } else {
-            s->pending |= 1 << irq;
+            s->pending |= mask;
         }
         grlib_irqmp_check_irqs(s);
 
@@ -166,7 +180,9 @@ static uint64_t grlib_irqmp_read(void *opaque, target_phys_addr_t addr,
                                  unsigned size)
 {
     IRQMP      *irqmp = opaque;
+    uint32_t    value;
     IRQMPState *state;
+    CPUSPARCState *env;
 
     assert(irqmp != NULL);
     state = irqmp->state;
@@ -187,9 +203,23 @@ static uint64_t grlib_irqmp_read(void *opaque, target_phys_addr_t addr,
         return state->force[0];
 
     case CLEAR_OFFSET:
-    case MP_STATUS_OFFSET:
-        /* Always read as 0 */
         return 0;
+    case MP_STATUS_OFFSET:
+        /* Number of CPUs */
+        value = (smp_cpus - 1) << 28;
+        /* Broadcast available */
+        if (smp_cpus > 1) {
+            value |= (1 << 27);
+        }
+        /* Extended interrupt number */
+        value |= EXTENDED_IRQ << 16;
+        /* Power-down status of all CPUs */
+        for (env = first_cpu; env; env = env->next_cpu) {
+            if (env->halted) {
+                value |= 1 << env->cpu_index;
+            }
+        }
+        return value;
 
     case BROADCAST_OFFSET:
         return state->broadcast;
@@ -231,6 +261,7 @@ static void grlib_irqmp_write(void *opaque, target_phys_addr_t addr,
 {
     IRQMP      *irqmp = opaque;
     IRQMPState *state;
+    CPUSPARCState *env;
 
     assert(irqmp != NULL);
     state = irqmp->state;
@@ -241,7 +272,7 @@ static void grlib_irqmp_write(void *opaque, target_phys_addr_t addr,
     /* global registers */
     switch (addr) {
     case LEVEL_OFFSET:
-        value &= 0xFFFF << 1; /* clean up the value */
+        value &= 0xFFFE; /* clean up the value */
         state->level = value;
         return;
 
@@ -263,7 +294,13 @@ static void grlib_irqmp_write(void *opaque, target_phys_addr_t addr,
         return;
 
     case MP_STATUS_OFFSET:
-        /* Read Only (no SMP support) */
+        /* Start CPU when bit is set */
+        for (env = first_cpu; env; env = env->next_cpu) {
+            if (value & (1 << env->cpu_index)) {
+                env->halted = 0;
+                qemu_cpu_kick(env);
+            }
+        }
         return;
 
     case BROADCAST_OFFSET:
@@ -336,14 +373,11 @@ static void grlib_irqmp_reset(DeviceState *d)
 static int grlib_irqmp_init(SysBusDevice *dev)
 {
     IRQMP *irqmp = FROM_SYSBUS(typeof(*irqmp), dev);
+    unsigned int i;
 
     assert(irqmp != NULL);
 
-    /* Check parameters */
-    if (irqmp->set_pil_in == NULL) {
-        return -1;
-    }
-
+    qdev_init_gpio_in(&dev->qdev, grlib_irqmp_set_irq, IRQMP_MAX_PILS);
     memory_region_init_io(&irqmp->iomem, &grlib_irqmp_ops, irqmp,
                           "irqmp", IRQMP_REG_SIZE);
 
@@ -351,15 +385,13 @@ static int grlib_irqmp_init(SysBusDevice *dev)
 
     sysbus_init_mmio(dev, &irqmp->iomem);
 
+    for (i = 0; i < IRQMP_MAX_CPU; i++) {
+        sysbus_init_irq(dev, &irqmp->cpu_irqs[i]);
+    }
+
     return 0;
 }
 
-static Property grlib_irqmp_properties[] = {
-    DEFINE_PROP_PTR("set_pil_in", IRQMP, set_pil_in),
-    DEFINE_PROP_PTR("set_pil_in_opaque", IRQMP, set_pil_in_opaque),
-    DEFINE_PROP_END_OF_LIST(),
-};
-
 static void grlib_irqmp_class_init(ObjectClass *klass, void *data)
 {
     DeviceClass *dc = DEVICE_CLASS(klass);
@@ -367,7 +399,6 @@ static void grlib_irqmp_class_init(ObjectClass *klass, void *data)
 
     k->init = grlib_irqmp_init;
     dc->reset = grlib_irqmp_reset;
-    dc->props = grlib_irqmp_properties;
 }
 
 static TypeInfo grlib_irqmp_info = {
diff --git a/hw/leon3.c b/hw/leon3.c
index 878d3aa..a9a1ebd 100644
--- a/hw/leon3.c
+++ b/hw/leon3.c
@@ -58,42 +58,33 @@ static void main_cpu_reset(void *opaque)
     env->npc    = s->entry + 4;
 }
 
-void leon3_irq_ack(void *irq_manager, int intno)
+static void cpu_set_irq(void *opaque, int irq, int level)
 {
-    grlib_irqmp_ack((DeviceState *)irq_manager, intno);
-}
-
-static void leon3_set_pil_in(void *opaque, uint32_t pil_in)
-{
-    CPUSPARCState *env = (CPUSPARCState *)opaque;
-
-    assert(env != NULL);
-
-    env->pil_in = pil_in;
-
-    if (env->pil_in && (env->interrupt_index == 0 ||
-                        (env->interrupt_index & ~15) == TT_EXTINT)) {
-        unsigned int i;
-
-        for (i = 15; i > 0; i--) {
-            if (env->pil_in & (1 << i)) {
-                int old_interrupt = env->interrupt_index;
-
-                env->interrupt_index = TT_EXTINT | i;
-                if (old_interrupt != env->interrupt_index) {
-                    trace_leon3_set_irq(i);
-                    cpu_interrupt(env, CPU_INTERRUPT_HARD);
-                }
-                break;
-            }
+    CPUSPARCState *env = opaque;
+
+    if (level != 0 && (env->interrupt_index == 0 ||
+                     (env->interrupt_index & ~15) == TT_EXTINT)) {
+        int old_interrupt = env->interrupt_index;
+
+        env->interrupt_index = TT_EXTINT | level;
+        if (old_interrupt != env->interrupt_index) {
+            trace_leon3_set_irq(env->cpu_index, level);
+            env->halted = 0;
+            cpu_interrupt(env, CPU_INTERRUPT_HARD);
+            qemu_cpu_kick(env);
         }
-    } else if (!env->pil_in && (env->interrupt_index & ~15) == TT_EXTINT) {
-        trace_leon3_reset_irq(env->interrupt_index & 15);
+    } else if (level == 0 && (env->interrupt_index & ~15) == TT_EXTINT) {
+        trace_leon3_reset_irq(env->cpu_index, env->interrupt_index & 15);
         env->interrupt_index = 0;
         cpu_reset_interrupt(env, CPU_INTERRUPT_HARD);
     }
 }
 
+void leon3_irq_ack(CPUSPARCState *env, void *irq_manager, int intno)
+{
+    grlib_irqmp_ack(env->cpu_index, (DeviceState *)irq_manager, intno);
+}
+
 static void leon3_generic_hw_init(ram_addr_t  ram_size,
                                   const char *boot_device,
                                   const char *kernel_filename,
@@ -107,11 +98,13 @@ static void leon3_generic_hw_init(ram_addr_t  ram_size,
     MemoryRegion *ram = g_new(MemoryRegion, 1);
     MemoryRegion *prom = g_new(MemoryRegion, 1);
     int         ret;
+    int         i;
     char       *filename;
-    qemu_irq   *cpu_irqs = NULL;
+    qemu_irq   *cpu_irq, irqmp_irqs[MAX_PILS];
     int         bios_size;
     int         prom_size;
     ResetData  *reset_info;
+    DeviceState *irqmp;
 
     /* Init CPU */
     if (!cpu_model) {
@@ -132,10 +125,14 @@ static void leon3_generic_hw_init(ram_addr_t  ram_size,
     reset_info->cpu   = cpu;
     qemu_register_reset(main_cpu_reset, reset_info);
 
+    cpu_irq = qemu_allocate_irqs(cpu_set_irq, env, 1);
+
     /* Allocate IRQ manager */
-    grlib_irqmp_create(0x80000200, env, &cpu_irqs, MAX_PILS, &leon3_set_pil_in);
+    irqmp = grlib_irqmp_create(0x80000200, &cpu_irq);
 
-    env->qemu_irq_ack = leon3_irq_manager;
+    for (i = 0; i < MAX_PILS; i++) {
+        irqmp_irqs[i] = qdev_get_gpio_in(irqmp, i);
+    }
 
     /* Allocate RAM */
     if ((uint64_t)ram_size > (1UL << 30)) {
@@ -202,11 +199,11 @@ static void leon3_generic_hw_init(ram_addr_t  ram_size,
     }
 
     /* Allocate timers */
-    grlib_gptimer_create(0x80000300, 2, CPU_CLK, cpu_irqs, 6);
+    grlib_gptimer_create(0x80000300, 2, CPU_CLK, irqmp_irqs, 6);
 
     /* Allocate uart */
     if (serial_hds[0]) {
-        grlib_apbuart_create(0x80000100, serial_hds[0], cpu_irqs[3]);
+        grlib_apbuart_create(0x80000100, serial_hds[0], irqmp_irqs[3]);
     }
 }
 
diff --git a/target-sparc/cpu.h b/target-sparc/cpu.h
index e16b7b3..409ad0a 100644
--- a/target-sparc/cpu.h
+++ b/target-sparc/cpu.h
@@ -560,7 +560,7 @@ void leon3_irq_manager(CPUSPARCState *env, void *irq_manager, int intno);
 void cpu_check_irqs(CPUSPARCState *env);
 
 /* leon3.c */
-void leon3_irq_ack(void *irq_manager, int intno);
+void leon3_irq_ack(CPUSPARCState *env, void *irq_manager, int intno);
 
 #if defined (TARGET_SPARC64)
 
diff --git a/target-sparc/int32_helper.c b/target-sparc/int32_helper.c
index 5e33d50..cdaff5a 100644
--- a/target-sparc/int32_helper.c
+++ b/target-sparc/int32_helper.c
@@ -163,7 +163,7 @@ static void leon3_cache_control_int(CPUSPARCState *env)
 
 void leon3_irq_manager(CPUSPARCState *env, void *irq_manager, int intno)
 {
-    leon3_irq_ack(irq_manager, intno);
+    leon3_irq_ack(env, irq_manager, intno);
     leon3_cache_control_int(env);
 }
 #endif
diff --git a/trace-events b/trace-events
index b48fe2d..cf48c9e 100644
--- a/trace-events
+++ b/trace-events
@@ -492,9 +492,9 @@ grlib_gptimer_readl(int id, uint64_t addr, uint32_t val) "timer:%d addr 0x%"PRIx
 grlib_gptimer_writel(int id, uint64_t addr, uint32_t val) "timer:%d addr 0x%"PRIx64" 0x%x"
 
 # hw/grlib_irqmp.c
-grlib_irqmp_check_irqs(uint32_t pend, uint32_t force, uint32_t mask, uint32_t lvl1, uint32_t lvl2) "pend:0x%04x force:0x%04x mask:0x%04x lvl1:0x%04x lvl0:0x%04x"
-grlib_irqmp_ack(int intno) "interrupt:%d"
-grlib_irqmp_set_irq(int irq) "Raise CPU IRQ %d"
+grlib_irqmp_check_irqs(uint32_t pend, uint32_t force, uint32_t mask, uint32_t irq) "pend:0x%04x force:0x%04x mask:0x%04x irq:0x%04x"
+grlib_irqmp_ack(int cpu, int intno) "Acknowledge CPU %d IRQ %d"
+grlib_irqmp_set_irq(int irq) "Raise IRQ %d"
 grlib_irqmp_readl_unknown(uint64_t addr) "addr 0x%"PRIx64
 grlib_irqmp_writel_unknown(uint64_t addr, uint32_t value) "addr 0x%"PRIx64" value 0x%x"
 
@@ -504,8 +504,8 @@ grlib_apbuart_writel_unknown(uint64_t addr, uint32_t value) "addr 0x%"PRIx64" va
 grlib_apbuart_readl_unknown(uint64_t addr) "addr 0x%"PRIx64""
 
 # hw/leon3.c
-leon3_set_irq(int intno) "Set CPU IRQ %d"
-leon3_reset_irq(int intno) "Reset CPU IRQ %d"
+leon3_set_irq(int cpu, int intno) "Set CPU %d IRQ %d"
+leon3_reset_irq(int cpu, int intno) "Reset CPU %d IRQ %d"
 
 # spice-qemu-char.c
 spice_vmc_write(ssize_t out, int len) "spice wrottn %zd of requested %d"
