Deepblu: add basic send/receive functionality

This was tested with a command to set the time and date, but only the
actual IO parts are here.

The packet format is fairly simple, even if it's not exactly clear why
everything is HEX-encoded.

Signed-off-by: Linus Torvalds <torvalds@linux-foundation.org>

src/deepblu.c

@@ -48,6 +48,203 @@ static const dc_device_vtable_t deepblu_device_vtable = {
 	deepblu_device_close, /* close */
 };
 
+// Maximum data in a packet. It's actually much
+// less than this, since BLE packets are small and
+// with the 7 bytes of headers and final newline
+// and the HEX encoding, the actual maximum is
+// just something like 6 bytes.
+//
+// But in theory the data could be done over
+// multiple packets. That doesn't seem to be
+// the case in anything I've seen so far.
+//
+// Pick something small and easy to use for
+// stack buffers.
+#define MAX_DATA 20
+
+static char *
+write_hex_byte(unsigned char data, char *p)
+{
+	static const char hex[16] = "0123456789ABCDEF";
+	*p++ = hex[data >> 4];
+	*p++ = hex[data & 0xf];
+	return p;
+}
+
+//
+// Send a cmd packet.
+//
+// The format of the cmd on the "wire" is:
+//  - byte '#'
+//  - HEX char of cmd
+//  - HEX char two's complement modular sum of packet data (including cmd/size)
+//  - HEX char size of data as encoded in HEX
+//  - n * HEX char data
+//  - byte '\n'
+// so you end up having 8 bytes of header/trailer overhead, and two bytes
+// for every byte of data sent due to the HEX encoding.
+//
+static dc_status_t
+deepblu_send_cmd(deepblu_device_t *device, const unsigned char cmd, const unsigned char data[], size_t size)
+{
+	char buffer[8+2*MAX_DATA], *p;
+	unsigned char csum;
+	int i;
+
+	if (size > MAX_DATA)
+		return DC_STATUS_INVALIDARGS;
+
+	// Calculate packet csum
+	csum = cmd + 2*size;
+	for (i = 0; i < size; i++)
+		csum += data[i];
+	csum = -csum;
+
+	// Fill the data buffer
+	p = buffer;
+	*p++ = '#';
+	p = write_hex_byte(cmd, p);
+	p = write_hex_byte(csum, p);
+	p = write_hex_byte(size*2, p);
+	for (i = 0; i < size; i++)
+		p = write_hex_byte(data[i], p);
+	*p++ = '\n';
+
+	// .. and send it out
+	return dc_iostream_write(device->iostream, buffer, p-buffer, NULL);
+}
+
+//
+// Receive one 'line' of data
+//
+// The deepblu BLE protocol is ASCII line based and packetized.
+// Normally one packet is one line, but it looks like the Nordic
+// Semi BLE chip will sometimes send packets early (some internal
+// serial buffer timeout?) with incompete data.
+//
+// So read packets until you get newline.
+static dc_status_t
+deepblu_recv_line(deepblu_device_t *device, unsigned char *buf, size_t size)
+{
+	while (1) {
+		unsigned char buffer[20];
+		size_t transferred = 0;
+		dc_status_t status;
+
+		status = dc_iostream_read(device->iostream, buffer, sizeof(buffer), &transferred);
+		if (status != DC_STATUS_SUCCESS) {
+			ERROR(device->base.context, "Failed to receive Deepblu reply packet.");
+			return status;
+		}
+		if (transferred > size) {
+			ERROR(device->base.context, "Deepblu reply packet with too much data (got %zu, expected %zu)", transferred, size);
+			return DC_STATUS_IO;
+		}
+		if (!transferred) {
+			ERROR(device->base.context, "Empty Deepblu reply packet");
+			return DC_STATUS_IO;
+		}
+		memcpy(buf, buffer, transferred);
+		buf += transferred;
+		size -= transferred;
+		if (buf[-1] == '\n')
+			break;
+	}
+	buf[-1] = 0;
+	return DC_STATUS_SUCCESS;
+}
+
+static int
+hex_nibble(char c)
+{
+	if (c >= '0' && c <= '9')
+		return c - '0';
+	if (c >= 'a' && c <= 'f')
+		return c - 'a' + 10;
+	if (c >= 'A' && c <= 'F')
+		return c - 'A' + 10;
+	return -1;
+}
+
+static int
+read_hex_byte(char *p)
+{
+	// This is negative if either of the nibbles is invalid
+	return (hex_nibble(p[0]) << 4) | hex_nibble(p[1]);
+}
+
+
+//
+// Receive a reply packet
+//
+// The reply packet has the same format as the cmd packet we
+// send, except the first byte is '$' instead of '#'.
+static dc_status_t
+deepblu_recv_data(deepblu_device_t *device, const unsigned char expected, unsigned char *buf, size_t size, size_t *received)
+{
+	int len, i;
+	dc_status_t status;
+	char buffer[8+2*MAX_DATA];
+	int cmd, csum, ndata;
+
+	status = deepblu_recv_line(device, buffer, sizeof(buffer));
+	if (status != DC_STATUS_SUCCESS)
+		return status;
+
+	// deepblu_recv_line() always zero-terminates the result
+	// if it returned success, and has removed the final newline.
+	len = strlen(buffer);
+	HEXDUMP(device->base.context, DC_LOGLEVEL_DEBUG, "rcv", buffer, len);
+
+	// A valid reply should always be at least 7 characters: the
+	// initial '$' and the three header HEX bytes.
+	if (len < 8 || buffer[0] != '$') {
+		ERROR(device->base.context, "Invalid Deepblu reply packet");
+		return DC_STATUS_IO;
+	}
+
+	cmd = read_hex_byte(buffer+1);
+	csum = read_hex_byte(buffer+3);
+	ndata = read_hex_byte(buffer+5);
+	if ((cmd | csum | ndata) < 0) {
+		ERROR(device->base.context, "non-hex Deepblu reply packet header");
+		return DC_STATUS_IO;
+	}
+
+	// Verify the data length: it's the size of the HEX data,
+	// and should also match the line length we got (the 7
+	// is for the header data we already decoded above).
+	if ((ndata & 1) || ndata != len - 7) {
+		ERROR(device->base.context, "Deepblu reply packet data length does not match (claimed %d, got %d)", ndata, len-7);
+		return DC_STATUS_IO;
+	}
+
+	if (ndata >> 1 > size) {
+		ERROR(device->base.context, "Deepblu reply packet too big for buffer");
+		return DC_STATUS_IO;
+	}
+
+	csum += cmd + ndata;
+
+	for (i = 7; i < len; i += 2) {
+		int byte = read_hex_byte(buffer + i);
+		if (byte < 0) {
+			ERROR(device->base.context, "Deepblu reply packet data not valid hex");
+			return DC_STATUS_IO;
+		}
+		*buf++ = byte;
+		csum += byte;
+	}
+
+	if (csum & 255) {
+		ERROR(device->base.context, "Deepblu reply packet csum not valid (%x)", csum);
+		return DC_STATUS_IO;
+	}
+
+	*received = ndata >> 1;
+	return DC_STATUS_SUCCESS;
+}
+
 dc_status_t
 deepblu_device_open (dc_device_t **out, dc_context_t *context, dc_iostream_t *iostream)
 {