1 files changed, 52 insertions, 76 deletions

diff --git a/board/keymile/kmp204x/kmp204x.c b/board/keymile/kmp204x/kmp204x.c
index f02642aece..95a19cdb2c 100644
--- a/board/keymile/kmp204x/kmp204x.c
+++ b/board/keymile/kmp204x/kmp204x.c
@@ -33,12 +33,51 @@ int checkboard(void)
 	return 0;
 }
 
-/* TODO: implement the I2C deblocking function */
-int i2c_make_abort(void)
+/* I2C deblocking uses the algorithm defined in board/keymile/common/common.c
+ * 2 dedicated QRIO GPIOs externally pull the SCL and SDA lines
+ * For I2C only the low state is activly driven and high state is pulled-up
+ * by a resistor. Therefore the deblock GPIOs are used
+ *  -> as an active output to drive a low state
+ *  -> as an open-drain input to have a pulled-up high state
+ */
+
+/* QRIO GPIOs used for deblocking */
+#define DEBLOCK_PORT1	GPIO_A
+#define DEBLOCK_SCL1	20
+#define DEBLOCK_SDA1	21
+
+/* By default deblock GPIOs are floating */
+static void i2c_deblock_gpio_cfg(void)
+{
+	/* set I2C bus 1 deblocking GPIOs input, but 0 value for open drain */
+	qrio_gpio_direction_input(DEBLOCK_PORT1, DEBLOCK_SCL1);
+	qrio_gpio_direction_input(DEBLOCK_PORT1, DEBLOCK_SDA1);
+
+	qrio_set_gpio(DEBLOCK_PORT1, DEBLOCK_SCL1, 0);
+	qrio_set_gpio(DEBLOCK_PORT1, DEBLOCK_SDA1, 0);
+}
+
+void set_sda(int state)
+{
+	qrio_set_opendrain_gpio(DEBLOCK_PORT1, DEBLOCK_SDA1, state);
+}
+
+void set_scl(int state)
+{
+	qrio_set_opendrain_gpio(DEBLOCK_PORT1, DEBLOCK_SCL1, state);
+}
+
+int get_sda(void)
+{
+	return qrio_get_gpio(DEBLOCK_PORT1, DEBLOCK_SDA1);
+}
+
+int get_scl(void)
 {
-	return 1;
+	return qrio_get_gpio(DEBLOCK_PORT1, DEBLOCK_SCL1);
 }
 
+
 #define ZL30158_RST	8
 #define ZL30343_RST	9
 
@@ -62,6 +101,7 @@ int board_early_init_f(void)
 
 int board_early_init_r(void)
 {
+	int ret = 0;
 	/* Flush d-cache and invalidate i-cache of any FLASH data */
 	flush_dcache();
 	invalidate_icache();
@@ -69,7 +109,11 @@ int board_early_init_r(void)
 	set_liodns();
 	setup_portals();
 
-	return 0;
+	ret = trigger_fpga_config();
+	if (ret)
+		printf("error triggering PCIe FPGA config\n");
+
+	return ret;
 }
 
 unsigned long get_board_sys_clk(unsigned long dummy)
@@ -77,80 +121,12 @@ unsigned long get_board_sys_clk(unsigned long dummy)
 	return 66666666;
 }
 
-#define WDMASK_OFF	0x16
-
-static void qrio_wdmask(u8 bit, bool wden)
-{
-	u16 wdmask;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	wdmask = in_be16(qrio_base + WDMASK_OFF);
-
-	if (wden)
-		wdmask |= (1 << bit);
-	else
-		wdmask &= ~(1 << bit);
-
-	out_be16(qrio_base + WDMASK_OFF, wdmask);
-}
-
-#define PRST_OFF	0x1a
-
-void qrio_prst(u8 bit, bool en, bool wden)
-{
-	u16 prst;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	qrio_wdmask(bit, wden);
-
-	prst = in_be16(qrio_base + PRST_OFF);
-
-	if (en)
-		prst &= ~(1 << bit);
-	else
-		prst |= (1 << bit);
-
-	out_be16(qrio_base + PRST_OFF, prst);
-}
-
-#define PRSTCFG_OFF	0x1c
-
-void qrio_prstcfg(u8 bit, u8 mode)
-{
-	u32 prstcfg;
-	u8 i;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	prstcfg = in_be32(qrio_base + PRSTCFG_OFF);
-
-	for (i = 0; i < 2; i++) {
-		if (mode & (1<<i))
-			set_bit(2*bit+i, &prstcfg);
-		else
-			clear_bit(2*bit+i, &prstcfg);
-	}
-
-	out_be32(qrio_base + PRSTCFG_OFF, prstcfg);
-}
-
-
-#define BOOTCOUNT_OFF	0x12
-
-void bootcount_store(ulong counter)
+int misc_init_f(void)
 {
-	u8 val;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-
-	val = (counter <= 255) ? (u8)counter : 255;
-	out_8(qrio_base + BOOTCOUNT_OFF, val);
-}
+	/* configure QRIO pis for i2c deblocking */
+	i2c_deblock_gpio_cfg();
 
-ulong bootcount_load(void)
-{
-	u8 val;
-	void __iomem *qrio_base = (void *)CONFIG_SYS_QRIO_BASE;
-	val = in_8(qrio_base + BOOTCOUNT_OFF);
-	return val;
+	return 0;
 }
 
 #define NUM_SRDS_BANKS	2