13 files changed, 641 insertions, 156 deletions

diff --git a/apps/debug_menu.c b/apps/debug_menu.c
index d35f40229e..000dbdc4fc 100644
--- a/apps/debug_menu.c
+++ b/apps/debug_menu.c
@@ -796,6 +796,12 @@ static bool dbg_cpufreq(void)
         lcd_putsf(0, line++, "Frequency: %ld.%ld MHz", temp, (FREQ-temp*1000000)/100000);
         lcd_putsf(0, line++, "boost_counter: %d", get_cpu_boost_counter());
 
+#ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
+        extern int get_cpu_voltage_setting(void);
+        temp = get_cpu_voltage_setting();
+        lcd_putsf(0, line++, "CPU voltage: %d.%03dV", temp / 1000, temp % 1000);
+#endif
+
         lcd_update();
         button = get_action(CONTEXT_STD,HZ/10);
 
diff --git a/firmware/drivers/adc-as3514.c b/firmware/drivers/adc-as3514.c
index 3b411a379d..9a81a52cc7 100644
--- a/firmware/drivers/adc-as3514.c
+++ b/firmware/drivers/adc-as3514.c
@@ -26,18 +26,35 @@
 /* Read 10-bit channel data */
 unsigned short adc_read(int channel)
 {
-    unsigned char buf[2];
+    unsigned short data = 0;
+
+    if ((unsigned)channel >= NUM_ADC_CHANNELS)
+        return 0;
 
     ascodec_lock();
 
     /* Select channel */
     ascodec_write(AS3514_ADC_0, (channel << 4));
+    /*
+     * The AS3514 ADC will trigger an interrupt when the conversion
+     * is finished, if the corresponding enable bit in IRQ_ENRD2
+     * is set.
+     * Previously the code did not wait and this apparently did
+     * not pose any problems, but this should be more correct.
+     * Without the wait the data read back may be completely or
+     * partially (first one of the two bytes) stale.
+     */
+    ascodec_wait_adc_finished();
+
 
+    /* Read data */
+    unsigned char buf[2] = { 0, 0 };
     ascodec_readbytes(AS3514_ADC_0, 2, buf);
+    data = (((buf[0] & 0x3) << 8) | buf[1]);
 
     ascodec_unlock();
 
-    return (((buf[0] & 0x3) << 8) | buf[1]);
+    return data;
 }
 
 void adc_init(void)
diff --git a/firmware/export/ascodec.h b/firmware/export/ascodec.h
index c2ff3c8242..b9c58c08ba 100644
--- a/firmware/export/ascodec.h
+++ b/firmware/export/ascodec.h
@@ -43,6 +43,8 @@ int ascodec_read(unsigned int index);
 
 void ascodec_readbytes(unsigned int index, unsigned int len, unsigned char *data);
 
+void ascodec_wait_adc_finished(void);
+
 #if CONFIG_CHARGING
 bool ascodec_endofch(void);
 bool ascodec_chg_status(void);
diff --git a/firmware/export/system.h b/firmware/export/system.h
index 49249f6bb5..050c3074aa 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -51,10 +51,6 @@ bool detect_original_firmware(void);
 #endif
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
-#if NUM_CORES > 1
-extern struct spinlock boostctrl_spin;
-#endif
-void cpu_boost_init(void);
 #define FREQ cpu_frequency
 void set_cpu_frequency(long frequency);
 #ifdef CPU_BOOST_LOGGING
@@ -214,6 +210,17 @@ enum {
 #define CPU_MODE_THREAD_CONTEXT 0
 #endif
 
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#ifndef CPU_BOOST_LOCK_DEFINED
+#define CPU_BOOST_LOCK_DEFINED
+/* Compatibility defauls */
+static inline bool cpu_boost_lock(void)
+    { return true; }
+static inline void cpu_boost_unlock(void)
+    { }
+#endif /* CPU_BOOST_LOCK */
+#endif /* HAVE_ADJUSTABLE_CPU_FREQ */
+
 #ifndef BIT_N
 #define BIT_N(n) (1U << (n))
 #endif
diff --git a/firmware/panic.c b/firmware/panic.c
index 7b2c79a872..a35291636d 100644
--- a/firmware/panic.c
+++ b/firmware/panic.c
@@ -127,7 +127,13 @@ void panicf( const char *fmt, ...)
     lcd_update();
     DEBUGF("%s", panic_buf);
 
-    set_cpu_frequency(0);
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+    if (cpu_boost_lock())
+    {
+        set_cpu_frequency(0);
+        cpu_boost_unlock();
+    }
+#endif /* HAVE_ADJUSTABLE_CPU_FREQ */
     
 #ifdef HAVE_ATA_POWER_OFF
     ide_power_enable(false);
diff --git a/firmware/system.c b/firmware/system.c
index 37631b8735..537e901b05 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -35,13 +35,6 @@ long cpu_frequency SHAREDBSS_ATTR = CPU_FREQ;
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 static int boost_counter SHAREDBSS_ATTR = 0;
 static bool cpu_idle SHAREDBSS_ATTR = false;
-#if NUM_CORES > 1
-static struct corelock boostctrl_cl SHAREDBSS_ATTR;
-void cpu_boost_init(void)
-{
-    corelock_init(&boostctrl_cl);
-}
-#endif
 
 int get_cpu_boost_counter(void)
 {
@@ -60,7 +53,7 @@ int cpu_boost_log_getcount(void)
 char * cpu_boost_log_getlog_first(void)
 {
     char *first;
-    corelock_lock(&boostctrl_cl);
+    cpu_boost_lock();
 
     first = NULL;
 
@@ -70,7 +63,7 @@ char * cpu_boost_log_getlog_first(void)
         first = cpu_boost_calls[cpu_boost_first];
     }
 
-    corelock_unlock(&boostctrl_cl);
+    cpu_boost_unlock();
     return first;
 }
 
@@ -79,7 +72,7 @@ char * cpu_boost_log_getlog_next(void)
     int message;
     char *next;
 
-    corelock_lock(&boostctrl_cl);
+    cpu_boost_lock();
 
     message = (cpu_boost_track_message+cpu_boost_first)%MAX_BOOST_LOG;
     next = NULL;
@@ -90,13 +83,14 @@ char * cpu_boost_log_getlog_next(void)
         next = cpu_boost_calls[message];
     }
 
-    corelock_unlock(&boostctrl_cl);
+    cpu_boost_unlock();
     return next;
 }
 
 void cpu_boost_(bool on_off, char* location, int line)
 {
-    corelock_lock(&boostctrl_cl);
+    if (!cpu_boost_lock())
+        return;
 
     if (cpu_boost_calls_count == MAX_BOOST_LOG)
     {
@@ -115,7 +109,9 @@ void cpu_boost_(bool on_off, char* location, int line)
 #else
 void cpu_boost(bool on_off)
 {
-    corelock_lock(&boostctrl_cl);
+    if (!cpu_boost_lock())
+        return;
+
 #endif /* CPU_BOOST_LOGGING */
     if(on_off)
     {
@@ -141,12 +137,13 @@ void cpu_boost(bool on_off)
         }
     }
 
-    corelock_unlock(&boostctrl_cl);
+    cpu_boost_unlock();
 }
 
 void cpu_idle_mode(bool on_off)
 {
-    corelock_lock(&boostctrl_cl);
+    if (!cpu_boost_lock())
+        return;
 
     cpu_idle = on_off;
 
@@ -160,7 +157,7 @@ void cpu_idle_mode(bool on_off)
             set_cpu_frequency(CPUFREQ_NORMAL);
     }
 
-    corelock_unlock(&boostctrl_cl);
+    cpu_boost_unlock();
 }
 #endif /* HAVE_ADJUSTABLE_CPU_FREQ */
 
diff --git a/firmware/target/arm/as3525/ascodec-as3525.c b/firmware/target/arm/as3525/ascodec-as3525.c
index ec25a415a5..e144f07ed4 100644
--- a/firmware/target/arm/as3525/ascodec-as3525.c
+++ b/firmware/target/arm/as3525/ascodec-as3525.c
@@ -50,7 +50,7 @@
 #include "system.h"
 #include "as3525.h"
 #include "i2c.h"
-#include "logf.h"
+#include <linked_list.h>
 
 #define I2C2_DATA       *((volatile unsigned int *)(I2C_AUDIO_BASE + 0x00))
 #define I2C2_SLAD0      *((volatile unsigned int *)(I2C_AUDIO_BASE + 0x04))
@@ -78,234 +78,495 @@
 #define I2C2_IRQ_RXOVER       0x10
 #define I2C2_IRQ_ACKTIMEO     0x80
 
+#define REQ_UNFINISHED 0
+#define REQ_FINISHED   1
+#define REQ_RETRY      2
 
-static struct mutex as_mtx;
-
-#if CONFIG_CHARGING
-static bool chg_status = false;
-static bool endofch = false;
+#ifdef DEBUG
+#define IFDEBUG(x) x
+#else
+#define IFDEBUG(x)
 #endif
 
-/* returns true when busy */
-static inline bool i2c_busy(void)
+#define ASCODEC_REQ_READ   0
+#define ASCODEC_REQ_WRITE  1
+
+/*
+ * How many bytes we using in struct ascodec_request for the data buffer.
+ * 4 fits the alignment best right now.
+ * We don't actually use more than 3 at the moment (when reading interrupts)
+ * Upper limit would be 255 since DACNT is 8 bits!
+ */
+#define ASCODEC_REQ_MAXLEN 4
+
+struct ascodec_request;
+typedef void (ascodec_cb_fn)(struct ascodec_request *req);
+
+struct ascodec_request {
+    /* standard request members */
+    struct ll_node node;        /* request list link (first!) */
+    unsigned char type;         /* reqest type (read or write) */
+    unsigned char index;        /* initial i2c sub address */
+    unsigned char cnt;          /* bytes remaining */
+    unsigned char data[ASCODEC_REQ_MAXLEN]; /* actual I/O data */
+
+    /* members relevant when a callback is specified (callback != NULL) */
+    ascodec_cb_fn *callback;    /* pointer to callback function */
+    intptr_t      cbdata;       /* data for callback function */
+    int           len_done;     /* amount actually transferred */
+};
+
+/* I2C driver data */
+static struct mutex as_mtx;
+static struct ll_head req_list;
+static unsigned char *req_data_ptr;
+#define REQ_FIRST ((struct ascodec_request *)req_list.head)
+
+/* INT_AUDIO interrupt data */
+static void ascodec_int_audio_cb(struct ascodec_request *req);
+void INT_I2C_AUDIO(void);
+static struct ascodec_request as_audio_req;
+static struct semaphore adc_done_sem;
+static unsigned long ascodec_enrd0_shadow = 0;
+
+static void ascodec_wait_cb(struct ascodec_request *req);
+
+/** --debugging help-- **/
+
+#ifdef DEBUG
+/* counters for debugging INT_AUDIO */
+static struct int_audio_counters {
+    int int_audio;
+    int int_chg_finished;
+    int int_chg_insert;
+    int int_chg_remove;
+    int int_usb_insert;
+    int int_usb_remove;
+    int int_rtc;
+    int int_adc;
+} int_audio_counters;
+#endif /* DEBUG */
+
+#define COUNT_INT(x) IFDEBUG((int_audio_counters.int_##x)++)
+
+
+/** --stock request and callback functionality -- **/
+
+/* init for common request data (call before submitting) */
+static inline void ascodec_req_init(struct ascodec_request *req, int type,
+                                    unsigned int index, unsigned int cnt)
 {
-    return (I2C2_SR & 1);
+    req->type  = type;
+    req->index = index;
+    req->cnt   = cnt;
 }
 
-void i2c_init(void)
+/* stock no-wait init for request (use any callback and data) */
+static inline void ascodec_async_init(struct ascodec_request *req,
+                                      ascodec_cb_fn *callback, intptr_t cbdata)
 {
+    /* cbdata is unused if no callback is used */
+    if ((req->callback = callback))
+        req->cbdata = cbdata;
 }
 
-static void i2c2_init(void)
+/* initialize the stock completion callback */
+static inline void ascodec_wait_init(struct ascodec_request *req,
+                                     struct semaphore *completep)
 {
-    int prescaler;
-
-    /* prescaler for i2c clock */
-    prescaler = AS3525_I2C_PRESCALER;
-    I2C2_CPSR0 = prescaler & 0xFF;          /* 8 lsb */
-    I2C2_CPSR1 = (prescaler >> 8) & 0x3;    /* 2 msb */
+    req->callback = ascodec_wait_cb;
+    req->cbdata   = (intptr_t)completep;
+    semaphore_init(completep, 1, 0);
+}
 
-    /* set i2c slave address of codec part */
-    I2C2_SLAD0 = AS3514_I2C_ADDR << 1;
+/* caller waits here when using ascodec_wait_cb to do synchronous transfers */
+static void ascodec_wait(struct ascodec_request *req)
+{
+    struct semaphore *completep = (struct semaphore *)req->cbdata;
+    int timeout = TIMEOUT_BLOCK;
 
-    I2C2_CNTRL = I2C2_CNTRL_DEFAULT;
+    if (!irq_enabled() || !is_thread_context()) {
+        timeout = TIMEOUT_NOBLOCK; /* poll semaphore, no block */
+    }
 
+    while (semaphore_wait(completep, timeout) == OBJ_WAIT_TIMEDOUT) {
+        /* pump the i2c interrupts ourselves (only waiting can do this!) */
+        if (I2C2_MIS) {
+            INT_I2C_AUDIO();
+        }
+    }
 }
 
-/* initialises the internal i2c bus and prepares for transfers to the codec */
-void ascodec_init(void)
+/* stock callback used in order to wait for a transfer to complete */
+static void ascodec_wait_cb(struct ascodec_request *req)
 {
+    struct semaphore *completep = (struct semaphore *)req->cbdata;
+    semaphore_release(completep);
+}
 
-    mutex_init(&as_mtx);
+
+/**-- I2C2 interrupt handling --**/
+
+/* start the controller on the next transfer */
+static void ascodec_start_req(struct ascodec_request *req)
+{
+    int unmask = 0;
 
     /* enable clock */
     bitset32(&CGU_PERI, CGU_I2C_AUDIO_MASTER_CLOCK_ENABLE);
 
-    i2c2_init();
+    /* start transfer */
+    I2C2_SADDR = req->index;
+
+    if (req->type == ASCODEC_REQ_READ) {
+        req_data_ptr = req->data;
+        I2C2_CNTRL = I2C2_CNTRL_DEFAULT | I2C2_CNTRL_READ;
+        unmask = I2C2_IRQ_RXFULL|I2C2_IRQ_RXOVER;
+    } else {
+        req_data_ptr = &req->data[1];
+        I2C2_CNTRL = I2C2_CNTRL_DEFAULT | I2C2_CNTRL_WRITE;
+        I2C2_DATA = req->data[0];
+        unmask = I2C2_IRQ_TXEMPTY|I2C2_IRQ_ACKTIMEO;
+    }
 
-    /* Generate irq for usb+charge status change */
-    ascodec_write(AS3514_IRQ_ENRD0,
-#if CONFIG_CHARGING /* m200v4 can't charge */
-        IRQ_CHGSTAT | IRQ_ENDOFCH |
-#endif
-        IRQ_USBSTAT);
+    I2C2_DACNT = req->cnt;
+    I2C2_IMR |= unmask; /* enable interrupts */
+}
 
-#if CONFIG_CPU == AS3525v2
-    /* XIRQ = IRQ, active low reset signal, 6mA push-pull output */
-    ascodec_write_pmu(0x1a, 3, (1<<2)|3); /* 1A-3 = Out_Cntr3 register */
-    /* reset for compatible with old bootloader */
-    ascodec_write(AS3514_IRQ_ENRD2, 0x0);
-#else
-    /* Generate irq for push-pull, active high, irq on rtc+adc change */
-    ascodec_write(AS3514_IRQ_ENRD2, IRQ_PUSHPULL | IRQ_HIGHACTIVE);
-#endif
+/* send the next bytes or read bytes received */
+static int ascodec_continue_req(struct ascodec_request *req, int irq_status)
+{
+    if ((irq_status & (I2C2_IRQ_RXOVER|I2C2_IRQ_ACKTIMEO)) > 0) {
+        /* some error occured, restart the request */
+        return REQ_RETRY;
+    }
 
-    VIC_INT_ENABLE = INTERRUPT_AUDIO;
+    if (req->type == ASCODEC_REQ_READ &&
+        (irq_status & I2C2_IRQ_RXFULL) > 0) {
+        *req_data_ptr++ = I2C2_DATA;
+    } else {
+        if (req->cnt > 1 &&
+            (irq_status & I2C2_IRQ_TXEMPTY) > 0) {
+            I2C2_DATA = *req_data_ptr++;
+        }
+    }
 
-    /* detect if USB was connected at startup since there is no transition */
-    int data = ascodec_read(AS3514_IRQ_ENRD0);
+    req->index++;
+    if (--req->cnt > 0)
+        return REQ_UNFINISHED;
 
-    if(data & USB_STATUS)
-        usb_insert_int();
+    return REQ_FINISHED;
+}
 
-#if CONFIG_CHARGING
-    chg_status = data & CHG_STATUS;
-#endif
+/* complete the request and call the completion callback, if any */
+static void ascodec_finish_req(struct ascodec_request *req)
+{
+    /*
+     * Wait if still busy, unfortunately this happens since
+     * the controller is running at a low divisor, so it's
+     * still busy when we serviced the interrupt.
+     * I tried upping the i2c speed to 4MHz which
+     * made the number of busywait cycles much smaller
+     * (none for reads and only a few for writes),
+     * but who knows if it's reliable at that frequency. ;)
+     * For one thing, 8MHz doesn't work, so 4MHz is likely
+     * borderline.
+     * In general writes need much more wait cycles than reads
+     * for some reason, possibly because we read the data register
+     * for reads, which will likely block the processor while
+     * the i2c controller responds to the register read.
+     */
+    while (I2C2_SR & 1);
+
+    if (req->callback) {
+        req->len_done = req_data_ptr - req->data;
+        req->callback(req);
+    }
 }
 
-/* returns false if transfer incomplete */
-static bool i2c2_transfer(void)
+/* ISR for I2C2 */
+void INT_I2C_AUDIO(void)
 {
-    static int try = 0;
+    struct ascodec_request *req = REQ_FIRST;
 
-    /* wait for transfer*/
-    int i = 10000;
-    while (I2C2_DACNT != 0 && i--);
+    int irq_status = I2C2_MIS;
+    int status = ascodec_continue_req(req, irq_status);
 
-    if (!i) {
-        if (try == 5)
-            panicf("I2C2 reset failed");
+    I2C2_INT_CLR = irq_status; /* clear interrupt status */
 
-        logf("reset I2C2 %d", try);
+    if (status != REQ_UNFINISHED) {
+        /* mask rx/tx interrupts */
+        I2C2_IMR &= ~(I2C2_IRQ_TXEMPTY|I2C2_IRQ_RXFULL|
+                      I2C2_IRQ_RXOVER|I2C2_IRQ_ACKTIMEO);
 
-        i2c2_init();
+        if (status == REQ_FINISHED)
+            ascodec_finish_req(req);
 
-        try++;
-        return false;
-    }
+        int oldlevel = disable_irq_save(); /* IRQs are stacked */
+
+        /*
+         * If status == REQ_RETRY, this will restart the request from where
+         * it left off because we didn't remove it from the request list
+         */
 
-    try = 0;
-    return true;
+        if (status == REQ_FINISHED) {
+            ll_remove_next(&req_list, NULL);
+        }
+
+        req = REQ_FIRST;
+        if (req == NULL) {
+            /* disable clock */
+            bitclr32(&CGU_PERI, CGU_I2C_AUDIO_MASTER_CLOCK_ENABLE);
+        } else {
+            ascodec_start_req(req);
+        }
+
+        restore_irq(oldlevel);
+    }
 }
 
-void ascodec_write(unsigned int index, unsigned int value)
+
+/** --Routines for reading and writing data on the bus-- **/
+
+/* add the request to the queue */
+static void ascodec_submit(struct ascodec_request *req)
 {
-    ascodec_lock();
+    int oldlevel = disable_irq_save();
+
+    ll_insert_last(&req_list, &req->node);
+
+    if (REQ_FIRST == req) {
+        /* first on list? start driver */
+        ascodec_start_req(req);
+    }
+
+    restore_irq(oldlevel);
+}
 
+/*
+ * The request struct passed in must be allocated statically.
+ * If you call ascodec_async_write from different places, each
+ * call needs it's own request struct.
+ */
+static void ascodec_async_write(struct ascodec_request *req,
+                                unsigned int index, unsigned int value)
+{
 #ifndef HAVE_AS3543
     if (index == AS3514_CVDD_DCDC3) /* prevent setting of the LREG_CP_not bit */
         value &= ~(1 << 5);
 #endif
 
-    do {
-        /* wait if still busy */
-        while (i2c_busy());
+    ascodec_req_init(req, ASCODEC_REQ_WRITE, index, 1);
+    req->data[0] = value;
+    ascodec_submit(req);
+}
+
+void ascodec_write(unsigned int index, unsigned int value)
+{
+    struct ascodec_request req;
+    struct semaphore complete;
 
-        /* start transfer */
-        I2C2_SADDR = index;
-        I2C2_CNTRL &= ~(1 << 1);
-        I2C2_DATA = value;
-        I2C2_DACNT = 1;
+    ascodec_wait_init(&req, &complete);
+    ascodec_async_write(&req, index, value);
+    ascodec_wait(&req);
+}
 
-    } while (!i2c2_transfer());
+/*
+ * The request struct passed in must be allocated statically.
+ * If you call ascodec_async_read from different places, each
+ * call needs it's own request struct.
+ * If len is bigger than ASCODEC_REQ_MAXLEN it will be
+ * set to ASCODEC_REQ_MAXLEN.
+ */
+static void ascodec_async_read(struct ascodec_request *req,
+                               unsigned int index, unsigned int len)
+{
+    if (len > ASCODEC_REQ_MAXLEN)
+        len = ASCODEC_REQ_MAXLEN; /* can't fit more in one request */
 
-    ascodec_unlock();
+    ascodec_req_init(req, ASCODEC_REQ_READ, index, len);
+    ascodec_submit(req);
 }
 
+/* read data synchronously  */
 int ascodec_read(unsigned int index)
 {
-    int data;
+    struct ascodec_request req;
+    struct semaphore complete;
 
-    ascodec_lock();
+    ascodec_wait_init(&req, &complete);
+    ascodec_async_read(&req, index, 1);
+    ascodec_wait(&req);
 
-    do {
-        /* wait if still busy */
-        while (i2c_busy());
+    return req.data[0];
+}
 
-        /* start transfer */
-        I2C2_SADDR = index;
-        I2C2_CNTRL |= (1 << 1);
-        I2C2_DACNT = 1;
+/* read an array of bytes */
+void ascodec_readbytes(unsigned int index, unsigned int len, unsigned char *data)
+{
+    struct ascodec_request req;
+    struct semaphore complete;
 
-    } while (!i2c2_transfer());
+    ascodec_wait_init(&req, &complete);
 
-    data = I2C2_DATA;
+    /* index and cnt will be filled in later, just use 0 */
+    ascodec_req_init(&req, ASCODEC_REQ_READ, 0, 0);
 
-    ascodec_unlock();
+    int i = 0;
 
-    return data;
-}
+    while (len > 0) {
+        int cnt = MIN(len, ASCODEC_REQ_MAXLEN);
 
-void ascodec_readbytes(unsigned int index, unsigned int len, unsigned char *data)
-{
-    unsigned int i;
+        req.index = index;
+        req.cnt = cnt;
+
+        ascodec_submit(&req);
+        ascodec_wait(&req);
+
+        for (int j = 0; j < cnt; j++)
+            data[i++] = req.data[j];
 
-    for(i = 0; i < len; i++)
-        data[i] = ascodec_read(index+i);
+        len   -= cnt;
+        index += cnt;
+    }
 }
 
 #if CONFIG_CPU == AS3525v2
+/* write special PMU subregisters */
 void ascodec_write_pmu(unsigned int index, unsigned int subreg,
                        unsigned int value)
 {
-    ascodec_lock();
+    struct ascodec_request reqs[2];
+    struct semaphore complete;
+
+    ascodec_async_init(&reqs[0], NULL, 0);
+    ascodec_wait_init(&reqs[1], &complete);
+
+    int oldstatus = disable_irq_save();
     /* we submit consecutive requests to make sure no operations happen on the
      * i2c bus between selecting the sub register and writing to it */
-    ascodec_write(AS3543_PMU_ENABLE, 8 | subreg);
-    ascodec_write(index, value);
+    ascodec_async_write(&reqs[0], AS3543_PMU_ENABLE, 8 | subreg);
+    ascodec_async_write(&reqs[1], index, value);
+    restore_irq(oldstatus);
 
-    ascodec_unlock();
+    /* Wait for second request to finish */
+    ascodec_wait(&reqs[1]);
 }
 
+/* read special PMU subregisters */
 int ascodec_read_pmu(unsigned int index, unsigned int subreg)
 {
-    ascodec_lock();
+    struct ascodec_request reqs[2];
+    struct semaphore complete;
+
+    ascodec_async_init(&reqs[0], NULL, 0);
+    ascodec_wait_init(&reqs[1], &complete);
+
+    int oldstatus = disable_irq_save();
     /* we submit consecutive requests to make sure no operations happen on the
      * i2c bus between selecting the sub register and reading it */
-    ascodec_write(AS3543_PMU_ENABLE, subreg);
-    int ret = ascodec_read(index);
+    ascodec_async_write(&reqs[0], AS3543_PMU_ENABLE, subreg);
+    ascodec_async_read(&reqs[1], index, 1);
+    restore_irq(oldstatus);
 
-    ascodec_unlock();
+    /* Wait for second request to finish */
+    ascodec_wait(&reqs[1]);
 
-    return ret;
+    return reqs[1].data[0];
 }
 #endif /* CONFIG_CPU == AS3525v2 */
 
-void INT_AUDIO(void)
+/* callback that receives results of reading INT_AUDIO status register */
+static void ascodec_int_audio_cb(struct ascodec_request *req)
 {
-    int oldstatus = disable_irq_save();
-    int data = ascodec_read(AS3514_IRQ_ENRD0);
+    unsigned char * const data = req->data;
 
-#if CONFIG_CHARGING
-    if (data & CHG_ENDOFCH) { /* chg finished */
-        endofch = true;
+    if (UNLIKELY(req->len_done != 3))  { /* some error happened? */
+        panicf("INT_AUDIO callback got %d regs", req->len_done);
     }
 
-    chg_status = data & CHG_STATUS;
-#endif
+    if (data[0] & CHG_ENDOFCH) { /* chg finished */
+        COUNT_INT(chg_finished);
+        ascodec_enrd0_shadow |= CHG_ENDOFCH;
+    }
+
+    if (data[0] & CHG_CHANGED) { /* chg status changed */
+        if (data[0] & CHG_STATUS) {
+            COUNT_INT(chg_insert);
+            ascodec_enrd0_shadow |= CHG_STATUS;
+        } else {
+            COUNT_INT(chg_remove);
+            ascodec_enrd0_shadow &= ~CHG_STATUS;
+        }
+    }
 
-    if (data & USB_CHANGED) { /* usb status changed */
-        if (data & USB_STATUS) {
+    if (data[0] & USB_CHANGED) { /* usb status changed */
+        if (data[0] & USB_STATUS) {
+            COUNT_INT(usb_insert);
             usb_insert_int();
         } else {
+            COUNT_INT(usb_remove);
             usb_remove_int();
         }
     }
 
-    restore_irq(oldstatus);
+    if (data[2] & IRQ_RTC) { /* rtc irq */
+        /*
+         * Can be configured for once per second or once per minute,
+         * default is once per second
+         */
+        COUNT_INT(rtc);
+    }
+
+    if (data[2] & IRQ_ADC) { /* adc finished */
+        COUNT_INT(adc);
+        semaphore_release(&adc_done_sem);
+    }
+
+    VIC_INT_ENABLE = INTERRUPT_AUDIO;
+}
+
+/* ISR for all various ascodec events */
+void INT_AUDIO(void)
+{
+    VIC_INT_EN_CLEAR = INTERRUPT_AUDIO;
+    COUNT_INT(audio);
+
+    ascodec_async_read(&as_audio_req, AS3514_IRQ_ENRD0, 3);
+}
+
+/* wait for ADC to finish conversion */
+void ascodec_wait_adc_finished(void)
+{
+    semaphore_wait(&adc_done_sem, TIMEOUT_BLOCK);
 }
 
 #if CONFIG_CHARGING
+/* read sticky end-of-charge bit and clear it */
 bool ascodec_endofch(void)
 {
-    int oldstatus = disable_irq_save();
-    bool ret = endofch;
-    endofch = false;
-    restore_irq(oldstatus);
+    int oldlevel = disable_irq_save();
+
+    bool ret = ascodec_enrd0_shadow & CHG_ENDOFCH;
+    ascodec_enrd0_shadow &= ~CHG_ENDOFCH; /* clear interrupt */
+
+    restore_irq(oldlevel);
 
     return ret;
 }
 
+/* read the presence state of the charger */
 bool ascodec_chg_status(void)
 {
-    return chg_status;
+    return ascodec_enrd0_shadow & CHG_STATUS;
 }
 
 void ascodec_monitor_endofch(void)
 {
-    /* already enabled */
+    /* end of charge status interrupt already enabled */
 }
 
+/* write charger control register */
 void ascodec_write_charger(int value)
 {
 #if CONFIG_CPU == AS3525
@@ -315,6 +576,7 @@ void ascodec_write_charger(int value)
 #endif
 }
 
+/* read charger control register */
 int ascodec_read_charger(void)
 {
 #if CONFIG_CPU == AS3525
@@ -325,6 +587,15 @@ int ascodec_read_charger(void)
 }
 #endif /* CONFIG_CHARGING */
 
+/*
+ * NOTE:
+ * After the conversion to interrupts, ascodec_(lock|unlock) are only used by
+ * adc-as3514.c to protect against other threads corrupting the result by using
+ * the ADC at the same time.
+ * Concurrent ascodec_(async_)?(read|write) calls are instead protected
+ * because ascodec_submit() is atomic and concurrent requests will wait
+ * in the queue until the current request is finished.
+ */
 void ascodec_lock(void)
 {
     mutex_lock(&as_mtx);
@@ -334,3 +605,63 @@ void ascodec_unlock(void)
 {
     mutex_unlock(&as_mtx);
 }
+
+
+/** --Startup initialization-- **/
+
+void i2c_init(void)
+{
+    /* required function but called too late for our needs */
+}
+
+/* initialises the internal i2c bus and prepares for transfers to the codec */
+void ascodec_init(void)
+{
+    int prescaler;
+
+    ll_init(&req_list);
+    mutex_init(&as_mtx);
+    ascodec_async_init(&as_audio_req, ascodec_int_audio_cb, 0);
+    semaphore_init(&adc_done_sem, 1, 0);
+
+    /* enable clock */
+    bitset32(&CGU_PERI, CGU_I2C_AUDIO_MASTER_CLOCK_ENABLE);
+
+    /* prescaler for i2c clock */
+    prescaler = AS3525_I2C_PRESCALER;
+    I2C2_CPSR0 = prescaler & 0xFF;          /* 8 lsb */
+    I2C2_CPSR1 = (prescaler >> 8) & 0x3;    /* 2 msb */
+
+    /* set i2c slave address of codec part */
+    I2C2_SLAD0 = AS3514_I2C_ADDR << 1;
+
+    I2C2_CNTRL = I2C2_CNTRL_DEFAULT;
+
+    I2C2_IMR = 0x00;         /* disable interrupts */
+    I2C2_INT_CLR = I2C2_RIS; /* clear interrupt status */
+    VIC_INT_ENABLE = INTERRUPT_I2C_AUDIO;
+    VIC_INT_ENABLE = INTERRUPT_AUDIO;
+
+    /* detect if USB was connected at startup since there is no transition */
+    ascodec_enrd0_shadow = ascodec_read(AS3514_IRQ_ENRD0);
+    if(ascodec_enrd0_shadow & USB_STATUS)
+        usb_insert_int();
+
+    /* Generate irq for usb+charge status change */
+    ascodec_write(AS3514_IRQ_ENRD0,
+#if CONFIG_CHARGING /* m200v4 can't charge */
+        IRQ_CHGSTAT | IRQ_ENDOFCH |
+#endif
+        IRQ_USBSTAT);
+
+#if CONFIG_CPU == AS3525v2
+    /* XIRQ = IRQ, active low reset signal, 6mA push-pull output */
+    ascodec_write_pmu(0x1a, 3, (1<<2)|3); /* 1A-3 = Out_Cntr3 register */
+    /* Generate irq on (rtc,) adc change */
+    ascodec_write(AS3514_IRQ_ENRD2, /*IRQ_RTC |*/ IRQ_ADC);
+#else
+    /* Generate irq for push-pull, active high, irq on rtc+adc change */
+    ascodec_write(AS3514_IRQ_ENRD2, IRQ_PUSHPULL | IRQ_HIGHACTIVE |
+                                    /*IRQ_RTC |*/ IRQ_ADC);
+#endif
+}
diff --git a/firmware/target/arm/as3525/debug-as3525.c b/firmware/target/arm/as3525/debug-as3525.c
index 1ee4de64cc..a1e69834dd 100644
--- a/firmware/target/arm/as3525/debug-as3525.c
+++ b/firmware/target/arm/as3525/debug-as3525.c
@@ -605,3 +605,33 @@ end:
     lcd_setfont(FONT_UI);
     return false;
 }
+
+#ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
+/* Return CPU voltage setting in millivolts */
+int get_cpu_voltage_setting(void)
+{
+    int value;
+
+#if CONFIG_CPU == AS3525
+    value = ascodec_read(AS3514_CVDD_DCDC3) & 0x3;
+    value = 1200 - value * 50;
+#else /* as3525v2 */
+    value = ascodec_read_pmu(0x17, 1) & 0x7f;
+
+    /* Calculate in 0.1mV steps */
+    if (value == 0)
+        /* 0 volts */;
+    else if (value <= 0x40)
+        value = 6000 + value * 125;
+    else if (value <= 0x70)
+        value = 14000 + (value - 0x40) * 250;
+    else if (value <= 0x7f)
+        value = 26000 + (value - 0x70) * 500;
+
+    /* Return voltage setting in millivolts */
+    value = (value + 5) / 10;
+#endif /* CONFIG_CPU */
+
+    return value;
+}
+#endif /* HAVE_ADJUSTABLE_CPU_VOLTAGE */
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index 8aa2d02ab7..0ea0c8fba4 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -33,6 +33,8 @@
 #include "backlight-target.h"
 #include "lcd.h"
 
+struct mutex cpufreq_mtx;
+
 /*  Charge Pump and Power management Settings  */
 #define AS314_CP_DCDC3_SETTING    \
                ((0<<7) |  /* CP_SW  Auto-Switch Margin 0=200/300 1=150/255 */  \
@@ -144,6 +146,7 @@ static const struct { int source; void (*isr) (void); } vec_int_srcs[] =
     { INT_SRC_USB, INT_USB_FUNC, },
     { INT_SRC_TIMER1, INT_TIMER1 },
     { INT_SRC_TIMER2, INT_TIMER2 },
+    { INT_SRC_I2C_AUDIO, INT_I2C_AUDIO },
     { INT_SRC_AUDIO, INT_AUDIO },
     /* Lowest priority at the end of the list */
 };
@@ -322,6 +325,12 @@ void system_init(void)
     setup_vic();
 
     dma_init();
+}
+
+/* this is called after kernel and threading are initialized */
+void kernel_device_init(void)
+{
+    mutex_init(&cpufreq_mtx);
 
     ascodec_init();
 
@@ -329,7 +338,8 @@ void system_init(void)
 #ifdef HAVE_AS3543
     /* PLL:       disable audio PLL, we use MCLK already */
     ascodec_write_pmu(0x1A, 7, 0x02);
-    /* DCDC_Cntr: set switching speed of CVDD1/2 power supplies to 1 MHz */
+    /* DCDC_Cntr: set switching speed of CVDD1/2 power supplies to 1 MHz,
+       immediate change */
     ascodec_write_pmu(0x17, 7, 0x30);
     /* Out_Cntr2: set drive strength of 24 MHz and 32 kHz clocks to 1 mA */
     ascodec_write_pmu(0x1A, 2, 0xCC);
@@ -414,11 +424,28 @@ void udelay(unsigned usecs)
 
 #ifndef BOOTLOADER
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+bool set_cpu_frequency__lock(void)
+{
+    if (get_processor_mode() != CPU_MODE_THREAD_CONTEXT)
+        return false;
+
+    mutex_lock(&cpufreq_mtx);
+    return true;
+}
+
+void set_cpu_frequency__unlock(void)
+{
+    mutex_unlock(&cpufreq_mtx);
+}
 
 #if CONFIG_CPU == AS3525
 void set_cpu_frequency(long frequency)
 {
-    if(frequency == CPUFREQ_MAX)
+    if (frequency == cpu_frequency)
+    {
+        /* avoid redundant activity */
+    }
+    else if(frequency == CPUFREQ_MAX)
     {
 #ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
         /* Increasing frequency so boost voltage before change */
@@ -464,7 +491,11 @@ void set_cpu_frequency(long frequency)
 #else   /* as3525v2  */
 void set_cpu_frequency(long frequency)
 {
-    if(frequency == CPUFREQ_MAX)
+    if (frequency == cpu_frequency)
+    {
+        /* avoid redundant activity */
+    }
+    else if(frequency == CPUFREQ_MAX)
     {
 #ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
         /* Set CVDD1 power supply */
diff --git a/firmware/target/arm/as3525/system-target.h b/firmware/target/arm/as3525/system-target.h
index db5bb892ef..4fbbb46d5d 100644
--- a/firmware/target/arm/as3525/system-target.h
+++ b/firmware/target/arm/as3525/system-target.h
@@ -21,12 +21,17 @@
 #ifndef SYSTEM_TARGET_H
 #define SYSTEM_TARGET_H
 
+/* we need some system things initialized after the kernel init */
+#define KDEV_INIT
+
 #include "system-arm.h"
 #include "mmu-arm.h"
 #include "panic.h"
 
 #include "clock-target.h" /* CPUFREQ_* are defined here */
 
+void kernel_device_init(void);
+
 #define STORAGE_WANTS_ALIGN
 
 /* We can use a interrupt-based mechanism on the fuzev2 */
@@ -68,4 +73,20 @@ static inline void mdelay(unsigned msecs)
 void usb_insert_int(void);
 void usb_remove_int(void);
 
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#define CPU_BOOST_LOCK_DEFINED
+
+static inline bool cpu_boost_lock(void)
+{
+    bool set_cpu_frequency__lock(void);
+    return set_cpu_frequency__lock();
+}
+
+static inline void cpu_boost_unlock(void)
+{
+    void set_cpu_frequency__unlock(void);
+    set_cpu_frequency__unlock();
+}
+#endif /* HAVE_ADJUSTABLE_CPU_FREQ */
+
 #endif /* SYSTEM_TARGET_H */
diff --git a/firmware/target/arm/pp/system-pp5002.c b/firmware/target/arm/pp/system-pp5002.c
index 3186d3739a..388f962fce 100644
--- a/firmware/target/arm/pp/system-pp5002.c
+++ b/firmware/target/arm/pp/system-pp5002.c
@@ -24,6 +24,11 @@
 #include "adc-target.h"
 #include "button-target.h"
 
+#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
+#include "corelock.h"
+static struct corelock cpufreq_cl SHAREDBSS_ATTR;
+#endif
+
 extern void TIMER1(void);
 extern void TIMER2(void);
 
@@ -122,6 +127,18 @@ static void ipod_init_cache(void)
 }
     
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#if NUM_CORES > 1
+void set_cpu_frequency__lock(void)
+{
+    corelock_lock(&cpufreq_cl);
+}
+
+void set_cpu_frequency__unlock(void)
+{
+    corelock_unlock(&cpufreq_cl);
+}
+#endif /* NUM_CORES > 1 */
+
 void set_cpu_frequency(long frequency)
 #else
 static void pp_set_cpu_frequency(long frequency)
@@ -193,7 +210,7 @@ void system_init(void)
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 #if NUM_CORES > 1
-        cpu_boost_init();
+        corelock_init(&cpufreq_cl);
 #endif
 #else
         pp_set_cpu_frequency(CPUFREQ_MAX);
diff --git a/firmware/target/arm/pp/system-pp502x.c b/firmware/target/arm/pp/system-pp502x.c
index 99b536e132..102cfd8fea 100644
--- a/firmware/target/arm/pp/system-pp502x.c
+++ b/firmware/target/arm/pp/system-pp502x.c
@@ -308,16 +308,24 @@ void scale_suspend_core(bool suspend)
 }
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#if NUM_CORES > 1
+void set_cpu_frequency__lock(void)
+{
+    corelock_lock(&cpufreq_cl);
+}
+
+void set_cpu_frequency__unlock(void)
+{
+    corelock_unlock(&cpufreq_cl);
+}
+#endif /* NUM_CORES > 1 */
+
 void set_cpu_frequency(long frequency) ICODE_ATTR;
 void set_cpu_frequency(long frequency)
 #else
 static void pp_set_cpu_frequency(long frequency)
 #endif
 {
-#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
-    corelock_lock(&cpufreq_cl);
-#endif
-
     switch (frequency)
     {
       /* Note1: The PP5022 PLL must be run at >= 96MHz
@@ -424,10 +432,6 @@ static void pp_set_cpu_frequency(long frequency)
         DEV_INIT2    &= ~INIT_PLL;   /* disable PLL power */
         break;
     }
-
-#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
-    corelock_unlock(&cpufreq_cl);
-#endif
 }
 #endif /* !BOOTLOADER || (SANSA_E200 || SANSA_C200 || PHILIPS_SA9200) */
 
@@ -544,7 +548,6 @@ void system_init(void)
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 #if NUM_CORES > 1
         corelock_init(&cpufreq_cl);
-        cpu_boost_init();
 #endif
 #else
         pp_set_cpu_frequency(CPUFREQ_MAX);
diff --git a/firmware/target/arm/pp/system-target.h b/firmware/target/arm/pp/system-target.h
index d372b65502..1e947195bd 100644
--- a/firmware/target/arm/pp/system-target.h
+++ b/firmware/target/arm/pp/system-target.h
@@ -199,4 +199,21 @@ void system_prepare_fw_start(void);
 
 #endif /* BOOTLOADER */
 
+#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
+#define CPU_BOOST_LOCK_DEFINED
+
+static inline bool cpu_boost_lock(void)
+{
+    void set_cpu_frequency__lock(void);
+    set_cpu_frequency__lock();
+    return true;
+}
+
+static inline void cpu_boost_unlock(void)
+{
+    void set_cpu_frequency__unlock(void);
+    set_cpu_frequency__unlock();
+}
+#endif /* HAVE_ADJUSTABLE_CPU_FREQ && NUM_CORES > 1 */
+
 #endif /* SYSTEM_TARGET_H */