f8071649001504b8fb1dfdf074c2f7e1d3c6bb79
[hmcfgusb] / hmland.c
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>
32 #include <sys/types.h>
33 #include <sys/socket.h>
34 #include <netinet/in.h>
35 #include <arpa/inet.h>
36 #include <libusb-1.0/libusb.h>
37
38 #include "hexdump.h"
39 #include "hmcfgusb.h"
40
41 static int impersonate_hmlanif = 0;
42
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
164 static void hmlan_format_out(uint8_t *buf, int buf_len, void *data)
165 {
166 uint8_t out[1024];
167 uint8_t *outpos;
168 uint8_t *inpos;
169 int fd = *((int*)data);
170
171 if (buf_len < 1)
172 return;
173
174 memset(out, 0, sizeof(out));
175 outpos = out;
176 inpos = buf;
177
178 format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), 1, 0);
179 switch(buf[0]) {
180 case 'H':
181 if (impersonate_hmlanif) {
182 buf[5] = 'L';
183 buf[6] = 'A';
184 buf[7] = 'N';
185 }
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);
193
194 break;
195 case 'E':
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
203 break;
204 case 'R':
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
212 break;
213 case 'I':
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;
220 default:
221 format_part_out(&inpos, (buf_len-(inpos-buf)), &outpos, (sizeof(out)-(outpos-out)), buf_len-1, FLAG_FORMAT_HEX | FLAG_NL);
222 hexdump(buf, buf_len, "Unknown> ");
223 break;
224 }
225 write(fd, out, outpos-out);
226 }
227
228 static int hmlan_parse_in(int fd, void *data)
229 {
230 struct hmcfgusb_dev *dev = data;
231 uint8_t buf[1024];
232 uint8_t out[0x40]; //FIXME!!!
233 uint8_t *outpos;
234 uint8_t *inpos;
235 int i;
236 int r;
237
238 r = read(fd, buf, sizeof(buf));
239 if (r > 0) {
240 memset(out, 0, sizeof(out));
241 for (i = 0; i < r; i++) {
242 if ((buf[i] == 0x0a) ||
243 (buf[i] == 0x0d)) {
244 r = i;
245 break;
246 }
247 }
248
249 if (r == 0)
250 return 1;
251
252 out[0] = buf[0];
253 outpos = out+1;
254 inpos = buf+1;
255
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;
268 }
269
270 hmcfgusb_send(dev, out, outpos-out, 1);
271 } else if (r < 0) {
272 perror("read");
273 return r;
274 } else {
275 return 0;
276 }
277
278 return 1;
279 }
280
281 static int comm(int fd_in, int fd_out, int master_socket)
282 {
283 struct hmcfgusb_dev *dev;
284 int quit = 0;
285
286 dev = hmcfgusb_init(hmlan_format_out, &fd_out);
287 if (!dev) {
288 fprintf(stderr, "Can't initialize HM-CFG-USB!\n");
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;
296 }
297
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 }
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) {
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 }
326 } else if (fd == -1) {
327 if (errno) {
328 perror("hmcfgusb_poll");
329 quit = 1;
330 }
331 }
332 }
333
334 hmcfgusb_close(dev);
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;
404
405 return EXIT_SUCCESS;
406 }
407
408 int main(int argc, char **argv)
409 {
410 //return interactive_server();
411 return socket_server(1234);
412 }
Impressum, Datenschutz