servo_v4: update uservo port init for tca6416

Make sure that ioexpander is set to output,
and that the uservo usb power enable is set.

BUG=chromium:651860
TEST=check that servo micro is initialized properly
BRANCH=None

Change-Id: Iff994c63cd333933d51db38202a41b7b6fc86d66
Signed-off-by: Nick Sanders <nsanders@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/395186
Reviewed-by: Kevin Cheng <kevcheng@chromium.org>

1 files changed, 41 insertions, 9 deletions

diff --git a/board/servo_v4/board.c b/board/servo_v4/board.c
index ecfc5fbf64..03410442db 100644
--- a/board/servo_v4/board.c
+++ b/board/servo_v4/board.c
@@ -226,10 +226,48 @@ const int num_rw_sections = ARRAY_SIZE(board_rw_sections);
 /******************************************************************************
  * Initialize board.
  */
-static void board_init(void)
+
+/* Write a GPIO output on the tca6416 I2C ioexpander. */
+static void write_ioexpander(int bank, int gpio, int val)
 {
 	int tmp;
 
+	/* Read output port register */
+	i2c_read8(1, 0x40, 0x2 + bank, &tmp);
+	if (val)
+		tmp |= (1 << gpio);
+	else
+		tmp &= ~(1 << gpio);
+	/* Write back modified output port register */
+	i2c_write8(1, 0x40, 0x2 + bank, tmp);
+
+	/* Set Configuration port to output/0 */
+	i2c_read8(1, 0x40, 0x6 + bank, &tmp);
+	i2c_write8(1, 0x40, 0x6 + bank, tmp & ~(1 << gpio));
+}
+
+/* Enable uservo USB. */
+static void init_uservo_port(void)
+{
+	/* Write USERVO_POWER_EN */
+	write_ioexpander(0, 7, 1);
+	/* Write USERVO_FASTBOOT_MUX_SEL */
+	write_ioexpander(1, 0, 0);
+}
+
+/* Enable all ioexpander outputs. */
+static void init_ioexpander(void)
+{
+	/* Write all GPIO to output 0 */
+	i2c_write8(1, 0x40, 0x2, 0x0);
+	i2c_write8(1, 0x40, 0x3, 0x0);
+	/* Write all GPIO to output direction */
+	i2c_write8(1, 0x40, 0x6, 0x0);
+	i2c_write8(1, 0x40, 0x7, 0x0);
+}
+
+static void board_init(void)
+{
 	/* USB to serial queues */
 	queue_init(&usart3_to_usb);
 	queue_init(&usb_to_usart3);
@@ -248,13 +286,7 @@ static void board_init(void)
 	i2c_write8(1, 0x20, 0x0, 0x20);
 
 	/* Enable uservo USB by default. */
-	/* Write USERVO_POWER_EN */
-	i2c_write8(1, 0x40, 0x1, 0xff | (1 << 7));
-	i2c_read8(1, 0x40, 0x3, &tmp);
-	i2c_write8(1, 0x40, 0x3, tmp & ~(1 << 7));
-	/* Write USERVO_FASTBOOT_MUX_SEL */
-	i2c_write8(1, 0x40, 0x0, 0xff & ~(1 << 0));
-	i2c_read8(1, 0x40, 0x2, &tmp);
-	i2c_write8(1, 0x40, 0x2, tmp & ~(1 << 0));
+	init_ioexpander();
+	init_uservo_port();
 }
 DECLARE_HOOK(HOOK_INIT, board_init, HOOK_PRIO_DEFAULT);