powerpc/p2041rdb: set sysclk according to status of physical switch SW1

P2041RDB supports 3 sysclk frequencies, it's selected by SW1[6~8],
software need to read the SW1 status to decide what the sysclk needs.

SW1[8~6] : frequency
0 0 1 : 83.3MHz
0 1 0 : 100MHz
others: 66.667MHz

Signed-off-by: Shaohui Xie <Shaohui.Xie@freescale.com>
Signed-off-by: Kumar Gala <galak@kernel.crashing.org>

diff --git a/board/freescale/p2041rdb/cpld.h b/board/freescale/p2041rdb/cpld.h
index dcdb410..bece5dd 100644 (file)
--- a/board/freescale/p2041rdb/cpld.h
+++ b/board/freescale/p2041rdb/cpld.h
@@ -30,6 +30,7 @@ typedef struct cpld_data {
        u8 serdes_mux;          /* 0xc - Multiplexed pin Select Register */
        u8 sw[1];               /* 0xd - SW2 Status */
        u8 system_rst_default;  /* 0xe - system reset to default register */
+       u8 sysclk_sw1;          /* 0xf - sysclk configuration register */
 } __attribute__ ((packed)) cpld_data_t;
 
 #define SERDES_MUX_LANE_6_MASK 0x2
@@ -41,6 +42,8 @@ typedef struct cpld_data {
 #define SERDES_MUX_LANE_D_MASK 0x8
 #define SERDES_MUX_LANE_D_SHIFT        3
 #define CPLD_SWITCH_BANK_ENABLE        0x40
+#define CPLD_SYSCLK_83         0x1     /* system clock 83.3MHz */
+#define CPLD_SYSCLK_100                0x2     /* system clock 100MHz */
 
 /* Pointer to the CPLD register set */
 #define cpld ((cpld_data_t *)CPLD_BASE)

diff --git a/board/freescale/p2041rdb/p2041rdb.c b/board/freescale/p2041rdb/p2041rdb.c
index 6ed404f..6461bd7 100644 (file)
--- a/board/freescale/p2041rdb/p2041rdb.c
+++ b/board/freescale/p2041rdb/p2041rdb.c
@@ -130,6 +130,20 @@ int board_early_init_r(void)
        return 0;
 }
 
+unsigned long get_board_sys_clk(unsigned long dummy)
+{
+       u8 sysclk_conf = CPLD_READ(sysclk_sw1);
+
+       switch (sysclk_conf & 0x7) {
+       case CPLD_SYSCLK_83:
+               return 83333333;
+       case CPLD_SYSCLK_100:
+               return 100000000;
+       default:
+               return 66666666;
+       }
+}
+
 static const char *serdes_clock_to_string(u32 clock)
 {
        switch (clock) {

diff --git a/include/configs/P2041RDB.h b/include/configs/P2041RDB.h
index 37e98bb..c7f0761 100644 (file)
--- a/include/configs/P2041RDB.h
+++ b/include/configs/P2041RDB.h
@@ -102,7 +102,10 @@
        #define CONFIG_ENV_SECT_SIZE    0x20000 /* 128K (one sector) */
 #endif
 
-#define CONFIG_SYS_CLK_FREQ    66666666
+#ifndef __ASSEMBLY__
+unsigned long get_board_sys_clk(unsigned long dummy);
+#endif
+#define CONFIG_SYS_CLK_FREQ    get_board_sys_clk(0)
 
 /*
  * These can be toggled for performance analysis, otherwise use default.