[hmcfgusb] / hmland.c

/* HM-CFG-LAN emuldation for HM-CFG-USB
 *
 * Copyright (c) 2013 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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <strings.h>
#include <poll.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <libusb-1.0/libusb.h>

#include "hexdump.h"
#include "hmcfgusb.h"

static int impersonate_hmlanif = 0;

#define	FLAG_LENGTH_BYTE	(1<<0)
#define	FLAG_FORMAT_HEX		(1<<1)
#define	FLAG_COMMA_BEFORE	(1<<2)
#define	FLAG_COMMA_AFTER	(1<<3)
#define	FLAG_NL			(1<<4)
#define	FLAG_IGNORE_COMMAS	(1<<5)

#define CHECK_SPACE(x)		if ((*outpos + x) > outend) { fprintf(stderr, "Not enough space!\n"); return 0; }
#define CHECK_AVAIL(x)		if ((*inpos + x) > inend) { fprintf(stderr, "Not enough input available!\n"); return 0; }

static int format_part_out(uint8_t **inpos, int inlen, uint8_t **outpos, int outlen, int len, int flags)
{
	uint8_t *buf_out = *outpos;
	uint8_t *outend = *outpos + outlen;
	uint8_t *inend = *inpos + inlen;
	int i;

	if (flags & FLAG_COMMA_BEFORE) {
		CHECK_SPACE(1);
		**outpos=',';
		*outpos += 1;
	}

	if (flags & FLAG_LENGTH_BYTE) {
		CHECK_AVAIL(1);
		len = **inpos;
		*inpos += 1;
	}

	if (flags & FLAG_FORMAT_HEX) {
		char hex[3];

		memset(hex, 0, sizeof(hex));

		CHECK_AVAIL(len);
		CHECK_SPACE(len*2);
		for (i = 0; i < len; i++) {
			if (snprintf(hex, sizeof(hex), "%02X", **inpos) != 2) {
				fprintf(stderr, "Can't format hex-string!\n");
				return 0;
			}
			*inpos += 1;
			memcpy(*outpos, hex, 2);
			*outpos += 2;
		}
	} else {
		CHECK_AVAIL(len);
		CHECK_SPACE(len);
		memcpy(*outpos, *inpos, len);
		*outpos += len;
		*inpos += len;
	}

	if (flags & FLAG_COMMA_AFTER) {
		CHECK_SPACE(1);
		**outpos=',';
		*outpos += 1;
	}

	if (flags & FLAG_NL) {
		CHECK_SPACE(2);
		**outpos='\r';
		*outpos += 1;
		**outpos='\n';
		*outpos += 1;
	}

	return *outpos - buf_out;
}

static int parse_part_in(uint8_t **inpos, int inlen, uint8_t **outpos, int outlen, int flags)
{
	uint8_t *buf_out = *outpos;
	uint8_t *outend = *outpos + outlen;
	uint8_t *inend = *inpos + inlen;
	char hex[3];

	memset(hex, 0, sizeof(hex));

	if (flags & FLAG_LENGTH_BYTE) {
		int len = 0;
		uint8_t *ip;

		ip = *inpos;
		while(ip < inend) {
			if (*ip == ',') {
				ip++;
				if (!(flags & FLAG_IGNORE_COMMAS))
					break;

				continue;
			}
			len++;
			ip++;
		}
		CHECK_SPACE(1);
		**outpos = (len / 2);
		*outpos += 1;
	}

	while(*inpos < inend) {
		if (**inpos == ',') {
			*inpos += 1;
			if (!(flags & FLAG_IGNORE_COMMAS))
				break;

			continue;
		}

		CHECK_SPACE(1);
		CHECK_AVAIL(2);
		memcpy(hex, *inpos, 2);
		*inpos += 2;

		**outpos = strtoul(hex, NULL, 16);
		*outpos += 1;
	}

	return *outpos - buf_out;
}

static void hmlan_format_out(uint8_t *buf, int buf_len, void *data)
{
	uint8_t out[1024];
	uint8_t *outpos;
	uint8_t *inpos;
	int fd = *((int*)data);

	if (buf_len < 1)
		return;

	memset(out, 0, sizeof(out));
	outpos = out;
	inpos = buf;

	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, 0);
	switch(buf[0]) {
		case 'H':
			if (impersonate_hmlanif) {
				buf[5] = 'L';
				buf[6] = 'A';
				buf[7] = 'N';
			}
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_LENGTH_BYTE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_COMMA_BEFORE | FLAG_LENGTH_BYTE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 3, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 3, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE | FLAG_NL);

			break;
		case 'E':
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 3, FLAG_FORMAT_HEX);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE | FLAG_LENGTH_BYTE | FLAG_NL);

			break;
		case 'R':
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE | FLAG_LENGTH_BYTE | FLAG_NL);

			break;
		case 'I':
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE);
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX | FLAG_COMMA_BEFORE | FLAG_NL);

			break;
		default:
			format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), buf_len-1, FLAG_FORMAT_HEX | FLAG_NL);
			hexdump(buf, buf_len, "Unknown> ");
			break;
	}
	write(fd, out, outpos-out);
}

static int hmlan_parse_in(int fd, void *data)
{
	struct hmcfgusb_dev *dev = data;
	uint8_t buf[1024];
	uint8_t out[0x40]; //FIXME!!!
	uint8_t *outpos;
	uint8_t *inpos;
	int i;
	int r;

	r = read(fd, buf, sizeof(buf));
	if (r > 0) {
		memset(out, 0, sizeof(out));
		for (i = 0; i < r; i++) {
			if ((buf[i] == 0x0a) ||
					(buf[i] == 0x0d)) {
				r = i;
				break;
			}
		}

		if (r == 0)
			return 1;

		out[0] = buf[0];
		outpos = out+1;
		inpos = buf+1;

		switch(buf[0]) {
			case 'S':
				parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
				parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
				parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
				parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
				parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
				parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), FLAG_LENGTH_BYTE);
				break;
			default:
				parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), FLAG_IGNORE_COMMAS);
				break;
		}

		hmcfgusb_send(dev, out, outpos-out, 1);
	} else if (r < 0) {
		perror("read");
		return r;
	} else {
		return 0;
	}

	return 1;
}

static int comm(int fd_in, int fd_out, int master_socket)
{
	struct hmcfgusb_dev *dev;
	int quit = 0;

	dev = hmcfgusb_init(hmlan_format_out, &fd_out);
	if (!dev) {
		fprintf(stderr, "Can't initialize HM-CFG-USB!\n");
		return 0;
	}

	if (!hmcfgusb_add_pfd(dev, fd_in, POLLIN)) {
		fprintf(stderr, "Can't add client to pollfd!\n");
		hmcfgusb_close(dev);
		return 0;
	}

	if (master_socket >= 0) {
		if (!hmcfgusb_add_pfd(dev, master_socket, POLLIN)) {
			fprintf(stderr, "Can't add master_socket to pollfd!\n");
			hmcfgusb_close(dev);
			return 0;
		}
	}

	hmcfgusb_send(dev, (unsigned char*)"K", 1, 1);

	while(!quit) {
		int fd;

		fd = hmcfgusb_poll(dev, 3600);
		if (fd >= 0) {
			if (fd == master_socket) {
				int client;

				client = accept(master_socket, NULL, 0);
				if (client >= 0) {
					shutdown(client, SHUT_RDWR);
					close(client);
				}
			} else {
				if (hmlan_parse_in(fd, dev) <= 0) {
					quit = 1;
				}
			}
		} else if (fd == -1) {
			if (errno) {
				perror("hmcfgusb_poll");
				quit = 1;
			}
		}
	}

	hmcfgusb_close(dev);
	return 1;
}

static int socket_server(int port)
{
	struct sockaddr_in sin;
	int sock;
	int n;

	impersonate_hmlanif = 1;

	sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (sock == -1) {
		perror("Can't open socket");
		return EXIT_FAILURE;
	}

	n = 1;
	if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &n, sizeof(n)) == -1) {
		perror("Can't set socket options");
		return EXIT_FAILURE;
	}

	memset(&sin, 0, sizeof(sin));
	sin.sin_family = AF_INET;
	sin.sin_port = htons(port);
	sin.sin_addr.s_addr = htonl(INADDR_ANY);

	if (bind(sock, (struct sockaddr*)&sin, sizeof(sin)) == -1) {
		perror("Can't bind socket");
		return EXIT_FAILURE;
	}

	if (listen(sock, 1) == -1) {
		perror("Can't listen on socket");
		return EXIT_FAILURE;
	}

	while(1) {
		struct sockaddr_in csin;
		socklen_t csinlen;
		int client;

		memset(&csin, 0, sizeof(csin));
		csinlen = sizeof(csin);
		client = accept(sock, (struct sockaddr*)&csin, &csinlen);
		if (client == -1) {
			perror("Couldn't accept client");
			continue;
		}

		printf("Client accepted!\n");

		comm(client, client, sock);

		shutdown(client, SHUT_RDWR);
		close(client);

		printf("Connection closed!\n");

	}

	return EXIT_SUCCESS;
}

static int interactive_server(void)
{
	if (!comm(STDIN_FILENO, STDOUT_FILENO, -1))
		return EXIT_FAILURE;

	return EXIT_SUCCESS;
}

int main(int argc, char **argv)
{
	//return interactive_server();
	return socket_server(1234);
}
Commit	Line	Data
9db2e455 MG	1	/* HM-CFG-LAN emuldation for HM-CFG-USB
	2	*
	3	* Copyright (c) 2013 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 <stdio.h>
	25	#include <stdlib.h>
	26	#include <unistd.h>
	27	#include <stdint.h>
	28	#include <string.h>
	29	#include <strings.h>
	30	#include <poll.h>
	31	#include <errno.h>
e0a7146e MG	32	#include <sys/types.h>
	33	#include <sys/socket.h>
	34	#include <netinet/in.h>
	35	#include <arpa/inet.h>
9db2e455 MG	36	#include <libusb-1.0/libusb.h>
	37
	38	#include "hexdump.h"
	39	#include "hmcfgusb.h"
	40
e0a7146e MG	41	static int impersonate_hmlanif = 0;
e0a7146e MG	42
e75295bb MG	43	#define FLAG_LENGTH_BYTE (1<<0)
	44	#define FLAG_FORMAT_HEX (1<<1)
	45	#define FLAG_COMMA_BEFORE (1<<2)
	46	#define FLAG_COMMA_AFTER (1<<3)
	47	#define FLAG_NL (1<<4)
	48	#define FLAG_IGNORE_COMMAS (1<<5)
	49
	50	#define CHECK_SPACE(x) if ((*outpos + x) > outend) { fprintf(stderr, "Not enough space!\n"); return 0; }
	51	#define CHECK_AVAIL(x) if ((*inpos + x) > inend) { fprintf(stderr, "Not enough input available!\n"); return 0; }
	52
	53	static int format_part_out(uint8_t inpos, int inlen, uint8_t outpos, int outlen, int len, int flags)
	54	{
	55	uint8_t buf_out = outpos;
	56	uint8_t outend = outpos + outlen;
	57	uint8_t inend = inpos + inlen;
	58	int i;
	59
	60	if (flags & FLAG_COMMA_BEFORE) {
	61	CHECK_SPACE(1);
	62	**outpos=',';
	63	*outpos += 1;
	64	}
	65
	66	if (flags & FLAG_LENGTH_BYTE) {
	67	CHECK_AVAIL(1);
	68	len = **inpos;
	69	*inpos += 1;
	70	}
	71
	72	if (flags & FLAG_FORMAT_HEX) {
	73	char hex[3];
	74
	75	memset(hex, 0, sizeof(hex));
	76
	77	CHECK_AVAIL(len);
	78	CHECK_SPACE(len*2);
	79	for (i = 0; i < len; i++) {
	80	if (snprintf(hex, sizeof(hex), "%02X", **inpos) != 2) {
	81	fprintf(stderr, "Can't format hex-string!\n");
	82	return 0;
	83	}
	84	*inpos += 1;
	85	memcpy(*outpos, hex, 2);
	86	*outpos += 2;
	87	}
	88	} else {
	89	CHECK_AVAIL(len);
	90	CHECK_SPACE(len);
	91	memcpy(outpos, inpos, len);
	92	*outpos += len;
	93	*inpos += len;
	94	}
	95
	96	if (flags & FLAG_COMMA_AFTER) {
	97	CHECK_SPACE(1);
	98	**outpos=',';
	99	*outpos += 1;
	100	}
	101
	102	if (flags & FLAG_NL) {
	103	CHECK_SPACE(2);
	104	**outpos='\r';
	105	*outpos += 1;
	106	**outpos='\n';
107	*outpos += 1;
108	}
109
110	return *outpos - buf_out;
111	}
112
113	static int parse_part_in(uint8_t inpos, int inlen, uint8_t outpos, int outlen, int flags)
114	{
115	uint8_t buf_out = outpos;
116	uint8_t outend = outpos + outlen;
117	uint8_t inend = inpos + inlen;
118	char hex[3];
119
120	memset(hex, 0, sizeof(hex));
121
122	if (flags & FLAG_LENGTH_BYTE) {
123	int len = 0;
124	uint8_t *ip;
125
126	ip = *inpos;
127	while(ip < inend) {
128	if (*ip == ',') {
129	ip++;
130	if (!(flags & FLAG_IGNORE_COMMAS))
131	break;
132
133	continue;
134	}
135	len++;
136	ip++;
137	}
138	CHECK_SPACE(1);
139	**outpos = (len / 2);
140	*outpos += 1;
141	}
142
143	while(*inpos < inend) {
144	if (**inpos == ',') {
145	*inpos += 1;
146	if (!(flags & FLAG_IGNORE_COMMAS))
147	break;
148
149	continue;
150	}
151
152	CHECK_SPACE(1);
153	CHECK_AVAIL(2);
154	memcpy(hex, *inpos, 2);
155	*inpos += 2;
156
157	**outpos = strtoul(hex, NULL, 16);
158	*outpos += 1;
159	}
160
161	return *outpos - buf_out;
162	}
163
e0a7146e	164	static void hmlan_format_out(uint8_t buf, int buf_len, void data)
9db2e455	165	{
e75295bb MG	166	uint8_t out[1024];
	167	uint8_t *outpos;
	168	uint8_t *inpos;
e0a7146e	169	int fd = ((int)data);
9db2e455 MG	170
	171	if (buf_len < 1)
	172	return;
	173
e0a7146e	174	memset(out, 0, sizeof(out));
e75295bb MG	175	outpos = out;
e75295bb MG	176	inpos = buf;
e0a7146e	177
e75295bb	178	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, 0);
9db2e455 MG	179	switch(buf[0]) {
9db2e455 MG	180	case 'H':
e0a7146e MG	181	if (impersonate_hmlanif) {
	182	buf[5] = 'L';
	183	buf[6] = 'A';
	184	buf[7] = 'N';
	185	}
e75295bb MG	186	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_LENGTH_BYTE);
	187	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	188	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_COMMA_BEFORE \| FLAG_LENGTH_BYTE);
	189	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 3, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	190	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 3, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	191	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	192	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE \| FLAG_NL);
9db2e455 MG	193
	194	break;
	195	case 'E':
e75295bb MG	196	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 3, FLAG_FORMAT_HEX);
	197	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	198	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	199	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	200	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	201	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE \| FLAG_LENGTH_BYTE \| FLAG_NL);
	202
9db2e455 MG	203	break;
9db2e455 MG	204	case 'R':
e75295bb MG	205	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX);
	206	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	207	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 4, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	208	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	209	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 2, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	210	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE \| FLAG_LENGTH_BYTE \| FLAG_NL);
	211
9db2e455 MG	212	break;
9db2e455 MG	213	case 'I':
e75295bb MG	214	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX);
	215	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	216	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE);
	217	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, FLAG_FORMAT_HEX \| FLAG_COMMA_BEFORE \| FLAG_NL);
	218
	219	break;
9db2e455	220	default:
e75295bb	221	format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), buf_len-1, FLAG_FORMAT_HEX \| FLAG_NL);
9db2e455 MG	222	hexdump(buf, buf_len, "Unknown> ");
	223	break;
	224	}
e75295bb	225	write(fd, out, outpos-out);
9db2e455 MG	226	}
9db2e455 MG	227
e0a7146e	228	static int hmlan_parse_in(int fd, void *data)
9db2e455 MG	229	{
9db2e455 MG	230	struct hmcfgusb_dev *dev = data;
e75295bb MG	231	uint8_t buf[1024];
	232	uint8_t out[0x40]; //FIXME!!!
	233	uint8_t *outpos;
	234	uint8_t *inpos;
9db2e455 MG	235	int i;
	236	int r;
	237
	238	r = read(fd, buf, sizeof(buf));
	239	if (r > 0) {
e75295bb	240	memset(out, 0, sizeof(out));
9db2e455 MG	241	for (i = 0; i < r; i++) {
	242	if ((buf[i] == 0x0a) \|\|
	243	(buf[i] == 0x0d)) {
	244	r = i;
	245	break;
	246	}
	247	}
	248
e75295bb MG	249	if (r == 0)
	250	return 1;
	251
	252	out[0] = buf[0];
	253	outpos = out+1;
	254	inpos = buf+1;
9db2e455	255
e75295bb MG	256	switch(buf[0]) {
	257	case 'S':
	258	parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
	259	parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
	260	parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
	261	parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
	262	parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 0);
	263	parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), FLAG_LENGTH_BYTE);
	264	break;
	265	default:
	266	parse_part_in(&inpos, (r-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), FLAG_IGNORE_COMMAS);
	267	break;
9db2e455	268	}
e75295bb MG	269
e75295bb MG	270	hmcfgusb_send(dev, out, outpos-out, 1);
9db2e455 MG	271	} else if (r < 0) {
9db2e455 MG	272	perror("read");
e0a7146e MG	273	return r;
	274	} else {
	275	return 0;
9db2e455	276	}
e0a7146e MG	277
e0a7146e MG	278	return 1;
9db2e455 MG	279	}
9db2e455 MG	280
e0a7146e	281	static int comm(int fd_in, int fd_out, int master_socket)
9db2e455 MG	282	{
	283	struct hmcfgusb_dev *dev;
	284	int quit = 0;
	285
e0a7146e	286	dev = hmcfgusb_init(hmlan_format_out, &fd_out);
9db2e455 MG	287	if (!dev) {
9db2e455 MG	288	fprintf(stderr, "Can't initialize HM-CFG-USB!\n");
e0a7146e MG	289	return 0;
	290	}
	291
	292	if (!hmcfgusb_add_pfd(dev, fd_in, POLLIN)) {
	293	fprintf(stderr, "Can't add client to pollfd!\n");
	294	hmcfgusb_close(dev);
	295	return 0;
9db2e455 MG	296	}
9db2e455 MG	297
e0a7146e MG	298	if (master_socket >= 0) {
	299	if (!hmcfgusb_add_pfd(dev, master_socket, POLLIN)) {
	300	fprintf(stderr, "Can't add master_socket to pollfd!\n");
	301	hmcfgusb_close(dev);
	302	return 0;
	303	}
9db2e455 MG	304	}
	305
	306	hmcfgusb_send(dev, (unsigned char*)"K", 1, 1);
	307
	308	while(!quit) {
	309	int fd;
	310
	311	fd = hmcfgusb_poll(dev, 3600);
	312	if (fd >= 0) {
e0a7146e MG	313	if (fd == master_socket) {
	314	int client;
	315
	316	client = accept(master_socket, NULL, 0);
	317	if (client >= 0) {
	318	shutdown(client, SHUT_RDWR);
	319	close(client);
	320	}
	321	} else {
	322	if (hmlan_parse_in(fd, dev) <= 0) {
	323	quit = 1;
	324	}
	325	}
9db2e455 MG	326	} else if (fd == -1) {
	327	if (errno) {
	328	perror("hmcfgusb_poll");
	329	quit = 1;
	330	}
	331	}
	332	}
	333
813afd12	334	hmcfgusb_close(dev);
e0a7146e MG	335	return 1;
	336	}
	337
	338	static int socket_server(int port)
	339	{
	340	struct sockaddr_in sin;
	341	int sock;
	342	int n;
	343
	344	impersonate_hmlanif = 1;
	345
	346	sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
	347	if (sock == -1) {
	348	perror("Can't open socket");
	349	return EXIT_FAILURE;
	350	}
	351
	352	n = 1;
	353	if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &n, sizeof(n)) == -1) {
	354	perror("Can't set socket options");
	355	return EXIT_FAILURE;
	356	}
	357
	358	memset(&sin, 0, sizeof(sin));
	359	sin.sin_family = AF_INET;
	360	sin.sin_port = htons(port);
	361	sin.sin_addr.s_addr = htonl(INADDR_ANY);
	362
	363	if (bind(sock, (struct sockaddr*)&sin, sizeof(sin)) == -1) {
	364	perror("Can't bind socket");
	365	return EXIT_FAILURE;
	366	}
	367
	368	if (listen(sock, 1) == -1) {
	369	perror("Can't listen on socket");
	370	return EXIT_FAILURE;
	371	}
	372
	373	while(1) {
	374	struct sockaddr_in csin;
	375	socklen_t csinlen;
	376	int client;
	377
	378	memset(&csin, 0, sizeof(csin));
	379	csinlen = sizeof(csin);
	380	client = accept(sock, (struct sockaddr*)&csin, &csinlen);
	381	if (client == -1) {
	382	perror("Couldn't accept client");
	383	continue;
	384	}
	385
	386	printf("Client accepted!\n");
	387
	388	comm(client, client, sock);
	389
	390	shutdown(client, SHUT_RDWR);
	391	close(client);
	392
	393	printf("Connection closed!\n");
	394
	395	}
	396
	397	return EXIT_SUCCESS;
	398	}
399
400	static int interactive_server(void)
401	{
402	if (!comm(STDIN_FILENO, STDOUT_FILENO, -1))
403	return EXIT_FAILURE;
9db2e455 MG	404
	405	return EXIT_SUCCESS;
	406	}
e0a7146e MG	407
	408	int main(int argc, char **argv)
	409	{
	410	//return interactive_server();
	411	return socket_server(1234);
	412	}