bccfa0d3f5bddae062afe8532ca37ec88f97eb4b
[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 static void hmlan_format_out(uint8_t *buf, int buf_len, void *data)
44 {
45 char out[1024];
46 char *pos;
47 int fd = *((int*)data);
48 int len;
49 int i;
50
51 if (buf_len < 1)
52 return;
53
54 memset(out, 0, sizeof(out));
55 pos = out;
56
57 *pos++ = buf[0];
58 switch(buf[0]) {
59 case 'H':
60 if (impersonate_hmlanif) {
61 buf[5] = 'L';
62 buf[6] = 'A';
63 buf[7] = 'N';
64 }
65 len = buf[1];
66 for (i = 2; i < len + 2; i++) {
67 *pos++=buf[i];
68 }
69 snprintf(pos, 7, ",%02X%02X,", buf[i], buf[i+1]);
70 pos += 6;
71
72 i += 2;
73 len = buf[i]+i+1;
74 i++;
75 for (; i < len; i++) {
76 *pos++=buf[i];
77 }
78 *pos++=',';
79 len = i+12;
80 for (; i < len; i++) {
81 snprintf(pos, 3, "%02X", buf[i]);
82 pos += 2;
83
84 switch(len-i) {
85 case 10:
86 case 7:
87 case 3:
88 *pos++=',';
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++) {
99 if (i != 12) {
100 snprintf(pos, 3, "%02X", buf[i+1]);
101 pos += 2;
102 }
103 switch(i) {
104 case 2:
105 case 4:
106 case 8:
107 case 9:
108 case 11:
109 *pos++=',';
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++) {
119 if (i != 13) {
120 snprintf(pos, 3, "%02X", buf[i+1]);
121 pos += 2;
122 }
123 switch(i) {
124 case 3:
125 case 5:
126 case 9:
127 case 10:
128 case 12:
129 *pos++=',';
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:
142 for (i = 1; i < buf_len; i++) {
143 snprintf(pos, 3, "%02X", buf[i]);
144 pos += 2;
145 }
146 hexdump(buf, buf_len, "Unknown> ");
147 break;
148 }
149 *pos++='\r';
150 *pos++='\n';
151 write(fd, out, pos-out);
152 }
153
154 static int hmlan_parse_in(int fd, void *data)
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");
208 return r;
209 } else {
210 return 0;
211 }
212
213 return 1;
214 }
215
216 static int comm(int fd_in, int fd_out, int master_socket)
217 {
218 struct hmcfgusb_dev *dev;
219 int quit = 0;
220
221 dev = hmcfgusb_init(hmlan_format_out, &fd_out);
222 if (!dev) {
223 fprintf(stderr, "Can't initialize HM-CFG-USB!\n");
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;
231 }
232
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 }
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) {
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 }
261 } else if (fd == -1) {
262 if (errno) {
263 perror("hmcfgusb_poll");
264 quit = 1;
265 }
266 }
267 }
268
269 hmcfgusb_close(dev);
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;
339
340 return EXIT_SUCCESS;
341 }
342
343 int main(int argc, char **argv)
344 {
345 //return interactive_server();
346 return socket_server(1234);
347 }
Impressum, Datenschutz