[hmcfgusb] / hmuartlgw.c

/* HM-MOD-UART/HM-LGW-O-TW-W-EU driver
 *
 * Copyright (c) 2016 Michael Gernoth <michael@gernoth.net>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include <errno.h>
#include <fcntl.h>
#include <inttypes.h>
#include <poll.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <strings.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <unistd.h>

#include "hexdump.h"
#include "hmuartlgw.h"

#define HMUARTLGW_INIT_TIMEOUT	10000

#define HMUARTLGW_SETTLE_TIME	1

static int debug = 0;

enum hmuartlgw_state {
	HMUARTLGW_QUERY_APPSTATE,
	HMUARTLGW_ENTER_BOOTLOADER,
	HMUARTLGW_ENTER_BOOTLOADER_ACK,
	HMUARTLGW_BOOTLOADER,
	HMUARTLGW_ENTER_APPLICATION,
	HMUARTLGW_ENTER_APPLICATION_ACK,
	HMUARTLGW_APPLICATION,
};

struct recv_data {
	enum hmuartlgw_state state;
};


#define CRC16_POLY	0x8005

static uint16_t crc16(uint8_t* buf, int length)
{
	uint16_t crc = 0xd77f;
	int i;

	while (length--) {
		crc ^= *buf++ << 8;
		for (i = 0; i < 8; i++) {
			if (crc & 0x8000) {
				crc <<= 1;
				crc ^= CRC16_POLY;
			} else {
				crc <<= 1;
			}
		}
	}

	return crc;
}

static int hmuartlgw_init_parse(enum hmuartlgw_dst dst, uint8_t *buf, int buf_len, void *data)
{
	struct recv_data *rdata = data;

#if 0
	if (debug) {
		printf("Length: %d\n", buf_len);
		hexdump(buf, buf_len, "INIT > ");
	}
#endif

	if (dst != HMUARTLGW_OS)
		return 0;

	if ((buf_len == 10) && (buf[0] == 0x00) && !strncmp(((char*)buf)+1, "Co_CPU_BL", 9)) {
		rdata->state = HMUARTLGW_BOOTLOADER;
		return 1;
	}

	if ((buf_len == 11) && (buf[0] == 0x00) && !strncmp(((char*)buf)+1, "Co_CPU_App", 10)) {
		rdata->state = HMUARTLGW_APPLICATION;
		return 1;
	}

	switch(rdata->state) {
		case HMUARTLGW_QUERY_APPSTATE:
			if ((buf[0] == HMUARTLGW_OS_ACK) && (buf[1] == 0x02)) {
				if (!strncmp(((char*)buf)+2, "Co_CPU_BL", 9)) {
					rdata->state = HMUARTLGW_BOOTLOADER;
				} else if (!strncmp(((char*)buf)+2, "Co_CPU_App", 10)) {
					rdata->state = HMUARTLGW_APPLICATION;
				}
			}
			break;
		case HMUARTLGW_ENTER_BOOTLOADER:
			if ((buf_len == 2) &&
			    (buf[0] == HMUARTLGW_OS_ACK) &&
			    (buf[1] == 0x01)) {
				rdata->state = HMUARTLGW_ENTER_BOOTLOADER_ACK;
			}
			break;
		case HMUARTLGW_ENTER_BOOTLOADER_ACK:
			rdata->state = HMUARTLGW_ENTER_BOOTLOADER;
			break;
		case HMUARTLGW_ENTER_APPLICATION:
			if ((buf_len == 2) &&
			    (buf[0] == HMUARTLGW_OS_ACK) &&
			    (buf[1] == 0x01)) {
				rdata->state = HMUARTLGW_ENTER_APPLICATION_ACK;
			}
			break;
		case HMUARTLGW_ENTER_APPLICATION_ACK:
			rdata->state = HMUARTLGW_ENTER_APPLICATION;
			break;
		default:
			return 0;
			break;
	}

        return 1;
}

struct hmuartlgw_dev *hmuart_init(char *device, hmuartlgw_cb_fn cb, void *data, int app)
{
	struct hmuartlgw_dev *dev = NULL;
	struct termios oldtio, tio;

	dev = malloc(sizeof(struct hmuartlgw_dev));
	if (dev == NULL) {
		perror("malloc(struct hmuartlgw_dev)");
		return NULL;
	}

	memset(dev, 0, sizeof(struct hmuartlgw_dev));

	dev->fd = open(device, O_RDWR | O_NOCTTY);
	if (dev->fd < 0) {
		perror("open(hmuartlgw)");
		goto out;
	}

	if (debug) {
		fprintf(stderr, "%s opened\n", device);
	}

	if (tcgetattr(dev->fd, &oldtio) == -1) {
		perror("tcgetattr");
		goto out2;
	}

	memset(&tio, 0, sizeof(tio));

	tio.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
	tio.c_iflag = IGNPAR;
	tio.c_oflag = 0;
	tio.c_lflag = 0;
	tio.c_cc[VTIME] = 0;
	tio.c_cc[VMIN] = 1;

	tcflush(dev->fd, TCIFLUSH);
	if (tcsetattr(dev->fd, TCSANOW, &tio) == -1) {
		perror("tcsetattr");
		goto out2;
	}

	if (debug) {
		fprintf(stderr, "serial parameters set\n");
	}

	hmuartlgw_flush(dev);

	if (app) {
		hmuartlgw_enter_app(dev);
	} else {
		hmuartlgw_enter_bootloader(dev);
	}

	dev->cb = cb;
	dev->cb_data = data;

	return dev;

out2:
	close(dev->fd);
out:
	free(dev);
	return NULL;
}

struct hmuartlgw_dev *hmlgw_init(char *device, hmuartlgw_cb_fn cb, void *data)
{
	struct hmuartlgw_dev *dev = NULL;

	return dev;
}

void hmuartlgw_enter_bootloader(struct hmuartlgw_dev *dev)
{
	hmuartlgw_cb_fn cb_old = dev->cb;
	void *cb_data_old = dev->cb_data;
	struct recv_data rdata = { 0 };
	uint8_t buf[128] = { 0 };

	if (debug) {
		fprintf(stderr, "Entering bootloader\n");
	}

	buf[0] = HMUARTLGW_OS_CHANGE_APP;

	dev->cb = hmuartlgw_init_parse;
	dev->cb_data = &rdata;

	rdata.state = HMUARTLGW_QUERY_APPSTATE;
	buf[0] = HMUARTLGW_OS_GET_APP;
	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
	do {
		hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
	} while (rdata.state == HMUARTLGW_QUERY_APPSTATE);

	if (rdata.state != HMUARTLGW_BOOTLOADER) {
		rdata.state = HMUARTLGW_ENTER_BOOTLOADER;
		buf[0] = HMUARTLGW_OS_CHANGE_APP;
		hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
		do {
			hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
		} while (rdata.state != HMUARTLGW_BOOTLOADER);

		printf("Waiting for bootloader to settle...\n");
		sleep(HMUARTLGW_SETTLE_TIME);
	}

	dev->cb = cb_old;
	dev->cb_data = cb_data_old;
}

void hmuartlgw_enter_app(struct hmuartlgw_dev *dev)
{
	hmuartlgw_cb_fn cb_old = dev->cb;
	void *cb_data_old = dev->cb_data;
	struct recv_data rdata = { 0 };
	uint8_t buf[128] = { 0 };

	if (debug) {
		fprintf(stderr, "Entering application\n");
	}

	dev->cb = hmuartlgw_init_parse;
	dev->cb_data = &rdata;

	rdata.state = HMUARTLGW_QUERY_APPSTATE;
	buf[0] = HMUARTLGW_OS_GET_APP;
	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
	do {
		hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
	} while (rdata.state == HMUARTLGW_QUERY_APPSTATE);

	if (rdata.state != HMUARTLGW_APPLICATION) {
		rdata.state = HMUARTLGW_ENTER_APPLICATION;
		buf[0] = HMUARTLGW_OS_CHANGE_APP;
		hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
		do {
			hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
		} while (rdata.state != HMUARTLGW_APPLICATION);

		printf("Waiting for application to settle...\n");
		sleep(HMUARTLGW_SETTLE_TIME);
	}

	dev->cb = cb_old;
	dev->cb_data = cb_data_old;
}

static int hmuartlgw_escape(uint8_t *frame, int framelen)
{
	int i;

	for (i = 1; i < framelen; i++) {
		if (frame[i] == 0xfc || frame[i] == 0xfd) {
			memmove(frame + i + 1, frame + i, framelen - i);
			frame[i++] = 0xfc;
			frame[i] &= 0x7f;
			framelen++;
		}
	}
	return framelen;
}

int hmuartlgw_send_raw(struct hmuartlgw_dev *dev, uint8_t *frame, int framelen)
{
	int w = 0;
	int ret;

	if (debug) {
		hexdump(frame, framelen, "UARTLGW < ");
	}

	framelen = hmuartlgw_escape(frame, framelen);

	do {
		ret = write(dev->fd, frame + w, framelen - w);
		if (ret < 0) {
			perror("write");
			return 0;
		}
		w += ret;
	} while (w < framelen);

	return 1;
}

int hmuartlgw_send(struct hmuartlgw_dev *dev, uint8_t *cmd, int cmdlen, enum hmuartlgw_dst dst)
{
	static uint8_t cnt = 0;
	uint8_t frame[4096] = { 0 };
	uint16_t crc;

	frame[0] = 0xfd;
	frame[1] = ((cmdlen + 2) >> 8) & 0xff;
	frame[2] = (cmdlen + 2) & 0xff;
	frame[3] = dst;
	dev->last_send_cnt = cnt;
	frame[4] = cnt++;
	memcpy(&(frame[5]), cmd, cmdlen);
	crc = crc16(frame, cmdlen + 5);
	frame[cmdlen + 5] = (crc >> 8) & 0xff;
	frame[cmdlen + 6] = crc & 0xff;

	return hmuartlgw_send_raw(dev, frame, cmdlen + 7);
}

int hmuartlgw_poll(struct hmuartlgw_dev *dev, int timeout)
{
	struct pollfd pfds[1];
	int ret;
	int r = 0;
	uint16_t crc;

	errno = 0;

	memset(pfds, 0, sizeof(struct pollfd) * 1);

	pfds[0].fd = dev->fd;
	pfds[0].events = POLLIN;

	ret = poll(pfds, 1, timeout);
	if (ret == -1)
		return -1;

	errno = 0;
	if (ret == 0) {
		errno = ETIMEDOUT;
		return -1;
	}

	if (!(pfds[0].revents & POLLIN)) {
		errno = EIO;
		return -1;
	}

	r = read(dev->fd, dev->buf+dev->pos, 1);
	if (r < 0)
		return -1;

	if (r == 0) {
		errno = EOF;
		return -1;
	}

	dev->pos += r;

	if (dev->buf[0] != 0xfd) {
		memset(dev->buf, 0, sizeof(dev->buf));
		dev->pos = 0;
		dev->unescape_next = 0;
		return -1;
	}

	if (dev->unescape_next) {
		dev->buf[dev->pos-1] |= 0x80;
		dev->unescape_next = 0;
	} else if (dev->buf[dev->pos-1] == 0xfc) {
		dev->unescape_next = 1;
		dev->pos--;
		return -1;
	}

	if (dev->pos >= 3) {
		uint16_t len;

		len = ((dev->buf[1] << 8) & 0xff00) | (dev->buf[2] & 0xff);

		if (dev->pos < len + 5)
			return -1;
	} else {
		return -1;
	}

	crc = crc16(dev->buf, dev->pos);
	if (crc == 0x0000) {
		if (debug)
			hexdump(dev->buf, dev->pos, "UARTLGW > ");

		dev->cb(dev->buf[3], dev->buf + 5 , dev->pos - 7, dev->cb_data);

		memset(dev->buf, 0, sizeof(dev->buf));
		dev->pos = 0;
		dev->unescape_next = 0;
	} else {
		fprintf(stderr, "Invalid checksum received!\n");
		hexdump(dev->buf, dev->pos, "ERR> ");
		printf("calculated: %04x\n", crc);

		memset(dev->buf, 0, sizeof(dev->buf));
		dev->pos = 0;
		dev->unescape_next = 0;
	}

	errno = 0;
	return -1;
}

void hmuartlgw_close(struct hmuartlgw_dev *dev)
{
	close(dev->fd);
}

void hmuartlgw_flush(struct hmuartlgw_dev *dev)
{
	struct pollfd pfds[1];
	int ret;
	int r = 0;
	uint8_t buf[1024];

	tcflush(dev->fd, TCIOFLUSH);

	while(1) {
		memset(pfds, 0, sizeof(struct pollfd) * 1);

		pfds[0].fd = dev->fd;
		pfds[0].events = POLLIN;

		ret = poll(pfds, 1, 100);
		if (ret <= 0)
			break;

		if (!(pfds[0].revents & POLLIN))
			break;

		memset(buf, 0, sizeof(buf));
		r = read(dev->fd, buf, sizeof(buf));
		if (r <= 0)
			break;
	}

	return;
}

void hmuartlgw_set_debug(int d)
{
	debug = d;
}
Commit	Line	Data
3e34d2ce MG	1	/* HM-MOD-UART/HM-LGW-O-TW-W-EU driver
	2	*
	3	* Copyright (c) 2016 Michael Gernoth <michael@gernoth.net>
	4	*
	5	* Permission is hereby granted, free of charge, to any person obtaining a copy
	6	* of this software and associated documentation files (the "Software"), to
	7	* deal in the Software without restriction, including without limitation the
	8	* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
	9	* sell copies of the Software, and to permit persons to whom the Software is
	10	* furnished to do so, subject to the following conditions:
	11	*
	12	* The above copyright notice and this permission notice shall be included in
	13	* all copies or substantial portions of the Software.
	14	*
	15	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
	16	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	17	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
	18	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
	19	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
	20	* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
	21	* IN THE SOFTWARE.
	22	*/
	23
	24	#include <errno.h>
	25	#include <fcntl.h>
	26	#include <inttypes.h>
	27	#include <poll.h>
	28	#include <stdlib.h>
	29	#include <stdio.h>
	30	#include <string.h>
	31	#include <strings.h>
	32	#include <sys/types.h>
	33	#include <sys/stat.h>
	34	#include <termios.h>
	35	#include <unistd.h>
	36
	37	#include "hexdump.h"
	38	#include "hmuartlgw.h"
	39
	40	#define HMUARTLGW_INIT_TIMEOUT 10000
	41
9248b988	42	#define HMUARTLGW_SETTLE_TIME 1
3e34d2ce MG	43
	44	static int debug = 0;
	45
	46	enum hmuartlgw_state {
	47	HMUARTLGW_QUERY_APPSTATE,
	48	HMUARTLGW_ENTER_BOOTLOADER,
	49	HMUARTLGW_ENTER_BOOTLOADER_ACK,
	50	HMUARTLGW_BOOTLOADER,
	51	HMUARTLGW_ENTER_APPLICATION,
	52	HMUARTLGW_ENTER_APPLICATION_ACK,
	53	HMUARTLGW_APPLICATION,
	54	};
	55
	56	struct recv_data {
	57	enum hmuartlgw_state state;
	58	};
	59
	60
	61	#define CRC16_POLY 0x8005
	62
	63	static uint16_t crc16(uint8_t* buf, int length)
	64	{
	65	uint16_t crc = 0xd77f;
	66	int i;
	67
	68	while (length--) {
	69	crc ^= *buf++ << 8;
	70	for (i = 0; i < 8; i++) {
	71	if (crc & 0x8000) {
	72	crc <<= 1;
	73	crc ^= CRC16_POLY;
	74	} else {
	75	crc <<= 1;
	76	}
	77	}
	78	}
	79
	80	return crc;
	81	}
	82
	83	static int hmuartlgw_init_parse(enum hmuartlgw_dst dst, uint8_t buf, int buf_len, void data)
	84	{
	85	struct recv_data *rdata = data;
	86
	87	#if 0
	88	if (debug) {
	89	printf("Length: %d\n", buf_len);
	90	hexdump(buf, buf_len, "INIT > ");
	91	}
	92	#endif
	93
	94	if (dst != HMUARTLGW_OS)
	95	return 0;
	96
	97	if ((buf_len == 10) && (buf[0] == 0x00) && !strncmp(((char*)buf)+1, "Co_CPU_BL", 9)) {
	98	rdata->state = HMUARTLGW_BOOTLOADER;
	99	return 1;
	100	}
	101
	102	if ((buf_len == 11) && (buf[0] == 0x00) && !strncmp(((char*)buf)+1, "Co_CPU_App", 10)) {
	103	rdata->state = HMUARTLGW_APPLICATION;
	104	return 1;
	105	}
	106
107	switch(rdata->state) {
108	case HMUARTLGW_QUERY_APPSTATE:
9248b988	109	if ((buf[0] == HMUARTLGW_OS_ACK) && (buf[1] == 0x02)) {
3e34d2ce MG	110	if (!strncmp(((char*)buf)+2, "Co_CPU_BL", 9)) {
	111	rdata->state = HMUARTLGW_BOOTLOADER;
	112	} else if (!strncmp(((char*)buf)+2, "Co_CPU_App", 10)) {
	113	rdata->state = HMUARTLGW_APPLICATION;
	114	}
	115	}
	116	break;
	117	case HMUARTLGW_ENTER_BOOTLOADER:
	118	if ((buf_len == 2) &&
9248b988	119	(buf[0] == HMUARTLGW_OS_ACK) &&
3e34d2ce MG	120	(buf[1] == 0x01)) {
	121	rdata->state = HMUARTLGW_ENTER_BOOTLOADER_ACK;
	122	}
	123	break;
	124	case HMUARTLGW_ENTER_BOOTLOADER_ACK:
	125	rdata->state = HMUARTLGW_ENTER_BOOTLOADER;
	126	break;
	127	case HMUARTLGW_ENTER_APPLICATION:
	128	if ((buf_len == 2) &&
9248b988	129	(buf[0] == HMUARTLGW_OS_ACK) &&
3e34d2ce MG	130	(buf[1] == 0x01)) {
	131	rdata->state = HMUARTLGW_ENTER_APPLICATION_ACK;
	132	}
	133	break;
	134	case HMUARTLGW_ENTER_APPLICATION_ACK:
	135	rdata->state = HMUARTLGW_ENTER_APPLICATION;
	136	break;
	137	default:
	138	return 0;
	139	break;
	140	}
	141
	142	return 1;
	143	}
	144
853cbce9	145	struct hmuartlgw_dev hmuart_init(char device, hmuartlgw_cb_fn cb, void *data, int app)
3e34d2ce MG	146	{
	147	struct hmuartlgw_dev *dev = NULL;
	148	struct termios oldtio, tio;
	149
	150	dev = malloc(sizeof(struct hmuartlgw_dev));
	151	if (dev == NULL) {
	152	perror("malloc(struct hmuartlgw_dev)");
	153	return NULL;
	154	}
	155
	156	memset(dev, 0, sizeof(struct hmuartlgw_dev));
	157
	158	dev->fd = open(device, O_RDWR \| O_NOCTTY);
	159	if (dev->fd < 0) {
	160	perror("open(hmuartlgw)");
	161	goto out;
	162	}
	163
	164	if (debug) {
	165	fprintf(stderr, "%s opened\n", device);
	166	}
	167
	168	if (tcgetattr(dev->fd, &oldtio) == -1) {
	169	perror("tcgetattr");
	170	goto out2;
	171	}
	172
	173	memset(&tio, 0, sizeof(tio));
	174
	175	tio.c_cflag = B115200 \| CS8 \| CLOCAL \| CREAD;
	176	tio.c_iflag = IGNPAR;
	177	tio.c_oflag = 0;
	178	tio.c_lflag = 0;
	179	tio.c_cc[VTIME] = 0;
	180	tio.c_cc[VMIN] = 1;
	181
	182	tcflush(dev->fd, TCIFLUSH);
	183	if (tcsetattr(dev->fd, TCSANOW, &tio) == -1) {
	184	perror("tcsetattr");
	185	goto out2;
	186	}
	187
	188	if (debug) {
	189	fprintf(stderr, "serial parameters set\n");
	190	}
	191
	192	hmuartlgw_flush(dev);
	193
853cbce9 MG	194	if (app) {
	195	hmuartlgw_enter_app(dev);
	196	} else {
	197	hmuartlgw_enter_bootloader(dev);
	198	}
3e34d2ce MG	199
	200	dev->cb = cb;
	201	dev->cb_data = data;
	202
	203	return dev;
	204
	205	out2:
	206	close(dev->fd);
	207	out:
	208	free(dev);
	209	return NULL;
	210	}
	211
	212	struct hmuartlgw_dev hmlgw_init(char device, hmuartlgw_cb_fn cb, void *data)
	213	{
	214	struct hmuartlgw_dev *dev = NULL;
	215
	216	return dev;
	217	}
	218
	219	void hmuartlgw_enter_bootloader(struct hmuartlgw_dev *dev)
	220	{
	221	hmuartlgw_cb_fn cb_old = dev->cb;
	222	void *cb_data_old = dev->cb_data;
	223	struct recv_data rdata = { 0 };
	224	uint8_t buf[128] = { 0 };
	225
	226	if (debug) {
	227	fprintf(stderr, "Entering bootloader\n");
	228	}
	229
	230	buf[0] = HMUARTLGW_OS_CHANGE_APP;
	231
	232	dev->cb = hmuartlgw_init_parse;
	233	dev->cb_data = &rdata;
	234
	235	rdata.state = HMUARTLGW_QUERY_APPSTATE;
	236	buf[0] = HMUARTLGW_OS_GET_APP;
	237	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
	238	do {
	239	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
	240	} while (rdata.state == HMUARTLGW_QUERY_APPSTATE);
	241
	242	if (rdata.state != HMUARTLGW_BOOTLOADER) {
	243	rdata.state = HMUARTLGW_ENTER_BOOTLOADER;
	244	buf[0] = HMUARTLGW_OS_CHANGE_APP;
	245	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
	246	do {
	247	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
	248	} while (rdata.state != HMUARTLGW_BOOTLOADER);
	249
	250	printf("Waiting for bootloader to settle...\n");
	251	sleep(HMUARTLGW_SETTLE_TIME);
	252	}
	253
	254	dev->cb = cb_old;
	255	dev->cb_data = cb_data_old;
	256	}
	257
	258	void hmuartlgw_enter_app(struct hmuartlgw_dev *dev)
	259	{
	260	hmuartlgw_cb_fn cb_old = dev->cb;
	261	void *cb_data_old = dev->cb_data;
	262	struct recv_data rdata = { 0 };
263	uint8_t buf[128] = { 0 };
264
265	if (debug) {
266	fprintf(stderr, "Entering application\n");
267	}
268
269	dev->cb = hmuartlgw_init_parse;
270	dev->cb_data = &rdata;
271
272	rdata.state = HMUARTLGW_QUERY_APPSTATE;
273	buf[0] = HMUARTLGW_OS_GET_APP;
274	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
275	do {
276	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
277	} while (rdata.state == HMUARTLGW_QUERY_APPSTATE);
278
279	if (rdata.state != HMUARTLGW_APPLICATION) {
280	rdata.state = HMUARTLGW_ENTER_APPLICATION;
281	buf[0] = HMUARTLGW_OS_CHANGE_APP;
282	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
283	do {
284	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
285	} while (rdata.state != HMUARTLGW_APPLICATION);
286
287	printf("Waiting for application to settle...\n");
288	sleep(HMUARTLGW_SETTLE_TIME);
289	}
290
291	dev->cb = cb_old;
292	dev->cb_data = cb_data_old;
293	}
294
295	static int hmuartlgw_escape(uint8_t *frame, int framelen)
296	{
297	int i;
298
299	for (i = 1; i < framelen; i++) {
300	if (frame[i] == 0xfc \|\| frame[i] == 0xfd) {
301	memmove(frame + i + 1, frame + i, framelen - i);
302	frame[i++] = 0xfc;
303	frame[i] &= 0x7f;
304	framelen++;
305	}
306	}
307	return framelen;
308	}
309
310	int hmuartlgw_send_raw(struct hmuartlgw_dev dev, uint8_t frame, int framelen)
311	{
312	int w = 0;
313	int ret;
314
315	if (debug) {
316	hexdump(frame, framelen, "UARTLGW < ");
317	}
318
319	framelen = hmuartlgw_escape(frame, framelen);
320
321	do {
322	ret = write(dev->fd, frame + w, framelen - w);
323	if (ret < 0) {
324	perror("write");
325	return 0;
326	}
327	w += ret;
328	} while (w < framelen);
329
330	return 1;
331	}
332
333	int hmuartlgw_send(struct hmuartlgw_dev dev, uint8_t cmd, int cmdlen, enum hmuartlgw_dst dst)
334	{
335	static uint8_t cnt = 0;
853cbce9	336	uint8_t frame[4096] = { 0 };
3e34d2ce MG	337	uint16_t crc;
	338
	339	frame[0] = 0xfd;
	340	frame[1] = ((cmdlen + 2) >> 8) & 0xff;
	341	frame[2] = (cmdlen + 2) & 0xff;
	342	frame[3] = dst;
	343	dev->last_send_cnt = cnt;
	344	frame[4] = cnt++;
	345	memcpy(&(frame[5]), cmd, cmdlen);
	346	crc = crc16(frame, cmdlen + 5);
	347	frame[cmdlen + 5] = (crc >> 8) & 0xff;
	348	frame[cmdlen + 6] = crc & 0xff;
	349
	350	return hmuartlgw_send_raw(dev, frame, cmdlen + 7);
	351	}
	352
	353	int hmuartlgw_poll(struct hmuartlgw_dev *dev, int timeout)
	354	{
	355	struct pollfd pfds[1];
	356	int ret;
	357	int r = 0;
	358	uint16_t crc;
	359
	360	errno = 0;
	361
	362	memset(pfds, 0, sizeof(struct pollfd) * 1);
	363
	364	pfds[0].fd = dev->fd;
	365	pfds[0].events = POLLIN;
	366
	367	ret = poll(pfds, 1, timeout);
	368	if (ret == -1)
	369	return -1;
	370
	371	errno = 0;
	372	if (ret == 0) {
	373	errno = ETIMEDOUT;
	374	return -1;
	375	}
	376
	377	if (!(pfds[0].revents & POLLIN)) {
	378	errno = EIO;
	379	return -1;
	380	}
	381
	382	r = read(dev->fd, dev->buf+dev->pos, 1);
	383	if (r < 0)
	384	return -1;
	385
	386	if (r == 0) {
	387	errno = EOF;
	388	return -1;
	389	}
	390
	391	dev->pos += r;
	392
	393	if (dev->buf[0] != 0xfd) {
	394	memset(dev->buf, 0, sizeof(dev->buf));
	395	dev->pos = 0;
	396	dev->unescape_next = 0;
	397	return -1;
	398	}
	399
	400	if (dev->unescape_next) {
401	dev->buf[dev->pos-1] \|= 0x80;
402	dev->unescape_next = 0;
403	} else if (dev->buf[dev->pos-1] == 0xfc) {
404	dev->unescape_next = 1;
405	dev->pos--;
406	return -1;
407	}
408
409	if (dev->pos >= 3) {
410	uint16_t len;
411
412	len = ((dev->buf[1] << 8) & 0xff00) \| (dev->buf[2] & 0xff);
413
414	if (dev->pos < len + 5)
415	return -1;
416	} else {
417	return -1;
418	}
419
b1d2ccc2 MG	420	crc = crc16(dev->buf, dev->pos);
b1d2ccc2 MG	421	if (crc == 0x0000) {
3e34d2ce MG	422	if (debug)
	423	hexdump(dev->buf, dev->pos, "UARTLGW > ");
	424
	425	dev->cb(dev->buf[3], dev->buf + 5 , dev->pos - 7, dev->cb_data);
	426
	427	memset(dev->buf, 0, sizeof(dev->buf));
	428	dev->pos = 0;
	429	dev->unescape_next = 0;
	430	} else {
	431	fprintf(stderr, "Invalid checksum received!\n");
	432	hexdump(dev->buf, dev->pos, "ERR> ");
	433	printf("calculated: %04x\n", crc);
	434
	435	memset(dev->buf, 0, sizeof(dev->buf));
	436	dev->pos = 0;
	437	dev->unescape_next = 0;
	438	}
	439
	440	errno = 0;
	441	return -1;
	442	}
	443
	444	void hmuartlgw_close(struct hmuartlgw_dev *dev)
	445	{
	446	close(dev->fd);
	447	}
	448
	449	void hmuartlgw_flush(struct hmuartlgw_dev *dev)
	450	{
	451	struct pollfd pfds[1];
	452	int ret;
	453	int r = 0;
	454	uint8_t buf[1024];
	455
	456	tcflush(dev->fd, TCIOFLUSH);
	457
	458	while(1) {
	459	memset(pfds, 0, sizeof(struct pollfd) * 1);
	460
	461	pfds[0].fd = dev->fd;
	462	pfds[0].events = POLLIN;
	463
	464	ret = poll(pfds, 1, 100);
	465	if (ret <= 0)
	466	break;
	467
	468	if (!(pfds[0].revents & POLLIN))
	469	break;
	470
	471	memset(buf, 0, sizeof(buf));
	472	r = read(dev->fd, buf, sizeof(buf));
	473	if (r <= 0)
	474	break;
	475	}
	476
	477	return;
	478	}
	479
	480	void hmuartlgw_set_debug(int d)
	481	{
	482	debug = d;
	483	}