1 files changed, 35 insertions, 8 deletions

diff --git a/futility/updater_utils.c b/futility/updater_utils.c
index 75f5c0f2..6e2d358d 100644
--- a/futility/updater_utils.c
+++ b/futility/updater_utils.c
@@ -451,20 +451,37 @@ static int host_get_platform_version(void)
 }
 
 /*
- * Helper function to detect type of Servo board attached to host,
- * and store the right programmer / prepare settings to arguments.
- * Returns 0 if success, non-zero if error.
+ * Helper function to detect type of Servo board attached to host.
+ * Returns a string as programmer parameter on success, otherwise NULL.
  */
-int host_detect_servo(const char **programmer_ptr, int *need_prepare_ptr)
+char *host_detect_servo(int *need_prepare_ptr)
 {
-	int ret = 0;
+	const char *servo_port = getenv(ENV_SERVOD_PORT);
 	char *servo_type = host_shell("dut-control -o servo_type 2>/dev/null");
 	const char *programmer = NULL;
+	char *ret = NULL;
 	int need_prepare = 0;  /* To prepare by dut-control cpu_fw_spi:on */
+	char *servo_serial = NULL;
+
+	/* Get serial name if servo port is provided. */
+	if (servo_port && *servo_port) {
+		const char *cmd = "dut-control -o serialname 2>/dev/null";
+
+		VB2_DEBUG("Select servod using port: %s\n", servo_port);
+		if (strstr(servo_type, "with_servo_micro"))
+			cmd = ("dut-control -o servo_micro_serialname"
+			       " 2>/dev/null");
+		else if (strstr(servo_type, "with_ccd"))
+			cmd = "dut-control -o ccd_serialname 2>/dev/null";
+
+		servo_serial = host_shell(cmd);
+		VB2_DEBUG("Servo SN=%s (serial cmd: %s)\n", servo_serial, cmd);
+	}
 
 	if (!*servo_type) {
 		ERROR("Failed to get servo type. Check servod.\n");
-		ret = 1;
+	} else if (servo_serial && !*servo_serial) {
+		ERROR("Failed to get serial at servo port %s.\n", servo_port);
 	} else if (strstr(servo_type, "servo_micro")) {
 		VB2_DEBUG("Selected Servo Micro.\n");
 		programmer = "raiden_debug_spi";
@@ -478,9 +495,19 @@ int host_detect_servo(const char **programmer_ptr, int *need_prepare_ptr)
 		need_prepare = 1;
 	}
 
+	if (programmer) {
+		if (!servo_serial) {
+			ret = strdup(programmer);
+		} else {
+			const char prefix = strchr(programmer, ':') ? ',' : ':';
+			ASPRINTF(&ret, "%s%cserial=%s", programmer, prefix,
+				 servo_serial);
+		}
+		VB2_DEBUG("Servo programmer: %s\n", ret);
+	}
+
 	free(servo_type);
-	if (programmer && !*programmer_ptr)
-		*programmer_ptr = programmer;
+	free(servo_serial);
 	*need_prepare_ptr = need_prepare;
 
 	return ret;