mb/lenovo/t60,x60: Split dock_(dis)connect() function

Avoid calling a function named mainboard_io_trap_handler() when
the dock (dis)connect is not triggered from IO trap.

Change-Id: Idc258a390f2de2c32d38a0e35fcce896d058d1b9
Signed-off-by: Kyösti Mälkki <kyosti.malkki@gmail.com>
Reviewed-on: https://review.coreboot.org/c/coreboot/+/70363
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Angel Pons <th3fanbus@gmail.com>
Reviewed-by: Arthur Heymans <arthur@aheymans.xyz>

2 files changed, 57 insertions, 37 deletions

diff --git a/src/mainboard/lenovo/t60/smihandler.c b/src/mainboard/lenovo/t60/smihandler.c
index 12a1e53be0..e87362bd92 100644
--- a/src/mainboard/lenovo/t60/smihandler.c
+++ b/src/mainboard/lenovo/t60/smihandler.c
@@ -45,32 +45,42 @@ static void mainboard_smi_brightness_up(void)
 		*(bar+LVTMA_BL_MOD_LEVEL) += 0x10;
 }
 
+static void mainboard_smi_dock_connect(void)
+{
+	/* If there's an legacy I/O module present, we're not
+	 * allowed to connect the Docking LPC Bus, as both Super I/O
+	 * chips are using 0x2e as base address.
+	 */
+	if (legacy_io_present())
+		return;
+
+	if (!dock_connect()) {
+		/* set dock LED to indicate status */
+		ec_write(0x0c, 0x08);
+		ec_write(0x0c, 0x89);
+	} else {
+		/* blink dock LED to indicate failure */
+		ec_write(0x0c, 0xc8);
+		ec_write(0x0c, 0x09);
+	}
+}
+
+static void mainboard_smi_dock_disconnect(void)
+{
+	dock_disconnect();
+	ec_write(0x0c, 0x09);
+	ec_write(0x0c, 0x08);
+}
+
 int mainboard_io_trap_handler(int smif)
 {
 	switch (smif) {
 	case SMI_DOCK_CONNECT:
-		/* If there's an legacy I/O module present, we're not
-		 * allowed to connect the Docking LPC Bus, as both Super I/O
-		 * chips are using 0x2e as base address.
-		 */
-		if (legacy_io_present())
-			break;
-
-		if (!dock_connect()) {
-			/* set dock LED to indicate status */
-			ec_write(0x0c, 0x08);
-			ec_write(0x0c, 0x89);
-		} else {
-			/* blink dock LED to indicate failure */
-			ec_write(0x0c, 0xc8);
-			ec_write(0x0c, 0x09);
-		}
+		mainboard_smi_dock_connect();
 		break;
 
 	case SMI_DOCK_DISCONNECT:
-		dock_disconnect();
-		ec_write(0x0c, 0x09);
-		ec_write(0x0c, 0x08);
+		mainboard_smi_dock_disconnect();
 		break;
 
 	case SMI_BRIGHTNESS_UP:
@@ -116,11 +126,11 @@ static void mainboard_smi_handle_ec_sci(void)
 		case 0x27:
 		/* undock event */
 		case 0x50:
-			mainboard_io_trap_handler(SMI_DOCK_DISCONNECT);
+			mainboard_smi_dock_disconnect();
 			break;
 		/* dock event */
 		case 0x37:
-			mainboard_io_trap_handler(SMI_DOCK_CONNECT);
+			mainboard_smi_dock_connect();
 			break;
 		default:
 			break;
diff --git a/src/mainboard/lenovo/x60/smihandler.c b/src/mainboard/lenovo/x60/smihandler.c
index e1a97f57ee..13c6406c4d 100644
--- a/src/mainboard/lenovo/x60/smihandler.c
+++ b/src/mainboard/lenovo/x60/smihandler.c
@@ -29,27 +29,37 @@ static void mainboard_smi_save_cmos(void)
 	outb(tmp72, 0x72);
 }
 
+static void mainboard_smi_dock_connect(void)
+{
+	ec_clr_bit(0x03, 2);
+	mdelay(250);
+	if (!dock_connect()) {
+		ec_set_bit(0x03, 2);
+		/* set dock LED to indicate status */
+		ec_write(0x0c, 0x09);
+		ec_write(0x0c, 0x88);
+	} else {
+		/* blink dock LED to indicate failure */
+		ec_write(0x0c, 0x08);
+		ec_write(0x0c, 0xc9);
+	}
+}
+
+static void mainboard_smi_dock_disconnect(void)
+{
+	ec_clr_bit(0x03, 2);
+	dock_disconnect();
+}
+
 int mainboard_io_trap_handler(int smif)
 {
 	switch (smif) {
 	case SMI_DOCK_CONNECT:
-		ec_clr_bit(0x03, 2);
-		mdelay(250);
-		if (!dock_connect()) {
-			ec_set_bit(0x03, 2);
-			/* set dock LED to indicate status */
-			ec_write(0x0c, 0x09);
-			ec_write(0x0c, 0x88);
-		} else {
-			/* blink dock LED to indicate failure */
-			ec_write(0x0c, 0x08);
-			ec_write(0x0c, 0xc9);
-		}
+		mainboard_smi_dock_connect();
 		break;
 
 	case SMI_DOCK_DISCONNECT:
-		ec_clr_bit(0x03, 2);
-		dock_disconnect();
+		mainboard_smi_dock_disconnect();
 		break;
 
 	case SMI_SAVE_CMOS:
@@ -108,12 +118,12 @@ static void mainboard_smi_handle_ec_sci(void)
 		case 0x27:
 			/* Undock Key */
 		case 0x50:
-			mainboard_io_trap_handler(SMI_DOCK_DISCONNECT);
+			mainboard_smi_dock_disconnect();
 			break;
 			/* Dock Event */
 		case 0x37:
 		case 0x58:
-			mainboard_io_trap_handler(SMI_DOCK_CONNECT);
+			mainboard_smi_dock_connect();
 			break;
 		default:
 			break;