X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2FIPStack%2Ftcp.c;h=cedb8ce7176d7575786ffbcb8bb420e4854296ad;hb=b1873b4cff47aae8ada8cc303ea01b475cc7ccc8;hp=87838f8cdbd47d9372ed137257e333a4435cece3;hpb=084ec29bd3b93d4974378e29b701b61b2c884cec;p=tpg%2Facess2.git

diff --git a/Modules/IPStack/tcp.c b/Modules/IPStack/tcp.c
index 87838f8c..cedb8ce7 100644
--- a/Modules/IPStack/tcp.c
+++ b/Modules/IPStack/tcp.c
@@ -8,6 +8,7 @@
 #include "tcp.h"
 
 #define USE_SELECT	1
+#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
@@ -39,6 +40,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};
@@ -112,21 +115,11 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 	Log_Log("TCP", "TCP_GetPacket: SequenceNumber = 0x%x", ntohl(hdr->SequenceNumber));
 	Log_Log("TCP", "TCP_GetPacket: AcknowlegementNumber = 0x%x", ntohl(hdr->AcknowlegementNumber));
 	Log_Log("TCP", "TCP_GetPacket: DataOffset = %i", hdr->DataOffset >> 4);
-	Log_Log("TCP", "TCP_GetPacket: Flags = {");
-	Log_Log("TCP", "TCP_GetPacket:   CWR = %B, ECE = %B",
-		!!(hdr->Flags & TCP_FLAG_CWR), !!(hdr->Flags & TCP_FLAG_ECE));
-	Log_Log("TCP", "TCP_GetPacket:   URG = %B, ACK = %B",
-		!!(hdr->Flags & TCP_FLAG_URG), !!(hdr->Flags & TCP_FLAG_ACK));
-	Log_Log("TCP", "TCP_GetPacket:   PSH = %B, RST = %B",
-		!!(hdr->Flags & TCP_FLAG_PSH), !!(hdr->Flags & TCP_FLAG_RST));
-	Log_Log("TCP", "TCP_GetPacket:   SYN = %B, FIN = %B",
-		!!(hdr->Flags & TCP_FLAG_SYN), !!(hdr->Flags & TCP_FLAG_FIN));
-	Log_Log("TCP", "TCP_GetPacket: }");
 	Log_Log("TCP", "TCP_GetPacket: WindowSize = %i", htons(hdr->WindowSize));
 	Log_Log("TCP", "TCP_GetPacket: Checksum = 0x%x", htons(hdr->Checksum));
 	Log_Log("TCP", "TCP_GetPacket: UrgentPointer = 0x%x", htons(hdr->UrgentPointer));
 */
-	Log_Log("TCP", "TCP_GetPacket: Flags = %s%s%s%s%s%s",
+	Log_Log("TCP", "TCP_GetPacket: Flags = %s%s%s%s%s%s%s%s",
 		(hdr->Flags & TCP_FLAG_CWR) ? "CWR " : "",
 		(hdr->Flags & TCP_FLAG_ECE) ? "ECE " : "",
 		(hdr->Flags & TCP_FLAG_URG) ? "URG " : "",
@@ -139,6 +132,7 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 
 	if( Length > (hdr->DataOffset >> 4)*4 )
 	{
+		Log_Log("TCP", "TCP_GetPacket: SequenceNumber = 0x%x", ntohl(hdr->SequenceNumber));
 		Debug_HexDump(
 			"[TCP  ] Packet Data = ",
 			(Uint8*)hdr + (hdr->DataOffset >> 4)*4,
@@ -194,7 +188,7 @@ void TCP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 			// 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 = ntohs(hdr->SourcePort);
 			conn->Interface = Interface;
@@ -284,16 +278,45 @@ void TCP_INT_HandleConnectionPacket(tTCPConnection *Connection, tTCPHeader *Head
 	tTCPStoredPacket	*pkt;
 	 int	dataLen;
 	
+	// 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 ;
+	}
+	
 	// Syncronise sequence values
 	if(Header->Flags & TCP_FLAG_SYN) {
-		Connection->NextSequenceRcv = ntohl(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_Log("TCP", "Conn %p, Sent packet 0x%x ACKed", Connection, Header->AcknowlegementNumber);
 	}
 	
-	// Handle a server replying to our initial SYN
-	if( Connection->State == TCP_ST_SYN_SENT )
+	// Get length of data
+	dataLen = Length - (Header->DataOffset>>4)*4;
+	Log_Log("TCP", "HandleConnectionPacket - dataLen = %i", dataLen);
+	
+	// 
+	// State Machine
+	//
+	switch( Connection->State )
 	{
-		if( (Header->Flags & (TCP_FLAG_SYN|TCP_FLAG_ACK)) == (TCP_FLAG_SYN|TCP_FLAG_ACK) )
-		{	
+	// Pre-init conneciton?
+	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);
@@ -301,120 +324,261 @@ void TCP_INT_HandleConnectionPacket(tTCPConnection *Connection, tTCPHeader *Head
 			Header->WindowSize = htons(TCP_WINDOW_SIZE);
 			Header->Flags = TCP_FLAG_ACK;
 			Header->DataOffset = (sizeof(tTCPHeader)/4) << 4;
-			Log_Log("TCP", "ACKing SYN-ACK");
 			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 )
+		{
+			// TODO: Handle max half-open limit
 			Connection->State = TCP_ST_OPEN;
+			Log_Log("TCP", "Connection fully opened");
+		}
+		break;
+		
+	// --- Established State ---
+	case TCP_ST_OPEN:
+		// - Handle State changes
+		//
+		if( Header->Flags & TCP_FLAG_FIN ) {
+			Log_Log("TCP", "Conn %p closed, recieved FIN, acknowledging", Connection);
+			VFS_MarkError(&Connection->Node, 1);
+			Connection->State = TCP_ST_CLOSE_WAIT;
+			// CLOSE WAIT requires the client to close (or does it?)
+			#if 0
+			
+			#endif
 		}
-	}
 	
-	// Handle a client confirming the connection
-	if( Connection->State == TCP_ST_HALFOPEN && (Header->Flags & TCP_FLAG_ACK) )
-	{
-		Connection->State = TCP_ST_OPEN;
-		Log_Log("TCP", "Connection fully opened");
-	}
+		// 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
+		
+		// Allocate and fill cached packet
+		pkt = malloc( sizeof(tTCPStoredPacket) + dataLen );
+		pkt->Next = NULL;
+		pkt->Sequence = ntohl(Header->SequenceNumber);
+		pkt->Length = dataLen;
+		memcpy(pkt->Data, (Uint8*)Header + (Header->DataOffset>>4)*4, dataLen);
+		
+		Log_Log("TCP", "0x%08x <= 0x%08x < 0x%08x",
+			Connection->NextSequenceRcv,
+			pkt->Sequence,
+			Connection->NextSequenceRcv + TCP_WINDOW_SIZE
+			);
+		
+		// Is this packet the next expected packet?
+		if( pkt->Sequence == Connection->NextSequenceRcv )
+		{
+			// Ooh, Goodie! Add it to the recieved list
+			TCP_INT_AppendRecieved(Connection, pkt);
+			free(pkt);
+			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.
+			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, pkt->Sequence,
+				Connection->NextSequenceRcv+TCP_WINDOW_SIZE, 0xFFFFFFFF) )
+		{
+			#if CACHE_FUTURE_PACKETS_IN_BYTES
+			Uint32	index;
+			 int	i;
+			
+			index = pkt->Sequence % TCP_WINDOW_SIZE;
+			for( i = 0; i < pkt->Length; i ++ )
+			{
+				Connection->FuturePacketValidBytes[index/8] |= 1 << (index%8);
+				Connection->FuturePacketValidBytes[index] = pkt->Data[i];
+				// Do a wrap increment
+				index ++;
+				if(index == TCP_WINDOW_SIZE)	index = 0;
+			}
+			#else
+			tTCPStoredPacket	*tmp, *prev = NULL;
+			
+			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);
+			}
+			SHORTREL( &Connection->lFuturePackets );
+			#endif
+		}
+		// Badly out of sequence packet
+		else
+		{
+			Log_Log("TCP", "Fully out of sequence packet (0x%08x not between 0x%08x and 0x%08x), dropped",
+				pkt->Sequence, Connection->NextSequenceRcv, Connection->NextSequenceRcv+TCP_WINDOW_SIZE);
+			free(pkt);
+			// TODO: Spec says we should send an empty ACK with the current state
+		}
+		break;
 	
-	// Get length of data
-	dataLen = Length - (Header->DataOffset>>4)*4;
-	Log_Log("TCP", "HandleConnectionPacket - dataLen = %i", dataLen);
+	// --- 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;
 	
-	if(Header->Flags & TCP_FLAG_ACK) {
-		// TODO: Process an ACKed Packet
-		Log_Log("TCP", "Conn %p, Packet 0x%x ACKed", Connection, Header->AcknowlegementNumber);
+	// 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 = htonl(Connection->NextSequenceRcv);
+			Header->SequenceNumber = htonl(Connection->NextSequenceSend);
+			Header->WindowSize = htons(TCP_WINDOW_SIZE);
+			Header->Flags = TCP_FLAG_ACK;
+			TCP_SendPacket( Connection, sizeof(tTCPHeader), Header );
+			break ;
+		}
 		
-		// HACK
 		// TODO: Make sure that the packet is actually ACKing the FIN
-		if( Connection->State == TCP_ST_FIN_SENT ) {
-			Connection->State = TCP_ST_FINISHED;
+		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;
 	
-	// TODO: Check what to do here
-	if(Header->Flags & TCP_FLAG_FIN) {
-		if( Connection->State == TCP_ST_FIN_SENT ) {
-			Connection->State = TCP_ST_FINISHED;
-			return ;
-		}
-		else {
-			Connection->State = TCP_ST_FINISHED;
+	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 = htonl(Connection->NextSequenceRcv);
 			Header->SequenceNumber = htonl(Connection->NextSequenceSend);
+			Header->WindowSize = htons(TCP_WINDOW_SIZE);
 			Header->Flags = TCP_FLAG_ACK;
 			TCP_SendPacket( Connection, sizeof(tTCPHeader), Header );
-			return ;
 		}
-	}
+		break;
 	
-	// Check for an empty packet
-	if(dataLen == 0) {
-		Log_Log("TCP", "Empty Packet");
-		return ;
-	}
+	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;
 	
-	// NOTES:
-	// Flags
-	//    PSH - Has Data?
-	// /NOTES
+	// --- Closed (or near closed) states) ---
+	case TCP_ST_TIME_WAIT:
+		Log_Log("TCP", "Packets on Time-Wait, ignored");
+		break;
 	
-	// Allocate and fill cached packet
-	pkt = malloc( dataLen + sizeof(tTCPStoredPacket) );
-	pkt->Next = NULL;
-	pkt->Sequence = ntohl(Header->SequenceNumber);
-	pkt->Length = dataLen;
-	memcpy(pkt->Data, (Uint8*)Header + (Header->DataOffset>>4)*4, dataLen);
+	case TCP_ST_FINISHED:
+		Log_Log("TCP", "Packets when CLOSED, ignoring");
+		break;
 	
-	// Is this packet the next expected packet?
-	// TODO: Fix this to check if the packet is in the window.
-	if( pkt->Sequence != Connection->NextSequenceRcv )
-	{
-		tTCPStoredPacket	*tmp, *prev = NULL;
-		
-		Log_Log("TCP", "Out of sequence packet (0x%08x != 0x%08x)",
-			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;
-		}
-		if(prev)
-			prev->Next = pkt;
-		else
-			Connection->FuturePackets = pkt;
-		pkt->Next = tmp;
-		SHORTREL( &Connection->lFuturePackets );
-	}
-	else
-	{
-		// Ooh, Goodie! Add it to the recieved list
-		TCP_INT_AppendRecieved(Connection, pkt);
-		free(pkt);
-		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.
-		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 ++;
+	//default:
+	//	Log_Warning("TCP", "Unhandled TCP state %i", Connection->State);
+	//	break;
 	}
+	
 }
 
 /**
@@ -453,27 +617,101 @@ void TCP_INT_AppendRecieved(tTCPConnection *Connection, tTCPStoredPacket *Pkt)
  */
 void TCP_INT_UpdateRecievedFromFuture(tTCPConnection *Connection)
 {
-	tTCPStoredPacket	*pkt, *prev;
+	#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
+	{
+		 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(;;)
 	{
-		prev = NULL;
-		// Look for the next expected packet in the cache.
 		SHORTLOCK( &Connection->lFuturePackets );
-		for(pkt = Connection->FuturePackets;
-			pkt && pkt->Sequence < Connection->NextSequenceRcv;
-			prev = pkt, pkt = pkt->Next);
 		
-		// If we can't find the expected next packet, stop looking
+		// 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)
+		{
+			tTCPStoredPacket	*next = pkt->Next;
+			free(pkt);
+			pkt = next;
+		}
+		
+		// If there's no packets left in cache, stop looking
 		if(!pkt || pkt->Sequence > Connection->NextSequenceRcv) {
 			SHORTREL( &Connection->lFuturePackets );
 			return;
 		}
 		
 		// Delete packet from future list
-		if(prev)
-			prev->Next = pkt->Next;
-		else
-			Connection->FuturePackets = pkt->Next;
+		Connection->FuturePackets = pkt->Next;
 		
 		// Release list
 		SHORTREL( &Connection->lFuturePackets );
@@ -483,6 +721,7 @@ void TCP_INT_UpdateRecievedFromFuture(tTCPConnection *Connection)
 		Connection->NextSequenceRcv += pkt->Length;
 		free(pkt);
 	}
+	#endif
 }
 
 /**
@@ -724,13 +963,12 @@ void TCP_Server_Close(tVFS_Node *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 = -1;
 	conn->RemotePort = -1;
-	memset( &conn->RemoteIP, 0, sizeof(conn->RemoteIP) );
 
 	conn->Node.ImplPtr = conn;
 	conn->Node.NumACLs = 1;
@@ -741,6 +979,16 @@ tVFS_Node *TCP_Client_Init(tInterface *Interface)
 	conn->Node.Close = TCP_Client_Close;
 
 	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;
@@ -758,43 +1006,51 @@ tVFS_Node *TCP_Client_Init(tInterface *Interface)
 Uint64 TCP_Client_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 {
 	tTCPConnection	*conn = Node->ImplPtr;
-	char	*destbuf = Buffer;
 	size_t	len;
 	
 	ENTER("pNode XOffset XLength pBuffer", Node, Offset, Length, Buffer);
-	LOG("conn = %p", conn);
-	LOG("conn->State = %i", conn->State);
+	LOG("conn = %p {State:%i}", conn, conn->State);
 	
-	// Check if connection is open
-	while( conn->State == TCP_ST_HALFOPEN || conn->State == TCP_ST_SYN_SENT )
+	// 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( conn->State != TCP_ST_OPEN ) {
-		LEAVE('i', 0);
-		return 0;
-	}
 	
-	// Poll packets
-	for(;;)
+	// If the conneciton is not nope, then clean out the recieved buffer
+	if( conn->State != TCP_ST_OPEN )
 	{
-		// Wait
-		VFS_SelectNode(Node, VFS_SELECT_READ, NULL);
-		// Lock list and read
 		Mutex_Acquire( &conn->lRecievedPackets );
+		len = RingBuffer_Read( Buffer, conn->RecievedBuffer, Length );
+		Mutex_Release( &conn->lRecievedPackets );
 		
-		// Attempt to read all `Length` bytes
-		len = RingBuffer_Read( destbuf, conn->RecievedBuffer, Length );
-		
-		if( len == 0 || conn->RecievedBuffer->Length == 0 ) {
-			LOG("Marking as none avaliable (len = %i)\n", len);
+		if( len == 0 ) {
 			VFS_MarkAvaliable(Node, 0);
+			LEAVE('i', -1);
+			return -1;
 		}
-			
-		// Release the lock (we don't need it any more)
-		Mutex_Release( &conn->lRecievedPackets );
-	
+		
 		LEAVE('i', len);
 		return len;
 	}
+	
+	// Wait
+	VFS_SelectNode(Node, VFS_SELECT_READ, 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;
 }
 
 /**
@@ -816,6 +1072,10 @@ void TCP_INT_SendDataPacket(tTCPConnection *Connection, size_t Length, void *Dat
 	
 	memcpy(packet->Options, Data, Length);
 	
+	Log_Debug("TCP", "Send sequence 0x%08x", Connection->NextSequenceSend);
+	Debug_HexDump("[TCP     ] TCP_INT_SendDataPacket: Data = ",
+		Data, Length);
+	
 	TCP_SendPacket( Connection, sizeof(tTCPHeader)+Length, packet );
 	
 	Connection->NextSequenceSend += Length;
@@ -832,20 +1092,33 @@ Uint64 TCP_Client_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buf
 	ENTER("pNode XOffset XLength pBuffer", Node, Offset, Length, Buffer);
 	
 	// Check if connection is open
-	while( conn->State == TCP_ST_HALFOPEN || conn->State == TCP_ST_SYN_SENT )
+	while( conn->State == TCP_ST_SYN_RCVD || conn->State == TCP_ST_SYN_SENT )
 		Threads_Yield();
+	
 	if( conn->State != TCP_ST_OPEN ) {
-		LEAVE('i', 0);
-		return 0;
+		VFS_MarkError(Node, 1);
+		LEAVE('i', -1);
+		return -1;
 	}
 	
-	while( rem > TCP_MAX_PACKET_SIZE )
+	do
 	{
-		TCP_INT_SendDataPacket(conn, TCP_MAX_PACKET_SIZE, Buffer);
-		Buffer += TCP_MAX_PACKET_SIZE;
-	}
-	
-	TCP_INT_SendDataPacket(conn, rem, Buffer);
+		 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;
@@ -948,22 +1221,73 @@ void TCP_Client_Close(tVFS_Node *Node)
 	
 	ENTER("pNode", Node);
 	
-	packet.SourcePort = htons(conn->LocalPort);
-	packet.DestPort = htons(conn->RemotePort);
-	packet.DataOffset = (sizeof(tTCPHeader)/4)*16;
-	packet.WindowSize = TCP_WINDOW_SIZE;
+	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_FLAG_ACK;
+		
+		TCP_SendPacket( conn, sizeof(tTCPHeader), &packet );
+	}
 	
-	packet.AcknowlegementNumber = 0;
-	packet.SequenceNumber = htonl(conn->NextSequenceSend);
-	packet.Flags = TCP_FLAG_FIN;
+	switch( conn->State )
+	{
+	case TCP_ST_CLOSE_WAIT:
+		conn->State = TCP_ST_CLOSED;
+		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;
+	}
 	
-	conn->State = TCP_ST_FIN_SENT;
+	free(conn);
 	
-	TCP_SendPacket( conn, sizeof(tTCPHeader), &packet );
+	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;
+	}
 	
-	while( conn->State == TCP_ST_FIN_SENT )	Threads_Yield();
+	// Simple Case, no wrap ?
+	//       Lower Value Higher
+	// | ... + ... + ... + ... |
+
+	if( Lower < Higher ) {
+		return Lower < Value && Value < Higher;
+	}
+	// Higher has wrapped below lower
 	
-	free(conn);
+	// Value > Lower ?
+	//       Higher Lower Value
+	// | ... +  ... + ... + ... |
+	if( Value > Lower ) {
+		return 1;
+	}
 	
-	LEAVE('-');
+	// Value < Higher ?
+	//       Value Higher Lower
+	// | ... + ... +  ... + ... |
+	if( Value < Higher ) {
+		return 1;
+	}
+	
+	return 0;
 }