[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	2

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] == 0x04) && (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] == 0x04) &&
			    (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] == 0x04) &&
			    (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)
{
	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);

	hmuartlgw_enter_app(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[1024] = { 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 - 2);
	if ((((crc >> 8) & 0xff) == dev->buf[dev->pos - 2]) &&
			((crc & 0xff) == dev->buf[dev->pos - 1])) {

		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
	42	#define HMUARTLGW_SETTLE_TIME 2
	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:
109	if ((buf[0] == 0x04) && (buf[1] == 0x02)) {
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) &&
119	(buf[0] == 0x04) &&
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) &&
129	(buf[0] == 0x04) &&
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
145	struct hmuartlgw_dev hmuart_init(char device, hmuartlgw_cb_fn cb, void *data)
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
194	hmuartlgw_enter_app(dev);
195
196	dev->cb = cb;
197	dev->cb_data = data;
198
199	return dev;
200
201	out2:
202	close(dev->fd);
203	out:
204	free(dev);
205	return NULL;
206	}
207
208	struct hmuartlgw_dev hmlgw_init(char device, hmuartlgw_cb_fn cb, void *data)
209	{
210	struct hmuartlgw_dev *dev = NULL;
211
212	return dev;
213	}
214
215	void hmuartlgw_enter_bootloader(struct hmuartlgw_dev *dev)
216	{
217	hmuartlgw_cb_fn cb_old = dev->cb;
218	void *cb_data_old = dev->cb_data;
219	struct recv_data rdata = { 0 };
220	uint8_t buf[128] = { 0 };
221
222	if (debug) {
223	fprintf(stderr, "Entering bootloader\n");
224	}
225
226	buf[0] = HMUARTLGW_OS_CHANGE_APP;
227
228	dev->cb = hmuartlgw_init_parse;
229	dev->cb_data = &rdata;
230
231	rdata.state = HMUARTLGW_QUERY_APPSTATE;
232	buf[0] = HMUARTLGW_OS_GET_APP;
233	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
234	do {
235	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
236	} while (rdata.state == HMUARTLGW_QUERY_APPSTATE);
237
238	if (rdata.state != HMUARTLGW_BOOTLOADER) {
239	rdata.state = HMUARTLGW_ENTER_BOOTLOADER;
240	buf[0] = HMUARTLGW_OS_CHANGE_APP;
241	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
242	do {
243	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
244	} while (rdata.state != HMUARTLGW_BOOTLOADER);
245
246	printf("Waiting for bootloader to settle...\n");
247	sleep(HMUARTLGW_SETTLE_TIME);
248	}
249
250	dev->cb = cb_old;
251	dev->cb_data = cb_data_old;
252	}
253
254	void hmuartlgw_enter_app(struct hmuartlgw_dev *dev)
255	{
256	hmuartlgw_cb_fn cb_old = dev->cb;
257	void *cb_data_old = dev->cb_data;
258	struct recv_data rdata = { 0 };
259	uint8_t buf[128] = { 0 };
260
261	if (debug) {
262	fprintf(stderr, "Entering application\n");
263	}
264
265	dev->cb = hmuartlgw_init_parse;
266	dev->cb_data = &rdata;
267
268	rdata.state = HMUARTLGW_QUERY_APPSTATE;
269	buf[0] = HMUARTLGW_OS_GET_APP;
270	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
271	do {
272	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
273	} while (rdata.state == HMUARTLGW_QUERY_APPSTATE);
274
275	if (rdata.state != HMUARTLGW_APPLICATION) {
276	rdata.state = HMUARTLGW_ENTER_APPLICATION;
277	buf[0] = HMUARTLGW_OS_CHANGE_APP;
278	hmuartlgw_send(dev, buf, 1, HMUARTLGW_OS);
279	do {
280	hmuartlgw_poll(dev, HMUARTLGW_INIT_TIMEOUT);
281	} while (rdata.state != HMUARTLGW_APPLICATION);
282
283	printf("Waiting for application to settle...\n");
284	sleep(HMUARTLGW_SETTLE_TIME);
285	}
286
287	dev->cb = cb_old;
288	dev->cb_data = cb_data_old;
289	}
290
291	static int hmuartlgw_escape(uint8_t *frame, int framelen)
292	{
293	int i;
294
295	for (i = 1; i < framelen; i++) {
296	if (frame[i] == 0xfc \|\| frame[i] == 0xfd) {
297	memmove(frame + i + 1, frame + i, framelen - i);
298	frame[i++] = 0xfc;
299	frame[i] &= 0x7f;
300	framelen++;
301	}
302	}
303	return framelen;
304	}
305
306	int hmuartlgw_send_raw(struct hmuartlgw_dev dev, uint8_t frame, int framelen)
307	{
308	int w = 0;
309	int ret;
310
311	if (debug) {
312	hexdump(frame, framelen, "UARTLGW < ");
313	}
314
315	framelen = hmuartlgw_escape(frame, framelen);
316
317	do {
318	ret = write(dev->fd, frame + w, framelen - w);
319	if (ret < 0) {
320	perror("write");
321	return 0;
322	}
323	w += ret;
324	} while (w < framelen);
325
326	return 1;
327	}
328
329	int hmuartlgw_send(struct hmuartlgw_dev dev, uint8_t cmd, int cmdlen, enum hmuartlgw_dst dst)
330	{
331	static uint8_t cnt = 0;
332	uint8_t frame[1024] = { 0 };
333	uint16_t crc;
334
335	frame[0] = 0xfd;
336	frame[1] = ((cmdlen + 2) >> 8) & 0xff;
337	frame[2] = (cmdlen + 2) & 0xff;
338	frame[3] = dst;
339	dev->last_send_cnt = cnt;
340	frame[4] = cnt++;
341	memcpy(&(frame[5]), cmd, cmdlen);
342	crc = crc16(frame, cmdlen + 5);
343	frame[cmdlen + 5] = (crc >> 8) & 0xff;
344	frame[cmdlen + 6] = crc & 0xff;
345
346	return hmuartlgw_send_raw(dev, frame, cmdlen + 7);
347	}
348
349	int hmuartlgw_poll(struct hmuartlgw_dev *dev, int timeout)
350	{
351	struct pollfd pfds[1];
352	int ret;
353	int r = 0;
354	uint16_t crc;
355
356	errno = 0;
357
358	memset(pfds, 0, sizeof(struct pollfd) * 1);
359
360	pfds[0].fd = dev->fd;
361	pfds[0].events = POLLIN;
362
363	ret = poll(pfds, 1, timeout);
364	if (ret == -1)
365	return -1;
366
367	errno = 0;
368	if (ret == 0) {
369	errno = ETIMEDOUT;
370	return -1;
371	}
372
373	if (!(pfds[0].revents & POLLIN)) {
374	errno = EIO;
375	return -1;
376	}
377
378	r = read(dev->fd, dev->buf+dev->pos, 1);
379	if (r < 0)
380	return -1;
381
382	if (r == 0) {
383	errno = EOF;
384	return -1;
385	}
386
387	dev->pos += r;
388
389	if (dev->buf[0] != 0xfd) {
390	memset(dev->buf, 0, sizeof(dev->buf));
391	dev->pos = 0;
392	dev->unescape_next = 0;
393	return -1;
394	}
395
396	if (dev->unescape_next) {
397	dev->buf[dev->pos-1] \|= 0x80;
398	dev->unescape_next = 0;
399	} else if (dev->buf[dev->pos-1] == 0xfc) {
400	dev->unescape_next = 1;
401	dev->pos--;
402	return -1;
403	}
404
405	if (dev->pos >= 3) {
406	uint16_t len;
407
408	len = ((dev->buf[1] << 8) & 0xff00) \| (dev->buf[2] & 0xff);
409
410	if (dev->pos < len + 5)
411	return -1;
412	} else {
413	return -1;
414	}
415
416	crc = crc16(dev->buf, dev->pos - 2);
417	if ((((crc >> 8) & 0xff) == dev->buf[dev->pos - 2]) &&
418	((crc & 0xff) == dev->buf[dev->pos - 1])) {
419
420	if (debug)
421	hexdump(dev->buf, dev->pos, "UARTLGW > ");
422
423	dev->cb(dev->buf[3], dev->buf + 5 , dev->pos - 7, dev->cb_data);
424
425	memset(dev->buf, 0, sizeof(dev->buf));
426	dev->pos = 0;
427	dev->unescape_next = 0;
428	} else {
429	fprintf(stderr, "Invalid checksum received!\n");
430	hexdump(dev->buf, dev->pos, "ERR> ");
431	printf("calculated: %04x\n", crc);
432
433	memset(dev->buf, 0, sizeof(dev->buf));
434	dev->pos = 0;
435	dev->unescape_next = 0;
436	}
437
438	errno = 0;
439	return -1;
440	}
441
442	void hmuartlgw_close(struct hmuartlgw_dev *dev)
443	{
444	close(dev->fd);
445	}
446
447	void hmuartlgw_flush(struct hmuartlgw_dev *dev)
448	{
449	struct pollfd pfds[1];
450	int ret;
451	int r = 0;
452	uint8_t buf[1024];
453
454	tcflush(dev->fd, TCIOFLUSH);
455
456	while(1) {
457	memset(pfds, 0, sizeof(struct pollfd) * 1);
458
459	pfds[0].fd = dev->fd;
460	pfds[0].events = POLLIN;
461
462	ret = poll(pfds, 1, 100);
463	if (ret <= 0)
464	break;
465
466	if (!(pfds[0].revents & POLLIN))
467	break;
468
469	memset(buf, 0, sizeof(buf));
470	r = read(dev->fd, buf, sizeof(buf));
471	if (r <= 0)
472	break;
473	}
474
475	return;
476	}
477
478	void hmuartlgw_set_debug(int d)
479	{
480	debug = d;
481	}