Fix cirrus voltage detection (for CPC45)

1 files changed, 31 insertions, 38 deletions

diff --git a/drivers/i82365.c b/drivers/i82365.c
index e20fd33..a40fcf4 100644
--- a/drivers/i82365.c
+++ b/drivers/i82365.c
@@ -1,5 +1,5 @@
 /*
- * (C) Copyright 2003-2004
+ * (C) Copyright 2003-2005
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
  *
  * See file CREDITS for list of people who contributed to this
@@ -443,31 +443,6 @@ static int cb_set_power (socket_info_t * s, socket_state_t * state)
 
 #ifdef CONFIG_CPC45
 
-	if ((state->Vcc == 0) && (state->Vpp == 0)) {
-		u_char power, vcc, vpp;
-
-		power = i365_get (s, I365_POWER);
-		state->flags |= (power & I365_PWR_AUTO) ? SS_PWR_AUTO : 0;
-		state->flags |= (power & I365_PWR_OUT) ? SS_OUTPUT_ENA : 0;
-		vcc = power & I365_VCC_MASK;
-		vpp = power & I365_VPP1_MASK;
-		state->Vcc = state->Vpp = 0;
-		if (i365_get (s, PD67_MISC_CTL_1) & PD67_MC1_VCC_3V) {
-			if (power & I365_VCC_5V)
-				state->Vcc = 33;
-			if (vpp == I365_VPP1_5V)
-				state->Vpp = 33;
-		} else {
-			if (power & I365_VCC_5V)
-				state->Vcc = 50;
-			if (vpp == I365_VPP1_5V)
-				state->Vpp = 50;
-		}
-		if (power == I365_VPP1_12V)
-			state->Vpp = 120;
-		printf ("POWER Vcc:%d Vpp: %d\n", state->Vcc, state->Vpp);
-	}
-
 	reg = I365_PWR_NORESET;
 	if (state->flags & SS_PWR_AUTO)
 		reg |= I365_PWR_AUTO;
@@ -497,8 +472,11 @@ static int cb_set_power (socket_info_t * s, socket_state_t * state)
 			return -1;
 		}
 	}
-	if (reg != i365_get (s, I365_POWER))
+
+	if (reg != i365_get (s, I365_POWER)) {
+		reg = (I365_PWR_OUT | I365_PWR_NORESET | I365_VCC_5V | I365_VPP1_5V);
 		i365_set (s, I365_POWER, reg);
+	}
 
 #else	/* ! CONFIG_CPC45 */
 
@@ -579,6 +557,12 @@ static void set_bridge_opts (socket_info_t * s)
 }
 
 /*====================================================================*/
+#define PD67_EXT_INDEX		0x2e	/* Extension index */
+#define PD67_EXT_DATA		0x2f	/* Extension data */
+#define PD67_EXD_VS1(s)		(0x01 << ((s)<<1))
+
+#define pd67_ext_get(s, r) \
+    (i365_set(s, PD67_EXT_INDEX, r), i365_get(s, PD67_EXT_DATA))
 
 static int i365_get_status (socket_info_t * s, u_int * value)
 {
@@ -586,6 +570,7 @@ static int i365_get_status (socket_info_t * s, u_int * value)
 #ifdef CONFIG_CPC45
 	u_char val;
 	u_char power, vcc, vpp;
+	u_int powerstate;
 #endif
 
 	status = i365_get (s, I365_IDENT);
@@ -620,24 +605,32 @@ static int i365_get_status (socket_info_t * s, u_int * value)
 		return -1;
 	}
 
-	i365_bset (s, I365_POWER, I365_VCC_5V);
 	power = i365_get (s, I365_POWER);
 	state.flags |= (power & I365_PWR_AUTO) ? SS_PWR_AUTO : 0;
 	state.flags |= (power & I365_PWR_OUT) ? SS_OUTPUT_ENA : 0;
 	vcc = power & I365_VCC_MASK;
 	vpp = power & I365_VPP1_MASK;
 	state.Vcc = state.Vpp = 0;
-	if (i365_get (s, PD67_MISC_CTL_1) & PD67_MC1_VCC_3V) {
-		if (power & I365_VCC_5V)
-			state.Vcc = 33;
-		if (vpp == I365_VPP1_5V)
-			state.Vpp = 33;
-	} else {
-		if (power & I365_VCC_5V)
-			state.Vcc = 50;
-		if (vpp == I365_VPP1_5V)
-			state.Vpp = 50;
+	if((vcc== 0) || (vpp == 0)) {
+		/*
+		 * On the Cirrus we get the info which card voltage
+		 * we have in EXTERN DATA and write it to MISC_CTL1
+		 */
+		powerstate = pd67_ext_get(s, PD67_EXTERN_DATA);
+		if (powerstate & PD67_EXD_VS1(0)) {
+			/* 5V Card */
+			i365_bclr (s, PD67_MISC_CTL_1, PD67_MC1_VCC_3V);
+		} else {
+			/* 3.3V Card */
+			i365_bset (s, PD67_MISC_CTL_1, PD67_MC1_VCC_3V);
+		}
+		i365_set (s, I365_POWER, (I365_PWR_OUT | I365_PWR_NORESET | I365_VCC_5V | I365_VPP1_5V));
+		power = i365_get (s, I365_POWER);
 	}
+	if (power & I365_VCC_5V) {
+		state.Vcc = (i365_get(s, PD67_MISC_CTL_1) & PD67_MC1_VCC_3V) ? 33 : 50;
+	}
+
 	if (power == I365_VPP1_12V)
 		state.Vpp = 120;