- unsigned char i=0, j=0, reg_offset = 0, gpio_core;
- unsigned long gpio_reg, gpio_core_add;
-
- for (gpio_core=0; gpio_core<GPIO_GROUP_MAX; gpio_core++) {
- j = 0;
- reg_offset = 0;
- /* GPIO config of the GPIOs 0 to 31 */
- for (i=0; i<GPIO_MAX; i++, j++) {
- if (i == GPIO_MAX/2) {
- reg_offset = 4;
- j = i-16;
- }
-
- gpio_core_add = (*gpio_tab)[gpio_core][i].add;
-
- if (((*gpio_tab)[gpio_core][i].in_out == GPIO_IN) ||
- ((*gpio_tab)[gpio_core][i].in_out == GPIO_BI)) {
-
- switch ((*gpio_tab)[gpio_core][i].alt_nb) {
- case GPIO_SEL:
- break;
-
- case GPIO_ALT1:
- gpio_reg = in32(GPIO_IS1(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
- out32(GPIO_IS1(gpio_core_add+reg_offset), gpio_reg);
- break;
-
- case GPIO_ALT2:
- gpio_reg = in32(GPIO_IS2(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
- out32(GPIO_IS2(gpio_core_add+reg_offset), gpio_reg);
- break;
-
- case GPIO_ALT3:
- gpio_reg = in32(GPIO_IS3(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_IN_SEL >> (j*2));
- out32(GPIO_IS3(gpio_core_add+reg_offset), gpio_reg);
- break;
- }
- }
-
- if (((*gpio_tab)[gpio_core][i].in_out == GPIO_OUT) ||
- ((*gpio_tab)[gpio_core][i].in_out == GPIO_BI)) {
-
- switch ((*gpio_tab)[gpio_core][i].alt_nb) {
- case GPIO_SEL:
- if (gpio_core == GPIO0) {
- gpio_reg = in32(GPIO0_TCR) | (0x80000000 >> (j));
- out32(GPIO0_TCR, gpio_reg);
- }
-
- if (gpio_core == GPIO1) {
- gpio_reg = in32(GPIO1_TCR) | (0x80000000 >> (j));
- out32(GPIO1_TCR, gpio_reg);
- }
-
- gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
- gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
- break;
-
- case GPIO_ALT1:
- gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_ALT1_SEL >> (j*2));
- out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
- gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_ALT1_SEL >> (j*2));
- out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
- break;
-
- case GPIO_ALT2:
- gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_ALT2_SEL >> (j*2));
- out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
- gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_ALT2_SEL >> (j*2));
- out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
- break;
-
- case GPIO_ALT3:
- gpio_reg = in32(GPIO_OS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_ALT3_SEL >> (j*2));
- out32(GPIO_OS(gpio_core_add+reg_offset), gpio_reg);
- gpio_reg = in32(GPIO_TS(gpio_core_add+reg_offset))
- & ~(GPIO_MASK >> (j*2));
- gpio_reg = gpio_reg | (GPIO_ALT3_SEL >> (j*2));
- out32(GPIO_TS(gpio_core_add+reg_offset), gpio_reg);
- break;
- }
- }
+#if defined(CONFIG_440EPX)
+ int reset_needed = 0;
+ u32 reg, temp;
+ u32 prbdv0, target_prbdv0, /* CLK_PRIMBD */
+ fwdva, target_fwdva, fwdvb, target_fwdvb, /* CLK_PLLD */
+ fbdv, target_fbdv, lfbdv, target_lfbdv,
+ perdv0, target_perdv0, /* CLK_PERD */
+ spcid0, target_spcid0; /* CLK_SPCID */
+
+ /* Reconfigure clocks if necessary.
+ * See PPC440EPx User's Manual, sections 8.2 and 14 */
+ if (new_cpu_freq == 667) {
+ target_prbdv0 = 2;
+ target_fwdva = 2;
+ target_fwdvb = 4;
+ target_fbdv = 20;
+ target_lfbdv = 1;
+ target_perdv0 = 4;
+ target_spcid0 = 4;
+
+ mfcpr(clk_primbd, reg);
+ temp = (reg & PRBDV_MASK) >> 24;
+ prbdv0 = temp ? temp : 8;
+ if (prbdv0 != target_prbdv0) {
+ reg &= ~PRBDV_MASK;
+ reg |= ((target_prbdv0 == 8 ? 0 : target_prbdv0) << 24);
+ mtcpr(clk_primbd, reg);
+ reset_needed = 1;