From c863db02f01c9e2ed9ed176ed2e289f3f2772e9d Mon Sep 17 00:00:00 2001
From: Linus Torvalds <torvalds@linux-foundation.org>
Date: Thu, 22 Jun 2017 21:44:28 -0700
Subject: [PATCH] Teach the EON Steel about HDLC encoding of the command
 packets

The BLE GATT transport ends up using HDLC for the stream encoding,
unlike the USB HID side.  The EON Steel BLE GATT protocol actually does
that for both the commands and for the replies, but this converts only
the command side, because that's the simpler one.

The reply side code will need to be re-architected a bit, because right
now it is very much oriented towards beign able to do everything one
single packet at a time (which is true for USB HID) rather than treating
the packets as a stream of data (as is necessary for the CRC32
verification and to handle the escaping of the 0x7e/0x7d bytes in the
stream).

So with this change, you can't actually do a download over BLE, but I
was able to verify that the first command transfers correctly, and the
EON Steel replies to it over Bluetooth LE GATT.

Signed-off-by: Linus Torvalds <torvalds@linux-foundation.org>
---
 examples/Makefile.am  |  2 +-
 src/suunto_eonsteel.c | 61 ++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/examples/Makefile.am b/examples/Makefile.am
index e830769..4bd290d 100644
--- a/examples/Makefile.am
+++ b/examples/Makefile.am
@@ -1,5 +1,5 @@
 AM_CPPFLAGS = -I$(top_builddir)/include -I$(top_srcdir)/include
-LDADD = $(top_builddir)/src/libdivecomputer.la
+LDADD = $(top_builddir)/src/libdivecomputer.la -lz
 
 bin_PROGRAMS = \
 	dctool
diff --git a/src/suunto_eonsteel.c b/src/suunto_eonsteel.c
index 841d649..8acb7de 100644
--- a/src/suunto_eonsteel.c
+++ b/src/suunto_eonsteel.c
@@ -22,6 +22,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <zlib.h>	/* For crc32() */
 
 #include "suunto_eonsteel.h"
 #include "context-private.h"
@@ -160,6 +161,42 @@ static int receive_packet(suunto_eonsteel_device_t *eon, unsigned char *buffer,
 	return len;
 }
 
+static int add_hdlc(unsigned char *dst, unsigned char val)
+{
+	int chars = 1;
+	switch (val) {
+	case 0x7e: case 0x7d:
+		*dst++ = 0x7d;
+		val ^= 0x20;
+		chars++;
+		/* fallthrough */
+	default:
+		*dst = val;
+	}
+	return chars;
+}
+
+static int hdlc_reencode(unsigned char *dst, unsigned char *src, int len)
+{
+	unsigned int crc = crc32(0, src, len);
+	int result = 0, i;
+
+	*dst++ = 0x7e; result++;
+	for (i = 0; i < len; i++) {
+		int chars = add_hdlc(dst, src[i]);
+		dst += chars;
+		result += chars;
+	}
+	for (i = 0; i < 4; i++) {
+		int chars = add_hdlc(dst, crc & 255);
+		dst += chars;
+		result += chars;
+		crc >>= 8;
+	}
+	*dst++ = 0x7e; result++;
+	return result;
+}
+
 static int send_cmd(suunto_eonsteel_device_t *eon,
 	unsigned short cmd,
 	unsigned int len,
@@ -200,7 +237,29 @@ static int send_cmd(suunto_eonsteel_device_t *eon,
 		memcpy(buf+14, buffer, len);
 	}
 
-	rc = io->packet_write(io, buf, sizeof(buf), &transferred);
+	// BLE GATT protocol?
+	if (io->packet_size < 64) {
+		int hdlc_len;
+		unsigned char hdlc[2+2*(62+4)]; /* start/stop + escaping*(maxbuf+crc32) */
+		unsigned char *ptr;
+
+		hdlc_len = hdlc_reencode(hdlc, buf+2, buf[1]);
+
+		ptr = hdlc;
+		do {
+			int len = hdlc_len;
+
+			if (len > io->packet_size)
+				len = io->packet_size;
+			rc = io->packet_write(io, ptr, len, &transferred);
+			if (rc != DC_STATUS_SUCCESS)
+				break;
+			ptr += len;
+			hdlc_len -= len;
+		} while (hdlc_len);
+	} else {
+		rc = io->packet_write(io, buf, sizeof(buf), &transferred);
+	}
 	if (rc != DC_STATUS_SUCCESS) {
 		ERROR(eon->base.context, "write interrupt transfer failed");
 		return -1;