clean it up
This commit is contained in:
Arisotura
parent
a29e645340
commit
4b73f81ee0
|
|
@ -21,34 +21,17 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <list>
|
||||
#include "../Wifi.h"
|
||||
#include "LAN_Socket.h"
|
||||
#include "../Config.h"
|
||||
|
||||
#include <list>
|
||||
|
||||
#include <slirp/libslirp.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
|
||||
|
||||
|
||||
|
|
@ -67,47 +50,7 @@ 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;
|
||||
|
||||
u32 IPv4ID;
|
||||
|
||||
Slirp* Ctx;
|
||||
|
||||
|
|
@ -221,7 +164,6 @@ void SlirpCbUnregisterPollFD(int fd, void* opaque)
|
|||
|
||||
void SlirpCbNotify(void* opaque)
|
||||
{
|
||||
//
|
||||
}
|
||||
|
||||
SlirpCb cb =
|
||||
|
|
@ -239,21 +181,7 @@ SlirpCb cb =
|
|||
|
||||
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;*/
|
||||
IPv4ID = 0;
|
||||
|
||||
FDListSize = 0;
|
||||
memset(FDList, 0, sizeof(FDList));
|
||||
|
|
@ -269,7 +197,6 @@ bool Init()
|
|||
cfg.vhostname = "melonServer";
|
||||
*(u32*)&cfg.vdhcp_start = htonl(kClientIP);
|
||||
*(u32*)&cfg.vnameserver = htonl(kDNSIP);
|
||||
//cfg.vdhcp_start.S_addr = kServerIP;
|
||||
|
||||
Ctx = slirp_new(&cfg, &cb, nullptr);
|
||||
|
||||
|
|
@ -278,18 +205,6 @@ bool Init()
|
|||
|
||||
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);
|
||||
}*/
|
||||
|
||||
slirp_cleanup(Ctx);
|
||||
}
|
||||
|
||||
|
|
@ -333,165 +248,6 @@ void FinishUDPFrame(u8* data, int len)
|
|||
*(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];
|
||||
|
|
@ -653,540 +409,6 @@ void HandleDNSFrame(u8* data, int len)
|
|||
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)
|
||||
|
|
@ -1196,52 +418,21 @@ int SendPacket(u8* data, int len)
|
|||
}
|
||||
|
||||
u16 ethertype = ntohs(*(u16*)&data[0xC]);
|
||||
printf("packet of type %04X\n", ethertype);
|
||||
|
||||
if (ethertype == 0x0806)
|
||||
{
|
||||
u16 protocol = ntohs(*(u16*)&data[0x10]);
|
||||
u16 op = ntohs(*(u16*)&data[0x14]);
|
||||
u32 sourceip = ntohl(*(u32*)&data[0x26-4-6]);
|
||||
u32 targetip = ntohl(*(u32*)&data[0x26]);
|
||||
printf("ARP: protocol=%04X, op=%04X, source=%08X, target=%08X\n", protocol, op, sourceip, targetip);
|
||||
}
|
||||
else if (ethertype == 0x800)
|
||||
if (ethertype == 0x800)
|
||||
{
|
||||
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
|
||||
if (dstport == 53 && htonl(*(u32*)&data[0x1E]) == kDNSIP) // DNS
|
||||
{
|
||||
printf("LANMAGIC: DHCP packet\n");
|
||||
}
|
||||
else if (dstport == 53 && htonl(*(u32*)&data[0x1E]) == kDNSIP) // DNS
|
||||
{
|
||||
printf("LANMAGIC: DNS packet\n");
|
||||
HandleDNSFrame(data, len);
|
||||
return len;
|
||||
}
|
||||
|
||||
printf("LANMAGIC: UDP packet %08X->%08X %d->%d\n", htonl(*(u32*)&data[0x1A]), htonl(*(u32*)&data[0x1E]), srcport, dstport);
|
||||
}
|
||||
else if (protocol == 0x06) // TCP
|
||||
{
|
||||
printf("LANMAGIC: TCP packet\n");
|
||||
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
printf("%02X ", data[i]);
|
||||
if ((i&0xF)==0xF) printf("\n");
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
else
|
||||
printf("LANMAGIC: unsupported IP protocol %02X\n", protocol);
|
||||
}
|
||||
|
||||
//HandlePacket(data, len);
|
||||
slirp_input(Ctx, data, len);
|
||||
return len;
|
||||
}
|
||||
|
|
@ -1313,13 +504,6 @@ int RecvPacket(u8* data)
|
|||
slirp_pollfds_fill(Ctx, &timeout, SlirpCbAddPoll, nullptr);
|
||||
int res = poll(FDList, FDListSize, timeout);
|
||||
slirp_pollfds_poll(Ctx, res<0, SlirpCbGetREvents, nullptr);
|
||||
|
||||
/*struct pollfd derp = {0};;
|
||||
derp.fd = FDList[0];
|
||||
derp.events = POLLIN | POLLWRNORM;//POLLPRI | POLLIN | POLLWRNORM;
|
||||
int res = poll(&derp, 1, 0);
|
||||
if (res==SOCKET_ERROR) printf("fart: %d / %d\n", WSAGetLastError(), FDList[0]);
|
||||
if(derp.revents) printf("derp: %04X\n", derp.revents);*/
|
||||
}
|
||||
|
||||
if (RXNum > 0)
|
||||
|
|
@ -1329,90 +513,6 @@ int RecvPacket(u8* data)
|
|||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue