Mercurial > repos > blastem

comparison megawifi.c @ 1969:852393cdec7c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
megawifi: implement UDP sockets
author doragasu <doragasu@hotmail.com>
date Fri, 08 May 2020 00:25:24 -0700
parents c16dabdb0aad
children 41b9509ede38
comparison
equal deleted inserted replaced
1968:c16dabdb0aad 1969:852393cdec7c
6 #define WINVER 0x501 6 #define WINVER 0x501
7 #include <winsock2.h> 7 #include <winsock2.h>
8 #include <ws2tcpip.h> 8 #include <ws2tcpip.h>
9 #else 9 #else
10 #include <sys/socket.h> 10 #include <sys/socket.h>
11 #include <arpa/inet.h>
11 #include <unistd.h> 12 #include <unistd.h>
12 #include <netinet/in.h> 13 #include <netinet/in.h>
13 #include <netdb.h> 14 #include <netdb.h>
14 #endif 15 #endif
15 #include <errno.h> 16 #include <errno.h>
59 SOCKST_TCP_LISTEN, 60 SOCKST_TCP_LISTEN,
60 SOCKST_TCP_EST, 61 SOCKST_TCP_EST,
61 SOCKST_UDP_READY 62 SOCKST_UDP_READY
62 }; 63 };
63 64
65 // TCP/UDP address message
66 struct mw_addr_msg {
67 char dst_port[6];
68 char src_port[6];
69 uint8_t channel;
70 char host[];
71 };
64 72
65 #define FLAG_ONLINE 73 #define FLAG_ONLINE
66 74
67 typedef struct { 75 typedef struct {
68 uint32_t transmit_bytes; 76 uint32_t transmit_bytes;
77 uint8_t transmit_state; 85 uint8_t transmit_state;
78 uint8_t module_state; 86 uint8_t module_state;
79 uint8_t flags; 87 uint8_t flags;
80 uint8_t transmit_buffer[4096]; 88 uint8_t transmit_buffer[4096];
81 uint8_t receive_buffer[4096]; 89 uint8_t receive_buffer[4096];
90 struct sockaddr_in remote_addr[15]; // Needed for UDP sockets
82 } megawifi; 91 } megawifi;
83 92
84 static megawifi *get_megawifi(void *context) 93 static megawifi *get_megawifi(void *context)
85 { 94 {
86 m68k_context *m68k = context; 95 m68k_context *m68k = context;
121 } 130 }
122 memcpy(mw->receive_buffer + mw->receive_bytes, src, count); 131 memcpy(mw->receive_buffer + mw->receive_bytes, src, count);
123 mw->receive_bytes += count; 132 mw->receive_bytes += count;
124 } 133 }
125 134
126 static void mw_putraw(megawifi *mw, const char *data, size_t len)
127 {
128 if ((mw->receive_bytes + len) > sizeof(mw->receive_buffer)) {
129 return;
130 }
131 memcpy(mw->receive_buffer + mw->receive_bytes, data, len);
132 mw->receive_bytes += len;
133 }
134
135 static void mw_puts(megawifi *mw, const char *s) 135 static void mw_puts(megawifi *mw, const char *s)
136 { 136 {
137 size_t len = strlen(s); 137 size_t len = strlen(s);
138 mw_putraw(mw, s, len); 138 mw_copy(mw, (uint8_t*)s, len);
139 }
140
141 static void udp_recv(megawifi *mw, uint8_t idx)
142 {
143 ssize_t recvd;
144 int s = mw->sock_fds[idx];
145 struct sockaddr_in remote;
146 socklen_t addr_len = sizeof(struct sockaddr_in);
147
148 if (mw->remote_addr[idx].sin_addr.s_addr != htonl(INADDR_ANY)) {
149 // Receive only from specified address
150 recvd = recvfrom(s, (char*)mw->receive_buffer + 3, MAX_RECV_SIZE, 0,
151 (struct sockaddr*)&remote, &addr_len);
152 if (recvd > 0) {
153 if (remote.sin_addr.s_addr != mw->remote_addr[idx].sin_addr.s_addr) {
154 printf("Discarding UDP packet from unknown addr %s:%d\n",
155 inet_ntoa(remote.sin_addr), ntohs(remote.sin_port));
156 recvd = 0;
157 }
158 }
159 } else {
160 // Reuse mode, data is preceded by remote IPv4 and port
161 recvd = recvfrom(s, (char*)mw->receive_buffer + 9, MAX_RECV_SIZE - 6,
162 0, (struct sockaddr*)&remote, &addr_len);
163 if (recvd > 0) {
164 mw->receive_buffer[3] = remote.sin_addr.s_addr;
165 mw->receive_buffer[4] = remote.sin_addr.s_addr>>8;
166 mw->receive_buffer[5] = remote.sin_addr.s_addr>>16;
167 mw->receive_buffer[6] = remote.sin_addr.s_addr>>24;
168 mw->receive_buffer[7] = remote.sin_port;
169 mw->receive_buffer[8] = remote.sin_port>>8;
170 recvd += 6;
171 }
172 }
173
174 if (recvd > 0) {
175 mw_putc(mw, STX);
176 mw_putc(mw, (recvd >> 8) | ((idx+1) << 4));
177 mw_putc(mw, recvd);
178 mw->receive_bytes += recvd;
179 mw_putc(mw, ETX);
180 //should this set the channel flag?
181 } else if (recvd < 0 && !socket_error_is_wouldblock()) {
182 socket_close(mw->sock_fds[idx]);
183 mw->channel_state[idx] = SOCKST_NONE;
184 mw->channel_flags |= 1 << (idx + 1);
185 }
186 }
187
188 static void udp_send(megawifi *mw, uint8_t idx)
189 {
190 struct sockaddr_in remote;
191 int s = mw->sock_fds[idx];
192 int sent;
193 char *data = (char*)mw->transmit_buffer;
194
195 if (mw->remote_addr[idx].sin_addr.s_addr != htonl(INADDR_ANY)) {
196 sent = sendto(s, data, mw->transmit_bytes, 0, (struct sockaddr*)&mw->remote_addr[idx],
197 sizeof(struct sockaddr_in));
198 } else {
199 // Reuse mode, extract address from leading bytes
200 // NOTE: mw->remote_addr[idx].sin_addr.s_addr == INADDR_ANY
201 remote.sin_addr.s_addr = *((int32_t*)data);
202 remote.sin_port = *((int16_t*)(data + 4));
203 remote.sin_family = AF_INET;
204 memset(remote.sin_zero, 0, sizeof(remote.sin_zero));
205 sent = sendto(s, data + 6, mw->transmit_bytes - 6, 0, (struct sockaddr*)&remote,
206 sizeof(struct sockaddr_in)) + 6;
207 }
208 if (sent < 0 && !socket_error_is_wouldblock()) {
209 socket_close(s);
210 mw->sock_fds[idx] = -1;
211 mw->channel_state[idx] = SOCKST_NONE;
212 mw->channel_flags |= 1 << (idx + 1);
213 } else if (sent < mw->transmit_bytes) {
214 //TODO: save this data somewhere so it can be sent in poll_socket
215 printf("Sent %d bytes on channel %d, but %d were requested\n", sent, idx + 1, mw->transmit_bytes);
216 }
139 } 217 }
140 218
141 static void poll_socket(megawifi *mw, uint8_t channel) 219 static void poll_socket(megawifi *mw, uint8_t channel)
142 { 220 {
143 if (mw->sock_fds[channel] < 0) { 221 if (mw->sock_fds[channel] < 0) {
154 } else if (errno != EAGAIN && errno != EWOULDBLOCK) { 232 } else if (errno != EAGAIN && errno != EWOULDBLOCK) {
155 socket_close(mw->sock_fds[channel]); 233 socket_close(mw->sock_fds[channel]);
156 mw->channel_state[channel] = SOCKST_NONE; 234 mw->channel_state[channel] = SOCKST_NONE;
157 mw->channel_flags |= 1 << (channel + 1); 235 mw->channel_flags |= 1 << (channel + 1);
158 } 236 }
159 } else if (mw->channel_state[channel] == SOCKST_TCP_EST && mw->receive_bytes < sizeof(mw->receive_buffer) - 4) { 237 } else if (mw->channel_state[channel] == SOCKST_TCP_EST && mw->receive_bytes < (sizeof(mw->receive_buffer) - 4)) {
160 size_t max = sizeof(mw->receive_buffer) - 4 - mw->receive_bytes; 238 size_t max = sizeof(mw->receive_buffer) - 4 - mw->receive_bytes;
161 if (max > MAX_RECV_SIZE) { 239 if (max > MAX_RECV_SIZE) {
162 max = MAX_RECV_SIZE; 240 max = MAX_RECV_SIZE;
163 } 241 }
164 int bytes = recv(mw->sock_fds[channel], (char*)(mw->receive_buffer + mw->receive_bytes + 3), max, 0); 242 int bytes = recv(mw->sock_fds[channel], (char*)(mw->receive_buffer + mw->receive_bytes + 3), max, 0);
172 } else if (bytes < 0 && !socket_error_is_wouldblock()) { 250 } else if (bytes < 0 && !socket_error_is_wouldblock()) {
173 socket_close(mw->sock_fds[channel]); 251 socket_close(mw->sock_fds[channel]);
174 mw->channel_state[channel] = SOCKST_NONE; 252 mw->channel_state[channel] = SOCKST_NONE;
175 mw->channel_flags |= 1 << (channel + 1); 253 mw->channel_flags |= 1 << (channel + 1);
176 } 254 }
255 } else if (mw->channel_state[channel] == SOCKST_UDP_READY && !mw->receive_bytes) {
256 udp_recv(mw, channel);
177 } 257 }
178 } 258 }
179 259
180 static void poll_all_sockets(megawifi *mw) 260 static void poll_all_sockets(megawifi *mw)
181 { 261 {
220 sprintf(ssid, "BLASTEM! SSID %d", slot + 1); 300 sprintf(ssid, "BLASTEM! SSID %d", slot + 1);
221 sprintf(pass, "BLASTEM! PASS %d", slot + 1); 301 sprintf(pass, "BLASTEM! PASS %d", slot + 1);
222 start_reply(mw, CMD_OK); 302 start_reply(mw, CMD_OK);
223 mw_putc(mw, slot); 303 mw_putc(mw, slot);
224 mw_putc(mw, 7); /// 11bgn 304 mw_putc(mw, 7); /// 11bgn
225 mw_putraw(mw, ssid, 32); 305 mw_copy(mw, (uint8_t*)ssid, 32);
226 mw_putraw(mw, pass, 64); 306 mw_copy(mw, (uint8_t*)pass, 64);
227 end_reply(mw); 307 end_reply(mw);
228 } 308 }
229 309
230 static void cmd_ip_cfg_get(megawifi *mw) 310 static void cmd_ip_cfg_get(megawifi *mw)
231 { 311 {
234 start_reply(mw, CMD_OK); 314 start_reply(mw, CMD_OK);
235 mw_putc(mw, mw->transmit_buffer[4]); 315 mw_putc(mw, mw->transmit_buffer[4]);
236 mw_putc(mw, 0); 316 mw_putc(mw, 0);
237 mw_putc(mw, 0); 317 mw_putc(mw, 0);
238 mw_putc(mw, 0); 318 mw_putc(mw, 0);
239 mw_putraw(mw, (char*)ipv4s, sizeof(ipv4s)); 319 mw_copy(mw, (uint8_t*)ipv4s, sizeof(ipv4s));
240 end_reply(mw); 320 end_reply(mw);
241 } 321 }
242 322
243 static void cmd_tcp_con(megawifi *mw, uint32_t size) 323 static void cmd_tcp_con(megawifi *mw, uint32_t size)
244 { 324 {
325 struct mw_addr_msg *addr = (struct mw_addr_msg*)(mw->transmit_buffer + 4);
245 struct addrinfo hints; 326 struct addrinfo hints;
246 struct addrinfo *res = NULL; 327 struct addrinfo *res = NULL;
247 char dst_port[6];
248 char src_port[6];
249 char host[MAX_RECV_SIZE - 13];
250 int s; 328 int s;
251
252 int err; 329 int err;
253 330
254 uint8_t channel = mw->transmit_buffer[16]; 331 uint8_t channel = addr->channel;
255 if (!channel || channel > 15 || mw->sock_fds[channel - 1] >= 0) { 332 if (!channel || channel > 15 || mw->sock_fds[channel - 1] >= 0) {
256 start_reply(mw, CMD_ERROR); 333 start_reply(mw, CMD_ERROR);
257 end_reply(mw); 334 end_reply(mw);
258 return; 335 return;
259 } 336 }
260 channel--; 337 channel--;
261
262 strncpy(dst_port, (char*)&mw->transmit_buffer[4], 5);
263 dst_port[5] = '\0';
264 // TODO src_port is unused
265 strncpy(src_port, (char*)&mw->transmit_buffer[10], 5);
266 src_port[5] = '\0';
267 strncpy(host, (char*)&mw->transmit_buffer[17], MAX_RECV_SIZE - 14);
268 host[MAX_RECV_SIZE - 14] = '\0';
269 338
270 memset(&hints, 0, sizeof(hints)); 339 memset(&hints, 0, sizeof(hints));
271 hints.ai_family = AF_INET; 340 hints.ai_family = AF_INET;
272 #ifndef _WIN32 341 #ifndef _WIN32
273 hints.ai_flags = AI_NUMERICSERV; 342 hints.ai_flags = AI_NUMERICSERV;
274 #endif 343 #endif
275 hints.ai_socktype = SOCK_STREAM; 344 hints.ai_socktype = SOCK_STREAM;
276 345
277 if ((err = getaddrinfo(host, dst_port, &hints, &res)) != 0) { 346 if ((err = getaddrinfo(addr->host, addr->dst_port, &hints, &res)) != 0) {
278 printf("getaddrinfo failed: %s\n", gai_strerror(err)); 347 printf("getaddrinfo failed: %s\n", gai_strerror(err));
279 start_reply(mw, CMD_ERROR); 348 start_reply(mw, CMD_ERROR);
280 end_reply(mw); 349 end_reply(mw);
281 return; 350 return;
282 } 351 }
293 362
294 socket_blocking(s, 0); 363 socket_blocking(s, 0);
295 mw->sock_fds[channel] = s; 364 mw->sock_fds[channel] = s;
296 mw->channel_state[channel] = SOCKST_TCP_EST; 365 mw->channel_state[channel] = SOCKST_TCP_EST;
297 mw->channel_flags |= 1 << (channel + 1); 366 mw->channel_flags |= 1 << (channel + 1);
298 printf("Connection established on ch %d with %s:%s\n", channel + 1, host, dst_port); 367 printf("Connection established on ch %d with %s:%s\n", channel + 1,
368 addr->host, addr->dst_port);
299 369
300 if (res) { 370 if (res) {
301 freeaddrinfo(res); 371 freeaddrinfo(res);
302 } 372 }
303 start_reply(mw, CMD_OK); 373 start_reply(mw, CMD_OK);
304 end_reply(mw); 374 end_reply(mw);
305 return; 375 return;
306 376
307 err: 377 err:
308 freeaddrinfo(res); 378 freeaddrinfo(res);
309 printf("Connection to %s:%s failed, %s\n", host, dst_port, strerror(errno)); 379 printf("Connection to %s:%s failed, %s\n", addr->host, addr->dst_port, strerror(errno));
310 start_reply(mw, CMD_ERROR); 380 start_reply(mw, CMD_ERROR);
311 end_reply(mw); 381 end_reply(mw);
312 } 382 }
313 383
384 static void cmd_close(megawifi *mw)
385 {
386 int channel = mw->transmit_buffer[4] - 1;
387
388 if (channel >= 15 || mw->sock_fds[channel] < 0) {
389 start_reply(mw, CMD_ERROR);
390 end_reply(mw);
391 return;
392 }
393
394 socket_close(mw->sock_fds[channel]);
395 mw->sock_fds[channel] = -1;
396 mw->channel_state[channel] = SOCKST_NONE;
397 mw->channel_flags |= 1 << (channel + 1);
398 start_reply(mw, CMD_OK);
399 end_reply(mw);
400 }
401
402 static void cmd_udp_set(megawifi *mw)
403 {
404 struct mw_addr_msg *addr = (struct mw_addr_msg*)(mw->transmit_buffer + 4);
405 unsigned int local_port, remote_port;
406 int s;
407 struct addrinfo *raddr;
408 struct addrinfo hints;
409 struct sockaddr_in local;
410 int err;
411
412 uint8_t channel = addr->channel;
413 if (!channel || channel > 15 || mw->sock_fds[channel - 1] >= 0) {
414 goto err;
415 }
416 channel--;
417 local_port = atoi(addr->src_port);
418 remote_port = atoi(addr->dst_port);
419
420 if ((s = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
421 printf("Datagram socket creation failed\n");
422 goto err;
423 }
424
425 memset(local.sin_zero, 0, sizeof(local.sin_zero));
426 local.sin_family = AF_INET;
427 local.sin_addr.s_addr = htonl(INADDR_ANY);
428 local.sin_port = htons(local_port);
429 if (remote_port && addr->host[0]) {
430 // Communication with remote peer
431 printf("Set UDP ch %d, port %d to addr %s:%d\n", addr->channel,
432 local_port, addr->host, remote_port);
433
434 memset(&hints, 0, sizeof(hints));
435 hints.ai_family = AF_INET;
436 #ifndef _WIN32
437 hints.ai_flags = AI_NUMERICSERV;
438 #endif
439 hints.ai_socktype = SOCK_DGRAM;
440
441 if ((err = getaddrinfo(addr->host, addr->dst_port, &hints, &raddr)) != 0) {
442 printf("getaddrinfo failed: %s\n", gai_strerror(err));
443 goto err;
444 }
445 mw->remote_addr[channel] = *((struct sockaddr_in*)raddr->ai_addr);
446 freeaddrinfo(raddr);
447 } else if (local_port) {
448 // Server in reuse mode
449 printf("Set UDP ch %d, src port %d\n", addr->channel, local_port);
450 mw->remote_addr[channel] = local;
451 } else {
452 printf("Invalid UDP socket data\n");
453 goto err;
454 }
455
456 if (bind(s, (struct sockaddr*)&local, sizeof(struct sockaddr_in)) < 0) {
457 printf("bind to port %d failed\n", local_port);
458 goto err;
459 }
460
461 socket_blocking(s, 0);
462 mw->sock_fds[channel] = s;
463 mw->channel_state[channel] = SOCKST_UDP_READY;
464 mw->channel_flags |= 1 << (channel + 1);
465
466 start_reply(mw, CMD_OK);
467 end_reply(mw);
468
469 return;
470
471 err:
472 start_reply(mw, CMD_ERROR);
473 end_reply(mw);
474 }
475
314 #define AVATAR_BYTES (32 * 48 / 2) 476 #define AVATAR_BYTES (32 * 48 / 2)
315 static void cmd_gamertag_get(megawifi *mw) 477 static void cmd_gamertag_get(megawifi *mw)
316 { 478 {
317 uint32_t id = htonl(1); 479 uint32_t id = htonl(1);
318 char buf[AVATAR_BYTES]; 480 char buf[AVATAR_BYTES];
319 481
320 start_reply(mw, CMD_OK); 482 start_reply(mw, CMD_OK);
321 // TODO Get items from config file 483 // TODO Get items from config file
322 mw_putraw(mw, (const char*)&id, 4); 484 mw_copy(mw, (uint8_t*)&id, 4);
323 strncpy(buf, "doragasu on Blastem!", 32); 485 strncpy(buf, "doragasu on Blastem!", 32);
324 mw_putraw(mw, buf, 32); 486 mw_copy(mw, (uint8_t*)buf, 32);
325 strncpy(buf, "My cool password", 32); 487 strncpy(buf, "My cool password", 32);
326 mw_putraw(mw, buf, 32); 488 mw_copy(mw, (uint8_t*)buf, 32);
327 strncpy(buf, "All your WiFi are belong to me!", 32); 489 strncpy(buf, "All your WiFi are belong to me!", 32);
328 mw_putraw(mw, buf, 32); 490 mw_copy(mw, (uint8_t*)buf, 32);
329 memset(buf, 0, 64); // Telegram token 491 memset(buf, 0, 64); // Telegram token
330 mw_putraw(mw, buf, 64); 492 mw_copy(mw, (uint8_t*)buf, 64);
331 mw_putraw(mw, buf, AVATAR_BYTES); // Avatar tiles 493 mw_copy(mw, (uint8_t*)buf, AVATAR_BYTES); // Avatar tiles
332 mw_putraw(mw, buf, 32); // Avatar palette 494 mw_copy(mw, (uint8_t*)buf, 32); // Avatar palette
333 end_reply(mw); 495 end_reply(mw);
334 } 496 }
335 497
336 static void cmd_hrng_get(megawifi *mw) 498 static void cmd_hrng_get(megawifi *mw)
337 { 499 {
463 socket_blocking(mw->sock_fds[channel], 0); 625 socket_blocking(mw->sock_fds[channel], 0);
464 } 626 }
465 end_reply(mw); 627 end_reply(mw);
466 break; 628 break;
467 } 629 }
630 case CMD_CLOSE:
631 cmd_close(mw);
632 break;
633 case CMD_UDP_SET:
634 cmd_udp_set(mw);
635 break;
468 case CMD_SOCK_STAT: { 636 case CMD_SOCK_STAT: {
469 uint8_t channel = mw->transmit_buffer[4]; 637 uint8_t channel = mw->transmit_buffer[4];
470 if (!channel || channel > 15) { 638 if (!channel || channel > 15) {
471 start_reply(mw, CMD_ERROR); 639 start_reply(mw, CMD_ERROR);
472 end_reply(mw); 640 end_reply(mw);
518 process_command(mw); 686 process_command(mw);
519 } else { 687 } else {
520 uint8_t channel = mw->transmit_channel - 1; 688 uint8_t channel = mw->transmit_channel - 1;
521 int channel_state = mw->channel_state[channel]; 689 int channel_state = mw->channel_state[channel];
522 int sock_fd = mw->sock_fds[channel]; 690 int sock_fd = mw->sock_fds[channel];
523 // TODO Handle UDP type sockets
524 if (sock_fd >= 0 && channel_state == SOCKST_TCP_EST) { 691 if (sock_fd >= 0 && channel_state == SOCKST_TCP_EST) {
525 int sent = send(sock_fd, (char*)mw->transmit_buffer, mw->transmit_bytes, 0); 692 int sent = send(sock_fd, (char*)mw->transmit_buffer, mw->transmit_bytes, 0);
526 if (sent < 0 && !socket_error_is_wouldblock()) { 693 if (sent < 0 && !socket_error_is_wouldblock()) {
527 socket_close(sock_fd); 694 socket_close(sock_fd);
528 mw->sock_fds[channel] = -1; 695 mw->sock_fds[channel] = -1;
530 mw->channel_flags |= 1 << mw->transmit_channel; 697 mw->channel_flags |= 1 << mw->transmit_channel;
531 } else if (sent < mw->transmit_bytes) { 698 } else if (sent < mw->transmit_bytes) {
532 //TODO: save this data somewhere so it can be sent in poll_socket 699 //TODO: save this data somewhere so it can be sent in poll_socket
533 printf("Sent %d bytes on channel %d, but %d were requested\n", sent, mw->transmit_channel, mw->transmit_bytes); 700 printf("Sent %d bytes on channel %d, but %d were requested\n", sent, mw->transmit_channel, mw->transmit_bytes);
534 } 701 }
702 } else if (sock_fd >= 0 && channel_state == SOCKST_UDP_READY) {
703 udp_send(mw, channel);
535 } else { 704 } else {
536 printf("Unhandled receive of MegaWiFi data on channel %d\n", mw->transmit_channel); 705 printf("Unhandled receive of MegaWiFi data on channel %d\n", mw->transmit_channel);
537 } 706 }
538 } 707 }
539 mw->transmit_bytes = mw->expected_bytes = 0; 708 mw->transmit_bytes = mw->expected_bytes = 0;