[RFC PATCH 03/10] Add a helper to put the OSTC3 into servicemode

[Anton Lundin]

This code is inspired by JeanDo ostc-companion.

Signed-off-by: Anton Lundin <glance at acc.umu.se>
---
 src/hw_ostc3.c | 53 ++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 52 insertions(+), 1 deletion(-)

diff --git a/src/hw_ostc3.c b/src/hw_ostc3.c
index cc1deb3..6bc50d5 100644
--- a/src/hw_ostc3.c
+++ b/src/hw_ostc3.c
@@ -50,6 +50,7 @@
 #define RB_LOGBOOK_SIZE  256
 #define RB_LOGBOOK_COUNT 256
 
+#define S_READY    0x4C
 #define READY      0x4D
 #define HEADER     0x61
 #define CLOCK      0x62
@@ -191,7 +192,7 @@ hw_ostc3_transfer (hw_ostc3_device_t *device,
 		}
 
 		// Verify the ready byte.
-		if (ready[0] != READY) {
+		if (!(ready[0] == READY || ready[0] == S_READY)) {
 			ERROR (abstract->context, "Unexpected ready byte.");
 			return DC_STATUS_PROTOCOL;
 		}
@@ -742,3 +743,53 @@ hw_ostc3_firmware_readfile (hw_ostc3_firmware_t *firmware, dc_context_t *context
 
 	return DC_STATUS_SUCCESS;
 }
+
+static dc_status_t
+hw_ostc3_service_mode (dc_device_t *abstract)
+{
+	hw_ostc3_device_t *device = (hw_ostc3_device_t *) abstract;
+	unsigned char command[] = {0xAA, 0xAB, 0xCD, 0xEF};
+	unsigned char output[5];
+	dc_status_t rc;
+
+	/* FIXME: this is ugly.
+	 * But libdivecomputer enters download mode on connect,
+	 * so we need to exit download mode and enter service mode.
+	 */
+
+	// Exit download mode
+	rc = hw_ostc3_transfer (device, NULL, EXIT, NULL, 0, NULL, 0);
+	if (rc != DC_STATUS_SUCCESS)
+		return rc;
+
+	// Wait for the device to reboot before
+	// sending command to enter service mode
+	serial_sleep(device->port, 6000);
+
+	// We cant use hw_ostc3_transfer here, due to the different echos
+	int n = serial_write (device->port, command, sizeof (command));
+	if (n != sizeof (command)) {
+		ERROR (abstract->context, "Failed to send the command.");
+		return EXITCODE (n);
+	}
+
+	// Give the device some time to enter service mode
+	serial_sleep(device->port, 100);
+
+	// Read the response
+	n = serial_read (device->port, output, sizeof (output));
+	if (n != sizeof (output)) {
+		ERROR (abstract->context, "Failed to receive the echo.");
+		return EXITCODE (n);
+	}
+
+	// Verify the response to service mode
+	if (output[0] != 0x4B || output[1] != 0xAB ||
+			output[2] != 0xCD || output[3] != 0xEF ||
+			output[4] != S_READY) {
+		ERROR (abstract->context, "Failed to verify echo.");
+		return DC_STATUS_IO;
+	}
+
+	return DC_STATUS_SUCCESS;
+}
-- 
1.9.1