mirror of
https://github.com/melonDS-emu/melonDS.git
synced 2025-07-24 14:49:53 -06:00
1146 lines
32 KiB
C++
1146 lines
32 KiB
C++
/*
|
|
Copyright 2016-2019 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;
|
|
}
|
|
|
|
}
|