profiles/oad/oad.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2014	Google, Inc.  (edjames@google.com)
 *  Copyright (C) 2012	Marcel Holtmann <marcel@holtmann.org>
 *  Copyright (C) 2012	Nordic Semiconductor Inc.
 *  Copyright (C) 2012	Instituto Nokia de Tecnologia - INdT
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

/* much lifted from input/hog.c */

/* OAD - over air download of firmware to devices */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdlib.h>
#include <stdbool.h>
#include <errno.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <bluetooth/bluetooth.h>

#include <glib.h>

#include "src/log.h"

#include "lib/uuid.h"
#include "src/adapter.h"
#include "src/device.h"
#include "src/profile.h"
#include "src/service.h"

#include "src/plugin.h"
#include "attrib/att.h"
#include "attrib/gattrib.h"
#include "src/attio.h"
#include "attrib/gatt.h"
#include "src/shared/util.h"

#define OAD_UUID		"f000ffc0-0451-4000-b000-000000000000"
#define OAD_IMG_IDENTITY_UUID	"f000ffc1-0451-4000-b000-000000000000"
#define OAD_IMG_BLOCK_UUID	"f000ffc2-0451-4000-b000-000000000000"

#define MAJOR_VERSION(x)	((x) >> 8)
#define MINOR_VERSION(x)	((x) & 0xff)
#define SLOT_VERSION(x)		(!(MINOR_VERSION(x) & 0x1))

#define FIRMWARE_SLOT0		"/tmp/gfrm200.slot0.bin"
#define FIRMWARE_SLOT1		"/tmp/gfrm200.slot1.bin"

//#define CHECK_TIME		(24*60*60)
#define CHECK_TIME		10

enum OAD_Status {
	OADSTATUS_None            = 0,
	OADSTATUS_DataTransfer    = 1,
	OADSTATUS_TransferSuccess = 2,
	OADSTATUS_CRCError        = 0xE0,
	OADSTATUS_LVD             = 0xE1,	// low voltage detection
	OADSTATUS_KeyPress        = 0xE2,	// user interrupted (not idle)
	OADSTATUS_GP              = 0xE3,	// bootloader said no
	OADSTATUS_Host            = 0xE4,	// host did something wrong (eg, timeout)
};

enum OAD_Action {
	OADACTION_GetRemoteInfo   = 0,
	OADACTION_TransferOADData = 1,
	OADACTION_StartUpdate     = 2,
};

struct fwinfo {
	uint16_t	gp_version;
	uint16_t	ti_version;
	uint32_t	gp_size;
	uint32_t	ti_size;
	int		slot;
};

struct service {
	guint			attio_id;
	uint16_t		ccc_handle;
	uint16_t		value_handle;
	int			notify_enabled;
};

struct oad_device {
	uint16_t		id;
	struct btd_device	*device;
	GAttrib			*attrib;
	guint			attioid;
	struct gatt_primary	*oad_primary;

	struct service		identity;
	struct service		block;

	time_t			lastCheck;
	struct fwinfo		remote;

	uint8_t			header[8];
	int			fd;
	guint			tid;		/* Timer id to delay starting upgrade */
};

struct characteristic {
	struct oad_device	*oaddev;
	char			uuid[MAX_LEN_UUID_STR + 1];
};

static GSList *devices = NULL;

//#define FAKEFW	1
#ifdef FAKEFW
static unsigned char HEADER[] = { 0xaa, 0x55, 0x01, 0x02, 0x0d, 0x04, 0x00, 0x54, 0, 0, 0, 0, 0, 0, 0, 0, };
#endif

static struct oad_device *oad_new_device(struct btd_device *device,
								uint16_t id)
{
	struct oad_device *oaddev;

	DBG("OAD trace");
	oaddev = g_try_new0(struct oad_device, 1);
	if (!oaddev)
		return NULL;

	oaddev->id = id;
	oaddev->device = btd_device_ref(device);
	oaddev->fd = -1;

	return oaddev;
}

static void oad_free_device(struct oad_device *oaddev)
{
	DBG("OAD trace");
	if (oaddev->fd != -1) {
		close(oaddev->fd);
		oaddev->fd = -1;
	}
	btd_device_unref(oaddev->device);
	g_attrib_unref(oaddev->attrib);
	g_free(oaddev->oad_primary);
	g_free(oaddev);
}

static void block_char_write_cb(guint8 status, const guint8 *pdu, guint16 len,
							gpointer user_data)
{
	struct oad_device *oaddev = user_data;

	DBG("OAD trace");
	if (status != 0) {
		error("OAD %s failed: %s", __func__, att_ecode2str(status));
		return;
	}
}

static void identity_char_write_cb(guint8 status, const guint8 *pdu, guint16 len,
							gpointer user_data)
{
	struct oad_device *oaddev = user_data;

	DBG("OAD trace");
	if (status != 0) {
		error("OAD %s failed: %s", __func__, att_ecode2str(status));
		return;
	}
}

static void block_notify_handler(const uint8_t *pdu, uint16_t len,
							gpointer user_data)
{
	struct oad_device *oaddev = user_data;

	DBG("OAD trace");

	/* should be at least opcode (1b) + handle (7a00) */
	if (len < 3) {
		error("OAD Invalid PDU received");
		return;
	}

	// 2 bytes block_num + status byte
	if (len != 6) {
		error("OAD block notify format error: expected 6 bytes, got %d", len);
		return;
	}

	/* request for block */
	uint16_t block_num = get_le16(pdu + 3);
	enum OAD_Status status = (enum OAD_Status) pdu[5];

	switch (status) {
	case OADSTATUS_DataTransfer:		// send a block
		{
			uint8_t block[18];	// 2 bytes block_num + 16 data bytes
			memset(block, 0, sizeof block);
			put_le16(block_num, block);

			if (lseek(oaddev->fd, 16 * block_num, SEEK_SET) < 0) {
				perror("OAD seek");
				close(oaddev->fd);
				oaddev->fd = -1;
				return;
			}
			if (read(oaddev->fd, block+2, sizeof block - 2) != sizeof block - 2) {
				perror("OAD short read");
				close(oaddev->fd);
				oaddev->fd = -1;
				return;
			}
#ifdef FAKEFW
if (block_num == 0) {
	memcpy(block+2, HEADER, sizeof block - 2);
} else {
	memset(block+2, 0, sizeof block - 2);
}
#endif

			DBG("OAD sending block %d", block_num);
			gatt_write_char(oaddev->attrib, oaddev->block.value_handle, block,
					sizeof(block), block_char_write_cb, oaddev);
		}
		break;
	case OADSTATUS_TransferSuccess:		// OK, successful transfer
		{
			DBG("OAD firmware transfer successful");
			close(oaddev->fd);
			oaddev->fd = -1;

			uint8_t attr_val[9];
			memset(attr_val, 0, sizeof attr_val);
			attr_val[8] = OADACTION_StartUpdate;		/* commit fw image */
			DBG("OAD committing firmware");
			gatt_write_char(oaddev->attrib, oaddev->identity.value_handle, attr_val,
						sizeof(attr_val), identity_char_write_cb, oaddev);
		}
		break;
	case OADSTATUS_CRCError:	// checksum error
		DBG("OAD firmware transfer checksum error");
		close(oaddev->fd);
		oaddev->fd = -1;
		break;
	case OADSTATUS_LVD:	// ???
	case OADSTATUS_KeyPress:	// ???
	case OADSTATUS_GP:	// ???
	case OADSTATUS_Host:	// ???
	default:
		DBG("OAD unexpected block notify status 0x%02x", status);
		close(oaddev->fd);
		oaddev->fd = -1;
		break;
	}
}

static time_t wallclock(void)
{
	struct timespec ts;

	if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) {
		return 0;
	}
	return ts.tv_sec;
}

static int isTimeForUpgradeCheck(struct oad_device* oaddev)
{
	DBG("OAD trace");

	time_t now = wallclock();

	time_t dt = now - oaddev->lastCheck;
	if (dt < CHECK_TIME) {
		DBG("OAD not time for upgrade check yet");
		return 0;
	}
	DBG("OAD time for upgrade check");
	return 1;
}

static void checkForUpgrade(struct oad_device *oaddev)
{
	DBG("OAD trace");

	if (!isTimeForUpgradeCheck(oaddev)) {
		return;
	}

	oaddev->lastCheck = wallclock();

	int slot2 = !oaddev->remote.slot;	// alternate between slot 0 and 1

	char* fw_file = slot2 ? FIRMWARE_SLOT1 : FIRMWARE_SLOT0;
	int fd = open(fw_file, O_RDONLY);
	if (fd < 0) {
		error("OAD open: %s: %s", fw_file, strerror(errno));
		return;
	}
	int len = read(fd, oaddev->header, sizeof (oaddev->header));
#ifdef FAKEFW
memcpy(oaddev->header, HEADER, sizeof oaddev->header);
#endif
	if (len < 0) {
		error("OAD read: %s: %s", fw_file, strerror(errno));
		close(fd);
		return;
	}

	if (len < sizeof oaddev->header) {
		error("OAD short read: %s: wanted %d, got %d", fw_file, sizeof oaddev->header, len);
		close(fd);
		return;
	}

	/* 8 bytes of header appear to be:
	 * 2 bytes checksum
	 * 2 bytes version
	 * 3 bytes size
	 * 1 byte type: "T" TI image, "G" GP image
	 */
	uint16_t version = get_le16(oaddev->header + 2);
	int type = oaddev->header[7];
	if (type == 'T') {
		int slot = SLOT_VERSION(version);
		DBG("OAD firmware on disk: file=%s ti_version=%d.%d slot=%d",
			fw_file, MAJOR_VERSION(version), MINOR_VERSION(version), slot);

		if (version <= oaddev->remote.ti_version) {
			DBG("OAD TI firmware is up to date");
			close(fd);
			return;
		}
	} else if (type == 'G') {
		DBG("OAD firmware on disk: file=%s gp_version=%d.%d",
			fw_file, MAJOR_VERSION(version), MINOR_VERSION(version));
		if (version <= oaddev->remote.gp_version) {
			DBG("OAD GP firmware is up to date");
			close(fd);
			return;
		}
	} else {
		error("OAD firmware on disk: unknown type '%c': file=%s version=%d.%d",
			type, fw_file, MAJOR_VERSION(version), MINOR_VERSION(version));
		close(fd);
		return;
	}

	DBG("OAD starting upgrade");
	if (oaddev->fd != -1) {
		close(oaddev->fd);
	}
	oaddev->fd = fd;	/* fd is left open */
	if (oaddev->tid) {
		g_source_remove(oaddev->tid);
	}

	uint8_t attr_val[9];
	memcpy(attr_val, oaddev->header, sizeof oaddev->header);
	attr_val[8] = OADACTION_TransferOADData;	/* start OAD image transfer */

	gatt_write_char(oaddev->attrib, oaddev->identity.value_handle, attr_val,
			sizeof(attr_val), identity_char_write_cb, oaddev);
}

static void identity_notify_handler(const uint8_t *pdu, uint16_t len,
							gpointer user_data)
{
	struct oad_device *oaddev = user_data;

	DBG("OAD trace");

	/* should be at least opcode (1b) + handle (76 00) */
	if (len < 3) {
		error("OAD Invalid PDU received");
		return;
	}

	/* Next 10 bytes appear to be:
	 *	char gp_version[2];	// 09 00 is 0.9
	 * 	char gp_size[3];	// 00 00 01 is 0x010000 == 64k
	 *	char ti_version[2];	// 2a 00 is 0.42
	 * 	char ti_size[3];	// 00 00 01 is 0x010000 == 64k
	 */

	if (len != 13) {
		error("OAD identity data format error: expected 13 bytes, got %d", len);
		return;
	}

	oaddev->remote.gp_version = get_le16(pdu+3);
	oaddev->remote.gp_size = get_le32(pdu+5) & 0x00ffffff;
	oaddev->remote.slot = SLOT_VERSION(oaddev->remote.gp_version);
	oaddev->remote.ti_version = get_le16(pdu+8);
	unsigned char size[4];
	memcpy(size, pdu+10, 3);
	size[3] = 0;				// need 4 bytes for get_le32
	oaddev->remote.ti_size = get_le32(size);

	DBG("OAD firmware on remote: gp_version=%d.%d gp_size=%d ti_version=%d.%d ti_size=%d slot=%d",
		MAJOR_VERSION(oaddev->remote.gp_version), MINOR_VERSION(oaddev->remote.gp_version),
		oaddev->remote.gp_size,
		MAJOR_VERSION(oaddev->remote.ti_version), MINOR_VERSION(oaddev->remote.ti_version),
		oaddev->remote.ti_size,
		oaddev->remote.slot);

	checkForUpgrade(oaddev);
}

static void check_fwversion(struct oad_device *oaddev)
{
	uint8_t attr_val[9];	// 8 byte header + 1 command byte (all zeros)

	DBG("OAD trace");

	if (!oaddev->identity.notify_enabled || !oaddev->block.notify_enabled) {
		return;
	}

	DBG("OAD sending fw version request");
	memset(attr_val, 0, sizeof attr_val);
	attr_val[8] = OADACTION_GetRemoteInfo;		/* trigger fw version notify */
	gatt_write_char(oaddev->attrib, oaddev->identity.value_handle, attr_val,
						sizeof(attr_val), identity_char_write_cb, oaddev);
}

static void ccc_char_write_identity_cb(guint8 status, const guint8 *pdu, guint16 len,
							gpointer user_data)
{
	struct oad_device *oaddev = user_data;

	DBG("OAD trace");
	if (status != 0) {
		error("OAD %s failed: %s", __func__, att_ecode2str(status));
		return;
	}
	oaddev->identity.notify_enabled = 1;

	check_fwversion(oaddev);
}

static void ccc_char_write_block_cb(guint8 status, const guint8 *pdu, guint16 len,
							gpointer user_data)
{
	struct oad_device *oaddev = user_data;

	DBG("OAD trace");
	if (status != 0) {
		error("OAD %s failed: %s", __func__, att_ecode2str(status));
		return;
	}
	oaddev->block.notify_enabled = 1;

	check_fwversion(oaddev);
}

static void enableNotify(struct oad_device* oaddev)
{
	DBG("OAD trace");
	if (oaddev->identity.ccc_handle == 0 || oaddev->block.ccc_handle == 0 ||
	    oaddev->identity.value_handle == 0 || oaddev->block.value_handle == 0) {
	    	error("OAD discovery not complete, cannot enable notifiers");
		return;
	}

	oaddev->identity.attio_id =
		g_attrib_register(oaddev->attrib,
			ATT_OP_HANDLE_NOTIFY, oaddev->identity.value_handle,
			identity_notify_handler, oaddev, NULL);
	oaddev->block.attio_id =
		g_attrib_register(oaddev->attrib,
			ATT_OP_HANDLE_NOTIFY, oaddev->block.value_handle,
			block_notify_handler, oaddev, NULL);

	uint8_t attr_val[2];
	put_le16(GATT_CLIENT_CHARAC_CFG_NOTIF_BIT, attr_val);
	DBG("OAD enabling notify for OAD_IMG_IDENTITY_UUID");
	gatt_write_char(oaddev->attrib, oaddev->identity.ccc_handle, attr_val,
				sizeof(attr_val), ccc_char_write_identity_cb, oaddev);
	DBG("OAD enabling notify for OAD_IMG_BLOCK_UUID");
	gatt_write_char(oaddev->attrib, oaddev->block.ccc_handle, attr_val,
				sizeof(attr_val), ccc_char_write_block_cb, oaddev);
}

static gboolean upgrade_timer(gpointer data)
{
	struct oad_device *oaddev = data;

	g_source_remove(oaddev->tid);
	oaddev->tid = 0;

	DBG("OAD timer fired");
	enableNotify(oaddev);
}

static void desc_discovered_cb(uint8_t status, GSList *descs, void* user_data)
{
	struct characteristic *ch = user_data;
	struct oad_device *oaddev = ch->oaddev;
	struct GSList *list = NULL;

	DBG("OAD trace");
	if (status != 0) {
		error("OAD %s failed: %s", __func__, att_ecode2str(status));
		goto done;
	}

	for (; descs; descs = descs->next) {
		struct gatt_desc *desc = descs->data;

		if (desc->uuid16 != GATT_CLIENT_CHARAC_CFG_UUID)
			continue;

		if (g_strcmp0(ch->uuid, OAD_IMG_IDENTITY_UUID) == 0) {
			DBG("OAD found OAD_IMG_IDENTITY_UUID ccc_handle=0x%04x", desc->handle);
			oaddev->identity.ccc_handle = desc->handle;

		} else if (g_strcmp0(ch->uuid, OAD_IMG_BLOCK_UUID) == 0) {
			DBG("OAD found OAD_IMG_BLOCK_UUID ccc_handle=0x%04x", desc->handle);
			oaddev->block.ccc_handle = desc->handle;
		}
	}
	if (oaddev->identity.ccc_handle && oaddev->block.ccc_handle)
		oaddev->tid = g_timeout_add_seconds(10, upgrade_timer, oaddev);

done:
	g_free(ch);
}

static void discover_desc(struct oad_device *oaddev, struct gatt_char *c,
						struct gatt_char *c_next)
{
	struct gatt_primary *prim = oaddev->oad_primary;
	struct characteristic *ch;
	uint16_t start, end;

	DBG("OAD trace");
	start = c->value_handle + 1;

	if (c_next != NULL) {
		if (start == c_next->handle)
			return;
		end = c_next->handle - 1;
	} else if (c->value_handle != prim->range.end) {
		end = prim->range.end;
	} else {
		return;
	}

	ch = g_new0(struct characteristic, 1);
	ch->oaddev = oaddev;
	memcpy(ch->uuid, c->uuid, sizeof(c->uuid));

	DBG("OAD discovering descriptors start=0x%04x end=0x%04x", start, end);

	gatt_discover_desc(oaddev->attrib, start, end, NULL, desc_discovered_cb, ch);
}

static void char_discovered_cb(uint8_t status, GSList *chars, void* user_data)
{
	struct oad_device *oaddev = user_data;
	struct gatt_primary *prim = oaddev->oad_primary;
	bt_uuid_t img_identity_uuid, img_block_uuid;
	GSList *l;
	uint16_t info_handle = 0, proto_mode_handle = 0;

	DBG("OAD inspecting characteristics");
	if (status != 0) {
		error("OAD %s failed: %s", __func__, att_ecode2str(status));
		return;
	}

	for (l = chars; l; l = g_slist_next(l)) {
		struct gatt_char *chr, *next;
		bt_uuid_t uuid;
		uint16_t start, end;

		chr = l->data;
		next = l->next ? l->next->data : NULL;

		DBG("OAD 0x%04x UUID: %s properties: %02x",
				chr->handle, chr->uuid, chr->properties);


		start = chr->value_handle + 1;
		end = (next ? next->handle - 1 : prim->range.end);

		if (g_strcmp0(chr->uuid, OAD_IMG_IDENTITY_UUID) == 0) {
			oaddev->identity.value_handle = chr->value_handle;
			DBG("OAD found OAD_IMG_IDENTITY_UUID value_handle=0x%04x", chr->value_handle);
			discover_desc(oaddev, chr, next);
		} else if (g_strcmp0(chr->uuid, OAD_IMG_BLOCK_UUID) == 0) {
			oaddev->block.value_handle = chr->value_handle;
			DBG("OAD found OAD_IMG_BLOCK_UUID value_handle=0x%04x", chr->value_handle);
			discover_desc(oaddev, chr, next);
		}

	}
}

static void attio_connected_cb(GAttrib *attrib, gpointer user_data)
{
	struct oad_device *oaddev = user_data;
	struct gatt_primary *prim = oaddev->oad_primary;
	GSList *l;

	DBG("OAD trace");
	DBG("OAD connected");

	oaddev->attrib = g_attrib_ref(attrib);
	oaddev->identity.notify_enabled = 0;
	oaddev->block.notify_enabled = 0;

	if (oaddev->identity.ccc_handle == 0 || oaddev->block.ccc_handle == 0 ||
	    oaddev->identity.value_handle == 0 || oaddev->block.value_handle == 0) {
		DBG("OAD discovering characteristics");

		gatt_discover_char(oaddev->attrib, prim->range.start,
						prim->range.end, NULL,
						char_discovered_cb, oaddev);
	} else {
		if (isTimeForUpgradeCheck(oaddev)) {
			enableNotify(oaddev);
		}
	}
}

static void attio_disconnected_cb(gpointer user_data)
{
	struct oad_device *oaddev = user_data;
	GSList *l;

	DBG("OAD trace");
	DBG("OAD disconnected");

	if (oaddev->identity.attio_id > 0) {
		g_attrib_unregister(oaddev->attrib, oaddev->identity.attio_id);
		oaddev->identity.attio_id = 0;
	}
	if (oaddev->block.attio_id > 0) {
		g_attrib_unregister(oaddev->attrib, oaddev->block.attio_id);
		oaddev->block.attio_id = 0;
	}
	if (oaddev->attrib)
		g_attrib_unref(oaddev->attrib);
	oaddev->attrib = NULL;
}

static struct oad_device *oad_register_device(struct btd_device *device,
						struct gatt_primary *prim)
{
	struct oad_device *oaddev;
	GIOCondition cond = G_IO_IN | G_IO_ERR | G_IO_NVAL;
	GIOChannel *io;

	DBG("OAD trace");
	oaddev = oad_new_device(device, prim->range.start);
	if (!oaddev)
		return NULL;

	oaddev->oad_primary = g_memdup(prim, sizeof(*prim));

	oaddev->attioid = btd_device_add_attio_callback(device,
							attio_connected_cb,
							attio_disconnected_cb,
							oaddev);

	return oaddev;
}

static int oad_unregister_device(struct oad_device *oaddev)
{
	DBG("OAD trace");
	btd_device_remove_attio_callback(oaddev->device, oaddev->attioid);
	oad_free_device(oaddev);

	return 0;
}

static int oad_probe(struct btd_service *service)
{
	struct btd_device *device = btd_service_get_device(service);
	const char *path = device_get_path(device);
	GSList *primaries, *l;

	DBG("OAD trace");
	DBG("OAD path %s", path);

	primaries = btd_device_get_primaries(device);
	if (primaries == NULL)
		return -EINVAL;

	for (l = primaries; l; l = g_slist_next(l)) {
		struct gatt_primary *prim = l->data;
		struct oad_device *oaddev;

		DBG("OAD uuid=%s", prim->uuid);
		if (strcmp(prim->uuid, OAD_UUID) != 0)
			continue;

		DBG("OAD matched OAD uuid", prim->uuid);
		oaddev = oad_register_device(device, prim);
		if (oaddev == NULL)
			continue;

		devices = g_slist_append(devices, oaddev);
	}

	return 0;
}

static void remove_device(gpointer a, gpointer b)
{
	struct oad_device *oaddev = a;
	struct btd_device *device = b;

	DBG("OAD trace");
	if (oaddev->device != device)
		return;

	devices = g_slist_remove(devices, oaddev);
	oad_unregister_device(oaddev);
}

static void oad_remove(struct btd_service *service)
{
	struct btd_device *device = btd_service_get_device(service);
	const char *path = device_get_path(device);

	DBG("OAD trace");
	DBG("OAD path %s", path);

	g_slist_foreach(devices, remove_device, device);
}

static struct btd_profile oad_profile = {
	.name		= "OAD",
	.remote_uuid	= OAD_UUID,
	.device_probe	= oad_probe,
	.device_remove	= oad_remove,
};

static int oad_init(void)
{
	int err;

	DBG("OAD trace");
	return btd_profile_register(&oad_profile);
}

static void oad_exit(void)
{
	DBG("OAD trace");
	btd_profile_unregister(&oad_profile);
}

BLUETOOTH_PLUGIN_DEFINE(oad, VERSION, BLUETOOTH_PLUGIN_PRIORITY_DEFAULT,
							oad_init, oad_exit)