Implement SBU swapping for CSv2

Signed-off-by: Marc Zyngier <maz@kernel.org>

3 files changed, 25 insertions, 8 deletions

diff --git a/m1-pd-bmc.h b/m1-pd-bmc.h
index d93430a..6c08739 100644
--- a/m1-pd-bmc.h
+++ b/m1-pd-bmc.h
@@ -25,6 +25,7 @@ enum m1_pd_bmc_pins {
 	FUSB_VBUS,
 	UART_TX,
 	UART_RX,
+	SBU_SWAP,
 };
 
 struct hw_context {
diff --git a/start.c b/start.c
index 895f7ab..633e14b 100644
--- a/start.c
+++ b/start.c
@@ -39,6 +39,11 @@ static const struct gpio_pin_config m1_pd_bmc_pin_config0[] = {
 		.pin	= 13,
 		.mode	= GPIO_FUNC_UART,
 	},
+	[SBU_SWAP] = {
+		.pin	= 20,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_OUT,
+	},
 };
 
 static const struct gpio_pin_config m1_pd_bmc_pin_config1[] = {
@@ -74,6 +79,11 @@ static const struct gpio_pin_config m1_pd_bmc_pin_config1[] = {
 		.pin	= 9,
 		.mode	= GPIO_FUNC_UART,
 	},
+	[SBU_SWAP] = {
+		.pin	= 21,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_OUT,
+	},
 };
 
 static void __not_in_flash_func(uart_irq_fn)(int port,
diff --git a/vdmtool.c b/vdmtool.c
index a4df405..4c5117b 100644
--- a/vdmtool.c
+++ b/vdmtool.c
@@ -31,6 +31,7 @@ struct vdm_context {
 	volatile bool			pending;
 	bool 				verbose;
 	bool				vdm_escape;
+	bool				cc_line;
 	uint8_t				serial_pin_set;
 };
 
@@ -100,18 +101,15 @@ static void evt_dfpconnect(struct vdm_context *cxt)
 
 	fusb302_pd_reset(PORT(cxt));
 	fusb302_tcpm_set_msg_header(PORT(cxt), 1, 1);	// Source
-	if (cc1 > cc2) {
-		fusb302_tcpm_set_polarity(PORT(cxt), 0);
-		cprintf(cxt, "Polarity: CC1 (normal)\n");
-	} else {
-		fusb302_tcpm_set_polarity(PORT(cxt), 1);
-		cprintf(cxt, "Polarity: CC2 (flipped)\n");
-	}
+	cxt->cc_line = !(cc1 > cc2);
+	fusb302_tcpm_set_polarity(PORT(cxt), cxt->cc_line);
+	cprintf(cxt, "Polarity: CC%d (%s)\n",
+		(int)cxt->cc_line + 1, cxt->cc_line ? "flipped" : "normal");
 
 	/* If none of the CCs are disconnected, enable VCONN */
 	if (cc1 && cc2) {
 		fusb302_tcpm_set_vconn(PORT(cxt), 1);
-		cprintf(cxt, "VCONN on CC%d\n", (cc2 < cc1) + 1);
+		cprintf(cxt, "VCONN on CC%d\n", (int)cxt->cc_line + 1);
 	}
 
 	fusb302_tcpm_set_rx_enable(PORT(cxt), 1);
@@ -427,6 +425,7 @@ static void vdm_claim_serial(struct vdm_context *cxt)
 	static const char *pinsets[] = {
 		"AltUSB", "PrimUSB", "SBU1/2",
 	};
+	bool sbu_swap;
 
 	//uint32_t vdm[] = { 0x5ac8010 }; // Get Action List
 	//uint32_t vdm[] = { 0x5ac8012, 0x0105, 0x8002<<16 }; // PMU Reset + DFU Hold
@@ -440,6 +439,12 @@ static void vdm_claim_serial(struct vdm_context *cxt)
 
 	vdm_send_msg(cxt, vdm, ARRAY_SIZE(vdm));
 	cprintf(cxt, ">VDM serial -> %s\n", pinsets[cxt->serial_pin_set]);
+
+	/* If using the SBU pins, swap the pins if using CC2. */
+	sbu_swap = (cxt->serial_pin_set == 2) ? cxt->cc_line : LOW;
+
+	gpio_put(PIN(cxt, SBU_SWAP), sbu_swap);
+	dprintf(cxt, "SBU_SWAP = %d\n", sbu_swap);
 }
 
 void vdm_send_reboot(struct vdm_context *cxt)
@@ -658,6 +663,7 @@ void m1_pd_bmc_fusb_setup(unsigned int port,
 	}
 
 	gpio_put(PIN(cxt, LED_G), HIGH);
+	gpio_put(PIN(cxt, SBU_SWAP), LOW);
 	vbus_off(cxt);
 
 	tcpc_read(PORT(cxt), TCPC_REG_DEVICE_ID, &reg);