Pinched pcf50606 i2c code used in iRiver players and adapted to x5. Boost ratios down as much as 100 percent for mp3 and 50 percent for vorbis. mp3 hardly ever boosts even with eq. Scrolling much sharper. Like a whole new player. The code can be used for both iRiver and x5 by just defining macros with the appropriate ports and bit numbers for each player. Probably will have a pcf50606-coldfire.c soon.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@11330 a1c6a512-1295-4272-9138-f99709370657

2 files changed, 477 insertions, 75 deletions

diff --git a/firmware/target/coldfire/iaudio/x5/pcf50606-x5.c b/firmware/target/coldfire/iaudio/x5/pcf50606-x5.c
index eaecc5bef6..59ecdb612c 100644
--- a/firmware/target/coldfire/iaudio/x5/pcf50606-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/pcf50606-x5.c
@@ -28,92 +28,455 @@
 #include "generic_i2c.h"
 #include "powermgmt.h"
 
-void pcf50606_sda_output(void)
+#define USE_ASM
+
+/* Data */
+#define SDA_BITNUM          12              /* SDA1/RXD1/GPIO44 */
+#define SDA_GPIO_READ       GPIO1_READ      /* MBAR2 + 0x0b0    */
+#define SDA_GPIO_OUT        GPIO1_OUT       /* MBAR2 + 0x0b4    */
+#define SDA_GPIO_ENABLE     GPIO1_ENABLE    /* MBAR2 + 0x0b8    */
+#define SDA_GPIO_FUNCTION   GPIO1_FUNCTION  /* MBAR2 + 0x0bc    */
+
+/* Clock */
+#define SCL_BITNUM          10              /* SCL1/TXD1/GPIO10 */
+#define SCL_GPIO_READ       GPIO_READ       /* MBAR2 + 0x000    */
+#define SCL_GPIO_OUT        GPIO_OUT        /* MBAR2 + 0x004    */
+#define SCL_GPIO_ENABLE     GPIO_ENABLE     /* MBAR2 + 0x008    */
+#define SCL_GPIO_FUNCTION   GPIO_FUNCTION   /* MBAR2 + 0x00c    */
+
+#define PCF50606_ADDR       0x10
+#define SCL_BIT             (1ul << SCL_BITNUM)
+#define SDA_BIT             (1ul << SDA_BITNUM)
+
+#define SDA             ( SDA_BIT & SDA_GPIO_READ)
+#define SDA_LO_OUT  or_l( SDA_BIT, &SDA_GPIO_ENABLE)
+#define SDA_HI_IN  and_l(~SDA_BIT, &SDA_GPIO_ENABLE)
+
+#define SCL             ( SCL_BIT & SCL_GPIO_READ)
+#define SCL_LO_OUT  or_l( SCL_BIT, &SCL_GPIO_ENABLE)
+#define SCL_HI_IN  and_l(~SCL_BIT, &SCL_GPIO_ENABLE); while(!SCL);
+
+#define DELAY                           \
+    asm (                               \
+        "move.l  %[dly],%%d0 \n"        \
+    "1:                      \n"        \
+        "subq.l  #1,%%d0     \n"        \
+        "bhi.s   1b          \n"        \
+        : : [dly]"d"(i2c_delay) : "d0" );
+
+static int i2c_delay IDATA_ATTR = 44;
+
+void pcf50606_i2c_recalc_delay(int cpu_clock)
 {
-    or_l( 0x00001000, &GPIO1_ENABLE);
+    i2c_delay = MAX(cpu_clock / (400000*2*3) - 7, 1);
 }
 
-void pcf50606_sda_input(void)
+inline void pcf50606_i2c_start(void)
 {
-    and_l(~0x00001000, &GPIO1_ENABLE);
+#ifdef USE_ASM
+    asm (
+        "not.l   %[sdab]              \n" /* SDA_HI_IN */
+        "and.l   %[sdab],(8,%[sdard]) \n"
+        "not.l   %[sdab]              \n"
+
+        "not.l   %[sclb]              \n" /* SCL_HI_IN */
+        "and.l   %[sclb],(8,%[sclrd]) \n"
+        "not.l   %[sclb]              \n"
+    "1:                               \n"
+        "move.l  (%[sclrd]),%%d0      \n"
+        "btst.l  %[sclbnum], %%d0     \n"
+        "beq.s   1b                   \n"
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "or.l    %[sdab],(8,%[sdard]) \n" /* SDA_LO_OUT */
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "or.l    %[sclb],(8,%[sclrd]) \n" /* SCL_LO_OUT */
+        : /* outputs */
+        : /* inputs */
+        [sclrd]   "a"(&SCL_GPIO_READ),
+        [sclb]    "d"(SCL_BIT),
+        [sclbnum] "i"(SCL_BITNUM),
+        [sdard]   "a"(&SDA_GPIO_READ),
+        [sdab]    "d"(SDA_BIT),
+        [dly]     "d"(i2c_delay)
+        : /* clobbers */
+        "d0"
+    );
+#else
+    SDA_HI_IN;
+    SCL_HI_IN;
+    DELAY;
+    SDA_LO_OUT;
+    DELAY;
+    SCL_LO_OUT;
+#endif
 }
 
-void pcf50606_sda_lo(void)
+inline void pcf50606_i2c_stop(void)
 {
-    and_l(~0x00001000, &GPIO1_OUT);
+#ifdef USE_ASM
+    asm (
+        "or.l    %[sdab],(8,%[sdard]) \n" /* SDA_LO_OUT */
+
+        "not.l   %[sclb]              \n" /* SCL_HI_IN */
+        "and.l   %[sclb],(8,%[sclrd]) \n"
+        "not.l   %[sclb]              \n"
+    "1:                               \n"
+        "move.l  (%[sclrd]),%%d0      \n"
+        "btst.l  %[sclbnum],%%d0      \n"
+        "beq.s   1b                   \n"
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "not.l   %[sdab]              \n" /* SDA_HI_IN */
+        "and.l   %[sdab],(8,%[sdard]) \n"
+        "not.l   %[sdab]              \n"
+        : /* outputs */
+        : /* inputs */
+        [sclrd]   "a"(&SCL_GPIO_READ),
+        [sclb]    "d"(SCL_BIT),
+        [sclbnum] "i"(SCL_BITNUM),
+        [sdard]   "a"(&SDA_GPIO_READ),
+        [sdab]    "d"(SDA_BIT),
+        [dly]     "d"(i2c_delay)
+        : /* clobbers */
+        "d0"
+    );
+#else
+    SDA_LO_OUT;
+    SCL_HI_IN;
+    DELAY;
+    SDA_HI_IN;
+#endif
 }
 
-void pcf50606_sda_hi(void)
+inline void pcf50606_i2c_ack(bool ack)
 {
-    or_l( 0x00001000, &GPIO1_OUT);
-}
+#ifdef USE_ASM
+    asm (
+        "tst.b   %[ack]               \n" /* if (!ack) */
+        "bne.s   1f                   \n"
+
+        "not.l   %[sdab]              \n" /*   SDA_HI_IN */
+        "and.l   %[sdab],(8,%[sdard]) \n"
+        "not.l   %[sdab]              \n"
+        ".word   0x51fb               \n" /* trapf.l : else */
+    "1:                               \n"
+        "or.l    %[sdab],(8,%[sdard]) \n" /*   SDA_LO_OUT */
+
+        "not.l   %[sclb]              \n" /* SCL_HI_IN */
+        "and.l   %[sclb],(8,%[sclrd]) \n"
+        "not.l   %[sclb]              \n"
+    "1:                               \n"
+        "move.l  (%[sclrd]),%%d0      \n"
+        "btst.l  %[sclbnum],%%d0      \n"
+        "beq.s   1b                   \n"
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "or.l    %[sclb],(8,%[sclrd]) \n" /* SCL_LO_OUT */
+        : /* outputs */
+        : /* inputs */
+        [sclrd]   "a"(&SCL_GPIO_READ),
+        [sclb]    "d"(SCL_BIT),
+        [sclbnum] "i"(SCL_BITNUM),
+        [sdard]   "a"(&SDA_GPIO_READ),
+        [sdab]    "d"(SDA_BIT),
+        [dly]     "d"(i2c_delay),
+        [ack]     "d"(ack)
+        : /* clobbers */
+        "d0"
+    );
+#else
+    if(ack)
+        SDA_LO_OUT;
+    else
+        SDA_HI_IN;
 
-int pcf50606_sda(void)
-{
-    return 0x00001000 & GPIO1_READ;
-}
+    SCL_HI_IN;
 
-void pcf50606_scl_output(void)
-{
-    or_l( 0x00000400, &GPIO_ENABLE);
+    DELAY;
+    SCL_LO_OUT;
+#endif
 }
 
-void pcf50606_scl_input(void)
+inline bool pcf50606_i2c_getack(void)
 {
-    and_l(~0x00000400, &GPIO_ENABLE);
+    bool ret;
+
+#ifdef USE_ASM
+    asm (
+        "not.l   %[sdab]              \n" /* SDA_HI_IN */
+        "and.l   %[sdab],(8,%[sdard]) \n"
+        "not.l   %[sdab]              \n"
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "not.l   %[sclb]              \n" /* SCL_HI_IN */
+        "and.l   %[sclb],(8,%[sclrd]) \n"
+        "not.l   %[sclb]              \n"
+    "1:                               \n"
+        "move.l  (%[sclrd]),%%d0      \n"
+        "btst.l  %[sclbnum],%%d0      \n"
+        "beq.s   1b                   \n"
+
+        "move.l  (%[sdard]),%%d0      \n" /* ret = !SDA */
+        "btst.l  %[sdabnum],%%d0      \n"
+        "seq.b   %[ret]               \n"
+
+        "or.l    %[sclb],(8,%[sclrd]) \n" /* SCL_LO_OUT */
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+        : /* outputs */
+        [ret]   "=&d"(ret)
+        : /* inputs */
+        [sclrd]   "a"(&SCL_GPIO_READ),
+        [sclb]    "d"(SCL_BIT),
+        [sclbnum] "i"(SCL_BITNUM),
+        [sdard]   "a"(&SDA_GPIO_READ),
+        [sdab]    "d"(SDA_BIT),
+        [sdabnum] "i"(SDA_BITNUM),
+        [dly]     "d"(i2c_delay)
+        : /* clobbers */
+        "d0"
+    );
+#else
+    SDA_HI_IN;
+    DELAY;
+    SCL_HI_IN;
+
+    ret = !SDA;
+
+    SCL_LO_OUT;
+    DELAY;
+#endif
+    return ret;
 }
 
-int pcf50606_scl(void)
+void pcf50606_i2c_outb(unsigned char byte)
 {
-    return 0x00000400 & GPIO_READ;
+#ifdef USE_ASM
+    asm volatile (
+        "moveq.l #24,%%d0             \n" /* byte <<= 24 */
+        "lsl.l   %%d0,%[byte]         \n"
+        "moveq.l #8,%%d1              \n" /* i = 8 */
+
+    "2:                               \n" /* do */
+        "lsl.l   #1,%[byte]           \n" /* if ((byte <<= 1) carry) */
+        "bcc.s   1f                   \n"
+
+        "not.l   %[sdab]              \n" /*   SDA_HI_IN */
+        "and.l   %[sdab],(8,%[sdard]) \n"
+        "not.l   %[sdab]              \n"
+        ".word   0x51fb               \n" /* trapf.l; else */
+    "1:                               \n"
+        "or.l    %[sdab],(8,%[sdard]) \n" /*   SDA_LO_OUT */
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "not.l   %[sclb]              \n" /* SCL_HI_IN */
+        "and.l   %[sclb],(8,%[sclrd]) \n"
+        "not.l   %[sclb]              \n"
+    "1:                               \n"
+        "move.l  (%[sclrd]),%%d0      \n"
+        "btst.l  %[sclbnum],%%d0      \n"
+        "beq.s   1b                   \n"
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "or.l    %[sclb],(8,%[sclrd]) \n" /* SCL_LO_OUT */
+
+        "subq.l  #1,%%d1              \n" /* i-- */
+        "bne.s   2b                   \n" /* while (i != 0) */
+        : /* outputs */
+        [byte]    "+d"(byte)
+        : /* inputs */
+        [sclrd]   "a"(&SCL_GPIO_READ),
+        [sclb]    "d"(SCL_BIT),
+        [sclbnum] "i"(SCL_BITNUM),
+        [sdard]   "a"(&SDA_GPIO_READ),
+        [sdab]    "d"(SDA_BIT),
+        [dly]     "d"(i2c_delay)
+        : /* clobbers */
+        "d0", "d1"
+    );
+#else
+    int i;
+
+    /* clock out each bit, MSB first */
+    for ( i=0x80; i; i>>=1 )
+    {
+        if ( i & byte )
+            SDA_HI_IN;
+        else
+            SDA_LO_OUT;
+        DELAY;
+        SCL_HI_IN;
+        DELAY;
+        SCL_LO_OUT;
+    }
+#endif
 }
 
-void pcf50606_scl_lo(void)
+unsigned char pcf50606_i2c_inb(bool ack)
 {
-    and_l(~0x00000400, &GPIO_OUT);
+    unsigned char byte = 0;
+
+#ifdef USE_ASM
+    asm (
+        "not.l   %[sdab]              \n" /* SDA_HI_IN */
+        "and.l   %[sdab],(8,%[sdard]) \n"
+        "not.l   %[sdab]              \n"
+
+        "moveq.l #8,%%d1              \n" /* i = 8 */
+        "clr.l   %[byte]              \n" /* byte = 0 */
+
+    "2:                               \n" /* do */
+        "not.l   %[sclb]              \n" /* SCL_HI_IN */
+        "and.l   %[sclb],(8,%[sclrd]) \n"
+        "not.l   %[sclb]              \n"
+    "1:                               \n"
+        "move.l  (%[sclrd]),%%d0      \n"
+        "btst.l  %[sclbnum],%%d0      \n"
+        "beq.s   1b                   \n"
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "lsl.l   #1,%[byte]           \n" /* byte <<= 1 */
+        "move.l  (%[sdard]),%%d0      \n" /* if (SDA) */
+        "btst.l  %[sdabnum],%%d0      \n"
+        "beq.s   1f                   \n"
+        "addq.l  #1,%[byte]           \n" /*   byte++ */
+    "1:                               \n"
+
+        "or.l    %[sclb],(8,%[sclrd]) \n" /* SCL_LO_OUT */
+
+        "move.l  %[dly],%%d0          \n" /* DELAY */
+    "1:                               \n"
+        "subq.l  #1,%%d0              \n"
+        "bhi.s   1b                   \n"
+
+        "subq.l  #1,%%d1              \n" /* i-- */
+        "bne.s   2b                   \n" /* while (i != 0) */
+        : /* outputs */
+        [byte]    "=&d"(byte)
+        : /* inputs */
+        [sclrd]   "a"(&SCL_GPIO_READ),
+        [sclb]    "d"(SCL_BIT),
+        [sclbnum] "i"(SCL_BITNUM),
+        [sdard]   "a"(&SDA_GPIO_READ),
+        [sdab]    "d"(SDA_BIT),
+        [sdabnum] "i"(SDA_BITNUM),
+        [dly]     "d"(i2c_delay)
+        : /* clobbers */
+        "d0", "d1"
+    );
+#else
+    int i;
+
+    /* clock in each bit, MSB first */
+    SDA_HI_IN;
+    for ( i=0x80; i; i>>=1 )
+    {
+        SCL_HI_IN;
+        DELAY;
+        if ( SDA )
+            byte |= i;
+        SCL_LO_OUT;
+        DELAY;
+    }
+#endif
+
+    pcf50606_i2c_ack(ack);
+
+    return byte;
 }
 
-void pcf50606_scl_hi(void)
+int pcf50606_i2c_write(int address, const unsigned char* buf, int count)
 {
-    pcf50606_scl_input();
-    while(!pcf50606_scl())
+    int i,x=0;
+
+    pcf50606_i2c_start();
+    pcf50606_i2c_outb(address & 0xfe);
+    if (pcf50606_i2c_getack())
+    {
+        for (i=0; i<count; i++)
+        {
+            pcf50606_i2c_outb(buf[i]);
+            if (!pcf50606_i2c_getack())
+            {
+                x=-2;
+                break;
+            }
+        }
+    }
+    else
     {
+        logf("pcf50606_i2c_write() - no ack\n");
+        x=-1;
     }
-    or_l(0x0400, &GPIO_OUT);
-    pcf50606_scl_output();
+    return x;
 }
 
-void pcf50606_delay(void)
+int pcf50606_read_multiple(int address, unsigned char* buf, int count)
 {
-    do { int _x; for(_x=0;_x<32;_x++);} while(0);
-}
+    int i=0;
+    int ret = 0;
+    unsigned char obuf[1];
 
-struct i2c_interface pcf50606_i2c = {
-    0x10, /* Address */
+    obuf[0] = address;
 
-    /* Bit-banged interface definitions */
-    pcf50606_scl_hi,  /* Drive SCL high, might sleep on clk stretch */
-    pcf50606_scl_lo,  /* Drive SCL low */
-    pcf50606_sda_hi,  /* Drive SDA high */
-    pcf50606_sda_lo,  /* Drive SDA low */
-    pcf50606_sda_input,  /* Set SDA as input */
-    pcf50606_sda_output, /* Set SDA as output */
-    pcf50606_scl_input,  /* Set SCL as input */
-    pcf50606_scl_output, /* Set SCL as output */
-    pcf50606_scl,      /* Read SCL, returns 0 or nonzero */
-    pcf50606_sda,      /* Read SDA, returns 0 or nonzero */
+    /* send read command */
+    if (pcf50606_i2c_write(PCF50606_ADDR, obuf, 1) >= 0)
+    {
+        pcf50606_i2c_start();
+        pcf50606_i2c_outb(0x11);
+        if (pcf50606_i2c_getack())
+        {
+            for(i = 0;i < count-1;i++)
+                buf[i] = pcf50606_i2c_inb(true);
 
-    pcf50606_delay,  /* START SDA hold time (tHD:SDA) */
-    pcf50606_delay,  /* SDA hold time (tHD:DAT) */
-    pcf50606_delay,  /* SDA setup time (tSU:DAT) */
-    pcf50606_delay,  /* STOP setup time (tSU:STO) */
-    pcf50606_delay,  /* Rep. START setup time (tSU:STA) */
-    pcf50606_delay,  /* SCL high period (tHIGH) */
-};
+            buf[i] = pcf50606_i2c_inb(false);
+        }
+        else
+        {
+            ret = -1;
+        }
+    }
 
-int pcf50606_read_multiple(int address, unsigned char* buf, int count)
-{
-    return i2c_read_data(0x10, address, buf, count);
+    pcf50606_i2c_stop();
+
+    return ret;
 }
 
 int pcf50606_read(int address)
@@ -130,7 +493,32 @@ int pcf50606_read(int address)
 
 int pcf50606_write_multiple(int address, const unsigned char* buf, int count)
 {
-    return i2c_write_data(0x10, address, buf, count);
+    unsigned char obuf[1];
+    int i;
+    int ret = 0;
+
+    obuf[0] = address;
+
+    /* send write command */
+    if (pcf50606_i2c_write(PCF50606_ADDR, obuf, 1) >= 0)
+    {
+        for (i=0; i<count; i++)
+        {
+            pcf50606_i2c_outb(buf[i]);
+            if (!pcf50606_i2c_getack())
+            {
+                ret = -2;
+                break;
+            }
+        }
+    }
+    else
+    {
+        ret = -1;
+    }
+
+    pcf50606_i2c_stop();
+    return ret;
 }
 
 int pcf50606_write(int address, unsigned char val)
@@ -142,18 +530,18 @@ int pcf50606_write(int address, unsigned char val)
    on a running X5, and verified by disassembling the original firmware */
 static void set_voltages(void)
 {
-    static const unsigned char buf[5] =
-        {
-            0xf4,   /* IOREGC = 2.9V, ON in all states */
-            0xf0,   /* D1REGC = 2.5V, ON in all states */
-            0xf6,   /* D2REGC = 3.1V, ON in all states */
-            0xf4,   /* D3REGC = 2.9V, ON in all states */
-        };
+    static const unsigned char buf[4] =
+    {
+        0xf4,   /* IOREGC = 2.9V, ON in all states */
+        0xf0,   /* D1REGC = 2.5V, ON in all states */
+        0xf6,   /* D2REGC = 3.1V, ON in all states */
+        0xf4,   /* D3REGC = 2.9V, ON in all states */
+    };
 
     pcf50606_write_multiple(0x23, buf, 4);
 }
 
-void pcf50606_init(void)
+static void init_pmu_interrupts(void)
 {
     /* inital data is interrupt masks */
     unsigned char data[3] =
@@ -163,18 +551,8 @@ void pcf50606_init(void)
         ~0x00
     };
 
-    /* Bit banged I2C */
-    or_l(0x00001000, &GPIO1_OUT);
-    or_l(0x00000400, &GPIO_OUT);
-    or_l(0x00001000, &GPIO1_ENABLE);
-    or_l(0x00000400, &GPIO_ENABLE);
-    or_l(0x00001000, &GPIO1_FUNCTION);
-    or_l(0x00000400, &GPIO_FUNCTION);
-
-    i2c_add_node(&pcf50606_i2c);
-
     /* make sure GPI0 interrupt is off before unmasking anything */
-    and_l(~0xF, &INTPRI5);  /* INT32 - Priority 0 (Off) */
+    and_l(~0xf, &INTPRI5);  /* INT32 - Priority 0 (Off) */
 
     /* unmask the PMU interrupts we want to service */
     pcf50606_write_multiple(0x05, data, 3);
@@ -185,7 +563,25 @@ void pcf50606_init(void)
     and_l(~0x00000001, &GPIO_ENABLE);
     or_l(0x00000001, &GPIO_FUNCTION);
     or_l(0x00000100, &GPIO_INT_EN);     /* GPI0 H-L */
+}
 
+static inline void enable_pmu_interrupts(void)
+{
+    or_l(0x3, &INTPRI5); /* INT32 - Priority 3 */
+}
+
+void pcf50606_init(void)
+{
+    /* Bit banged I2C */
+    or_l(SDA_BIT, &SDA_GPIO_FUNCTION);
+    or_l(SCL_BIT, &SCL_GPIO_FUNCTION);
+    and_l(~SDA_BIT, &SDA_GPIO_OUT);
+    and_l(~SCL_BIT, &SCL_GPIO_OUT);
+    and_l(~SDA_BIT, &SDA_GPIO_ENABLE);
+    and_l(~SCL_BIT, &SCL_GPIO_ENABLE);
+
+    /* initialize pmu interrupts but don't service them yet */
+    init_pmu_interrupts();
     set_voltages();
 
     pcf50606_write(0x39, 0x00); /* GPOOD0 = green led OFF */
@@ -201,7 +597,7 @@ void pcf50606_init(void)
 #endif
 
     /* allow GPI0 interrupts from PMU now */
-    or_l(0x3, &INTPRI5); /* INT32 - Priority 3 */
+    enable_pmu_interrupts();
 }
 
 void pcf50606_reset_timeout(void)
diff --git a/firmware/target/coldfire/iaudio/x5/system-x5.c b/firmware/target/coldfire/iaudio/x5/system-x5.c
index 97d5ecc715..fcfbbc93c2 100644
--- a/firmware/target/coldfire/iaudio/x5/system-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/system-x5.c
@@ -27,6 +27,9 @@
 #define NORMAL_REFRESH_TIMER  21
 #define DEFAULT_REFRESH_TIMER 4
 
+#define RECALC_DELAYS(f) \
+        pcf50606_i2c_recalc_delay(f)
+
 void set_cpu_frequency (long) __attribute__ ((section (".icode")));
 void set_cpu_frequency(long frequency)
 {
@@ -37,6 +40,7 @@ void set_cpu_frequency(long frequency)
               /* Refresh timer for bypass frequency */
         PLLCR &= ~1;  /* Bypass mode */
         timers_adjust_prescale(CPUFREQ_DEFAULT_MULT, false);
+        RECALC_DELAYS(CPUFREQ_MAX);
         PLLCR = 0x13442045;
         CSCR0 = 0x00001180; /* Flash: 4 wait states */
         CSCR1 = 0x00000980; /* LCD: 2 wait states */
@@ -48,12 +52,13 @@ void set_cpu_frequency(long frequency)
         IDECONFIG1 = 0x106000 | (5 << 10); /* BUFEN2 enable + CS2Pre/CS2Post */
         IDECONFIG2 = 0x40000 | (1 << 8); /* TA enable + CS2wait */
         break;
-        
+
     case CPUFREQ_NORMAL:
         DCR = (DCR & ~0x01ff) | DEFAULT_REFRESH_TIMER;
               /* Refresh timer for bypass frequency */
         PLLCR &= ~1;  /* Bypass mode */
         timers_adjust_prescale(CPUFREQ_DEFAULT_MULT, false);
+        RECALC_DELAYS(CPUFREQ_NORMAL);
         PLLCR = 0x16430045;
         CSCR0 = 0x00000580; /* Flash: 1 wait state */
         CSCR1 = 0x00000180; /* LCD: 0 wait states */
@@ -70,6 +75,7 @@ void set_cpu_frequency(long frequency)
               /* Refresh timer for bypass frequency */
         PLLCR &= ~1;  /* Bypass mode */
         timers_adjust_prescale(CPUFREQ_DEFAULT_MULT, true);
+        RECALC_DELAYS(CPUFREQ_DEFAULT);
         PLLCR = 0x10400200; /* Power down PLL, but keep CLSEL and CRSEL */
         CSCR0 = 0x00000180; /* Flash: 0 wait states */
         CSCR1 = 0x00000180; /* LCD: 0 wait states */