arm, am335x: Enable Spread Spectrum for the MPU

Enable Spread Spectrum for the MPU by calculating the required
values and setting the registers accordingly.

Signed-off-by: Heiko Schocher <hs@denx.de>
Reviewed-by: Tom Rini <trini@konsulko.com>

arch/arm/cpu/armv7/am33xx/clock_am33xx.c

@@ -159,3 +159,76 @@ void enable_basic_clocks(void)
 	/* Select the Master osc 24 MHZ as Timer2 clock source */
 	writel(0x1, &cmdpll->clktimer2clk);
 }
+
+/*
+ * Enable Spread Spectrum for the MPU by calculating the required
+ * values and setting the registers accordingly.
+ * @param permille The spreading in permille (10th of a percent)
+ */
+void set_mpu_spreadspectrum(int permille)
+{
+	u32 multiplier_m;
+	u32 predivider_n;
+	u32 cm_clksel_dpll_mpu;
+	u32 cm_clkmode_dpll_mpu;
+	u32 ref_clock;
+	u32 pll_bandwidth;
+	u32 mod_freq_divider;
+	u32 exponent;
+	u32 mantissa;
+	u32 delta_m_step;
+
+	printf("Enabling Spread Spectrum of %d permille for MPU\n",
+	       permille);
+
+	/* Read PLL parameter m and n */
+	cm_clksel_dpll_mpu = readl(&cmwkup->clkseldpllmpu);
+	multiplier_m = (cm_clksel_dpll_mpu >> 8) & 0x3FF;
+	predivider_n = cm_clksel_dpll_mpu & 0x7F;
+
+	/*
+	 * Calculate reference clock (clock after pre-divider),
+	 * its max. PLL bandwidth,
+	 * and resulting mod_freq_divider
+	 */
+	ref_clock = V_OSCK / (predivider_n + 1);
+	pll_bandwidth = ref_clock / 70;
+	mod_freq_divider = ref_clock / (4 * pll_bandwidth);
+
+	/* Calculate Mantissa/Exponent */
+	exponent = 0;
+	mantissa = mod_freq_divider;
+	while ((mantissa > 127) && (exponent < 7)) {
+		exponent++;
+		mantissa /= 2;
+	}
+	if (mantissa > 127)
+		mantissa = 127;
+
+	mod_freq_divider = mantissa << exponent;
+
+	/*
+	 * Calculate Modulation steps
+	 * As we use Downspread only, the spread is twice the value of
+	 * permille, so Div2!
+	 * As it takes the value in percent, divide by ten!
+	 */
+	delta_m_step = ((u32)((multiplier_m * permille) / 10 / 2)) << 18;
+	delta_m_step /= 100;
+	delta_m_step /= mod_freq_divider;
+	if (delta_m_step > 0xFFFFF)
+		delta_m_step = 0xFFFFF;
+
+	/* Setup Spread Spectrum */
+	writel(delta_m_step, &cmwkup->sscdeltamstepdllmpu);
+	writel((exponent << 8) | mantissa, &cmwkup->sscmodfreqdivdpllmpu);
+	cm_clkmode_dpll_mpu = readl(&cmwkup->clkmoddpllmpu);
+	/* clear all SSC flags */
+	cm_clkmode_dpll_mpu &= ~(0xF << CM_CLKMODE_DPLL_SSC_EN_SHIFT);
+	/* enable SSC with Downspread only */
+	cm_clkmode_dpll_mpu |=  CM_CLKMODE_DPLL_SSC_EN_MASK |
+				CM_CLKMODE_DPLL_SSC_DOWNSPREAD_MASK;
+	writel(cm_clkmode_dpll_mpu, &cmwkup->clkmoddpllmpu);
+	while (!(readl(&cmwkup->clkmoddpllmpu) & 0x2000))
+		;
+}

arch/arm/include/asm/arch-am33xx/clock.h

@@ -117,4 +117,5 @@ void enable_basic_clocks(void);
 void do_enable_clocks(u32 *const *, u32 *const *, u8);
 void do_disable_clocks(u32 *const *, u32 *const *, u8);
 
+void set_mpu_spreadspectrum(int permille);
 #endif

arch/arm/include/asm/arch-am33xx/cpu.h

@@ -99,7 +99,8 @@ struct cm_wkuppll {
 	unsigned int timer0clkctrl;	/* offset 0x10 */
 	unsigned int resv2[3];
 	unsigned int idlestdpllmpu;	/* offset 0x20 */
-	unsigned int resv3[2];
+	unsigned int sscdeltamstepdllmpu; /* off  0x24 */
+	unsigned int sscmodfreqdivdpllmpu; /* off 0x28 */
 	unsigned int clkseldpllmpu;	/* offset 0x2c */
 	unsigned int resv4[1];
 	unsigned int idlestdpllddr;	/* offset 0x34 */