aboutsummaryrefslogtreecommitdiff
path: root/src/frontend/qt_sdl/LAN_Socket.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/frontend/qt_sdl/LAN_Socket.cpp')
-rw-r--r--src/frontend/qt_sdl/LAN_Socket.cpp1145
1 files changed, 1145 insertions, 0 deletions
diff --git a/src/frontend/qt_sdl/LAN_Socket.cpp b/src/frontend/qt_sdl/LAN_Socket.cpp
new file mode 100644
index 0000000..c6fbd4b
--- /dev/null
+++ b/src/frontend/qt_sdl/LAN_Socket.cpp
@@ -0,0 +1,1145 @@
+/*
+ Copyright 2016-2020 Arisotura
+
+ This file is part of melonDS.
+
+ melonDS is free software: you can redistribute it and/or modify it under
+ the terms of the GNU General Public License as published by the Free
+ Software Foundation, either version 3 of the License, or (at your option)
+ any later version.
+
+ melonDS is distributed in the hope that it will be useful, but WITHOUT ANY
+ WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with melonDS. If not, see http://www.gnu.org/licenses/.
+*/
+
+// indirect LAN interface, powered by BSD sockets.
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "../Wifi.h"
+#include "LAN_Socket.h"
+#include "../Config.h"
+
+#ifdef __WIN32__
+ #include <winsock2.h>
+ #include <ws2tcpip.h>
+ #define socket_t SOCKET
+ #define sockaddr_t SOCKADDR
+#else
+ #include <unistd.h>
+ #include <arpa/inet.h>
+ #include <netinet/in.h>
+ #include <sys/types.h>
+ #include <sys/select.h>
+ #include <sys/socket.h>
+ #include <netdb.h>
+ #define socket_t int
+ #define sockaddr_t struct sockaddr
+ #define closesocket close
+#endif
+
+#ifndef INVALID_SOCKET
+#define INVALID_SOCKET (socket_t)-1
+#endif
+
+
+namespace LAN_Socket
+{
+
+const u32 kSubnet = 0x0A400000;
+const u32 kServerIP = kSubnet | 0x01;
+const u32 kDNSIP = kSubnet | 0x02;
+const u32 kClientIP = kSubnet | 0x10;
+
+const u8 kServerMAC[6] = {0x00, 0xAB, 0x33, 0x28, 0x99, 0x44};
+const u8 kDNSMAC[6] = {0x00, 0xAB, 0x33, 0x28, 0x99, 0x55};
+
+u8 PacketBuffer[2048];
+int PacketLen;
+volatile int RXNum;
+
+u16 IPv4ID;
+
+
+// TODO: UDP sockets
+// * use FIFO list
+// * assign new socket when seeing new IP/port
+
+
+typedef struct
+{
+ u8 DestIP[4];
+ u16 SourcePort;
+ u16 DestPort;
+
+ u32 SeqNum; // sequence number for incoming frames
+ u32 AckNum;
+
+ // 0: unused
+ // 1: connected
+ u8 Status;
+
+ socket_t Backend;
+
+} TCPSocket;
+
+typedef struct
+{
+ u8 DestIP[4];
+ u16 SourcePort;
+ u16 DestPort;
+
+ socket_t Backend;
+ struct sockaddr_in BackendAddr;
+
+} UDPSocket;
+
+TCPSocket TCPSocketList[16];
+UDPSocket UDPSocketList[4];
+
+int UDPSocketID = 0;
+
+
+bool Init()
+{
+ // TODO: how to deal with cases where an adapter is unplugged or changes config??
+ //if (PCapLib) return true;
+
+ //Lib = NULL;
+ PacketLen = 0;
+ RXNum = 0;
+
+ IPv4ID = 1;
+
+ memset(TCPSocketList, 0, sizeof(TCPSocketList));
+ memset(UDPSocketList, 0, sizeof(UDPSocketList));
+
+ UDPSocketID = 0;
+
+ return true;
+}
+
+void DeInit()
+{
+ for (int i = 0; i < (sizeof(TCPSocketList)/sizeof(TCPSocket)); i++)
+ {
+ TCPSocket* sock = &TCPSocketList[i];
+ if (sock->Backend) closesocket(sock->Backend);
+ }
+
+ for (int i = 0; i < (sizeof(UDPSocketList)/sizeof(UDPSocket)); i++)
+ {
+ UDPSocket* sock = &UDPSocketList[i];
+ if (sock->Backend) closesocket(sock->Backend);
+ }
+}
+
+
+void FinishUDPFrame(u8* data, int len)
+{
+ u8* ipheader = &data[0xE];
+ u8* udpheader = &data[0x22];
+
+ // lengths
+ *(u16*)&ipheader[2] = htons(len - 0xE);
+ *(u16*)&udpheader[4] = htons(len - (0xE + 0x14));
+
+ // IP checksum
+ u32 tmp = 0;
+
+ for (int i = 0; i < 20; i += 2)
+ tmp += ntohs(*(u16*)&ipheader[i]);
+ while (tmp >> 16)
+ tmp = (tmp & 0xFFFF) + (tmp >> 16);
+ tmp ^= 0xFFFF;
+ *(u16*)&ipheader[10] = htons(tmp);
+
+ // UDP checksum
+ // (note: normally not mandatory, but some older sgIP versions require it)
+ tmp = 0;
+ tmp += ntohs(*(u16*)&ipheader[12]);
+ tmp += ntohs(*(u16*)&ipheader[14]);
+ tmp += ntohs(*(u16*)&ipheader[16]);
+ tmp += ntohs(*(u16*)&ipheader[18]);
+ tmp += ntohs(0x1100);
+ tmp += (len-0x22);
+ for (u8* i = udpheader; i < &udpheader[len-0x23]; i += 2)
+ tmp += ntohs(*(u16*)i);
+ if (len & 1)
+ tmp += ntohs((u_short)udpheader[len-0x23]);
+ while (tmp >> 16)
+ tmp = (tmp & 0xFFFF) + (tmp >> 16);
+ tmp ^= 0xFFFF;
+ if (tmp == 0) tmp = 0xFFFF;
+ *(u16*)&udpheader[6] = htons(tmp);
+}
+
+void FinishTCPFrame(u8* data, int len)
+{
+ u8* ipheader = &data[0xE];
+ u8* tcpheader = &data[0x22];
+
+ // lengths
+ *(u16*)&ipheader[2] = htons(len - 0xE);
+
+ // IP checksum
+ u32 tmp = 0;
+
+ for (int i = 0; i < 20; i += 2)
+ tmp += ntohs(*(u16*)&ipheader[i]);
+ while (tmp >> 16)
+ tmp = (tmp & 0xFFFF) + (tmp >> 16);
+ tmp ^= 0xFFFF;
+ *(u16*)&ipheader[10] = htons(tmp);
+
+ u32 tcplen = ntohs(*(u16*)&ipheader[2]) - 0x14;
+
+ // TCP checksum
+ tmp = 0;
+ tmp += ntohs(*(u16*)&ipheader[12]);
+ tmp += ntohs(*(u16*)&ipheader[14]);
+ tmp += ntohs(*(u16*)&ipheader[16]);
+ tmp += ntohs(*(u16*)&ipheader[18]);
+ tmp += ntohs(0x0600);
+ tmp += tcplen;
+ for (u8* i = tcpheader; i < &tcpheader[tcplen-1]; i += 2)
+ tmp += ntohs(*(u16*)i);
+ if (tcplen & 1)
+ tmp += ntohs((u_short)tcpheader[tcplen-1]);
+ while (tmp >> 16)
+ tmp = (tmp & 0xFFFF) + (tmp >> 16);
+ tmp ^= 0xFFFF;
+ *(u16*)&tcpheader[16] = htons(tmp);
+}
+
+
+void HandleDHCPFrame(u8* data, int len)
+{
+ u8 type = 0xFF;
+
+ u32 transid = *(u32*)&data[0x2E];
+
+ u8* options = &data[0x11A];
+ for (;;)
+ {
+ if (options >= &data[len]) break;
+ u8 opt = *options++;
+ if (opt == 255) break;
+
+ u8 len = *options++;
+ switch (opt)
+ {
+ case 53: // frame type
+ type = options[0];
+ break;
+ }
+
+ options += len;
+ }
+
+ if (type == 0xFF)
+ {
+ printf("DHCP: bad frame\n");
+ return;
+ }
+
+ printf("DHCP: frame type %d, transid %08X\n", type, transid);
+
+ if (type == 1 || // discover
+ type == 3) // request
+ {
+ u8 resp[512];
+ u8* out = &resp[0];
+
+ // ethernet
+ memcpy(out, &data[6], 6); out += 6;
+ memcpy(out, kServerMAC, 6); out += 6;
+ *(u16*)out = htons(0x0800); out += 2;
+
+ // IP
+ u8* ipheader = out;
+ *out++ = 0x45;
+ *out++ = 0x00;
+ *(u16*)out = 0; out += 2; // total length
+ *(u16*)out = htons(IPv4ID); out += 2; IPv4ID++;
+ *out++ = 0x00;
+ *out++ = 0x00;
+ *out++ = 0x80; // TTL
+ *out++ = 0x11; // protocol (UDP)
+ *(u16*)out = 0; out += 2; // checksum
+ *(u32*)out = htonl(kServerIP); out += 4; // source IP
+ if (type == 1)
+ {
+ *(u32*)out = htonl(0xFFFFFFFF); out += 4; // destination IP
+ }
+ else if (type == 3)
+ {
+ *(u32*)out = htonl(kClientIP); out += 4; // destination IP
+ }
+
+ // UDP
+ u8* udpheader = out;
+ *(u16*)out = htons(67); out += 2; // source port
+ *(u16*)out = htons(68); out += 2; // destination port
+ *(u16*)out = 0; out += 2; // length
+ *(u16*)out = 0; out += 2; // checksum
+
+ // DHCP
+ u8* body = out;
+ *out++ = 0x02;
+ *out++ = 0x01;
+ *out++ = 0x06;
+ *out++ = 0x00;
+ *(u32*)out = transid; out += 4;
+ *(u16*)out = 0; out += 2; // seconds elapsed
+ *(u16*)out = 0; out += 2;
+ *(u32*)out = htonl(0x00000000); out += 4; // client IP
+ *(u32*)out = htonl(kClientIP); out += 4; // your IP
+ *(u32*)out = htonl(kServerIP); out += 4; // server IP
+ *(u32*)out = htonl(0x00000000); out += 4; // gateway IP
+ memcpy(out, &data[6], 6); out += 6;
+ memset(out, 0, 10); out += 10;
+ memset(out, 0, 192); out += 192;
+ *(u32*)out = 0x63538263; out += 4; // DHCP magic
+
+ // DHCP options
+ *out++ = 53; *out++ = 1;
+ *out++ = (type==1) ? 2 : 5; // DHCP type: offer/ack
+ *out++ = 1; *out++ = 4;
+ *(u32*)out = htonl(0xFFFFFF00); out += 4; // subnet mask
+ *out++ = 3; *out++ = 4;
+ *(u32*)out = htonl(kServerIP); out += 4; // router
+ *out++ = 51; *out++ = 4;
+ *(u32*)out = htonl(442030); out += 4; // lease time
+ *out++ = 54; *out++ = 4;
+ *(u32*)out = htonl(kServerIP); out += 4; // DHCP server
+ *out++ = 6; *out++ = 4;
+ *(u32*)out = htonl(kDNSIP); out += 4; // DNS (hax)
+
+ *out++ = 0xFF;
+ memset(out, 0, 20); out += 20;
+
+ u32 framelen = (u32)(out - &resp[0]);
+ if (framelen & 1) { *out++ = 0; framelen++; }
+ FinishUDPFrame(resp, framelen);
+
+ // TODO: if there is already a packet queued, this will overwrite it
+ // that being said, this will only happen during DHCP setup, so probably
+ // not a big deal
+
+ PacketLen = framelen;
+ memcpy(PacketBuffer, resp, PacketLen);
+ RXNum = 1;
+ }
+}
+
+void HandleDNSFrame(u8* data, int len)
+{
+ u8* ipheader = &data[0xE];
+ u8* udpheader = &data[0x22];
+ u8* dnsbody = &data[0x2A];
+
+ u32 srcip = ntohl(*(u32*)&ipheader[12]);
+ u16 srcport = ntohs(*(u16*)&udpheader[0]);
+
+ u16 id = ntohs(*(u16*)&dnsbody[0]);
+ u16 flags = ntohs(*(u16*)&dnsbody[2]);
+ u16 numquestions = ntohs(*(u16*)&dnsbody[4]);
+ u16 numanswers = ntohs(*(u16*)&dnsbody[6]);
+ u16 numauth = ntohs(*(u16*)&dnsbody[8]);
+ u16 numadd = ntohs(*(u16*)&dnsbody[10]);
+
+ printf("DNS: ID=%04X, flags=%04X, Q=%d, A=%d, auth=%d, add=%d\n",
+ id, flags, numquestions, numanswers, numauth, numadd);
+
+ // for now we only take 'simple' DNS requests
+ if (flags & 0x8000) return;
+ if (numquestions != 1 || numanswers != 0) return;
+
+ u8 resp[1024];
+ u8* out = &resp[0];
+
+ // ethernet
+ memcpy(out, &data[6], 6); out += 6;
+ memcpy(out, kServerMAC, 6); out += 6;
+ *(u16*)out = htons(0x0800); out += 2;
+
+ // IP
+ u8* resp_ipheader = out;
+ *out++ = 0x45;
+ *out++ = 0x00;
+ *(u16*)out = 0; out += 2; // total length
+ *(u16*)out = htons(IPv4ID); out += 2; IPv4ID++;
+ *out++ = 0x00;
+ *out++ = 0x00;
+ *out++ = 0x80; // TTL
+ *out++ = 0x11; // protocol (UDP)
+ *(u16*)out = 0; out += 2; // checksum
+ *(u32*)out = htonl(kDNSIP); out += 4; // source IP
+ *(u32*)out = htonl(srcip); out += 4; // destination IP
+
+ // UDP
+ u8* resp_udpheader = out;
+ *(u16*)out = htons(53); out += 2; // source port
+ *(u16*)out = htons(srcport); out += 2; // destination port
+ *(u16*)out = 0; out += 2; // length
+ *(u16*)out = 0; out += 2; // checksum
+
+ // DNS
+ u8* resp_body = out;
+ *(u16*)out = htons(id); out += 2; // ID
+ *(u16*)out = htons(0x8000); out += 2; // flags
+ *(u16*)out = htons(numquestions); out += 2; // num questions
+ *(u16*)out = htons(numquestions); out += 2; // num answers
+ *(u16*)out = 0; out += 2; // num authority
+ *(u16*)out = 0; out += 2; // num additional
+
+ u32 curoffset = 12;
+ for (u16 i = 0; i < numquestions; i++)
+ {
+ if (curoffset >= (len-0x2A)) return;
+
+ u8 bitlength = 0;
+ while ((bitlength = dnsbody[curoffset++]) != 0)
+ curoffset += bitlength;
+
+ curoffset += 4;
+ }
+
+ u32 qlen = curoffset-12;
+ if (qlen > 512) return;
+ memcpy(out, &dnsbody[12], qlen); out += qlen;
+
+ curoffset = 12;
+ for (u16 i = 0; i < numquestions; i++)
+ {
+ // assemble the requested domain name
+ u8 bitlength = 0;
+ char domainname[256] = ""; int o = 0;
+ while ((bitlength = dnsbody[curoffset++]) != 0)
+ {
+ if ((o+bitlength) >= 255)
+ {
+ // welp. atleast try not to explode.
+ domainname[o++] = '\0';
+ break;
+ }
+
+ strncpy(&domainname[o], (const char *)&dnsbody[curoffset], bitlength);
+ o += bitlength;
+
+ curoffset += bitlength;
+ if (dnsbody[curoffset] != 0)
+ domainname[o++] = '.';
+ else
+ domainname[o++] = '\0';
+ }
+
+ u16 type = ntohs(*(u16*)&dnsbody[curoffset]);
+ u16 cls = ntohs(*(u16*)&dnsbody[curoffset+2]);
+
+ printf("- q%d: %04X %04X %s", i, type, cls, domainname);
+
+ // get answer
+ struct addrinfo dns_hint;
+ struct addrinfo* dns_res;
+ u32 addr_res;
+
+ memset(&dns_hint, 0, sizeof(dns_hint));
+ dns_hint.ai_family = AF_INET; // TODO: other address types (INET6, etc)
+ if (getaddrinfo(domainname, "0", &dns_hint, &dns_res) == 0)
+ {
+ struct addrinfo* p = dns_res;
+ while (p)
+ {
+ struct sockaddr_in* addr = (struct sockaddr_in*)p->ai_addr;
+ /*printf(" -> %d.%d.%d.%d",
+ addr->sin_addr.S_un.S_un_b.s_b1, addr->sin_addr.S_un.S_un_b.s_b2,
+ addr->sin_addr.S_un.S_un_b.s_b3, addr->sin_addr.S_un.S_un_b.s_b4);*/
+
+ //addr_res = addr->sin_addr.S_un.S_addr;
+ addr_res = *(u32*)&addr->sin_addr;
+ p = p->ai_next;
+ }
+ }
+ else
+ {
+ printf(" shat itself :(");
+ addr_res = 0;
+ }
+
+ printf("\n");
+ curoffset += 4;
+
+ // TODO: betterer support
+ // (under which conditions does the C00C marker work?)
+ *(u16*)out = htons(0xC00C); out += 2;
+ *(u16*)out = htons(type); out += 2;
+ *(u16*)out = htons(cls); out += 2;
+ *(u32*)out = htonl(3600); out += 4; // TTL (hardcoded for now)
+ *(u16*)out = htons(4); out += 2; // address length
+ *(u32*)out = addr_res; out += 4; // address
+ }
+
+ u32 framelen = (u32)(out - &resp[0]);
+ if (framelen & 1) { *out++ = 0; framelen++; }
+ FinishUDPFrame(resp, framelen);
+
+ // TODO: if there is already a packet queued, this will overwrite it
+ // that being said, this will only happen during DHCP setup, so probably
+ // not a big deal
+
+ PacketLen = framelen;
+ memcpy(PacketBuffer, resp, PacketLen);
+ RXNum = 1;
+}
+
+void UDP_BuildIncomingFrame(UDPSocket* sock, u8* data, int len)
+{
+ u8 resp[2048];
+ u8* out = &resp[0];
+
+ if (len > 1536) return;
+
+ // ethernet
+ memcpy(out, Wifi::GetMAC(), 6); out += 6; // hurf
+ memcpy(out, kServerMAC, 6); out += 6;
+ *(u16*)out = htons(0x0800); out += 2;
+
+ // IP
+ u8* resp_ipheader = out;
+ *out++ = 0x45;
+ *out++ = 0x00;
+ *(u16*)out = 0; out += 2; // total length
+ *(u16*)out = htons(IPv4ID); out += 2; IPv4ID++;
+ *out++ = 0x00;
+ *out++ = 0x00;
+ *out++ = 0x80; // TTL
+ *out++ = 0x11; // protocol (UDP)
+ *(u16*)out = 0; out += 2; // checksum
+ memcpy(out, sock->DestIP, 4); out += 4; // source IP
+ *(u32*)out = htonl(kClientIP); out += 4; // destination IP
+
+ // UDP
+ u8* resp_tcpheader = out;
+ *(u16*)out = htons(sock->DestPort); out += 2; // source port
+ *(u16*)out = htons(sock->SourcePort); out += 2; // destination port
+ *(u16*)out = htons(len+8); out += 2; // length of header+data
+ *(u16*)out = 0; out += 2; // checksum
+
+ memcpy(out, data, len); out += len;
+
+ u32 framelen = (u32)(out - &resp[0]);
+ FinishUDPFrame(resp, framelen);
+
+ // TODO: if there is already a packet queued, this will overwrite it
+ // that being said, this will only happen during DHCP setup, so probably
+ // not a big deal
+
+ PacketLen = framelen;
+ memcpy(PacketBuffer, resp, PacketLen);
+ RXNum = 1;
+}
+
+void HandleUDPFrame(u8* data, int len)
+{
+ u8* ipheader = &data[0xE];
+ u8* udpheader = &data[0x22];
+
+ // debug
+ /*for (int j = 0; j < len; j += 16)
+ {
+ int rem = len - j;
+ if (rem > 16) rem = 16;
+ for (int i = 0; i < rem; i++)
+ {
+ printf("%02X ", data[i+j]);
+ }
+ printf("\n");
+ }*/
+
+ u16 srcport = ntohs(*(u16*)&udpheader[0]);
+ u16 dstport = ntohs(*(u16*)&udpheader[2]);
+
+ int sockid = -1;
+ UDPSocket* sock;
+ for (int i = 0; i < (sizeof(UDPSocketList)/sizeof(UDPSocket)); i++)
+ {
+ sock = &UDPSocketList[i];
+ if (sock->Backend != 0 && !memcmp(&sock->DestIP, &ipheader[16], 4) &&
+ sock->SourcePort == srcport && sock->DestPort == dstport)
+ {
+ sockid = i;
+ break;
+ }
+ }
+
+ if (sockid == -1)
+ {
+ sockid = UDPSocketID;
+ sock = &UDPSocketList[sockid];
+
+ UDPSocketID++;
+ if (UDPSocketID >= (sizeof(UDPSocketList)/sizeof(UDPSocket)))
+ UDPSocketID = 0;
+
+ if (sock->Backend != 0)
+ {
+ printf("LANMAGIC: closing previous UDP socket #%d\n", sockid);
+ closesocket(sock->Backend);
+ }
+
+ sock->Backend = socket(AF_INET, SOCK_DGRAM, 0);
+
+ memcpy(sock->DestIP, &ipheader[16], 4);
+ sock->SourcePort = srcport;
+ sock->DestPort = dstport;
+
+ memset(&sock->BackendAddr, 0, sizeof(sock->BackendAddr));
+ sock->BackendAddr.sin_family = AF_INET;
+ sock->BackendAddr.sin_port = htons(dstport);
+ memcpy(&sock->BackendAddr.sin_addr, &ipheader[16], 4);
+ /*if (bind(sock->Backend, (struct sockaddr*)&sock->BackendAddr, sizeof(sock->BackendAddr)) == -1)
+ {
+ printf("bind() shat itself :(\n");
+ }*/
+
+ printf("LANMAGIC: opening UDP socket #%d to %d.%d.%d.%d:%d, srcport %d\n",
+ sockid,
+ ipheader[16], ipheader[17], ipheader[18], ipheader[19],
+ dstport, srcport);
+ }
+
+ u16 udplen = ntohs(*(u16*)&udpheader[4]) - 8;
+
+ printf("UDP: socket %d sending %d bytes\n", sockid, udplen);
+ sendto(sock->Backend, (char*)&udpheader[8], udplen, 0,
+ (struct sockaddr*)&sock->BackendAddr, sizeof(sock->BackendAddr));
+}
+
+void TCP_SYNACK(TCPSocket* sock, u8* data, int len)
+{
+ u8 resp[128];
+ u8* out = &resp[0];
+
+ u8* ipheader = &data[0xE];
+ u8* tcpheader = &data[0x22];
+
+ u32 seqnum = htonl(*(u32*)&tcpheader[4]);
+ seqnum++;
+ sock->AckNum = seqnum;
+
+ //printf("SYNACK SEQ=%08X|%08X\n", sock->SeqNum, sock->AckNum);
+
+ // ethernet
+ memcpy(out, &data[6], 6); out += 6;
+ memcpy(out, kServerMAC, 6); out += 6;
+ *(u16*)out = htons(0x0800); out += 2;
+
+ // IP
+ u8* resp_ipheader = out;
+ *out++ = 0x45;
+ *out++ = 0x00;
+ *(u16*)out = 0; out += 2; // total length
+ *(u16*)out = htons(IPv4ID); out += 2; IPv4ID++;
+ *out++ = 0x00;
+ *out++ = 0x00;
+ *out++ = 0x80; // TTL
+ *out++ = 0x06; // protocol (TCP)
+ *(u16*)out = 0; out += 2; // checksum
+ *(u32*)out = *(u32*)&ipheader[16]; out += 4; // source IP
+ *(u32*)out = *(u32*)&ipheader[12]; out += 4; // destination IP
+
+ // TCP
+ u8* resp_tcpheader = out;
+ *(u16*)out = *(u16*)&tcpheader[2]; out += 2; // source port
+ *(u16*)out = *(u16*)&tcpheader[0]; out += 2; // destination port
+ *(u32*)out = htonl(sock->SeqNum); out += 4; sock->SeqNum++; // seq number
+ *(u32*)out = htonl(seqnum); out += 4; // ack seq number
+ *(u16*)out = htons(0x8012); out += 2; // flags (SYN+ACK)
+ *(u16*)out = htons(0x7000); out += 2; // window size (uuuh)
+ *(u16*)out = 0; out += 2; // checksum
+ *(u16*)out = 0; out += 2; // urgent pointer
+
+ // TCP options
+ *out++ = 0x02; *out++ = 0x04; // max segment size
+ *(u16*)out = htons(0x05B4); out += 2;
+ *out++ = 0x01;
+ *out++ = 0x01;
+ *out++ = 0x04; *out++ = 0x02; // SACK permitted
+ *out++ = 0x01;
+ *out++ = 0x03; *out++ = 0x03; // window size
+ *out++ = 0x08;
+
+ u32 framelen = (u32)(out - &resp[0]);
+ //if (framelen & 1) { *out++ = 0; framelen++; }
+ FinishTCPFrame(resp, framelen);
+
+ // TODO: if there is already a packet queued, this will overwrite it
+ // that being said, this will only happen during DHCP setup, so probably
+ // not a big deal
+
+ PacketLen = framelen;
+ memcpy(PacketBuffer, resp, PacketLen);
+ RXNum = 1;
+}
+
+void TCP_ACK(TCPSocket* sock, bool fin)
+{
+ u8 resp[64];
+ u8* out = &resp[0];
+
+ u16 flags = 0x5010;
+ if (fin) flags |= 0x0001;
+
+ //printf("%sACK SEQ=%08X|%08X\n", fin?"FIN":" ", sock->SeqNum, sock->AckNum);
+
+ // ethernet
+ memcpy(out, Wifi::GetMAC(), 6); out += 6;
+ memcpy(out, kServerMAC, 6); out += 6;
+ *(u16*)out = htons(0x0800); out += 2;
+
+ // IP
+ u8* resp_ipheader = out;
+ *out++ = 0x45;
+ *out++ = 0x00;
+ *(u16*)out = 0; out += 2; // total length
+ *(u16*)out = htons(IPv4ID); out += 2; IPv4ID++;
+ *out++ = 0x00;
+ *out++ = 0x00;
+ *out++ = 0x80; // TTL
+ *out++ = 0x06; // protocol (TCP)
+ *(u16*)out = 0; out += 2; // checksum
+ *(u32*)out = *(u32*)&sock->DestIP; out += 4; // source IP
+ *(u32*)out = htonl(kClientIP); out += 4; // destination IP
+
+ // TCP
+ u8* resp_tcpheader = out;
+ *(u16*)out = htonl(sock->DestPort); out += 2; // source port
+ *(u16*)out = htonl(sock->SourcePort); out += 2; // destination port
+ *(u32*)out = htonl(sock->SeqNum); out += 4; // seq number
+ *(u32*)out = htonl(sock->AckNum); out += 4; // ack seq number
+ *(u16*)out = htons(flags); out += 2; // flags
+ *(u16*)out = htons(0x7000); out += 2; // window size (uuuh)
+ *(u16*)out = 0; out += 2; // checksum
+ *(u16*)out = 0; out += 2; // urgent pointer
+
+ u32 framelen = (u32)(out - &resp[0]);
+ //if (framelen & 1) { *out++ = 0; framelen++; }
+ FinishTCPFrame(resp, framelen);
+
+ // TODO: if there is already a packet queued, this will overwrite it
+ // that being said, this will only happen during DHCP setup, so probably
+ // not a big deal
+
+ PacketLen = framelen;
+ memcpy(PacketBuffer, resp, PacketLen);
+ RXNum = 1;
+}
+
+void TCP_BuildIncomingFrame(TCPSocket* sock, u8* data, int len)
+{
+ u8 resp[2048];
+ u8* out = &resp[0];
+
+ if (len > 1536) return;
+//printf("INCOMING SEQ=%08X|%08X\n", sock->SeqNum, sock->AckNum);
+ // ethernet
+ memcpy(out, Wifi::GetMAC(), 6); out += 6; // hurf
+ memcpy(out, kServerMAC, 6); out += 6;
+ *(u16*)out = htons(0x0800); out += 2;
+
+ // IP
+ u8* resp_ipheader = out;
+ *out++ = 0x45;
+ *out++ = 0x00;
+ *(u16*)out = 0; out += 2; // total length
+ *(u16*)out = htons(IPv4ID); out += 2; IPv4ID++;
+ *out++ = 0x00;
+ *out++ = 0x00;
+ *out++ = 0x80; // TTL
+ *out++ = 0x06; // protocol (TCP)
+ *(u16*)out = 0; out += 2; // checksum
+ memcpy(out, sock->DestIP, 4); out += 4; // source IP
+ *(u32*)out = htonl(kClientIP); out += 4; // destination IP
+
+ // TCP
+ u8* resp_tcpheader = out;
+ *(u16*)out = htons(sock->DestPort); out += 2; // source port
+ *(u16*)out = htons(sock->SourcePort); out += 2; // destination port
+ *(u32*)out = htonl(sock->SeqNum); out += 4; // seq number
+ *(u32*)out = htonl(sock->AckNum); out += 4; // ack seq number
+ *(u16*)out = htons(0x5018); out += 2; // flags (ACK, PSH)
+ *(u16*)out = htons(0x7000); out += 2; // window size (uuuh)
+ *(u16*)out = 0; out += 2; // checksum
+ *(u16*)out = 0; out += 2; // urgent pointer
+
+ memcpy(out, data, len); out += len;
+
+ u32 framelen = (u32)(out - &resp[0]);
+ FinishTCPFrame(resp, framelen);
+
+ // TODO: if there is already a packet queued, this will overwrite it
+ // that being said, this will only happen during DHCP setup, so probably
+ // not a big deal
+
+ PacketLen = framelen;
+ memcpy(PacketBuffer, resp, PacketLen);
+ RXNum = 1;
+
+ sock->SeqNum += len;
+}
+
+void HandleTCPFrame(u8* data, int len)
+{
+ u8* ipheader = &data[0xE];
+ u8* tcpheader = &data[0x22];
+
+ u16 srcport = ntohs(*(u16*)&tcpheader[0]);
+ u16 dstport = ntohs(*(u16*)&tcpheader[2]);
+ u16 flags = ntohs(*(u16*)&tcpheader[12]);
+
+ u32 tcpheaderlen = 4 * (flags >> 12);
+ u32 tcplen = ntohs(*(u16*)&ipheader[2]) - 0x14;
+ u32 tcpdatalen = tcplen - tcpheaderlen;
+
+ /*printf("tcpflags=%04X header=%d data=%d seq=%08X|%08X\n",
+ flags, tcpheaderlen, tcpdatalen,
+ ntohl(*(u32*)&tcpheader[4]),
+ ntohl(*(u32*)&tcpheader[8]));*/
+
+ if (flags & 0x002) // SYN
+ {
+ int sockid = -1;
+ TCPSocket* sock;
+ for (int i = 0; i < (sizeof(TCPSocketList)/sizeof(TCPSocket)); i++)
+ {
+ sock = &TCPSocketList[i];
+ if (sock->Status != 0 && !memcmp(&sock->DestIP, &ipheader[16], 4) &&
+ sock->SourcePort == srcport && sock->DestPort == dstport)
+ {
+ printf("LANMAGIC: duplicate TCP socket\n");
+ sockid = i;
+ break;
+ }
+ }
+
+ if (sockid == -1)
+ {
+ for (int i = 0; i < (sizeof(TCPSocketList)/sizeof(TCPSocket)); i++)
+ {
+ sock = &TCPSocketList[i];
+ if (sock->Status == 0)
+ {
+ sockid = i;
+ break;
+ }
+ }
+ }
+
+ if (sockid == -1)
+ {
+ printf("LANMAGIC: !! TCP SOCKET LIST FULL\n");
+ return;
+ }
+
+ printf("LANMAGIC: opening TCP socket #%d to %d.%d.%d.%d:%d, srcport %d\n",
+ sockid,
+ ipheader[16], ipheader[17], ipheader[18], ipheader[19],
+ dstport, srcport);
+
+ // keep track of it
+ sock->Status = 1;
+ memcpy(sock->DestIP, &ipheader[16], 4);
+ sock->DestPort = dstport;
+ sock->SourcePort = srcport;
+ sock->SeqNum = 0x13370000;
+ sock->AckNum = 0;
+
+ // open backend socket
+ if (!sock->Backend)
+ {
+ sock->Backend = socket(AF_INET, SOCK_STREAM, 0);
+ }
+
+ struct sockaddr_in conn_addr;
+ memset(&conn_addr, 0, sizeof(conn_addr));
+ conn_addr.sin_family = AF_INET;
+ memcpy(&conn_addr.sin_addr, &ipheader[16], 4);
+ conn_addr.sin_port = htons(dstport);
+ if (connect(sock->Backend, (sockaddr*)&conn_addr, sizeof(conn_addr)) == -1)
+ {
+ printf("connect() shat itself :(\n");
+ }
+ else
+ {
+ // acknowledge it
+ TCP_SYNACK(sock, data, len);
+ }
+ }
+ else
+ {
+ int sockid = -1;
+ TCPSocket* sock;
+ for (int i = 0; i < (sizeof(TCPSocketList)/sizeof(TCPSocket)); i++)
+ {
+ sock = &TCPSocketList[i];
+ if (sock->Status != 0 && !memcmp(&sock->DestIP, &ipheader[16], 4) &&
+ sock->SourcePort == srcport && sock->DestPort == dstport)
+ {
+ sockid = i;
+ break;
+ }
+ }
+
+ if (sockid == -1)
+ {
+ printf("LANMAGIC: bad TCP packet\n");
+ return;
+ }
+
+ // TODO: check those
+ u32 seqnum = ntohl(*(u32*)&tcpheader[4]);
+ u32 acknum = ntohl(*(u32*)&tcpheader[8]);
+ sock->SeqNum = acknum;
+ sock->AckNum = seqnum + tcpdatalen;
+
+ // send data over the socket
+ if (tcpdatalen > 0)
+ {
+ u8* tcpdata = &tcpheader[tcpheaderlen];
+
+ printf("TCP: socket %d sending %d bytes (flags=%04X)\n", sockid, tcpdatalen, flags);
+ send(sock->Backend, (char*)tcpdata, tcpdatalen, 0);
+
+ // kind of a hack, there
+ TCP_ACK(sock, false);
+ }
+
+ if (flags & 0x001) // FIN
+ {
+ // TODO: timeout etc
+ printf("TCP: socket %d closing\n", sockid);
+
+ sock->Status = 0;
+ closesocket(sock->Backend);
+ sock->Backend = 0;
+ }
+ }
+}
+
+void HandleARPFrame(u8* data, int len)
+{
+ u16 protocol = ntohs(*(u16*)&data[0x10]);
+ if (protocol != 0x0800) return;
+
+ u16 op = ntohs(*(u16*)&data[0x14]);
+ u32 targetip = ntohl(*(u32*)&data[0x26]);
+
+ // TODO: handle ARP to the client
+ // this only handles ARP to the DHCP/router
+
+ if (op == 1)
+ {
+ // opcode 1=req 2=reply
+ // sender MAC
+ // sender IP
+ // target MAC
+ // target IP
+
+ const u8* targetmac;
+ if (targetip == kServerIP) targetmac = kServerMAC;
+ else if (targetip == kDNSIP) targetmac = kDNSMAC;
+ else return;
+
+ u8 resp[64];
+ u8* out = &resp[0];
+
+ // ethernet
+ memcpy(out, &data[6], 6); out += 6;
+ memcpy(out, kServerMAC, 6); out += 6;
+ *(u16*)out = htons(0x0806); out += 2;
+
+ // ARP
+ *(u16*)out = htons(0x0001); out += 2; // hardware type
+ *(u16*)out = htons(0x0800); out += 2; // protocol
+ *out++ = 6; // MAC address size
+ *out++ = 4; // IP address size
+ *(u16*)out = htons(0x0002); out += 2; // opcode
+ memcpy(out, targetmac, 6); out += 6;
+ *(u32*)out = htonl(targetip); out += 4;
+ memcpy(out, &data[0x16], 6+4); out += 6+4;
+
+ u32 framelen = (u32)(out - &resp[0]);
+
+ // TODO: if there is already a packet queued, this will overwrite it
+ // that being said, this will only happen during DHCP setup, so probably
+ // not a big deal
+
+ PacketLen = framelen;
+ memcpy(PacketBuffer, resp, PacketLen);
+ RXNum = 1;
+ }
+ else
+ {
+ printf("wat??\n");
+ }
+}
+
+void HandlePacket(u8* data, int len)
+{
+ u16 ethertype = ntohs(*(u16*)&data[0xC]);
+
+ if (ethertype == 0x0800) // IPv4
+ {
+ u8 protocol = data[0x17];
+ if (protocol == 0x11) // UDP
+ {
+ u16 srcport = ntohs(*(u16*)&data[0x22]);
+ u16 dstport = ntohs(*(u16*)&data[0x24]);
+ if (srcport == 68 && dstport == 67) // DHCP
+ {
+ printf("LANMAGIC: DHCP packet\n");
+ return HandleDHCPFrame(data, len);
+ }
+ else if (dstport == 53 && htonl(*(u32*)&data[0x1E]) == kDNSIP) // DNS
+ {
+ printf("LANMAGIC: DNS packet\n");
+ return HandleDNSFrame(data, len);
+ }
+
+ printf("LANMAGIC: UDP packet %d->%d\n", srcport, dstport);
+ return HandleUDPFrame(data, len);
+ }
+ else if (protocol == 0x06) // TCP
+ {
+ printf("LANMAGIC: TCP packet\n");
+ return HandleTCPFrame(data, len);
+ }
+ else
+ printf("LANMAGIC: unsupported IP protocol %02X\n", protocol);
+ }
+ else if (ethertype == 0x0806) // ARP
+ {
+ printf("LANMAGIC: ARP packet\n");
+ return HandleARPFrame(data, len);
+ }
+ else
+ printf("LANMAGIC: unsupported ethernet type %04X\n", ethertype);
+}
+
+int SendPacket(u8* data, int len)
+{
+ if (len > 2048)
+ {
+ printf("LAN_SendPacket: error: packet too long (%d)\n", len);
+ return 0;
+ }
+
+ HandlePacket(data, len);
+ return len;
+}
+
+int RecvPacket(u8* data)
+{
+ int ret = 0;
+ if (RXNum > 0)
+ {
+ memcpy(data, PacketBuffer, PacketLen);
+ ret = PacketLen;
+ RXNum = 0;
+ }
+
+ for (int i = 0; i < (sizeof(TCPSocketList)/sizeof(TCPSocket)); i++)
+ {
+ TCPSocket* sock = &TCPSocketList[i];
+ if (sock->Status != 1) continue;
+
+ fd_set fd;
+ struct timeval tv;
+
+ FD_ZERO(&fd);
+ FD_SET(sock->Backend, &fd);
+ tv.tv_sec = 0;
+ tv.tv_usec = 0;
+
+ if (!select(sock->Backend+1, &fd, 0, 0, &tv))
+ {
+ continue;
+ }
+
+ u8 recvbuf[1024];
+ int recvlen = recv(sock->Backend, (char*)recvbuf, 1024, 0);
+ if (recvlen < 1)
+ {
+ if (recvlen == 0)
+ {
+ // socket has closed from the other side
+ printf("TCP: socket %d closed from other side\n", i);
+ sock->Status = 2;
+ TCP_ACK(sock, true);
+ }
+ continue;
+ }
+
+ printf("TCP: socket %d receiving %d bytes\n", i, recvlen);
+ TCP_BuildIncomingFrame(sock, recvbuf, recvlen);
+
+ // debug
+ /*for (int j = 0; j < recvlen; j += 16)
+ {
+ int rem = recvlen - j;
+ if (rem > 16) rem = 16;
+ for (int k = 0; k < rem; k++)
+ {
+ printf("%02X ", recvbuf[k+j]);
+ }
+ printf("\n");
+ }*/
+
+ //recvlen = recv(sock->Backend, (char*)recvbuf, 1024, 0);
+ //if (recvlen == 0) printf("it closed immediately after\n");
+ }
+
+ for (int i = 0; i < (sizeof(UDPSocketList)/sizeof(UDPSocket)); i++)
+ {
+ UDPSocket* sock = &UDPSocketList[i];
+ if (sock->Backend == 0) continue;
+
+ fd_set fd;
+ struct timeval tv;
+
+ FD_ZERO(&fd);
+ FD_SET(sock->Backend, &fd);
+ tv.tv_sec = 0;
+ tv.tv_usec = 0;
+
+ if (!select(sock->Backend+1, &fd, 0, 0, &tv))
+ {
+ continue;
+ }
+
+ u8 recvbuf[1024];
+ sockaddr_t fromAddr;
+ socklen_t fromLen = sizeof(sockaddr_t);
+ int recvlen = recvfrom(sock->Backend, (char*)recvbuf, 1024, 0, &fromAddr, &fromLen);
+ if (recvlen < 1) continue;
+
+ if (fromAddr.sa_family != AF_INET) continue;
+ struct sockaddr_in* fromAddrIn = (struct sockaddr_in*)&fromAddr;
+ if (memcmp(&fromAddrIn->sin_addr, sock->DestIP, 4)) continue;
+ if (ntohs(fromAddrIn->sin_port) != sock->DestPort) continue;
+
+ printf("UDP: socket %d receiving %d bytes\n", i, recvlen);
+ UDP_BuildIncomingFrame(sock, recvbuf, recvlen);
+ }
+
+ return ret;
+}
+
+}