diff --git a/projects/tlayer/main.c b/projects/tlayer/main.c
index e62475b0b8..533820a854 100644
--- a/projects/tlayer/main.c
+++ b/projects/tlayer/main.c
@@ -109,20 +109,21 @@ void init_udp_server(void)
 		}
 	for (;;)
 		{
-		print_socket(sock);
 		recsize = recvfrom(sock, (void *)buffer_main, 256, 0, &sa, &fromlen);
 		if (recsize < 0)
 			{
 			printf("ERROR: recsize < 0!\n");
 			}
 		printf("recsize: %i\n ", recsize);
-		printf("datagram: %.*s\n", (int)recsize, buffer_main);
+		printf("datagram: %s\n", buffer_main);
 		}
 	}
 
 void init_tcp_server(void)
 	{
 	sockaddr6_t stSockAddr;
+	uint8_t read_bytes;
+	char buff_msg[MAX_TCP_BUFFER];
 	int SocketFD = socket(PF_INET6, SOCK_STREAM, IPPROTO_TCP);
 
 	if(-1 == SocketFD)
@@ -145,7 +146,7 @@ void init_tcp_server(void)
 		close(SocketFD);
 		exit(EXIT_FAILURE);
 		}
-	print_socket(SocketFD);
+	print_internal_socket(getSocket(SocketFD));
 	if(-1 == listen(SocketFD, 10))
 		{
 		perror("error listen failed");
@@ -165,7 +166,11 @@ void init_tcp_server(void)
 			exit(EXIT_FAILURE);
 			}
 
-		// TODO: read and writes
+		while (1)
+			{
+			read_bytes = recv(ConnectFD, buff_msg, MAX_TCP_BUFFER, 0);
+			printf("--- Message: %s ---\n", buff_msg);
+			}
 
 		shutdown(ConnectFD, 0);
 
diff --git a/sys/net/destiny/socket.c b/sys/net/destiny/socket.c
index b12af7b4bb..0f39296e75 100644
--- a/sys/net/destiny/socket.c
+++ b/sys/net/destiny/socket.c
@@ -12,6 +12,7 @@
 #include "tcp.h"
 #include "socket.h"
 #include "sys/net/net_help/net_help.h"
+#include "sys/net/net_help/msg_help.h"
 
 void print_tcp_flags (tcp_hdr_t *tcp_header)
 	{
@@ -48,36 +49,37 @@ void print_tcp_status(int in_or_out, ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_hea
 	print_tcp_flags(tcp_header);
 	}
 
-void print_socket(uint8_t socket)
+void print_socket(socket_t *current_socket)
 	{
-	socket_internal_t *current_socket_internal = &sockets[socket - 1];
-	socket_t *current_socket = &current_socket_internal->in_socket;
-	printf("ID: %i, PID: %i, Domain: %i, Type: %i, Protocol: %i State: %i\n",
-			current_socket_internal->socket_id,
-			current_socket_internal->pid,
+	printf("Domain: %i, Type: %i, Protocol: %i \n",
 			current_socket->domain,
 			current_socket->type,
-			current_socket->protocol,
-			current_socket->local_tcp_status.state);
-	printf("Local address: \tPort: %i, \tFamily: %i\n",
-			NTOHS(current_socket->local_address.sin6_port),
-			current_socket->local_address.sin6_family);
+			current_socket->protocol);
+//	printf("Local address: \tPort: %i, \tFamily: %i\n",
+//			NTOHS(current_socket->local_address.sin6_port),
+//			current_socket->local_address.sin6_family);
 	ipv6_print_addr(&current_socket->local_address.sin6_addr);
-	printf("\n");
-	printf("Foreign address: \tPort: %i, \tFamily: %i\n",
-			NTOHS(current_socket->foreign_address.sin6_port),
-			current_socket->foreign_address.sin6_family);
+//	printf("Foreign address: \tPort: %i, \tFamily: %i\n",
+//			NTOHS(current_socket->foreign_address.sin6_port),
+//			current_socket->foreign_address.sin6_family);
 	ipv6_print_addr(&current_socket->foreign_address.sin6_addr);
-	printf("\n");
-	printf("Local TCP status: ACK: %li, SEQ: %li, STATE: %i\n",
-			current_socket->local_tcp_status.ack_nr,
-			current_socket->local_tcp_status.seq_nr,
-			current_socket->local_tcp_status.state);
-	printf("\n");
-	printf("Foreign TCP status: ACK: %li, SEQ: %li, STATE: %i\n",
-				current_socket->foreign_tcp_status.ack_nr,
-				current_socket->foreign_tcp_status.seq_nr,
-				current_socket->foreign_tcp_status.state);
+//	printf("Local TCP status: ACK: %li, SEQ: %li, STATE: %i\n",
+//			current_socket->local_tcp_status.ack_nr,
+//			current_socket->local_tcp_status.seq_nr,
+//			current_socket->local_tcp_status.state);
+//	printf("Foreign TCP status: ACK: %li, SEQ: %li, STATE: %i\n",
+//				current_socket->foreign_tcp_status.ack_nr,
+//				current_socket->foreign_tcp_status.seq_nr,
+//				current_socket->foreign_tcp_status.state);
+	}
+
+void print_internal_socket(socket_internal_t *current_socket_internal)
+	{
+	socket_t *current_socket = &current_socket_internal->in_socket;
+	printf("\n--------------------------\n");
+	printf("ID: %i, PID: %i\n",	current_socket_internal->socket_id,	current_socket_internal->pid);
+	print_socket(current_socket);
+	printf("\n--------------------------\n");
 	}
 
 socket_internal_t *getSocket(uint8_t s)
@@ -100,8 +102,7 @@ void print_sockets(void)
 		{
 		if(getSocket(i) != NULL)
 			{
-			print_socket(i);
-			printf("\n----------------------------------------------------------------\n");
+			print_internal_socket(getSocket(i));
 			}
 		}
 	}
@@ -246,13 +247,13 @@ socket_internal_t *get_tcp_socket(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header
 			}
 		// TODO: Figure out a better strategy of using *.LOCAL_ADRESS than using the last Byte
 		else if ( isTCPSocket(i) &&
-				(current_socket->in_socket.local_tcp_status.state == LISTEN) &&
+				((current_socket->in_socket.local_tcp_status.state == LISTEN) || (current_socket->in_socket.local_tcp_status.state == SYN_RCVD)) &&
 				(current_socket->in_socket.local_address.sin6_addr.uint8[15] == ipv6_header->destaddr.uint8[15]) &&
 				(current_socket->in_socket.local_address.sin6_port == tcp_header->dst_port) &&
 				(current_socket->in_socket.foreign_address.sin6_addr.uint8[15] == 0x00) &&
 				(current_socket->in_socket.foreign_address.sin6_port == 0))
 			{
-			listening_socket = *(&current_socket);
+			listening_socket = current_socket;
 			}
 		i++;
 		}
@@ -307,23 +308,27 @@ void set_tcp_packet(tcp_hdr_t *tcp_hdr, uint16_t src_port, uint16_t dst_port, ui
 	}
 
 // Check for consistent ACK and SEQ number
-// TODO: Use error codes for different kinds of inconsistency when needed!
 int check_tcp_consistency(socket_t *current_tcp_socket, tcp_hdr_t *tcp_header)
 	{
 	if (IS_TCP_ACK(tcp_header->reserved_flags))
 		{
-		if(tcp_header->ack_nr != (current_tcp_socket->local_tcp_status.seq_nr+1))
+		if(tcp_header->ack_nr > (current_tcp_socket->local_tcp_status.seq_nr+1))
 			{
-			// TODO: possible loss of a packet (MAYBE!)
-			return -1;
+			// ACK of not yet sent byte, discard
+			return ACK_NO_TOO_BIG;
+			}
+		else if (tcp_header->ack_nr < (current_tcp_socket->local_tcp_status.seq_nr+1))
+			{
+			// ACK of previous segments, maybe dropped?
+			return ACK_NO_TOO_SMALL;
 			}
 		}
-	if (tcp_header->seq_nr <= current_tcp_socket->foreign_tcp_status.seq_nr)
+	if ((current_tcp_socket->foreign_tcp_status.seq_nr > 0) && (tcp_header->seq_nr <= current_tcp_socket->foreign_tcp_status.seq_nr))
 		{
-		// TODO: possible repetition of a packet or wrong order of arriving
-		return -1;
+		// segment repetition, maybe ACK got lost?
+		return SEQ_NO_SAME;
 		}
-	return 1;
+	return PACKET_OK;
 	}
 
 int connect(int socket, sockaddr6_t *addr, uint32_t addrlen, uint8_t tcp_client_thread)
@@ -392,7 +397,7 @@ int connect(int socket, sockaddr6_t *addr, uint32_t addrlen, uint8_t tcp_client_
 
 	// Got SYN ACK from Server
 	// Refresh foreign TCP socket information
-	set_tcp_status(&current_tcp_socket->foreign_tcp_status, tcp_header->ack_nr, STATIC_MSS, tcp_header->seq_nr, SYN_RCVD, tcp_header->window);
+	set_tcp_status(&current_tcp_socket->foreign_tcp_status, tcp_header->ack_nr, STATIC_MSS, tcp_header->seq_nr, ESTABLISHED, tcp_header->window);
 
 	// Refresh local TCP socket information
 	set_tcp_status(&current_tcp_socket->local_tcp_status, tcp_header->seq_nr + 1, STATIC_MSS, tcp_header->ack_nr, ESTABLISHED, STATIC_WINDOW);
@@ -417,21 +422,28 @@ void set_tcp_packet_auto(tcp_hdr_t *current_tcp_packet, socket_t *current_socket
 int32_t send(int s, void *msg, uint64_t len, int flags)
 	{
 	// Variables
+	int32_t sent_bytes = 0;
 	socket_internal_t *current_int_tcp_socket;
 	socket_t *current_tcp_socket;
 	uint8_t send_buffer[BUFFER_SIZE];
 	ipv6_hdr_t *temp_ipv6_header = ((ipv6_hdr_t*)(&send_buffer));
 	tcp_hdr_t *current_tcp_packet = ((tcp_hdr_t*)(&send_buffer[IPV6_HDR_LEN]));
 
-	// Check if socket exists
-	current_int_tcp_socket = getSocket(s);
-	if (current_int_tcp_socket == NULL)
+	// Check if socket exists and is TCP socket
+	if (!isTCPSocket(s))
 		{
 		return -1;
 		}
 
+	current_int_tcp_socket = getSocket(s);
 	current_tcp_socket = &current_int_tcp_socket->in_socket;
 
+	// Check for ESTABLISHED STATE
+	if (current_tcp_socket->local_tcp_status.state != ESTABLISHED)
+		{
+		return -1;
+		}
+
 	// Refresh local TCP socket information
 	current_tcp_socket->local_tcp_status.seq_nr = current_tcp_socket->local_tcp_status.seq_nr + len;
 
@@ -444,17 +456,72 @@ int32_t send(int s, void *msg, uint64_t len, int flags)
 	set_tcp_packet_auto(current_tcp_packet, current_tcp_socket);
 
 	// Add packet data
-	memcpy(&send_buffer[IPV6_HDR_LEN+TCP_HDR_LEN], msg, len);
+	if (len > current_tcp_socket->foreign_tcp_status.window)
+		{
+		memcpy(&send_buffer[IPV6_HDR_LEN+TCP_HDR_LEN], msg, current_tcp_socket->foreign_tcp_status.window);
+		sent_bytes = current_tcp_socket->foreign_tcp_status.window;
+		}
+	else
+		{
+		memcpy(&send_buffer[IPV6_HDR_LEN+TCP_HDR_LEN], msg, len);
+		sent_bytes = len;
+		}
+
 
 	// Checksum
 	current_tcp_packet->checksum = ~tcp_csum(temp_ipv6_header, current_tcp_packet);
 
 	sixlowpan_send(&current_tcp_socket->foreign_address.sin6_addr, (uint8_t*)(current_tcp_packet), TCP_HDR_LEN+len, IPPROTO_TCP);
-	return 1;
+	return sent_bytes;
+	}
+
+uint8_t read_from_socket(socket_internal_t *current_int_tcp_socket, void *buf, int len)
+	{
+	if (len > current_int_tcp_socket->tcp_input_buffer_end)
+		{
+		uint8_t read_bytes = current_int_tcp_socket->tcp_input_buffer_end;
+		memcpy(buf, current_int_tcp_socket->tcp_input_buffer, current_int_tcp_socket->tcp_input_buffer_end);
+		current_int_tcp_socket->tcp_input_buffer_end = 0;
+		return read_bytes;
+		}
+	else
+		{
+		memcpy(buf, current_int_tcp_socket->tcp_input_buffer, len);
+		memmove(current_int_tcp_socket->tcp_input_buffer, (current_int_tcp_socket->tcp_input_buffer+len), current_int_tcp_socket->tcp_input_buffer_end-len);
+		current_int_tcp_socket->tcp_input_buffer_end = current_int_tcp_socket->tcp_input_buffer_end-len;
+		return len;
+		}
 	}
 
 int32_t recv(int s, void *buf, uint64_t len, int flags)
 	{
+	// Variables
+	msg_t m_recv;
+	socket_internal_t *current_int_tcp_socket;
+	socket_t *current_tcp_socket;
+
+	// Check if socket exists
+	if (!isTCPSocket(s))
+		{
+		return -1;
+		}
+
+	current_int_tcp_socket = getSocket(s);
+	current_tcp_socket = &current_int_tcp_socket->in_socket;
+
+
+	if (current_int_tcp_socket->tcp_input_buffer_end > 0)
+		{
+		return read_from_socket(current_int_tcp_socket, buf, len);
+		}
+
+	net_msg_receive(&m_recv, FID_RECV);
+
+	if (current_int_tcp_socket->tcp_input_buffer_end > 0)
+		{
+		return read_from_socket(current_int_tcp_socket, buf, len);
+		}
+
 	return -1;
 	}
 
@@ -466,8 +533,7 @@ int32_t recvfrom(int s, void *buf, uint64_t len, int flags, sockaddr6_t *from, u
 		ipv6_hdr_t *ipv6_header;
 		udp_hdr_t *udp_header;
 		uint8_t *payload;
-		msg_receive(&m_recv);
-
+		net_msg_receive(&m_recv, FID_RECV_FROM);
 		ipv6_header = ((ipv6_hdr_t*)&buffer_udp);
 		udp_header = ((udp_hdr_t*)(&buffer_udp[IPV6_HDR_LEN]));
 		payload = &buffer_udp[IPV6_HDR_LEN+UDP_HDR_LEN];
@@ -479,7 +545,7 @@ int32_t recvfrom(int s, void *buf, uint64_t len, int flags, sockaddr6_t *from, u
 		from->sin6_flowinfo = 0;
 		from->sin6_port = udp_header->src_port;
 		memcpy(fromlen, (void*)(sizeof(sockaddr6_t)), sizeof(fromlen));
-		msg_reply(&m_recv, &m_send);
+		net_msg_reply(&m_recv, &m_send, FID_UDP_PH);
 		return udp_header->length;
 		}
 	else if (isTCPSocket(s))
@@ -646,7 +712,7 @@ socket_t *getWaitingConnectionSocket(int socket)
 	int i;
 	for (i = 0; i < MAX_QUEUED_SOCKETS; i++)
 		{
-		if (getSocket(socket)->queued_sockets[i].type != 0)
+		if (getSocket(socket)->queued_sockets[i].local_tcp_status.state == SYN_RCVD)
 			{
 			return &getSocket(socket)->queued_sockets[i];
 			}
@@ -664,6 +730,9 @@ int handle_new_tcp_connection(socket_t *current_queued_socket, socket_internal_t
 	ipv6_hdr_t *temp_ipv6_header = ((ipv6_hdr_t*)(&send_buffer));
 	tcp_hdr_t *syn_ack_packet = ((tcp_hdr_t*)(&send_buffer[IPV6_HDR_LEN]));
 
+	// Set status of internal socket to SYN_RCVD until connection with second socket is established!
+	server_socket->in_socket.local_tcp_status.state = SYN_RCVD;
+
 	// Fill SYN ACK TCP packet, still use queued socket for port number until connection is completely established!
 	// Otherwise the program doesnt return to this function and instead trys to call the new registered thread
 	// which isnt prepared to complete the threeway handshake process!
@@ -678,6 +747,7 @@ int handle_new_tcp_connection(socket_t *current_queued_socket, socket_internal_t
 
 	syn_ack_packet->checksum = ~tcp_csum(temp_ipv6_header, syn_ack_packet);
 
+	printf("BEFORE SENDING SYN ACK PACKET!\n");
 	sixlowpan_send(&current_queued_socket->foreign_address.sin6_addr, (uint8_t*)(syn_ack_packet), TCP_HDR_LEN, IPPROTO_TCP);
 
 	// wait for ACK from Client
@@ -690,7 +760,7 @@ int handle_new_tcp_connection(socket_t *current_queued_socket, socket_internal_t
 	tcp_header = ((tcp_hdr_t*)(buffer_tcp+IPV6_HDR_LEN));
 
 	// Check for consistency
-	if (check_tcp_consistency(current_queued_socket, tcp_header) == -1)
+	if (check_tcp_consistency(current_queued_socket, tcp_header) != PACKET_OK)
 		{
 		printf("TCP packets not consistent!\n");
 		}
@@ -699,6 +769,9 @@ int handle_new_tcp_connection(socket_t *current_queued_socket, socket_internal_t
 	set_tcp_status(&current_queued_socket->foreign_tcp_status, tcp_header->ack_nr, STATIC_MSS, tcp_header->seq_nr, ESTABLISHED, tcp_header->window);
 	set_tcp_status(&current_queued_socket->local_tcp_status, tcp_header->seq_nr+1, STATIC_MSS, tcp_header->ack_nr, ESTABLISHED, STATIC_WINDOW);
 
+	// Set status of internal socket back to LISTEN
+	server_socket->in_socket.local_tcp_status.state = LISTEN;
+
 	// send a reply to the TCP handler after processing every information from the TCP ACK packet
 	msg_reply(&msg_recv_client_ack, &msg_send_client_ack);
 
@@ -706,8 +779,9 @@ int handle_new_tcp_connection(socket_t *current_queued_socket, socket_internal_t
 	current_new_socket = getSocket(new_socket);
 
 	current_new_socket->pid = pid;
-	memcpy(&current_new_socket->in_socket, &current_queued_socket, sizeof(socket_t));
+	memcpy(&current_new_socket->in_socket, current_queued_socket, sizeof(socket_t));
 	close_socket(current_queued_socket);
+	print_sockets();
 	return new_socket;
 	}
 
diff --git a/sys/net/destiny/socket.h b/sys/net/destiny/socket.h
index f2ad696254..9720ede782 100644
--- a/sys/net/destiny/socket.h
+++ b/sys/net/destiny/socket.h
@@ -105,13 +105,14 @@
 #define	PF_NETGRAPH			AF_NETGRAPH
 #define	PF_MAX				AF_MAX
 
-#define MAX_SOCKETS			8
-#define MAX_QUEUED_SOCKETS	5
+#define MAX_SOCKETS			5
+#define MAX_QUEUED_SOCKETS	2
 
 #define EPHEMERAL_PORTS 	49152
 
 #define STATIC_MSS			32
-#define STATIC_WINDOW		32
+#define STATIC_WINDOW		1 * STATIC_MSS
+#define MAX_TCP_BUFFER		1 * STATIC_WINDOW
 
 #define INC_PACKET			0
 #define OUT_PACKET			1
@@ -139,6 +140,9 @@ typedef struct __attribute__ ((packed)) socket_in_t
 	{
 	uint8_t				socket_id;
 	uint8_t				pid;
+	// TODO: Maybe use ring buffer instead of copying array values each time
+	uint8_t				tcp_input_buffer_end;
+	uint8_t				tcp_input_buffer[MAX_TCP_BUFFER];
 	socket_t			in_socket;
 	socket_t			queued_sockets[MAX_QUEUED_SOCKETS];
 	} socket_internal_t;
@@ -160,10 +164,16 @@ int shutdown(int s , int how);
 void socket_init(void);
 socket_internal_t *get_udp_socket(ipv6_hdr_t *ipv6_header, udp_hdr_t *udp_header);
 socket_internal_t *get_tcp_socket(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header);
+socket_internal_t *getSocket(uint8_t s);
 void print_sockets(void);
-void print_socket(uint8_t socket);
+void print_internal_socket(socket_internal_t *current_socket_internal);
+void print_socket(socket_t *current_socket);
 bool exists_socket(uint8_t socket);
 socket_t *new_tcp_queued_socket(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header, socket_internal_t *socket);
 void print_tcp_status(int in_or_out, ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header);
-
+void set_tcp_status(tcp_socket_status_t *tcp_socket_status, uint32_t ack_nr, uint8_t mss, uint32_t seq_nr, uint8_t state, uint16_t window);
+void set_socket_address(sockaddr6_t *sockaddr, uint8_t sin6_family, uint16_t sin6_port, uint32_t sin6_flowinfo, ipv6_addr_t *sin6_addr);
+void set_tcp_packet(tcp_hdr_t *tcp_hdr, uint16_t src_port, uint16_t dst_port, uint32_t seq_nr, uint32_t ack_nr,
+		uint8_t dataOffset_reserved, uint8_t reserved_flags, uint16_t window, uint16_t checksum, uint16_t urg_pointer);
+int check_tcp_consistency(socket_t *current_tcp_socket, tcp_hdr_t *tcp_header);
 #endif /* SOCKET_H_ */
diff --git a/sys/net/destiny/tcp.c b/sys/net/destiny/tcp.c
index 2fa3264e79..0c3edad0f5 100644
--- a/sys/net/destiny/tcp.c
+++ b/sys/net/destiny/tcp.c
@@ -13,6 +13,7 @@
 #include "in.h"
 #include "socket.h"
 #include "sys/net/net_help/net_help.h"
+#include "sys/net/net_help/msg_help.h"
 
 void printTCPHeader(tcp_hdr_t *tcp_header)
 	{
@@ -51,13 +52,40 @@ uint16_t tcp_csum(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header)
     return (sum == 0) ? 0xffff : HTONS(sum);
 	}
 
+uint8_t handle_payload(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header, socket_internal_t *tcp_socket, uint8_t *payload)
+	{
+	msg_t m_send_tcp;
+	uint8_t tcp_payload_len = ipv6_header->length-TCP_HDR_LEN;
+	uint8_t acknowledged_bytes = 0;
+	if (tcp_payload_len > tcp_socket->in_socket.local_tcp_status.window)
+		{
+		memcpy(tcp_socket->tcp_input_buffer, payload, tcp_socket->in_socket.local_tcp_status.window);
+		acknowledged_bytes = tcp_socket->in_socket.local_tcp_status.window;
+		tcp_socket->in_socket.local_tcp_status.window = 0;
+		tcp_socket->tcp_input_buffer_end = tcp_socket->tcp_input_buffer_end + tcp_socket->in_socket.local_tcp_status.window;
+		}
+	else
+		{
+		memcpy(tcp_socket->tcp_input_buffer, payload, tcp_payload_len);
+
+		tcp_socket->in_socket.local_tcp_status.window = tcp_socket->in_socket.local_tcp_status.window - tcp_payload_len;
+		acknowledged_bytes = tcp_payload_len;
+		tcp_socket->tcp_input_buffer_end = tcp_socket->tcp_input_buffer_end + tcp_payload_len;
+		}
+
+	net_msg_send(&m_send_tcp, tcp_socket->pid, 0, FID_RECV);
+	return acknowledged_bytes;
+	}
+
 void handle_tcp_ack_packet(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header, socket_internal_t *tcp_socket)
 	{
 	msg_t m_recv_tcp, m_send_tcp;
+
 	if (getWaitingConnectionSocket(tcp_socket->socket_id)->local_tcp_status.state == SYN_RCVD)
 		{
 		msg_send_receive(&m_send_tcp, &m_recv_tcp, tcp_socket->pid);
 		}
+	printf("GOT REGULAR ACK FOR DATA!\n");
 	}
 
 void handle_tcp_rst_packet(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header, socket_internal_t *tcp_socket)
@@ -84,7 +112,7 @@ void handle_tcp_syn_packet(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header, socke
 		}
 	else
 		{
-		printf("Droppec TCP SYN Message because socket was not in state LISTEN!");
+		printf("Dropped TCP SYN Message because socket was not in state LISTEN!");
 		}
 	}
 
@@ -113,9 +141,42 @@ void handle_tcp_fin_ack_packet(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header, s
 
 void handle_tcp_no_flags_packet(ipv6_hdr_t *ipv6_header, tcp_hdr_t *tcp_header, socket_internal_t *tcp_socket, uint8_t *payload)
 	{
-	char message[128];
-	memcpy(message, payload, ipv6_header->length-TCP_HDR_LEN);
-	printf("Packet-Content: %s\n", message);
+	uint8_t tcp_payload_len = ipv6_header->length-TCP_HDR_LEN, read_bytes = 0;
+	socket_t *current_tcp_socket = &tcp_socket->in_socket;
+	uint8_t send_buffer[BUFFER_SIZE];
+	ipv6_hdr_t *temp_ipv6_header = ((ipv6_hdr_t*)(&send_buffer));
+	tcp_hdr_t *current_tcp_packet = ((tcp_hdr_t*)(&send_buffer[IPV6_HDR_LEN]));
+
+	if (tcp_payload_len > 0)
+		{
+		read_bytes = handle_payload(ipv6_header, tcp_header, tcp_socket, payload);
+		// Refresh TCP status values
+		set_tcp_status(&tcp_socket->in_socket.foreign_tcp_status,
+				tcp_header->ack_nr,
+				STATIC_MSS,
+				tcp_header->seq_nr,
+				ESTABLISHED,
+				tcp_header->window);
+		set_tcp_status(&tcp_socket->in_socket.local_tcp_status,
+				tcp_header->seq_nr - tcp_payload_len + read_bytes + 1,
+				STATIC_MSS,
+				tcp_socket->in_socket.local_tcp_status.seq_nr,
+				ESTABLISHED,
+				tcp_socket->in_socket.local_tcp_status.window);
+
+		// Fill IPv6 Header
+		memcpy(&(temp_ipv6_header->destaddr), &current_tcp_socket->foreign_address.sin6_addr, 16);
+		memcpy(&(temp_ipv6_header->srcaddr), &current_tcp_socket->local_address.sin6_addr, 16);
+		temp_ipv6_header->length = TCP_HDR_LEN;
+
+		// Fill TCP ACK packet
+		set_tcp_packet(current_tcp_packet, current_tcp_socket->local_address.sin6_port, current_tcp_socket->foreign_address.sin6_port,
+				current_tcp_socket->local_tcp_status.seq_nr, current_tcp_socket->local_tcp_status.ack_nr, 0, TCP_ACK, current_tcp_socket->local_tcp_status.window,
+				0, 0);
+
+		current_tcp_packet->checksum = ~tcp_csum(temp_ipv6_header, current_tcp_packet);
+		sixlowpan_send(&current_tcp_socket->foreign_address.sin6_addr, (uint8_t*)(current_tcp_packet), TCP_HDR_LEN, IPPROTO_TCP);
+		}
 	}
 
 void tcp_packet_handler (void)
@@ -129,24 +190,21 @@ void tcp_packet_handler (void)
 
 	while (1)
 		{
-		msg_receive(&m_recv_ip);
+		net_msg_receive(&m_recv_ip, FID_TCP_PH);
 		ipv6_header = ((ipv6_hdr_t*)&buffer_tcp);
 		tcp_header = ((tcp_hdr_t*)(&buffer_tcp[IPV6_HDR_LEN]));
 		payload = &buffer_tcp[IPV6_HDR_LEN+TCP_HDR_LEN];
-		//printTCPHeader(tcp_header);
 		chksum = tcp_csum(ipv6_header, tcp_header);
 		printf("Checksum is %x!\n", chksum);
-
-		tcp_socket = get_tcp_socket(ipv6_header, tcp_header);
-
 		print_tcp_status(INC_PACKET, ipv6_header, tcp_header);
+		tcp_socket = get_tcp_socket(ipv6_header, tcp_header);
 
 		if ((chksum == 0xffff) && (tcp_socket != NULL))
 			{
 			// Remove reserved bits from tcp flags field
 			uint8_t tcp_flags = tcp_header->reserved_flags & REMOVE_RESERVED;
 
-			//TODO: URG Flag and PSH flag are currently being ignored
+			// TODO: URG Flag and PSH flag are currently being ignored
 			switch (tcp_flags)
 				{
 				case TCP_ACK:
@@ -200,9 +258,7 @@ void tcp_packet_handler (void)
 			{
 			printf("Wrong checksum (%x) or no corresponding socket found!\n", chksum);
 			}
-
-		tcp_socket = NULL;
-		msg_reply(&m_recv_ip, &m_send_ip);
+		net_msg_reply(&m_recv_ip, &m_send_ip, FID_SIXLOWIP_TCP);
 		}
 	}
 
diff --git a/sys/net/destiny/tcp.h b/sys/net/destiny/tcp.h
index 28853a5963..4e235210ef 100644
--- a/sys/net/destiny/tcp.h
+++ b/sys/net/destiny/tcp.h
@@ -37,14 +37,23 @@ enum tcp_states
 	UNKNOWN			= 11
 	};
 
+enum tcp_codes
+	{
+	SEQ_NO_SAME			= -3,
+	ACK_NO_TOO_SMALL 	= -2,
+	ACK_NO_TOO_BIG		= -1,
+	UNDEFINED			= 0,
+	PACKET_OK			= 1
+	};
+
 #define REMOVE_RESERVED 		0xFC
 
-#define IS_TCP_ACK(a) 			(((a & REMOVE_RESERVED) & ~TCP_URG_PSH) | TCP_ACK)			// Test for ACK flag only, iognore URG und PSH flag
-#define IS_TCP_RST(a) 			(((a & REMOVE_RESERVED) & ~TCP_URG_PSH) | TCP_RST)
-#define IS_TCP_SYN(a) 			(((a & REMOVE_RESERVED) & ~TCP_URG_PSH) | TCP_SYN)
-#define IS_TCP_SYN_ACK(a) 		(((a & REMOVE_RESERVED) & ~TCP_URG_PSH) | TCP_SYN_ACK)
-#define IS_TCP_FIN(a) 			(((a & REMOVE_RESERVED) & ~TCP_URG_PSH) | TCP_FIN)
-#define IS_TCP_FIN_ACK(a) 		(((a & REMOVE_RESERVED) & ~TCP_URG_PSH) | TCP_FIN_ACK)
+#define IS_TCP_ACK(a) 			((a & TCP_ACK) > 0)			// Test for ACK flag only, iognore URG und PSH flag
+#define IS_TCP_RST(a) 			((a & TCP_RST) > 0)
+#define IS_TCP_SYN(a) 			((a & TCP_SYN) > 0)
+#define IS_TCP_SYN_ACK(a) 		((a & TCP_SYN_ACK) > 0)
+#define IS_TCP_FIN(a) 			((a & TCP_FIN) > 0)
+#define IS_TCP_FIN_ACK(a) 		((a & TCP_FIN_ACK) > 0)
 
 #define SET_TCP_ACK(a)			a = ((a & 0x00) | TCP_ACK)
 #define SET_TCP_RST(a) 			a = ((a & 0x00) | TCP_RST)
@@ -54,7 +63,7 @@ enum tcp_states
 #define SET_TCP_FIN_ACK(a) 		a = ((a & 0x00) | TCP_FIN_ACK)
 
 // TODO: Probably stack size too high
-#define TCP_STACK_SIZE 			2048
+#define TCP_STACK_SIZE 			4096
 
 #include "sys/net/sixlowpan/sixlowip.h"
 
diff --git a/sys/net/destiny/udp.c b/sys/net/destiny/udp.c
index 6a594d1681..84bd0a023d 100644
--- a/sys/net/destiny/udp.c
+++ b/sys/net/destiny/udp.c
@@ -9,6 +9,7 @@
 #include "socket.h"
 #include "in.h"
 #include "sys/net/net_help/net_help.h"
+#include "sys/net/net_help/msg_help.h"
 
 void printArrayRange_udp(uint8_t *array, uint16_t len)
 	{
@@ -43,8 +44,7 @@ void udp_packet_handler(void)
 
 	while (1)
 		{
-		msg_receive(&m_recv_ip);
-		printf("Inside UDP handler!\n");
+		net_msg_receive(&m_recv_ip, FID_UDP_PH);
 		ipv6_header = ((ipv6_hdr_t*)&buffer_udp);
 		udp_header = ((udp_hdr_t*)(&buffer_udp[IPV6_HDR_LEN]));
 		payload = &buffer_udp[IPV6_HDR_LEN+UDP_HDR_LEN];
@@ -53,12 +53,10 @@ void udp_packet_handler(void)
 
 		if (chksum == 0xffff)
 			{
-			m_send_udp.content.ptr = (char*)buffer;
-			m_send_udp.content.value = IPV6_HDR_LEN + UDP_HDR_LEN + udp_header->length;
 			udp_socket = get_udp_socket(ipv6_header, udp_header);
 			if (udp_socket != NULL)
 				{
-				msg_send_receive(&m_send_udp, &m_recv_udp, udp_socket->pid);
+				net_msg_send_recv(&m_send_udp, &m_recv_udp, udp_socket->pid, FID_RECV_FROM, FID_UDP_PH);
 				}
 			else
 				{
@@ -69,9 +67,7 @@ void udp_packet_handler(void)
 			{
 			printf("Wrong checksum (%x)!\n", chksum);
 			}
-
-		udp_socket = NULL;
-		msg_reply(&m_recv_ip, &m_send_ip);
+		net_msg_reply(&m_recv_ip, &m_send_ip, FID_SIXLOWIP_UDP);
 		}
 	}
 
diff --git a/sys/net/net_help/Jamfile b/sys/net/net_help/Jamfile
index 92efe589f7..6b89b45ac3 100644
--- a/sys/net/net_help/Jamfile
+++ b/sys/net/net_help/Jamfile
@@ -2,5 +2,6 @@ SubDir TOP sys net net_help ;
 
 # HDRS += $(TOP)/sys/net/net_help/ ;
 
-Module net_help : net_help.c ;
+Module net_help : net_help.c msg_help.c ;
+
 
diff --git a/sys/net/net_help/msg_help.c b/sys/net/net_help/msg_help.c
new file mode 100644
index 0000000000..7a0d3251f9
--- /dev/null
+++ b/sys/net/net_help/msg_help.c
@@ -0,0 +1,45 @@
+/*
+ * msg_help.c
+ *
+ *  Created on: 24.11.2011
+ *      Author: Oliver
+ */
+
+#include <thread.h>
+#include <stdio.h>
+#include "msg_help.h"
+
+int net_msg_receive(msg_t *m, uint16_t function_id)
+	{
+	int ret_val;
+	ret_val = msg_receive(m);
+	while (m->type != function_id)
+		{
+		ret_val = msg_receive(m);
+		}
+	return ret_val;
+	}
+
+int net_msg_reply(msg_t *m, msg_t *reply, uint16_t function_id)
+	{
+	reply->type = function_id;
+	return msg_reply(m, reply);
+	}
+
+int net_msg_send(msg_t *m, unsigned int pid, bool block, uint16_t function_id)
+	{
+	m->type = function_id;
+	return msg_send(m, pid, block);
+	}
+
+int net_msg_send_recv(msg_t *m, msg_t *reply, unsigned int pid, uint16_t function_id_m, uint16_t function_id_reply)
+	{
+	int ret_val;
+	m->type = function_id_m;
+	ret_val = msg_send_receive(m, reply, pid);
+	while(reply->type != function_id_reply)
+		{
+		ret_val = msg_receive(reply);
+		}
+	return ret_val;
+	}
diff --git a/sys/net/net_help/msg_help.h b/sys/net/net_help/msg_help.h
new file mode 100644
index 0000000000..00a70e85bb
--- /dev/null
+++ b/sys/net/net_help/msg_help.h
@@ -0,0 +1,27 @@
+/*
+ * msg_help.h
+ *
+ *  Created on: 24.11.2011
+ *      Author: Oliver
+ */
+
+#ifndef MSG_HELP_H_
+#define MSG_HELP_H_
+
+// Function IDs
+#define FID_SIXLOWIP_TCP		0
+#define FID_SIXLOWIP_UDP		1
+#define FID_TCP_PH				2
+#define FID_UDP_PH				3
+#define FID_H_PAYLOAD			4
+#define FID_RECV				5
+#define	FID_RECV_FROM			6
+
+
+
+int net_msg_receive(msg_t *m, uint16_t function_id);
+int net_msg_reply(msg_t *m, msg_t *reply, uint16_t function_id);
+int net_msg_send(msg_t *m, unsigned int pid, bool block, uint16_t function_id);
+int net_msg_send_recv(msg_t *m, msg_t *reply, unsigned int pid, uint16_t function_id_m, uint16_t function_id_reply);
+
+#endif /* MSG_HELP_H_ */
diff --git a/sys/net/sixlowpan/sixlowip.c b/sys/net/sixlowpan/sixlowip.c
index 8c7d96cdca..9eae43088d 100644
--- a/sys/net/sixlowpan/sixlowip.c
+++ b/sys/net/sixlowpan/sixlowip.c
@@ -8,7 +8,9 @@
 #include "sixlownd.h"
 #include "sixlowpan.h"
 #include "sys/net/destiny/in.h"
+#include "sys/net/destiny/socket.h"
 #include "sys/net/net_help/net_help.h"
+#include "sys/net/net_help/msg_help.h"
 
 uint8_t buffer[BUFFER_SIZE];
 msg_t msg_queue[IP_PKT_RECV_BUF_SIZE];
@@ -63,7 +65,12 @@ void sixlowpan_send(ipv6_addr_t *addr, uint8_t *payload, uint16_t p_len, uint8_t
 
     packet_length = IPV6_HDR_LEN + p_len;
 
-    lowpan_init((ieee_802154_long_t*)&(ipv6_buf->destaddr.uint16[4]),(uint8_t*)ipv6_buf); 
+    if (next_header == IPPROTO_TCP)
+		{
+		print_tcp_status(OUT_PACKET, ipv6_buf, (tcp_hdr_t *)(payload));
+		}
+
+    lowpan_init((ieee_802154_long_t*)&(ipv6_buf->destaddr.uint16[4]),(uint8_t*)ipv6_buf);
 }
 
 int icmpv6_demultiplex(const struct icmpv6_hdr_t *hdr) {
@@ -124,12 +131,12 @@ void ipv6_process(void){
 			}
 			case(IPPROTO_TCP):
 				{
-				printf("INFO: TCP Packet received.\n");
+				// printf("INFO: TCP Packet received.\n");
 
 				if (tcp_packet_handler_pid != 0)
 					{
 					memcpy(tcp_packet_buffer, (char*) ipv6_buf, IPV6_HDR_LEN+ipv6_buf->length);
-					msg_send_receive(&m_send, &m_recv, tcp_packet_handler_pid);
+					net_msg_send_recv(&m_send, &m_recv, tcp_packet_handler_pid, FID_TCP_PH, FID_SIXLOWIP_TCP);
 					}
 				else
 					{
@@ -144,7 +151,7 @@ void ipv6_process(void){
 				if (udp_packet_handler_pid != 0)
 					{
 					memcpy(udp_packet_buffer, (char*) ipv6_buf, IPV6_HDR_LEN+ipv6_buf->length);
-					msg_send_receive(&m_send, &m_recv, udp_packet_handler_pid);
+					net_msg_send_recv(&m_send, &m_recv, udp_packet_handler_pid, FID_UDP_PH, FID_SIXLOWIP_UDP);
 					}
 				else
 					{