X-Git-Url: https://git.ucc.asn.au/?p=tpg%2Facess2.git;a=blobdiff_plain;f=Modules%2FIPStack%2Ftcp.c;h=bb71fdbf58638d9fc2447d292cc140f870641a10;hp=326f044bdd1d659ec420bcb827b1584f290368bf;hb=c205f388f8722a5d73f71ee98b0232444c5c7e9d;hpb=37e9151e7bc0b7341050286fde4341a8f0f9333c

diff --git a/Modules/IPStack/tcp.c b/Modules/IPStack/tcp.c
index 326f044b..bb71fdbf 100644
--- a/Modules/IPStack/tcp.c
+++ b/Modules/IPStack/tcp.c
@@ -2,26 +2,39 @@
  * Acess2 IP Stack
  * - TCP Handling
  */
+#define DEBUG	1
 #include "ipstack.h"
 #include "ipv4.h"
+#include "ipv6.h"
 #include "tcp.h"
 
+#define USE_SELECT	1
+#define HEXDUMP_INCOMING	0
+#define HEXDUMP_OUTGOING	0
+#define	CACHE_FUTURE_PACKETS_IN_BYTES	1	// Use a ring buffer to cache out of order packets
+
 #define TCP_MIN_DYNPORT	0xC000
 #define TCP_MAX_HALFOPEN	1024	// Should be enough
 
+#define TCP_MAX_PACKET_SIZE	1024
+#define TCP_WINDOW_SIZE	0x2000
+#define TCP_RECIEVE_BUFFER_SIZE	0x4000
+
 // === PROTOTYPES ===
-void	TCP_Initialise();
+void	TCP_Initialise(void);
 void	TCP_StartConnection(tTCPConnection *Conn);
 void	TCP_SendPacket(tTCPConnection *Conn, size_t Length, tTCPHeader *Data);
 void	TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer);
 void	TCP_INT_HandleConnectionPacket(tTCPConnection *Connection, tTCPHeader *Header, int Length);
+int	TCP_INT_AppendRecieved(tTCPConnection *Connection, const void *Data, size_t Length);
+void	TCP_INT_UpdateRecievedFromFuture(tTCPConnection *Connection);
 Uint16	TCP_GetUnusedPort();
  int	TCP_AllocatePort(Uint16 Port);
  int	TCP_DeallocatePort(Uint16 Port);
 // --- Server
 tVFS_Node	*TCP_Server_Init(tInterface *Interface);
 char	*TCP_Server_ReadDir(tVFS_Node *Node, int Pos);
-tVFS_Node	*TCP_Server_FindDir(tVFS_Node *Node, char *Name);
+tVFS_Node	*TCP_Server_FindDir(tVFS_Node *Node, const char *Name);
  int	TCP_Server_IOCtl(tVFS_Node *Node, int ID, void *Data);
 void	TCP_Server_Close(tVFS_Node *Node);
 // --- Client
@@ -30,6 +43,8 @@ Uint64	TCP_Client_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buff
 Uint64	TCP_Client_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
  int	TCP_Client_IOCtl(tVFS_Node *Node, int ID, void *Data);
 void	TCP_Client_Close(tVFS_Node *Node);
+// --- Helpers
+ int	WrapBetween(Uint32 Lower, Uint32 Value, Uint32 Higher, Uint32 MaxValue);
 
 // === TEMPLATES ===
 tSocketFile	gTCP_ServerFile = {NULL, "tcps", TCP_Server_Init};
@@ -37,76 +52,82 @@ tSocketFile	gTCP_ClientFile = {NULL, "tcpc", TCP_Client_Init};
 
 // === GLOBALS ===
  int	giTCP_NumHalfopen = 0;
-tSpinlock	glTCP_Listeners;
+tShortSpinlock	glTCP_Listeners;
 tTCPListener	*gTCP_Listeners;
-tSpinlock	glTCP_OutbountCons;
+tShortSpinlock	glTCP_OutbountCons;
 tTCPConnection	*gTCP_OutbountCons;
 Uint32	gaTCP_PortBitmap[0x800];
  int	giTCP_NextOutPort = TCP_MIN_DYNPORT;
 
 // === CODE ===
 /**
- * \fn void TCP_Initialise()
  * \brief Initialise the TCP Layer
+ * 
+ * Registers the client and server files and the GetPacket callback
  */
-void TCP_Initialise()
+void TCP_Initialise(void)
 {
+	giTCP_NextOutPort += rand()%32;
 	IPStack_AddFile(&gTCP_ServerFile);
 	IPStack_AddFile(&gTCP_ClientFile);
 	IPv4_RegisterCallback(IP4PROT_TCP, TCP_GetPacket);
-}
-
-/**
- * \brief Open a connection to another host using TCP
- */
-void TCP_StartConnection(tTCPConnection *Conn)
-{
-	tTCPHeader	hdr;
-
-	hdr.SourcePort = Conn->LocalPort;
-	hdr.DestPort = Conn->RemotePort;
-	Conn->NextSequenceSend = rand();
-	hdr.SequenceNumber = Conn->NextSequenceSend;
-	hdr.DataOffset = (sizeof(tTCPHeader)+3)/4;
-	hdr.Flags = TCP_FLAG_SYN;
-	hdr.WindowSize = 0;	// TODO
-	hdr.Checksum = 0;	// TODO
-	hdr.UrgentPointer = 0;
-	// SEND PACKET
-	TCP_SendPacket( Conn, sizeof(tTCPHeader), &hdr );
-	return ;
+	IPv6_RegisterCallback(IP4PROT_TCP, TCP_GetPacket);
 }
 
 /**
  * \brief Sends a packet from the specified connection, calculating the checksums
  * \param Conn	Connection
  * \param Length	Length of data
- * \param Data	Packet data
+ * \param Data	Packet data (cast as a TCP Header)
  */
 void TCP_SendPacket( tTCPConnection *Conn, size_t Length, tTCPHeader *Data )
 {
-	size_t	buflen;
-	Uint32	*buf;
+	Uint16	checksum[2];
+	
+	Data->Checksum = 0;
+	checksum[1] = htons( ~IPv4_Checksum( (void*)Data, Length ) );	// Partial checksum
+	if(Length & 1)
+		((Uint8*)Data)[Length] = 0;
+	
+	// TODO: Fragment packet
+	
 	switch( Conn->Interface->Type )
 	{
-	case 4:	// Append IPv4 Pseudo Header
-		buflen = 4 + 4 + 4 + ((Length+1)&1);
-		buf = malloc( buflen );
-		buf[0] = Conn->Interface->IP4.Address.L;
-		buf[1] = Conn->RemoteIP.v4.L;
-		buf[2] = (htons(Length)<<16) | (6<<8) | 0;
-		Data->Checksum = 0;
-		memcpy( &buf[3], Data, Length );
-		Data->Checksum = IPv4_Checksum( buf, buflen );
-		free(buf);
-		IPv4_SendPacket(Conn->Interface, Conn->RemoteIP.v4, IP4PROT_TCP, 0, buflen, buf);
+	case 4:
+		// Append IPv4 Pseudo Header
+		{
+			Uint32	buf[3];
+			buf[0] = ((tIPv4*)Conn->Interface->Address)->L;
+			buf[1] = Conn->RemoteIP.v4.L;
+			buf[2] = (htons(Length)<<16) | (6<<8) | 0;
+			checksum[0] = htons( ~IPv4_Checksum(buf, sizeof(buf)) );	// Partial checksum
+		}
+		Data->Checksum = htons( IPv4_Checksum(checksum, 2*2) );	// Combine the two
+		IPv4_SendPacket(Conn->Interface, Conn->RemoteIP.v4, IP4PROT_TCP, 0, Length, Data);
+		break;
+		
+	case 6:
+		// Append IPv6 Pseudo Header
+		{
+			Uint32	buf[4+4+1+1];
+			memcpy(buf, Conn->Interface->Address, 16);
+			memcpy(&buf[4], &Conn->RemoteIP, 16);
+			buf[8] = htonl(Length);
+			buf[9] = htonl(6);
+			checksum[0] = htons( ~IPv4_Checksum(buf, sizeof(buf)) );	// Partial checksum
+		}
+		Data->Checksum = htons( IPv4_Checksum(checksum, 2*2) );	// Combine the two
+		IPv6_SendPacket(Conn->Interface, Conn->RemoteIP.v6, IP4PROT_TCP, Length, Data);
 		break;
 	}
 }
 
 /**
- * \fn void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
  * \brief Handles a packet from the IP Layer
+ * \param Interface	Interface the packet arrived from
+ * \param Address	Pointer to the addres structure
+ * \param Length	Size of packet in bytes
+ * \param Buffer	Packet data
  */
 void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 {
@@ -114,25 +135,31 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 	tTCPListener	*srv;
 	tTCPConnection	*conn;
 
-	Log("[TCP  ] sizeof(tTCPHeader) = %i", sizeof(tTCPHeader));
-	Log("[TCP  ] SourcePort = %i", ntohs(hdr->SourcePort));
-	Log("[TCP  ] DestPort = %i", ntohs(hdr->DestPort));
-	Log("[TCP  ] SequenceNumber = 0x%x", ntohl(hdr->SequenceNumber));
-	Log("[TCP  ] AcknowlegementNumber = 0x%x", ntohl(hdr->AcknowlegementNumber));
-	Log("[TCP  ] DataOffset = %i", hdr->DataOffset >> 4);
-	Log("[TCP  ] Flags = {");
-	Log("[TCP  ]   CWR = %B", !!(hdr->Flags & TCP_FLAG_CWR));
-	Log("[TCP  ]   ECE = %B", !!(hdr->Flags & TCP_FLAG_ECE));
-	Log("[TCP  ]   URG = %B", !!(hdr->Flags & TCP_FLAG_URG));
-	Log("[TCP  ]   ACK = %B", !!(hdr->Flags & TCP_FLAG_ACK));
-	Log("[TCP  ]   PSH = %B", !!(hdr->Flags & TCP_FLAG_PSH));
-	Log("[TCP  ]   RST = %B", !!(hdr->Flags & TCP_FLAG_RST));
-	Log("[TCP  ]   SYN = %B", !!(hdr->Flags & TCP_FLAG_SYN));
-	Log("[TCP  ]   FIN = %B", !!(hdr->Flags & TCP_FLAG_FIN));
-	Log("[TCP  ] }");
-	Log("[TCP  ] WindowSize = %i", htons(hdr->WindowSize));
-	Log("[TCP  ] Checksum = 0x%x", htons(hdr->Checksum));
-	Log("[TCP  ] UrgentPointer = 0x%x", htons(hdr->UrgentPointer));
+	Log_Log("TCP", "TCP_GetPacket: <Local>:%i from [%s]:%i, Flags= %s%s%s%s%s%s%s%s",
+		ntohs(hdr->SourcePort),
+		IPStack_PrintAddress(Interface->Type, Address),
+		ntohs(hdr->DestPort),
+		(hdr->Flags & TCP_FLAG_CWR) ? "CWR " : "",
+		(hdr->Flags & TCP_FLAG_ECE) ? "ECE " : "",
+		(hdr->Flags & TCP_FLAG_URG) ? "URG " : "",
+		(hdr->Flags & TCP_FLAG_ACK) ? "ACK " : "",
+		(hdr->Flags & TCP_FLAG_PSH) ? "PSH " : "",
+		(hdr->Flags & TCP_FLAG_RST) ? "RST " : "",
+		(hdr->Flags & TCP_FLAG_SYN) ? "SYN " : "",
+		(hdr->Flags & TCP_FLAG_FIN) ? "FIN " : ""
+		);
+
+	if( Length > (hdr->DataOffset >> 4)*4 )
+	{
+		Log_Log("TCP", "TCP_GetPacket: SequenceNumber = 0x%x", ntohl(hdr->SequenceNumber));
+#if HEXDUMP_INCOMING
+		Debug_HexDump(
+			"TCP_GetPacket: Packet Data = ",
+			(Uint8*)hdr + (hdr->DataOffset >> 4)*4,
+			Length - (hdr->DataOffset >> 4)*4
+			);
+#endif
+	}
 
 	// Check Servers
 	{
@@ -143,8 +170,9 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 			// Check the interface
 			if(srv->Interface && srv->Interface != Interface)	continue;
 			// Check the destination port
-			if(srv->Port != hdr->DestPort)	continue;
-
+			if(srv->Port != htons(hdr->DestPort))	continue;
+			
+			Log_Log("TCP", "TCP_GetPacket: Matches server %p", srv);
 			// Is this in an established connection?
 			for( conn = srv->Connections; conn; conn = conn->Next )
 			{
@@ -152,32 +180,34 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 				if(conn->Interface != Interface)	continue;
 
 				// Check Source Port
-				if(conn->RemotePort != hdr->SourcePort)	continue;
+				Log_Log("TCP", "TCP_GetPacket: conn->RemotePort(%i) == hdr->SourcePort(%i)",
+					conn->RemotePort, ntohs(hdr->SourcePort));
+				if(conn->RemotePort != ntohs(hdr->SourcePort))	continue;
 
 				// Check Source IP
-				if(conn->Interface->Type == 6 && !IP6_EQU(conn->RemoteIP.v6, *(tIPv6*)Address))
-					continue;
-				if(conn->Interface->Type == 4 && !IP4_EQU(conn->RemoteIP.v4, *(tIPv4*)Address))
-					continue;
+				if( IPStack_CompareAddress(conn->Interface->Type, &conn->RemoteIP, Address, -1) != 0 )
+					continue ;
 
+				Log_Log("TCP", "TCP_GetPacket: Matches connection %p", conn);
 				// We have a response!
 				TCP_INT_HandleConnectionPacket(conn, hdr, Length);
 
 				return;
 			}
 
+			Log_Log("TCP", "TCP_GetPacket: Opening Connection");
 			// Open a new connection (well, check that it's a SYN)
 			if(hdr->Flags != TCP_FLAG_SYN) {
-				Log("[TCP  ] Packet is not a SYN");
-				continue;
+				Log_Log("TCP", "TCP_GetPacket: Packet is not a SYN");
+				return ;
 			}
 			
 			// TODO: Check for halfopen max
 			
 			conn = calloc(1, sizeof(tTCPConnection));
-			conn->State = TCP_ST_HALFOPEN;
+			conn->State = TCP_ST_SYN_RCVD;
 			conn->LocalPort = srv->Port;
-			conn->RemotePort = hdr->SourcePort;
+			conn->RemotePort = ntohs(hdr->SourcePort);
 			conn->Interface = Interface;
 			
 			switch(Interface->Type)
@@ -186,12 +216,15 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 			case 6:	conn->RemoteIP.v6 = *(tIPv6*)Address;	break;
 			}
 			
-			conn->NextSequenceRcv = ntohl( hdr->SequenceNumber );
+			conn->RecievedBuffer = RingBuffer_Create( TCP_RECIEVE_BUFFER_SIZE );
+			
+			conn->NextSequenceRcv = ntohl( hdr->SequenceNumber ) + 1;
 			conn->NextSequenceSend = rand();
 			
 			// Create node
 			conn->Node.NumACLs = 1;
 			conn->Node.ACLs = &gVFS_ACL_EveryoneRW;
+			conn->Node.ImplPtr = conn;
 			conn->Node.ImplInt = srv->NextID ++;
 			conn->Node.Read = TCP_Client_Read;
 			conn->Node.Write = TCP_Client_Write;
@@ -201,21 +234,28 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 			// as the interface is locked to the watching thread, and this
 			// runs in the watching thread. But, it's a good idea to have
 			// it, just in case
-			LOCK(&srv->lConnections);
-			conn->Next = srv->Connections;
-			srv->Connections = conn;
-			RELEASE(&srv->lConnections);
+			// Oh, wait, there is a case where a wildcard can be used
+			// (srv->Interface == NULL) so having the lock is a good idea
+			SHORTLOCK(&srv->lConnections);
+			if( !srv->Connections )
+				srv->Connections = conn;
+			else
+				srv->ConnectionsTail->Next = conn;
+			srv->ConnectionsTail = conn;
+			if(!srv->NewConnections)
+				srv->NewConnections = conn;
+			SHORTREL(&srv->lConnections);
 
 			// Send the SYN ACK
-			Log("[TCP  ] TODO: Sending SYN ACK");
 			hdr->Flags |= TCP_FLAG_ACK;
-			hdr->AcknowlegementNumber = hdr->SequenceNumber;
-			hdr->SequenceNumber = conn->NextSequenceSend;
+			hdr->AcknowlegementNumber = htonl(conn->NextSequenceRcv);
+			hdr->SequenceNumber = htonl(conn->NextSequenceSend);
 			hdr->DestPort = hdr->SourcePort;
-			hdr->SourcePort = srv->Port;
+			hdr->SourcePort = htons(srv->Port);
+			hdr->DataOffset = (sizeof(tTCPHeader)/4) << 4;
 			TCP_SendPacket( conn, sizeof(tTCPHeader), hdr );
-
-			break;
+			conn->NextSequenceSend ++;
+			return ;
 		}
 	}
 
@@ -228,7 +268,7 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 			if(conn->Interface != Interface)	continue;
 
 			// Check Source Port
-			if(conn->RemotePort != hdr->SourcePort)	continue;
+			if(conn->RemotePort != ntohs(hdr->SourcePort))	continue;
 
 			// Check Source IP
 			if(conn->Interface->Type == 6 && !IP6_EQU(conn->RemoteIP.v6, *(tIPv6*)Address))
@@ -237,66 +277,481 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 				continue;
 
 			TCP_INT_HandleConnectionPacket(conn, hdr, Length);
+			return ;
 		}
 	}
+	
+	Log_Log("TCP", "TCP_GetPacket: No Match");
 }
 
 /**
  * \brief Handles a packet sent to a specific connection
+ * \param Connection	TCP Connection pointer
+ * \param Header	TCP Packet pointer
+ * \param Length	Length of the packet
  */
 void TCP_INT_HandleConnectionPacket(tTCPConnection *Connection, tTCPHeader *Header, int Length)
-{	
-	tTCPStoredPacket	*pkt;
+{
 	 int	dataLen;
+	Uint32	sequence_num;
+	
+	// Silently drop once finished
+	// TODO: Check if this needs to be here
+	if( Connection->State == TCP_ST_FINISHED ) {
+		Log_Log("TCP", "Packet ignored - connection finnished");
+		return ;
+	}
 	
-	Connection->State = TCP_ST_OPEN;
+	// Syncronise sequence values
 	if(Header->Flags & TCP_FLAG_SYN) {
-		Connection->NextSequenceRcv = Header->SequenceNumber + 1;
+		// TODO: What if the packet also has data?
+		Connection->NextSequenceRcv = ntohl(Header->SequenceNumber);
 	}
 	
+	// Ackowledge a sent packet
 	if(Header->Flags & TCP_FLAG_ACK) {
 		// TODO: Process an ACKed Packet
-		Log("[TCP  ] Conn %p, Packet 0x%x ACKed", Connection, Header->AcknowlegementNumber);
-		return ;
+		Log_Log("TCP", "Conn %p, Sent packet 0x%x ACKed", Connection, Header->AcknowlegementNumber);
 	}
 	
-	// Allocate and fill cached packet
-	dataLen =  Length - (Header->DataOffset&0xF)*4;
-	pkt = malloc( dataLen + sizeof(tTCPStoredPacket) );
-	pkt->Next = NULL;
-	pkt->Sequence = Header->SequenceNumber;
-	memcpy(pkt->Data, (Uint8*)Header + (Header->DataOffset&0xF)*4, dataLen);
+	// Get length of data
+	dataLen = Length - (Header->DataOffset>>4)*4;
+	Log_Log("TCP", "HandleConnectionPacket - dataLen = %i", dataLen);
 	
-	if( Header->SequenceNumber != Connection->NextSequenceRcv )
+	// 
+	// State Machine
+	//
+	switch( Connection->State )
 	{
-		tTCPStoredPacket	*tmp, *prev;
-		for(tmp = Connection->FuturePackets;
-			tmp;
-			prev = tmp, tmp = tmp->Next)
+	// Pre-init connection?
+	case TCP_ST_CLOSED:
+		Log_Log("TCP", "Packets to a closed connection?!");
+		break;
+	
+	// --- Init States ---
+	// SYN sent, expecting SYN-ACK Connection Opening
+	case TCP_ST_SYN_SENT:
+		if( Header->Flags & TCP_FLAG_SYN )
+		{
+			Connection->NextSequenceRcv ++;
+			Header->DestPort = Header->SourcePort;
+			Header->SourcePort = htons(Connection->LocalPort);
+			Header->AcknowlegementNumber = htonl(Connection->NextSequenceRcv);
+			Header->SequenceNumber = htonl(Connection->NextSequenceSend);
+			Header->WindowSize = htons(TCP_WINDOW_SIZE);
+			Header->Flags = TCP_FLAG_ACK;
+			Header->DataOffset = (sizeof(tTCPHeader)/4) << 4;
+			TCP_SendPacket( Connection, sizeof(tTCPHeader), Header );
+			
+			if( Header->Flags & TCP_FLAG_ACK )
+			{	
+				Log_Log("TCP", "ACKing SYN-ACK");
+				Connection->State = TCP_ST_OPEN;
+			}
+			else
+			{
+				Log_Log("TCP", "ACKing SYN");
+				Connection->State = TCP_ST_SYN_RCVD;
+			}
+		}
+		break;
+	
+	// SYN-ACK sent, expecting ACK
+	case TCP_ST_SYN_RCVD:
+		if( Header->Flags & TCP_FLAG_ACK )
 		{
-			if(tmp->Sequence > pkt->Sequence)	break;
+			// TODO: Handle max half-open limit
+			Connection->State = TCP_ST_OPEN;
+			Log_Log("TCP", "Connection fully opened");
 		}
-		if(prev)
-			prev->Next = pkt;
+		break;
+		
+	// --- Established State ---
+	case TCP_ST_OPEN:
+		// - Handle State changes
+		//
+		if( Header->Flags & TCP_FLAG_FIN ) {
+			Log_Log("TCP", "Conn %p closed, recieved FIN", Connection);
+			VFS_MarkError(&Connection->Node, 1);
+			Connection->State = TCP_ST_CLOSE_WAIT;
+//			Header->Flags &= ~TCP_FLAG_FIN;
+			// CLOSE WAIT requires the client to close (or does it?)
+			#if 0
+			
+			#endif
+		}
+	
+		// Check for an empty packet
+		if(dataLen == 0) {
+			if( Header->Flags == TCP_FLAG_ACK )
+			{
+				Log_Log("TCP", "ACK only packet");
+				return ;
+			}
+			Connection->NextSequenceRcv ++;	// TODO: Is this right? (empty packet counts as one byte)
+			Log_Log("TCP", "Empty Packet, inc and ACK the current sequence number");
+			Header->DestPort = Header->SourcePort;
+			Header->SourcePort = htons(Connection->LocalPort);
+			Header->AcknowlegementNumber = htonl(Connection->NextSequenceRcv);
+			Header->SequenceNumber = htonl(Connection->NextSequenceSend);
+			Header->Flags |= TCP_FLAG_ACK;
+			TCP_SendPacket( Connection, sizeof(tTCPHeader), Header );
+			return ;
+		}
+		
+		// NOTES:
+		// Flags
+		//    PSH - Has Data?
+		// /NOTES
+		
+		sequence_num = ntohl(Header->SequenceNumber);
+		
+		Log_Log("TCP", "0x%08x <= 0x%08x < 0x%08x",
+			Connection->NextSequenceRcv,
+			ntohl(Header->SequenceNumber),
+			Connection->NextSequenceRcv + TCP_WINDOW_SIZE
+			);
+		
+		// Is this packet the next expected packet?
+		if( sequence_num == Connection->NextSequenceRcv )
+		{
+			 int	rv;
+			// Ooh, Goodie! Add it to the recieved list
+			rv = TCP_INT_AppendRecieved(Connection,
+				(Uint8*)Header + (Header->DataOffset>>4)*4,
+				dataLen
+				);
+			if(rv != 0) {
+				break;
+			}
+			Log_Log("TCP", "0x%08x += %i", Connection->NextSequenceRcv, dataLen);
+			Connection->NextSequenceRcv += dataLen;
+			
+			// TODO: This should be moved out of the watcher thread,
+			// so that a single lost packet on one connection doesn't cause
+			// all connections on the interface to lag.
+			// - Meh, no real issue, as the cache shouldn't be that large
+			TCP_INT_UpdateRecievedFromFuture(Connection);
+		
+			// ACK Packet
+			Header->DestPort = Header->SourcePort;
+			Header->SourcePort = htons(Connection->LocalPort);
+			Header->AcknowlegementNumber = htonl(Connection->NextSequenceRcv);
+			Header->SequenceNumber = htonl(Connection->NextSequenceSend);
+			Header->WindowSize = htons(TCP_WINDOW_SIZE);
+			Header->Flags &= TCP_FLAG_SYN;	// Eliminate all flags save for SYN
+			Header->Flags |= TCP_FLAG_ACK;	// Add ACK
+			Log_Log("TCP", "Sending ACK for 0x%08x", Connection->NextSequenceRcv);
+			TCP_SendPacket( Connection, sizeof(tTCPHeader), Header );
+			//Connection->NextSequenceSend ++;
+		}
+		// Check if the packet is in window
+		else if( WrapBetween(Connection->NextSequenceRcv, sequence_num,
+				Connection->NextSequenceRcv+TCP_WINDOW_SIZE, 0xFFFFFFFF) )
+		{
+			Uint8	*dataptr = (Uint8*)Header + (Header->DataOffset>>4)*4;
+			#if CACHE_FUTURE_PACKETS_IN_BYTES
+			Uint32	index;
+			 int	i;
+			
+			index = sequence_num % TCP_WINDOW_SIZE;
+			for( i = 0; i < dataLen; i ++ )
+			{
+				Connection->FuturePacketValidBytes[index/8] |= 1 << (index%8);
+				Connection->FuturePacketData[index] = dataptr[i];
+				// Do a wrap increment
+				index ++;
+				if(index == TCP_WINDOW_SIZE)	index = 0;
+			}
+			#else
+			tTCPStoredPacket	*pkt, *tmp, *prev = NULL;
+			
+			// Allocate and fill cached packet
+			pkt = malloc( sizeof(tTCPStoredPacket) + dataLen );
+			pkt->Next = NULL;
+			pkt->Sequence = ntohl(Header->SequenceNumber);
+			pkt->Length = dataLen;
+			memcpy(pkt->Data, dataptr, dataLen);
+			
+			Log_Log("TCP", "We missed a packet, caching",
+				pkt->Sequence, Connection->NextSequenceRcv);
+			
+			// No? Well, let's cache it and look at it later
+			SHORTLOCK( &Connection->lFuturePackets );
+			for(tmp = Connection->FuturePackets;
+				tmp;
+				prev = tmp, tmp = tmp->Next)
+			{
+				if(tmp->Sequence >= pkt->Sequence)	break;
+			}
+			
+			// Add if before first, or sequences don't match 
+			if( !tmp || tmp->Sequence != pkt->Sequence )
+			{
+				if(prev)
+					prev->Next = pkt;
+				else
+					Connection->FuturePackets = pkt;
+				pkt->Next = tmp;
+			}
+			// Replace if larger
+			else if(pkt->Length > tmp->Length)
+			{
+				if(prev)
+					prev->Next = pkt;
+				pkt->Next = tmp->Next;
+				free(tmp);
+			}
+			else
+			{
+				free(pkt);	// TODO: Find some way to remove this
+			}
+			SHORTREL( &Connection->lFuturePackets );
+			#endif
+		}
+		// Badly out of sequence packet
 		else
-			Connection->FuturePackets = pkt;
-		pkt->Next = tmp;
+		{
+			Log_Log("TCP", "Fully out of sequence packet (0x%08x not between 0x%08x and 0x%08x), dropped",
+				sequence_num, Connection->NextSequenceRcv, Connection->NextSequenceRcv+TCP_WINDOW_SIZE);
+			// TODO: Spec says we should send an empty ACK with the current state
+		}
+		break;
+	
+	// --- Remote close states
+	case TCP_ST_CLOSE_WAIT:
+		
+		// Ignore everything, CLOSE_WAIT is terminated by the client
+		Log_Debug("TCP", "CLOSE WAIT - Ignoring packets");
+		
+		break;
+	
+	// LAST-ACK - Waiting for the ACK of FIN (from CLOSE WAIT)
+	case TCP_ST_LAST_ACK:
+		if( Header->Flags & TCP_FLAG_ACK )
+		{
+			Connection->State = TCP_ST_FINISHED;	// Connection completed
+			Log_Log("TCP", "LAST-ACK to CLOSED - Connection remote closed");
+			// TODO: Destrory the TCB
+		}
+		break;
+	
+	// --- Local close States
+	case TCP_ST_FIN_WAIT1:
+		if( Header->Flags & TCP_FLAG_FIN )
+		{
+			Connection->State = TCP_ST_CLOSING;
+			Log_Debug("TCP", "Conn %p closed, sent FIN and recieved FIN", Connection);
+			VFS_MarkError(&Connection->Node, 1);
+			
+			// ACK Packet
+			Header->DestPort = Header->SourcePort;
+			Header->SourcePort = htons(Connection->LocalPort);
+			Header->AcknowlegementNumber = Header->SequenceNumber;
+			Header->SequenceNumber = htonl(Connection->NextSequenceSend);
+			Header->WindowSize = htons(TCP_WINDOW_SIZE);
+			Header->Flags = TCP_FLAG_ACK;
+			TCP_SendPacket( Connection, sizeof(tTCPHeader), Header );
+			break ;
+		}
+		
+		// TODO: Make sure that the packet is actually ACKing the FIN
+		if( Header->Flags & TCP_FLAG_ACK )
+		{
+			Connection->State = TCP_ST_FIN_WAIT2;
+			Log_Debug("TCP", "Conn %p closed, sent FIN ACKed", Connection);
+			VFS_MarkError(&Connection->Node, 1);
+			return ;
+		}
+		break;
+	
+	case TCP_ST_FIN_WAIT2:
+		if( Header->Flags & TCP_FLAG_FIN )
+		{
+			Connection->State = TCP_ST_TIME_WAIT;
+			Log_Debug("TCP", "FIN sent and recieved, ACKing and going into TIME WAIT %p FINWAIT-2 -> TIME WAIT", Connection);
+			// Send ACK
+			Header->DestPort = Header->SourcePort;
+			Header->SourcePort = htons(Connection->LocalPort);
+			Header->AcknowlegementNumber = Header->SequenceNumber;
+			Header->SequenceNumber = htonl(Connection->NextSequenceSend);
+			Header->WindowSize = htons(TCP_WINDOW_SIZE);
+			Header->Flags = TCP_FLAG_ACK;
+			TCP_SendPacket( Connection, sizeof(tTCPHeader), Header );
+		}
+		break;
+	
+	case TCP_ST_CLOSING:
+		// TODO: Make sure that the packet is actually ACKing the FIN
+		if( Header->Flags & TCP_FLAG_ACK )
+		{
+			Connection->State = TCP_ST_TIME_WAIT;
+			Log_Debug("TCP", "Conn %p CLOSING -> TIME WAIT", Connection);
+			VFS_MarkError(&Connection->Node, 1);
+			return ;
+		}
+		break;
+	
+	// --- Closed (or near closed) states) ---
+	case TCP_ST_TIME_WAIT:
+		Log_Log("TCP", "Packets on Time-Wait, ignored");
+		break;
+	
+	case TCP_ST_FINISHED:
+		Log_Log("TCP", "Packets when CLOSED, ignoring");
+		break;
+	
+	//default:
+	//	Log_Warning("TCP", "Unhandled TCP state %i", Connection->State);
+	//	break;
+	}
+	
+}
+
+/**
+ * \brief Appends a packet to the recieved list
+ * \param Connection	Connection structure
+ * \param Data	Packet contents
+ * \param Length	Length of \a Data
+ */
+int TCP_INT_AppendRecieved(tTCPConnection *Connection, const void *Data, size_t Length)
+{
+	Mutex_Acquire( &Connection->lRecievedPackets );
+
+	if(Connection->RecievedBuffer->Length + Length > Connection->RecievedBuffer->Space )
+	{
+		VFS_MarkAvaliable(&Connection->Node, 1);
+		Log_Error("TCP", "Buffer filled, packet dropped (:%i) - %i + %i > %i",
+			Connection->LocalPort, Connection->RecievedBuffer->Length, Length,
+			Connection->RecievedBuffer->Space
+			);
+		Mutex_Release( &Connection->lRecievedPackets );
+		return 1;
+	}
+	
+	RingBuffer_Write( Connection->RecievedBuffer, Data, Length );
+
+	VFS_MarkAvaliable(&Connection->Node, 1);
+	
+	Mutex_Release( &Connection->lRecievedPackets );
+	return 0;
+}
+
+/**
+ * \brief Updates the connections recieved list from the future list
+ * \param Connection	Connection structure
+ * 
+ * Updates the recieved packets list with packets from the future (out 
+ * of order) packets list that are now able to be added in direct
+ * sequence.
+ */
+void TCP_INT_UpdateRecievedFromFuture(tTCPConnection *Connection)
+{
+	#if CACHE_FUTURE_PACKETS_IN_BYTES
+	 int	i, length = 0;
+	Uint32	index;
+	
+	// Calculate length of contiguous bytes
+	length = Connection->HighestSequenceRcvd - Connection->NextSequenceRcv;
+	index = Connection->NextSequenceRcv % TCP_WINDOW_SIZE;
+	for( i = 0; i < length; i ++ )
+	{
+		if( Connection->FuturePacketValidBytes[i / 8] == 0xFF ) {
+			i += 7;	index += 7;
+			continue;
+		}
+		else if( !(Connection->FuturePacketValidBytes[i / 8] & (1 << (i%8))) )
+			break;
+		
+		index ++;
+		if(index > TCP_WINDOW_SIZE)
+			index -= TCP_WINDOW_SIZE;
+	}
+	length = i;
+	
+	index = Connection->NextSequenceRcv % TCP_WINDOW_SIZE;
+	
+	// Write data to to the ring buffer
+	if( TCP_WINDOW_SIZE - index > length )
+	{
+		// Simple case
+		RingBuffer_Write( Connection->RecievedBuffer, Connection->FuturePacketData + index, length );
 	}
 	else
 	{
-		LOCK( &Connection->lRecievedPackets );
-		if(Connection->RecievedPackets)
+		 int	endLen = TCP_WINDOW_SIZE - index;
+		// 2-part case
+		RingBuffer_Write( Connection->RecievedBuffer, Connection->FuturePacketData + index, endLen );
+		RingBuffer_Write( Connection->RecievedBuffer, Connection->FuturePacketData, endLen - length );
+	}
+	
+	// Mark (now saved) bytes as invalid
+	// - Align index
+	while(index % 8 && length)
+	{
+		Connection->FuturePacketData[index] = 0;
+		Connection->FuturePacketData[index/8] &= ~(1 << (index%8));
+		index ++;
+		if(index > TCP_WINDOW_SIZE)
+			index -= TCP_WINDOW_SIZE;
+		length --;
+	}
+	while( length > 7 )
+	{
+		Connection->FuturePacketData[index] = 0;
+		Connection->FuturePacketValidBytes[index/8] = 0;
+		length -= 8;
+		index += 8;
+		if(index > TCP_WINDOW_SIZE)
+			index -= TCP_WINDOW_SIZE;
+	}
+	while(length)
+	{
+		Connection->FuturePacketData[index] = 0;
+		Connection->FuturePacketData[index/8] &= ~(1 << (index%8));
+		index ++;
+		if(index > TCP_WINDOW_SIZE)
+			index -= TCP_WINDOW_SIZE;
+		length --;
+	}
+	
+	#else
+	tTCPStoredPacket	*pkt;
+	for(;;)
+	{
+		SHORTLOCK( &Connection->lFuturePackets );
+		
+		// Clear out duplicates from cache
+		// - If a packet has just been recieved, and it is expected, then
+		//   (since NextSequenceRcv = rcvd->Sequence + rcvd->Length) all
+		//   packets in cache that are smaller than the next expected
+		//   are now defunct.
+		pkt = Connection->FuturePackets;
+		while(pkt && pkt->Sequence < Connection->NextSequenceRcv)
 		{
-			Connection->RecievedPacketsTail->Next = pkt;
-			Connection->RecievedPacketsTail = pkt;
+			tTCPStoredPacket	*next = pkt->Next;
+			free(pkt);
+			pkt = next;
 		}
-		else
-		{
-			Connection->RecievedPackets = pkt;
-			Connection->RecievedPacketsTail = pkt;
+		
+		// If there's no packets left in cache, stop looking
+		if(!pkt || pkt->Sequence > Connection->NextSequenceRcv) {
+			SHORTREL( &Connection->lFuturePackets );
+			return;
 		}
-		RELEASE( &Connection->lRecievedPackets );
+		
+		// Delete packet from future list
+		Connection->FuturePackets = pkt->Next;
+		
+		// Release list
+		SHORTREL( &Connection->lFuturePackets );
+		
+		// Looks like we found one
+		TCP_INT_AppendRecieved(Connection, pkt);
+		Connection->NextSequenceRcv += pkt->Length;
+		free(pkt);
 	}
+	#endif
 }
 
 /**
@@ -359,13 +814,24 @@ int TCP_DeallocatePort(Uint16 Port)
 // --- Server
 tVFS_Node *TCP_Server_Init(tInterface *Interface)
 {
-	tTCPListener	*srv = malloc( sizeof(tTCPListener) );
+	tTCPListener	*srv;
+	
+	srv = malloc( sizeof(tTCPListener) );
+
+	if( srv == NULL ) {
+		Log_Warning("TCP", "malloc failed for listener (%i) bytes", sizeof(tTCPListener));
+		return NULL;
+	}
 
 	srv->Interface = Interface;
 	srv->Port = 0;
 	srv->NextID = 0;
 	srv->Connections = NULL;
+	srv->ConnectionsTail = NULL;
+	srv->NewConnections = NULL;
 	srv->Next = NULL;
+	srv->Node.Flags = VFS_FFLAG_DIRECTORY;
+	srv->Node.Size = -1;
 	srv->Node.ImplPtr = srv;
 	srv->Node.NumACLs = 1;
 	srv->Node.ACLs = &gVFS_ACL_EveryoneRW;
@@ -374,10 +840,10 @@ tVFS_Node *TCP_Server_Init(tInterface *Interface)
 	srv->Node.IOCtl = TCP_Server_IOCtl;
 	srv->Node.Close = TCP_Server_Close;
 
-	LOCK(&glTCP_Listeners);
+	SHORTLOCK(&glTCP_Listeners);
 	srv->Next = gTCP_Listeners;
 	gTCP_Listeners = srv;
-	RELEASE(&glTCP_Listeners);
+	SHORTREL(&glTCP_Listeners);
 
 	return &srv->Node;
 }
@@ -393,14 +859,36 @@ char *TCP_Server_ReadDir(tVFS_Node *Node, int Pos)
 	tTCPListener	*srv = Node->ImplPtr;
 	tTCPConnection	*conn;
 	char	*ret;
+	
+	ENTER("pNode iPos", Node, Pos);
 
-	while( srv->NewConnections == NULL )	Threads_Yield();
+	Log_Log("TCP", "Thread %i waiting for a connection", Threads_GetTID());
+	for(;;)
+	{
+		SHORTLOCK( &srv->lConnections );
+		if( srv->NewConnections != NULL )	break;
+		SHORTREL( &srv->lConnections );
+		Threads_Yield();	// TODO: Sleep until poked
+		continue;
+	}
+	
 
+	// Increment the new list (the current connection is still on the 
+	// normal list)
 	conn = srv->NewConnections;
 	srv->NewConnections = conn->Next;
+	
+	SHORTREL( &srv->lConnections );
+	
+	LOG("conn = %p", conn);
+	LOG("srv->Connections = %p", srv->Connections);
+	LOG("srv->NewConnections = %p", srv->NewConnections);
+	LOG("srv->ConnectionsTail = %p", srv->ConnectionsTail);
 
 	ret = malloc(9);
-	itoa(ret, Node->ImplInt, 16, '0', 8);
+	itoa(ret, conn->Node.ImplInt, 16, 8, '0');
+	Log_Log("TCP", "Thread %i got '%s'", Threads_GetTID(), ret);
+	LEAVE('s', ret);
 	return ret;
 }
 
@@ -409,11 +897,53 @@ char *TCP_Server_ReadDir(tVFS_Node *Node, int Pos)
  * \param Node	Server node
  * \param Name	Hexadecimal ID of the node
  */
-tVFS_Node *TCP_Server_FindDir(tVFS_Node *Node, char *Name)
+tVFS_Node *TCP_Server_FindDir(tVFS_Node *Node, const char *Name)
 {
-	return NULL;
+	tTCPConnection	*conn;
+	tTCPListener	*srv = Node->ImplPtr;
+	char	tmp[9];
+	 int	id = atoi(Name);
+	
+	ENTER("pNode sName", Node, Name);
+	
+	// Sanity Check
+	itoa(tmp, id, 16, 8, '0');
+	if(strcmp(tmp, Name) != 0) {
+		LOG("'%s' != '%s' (%08x)", Name, tmp, id);
+		LEAVE('n');
+		return NULL;
+	}
+	
+	Log_Debug("TCP", "srv->Connections = %p", srv->Connections);
+	Log_Debug("TCP", "srv->NewConnections = %p", srv->NewConnections);
+	Log_Debug("TCP", "srv->ConnectionsTail = %p", srv->ConnectionsTail);
+	
+	// Search
+	SHORTLOCK( &srv->lConnections );
+	for(conn = srv->Connections;
+		conn;
+		conn = conn->Next)
+	{
+		LOG("conn->Node.ImplInt = %i", conn->Node.ImplInt);
+		if(conn->Node.ImplInt == id)	break;
+	}
+	SHORTREL( &srv->lConnections );
+	
+	// If not found, ret NULL
+	if(!conn) {
+		LOG("Connection %i not found", id);
+		LEAVE('n');
+		return NULL;
+	}
+	
+	// Return node
+	LEAVE('p', &conn->Node);
+	return &conn->Node;
 }
 
+/**
+ * \brief Handle IOCtl calls
+ */
 int TCP_Server_IOCtl(tVFS_Node *Node, int ID, void *Data)
 {
 	tTCPListener	*srv = Node->ImplPtr;
@@ -444,6 +974,9 @@ int TCP_Server_IOCtl(tVFS_Node *Node, int ID, void *Data)
 			srv->Port = TCP_GetUnusedPort();
 		else	// Else, mark this as used
 			TCP_AllocatePort(srv->Port);
+		
+		Log_Log("TCP", "Server %p listening on port %i", srv, srv->Port);
+		
 		return srv->Port;
 	}
 	return 0;
@@ -455,15 +988,17 @@ void TCP_Server_Close(tVFS_Node *Node)
 }
 
 // --- Client
+/**
+ * \brief Create a client node
+ */
 tVFS_Node *TCP_Client_Init(tInterface *Interface)
 {
-	tTCPConnection	*conn = malloc( sizeof(tTCPConnection) );
+	tTCPConnection	*conn = calloc( sizeof(tTCPConnection) + TCP_WINDOW_SIZE + TCP_WINDOW_SIZE/8, 1 );
 
 	conn->State = TCP_ST_CLOSED;
 	conn->Interface = Interface;
-	conn->LocalPort = 0;
-	conn->RemotePort = 0;
-	memset( &conn->RemoteIP, 0, sizeof(conn->RemoteIP) );
+	conn->LocalPort = -1;
+	conn->RemotePort = -1;
 
 	conn->Node.ImplPtr = conn;
 	conn->Node.NumACLs = 1;
@@ -473,22 +1008,183 @@ tVFS_Node *TCP_Client_Init(tInterface *Interface)
 	conn->Node.IOCtl = TCP_Client_IOCtl;
 	conn->Node.Close = TCP_Client_Close;
 
-	LOCK(&glTCP_OutbountCons);
+	conn->RecievedBuffer = RingBuffer_Create( TCP_RECIEVE_BUFFER_SIZE );
+	#if 0
+	conn->SentBuffer = RingBuffer_Create( TCP_SEND_BUFFER_SIZE );
+	Semaphore_Init(conn->SentBufferSpace, 0, TCP_SEND_BUFFER_SIZE, "TCP SentBuffer", conn->Name);
+	#endif
+	
+	#if CACHE_FUTURE_PACKETS_IN_BYTES
+	// Future recieved data (ahead of the expected sequence number)
+	conn->FuturePacketData = (Uint8*)conn + sizeof(tTCPConnection);
+	conn->FuturePacketValidBytes = conn->FuturePacketData + TCP_WINDOW_SIZE;
+	#endif
+
+	SHORTLOCK(&glTCP_OutbountCons);
 	conn->Next = gTCP_OutbountCons;
 	gTCP_OutbountCons = conn;
-	RELEASE(&glTCP_OutbountCons);
+	SHORTREL(&glTCP_OutbountCons);
 
 	return &conn->Node;
 }
 
+/**
+ * \brief Wait for a packet and return it
+ * \note If \a Length is smaller than the size of the packet, the rest
+ *       of the packet's data will be discarded.
+ */
 Uint64 TCP_Client_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 {
-	return 0;
+	tTCPConnection	*conn = Node->ImplPtr;
+	size_t	len;
+	
+	ENTER("pNode XOffset XLength pBuffer", Node, Offset, Length, Buffer);
+	LOG("conn = %p {State:%i}", conn, conn->State);
+	
+	// Check if connection is estabilishing
+	// - TODO: Sleep instead (maybe using VFS_SelectNode to wait for the
+	//   data to be availiable
+	while( conn->State == TCP_ST_SYN_RCVD || conn->State == TCP_ST_SYN_SENT )
+		Threads_Yield();
+	
+	// If the conneciton is not open, then clean out the recieved buffer
+	if( conn->State != TCP_ST_OPEN )
+	{
+		Mutex_Acquire( &conn->lRecievedPackets );
+		len = RingBuffer_Read( Buffer, conn->RecievedBuffer, Length );
+		Mutex_Release( &conn->lRecievedPackets );
+		
+		if( len == 0 ) {
+			VFS_MarkAvaliable(Node, 0);
+			LEAVE('i', -1);
+			return -1;
+		}
+		
+		LEAVE('i', len);
+		return len;
+	}
+	
+	// Wait
+	VFS_SelectNode(Node, VFS_SELECT_READ|VFS_SELECT_ERROR, NULL, "TCP_Client_Read");
+	
+	// Lock list and read as much as possible (up to `Length`)
+	Mutex_Acquire( &conn->lRecievedPackets );
+	len = RingBuffer_Read( Buffer, conn->RecievedBuffer, Length );
+	
+	if( len == 0 || conn->RecievedBuffer->Length == 0 ) {
+		LOG("Marking as none avaliable (len = %i)", len);
+		VFS_MarkAvaliable(Node, 0);
+	}
+		
+	// Release the lock (we don't need it any more)
+	Mutex_Release( &conn->lRecievedPackets );
+
+	LEAVE('i', len);
+	return len;
 }
 
+/**
+ * \brief Send a data packet on a connection
+ */
+void TCP_INT_SendDataPacket(tTCPConnection *Connection, size_t Length, void *Data)
+{
+	char	buf[sizeof(tTCPHeader)+Length];
+	tTCPHeader	*packet = (void*)buf;
+	
+	packet->SourcePort = htons(Connection->LocalPort);
+	packet->DestPort = htons(Connection->RemotePort);
+	packet->DataOffset = (sizeof(tTCPHeader)/4)*16;
+	packet->WindowSize = htons(TCP_WINDOW_SIZE);
+	
+	packet->AcknowlegementNumber = htonl(Connection->NextSequenceRcv);
+	packet->SequenceNumber = htonl(Connection->NextSequenceSend);
+	packet->Flags = TCP_FLAG_PSH|TCP_FLAG_ACK;	// Hey, ACK if you can!
+	
+	memcpy(packet->Options, Data, Length);
+	
+	Log_Debug("TCP", "Send sequence 0x%08x", Connection->NextSequenceSend);
+#if HEXDUMP_OUTGOING
+	Debug_HexDump("TCP_INT_SendDataPacket: Data = ", Data, Length);
+#endif
+	
+	TCP_SendPacket( Connection, sizeof(tTCPHeader)+Length, packet );
+	
+	Connection->NextSequenceSend += Length;
+}
+
+/**
+ * \brief Send some bytes on a connection
+ */
 Uint64 TCP_Client_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 {
-	return 0;
+	tTCPConnection	*conn = Node->ImplPtr;
+	size_t	rem = Length;
+	
+	ENTER("pNode XOffset XLength pBuffer", Node, Offset, Length, Buffer);
+	
+//	#if DEBUG
+//	Debug_HexDump("TCP_Client_Write: Buffer = ",
+//		Buffer, Length);
+//	#endif
+	
+	// Check if connection is open
+	while( conn->State == TCP_ST_SYN_RCVD || conn->State == TCP_ST_SYN_SENT )
+		Threads_Yield();
+	
+	if( conn->State != TCP_ST_OPEN ) {
+		VFS_MarkError(Node, 1);
+		LEAVE('i', -1);
+		return -1;
+	}
+	
+	do
+	{
+		 int	len = (rem < TCP_MAX_PACKET_SIZE) ? rem : TCP_MAX_PACKET_SIZE;
+		
+		#if 0
+		// Wait for space in the buffer
+		Semaphore_Signal( &Connection->SentBufferSpace, len );
+		
+		// Save data to buffer (and update the length read by the ammount written)
+		len = RingBuffer_Write( &Connection->SentBuffer, Buffer, len);
+		#endif
+		
+		// Send packet
+		TCP_INT_SendDataPacket(conn, len, Buffer);
+		
+		Buffer += len;
+		rem -= len;
+	} while( rem > 0 );
+	
+	LEAVE('i', Length);
+	return Length;
+}
+
+/**
+ * \brief Open a connection to another host using TCP
+ * \param Conn	Connection structure
+ */
+void TCP_StartConnection(tTCPConnection *Conn)
+{
+	tTCPHeader	hdr = {0};
+
+	Conn->State = TCP_ST_SYN_SENT;
+
+	hdr.SourcePort = htons(Conn->LocalPort);
+	hdr.DestPort = htons(Conn->RemotePort);
+	Conn->NextSequenceSend = rand();
+	hdr.SequenceNumber = htonl(Conn->NextSequenceSend);
+	hdr.DataOffset = (sizeof(tTCPHeader)/4) << 4;
+	hdr.Flags = TCP_FLAG_SYN;
+	hdr.WindowSize = htons(TCP_WINDOW_SIZE);	// Max
+	hdr.Checksum = 0;	// TODO
+	
+	TCP_SendPacket( Conn, sizeof(tTCPHeader), &hdr );
+	
+	Conn->NextSequenceSend ++;
+	Conn->State = TCP_ST_SYN_SENT;
+
+	return ;
 }
 
 /**
@@ -497,53 +1193,70 @@ Uint64 TCP_Client_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buf
 int TCP_Client_IOCtl(tVFS_Node *Node, int ID, void *Data)
 {
 	tTCPConnection	*conn = Node->ImplPtr;
+	
+	ENTER("pNode iID pData", Node, ID, Data);
 
 	switch(ID)
 	{
 	case 4:	// Get/Set local port
 		if(!Data)
-			return conn->LocalPort;
+			LEAVE_RET('i', conn->LocalPort);
 		if(conn->State != TCP_ST_CLOSED)
-			return -1;
+			LEAVE_RET('i', -1);
 		if(!CheckMem(Data, sizeof(Uint16)))
-			return -1;
+			LEAVE_RET('i', -1);
 
 		if(Threads_GetUID() != 0 && *(Uint16*)Data < 1024)
-			return -1;
+			LEAVE_RET('i', -1);
 
 		conn->LocalPort = *(Uint16*)Data;
-		return 0;
+		LEAVE_RET('i', conn->LocalPort);
 
 	case 5:	// Get/Set remote port
-		if(!Data)	return conn->RemotePort;
-		if(conn->State != TCP_ST_CLOSED)	return -1;
-		if(!CheckMem(Data, sizeof(Uint16)))	return -1;
+		if(!Data)	LEAVE_RET('i', conn->RemotePort);
+		if(conn->State != TCP_ST_CLOSED)	LEAVE_RET('i', -1);
+		if(!CheckMem(Data, sizeof(Uint16)))	LEAVE_RET('i', -1);
 		conn->RemotePort = *(Uint16*)Data;
-		return 0;
+		LEAVE_RET('i', conn->RemotePort);
 
 	case 6:	// Set Remote IP
 		if( conn->State != TCP_ST_CLOSED )
-			return -1;
+			LEAVE_RET('i', -1);
 		if( conn->Interface->Type == 4 )
 		{
-			if(!CheckMem(Data, sizeof(tIPv4)))	return -1;
+			if(!CheckMem(Data, sizeof(tIPv4)))	LEAVE_RET('i', -1);
 			conn->RemoteIP.v4 = *(tIPv4*)Data;
 		}
 		else if( conn->Interface->Type == 6 )
 		{
-			if(!CheckMem(Data, sizeof(tIPv6)))	return -1;
+			if(!CheckMem(Data, sizeof(tIPv6)))	LEAVE_RET('i', -1);
 			conn->RemoteIP.v6 = *(tIPv6*)Data;
 		}
-		return 0;
+		LEAVE_RET('i', 0);
 
 	case 7:	// Connect
-		if(conn->LocalPort == -1)
+		if(conn->LocalPort == 0xFFFF)
 			conn->LocalPort = TCP_GetUnusedPort();
 		if(conn->RemotePort == -1)
-			return 0;
+			LEAVE_RET('i', 0);
 
-		TCP_StartConnection(conn);
-		return 1;
+		{
+			tTime	timeout_end = now() + conn->Interface->TimeoutDelay;
+	
+			TCP_StartConnection(conn);
+			// TODO: Wait for connection to open
+			while( conn->State == TCP_ST_SYN_SENT && timeout_end > now() ) {
+				Threads_Yield();
+			}
+			if( conn->State == TCP_ST_SYN_SENT )
+				LEAVE_RET('i', 0);
+		}
+
+		LEAVE_RET('i', 1);
+	
+	// Get recieve buffer length
+	case 8:
+		LEAVE_RET('i', conn->RecievedBuffer->Length);
 	}
 
 	return 0;
@@ -551,5 +1264,78 @@ int TCP_Client_IOCtl(tVFS_Node *Node, int ID, void *Data)
 
 void TCP_Client_Close(tVFS_Node *Node)
 {
-	free(Node->ImplPtr);
+	tTCPConnection	*conn = Node->ImplPtr;
+	tTCPHeader	packet;
+	
+	ENTER("pNode", Node);
+	
+	if( conn->State == TCP_ST_CLOSE_WAIT || conn->State == TCP_ST_OPEN )
+	{
+		packet.SourcePort = htons(conn->LocalPort);
+		packet.DestPort = htons(conn->RemotePort);
+		packet.DataOffset = (sizeof(tTCPHeader)/4)*16;
+		packet.WindowSize = TCP_WINDOW_SIZE;
+		
+		packet.AcknowlegementNumber = 0;
+		packet.SequenceNumber = htonl(conn->NextSequenceSend);
+		packet.Flags = TCP_FLAG_FIN;
+		
+		TCP_SendPacket( conn, sizeof(tTCPHeader), &packet );
+	}
+	
+	switch( conn->State )
+	{
+	case TCP_ST_CLOSE_WAIT:
+		conn->State = TCP_ST_LAST_ACK;
+		break;
+	case TCP_ST_OPEN:
+		conn->State = TCP_ST_FIN_WAIT1;
+		while( conn->State == TCP_ST_FIN_WAIT1 )	Threads_Yield();
+		break;
+	default:
+		Log_Warning("TCP", "Unhandled connection state in TCP_Client_Close");
+		break;
+	}
+	
+	free(conn);
+	
+	LEAVE('-');
+}
+
+/**
+ * \brief Checks if a value is between two others (after taking into account wrapping)
+ */
+int WrapBetween(Uint32 Lower, Uint32 Value, Uint32 Higher, Uint32 MaxValue)
+{
+	if( MaxValue < 0xFFFFFFFF )
+	{
+		Lower %= MaxValue + 1;
+		Value %= MaxValue + 1;
+		Higher %= MaxValue + 1;
+	}
+	
+	// Simple Case, no wrap ?
+	//       Lower Value Higher
+	// | ... + ... + ... + ... |
+
+	if( Lower < Higher ) {
+		return Lower < Value && Value < Higher;
+	}
+	// Higher has wrapped below lower
+	
+	// Value > Lower ?
+	//       Higher Lower Value
+	// | ... +  ... + ... + ... |
+	if( Value > Lower ) {
+		return 1;
+	}
+	
+	// Value < Higher ?
+	//       Value Higher Lower
+	// | ... + ... +  ... + ... |
+	if( Value < Higher ) {
+		return 1;
+	}
+	
+	return 0;
 }