iPod Classic: s5l8702 clocking rewrite+documentation

This is a rewrite of the clocking section, the resulting system
frequencies are the same as the current git version.

This pàtch uses fixed FClk and just one register is written to switch
all system frequencies, it needs less steps than the current git
version to reach the desired frequency, so it is faster and safer.
Includes functions to step-up/down over a table of predefined set of
frequencies.

The major difference is that Vcore is decreased from 1050 to 1000 mV.

See clocking-s5l8702.h for more information.

Change-Id: I58ac6634e1996adbe1c0c0918a7ce94ad1917d8e

7 files changed, 792 insertions, 79 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 091b3b1da7..cd1bf492d8 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -1600,6 +1600,7 @@ target/arm/s5l8702/system-s5l8702.c
 target/arm/s5l8702/gpio-s5l8702.c
 target/arm/s5l8702/pl080.c
 target/arm/s5l8702/dma-s5l8702.c
+target/arm/s5l8702/clocking-s5l8702.c
 target/arm/s5l8702/ipod6g/lcd-ipod6g.c
 target/arm/s5l8702/ipod6g/lcd-asm-ipod6g.S
 #if 0 //TODO
diff --git a/firmware/export/s5l8702.h b/firmware/export/s5l8702.h
index 1a59d47036..6ec59eaf37 100644
--- a/firmware/export/s5l8702.h
+++ b/firmware/export/s5l8702.h
@@ -39,6 +39,12 @@
 #define TTB_SIZE                  0x4000
 #define TTB_BASE_ADDR             (DRAM_ORIG + DRAM_SIZE - TTB_SIZE)
 
+#define IRAM0_ORIG      0x22000000
+#define IRAM0_SIZE      0x20000
+#define IRAM1_ORIG      0x22020000
+#define IRAM1_SIZE      0x20000
+
+
 /////SYSTEM CONTROLLER/////
 #define CLKCON0      (*((volatile uint32_t*)(0x3C500000)))
 #define CLKCON1      (*((volatile uint32_t*)(0x3C500004)))
@@ -151,6 +157,9 @@
 
 
 /////I2C/////
+#define I2CCLKGATE(i)   ((i) == 1 ? CLOCKGATE_I2C1 : \
+                                    CLOCKGATE_I2C0)
+
 #define IICCON(bus)  (*((uint32_t volatile*)(0x3C600000 + 0x300000 * (bus))))
 #define IICSTAT(bus) (*((uint32_t volatile*)(0x3C600004 + 0x300000 * (bus))))
 #define IICADD(bus)  (*((uint32_t volatile*)(0x3C600008 + 0x300000 * (bus))))
@@ -374,9 +383,9 @@
 #define SPIBASE(i)      ((i) == 2 ? 0x3d200000 : \
                          (i) == 1 ? 0x3ce00000 : \
                                     0x3c300000)
-#define SPICLKGATE(i)   ((i) == 2 ? 0x2f : \
-                         (i) == 1 ? 0x2b : \
-                                    0x22)
+#define SPICLKGATE(i)   ((i) == 2 ? CLOCKGATE_SPI2 : \
+                         (i) == 1 ? CLOCKGATE_SPI1 : \
+                                    CLOCKGATE_SPI0)
 #define SPIDMA(i)       ((i) == 2 ? 0xd : \
                          (i) == 1 ? 0xf : \
                                     0x5)
@@ -658,6 +667,10 @@
 
 
 /////I2S/////
+#define I2SCLKGATE(i)   ((i) == 2 ? CLOCKGATE_I2S2 : \
+                         (i) == 1 ? CLOCKGATE_I2S1 : \
+                                    CLOCKGATE_I2S0)
+
 #define I2SCLKCON           (*((volatile uint32_t*)(0x3CA00000)))
 #define I2STXCON            (*((volatile uint32_t*)(0x3CA00004)))
 #define I2STXCOM            (*((volatile uint32_t*)(0x3CA00008)))
@@ -676,38 +689,67 @@
 
 
 /////CLOCK GATES/////
-#define CLOCKGATE_USB_1 2
-#define CLOCKGATE_USB_2 35
-#define CLOCKGATE_DMAC0 25
-#define CLOCKGATE_DMAC1 26
-#define CLOCKGATE_UART  41
+#define CLOCKGATE_SHA       0
+#define CLOCKGATE_LCD       1
+#define CLOCKGATE_USBOTG    2
+#define CLOCKGATE_SMx       3
+#define CLOCKGATE_SM1       4
+#define CLOCKGATE_ATA       5
+#define CLOCKGATE_SDCI      9
+#define CLOCKGATE_AES       10
+#define CLOCKGATE_DMAC0     25
+#define CLOCKGATE_DMAC1     26
+#define CLOCKGATE_ROM       30
+
+#define CLOCKGATE_RTC       32
+#define CLOCKGATE_CWHEEL    33
+#define CLOCKGATE_SPI0      34
+#define CLOCKGATE_USBPHY    35
+#define CLOCKGATE_I2C0      36
+#define CLOCKGATE_TIMER     37
+#define CLOCKGATE_I2C1      38
+#define CLOCKGATE_I2S0      39
+#define CLOCKGATE_UART      41
+#define CLOCKGATE_I2S1      42
+#define CLOCKGATE_SPI1      43
+#define CLOCKGATE_GPIO      44
+#define CLOCKGATE_CHIPID    46
+#define CLOCKGATE_I2S2      47
+#define CLOCKGATE_SPI2      48
 
 
 /////INTERRUPTS/////
-#define IRQ_TIMER32 7
-#define IRQ_TIMER 8
-#define IRQ_USB_FUNC 19
-#define IRQ_DMAC(d) 16 + d
-#define IRQ_DMAC0 16
-#define IRQ_DMAC1 17
-#define IRQ_WHEEL 23
-#define IRQ_ATA 29
-#define IRQ_MMC 44
-
-#define IRQ_UART(i) (24+i)
-#define IRQ_UART0 24
-#define IRQ_UART1 25
-#define IRQ_UART2 26
-#define IRQ_UART3 27
-#define IRQ_UART4 28    /* obsolete/not implemented on s5l8702 ??? */
-
-#define IRQ_EXT0    0
-#define IRQ_EXT1    1
-#define IRQ_EXT2    2
-#define IRQ_EXT3    3
-#define IRQ_EXT4    31
-#define IRQ_EXT5    32
-#define IRQ_EXT6    33
+#define IRQ_TIMER32     7
+#define IRQ_TIMER       8
+#define IRQ_SPI(i)      (9+i) /* TBC */
+#define IRQ_SPI0        9
+#define IRQ_SPI1        10
+#define IRQ_SPI2        11
+#define IRQ_LCD         14
+#define IRQ_DMAC(d)     (16+d)
+#define IRQ_DMAC0       16
+#define IRQ_DMAC1       17
+#define IRQ_USB_FUNC    19
+#define IRQ_I2C         21    /* TBC */
+#define IRQ_WHEEL       23
+#define IRQ_UART(i)     (24+i)
+#define IRQ_UART0       24
+#define IRQ_UART1       25
+#define IRQ_UART2       26
+#define IRQ_UART3       27
+#define IRQ_UART4       28    /* obsolete/not implemented on s5l8702 ??? */
+#define IRQ_ATA         29
+#define IRQ_SBOOT       36
+#define IRQ_AES         39
+#define IRQ_SHA         40
+#define IRQ_MMC         44
+
+#define IRQ_EXT0        0
+#define IRQ_EXT1        1
+#define IRQ_EXT2        2
+#define IRQ_EXT3        3
+#define IRQ_EXT4        31
+#define IRQ_EXT5        32
+#define IRQ_EXT6        33
 
 #endif
-
diff --git a/firmware/target/arm/s5l8702/clocking-s5l8702.c b/firmware/target/arm/s5l8702/clocking-s5l8702.c
new file mode 100644
index 0000000000..5293385453
--- /dev/null
+++ b/firmware/target/arm/s5l8702/clocking-s5l8702.c
@@ -0,0 +1,230 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id:
+ *
+ * Copyright (C) 2015 by Cástor Muñoz
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include <stdbool.h>
+
+#include "config.h"
+#include "system.h"  /* udelay() */
+#include "s5l8702.h"
+#include "clocking-s5l8702.h"
+
+/* returns configured frequency (PLLxFreq, when locked) */
+unsigned pll_get_cfg_freq(int pll)
+{
+    unsigned pdiv, mdiv, sdiv, f_in;
+    uint32_t pllpms;
+
+    pllpms = PLLPMS(pll);
+
+    pdiv = (pllpms >> PLLPMS_PDIV_POS) & PLLPMS_PDIV_MSK;
+    if (pdiv == 0) return 0;
+    mdiv = (pllpms >> PLLPMS_MDIV_POS) & PLLPMS_MDIV_MSK;
+    sdiv = (pllpms >> PLLPMS_SDIV_POS) & PLLPMS_SDIV_MSK;
+
+    /* experimental results sugests that the HW is working this way */
+    if (mdiv < 2) mdiv += 256;
+
+    if (GET_PMSMOD(pll) == PMSMOD_DIV)
+    {
+        f_in = (GET_DMOSC(pll))
+                    ? ((PLLMOD2 & PLLMOD2_ALTOSC_BIT(pll))
+                                            ? S5L8702_ALTOSC1_HZ
+                                            : S5L8702_ALTOSC0_HZ)
+                    : S5L8702_OSC0_HZ;
+
+        return (f_in * mdiv / pdiv) >> sdiv; /* divide */
+    }
+    else
+    {
+        /* XXX: overflows for high f_in, safe for 32768 Hz */
+        f_in = S5L8702_OSC1_HZ;
+        return (f_in * mdiv * pdiv) >> sdiv; /* multiply */
+    }
+}
+
+/* returns PLLxClk */
+unsigned pll_get_out_freq(int pll)
+{
+    uint32_t pllmode = PLLMODE;
+
+    if ((pllmode & PLLMODE_PLLOUT_BIT(pll)) == 0)
+        return S5L8702_OSC1_HZ; /* slow mode */
+
+    if ((pllmode & PLLMODE_EN_BIT(pll)) == 0)
+        return 0;
+
+    return pll_get_cfg_freq(pll);
+}
+
+/* returns selected oscillator for CG16_SEL_OSC source */
+unsigned soc_get_oscsel_freq(void)
+{
+    return (PLLMODE & PLLMODE_OSCSEL_BIT) ? S5L8702_OSC1_HZ
+                                          : S5L8702_OSC0_HZ;
+}
+
+/* returns output frequency */
+unsigned cg16_get_freq(volatile uint16_t* cg16)
+{
+    unsigned sel, freq;
+    uint16_t val16 = *cg16;
+
+    if (val16 & CG16_DISABLE_BIT)
+        return 0;
+
+    sel = (val16 >> CG16_SEL_POS) & CG16_SEL_MSK;
+
+    if (val16 & CG16_UNKOSC_BIT)
+        freq = S5L8702_UNKOSC_HZ;
+    else if (sel == CG16_SEL_OSC)
+        freq = soc_get_oscsel_freq();
+    else
+        freq = pll_get_out_freq(--sel);
+
+    freq /= (((val16 >> CG16_DIV1_POS) & CG16_DIV1_MSK) + 1) *
+                (((val16 >> CG16_DIV2_POS) & CG16_DIV2_MSK) + 1);
+
+    return freq;
+}
+
+void soc_set_system_divs(unsigned cdiv, unsigned hdiv, unsigned hprat)
+{
+    uint32_t val = 0;
+    unsigned pdiv = hdiv * hprat;
+
+    if (cdiv > 1)
+        val |= CLKCON1_CDIV_EN_BIT |
+                ((((cdiv >> 1) - 1) & CLKCON1_CDIV_MSK) << CLKCON1_CDIV_POS);
+    if (hdiv > 1)
+        val |= CLKCON1_HDIV_EN_BIT |
+                ((((hdiv >> 1) - 1) & CLKCON1_HDIV_MSK) << CLKCON1_HDIV_POS);
+    if (pdiv > 1)
+        val |= CLKCON1_PDIV_EN_BIT |
+                ((((pdiv >> 1) - 1) & CLKCON1_PDIV_MSK) << CLKCON1_PDIV_POS);
+
+    val |= ((hprat - 1) & CLKCON1_HPRAT_MSK) << CLKCON1_HPRAT_POS;
+
+    CLKCON1 = val;
+
+    while ((CLKCON1 >> 8) != (val >> 8));
+}
+
+unsigned soc_get_system_divs(unsigned *cdiv, unsigned *hdiv, unsigned *pdiv)
+{
+    uint32_t val = CLKCON1;
+
+    if (cdiv)
+        *cdiv = !(val & CLKCON1_CDIV_EN_BIT) ? 1 :
+                (((val >> CLKCON1_CDIV_POS) & CLKCON1_CDIV_MSK) + 1) << 1;
+    if (hdiv)
+        *hdiv = !(val & CLKCON1_HDIV_EN_BIT) ? 1 :
+                (((val >> CLKCON1_HDIV_POS) & CLKCON1_HDIV_MSK) + 1) << 1;
+    if (pdiv)
+        *pdiv = !(val & CLKCON1_PDIV_EN_BIT) ? 1 :
+                (((val >> CLKCON1_PDIV_POS) & CLKCON1_PDIV_MSK) + 1) << 1;
+
+    return cg16_get_freq(&CG16_SYS);  /* FClk */
+}
+
+unsigned get_system_freqs(unsigned *cclk, unsigned *hclk, unsigned *pclk)
+{
+    unsigned fclk, cdiv, hdiv, pdiv;
+
+    fclk = soc_get_system_divs(&cdiv, &hdiv, &pdiv);
+
+    if (cclk) *cclk = fclk / cdiv;
+    if (hclk) *hclk = fclk / hdiv;
+    if (pclk) *pclk = fclk / pdiv;
+
+    return fclk;
+}
+
+void soc_set_hsdiv(int hsdiv)
+{
+    SM1_DIV = hsdiv - 1;
+}
+
+int soc_get_hsdiv(void)
+{
+    return (SM1_DIV & 0xf) + 1;
+}
+
+/* each target/app could define its own clk_modes table */
+struct clocking_mode *clk_modes;
+int cur_level = -1;
+
+void clocking_init(struct clocking_mode *modes, int level)
+{
+    /* at this point, CK16_SYS should be already configured
+       and enabled by emCORE/bootloader */
+
+    /* initialize global clocking */
+    clk_modes = modes;
+    cur_level = level;
+
+    /* start initial level */
+    struct clocking_mode *m = clk_modes + cur_level;
+    soc_set_hsdiv(m->hsdiv);
+    soc_set_system_divs(m->cdiv, m->hdiv, m->hprat);
+}
+
+void set_clocking_level(int level)
+{
+    struct clocking_mode *cur, *next;
+
+    int step = (level < cur_level) ? -1 : 1;
+
+    while (cur_level != level)
+    {
+        cur = clk_modes + cur_level;
+        next = cur + step;
+
+        /* step-up */
+        if (next->hsdiv > cur->hsdiv)
+            soc_set_hsdiv(next->hsdiv);
+
+        /* step up/down */
+        soc_set_system_divs(next->cdiv, next->hdiv, next->hprat);
+
+        /* step-down */
+        if (next->hsdiv < cur->hsdiv)
+            soc_set_hsdiv(next->hsdiv);
+
+        cur_level += step;
+    }
+
+    udelay(50); /* TBC: probably not needed */
+}
+
+#if 0
+/* - This function is mainly to documment how s5l8702 ROMBOOT and iPod
+ *   Classic diagnostic OF detects primary external clock.
+ * - ATM it is unknown if 24 MHz are used on other targets (i.e. Nano 3G),
+ *   other SoC (ROMBOOT identifies itself as s5l8900/s5l8702), a Classic
+ *   prototype, or (probably) never used...
+ * - This function should be called only at boot time, GPIO3.5 is also
+ *   used for ATA controller.
+ */
+unsigned soc_get_osc0(void)
+{
+    GPIOCMD = 0x30500;  /* configure GPIO3.5 as input */
+    return (PDAT3 & 0x20) ? 24000000 : 12000000;
+}
+#endif
diff --git a/firmware/target/arm/s5l8702/clocking-s5l8702.h b/firmware/target/arm/s5l8702/clocking-s5l8702.h
new file mode 100644
index 0000000000..e7e4fab3bd
--- /dev/null
+++ b/firmware/target/arm/s5l8702/clocking-s5l8702.h
@@ -0,0 +1,436 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id:
+ *
+ * Copyright (C) 2015 by Cástor Muñoz
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef __CLOCKING_S5L8702_H
+#define __CLOCKING_S5L8702_H
+
+/*
+ *  - This is work in progress, results are mainly based on experimental
+ *    tests using emCORE and/or iPod Classic OF reverse engineering.
+ *  - Things marked as TBC are 'somewhat' speculations, so might be
+ *    inconplete, inaccurate or erroneous.
+ *  - Things not marked as TBC could be also incomplete, inaccurate or
+ *    erroneous.
+ */
+
+/* S5L8702 _figured_ clocking:
+ *
+ *                                         CG16_SEL
+ *                                      CG16_UNKOSC_BIT
+ *                                            ||
+ *                                            ||
+ *                                     UNKOSC ||
+ *                       OSCSEL        ------>||
+ *                         |\                 ||
+ *  OSC0 -+--------------->| |         OSCClk ||
+ *        |                | |--------------->||      ______
+ *  OSC1 -)+-------------->| |                ||     |      |            FClk
+ *        ||               |/                 ||---->| DIV1 |---------------->
+ *        ||     ______                       ||     |______|
+ *        ||    |      | Pll0Freq |\          ||     CG16_SYS
+ *        ++===>| PMS0 |--------->| |         ||
+ *        ||    |______|          | | PLL0Clk ||      ____________
+ *        ||      PLL0            | |-------->||     |            |   U2L_Clk
+ *        ||                 OSC1 | |         ||---->| DIV1,UNK14 |---------->
+ *        |+--------------------->| |         ||     |____________|
+ *        ||                      |/          ||     CG16_2L
+ *        ||                   PLLOUT0        ||
+ *        ||                                  ||      ______
+ *        ||     ______                       ||     |      |        SVID_Clk
+ *        ||    |      | Pll1Freq |\          ||---->| DIV1 |---------------->
+ *        ++===>| PMS1 |--------->| |         ||     |______|
+ *         |    |______|          | | PLL1Clk ||     CG16_SVID (TBC)
+ *         |      PLL1            | |-------->||
+ *         |                 OSC1 | |         ||      ___________
+ *         +--------------------->| |         ||     |           |   AUDx_Clk
+ *         |                      |/          ||---->| DIV1,DIV2 |----------->
+ *         |                   PLLOUT1        ||     |___________|
+ *         |                                  ||     CG16_AUDx
+ *         |     ______                       ||
+ *         |    |      | Pll2Freq |\          ||      ___________
+ *         +--->| PMS2 |--------->| |         ||     |           |       EClk
+ *         |    |______|          | | PLL2Clk ||---->| DIV1,DIV2 |--+-------->
+ *         |      PLL2            | |-------->||     |___________|  |
+ *         |                 OSC1 | |         ||     CG16_RTIME     | MIU_Clk
+ *         +--------------------->| |         ||                    +-------->
+ *                                |/          ||      ______
+ *                             PLLOUT2        ||     |      |         U5L_Clk
+ *                                            ||---->| DIV1 |---------------->
+ *                                            ||     |______|
+ *                                            ||     CG16_5L
+ *
+ *               ______
+ *              |      | CClk
+ *  FClk --+--->| CDIV |------------> ARM_Clk
+ *         |    |______|
+ *         |
+ *         |     ______
+ *         |    |      | HClk
+ *         +--->| HDIV |---------> CLKCON0[31] --> SDR_Clk
+ *         |    |______|      |
+ *         |                  |    PWRCON_AHB
+ *         |                  +-----> [0] ----> Clk_SHA1
+ *         |                          [1] ----> Clk_LCD
+ *         |                          [2] ----> Clk_USBOTG
+ *         |                          [3] ----> Clk_SMx     TBC: Static Memory
+ *         |                          [4] ----> Clk_SM1          (ctrl) related
+ *         |                          [5] ----> Clk_ATA
+ *         |                          [6] ----> Clk_UNK
+ *         |                          [7] ----> Clk_UNK
+ *         |                          [8] ----> Clk_NAND    TBC
+ *         |                          [9] ----> Clk_SDCI
+ *         |                          [10] ---> Clk_AES
+ *         |                          [11] ---> Clk_UNK
+ *         |                          [12] ---> Clk_ECC     TBC
+ *         |                          [13] ---> Clk_UNK
+ *         |                          [14] ---> Clk_EV0     TBC: Ext. Video
+ *         |                          [15] ---> Clk_EV1     TBC: Ext. Video
+ *         |                          [16] ---> Clk_EV2     TBC: Ext. Video
+ *         |                          [17] ---> Clk_UNK
+ *         |                          [18] ---> Clk_UNK
+ *         |                          [19] ---> Clk_UNK
+ *         |                          [20] ---> Clk_UNK
+ *         |                          [21] ---> Clk_UNK
+ *         |                          [22] ---> Clk_UNK
+ *         |                          [23] ---> Clk_UNK
+ *         |                          [24] ---> Clk_UNK
+ *         |                          [25] ---> Clk_DMA0
+ *         |                          [26] ---> Clk_DMA1
+ *         |                          [27] ---> Clk_UNK
+ *         |                          [28] ---> Clk_UNK
+ *         |                          [29] ---> Clk_UNK
+ *         |                          [30] ---> Clk_ROM
+ *         |                          [31] ---> Clk_UNK
+ *         |
+ *         |     ______
+ *         |    |      | PClk      PWRCON_APB
+ *         +--->| PDIV |------------> [0] ----> Clk_RTC
+ *              |______|              [1] ----> Clk_CWHEEL
+ *                                    [2] ----> Clk_SPI0
+ *                                    [3] ----> Clk_USBPHY
+ *                                    [4] ----> Clk_I2C0
+ *                                    [5] ----> Clk_TIMER   16/32-bit timer
+ *                                    [6] ----> Clk_I2C1
+ *                                    [7] ----> Clk_I2S0
+ *                                    [8] ----> Clk_UNK     TBC: SPDIF Out
+ *                                    [9] ----> Clk_UART
+ *                                    [10] ---> Clk_I2S1
+ *                                    [11] ---> Clk_SPI1
+ *                                    [12] ---> Clk_GPIO
+ *                                    [13] ---> Clk_SBOOT   TBC: Secure Boot
+ *                                    [14] ---> Clk_CHIPID
+ *                                    [15] ---> Clk_SPI2
+ *                                    [16] ---> Clk_I2S2
+ *                                    [17] ---> Clk_UNK
+ *
+ * IRAM notes:
+ * - IRAM0 (1st. 128 Kb) and IRAM1 (2nd. 128 Kb) uses diferent clocks,
+ *   maximum rd/wr speed for IRAM1 is about a half of maximum rd/wr
+ *   speed for IRAM0, it is unknown but probably they are different
+ *   HW memory.
+ * - masking Clk_SMx disables access to IRAM0 and IRAM1
+ * - masking Clk_SM1 disables access to IRAM1
+ * - IRAM1 rd/wr speed is scaled by SM1_DIV, so it could be related
+ *   with Clk_SM1 (TBC)
+ */
+
+#include <inttypes.h>
+#include "config.h"
+
+#define CLOCKING_DEBUG
+
+#if defined(IPOD_6G)
+/* iPod Classic target */
+#define S5L8702_OSC0_HZ         12000000  /* external OSC */
+#define S5L8702_OSC1_HZ         32768     /* from PMU */
+
+#define S5L8702_ALTOSC0_HZ      0   /* TBC */
+#define S5L8702_ALTOSC1_HZ      0   /* TBC */
+
+/* this clock is selected when CG16_UNKOSC_BIT is set,
+   ignoring PLLMODE_CLKSEL and CG16_SEL settings */
+/* TBC: OSC0*2 ???, 24 MHz Xtal ???, USB ??? */
+#define S5L8702_UNKOSC_HZ       24000000
+
+#else
+/* s5l8702 ROMBOOT */
+#define S5L8702_OSC0_HZ         (soc_get_osc0())  /* external OSC */
+#define S5L8702_OSC1_HZ         32768             /* from PMU */
+
+#define S5L8702_ALTOSC0_HZ      1800000
+#define S5L8702_ALTOSC1_HZ      27000000
+#endif
+
+/* TODO: join all these definitions in an unique place */
+#if 1
+#include "s5l8702.h"
+#else
+#define CLKCON0     (*((volatile uint32_t*)(0x3C500000)))
+#define CLKCON1     (*((volatile uint32_t*)(0x3C500004)))
+#define CLKCON2     (*((volatile uint32_t*)(0x3C500008)))
+#define CLKCON3     (*((volatile uint32_t*)(0x3C50000C)))
+#define CLKCON4     (*((volatile uint32_t*)(0x3C500010)))
+#define CLKCON5     (*((volatile uint32_t*)(0x3C500014)))
+#define PLL0PMS     (*((volatile uint32_t*)(0x3C500020)))
+#define PLL1PMS     (*((volatile uint32_t*)(0x3C500024)))
+#define PLL2PMS     (*((volatile uint32_t*)(0x3C500028)))
+#define PLL0LCNT    (*((volatile uint32_t*)(0x3C500030)))
+#define PLL1LCNT    (*((volatile uint32_t*)(0x3C500034)))
+#define PLL2LCNT    (*((volatile uint32_t*)(0x3C500038)))
+#define PLLLOCK     (*((volatile uint32_t*)(0x3C500040)))
+#define PLLMODE     (*((volatile uint32_t*)(0x3C500044)))
+#define PWRCON(i)   (*((volatile uint32_t*)(0x3C500048 + ((i)*4)))) /*i=1,2*/
+#endif
+
+/* TBC: ATM i am assuming that PWRCON_AHB/APB registers are clockgates
+ * for SoC internal controllers sitting on AHB/APB buses, this is based
+ * on other similar SoC documentation and experimental results for many
+ * (not all) s5l8702 controllers.
+ */
+#define PWRCON_AHB  (*((uint32_t volatile*)(0x3C500048)))
+#define PWRCON_APB  (*((uint32_t volatile*)(0x3C50004c)))
+
+#define PLLPMS(i)   (*((volatile uint32_t*)(0x3C500020 + ((i) * 4))))
+#define PLLCNT(i)   (*((volatile uint32_t*)(0x3C500030 + ((i) * 4))))
+#define PLLMOD2     (*((volatile uint32_t*)(0x3C500060)))
+#define PLLCNT_MSK  0x3fffff
+
+/* TBC: Clk_SM1 = HClk / (SM1_DIV[3:0] + 1) */
+#define SM1_DIV     (*((volatile uint32_t*)(0x38501000)))
+
+
+/* CG16_x: for readability and debug, these gates are defined as
+ * 16-bit registers, on HW they are really halves of 32-bit registers.
+ * Some functionallity is not available on all CG16 gates (when so,
+ * related bits are read-only and fixed to 0).
+ *
+ *                CLKCONx   DIV1    DIV2    UNKOSC   UNK14
+ *  CG16_SYS      0L        +
+ *  CG16_2L       2L        +               +(TBC)   +(TBC)
+ *  CG16_SVID     2H        +               +(TBC)
+ *  CG16_AUD0     3L        +       +
+ *  CG16_AUD1     3H        +       +
+ *  CG16_AUD2     4L        +       +
+ *  CG16_RTIME    4H        +       +       +
+ *  CG16_5L       5L        +
+ *
+ * Not all gates are fully tested, this information is mainly based
+ * on experimental test using emCORE:
+ *  - CG16_SYS and CG16_RTIME were tested mainly using time benchs.
+ *  - EClk is used as a fixed clock (not depending on CPU/AHB/APB
+ *    settings) for the timer contrller. MIU_Clk is used by the MIU
+ *    controller to generate the DRAM refresh signals.
+ *  - AUDxClk are a source selection for I2Sx modules, so they can
+ *    can be scaled and routed to the I2S GPIO ports, where they
+ *    were sampled (using emCORE) to inspect how they behave.
+ *  - CG16_SVID seem to be used for external video, this info is
+ *    based on OF diagnostics reverse engineering.
+ *  - CG16_2L an CG16_5L usage is unknown.
+ */
+#define CG16_SYS        (*((volatile uint16_t*)(0x3C500000)))
+#define CG16_2L         (*((volatile uint16_t*)(0x3C500008)))
+#define CG16_SVID       (*((volatile uint16_t*)(0x3C50000A)))
+#define CG16_AUD0       (*((volatile uint16_t*)(0x3C50000C)))
+#define CG16_AUD1       (*((volatile uint16_t*)(0x3C50000E)))
+#define CG16_AUD2       (*((volatile uint16_t*)(0x3C500010)))
+#define CG16_RTIME      (*((volatile uint16_t*)(0x3C500012)))
+#define CG16_5L         (*((volatile uint16_t*)(0x3C500014)))
+
+/* CG16 output frequency =
+   !DISABLE_BIT * SEL_x frequency / DIV1+1 / DIV2+1 */
+#define CG16_DISABLE_BIT    (1 << 15)   /* mask clock output */
+#define CG16_UNK14_BIT      (1 << 14)   /* writable on CG16_2L */
+
+#define CG16_SEL_POS        12          /* source clock selection */
+#define CG16_SEL_MSK        0x3
+#define CG16_SEL_OSC        0
+#define CG16_SEL_PLL0       1
+#define CG16_SEL_PLL1       2
+#define CG16_SEL_PLL2       3
+
+#define CG16_UNKOSC_BIT     (1 << 11)
+
+#define CG16_DIV2_POS       4           /* 2nd divisor */
+#define CG16_DIV2_MSK       0xf
+
+#define CG16_DIV1_POS       0           /* 1st divisor */
+#define CG16_DIV1_MSK       0xf
+
+/*
+ *  CLKCON0
+ */
+#define CLKCON0_SDR_DISABLE_BIT (1 << 31)
+
+/*
+ *  CLKCON1
+ */
+/* CPU/AHB/APB real_divisor =
+   xDIV_EN_BIT ? 2*(reg_value+1) : 1 */
+#define CLKCON1_CDIV_POS        24
+#define CLKCON1_CDIV_MSK        0x1f
+#define CLKCON1_CDIV_EN_BIT     (1 << 30)
+
+#define CLKCON1_HDIV_POS        16
+#define CLKCON1_HDIV_MSK        0x1f
+#define CLKCON1_HDIV_EN_BIT     (1 << 22)
+
+#define CLKCON1_PDIV_POS        8
+#define CLKCON1_PDIV_MSK        0x1f
+#define CLKCON1_PDIV_EN_BIT     (1 << 14)
+
+/* AHB/APB ratio: must be written when HDIV and/or PDIV
+   are modified, real_ratio = reg_value + 1 */
+#define CLKCON1_HPRAT_POS       0
+#define CLKCON1_HPRAT_MSK       0x3f
+
+/*
+ *  CLKCON5
+ */
+/* TBC: this bit selects a clock routed (at least) to all I2S modules
+ * (AUDAUX_Clk, see i2s-s5l8702.h), it can be selected as a source
+ * for CODEC_CLK (MCLK), on iPod Classic AUDAUX_Clk is:
+ *  0 -> 12 MHz (TBC: OSC0 ???)
+ *  1 -> 24 MHz (TBC: 2*OSC0 ???)
+ */
+#define CLKCON5_AUDAUXCLK_BIT   (1 << 31)
+
+/*
+ *  PLLnPMS
+ */
+#define PLLPMS_PDIV_POS         24      /* pre-divider */
+#define PLLPMS_PDIV_MSK         0x3f
+#define PLLPMS_MDIV_POS         8       /* main divider */
+#define PLLPMS_MDIV_MSK         0xff
+#define PLLPMS_SDIV_POS         0       /* post-divider (2^S) */
+#define PLLPMS_SDIV_MSK         0x7
+
+/*
+ *  PLLLOCK
+ */
+/* Start status:
+   0 -> in progress, 1 -> locked */
+#define PLLLOCK_LCK_BIT(n)      (1 << (n))
+
+/* Lock status for Divisor Mode (DM):
+   0 -> DM unlocked, 1 -> DM locked */
+#define PLLLOCK_DMLCK_BIT(n)    (1 << (4 + (n)))
+
+/*
+ *  PLLMODE
+ */
+/* Enable PLL0,1,2:
+   0 -> turned off, 1 -> turned on */
+#define PLLMODE_EN_BIT(n)       (1 << (n))
+
+/* Select PMS mode for PLL0,1:
+   0 -> mutiply mode (MM), 1 -> divide mode (DM) */
+#define PLLMODE_PMSMOD_BIT(n)   (1 << (4 + (n)))
+
+/* Select DMOSC for PLL2:
+   0 -> DMOSC_STD, 1 -> DMOSC_ALT */
+#define PLLMODE_PLL2DMOSC_BIT   (1 << 6)
+
+/* Select oscilator for CG16_SEL_OSC source:
+   0 -> S5L8702_OSC0, 1 -> S5L8702_OSC1 */
+#define PLLMODE_OSCSEL_BIT      (1 << 8)
+
+/* Select PLLxClk (a.k.a. "slow mode" (see s3c2440-DS) for PLL0,1,2:
+   O -> S5L8702_OSC1, 1 -> PLLxFreq */
+#define PLLMODE_PLLOUT_BIT(n)   (1 << (16 + (n)))
+
+/*
+ *  PLLMOD2
+ */
+/* Selects ALTOSCx for PLL0,1,2 when DMOSC == DMOSC_ALT:
+   0 -> S5L8702_ALTOSC0, 1 -> S5L8702_ALTOSC1 */
+#define PLLMOD2_ALTOSC_BIT(n)   (1 << (n))
+
+/* Selects DMOSC for PLL0,1:
+   0 -> DMOSC_STD, 1 -> DMOSC_ALT */
+#define PLLMOD2_DMOSC_BIT(n)    (1 << (4 + (n)))
+
+
+/* See s3c2440-DS (figure 7.2) for similar SoC reference.
+ *
+ * There are two different PMS modes, PLLxFreq is:
+ *   Divide Mode (DM):   (F_in * MDIV / PDIV) / 2^SDIV
+ *   Multiply Mode (MM): (F_in * MDIV * PDIV) / 2^SDIV
+ *
+ * PLL0 and PLL1 supports DM and MM, PLL2 only supports DM.
+ *
+ * MM uses S5L8702_OSC1. DM oscillator is selected using DMOSC_BIT
+ * and ALTOSC_BIT.
+ *
+ * PLLLOCK_LCK_BIT is not enabled when PLL gets locked, and being
+ * enabled doesn't meant that the PLL is locked. When using MULTIPLY
+ * mode, there is no (known) way to verify that the PLL is locked.
+ * On DIVIDE mode, PLLLOCK_DMLCK_BIT is enabled when the PLL is
+ * locked at the correct frequency.
+ * PLLLOCK_LCK_BIT is enabled only when lock_time expires, lock_time
+ * is configured in PLLCNT as ticks of PClk. The maximum needed time
+ * to get a good lock is ~300nS (TBC).
+ *
+ * TODO: F_vco notes
+ */
+#define PMSMOD_MUL  0
+#define PMSMOD_DIV  1
+#define GET_PMSMOD(pll) (((pll) == 2) \
+            ? PMSMOD_DIV \
+            : ((PLLMODE & PLLMODE_PMSMOD_BIT(pll)) ? PMSMOD_DIV \
+                                                   : PMSMOD_MUL))
+
+#define DMOSC_STD  0
+#define DMOSC_ALT  1
+#define GET_DMOSC(pll) ((((pll) == 2) \
+            ? (PLLMODE & PLLMODE_PLL2DMOSC_BIT) \
+            : (PLLMOD2 & PLLMOD2_DMOSC_BIT(pll))) ? DMOSC_ALT \
+                                                  : DMOSC_STD)
+
+/* available PLL operation modes */
+#define PLLOP_MM    0   /* Multiply Mode, F_in = S5L8702_OSC1 */
+#define PLLOP_DM    1   /* Divisor Mode,  F_in = S5L8702_OSC0 */
+#define PLLOP_ALT0  2   /* Divisor Mode,  F_in = S5L8702_ALTOSC0 */
+#define PLLOP_ALT1  3   /* Divisor Mode,  F_in = S5L8702_ALTOSC1 */
+
+
+/* These are real clock divisor values, to be encoded into registers
+ * as required. We are using fixed FClk:
+ *   FClk = CG16_SYS_SEL / fdiv, fdiv >= 1
+ * On Classic CG16_SYS_SEL = 216 MHz from PLL2, fdiv = 1.
+ */
+struct clocking_mode
+{
+    uint8_t cdiv;   /* CClk = FClk / cdiv, cdiv = 1,2,4,6,.. */
+    uint8_t hdiv;   /* HClk = FClk / hdiv, hdiv = 1,2,4,6,.. */
+    uint8_t hprat;  /* PClk = HClk / hprat, hprat >= 1 */
+    uint8_t hsdiv;  /* TBC: SM1_Clk = HClk / hsdiv, hsdiv >= 1 */
+};
+
+void clocking_init(struct clocking_mode *modes, int init_level);
+void set_clocking_level(int level);
+unsigned get_system_freqs(unsigned *cclk, unsigned *hclk, unsigned *pclk);
+
+/* debug */
+unsigned pll_get_cfg_freq(int pll);
+unsigned pll_get_out_freq(int pll);
+unsigned soc_get_oscsel_freq(void);
+int soc_get_hsdiv(void);
+
+#endif /* __CLOCKING_S5L8702_H */
diff --git a/firmware/target/arm/s5l8702/debug-s5l8702.c b/firmware/target/arm/s5l8702/debug-s5l8702.c
index a2c6c0b193..fe2044690d 100644
--- a/firmware/target/arm/s5l8702/debug-s5l8702.c
+++ b/firmware/target/arm/s5l8702/debug-s5l8702.c
@@ -33,6 +33,7 @@
 #ifdef HAVE_SERIAL
 #include "uc8702.h"
 #endif
+#include "clocking-s5l8702.h"
 
 #define DEBUG_CANCEL BUTTON_MENU
 
@@ -66,9 +67,10 @@ bool dbg_hw_info(void)
 
         if(state == 0)
         {
+            unsigned cpu_hz;
+            get_system_freqs(&cpu_hz, NULL, NULL);
             _DEBUG_PRINTF("CPU:");
-            _DEBUG_PRINTF("speed: %d MHz", ((CLKCON0 & 1) ?
-                                CPUFREQ_NORMAL : CPUFREQ_MAX) / 1000000);
+            _DEBUG_PRINTF("speed: %d MHz", cpu_hz / 1000000);
             _DEBUG_PRINTF("current_tick: %d", (unsigned int)current_tick);
             uint32_t __res;
             asm volatile ("mrc p15, 0, %0, c0, c0, 0" : "=r"(__res));
@@ -82,6 +84,31 @@ bool dbg_hw_info(void)
 
             _DEBUG_PRINTF("capture HW: %d", rec_hw_ver);
             line++;
+
+#ifdef CLOCKING_DEBUG
+            /* show all clocks */
+            unsigned f_clk, c_clk, h_clk, p_clk, l_clk, s_clk;
+
+            f_clk = get_system_freqs(&c_clk, &h_clk, &p_clk);
+            s_clk = h_clk / soc_get_hsdiv();
+            l_clk = h_clk >> ((LCD_CONFIG & 7) + 1); /* div = 2^(val+1) */
+
+            #define MHZ 1000000
+            #define TMHZ 100000
+            _DEBUG_PRINTF("Clocks (MHz):");
+            _DEBUG_PRINTF(" FClk: %d.%d",  f_clk / MHZ, (f_clk % MHZ) / TMHZ);
+            _DEBUG_PRINTF("  CPU: %d.%d",  c_clk / MHZ, (c_clk % MHZ) / TMHZ);
+            _DEBUG_PRINTF("  AHB: %d.%d",  h_clk / MHZ, (h_clk % MHZ) / TMHZ);
+            _DEBUG_PRINTF("   SM1: %d.%d", s_clk / MHZ, (s_clk % MHZ) / TMHZ);
+            _DEBUG_PRINTF("   LCD: %d.%d", l_clk / MHZ, (l_clk % MHZ) / TMHZ);
+            _DEBUG_PRINTF("  APB: %d.%d",  p_clk / MHZ, (p_clk % MHZ) / TMHZ);
+            line++;
+            _DEBUG_PRINTF("CG16_SEL_x (Hz):");
+            _DEBUG_PRINTF(" OSC: %d", soc_get_oscsel_freq());
+            for (int i = 0; i < 3; i++)
+                _DEBUG_PRINTF(" PLL%d: %d (%d)", i,
+                                pll_get_out_freq(i), pll_get_cfg_freq(i));
+#endif
         }
         else if(state==1)
         {
diff --git a/firmware/target/arm/s5l8702/system-s5l8702.c b/firmware/target/arm/s5l8702/system-s5l8702.c
index 014ed910de..3e84e5cf54 100644
--- a/firmware/target/arm/s5l8702/system-s5l8702.c
+++ b/firmware/target/arm/s5l8702/system-s5l8702.c
@@ -27,6 +27,7 @@
 #include "gpio-s5l8702.h"
 #include "dma-s5l8702.h"
 #include "uart-s5l8702.h"
+#include "clocking-s5l8702.h"
 
 #define default_interrupt(name) \
   extern __attribute__((weak,alias("UIRQ"))) void name (void)
@@ -180,9 +181,24 @@ void fiq_dummy(void)
     );
 }
 
+static struct clocking_mode clk_modes[] =
+{
+   /* cdiv  hdiv  hprat  hsdiv */    /* CClk  HClk  PClk  SM1Clk  FPS */
+    { 1,    2,    2,     4 },        /* 216   108   54    27      42  */
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+    { 4,    4,    2,     2 },        /* 54    54    27    27      21  */
+#endif
+};
+#define N_CLK_MODES (sizeof(clk_modes) / sizeof(struct clocking_mode))
+
+enum {
+    CLK_BOOST = 0,
+    CLK_UNBOOST = N_CLK_MODES - 1,
+};
 
 void system_init(void)
 {
+    clocking_init(clk_modes, 0);
     gpio_init();
     pmu_init();
     dma_init();
@@ -223,61 +239,22 @@ int system_memory_guard(int newmode)
 }
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
-
 void set_cpu_frequency(long frequency)
 {
     if (cpu_frequency == frequency)
         return;
 
-    /*
-     * CPU scaling parameters:
-     * CPUFREQ_MAX:    CPU = 216MHz, AHB = 108MHz, Vcore = 1.200V
-     * CPUFREQ_NORMAL: CPU =  54MHz, AHB =  54MHz, Vcore = 1.050V
-     *
-     * CLKCON0 sets PLL2->FCLK divider (CPU clock)
-     * CLKCON1 sets FCLK->HCLK divider (AHB clock)
-     *
-     * HCLK is derived from FCLK, the system goes unstable if HCLK
-     * is out of the range 54-108 MHz, so two stages are required to
-     * switch FCLK (216 MHz <-> 54 MHz), adjusting HCLK in between
-     * to ensure system stability.
-     */
     if (frequency == CPUFREQ_MAX)
     {
-        /* Vcore = 1.200V */
-        pmu_write(0x1e, 0x17);
-
-        /* FCLK = PLL2 / 2  (FCLK = 108MHz, HCLK = 108MHz) */
-        CLKCON0 = 0x3011;
-        udelay(50);
-
-        /* HCLK = FCLK / 2  (HCLK = 54MHz) */
-        CLKCON1 = 0x404101;
-        udelay(50);
-
-        /* FCLK = PLL2  (FCLK = 216MHz, HCLK = 108MHz) */
-        CLKCON0 = 0x3000;
-        udelay(100);
+        pmu_write(0x1e, 0x13);  /* Vcore = 1100 mV */
+        set_clocking_level(CLK_BOOST);
     }
     else
     {
-        /* FCLK = PLL2 / 2  (FCLK = 108MHz, HCLK = 54MHz) */
-        CLKCON0 = 0x3011;
-        udelay(50);
-
-        /* HCLK = FCLK  (HCLK = 108MHz) */
-        CLKCON1 = 0x4001;
-        udelay(50);
-
-        /* FCLK = PLL2 / 4  (FCLK = 54MHz, HCLK = 54MHz) */
-        CLKCON0 = 0x3013;
-        udelay(100);
-
-        /* Vcore = 1.050V */
-        pmu_write(0x1e, 0x11);
+        set_clocking_level(CLK_UNBOOST);
+        pmu_write(0x1e, 0xf);   /* Vcore = 1000 mV */
     }
 
     cpu_frequency = frequency;
 }
-
 #endif
diff --git a/firmware/target/arm/s5l8702/system-target.h b/firmware/target/arm/s5l8702/system-target.h
index ba05c2f6fb..43ab28d37b 100644
--- a/firmware/target/arm/s5l8702/system-target.h
+++ b/firmware/target/arm/s5l8702/system-target.h
@@ -24,7 +24,7 @@
 #include "system-arm.h"
 #include "mmu-arm.h"
 
-#define CPUFREQ_SLEEP      32768
+#define CPUFREQ_SLEEP   32768
 #define CPUFREQ_MAX     216000000
 #define CPUFREQ_DEFAULT 54000000
 #define CPUFREQ_NORMAL  54000000