AMS: Return ascodec to interrupt-based I2C2 driver

1. Slightly revised and regularized internal interface. Callback is used
for read and write to provide completion signal instead of having two
mechanisms.

2. Lower overhead for asynchronous or alterate completion callbacks. We
now only init what is required by the transfer. A couple unneeded
structure members were also nixed.

3. Fixes a bug that would neglect a semaphore wait if pumping the I2C
interrupts in a loop when not in thread state or interrupts are masked.

4. Corrects broken initialization order by defining KDEV_INIT, which
makes kernel_init() call kernel_device_init() to initialize additional
devices _after_ the kernel, threading and synchronization objects are
safe to use.

5. Locking set_cpu_frequency has to be done at the highest level in
system.c to ensure the boost counter and the frequency are both set in
agreement. Reconcile the locking inteface between PP and AMS (the only
two currently using locking there) to keep it clean.

Now works fine with voltages in GIT HEAD on my Fuze v2, type 0.
Previously, everything crashed and died instantly. action.c calling
set_cpu_frequency from a tick was part of it. The rest may have been
related to 3. and 4. Honestly, I'm not certain!

Testing by Mihail Zenkov indicates it solves our problems. This will
get the developer builds running again after the kernel assert code
push.

Change-Id: Ie245994fb3e318dd5ef48e383ce61fdd977224d4

7 files changed, 583 insertions, 133 deletions

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 */