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

static void hmlan_format_out(uint8_t *buf, int buf_len, void *data)
{
	char out[1024];
	char *pos;
	int fd = *((int*)data);
	int len;
	int i;

	if (buf_len < 1)
		return;

	memset(out, 0, sizeof(out));
	pos = out;

	*pos++ = buf[0];
	switch(buf[0]) {
		case 'H':
			if (impersonate_hmlanif) {
				buf[5] = 'L';
				buf[6] = 'A';
				buf[7] = 'N';
			}
			len = buf[1];
			for (i = 2; i < len + 2; i++) {
				*pos++=buf[i];
			}
			snprintf(pos, 7, ",%02X%02X,", buf[i], buf[i+1]);
			pos += 6;

			i += 2;
			len = buf[i]+i+1;
			i++;
			for (; i < len; i++) {
				*pos++=buf[i];
			}
			*pos++=',';
			len = i+12;
			for (; i < len; i++) {
				snprintf(pos, 3, "%02X", buf[i]);
				pos += 2;

				switch(len-i) {
					case 10:
					case 7:
					case 3:
						*pos++=',';
						break;
					default:
						break;
				}
			}

			break;
		case 'E':
			len = 13 + buf[13];
			for (i = 0; i < len; i++) {
				if (i != 12) {
					snprintf(pos, 3, "%02X", buf[i+1]);
					pos += 2;
				}
				switch(i) {
					case 2:
					case 4:
					case 8:
					case 9:
					case 11:
						*pos++=',';
						break;
					default:
						break;
				}
			}
			break;
		case 'R':
			len = 14 + buf[14];
			for (i = 0; i < len; i++) {
				if (i != 13) {
					snprintf(pos, 3, "%02X", buf[i+1]);
					pos += 2;
				}
				switch(i) {
					case 3:
					case 5:
					case 9:
					case 10:
					case 12:
						*pos++=',';
						break;
					default:
						break;
				}
			}
			break;
		case 'I':
			//HM> 0x0000: 49 00 00 00 00 55 53 42 2d 49 46 03 bc 0a 4a 45   I....USB-IF...JE
			//HM> 0x0010: 51 30 35 33 35 31 32 32 1d b1 55 68 ea 13 00 14   Q0535122..Uh....
			//HM> 0x0020: 9f a6 00 03 00 00 00 00 00 00 00 00 00 00 00 00   ................
			//HM> 0x0030: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00   ................ 
		default:
			for (i = 1; i < buf_len; i++) {
				snprintf(pos, 3, "%02X", buf[i]);
				pos += 2;
			}
			hexdump(buf, buf_len, "Unknown> ");
			break;
	}
	*pos++='\r';
	*pos++='\n';
	write(fd, out, pos-out);
}

static int hmlan_parse_in(int fd, void *data)
{
	struct hmcfgusb_dev *dev = data;
	unsigned char buf[1024];
	unsigned char send_buf[0x40]; //FIXME!!!
	char tmp[3];
	int i;
	int r;

	r = read(fd, buf, sizeof(buf));
	if (r > 0) {
		int cnt;

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

		send_buf[0] = buf[0];

		cnt = 0;
		for (i = 1; i < r; i++) {
			if (buf[i] == ',') {
				switch (buf[0]) {
					case 'S':
						if (cnt == 4) {
							/* Add msg length */
							memmove(buf+i+2, buf+i+1, r-(i+1));
							snprintf(tmp, 3, "%02X", (int)((r-(i+1))/2));
							memcpy(buf+i, tmp, 2);
							r++;
							break;
						}
					default:
						memmove(buf+i, buf+i+1, r-(i+1));
						r--;
						break;
				}
				cnt++;
			}
		}

		memset(tmp, 0, sizeof(tmp));
		for (i = 1; i < r; i+=2) {
			memcpy(tmp, buf + i, 2);
			send_buf[1+(i/2)] = strtoul(tmp, NULL, 16);
		}
		hmcfgusb_send(dev, send_buf, 1+(i/2), 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;
	42
	43	static void hmlan_format_out(uint8_t buf, int buf_len, void data)
9db2e455	44	{
e0a7146e MG	45	char out[1024];
	46	char *pos;
	47	int fd = ((int)data);
9db2e455 MG	48	int len;
	49	int i;
	50
	51	if (buf_len < 1)
	52	return;
	53
e0a7146e MG	54	memset(out, 0, sizeof(out));
	55	pos = out;
	56
	57	*pos++ = buf[0];
9db2e455 MG	58	switch(buf[0]) {
9db2e455 MG	59	case 'H':
e0a7146e MG	60	if (impersonate_hmlanif) {
	61	buf[5] = 'L';
	62	buf[6] = 'A';
	63	buf[7] = 'N';
	64	}
9db2e455 MG	65	len = buf[1];
9db2e455 MG	66	for (i = 2; i < len + 2; i++) {
e0a7146e	67	*pos++=buf[i];
9db2e455	68	}
e0a7146e MG	69	snprintf(pos, 7, ",%02X%02X,", buf[i], buf[i+1]);
	70	pos += 6;
	71
	72	i += 2;
9db2e455 MG	73	len = buf[i]+i+1;
	74	i++;
	75	for (; i < len; i++) {
e0a7146e	76	*pos++=buf[i];
9db2e455	77	}
e0a7146e	78	*pos++=',';
9db2e455 MG	79	len = i+12;
9db2e455 MG	80	for (; i < len; i++) {
e0a7146e MG	81	snprintf(pos, 3, "%02X", buf[i]);
	82	pos += 2;
	83
9db2e455 MG	84	switch(len-i) {
	85	case 10:
	86	case 7:
	87	case 3:
e0a7146e	88	*pos++=',';
9db2e455 MG	89	break;
	90	default:
	91	break;
	92	}
	93	}
	94
	95	break;
	96	case 'E':
	97	len = 13 + buf[13];
	98	for (i = 0; i < len; i++) {
e0a7146e MG	99	if (i != 12) {
	100	snprintf(pos, 3, "%02X", buf[i+1]);
	101	pos += 2;
	102	}
9db2e455 MG	103	switch(i) {
	104	case 2:
	105	case 4:
	106	case 8:
	107	case 9:
	108	case 11:
e0a7146e	109	*pos++=',';
9db2e455 MG	110	break;
	111	default:
	112	break;
	113	}
	114	}
	115	break;
	116	case 'R':
	117	len = 14 + buf[14];
	118	for (i = 0; i < len; i++) {
e0a7146e MG	119	if (i != 13) {
	120	snprintf(pos, 3, "%02X", buf[i+1]);
	121	pos += 2;
	122	}
9db2e455 MG	123	switch(i) {
	124	case 3:
	125	case 5:
	126	case 9:
	127	case 10:
	128	case 12:
e0a7146e	129	*pos++=',';
9db2e455 MG	130	break;
	131	default:
	132	break;
	133	}
	134	}
	135	break;
	136	case 'I':
	137	//HM> 0x0000: 49 00 00 00 00 55 53 42 2d 49 46 03 bc 0a 4a 45 I....USB-IF...JE
	138	//HM> 0x0010: 51 30 35 33 35 31 32 32 1d b1 55 68 ea 13 00 14 Q0535122..Uh....
	139	//HM> 0x0020: 9f a6 00 03 00 00 00 00 00 00 00 00 00 00 00 00 ................
	140	//HM> 0x0030: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 ................
	141	default:
e0a7146e MG	142	for (i = 1; i < buf_len; i++) {
	143	snprintf(pos, 3, "%02X", buf[i]);
	144	pos += 2;
	145	}
9db2e455 MG	146	hexdump(buf, buf_len, "Unknown> ");
	147	break;
	148	}
e0a7146e MG	149	*pos++='\r';
	150	*pos++='\n';
	151	write(fd, out, pos-out);
9db2e455 MG	152	}
9db2e455 MG	153
e0a7146e	154	static int hmlan_parse_in(int fd, void *data)
9db2e455 MG	155	{
	156	struct hmcfgusb_dev *dev = data;
	157	unsigned char buf[1024];
	158	unsigned char send_buf[0x40]; //FIXME!!!
	159	char tmp[3];
	160	int i;
	161	int r;
	162
	163	r = read(fd, buf, sizeof(buf));
	164	if (r > 0) {
	165	int cnt;
	166
	167	memset(send_buf, 0, sizeof(send_buf));
	168	for (i = 0; i < r; i++) {
	169	if ((buf[i] == 0x0a) \|\|
	170	(buf[i] == 0x0d)) {
	171	r = i;
	172	break;
	173	}
	174	}
	175
	176	send_buf[0] = buf[0];
	177
	178	cnt = 0;
	179	for (i = 1; i < r; i++) {
	180	if (buf[i] == ',') {
	181	switch (buf[0]) {
	182	case 'S':
	183	if (cnt == 4) {
	184	/* Add msg length */
	185	memmove(buf+i+2, buf+i+1, r-(i+1));
	186	snprintf(tmp, 3, "%02X", (int)((r-(i+1))/2));
	187	memcpy(buf+i, tmp, 2);
	188	r++;
	189	break;
	190	}
	191	default:
	192	memmove(buf+i, buf+i+1, r-(i+1));
	193	r--;
	194	break;
	195	}
	196	cnt++;
	197	}
	198	}
	199
	200	memset(tmp, 0, sizeof(tmp));
	201	for (i = 1; i < r; i+=2) {
	202	memcpy(tmp, buf + i, 2);
	203	send_buf[1+(i/2)] = strtoul(tmp, NULL, 16);
	204	}
	205	hmcfgusb_send(dev, send_buf, 1+(i/2), 1);
	206	} else if (r < 0) {
	207	perror("read");
e0a7146e MG	208	return r;
	209	} else {
	210	return 0;
9db2e455	211	}
e0a7146e MG	212
e0a7146e MG	213	return 1;
9db2e455 MG	214	}
9db2e455 MG	215
e0a7146e	216	static int comm(int fd_in, int fd_out, int master_socket)
9db2e455 MG	217	{
	218	struct hmcfgusb_dev *dev;
	219	int quit = 0;
	220
e0a7146e	221	dev = hmcfgusb_init(hmlan_format_out, &fd_out);
9db2e455 MG	222	if (!dev) {
9db2e455 MG	223	fprintf(stderr, "Can't initialize HM-CFG-USB!\n");
e0a7146e MG	224	return 0;
	225	}
	226
	227	if (!hmcfgusb_add_pfd(dev, fd_in, POLLIN)) {
	228	fprintf(stderr, "Can't add client to pollfd!\n");
	229	hmcfgusb_close(dev);
	230	return 0;
9db2e455 MG	231	}
9db2e455 MG	232
e0a7146e MG	233	if (master_socket >= 0) {
	234	if (!hmcfgusb_add_pfd(dev, master_socket, POLLIN)) {
	235	fprintf(stderr, "Can't add master_socket to pollfd!\n");
	236	hmcfgusb_close(dev);
	237	return 0;
	238	}
9db2e455 MG	239	}
	240
	241	hmcfgusb_send(dev, (unsigned char*)"K", 1, 1);
	242
	243	while(!quit) {
	244	int fd;
	245
	246	fd = hmcfgusb_poll(dev, 3600);
	247	if (fd >= 0) {
e0a7146e MG	248	if (fd == master_socket) {
	249	int client;
	250
	251	client = accept(master_socket, NULL, 0);
	252	if (client >= 0) {
	253	shutdown(client, SHUT_RDWR);
	254	close(client);
	255	}
	256	} else {
	257	if (hmlan_parse_in(fd, dev) <= 0) {
	258	quit = 1;
	259	}
	260	}
9db2e455 MG	261	} else if (fd == -1) {
	262	if (errno) {
	263	perror("hmcfgusb_poll");
	264	quit = 1;
	265	}
	266	}
	267	}
	268
813afd12	269	hmcfgusb_close(dev);
e0a7146e MG	270	return 1;
	271	}
	272
	273	static int socket_server(int port)
	274	{
	275	struct sockaddr_in sin;
	276	int sock;
	277	int n;
	278
	279	impersonate_hmlanif = 1;
	280
	281	sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
	282	if (sock == -1) {
	283	perror("Can't open socket");
	284	return EXIT_FAILURE;
	285	}
	286
	287	n = 1;
	288	if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &n, sizeof(n)) == -1) {
	289	perror("Can't set socket options");
	290	return EXIT_FAILURE;
	291	}
	292
	293	memset(&sin, 0, sizeof(sin));
	294	sin.sin_family = AF_INET;
	295	sin.sin_port = htons(port);
	296	sin.sin_addr.s_addr = htonl(INADDR_ANY);
	297
	298	if (bind(sock, (struct sockaddr*)&sin, sizeof(sin)) == -1) {
	299	perror("Can't bind socket");
	300	return EXIT_FAILURE;
	301	}
	302
	303	if (listen(sock, 1) == -1) {
	304	perror("Can't listen on socket");
	305	return EXIT_FAILURE;
	306	}
	307
	308	while(1) {
	309	struct sockaddr_in csin;
	310	socklen_t csinlen;
	311	int client;
	312
	313	memset(&csin, 0, sizeof(csin));
	314	csinlen = sizeof(csin);
	315	client = accept(sock, (struct sockaddr*)&csin, &csinlen);
	316	if (client == -1) {
	317	perror("Couldn't accept client");
	318	continue;
	319	}
	320
	321	printf("Client accepted!\n");
	322
	323	comm(client, client, sock);
	324
	325	shutdown(client, SHUT_RDWR);
	326	close(client);
	327
	328	printf("Connection closed!\n");
	329
	330	}
	331
	332	return EXIT_SUCCESS;
	333	}
334
335	static int interactive_server(void)
336	{
337	if (!comm(STDIN_FILENO, STDOUT_FILENO, -1))
338	return EXIT_FAILURE;
9db2e455 MG	339
	340	return EXIT_SUCCESS;
	341	}
e0a7146e MG	342
	343	int main(int argc, char **argv)
	344	{
	345	//return interactive_server();
	346	return socket_server(1234);
	347	}