11 files changed, 545 insertions, 19 deletions

diff --git a/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c b/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c
index 194bc11ed6..6b64bfad77 100644
--- a/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/avic-imx31.c
@@ -43,7 +43,7 @@ static const char * avic_int_names[64] =
     "EXT_SENS1", "EXT_SENS2",     "EXT_WDOG",  "EXT_TV"
 };
 
-static void UIE_VECTOR(void)
+void UIE_VECTOR(void)
 {
     int mode;
     long offset;
diff --git a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
index 051b1c8479..1a41f0c53e 100644
--- a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
@@ -34,13 +34,13 @@ bool _backlight_init(void)
 void _backlight_on(void)
 {
     /* LEDEN=1 */
-    mc13783_set(MC13783_LED_CONTROL0, (1 << 0));
+    mc13783_set(MC13783_LED_CONTROL0, MC13783_LEDEN);
 }
 
 void _backlight_off(void)
 {
     /* LEDEN=0 */
-    mc13783_clear(MC13783_LED_CONTROL0, (1 << 0));
+    mc13783_clear(MC13783_LED_CONTROL0, MC13783_LEDEN);
 }
 
 /* Assumes that the backlight has been initialized */
diff --git a/firmware/target/arm/imx31/gigabeat-s/button-imx31.c b/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
index 602f41abb9..c85fe5e37e 100644
--- a/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
@@ -29,7 +29,8 @@
 /* Most code in here is taken from the Linux BSP provided by Freescale
  * Copyright 2004-2006 Freescale Semiconductor, Inc. All Rights Reserved. */
 
-static uint32_t int_btn = BUTTON_NONE;
+static bool headphones_detect = false;
+static uint32_t int_btn     = BUTTON_NONE;
 static bool hold_button     = false;
 static bool hold_button_old = false;
 #define _button_hold() (GPIO3_DR & 0x10)
@@ -48,7 +49,8 @@ static __attribute__((interrupt("IRQ"))) void KPP_HANDLER(void)
     unsigned short reg_val;
     int col, row;
     int i;
-    int button = BUTTON_NONE;
+    /* Power button is handled separately on PMIC */
+    int button = int_btn & BUTTON_POWER;
 
     /* 1. Disable both (depress and release) keypad interrupts. */
     KPP_KPSR &= ~(KPP_KPSR_KRIE | KPP_KPSR_KDIE);
@@ -161,3 +163,35 @@ int button_read_device(void)
     /* If hold, ignore any pressed button */
     return hold_button ? BUTTON_NONE : int_btn;
 }
+
+/* This is called from the mc13783 interrupt thread */
+void button_power_set_state(bool pressed)
+{
+    /* Prevent KPP_HANDLER from changing things */
+    int oldlevel = disable_irq_save();
+
+    if (pressed)
+    {
+        int_btn |= BUTTON_POWER;
+    }
+    else
+    {
+        int_btn &= ~BUTTON_POWER;
+    }
+
+    restore_irq(oldlevel);
+}
+
+/* This is called from the mc13783 interrupt thread */
+void set_headphones_inserted(bool inserted)
+{
+    headphones_detect = inserted;
+}
+
+/* This is called from the mc13783 interrupt thread */
+/* TODO: Just do a post to the button queue directly - implement the
+ * appropriate variant in the driver. */
+bool headphones_inserted(void)
+{
+    return headphones_detect;
+}
diff --git a/firmware/target/arm/imx31/gigabeat-s/button-target.h b/firmware/target/arm/imx31/gigabeat-s/button-target.h
index 33f018e0bf..836a4c02cd 100644
--- a/firmware/target/arm/imx31/gigabeat-s/button-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/button-target.h
@@ -27,6 +27,9 @@
 bool button_hold(void);
 void button_init_device(void);
 int button_read_device(void);
+void button_power_set_state(bool pressed);
+void set_headphones_inserted(bool inserted);
+bool headphones_inserted(void);
 
 /* Toshiba Gigabeat specific button codes */
 
diff --git a/firmware/target/arm/imx31/gigabeat-s/gpio-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/gpio-gigabeat-s.c
new file mode 100644
index 0000000000..cfbb7fcc4c
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/gpio-gigabeat-s.c
@@ -0,0 +1,39 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (c) 2008 by Michael Sevakis
+ *
+ * Gigabeat S GPIO interrupt event descriptions
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "config.h"
+#include "system.h"
+#include "gpio-imx31.h"
+
+extern int mc13783_event(void);
+
+static const struct gpio_event gpio1_events =
+{
+    .line     = MC13783_GPIO_LINE,
+    .sense    = GPIO_SENSE_RISING,
+    .callback = mc13783_event,
+};
+
+const struct gpio_event_list gpio1_event_list =
+{
+    .priority = 7,
+    .count    = 1,
+    .events   = &gpio1_events,
+};
diff --git a/firmware/target/arm/imx31/gigabeat-s/gpio-imx31.c b/firmware/target/arm/imx31/gigabeat-s/gpio-imx31.c
new file mode 100644
index 0000000000..a7427f16c0
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/gpio-imx31.c
@@ -0,0 +1,207 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (c) 2008 by Michael Sevakis
+ *
+ * IMX31 GPIO event manager
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "config.h"
+#include "system.h"
+#include "avic-imx31.h"
+#include "gpio-imx31.h"
+
+/* UIE vector found in avic-imx31.c */
+extern void UIE_VECTOR(void);
+
+/* Event lists are allocated for the specific target */
+#if (GPIO_EVENT_MASK & USE_GPIO1_EVENTS)
+static __attribute__((interrupt("IRQ"))) void GPIO1_HANDLER(void);
+extern const struct gpio_event_list gpio1_event_list;
+#endif
+
+#if (GPIO_EVENT_MASK & USE_GPIO2_EVENTS)
+static __attribute__((interrupt("IRQ"))) void GPIO2_HANDLER(void);
+extern const struct gpio_event_list gpio2_event_list;
+#endif
+
+#if (GPIO_EVENT_MASK & USE_GPIO3_EVENTS)
+static __attribute__((interrupt("IRQ"))) void GPIO3_HANDLER(void);
+extern const struct gpio_event_list gpio3_event_list;
+#endif
+
+static struct gpio_module_descriptor
+{
+    volatile unsigned long *base;       /* Module base address */
+    enum IMX31_INT_LIST ints;           /* AVIC int number */
+    void (*handler)(void);              /* Interrupt function */
+    const struct gpio_event_list *list; /* Event handler list */
+} gpio_descs[GPIO_NUM_GPIO] =
+{
+#if (GPIO_EVENT_MASK & USE_GPIO1_EVENTS)
+    {
+        .base    = (unsigned long *)GPIO1_BASE_ADDR,
+        .ints    = GPIO1,
+        .handler = GPIO1_HANDLER,
+    },
+#endif
+#if (GPIO_EVENT_MASK & USE_GPIO2_EVENTS)
+    {
+        .base    = (unsigned long *)GPIO2_BASE_ADDR,
+        .ints    = GPIO2,
+        .handler = GPIO2_HANDLER,
+    },
+#endif
+#if (GPIO_EVENT_MASK & USE_GPIO3_EVENTS)
+    {
+        .base    = (unsigned long *)GPIO3_BASE_ADDR,
+        .ints    = GPIO3,
+        .handler = GPIO3_HANDLER,
+    },
+#endif
+};
+
+static void gpio_call_events(enum gpio_module_number gpio)
+{
+    const struct gpio_module_descriptor * const desc = &gpio_descs[gpio];
+    const struct gpio_event_list * const list = desc->list;
+    volatile unsigned long * const base = desc->base;
+    unsigned i;
+
+    /* Intersect pending and unmasked bits */
+    unsigned long pending = base[GPIO_ISR_I] & base[GPIO_IMR_I];
+
+    /* Call each event handler in order */
+    for (i = 0; i < list->count; i++)
+    {
+        const struct gpio_event * const event = &list->events[i];
+        unsigned long bit = 1ul << event->line;
+
+        if ((pending & bit) && event->callback())
+            pending &= ~bit;
+    }
+
+    if (pending != 0)
+    {
+        /* Wasn't handled */
+        UIE_VECTOR();
+    }
+}
+
+#if (GPIO_EVENT_MASK & USE_GPIO1_EVENTS)
+static __attribute__((interrupt("IRQ"))) void GPIO1_HANDLER(void)
+{
+    gpio_call_events(GPIO1_NUM);
+}
+#endif
+
+#if (GPIO_EVENT_MASK & USE_GPIO2_EVENTS)
+static __attribute__((interrupt("IRQ"))) void GPIO2_HANDLER(void)
+{
+    gpio_call_events(GPIO2_NUM);
+}
+#endif
+
+#if (GPIO_EVENT_MASK & USE_GPIO3_EVENTS)
+static __attribute__((interrupt("IRQ"))) void GPIO3_HANDLER(void)
+{
+    gpio_call_events(GPIO3_NUM);
+}
+#endif
+
+void gpio_init(void)
+{
+    /* Mask-out GPIO interrupts - enable what's wanted later */
+    GPIO1_IMR = 0;
+    GPIO2_IMR = 0;
+    GPIO3_IMR = 0;
+
+    /* Init the externally-defined event lists for each port */
+#if (GPIO_EVENT_MASK & USE_GPIO1_EVENTS)
+    gpio_descs[GPIO1_NUM].list = &gpio1_event_list;
+#endif
+#if (GPIO_EVENT_MASK & USE_GPIO2_EVENTS)
+    gpio_descs[GPIO2_NUM].list = &gpio2_event_list;
+#endif
+#if (GPIO_EVENT_MASK & USE_GPIO3_EVENTS)
+    gpio_descs[GPIO3_NUM].list = &gpio3_event_list;
+#endif
+}
+
+bool gpio_enable_event(enum gpio_module_number gpio, unsigned id)
+{
+    const struct gpio_module_descriptor * const desc = &gpio_descs[gpio];
+    const struct gpio_event * const event = &desc->list->events[id];
+    volatile unsigned long * const base = desc->base;
+    volatile unsigned long * icr;
+    unsigned long mask;
+    unsigned long imr;
+    int shift;
+
+    if (id >= desc->list->count)
+        return false;
+
+    int oldlevel = disable_irq_save();
+
+    imr =  base[GPIO_IMR_I];
+
+    if (imr == 0)
+    {
+        /* First enabled interrupt for this GPIO */
+        avic_enable_int(desc->ints, IRQ, desc->list->priority,
+                        desc->handler);
+    }
+
+    /* Set the line sense */
+    icr = &base[GPIO_ICR1_I] + event->line / 16;
+    shift = 2*(event->line % 16);
+    mask = GPIO_SENSE_CONFIG_MASK << shift;
+
+    *icr = (*icr & ~mask) | ((event->sense << shift) & mask);
+
+    /* Unmask the line */
+    base[GPIO_IMR_I] = imr | (1ul << event->line);
+
+    restore_irq(oldlevel);
+
+    return true;
+}
+
+void gpio_disable_event(enum gpio_module_number gpio, unsigned id)
+{
+    const struct gpio_module_descriptor * const desc = &gpio_descs[gpio];
+    const struct gpio_event * const event = &desc->list->events[id];
+    volatile unsigned long * const base = desc->base;
+    unsigned long imr;
+
+    if (id >= desc->list->count)
+        return;
+
+    int oldlevel = disable_irq_save();
+
+    /* Remove bit from mask */
+    imr = base[GPIO_IMR_I] & ~(1ul << event->line);
+
+    /* Mask the line */
+    base[GPIO_IMR_I] = imr;
+
+    if (imr == 0)
+    {
+        /* No events remain enabled */
+        avic_disable_int(desc->ints);
+    }
+
+    restore_irq(oldlevel);
+}
diff --git a/firmware/target/arm/imx31/gigabeat-s/gpio-imx31.h b/firmware/target/arm/imx31/gigabeat-s/gpio-imx31.h
new file mode 100644
index 0000000000..a197558ad4
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/gpio-imx31.h
@@ -0,0 +1,77 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (c) 2008 by Michael Sevakis
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef _GPIO_IMX31_H_
+#define _GPIO_IMX31_H_
+
+#include "gpio-target.h"
+
+#define USE_GPIO1_EVENTS (1 << 0)
+#define USE_GPIO2_EVENTS (1 << 1)
+#define USE_GPIO3_EVENTS (1 << 2)
+
+enum gpio_module_number
+{
+    __GPIO_NUM_START = -1,
+#if (GPIO_EVENT_MASK & USE_GPIO1_EVENTS)
+    GPIO1_NUM,
+#endif
+#if (GPIO_EVENT_MASK & USE_GPIO2_EVENTS)
+    GPIO2_NUM,
+#endif
+#if (GPIO_EVENT_MASK & USE_GPIO3_EVENTS)
+    GPIO3_NUM,
+#endif
+    GPIO_NUM_GPIO,
+};
+
+/* Possible values for gpio interrupt line config */
+enum gpio_int_sense_enum
+{
+    GPIO_SENSE_LOW_LEVEL = 0, /* High-level sensitive */
+    GPIO_SENSE_HIGH_LEVEL,    /* Low-level sensitive */
+    GPIO_SENSE_RISING,        /* Rising-edge sensitive */
+    GPIO_SENSE_FALLING,       /* Falling-edge sensitive */
+};
+
+#define GPIO_SENSE_CONFIG_MASK 0x3
+
+/* Pending events will be called in array order */
+
+/* Describes a single event for a pin */
+struct gpio_event
+{
+    int line;                       /* Line number (0-31) */
+    enum gpio_int_sense_enum sense; /* Type of sense */
+    int (*callback)(void);          /* Callback function (return nonzero
+                                     * to indicate this event was handled) */
+};
+
+/* Describes the events attached to a port */
+struct gpio_event_list
+{
+    int priority;                    /* Interrupt priority for this GPIO */
+    unsigned count;                  /* Count of events */
+    const struct gpio_event *events; /* List of events */
+};
+
+void gpio_init(void);
+bool gpio_enable_event(enum gpio_module_number gpio, unsigned id);
+void gpio_disable_event(enum gpio_module_number gpio, unsigned id);
+
+#endif /* _GPIO_IMX31_H_ */
diff --git a/firmware/target/arm/imx31/gigabeat-s/gpio-target.h b/firmware/target/arm/imx31/gigabeat-s/gpio-target.h
new file mode 100644
index 0000000000..46e43af28d
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/gpio-target.h
@@ -0,0 +1,31 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (c) 2008 by Michael Sevakis
+ *
+ * Gigabeat S GPIO interrupt event descriptions header
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef GPIO_TARGET_H
+#define GPIO_TARGET_H
+
+#define GPIO_EVENT_MASK (USE_GPIO1_EVENTS)
+
+#define MC13783_GPIO_NUM    GPIO1_NUM   
+#define MC13783_GPIO_ISR    GPIO1_ISR
+#define MC13783_GPIO_LINE   31
+#define MC13783_EVENT_ID    0
+
+#endif /* GPIO_TARGET_H */
diff --git a/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
index 3af8f35f69..a04cd7f893 100644
--- a/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
@@ -19,10 +19,13 @@
 #include "system.h"
 #include "cpu.h"
 #include "spi-imx31.h"
+#include "gpio-imx31.h"
 #include "mc13783.h"
 #include "debug.h"
 #include "kernel.h"
 
+#include "button-target.h"
+
 /* This is all based on communicating with the MC13783 PMU which is on 
  * CSPI2 with the chip select at 0. The LCD controller resides on
  * CSPI3 cs1, but we have no idea how to communicate to it */
@@ -43,17 +46,68 @@ static int mc13783_thread_stack[DEFAULT_STACK_SIZE/sizeof(int)];
 static const char *mc13783_thread_name = "pmic";
 static struct wakeup mc13783_wake;
 
+/* The next two functions are rather target-specific but they'll just be left
+ * here for the moment */
 static __attribute__((noreturn)) void mc13783_interrupt_thread(void)
 {
+    const unsigned char status_regs[2] =
+    {
+        MC13783_INTERRUPT_STATUS0,
+        MC13783_INTERRUPT_STATUS1,
+    };
+    uint32_t pending[2];
+    uint32_t value;
+
+    mc13783_read_regset(status_regs, pending, 2);
+    mc13783_write_regset(status_regs, pending, 2);
+
+    gpio_enable_event(MC13783_GPIO_NUM, MC13783_EVENT_ID);
+
+    /* Check initial states */
+    value = mc13783_read(MC13783_INTERRUPT_SENSE1);
+    button_power_set_state((value & MC13783_ON1B) == 0);
+    set_headphones_inserted((value & MC13783_ON2B) == 0);
+
+    /* Enable desired PMIC interrupts */
+    mc13783_clear(MC13783_INTERRUPT_MASK1, MC13783_ON1B | MC13783_ON2B);
+    
     while (1)
     {
         wakeup_wait(&mc13783_wake, TIMEOUT_BLOCK);
+
+        mc13783_read_regset(status_regs, pending, 2);
+        mc13783_write_regset(status_regs, pending, 2);
+
+#if 0
+        if (pending[0])
+        {
+            /* Handle ...PENDING0 */
+        }
+#endif
+
+        if (pending[1])
+        {
+            /* Handle ...PENDING1 */
+            if (pending[1] & (MC13783_ON1B | MC13783_ON2B))
+            {
+                value = mc13783_read(MC13783_INTERRUPT_SENSE1);
+
+                if (pending[1] & MC13783_ON1B)
+                    button_power_set_state((value & MC13783_ON1B) == 0);
+
+                if (pending[1] & MC13783_ON2B)
+                    set_headphones_inserted((value & MC13783_ON2B) == 0);
+            }
+        }
     }
 }
 
-static __attribute__((interrupt("IRQ"))) void mc13783_interrupt(void)
+/* GPIO interrupt handler for mc13783 */
+int mc13783_event(void)
 {
+    MC13783_GPIO_ISR = (1ul << MC13783_GPIO_LINE);
     wakeup_signal(&mc13783_wake);
+    return 1; /* Yes, it's handled */
 }
 
 void mc13783_init(void)
@@ -64,25 +118,44 @@ void mc13783_init(void)
     /* Enable the PMIC SPI module */
     spi_enable_module(&mc13783_spi);
 
+    /* Mask any PMIC interrupts for now - poll initial status in thread
+     * and enable them there */
+    mc13783_write(MC13783_INTERRUPT_MASK0, 0xffffff);
+    mc13783_write(MC13783_INTERRUPT_MASK1, 0xffffff);
+
+    MC13783_GPIO_ISR = (1ul << MC13783_GPIO_LINE);
+
     create_thread(mc13783_interrupt_thread, mc13783_thread_stack,
                   sizeof(mc13783_thread_stack), 0, mc13783_thread_name
                   IF_PRIO(, PRIORITY_REALTIME) IF_COP(, CPU));
 }
 
-void mc13783_set(unsigned address, uint32_t bits)
+uint32_t mc13783_set(unsigned address, uint32_t bits)
 {
     spi_lock(&mc13783_spi);
+
     uint32_t data = mc13783_read(address);
-    mc13783_write(address, data | bits);
+
+    if (data != (uint32_t)-1)
+        mc13783_write(address, data | bits);
+
     spi_unlock(&mc13783_spi);
+
+    return data;
 }
 
-void mc13783_clear(unsigned address, uint32_t bits)
+uint32_t mc13783_clear(unsigned address, uint32_t bits)
 {
     spi_lock(&mc13783_spi);
+
     uint32_t data = mc13783_read(address);
-    mc13783_write(address, data & ~bits);
+
+    if (data != (uint32_t)-1)
+        mc13783_write(address, data & ~bits);
+
     spi_unlock(&mc13783_spi);
+
+    return data;
 }
 
 int mc13783_write(unsigned address, uint32_t data)
@@ -108,7 +181,7 @@ int mc13783_write_multiple(unsigned start, const uint32_t *data, int count)
 {
     int i;
     struct spi_transfer xfer;
-    uint32_t packets[64];
+    uint32_t packets[MC13783_NUM_REGS];
 
     if (start + count > MC13783_NUM_REGS)
         return -1;
@@ -129,6 +202,36 @@ int mc13783_write_multiple(unsigned start, const uint32_t *data, int count)
     return count - xfer.count;
 }
 
+int mc13783_write_regset(const unsigned char *regs, const uint32_t *data,
+                         int count)
+{
+    int i;
+    struct spi_transfer xfer;
+    uint32_t packets[MC13783_NUM_REGS];
+
+    if (count > MC13783_NUM_REGS)
+        return -1;
+
+    for (i = 0; i < count; i++)
+    {
+        uint32_t reg = regs[i];
+
+        if (reg >= MC13783_NUM_REGS)
+            return -1;
+
+        packets[i] = (1 << 31) | (reg << 25) | (data[i] & 0xffffff);
+    }
+
+    xfer.txbuf = packets;
+    xfer.rxbuf = packets;
+    xfer.count = count;
+
+    if (!spi_transfer(&mc13783_spi, &xfer))
+        return -1;
+
+    return count - xfer.count;
+}
+
 uint32_t mc13783_read(unsigned address)
 {
     uint32_t packet;
@@ -146,25 +249,53 @@ uint32_t mc13783_read(unsigned address)
     if (!spi_transfer(&mc13783_spi, &xfer))
         return (uint32_t)-1;
 
-    return packet & 0xffffff;
+    return packet;
 }
 
 int mc13783_read_multiple(unsigned start, uint32_t *buffer, int count)
 {
     int i;
-    uint32_t packets[64];
     struct spi_transfer xfer;
 
     if (start + count > MC13783_NUM_REGS)
         return -1;
 
-    xfer.txbuf = packets;
+    xfer.txbuf = buffer;
     xfer.rxbuf = buffer;
     xfer.count = count;
 
     /* Prepare TX payload */
     for (i = 0; i < count; i++, start++)
-        packets[i] = start << 25;
+        buffer[i] = start << 25;
+
+    if (!spi_transfer(&mc13783_spi, &xfer))
+        return -1;
+
+    return count - xfer.count;
+}
+
+int mc13783_read_regset(const unsigned char *regs, uint32_t *buffer,
+                        int count)
+{
+    int i;
+    struct spi_transfer xfer;
+
+    if (count > MC13783_NUM_REGS)
+        return -1;
+
+    for (i = 0; i < count; i++)
+    {
+        unsigned reg = regs[i];
+
+        if (reg >= MC13783_NUM_REGS)
+            return -1;
+
+        buffer[i] = reg << 25;
+    }
+
+    xfer.txbuf = buffer;
+    xfer.rxbuf = buffer;
+    xfer.count = count;
 
     if (!spi_transfer(&mc13783_spi, &xfer))
         return -1;
diff --git a/firmware/target/arm/imx31/gigabeat-s/power-imx31.c b/firmware/target/arm/imx31/gigabeat-s/power-imx31.c
index c739a19cba..f57c55a70b 100644
--- a/firmware/target/arm/imx31/gigabeat-s/power-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/power-imx31.c
@@ -17,14 +17,12 @@
  *
  ****************************************************************************/
 #include "config.h"
-#include "cpu.h"
-#include <stdbool.h>
-#include "kernel.h"
 #include "system.h"
 #include "power.h"
-#include "pcf50606.h"
 #include "backlight.h"
 #include "backlight-target.h"
+#include "avic-imx31.h"
+#include "mc13783.h"
 
 #ifndef SIMULATOR
 
@@ -54,6 +52,10 @@ bool ide_powered(void)
 
 void power_off(void)
 {
+    mc13783_set(MC13783_POWER_CONTROL0, MC13783_USEROFFSPI);
+
+    disable_interrupt(IRQ_FIQ_STATUS);
+    while (1);
 }
 
 #else /* SIMULATOR */
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
index bd7999558b..da5026a292 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
@@ -2,6 +2,7 @@
 #include "system.h"
 #include "panic.h"
 #include "avic-imx31.h"
+#include "gpio-imx31.h"
 #include "mmu-imx31.h"
 #include "system-target.h"
 #include "lcd.h"
@@ -23,6 +24,7 @@ void system_init(void)
     /* MCR WFI enables wait mode */
     CLKCTL_CCMR &= ~(3 << 14);
     avic_init();
+    gpio_init();
 }
 
 #ifdef BOOTLOADER