From b9b8d43c57b4b1a61483f5d70942aec84b68712d Mon Sep 17 00:00:00 2001
From: Oliver Ruiz Dorantes <oliver.ruiz.dorantes@gmail.com>
Date: Tue, 14 Oct 2008 20:47:20 +0000
Subject: [PATCH] Current L2CAP code. Replying ConnectionResponses signals. The
 style should be more acurate than my previous commits(so use this commit to
 correct my remaining bad habits). The Code is not completed but need to
 commit to have a chance to go on from Germany....

git-svn-id: file:///srv/svn/repos/haiku/haiku/trunk@28107 a95241bf-73f2-0310-859d-f6bbb57e9c96
---
 .../kernel/network/protocols/l2cap/Jamfile    |    8 +-
 .../network/protocols/l2cap/L2capEndpoint.cpp |  251 ++++
 .../network/protocols/l2cap/L2capEndpoint.h   |   95 ++
 .../kernel/network/protocols/l2cap/l2cap.cpp  |   74 +-
 .../network/protocols/l2cap/l2cap_command.cpp |  160 ++-
 .../network/protocols/l2cap/l2cap_command.h   |   29 +-
 .../network/protocols/l2cap/l2cap_internal.h  |   17 +
 .../network/protocols/l2cap/l2cap_lower.cpp   |  222 ++++
 .../network/protocols/l2cap/l2cap_lower.h     |   15 +
 .../network/protocols/l2cap/l2cap_signal.cpp  | 1090 +++++++++++++++++
 .../network/protocols/l2cap/l2cap_signal.h    |   26 +
 .../network/protocols/l2cap/l2cap_upper.cpp   |   64 +
 .../network/protocols/l2cap/l2cap_upper.h     |   13 +
 13 files changed, 1976 insertions(+), 88 deletions(-)
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/l2cap_internal.h
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.h
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.h
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.cpp
 create mode 100644 src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h

diff --git a/src/add-ons/kernel/network/protocols/l2cap/Jamfile b/src/add-ons/kernel/network/protocols/l2cap/Jamfile
index 12ffa498be..f9c62bce2a 100644
--- a/src/add-ons/kernel/network/protocols/l2cap/Jamfile
+++ b/src/add-ons/kernel/network/protocols/l2cap/Jamfile
@@ -10,12 +10,18 @@ if $(TARGET_PLATFORM) != haiku {
 		# Unfortunately we get more than we want, namely all POSIX headers.
 }
 
-UsePrivateHeaders kernel net bluetooth ;
+UsePrivateKernelHeaders ;
+UsePrivateHeaders net bluetooth ;
 
 KernelAddon l2cap :
 	l2cap.cpp
 	l2cap_address.cpp
 	l2cap_command.cpp
+	l2cap_signal.cpp
+	l2cap_lower.cpp
+	l2cap_upper.cpp
+# Stack subModules
+    L2capEndpoint.cpp
 ;
 
 # Installation
diff --git a/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp
new file mode 100644
index 0000000000..751ad60a81
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp
@@ -0,0 +1,251 @@
+#include "L2capEndpoint.h"
+
+
+#include <stdio.h>
+#include <sys/stat.h>
+#include <string.h>
+
+
+#include "l2cap_internal.h"
+#include "l2cap_address.h"
+
+#include <bluetooth/L2CAP/btL2CAP.h>
+#define BT_DEBUG_THIS_MODULE
+#include <btDebug.h>
+
+
+static inline bigtime_t
+absolute_timeout(bigtime_t timeout)
+{
+	if (timeout == 0 || timeout == B_INFINITE_TIMEOUT)
+		return timeout;
+
+	// TODO: Make overflow safe!
+	return timeout + system_time();
+}
+
+
+L2capEndpoint::L2capEndpoint(net_socket* socket)
+	:
+	ProtocolSocket(socket),
+	fPeerEndpoint(NULL),
+	fAcceptSemaphore(-1)
+{
+	debugf("[%ld] %p->L2capEndpoint::L2capEndpoint()\n", find_thread(NULL), this);
+
+
+}
+
+
+L2capEndpoint::~L2capEndpoint()
+{
+	debugf("[%ld] %p->L2capEndpoint::~L2capEndpoint()\n", find_thread(NULL), this);
+
+
+}
+
+
+status_t
+L2capEndpoint::Init()
+{
+	debugf("[%ld] %p->L2capEndpoint::Init()\n", find_thread(NULL), this);
+
+	return(B_OK);
+}
+
+
+void
+L2capEndpoint::Uninit()
+{
+	debugf("[%ld] %p->L2capEndpoint::Uninit()\n", find_thread(NULL), this);
+
+
+}
+
+
+status_t
+L2capEndpoint::Open()
+{
+	debugf("[%ld] %p->L2capEndpoint::Open()\n", find_thread(NULL), this);
+
+    status_t error = ProtocolSocket::Open();
+	if (error != B_OK)
+		return(error);
+
+
+
+	return(B_OK);
+}
+
+
+status_t
+L2capEndpoint::Close()
+{
+	debugf("[%ld] %p->L2capEndpoint::Close()\n", find_thread(NULL), this);
+
+	return(B_OK);
+}
+
+
+status_t
+L2capEndpoint::Free()
+{
+	debugf("[%ld] %p->L2capEndpoint::Free()\n", find_thread(NULL), this);
+
+	return(B_OK);
+}
+
+
+status_t
+L2capEndpoint::Bind(const struct sockaddr *_address)
+{
+	if (_address == NULL)
+		panic("null adrresss!");
+	
+	if (_address->sa_family != AF_BLUETOOTH )
+		return(EAFNOSUPPORT);
+
+	// TODO: Check socladdr_l2cap size
+
+	// TODO: Check if that PSM is already bound
+	// return EADDRINUSE;
+
+
+	// TODO: Check if the PSM is valid, check assigned numbers document for valid
+	// psm available to applications.
+	// All PSM values shall be ODD, that is, the least significant bit of the least
+	// significant octet must be ’1’. Also, all PSM values shall have the least 
+	// significant bit of the most significant octet equal to ’0’. This allows the
+	// PSM field to be extended beyond 16 bits.
+	if ((((struct sockaddr_l2cap*)_address)->l2cap_psm & 1) == 0)
+		return B_ERROR;
+	
+	flowf("\n")
+	memcpy(&socket->address, _address, sizeof(struct sockaddr_l2cap));
+	socket->address.ss_len = sizeof(struct sockaddr_l2cap);
+
+	fState = LISTEN;
+
+	return B_OK;
+
+}
+
+
+status_t
+L2capEndpoint::Unbind()
+{
+	debugf("[%ld] %p->L2capEndpoint::Unbind()\n", find_thread(NULL), this);
+
+	return(B_OK);
+}
+
+
+status_t
+L2capEndpoint::Listen(int backlog)
+{
+	if (fState != CLOSED)
+		return B_BAD_VALUE;
+
+	fAcceptSemaphore = create_sem(0, "tcp accept");
+	if (fAcceptSemaphore < B_OK)
+		return ENOBUFS;
+
+	fState = LISTEN;
+
+	return B_OK;
+}
+
+
+status_t
+L2capEndpoint::Connect(const struct sockaddr *_address)
+{
+	if (_address->sa_family != AF_BLUETOOTH)
+		return(EAFNOSUPPORT);
+
+	debugf("[%ld] %p->UnixEndpoint::Connect(\"%s\")\n", find_thread(NULL), this,
+		ConstSocketAddress(&gL2cap4AddressModule, _address).AsString().Data());
+
+	const sockaddr_l2cap* address = (const sockaddr_l2cap*)_address;
+
+	/**/
+	TOUCH(address);
+
+	return B_OK;
+}
+
+
+status_t
+L2capEndpoint::Accept(net_socket **_acceptedSocket)
+{
+	debugf("[%ld] %p->UnixEndpoint::Accept()\n", find_thread(NULL), this);
+
+	bigtime_t timeout = absolute_timeout(socket->receive.timeout);
+
+	TOUCH(timeout);
+	return B_OK;
+}
+
+
+ssize_t
+L2capEndpoint::Send(const iovec *vecs, size_t vecCount,
+	ancillary_data_container *ancillaryData)
+{
+	debugf("[%ld] %p->UnixEndpoint::Send(%p, %ld, %p)\n", find_thread(NULL),
+		this, vecs, vecCount, ancillaryData);
+
+	return B_OK;
+}
+
+
+ssize_t
+L2capEndpoint::Receive(const iovec *vecs, size_t vecCount,
+	ancillary_data_container **_ancillaryData, struct sockaddr *_address,
+	socklen_t *_addressLength)
+{
+	debugf("[%ld] %p->UnixEndpoint::Receive(%p, %ld)\n", find_thread(NULL),
+		this, vecs, vecCount);
+
+
+	return B_OK;
+}
+
+
+ssize_t
+L2capEndpoint::Sendable()
+{
+	debugf("[%ld] %p->UnixEndpoint::Sendable()\n", find_thread(NULL), this);
+
+
+	return 0;
+}
+
+
+ssize_t
+L2capEndpoint::Receivable()
+{
+	debugf("[%ld] %p->UnixEndpoint::Receivable()\n", find_thread(NULL), this);
+
+	return 0;
+}
+
+
+L2capEndpoint*
+L2capEndpoint::ForPsm(uint16 psm)
+{
+
+	L2capEndpoint*	endpoint;
+
+	DoublyLinkedList<L2capEndpoint>::Iterator iterator = EndpointList.GetIterator();
+
+	while (iterator.HasNext()) {
+
+		endpoint = iterator.Next();
+		if (((struct sockaddr_l2cap*)&endpoint->socket->address)->l2cap_psm == psm && 
+			endpoint->fState == LISTEN) {
+			// TODO endpoint ocupied, lock it!
+			return endpoint;
+		}
+	}
+
+	return NULL;
+}
diff --git a/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h
new file mode 100644
index 0000000000..54d77a9c37
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h
@@ -0,0 +1,95 @@
+#ifndef L2CAP_ENDPOINT_H
+#define L2CAP_ENDPOINT_H
+
+
+#include <sys/stat.h>
+
+#include <lock.h>
+#include <util/DoublyLinkedList.h>
+#include <util/OpenHashTable.h>
+
+#include <net_protocol.h>
+#include <net_socket.h>
+#include <ProtocolUtilities.h>
+
+
+class L2capEndpoint : public net_protocol, 
+					  public ProtocolSocket,
+                      public DoublyLinkedListLinkImpl<L2capEndpoint>
+{
+
+public:
+	L2capEndpoint(net_socket* socket);
+	virtual ~L2capEndpoint();
+
+	status_t Init();
+	void Uninit();
+
+	status_t Open();
+	status_t Close();
+	status_t Free();
+
+	bool Lock()
+	{
+		return mutex_lock(&fLock) == B_OK;
+	}
+
+	void Unlock()
+	{
+		mutex_unlock(&fLock);
+	}
+
+	status_t Bind(const struct sockaddr *_address);
+	status_t Unbind();
+	status_t Listen(int backlog);
+	status_t Connect(const struct sockaddr *address);
+	status_t Accept(net_socket **_acceptedSocket);
+
+	ssize_t Send(const iovec *vecs, size_t vecCount,
+		ancillary_data_container *ancillaryData);
+	ssize_t Receive(const iovec *vecs, size_t vecCount,
+		ancillary_data_container **_ancillaryData, struct sockaddr *_address,
+		socklen_t *_addressLength);
+
+	ssize_t Sendable();
+	ssize_t Receivable();
+
+	status_t SetReceiveBufferSize(size_t size);
+	status_t GetPeerCredentials(ucred* credentials);
+
+	status_t Shutdown(int direction);
+
+	static   L2capEndpoint* ForPsm(uint16 psm);
+
+private:
+	typedef enum {
+		// establishing a connection
+		CLOSED,
+		LISTEN,
+		SYNCHRONIZE_SENT,
+		SYNCHRONIZE_RECEIVED,
+		ESTABLISHED,
+
+		// peer closes the connection
+		FINISH_RECEIVED,
+		WAIT_FOR_FINISH_ACKNOWLEDGE,
+
+		// we close the connection
+		FINISH_SENT,
+		FINISH_ACKNOWLEDGED,
+		CLOSING,
+		TIME_WAIT
+	} State;
+	
+private:
+	mutex			fLock;
+	State    		fState;
+	L2capEndpoint*	fPeerEndpoint;
+	sem_id			fAcceptSemaphore;
+	
+};
+
+
+extern DoublyLinkedList<L2capEndpoint> EndpointList;
+
+#endif	// L2cap_ENDPOINT_H
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp
index 7a6ec93685..7bda9f93e9 100644
--- a/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp
@@ -25,31 +25,45 @@
 
 #include <KernelExport.h>
 #include <util/list.h>
+#include <util/DoublyLinkedList.h>
 
 #include <new>
 #include <stdlib.h>
 #include <string.h>
 
-#include <bluetooth/HCI/btHCI_acl.h>
-
 #include "l2cap_address.h"
+#include "l2cap_internal.h"
+#include "l2cap_lower.h"
+#include "L2capEndpoint.h"
 
+#include <bluetooth/HCI/btHCI_acl.h>
+#include <BTCoreData.h>
+#include <btModules.h>
 
 #define BT_DEBUG_THIS_MODULE
+#define SUBMODULE_NAME "L2cap"
+#define SUBMODULE_COLOR 32
 #include <btDebug.h>
 
 
 typedef NetBufferField<uint16, offsetof(hci_acl_header, alen)> AclLenField;
+DoublyLinkedList<L2capEndpoint> EndpointList;
 
 
 struct l2cap_protocol : net_protocol {
+
 };
 
 
 extern net_protocol_module_info gL2CAPModule;
 
+
+
+// module references
+bluetooth_core_data_module_info *btCoreData;
 net_buffer_module_info *gBufferModule;
-static net_stack_module_info *sStackModule;
+net_stack_module_info *sStackModule;
+net_socket_module_info *gSocketModule;
 
 static struct net_domain *sDomain;
 
@@ -58,10 +72,12 @@ l2cap_init_protocol(net_socket *socket)
 {
 	flowf("\n");
 
-	l2cap_protocol *protocol = new (std::nothrow) l2cap_protocol;
+	L2capEndpoint* protocol = new (std::nothrow) L2capEndpoint(socket);
 	if (protocol == NULL)
 		return NULL;
 
+	EndpointList.Add(protocol);
+
 	return protocol;
 }
 
@@ -71,6 +87,9 @@ l2cap_uninit_protocol(net_protocol *protocol)
 {
 	flowf("\n");
 	
+	// TODO: Some more checkins	/ uninit
+	EndpointList.Remove((L2capEndpoint*)protocol);
+	
 	delete protocol;
 	return B_OK;
 }
@@ -128,7 +147,7 @@ l2cap_control(net_protocol *protocol, int level, int option, void *value,
 	flowf("\n");
 	
 /*	return protocol->next->module->control(protocol->next, level, option, value, _length); */
-	return EOPNOTSUPP;
+	return B_OK;
 }
 
 
@@ -139,7 +158,7 @@ l2cap_getsockopt(net_protocol *protocol, int level, int option,
 	flowf("\n");
 	
 /*	return protocol->next->module->getsockopt(protocol->next, level, option, value, length); */
-	return EOPNOTSUPP;
+	return B_OK;
 }
 
 
@@ -155,11 +174,9 @@ l2cap_setsockopt(net_protocol *protocol, int level, int option,
 
 
 status_t
-l2cap_bind(net_protocol *protocol, const struct sockaddr *address)
+l2cap_bind(net_protocol* protocol, const struct sockaddr* address)
 {
-	flowf("\n");
-	
-	return B_ERROR;
+	return ((L2capEndpoint*)protocol)->Bind(address);
 }
 
 
@@ -175,9 +192,7 @@ l2cap_unbind(net_protocol *protocol, struct sockaddr *address)
 status_t
 l2cap_listen(net_protocol *protocol, int count)
 {
-	flowf("\n");
-	
-	return EOPNOTSUPP;
+	return ((L2capEndpoint*)protocol)->Listen(count);
 }
 
 
@@ -258,21 +273,11 @@ l2cap_get_mtu(net_protocol *protocol, const struct sockaddr *address)
 status_t
 l2cap_receive_data(net_buffer *buffer)
 {
-	debugf("received some data, buffer length %lu\n", buffer->size);
+	HciConnection* conn = (HciConnection*) buffer;
+	debugf("received some data, buffer length %lu\n", conn->currentRxPacket->size);
+	
+	l2cap_receive(conn, conn->currentRxPacket);
 
-	NetBufferHeaderReader<hci_acl_header> bufferHeader(buffer);
-	if (bufferHeader.Status() < B_OK)
-		return bufferHeader.Status();
-
-	hci_acl_header &header = bufferHeader.Data();
-
-	debugf("  got handle %u, len %u\n", header.handle, header.alen);
-	debugf("  computed checksum: %ld\n", gBufferModule->checksum(buffer, 0, buffer->size, true));
-
-	if (gBufferModule->checksum(buffer, 0, buffer->size, true) != 0)
-		return B_BAD_DATA;
-
-	gBufferModule->free(buffer);
 	return B_OK;
 }
 
@@ -303,13 +308,13 @@ l2cap_error_reply(net_protocol *protocol, net_buffer *causedError, uint32 code,
 static status_t
 l2cap_std_ops(int32 op, ...)
 {
+	status_t error;		
+
 	flowf("\n");
 	
 	switch (op) {
 		case B_MODULE_INIT:
 		{
-			status_t error;		
-
 			error = sStackModule->register_domain_protocols(AF_BLUETOOTH, SOCK_STREAM, BLUETOOTH_PROTO_L2CAP,
 				"network/protocols/l2cap/v1",
 				NULL);
@@ -329,12 +334,17 @@ l2cap_std_ops(int32 op, ...)
 				return error;
 			}
 
+			new (&EndpointList) DoublyLinkedList<L2capEndpoint>;
+
+			error = InitializeConnectionPurgeThread();
 
 			return B_OK;
 		}
 
 		case B_MODULE_UNINIT:
 		
+			error = QuitConnectionPurgeThread();
+
 			sStackModule->unregister_domain(sDomain);
 			return B_OK;
 
@@ -346,8 +356,8 @@ l2cap_std_ops(int32 op, ...)
 
 net_protocol_module_info gL2CAPModule = {
 	{
-		"network/protocols/l2cap/v1",
-		0,
+		NET_BLUETOOTH_L2CAP_NAME,
+		B_KEEP_LOADED,
 		l2cap_std_ops
 	},
 	NET_PROTOCOL_ATOMIC_MESSAGES,
@@ -382,6 +392,8 @@ net_protocol_module_info gL2CAPModule = {
 module_dependency module_dependencies[] = {
 	{NET_STACK_MODULE_NAME, (module_info **)&sStackModule},
 	{NET_BUFFER_MODULE_NAME, (module_info **)&gBufferModule},
+	{BT_CORE_DATA_MODULE_NAME, (module_info **)&btCoreData},
+	{NET_SOCKET_MODULE_NAME, (module_info **)&gSocketModule},
 	{}
 };
 
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.cpp
index 13549ca85e..7c0d705e64 100644
--- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.cpp
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.cpp
@@ -79,12 +79,31 @@ struct _info_rsp {
 	l2cap_info_rsp_data_t data;
 } __attribute__ ((packed));
 
+// Configuration options
+struct _cfg_opt_flow {
+	l2cap_cfg_opt_t	 hdr;
+	l2cap_flow_t		 val;
+} __attribute__ ((packed));
+
+
+struct _cfg_opt_flush {
+	l2cap_cfg_opt_t	 hdr;
+	uint16		 val;
+} __attribute__ ((packed));
+
+struct _cfg_opt_mtu {
+	l2cap_cfg_opt_t	 hdr;
+	uint16		 val;
+} __attribute__ ((packed));
+
+
+
 
 /* L2CAP_CommandRej */
-static inline net_buffer*
+net_buffer*
 l2cap_cmd_rej(uint8 _ident, uint16 _reason, uint16 _mtu, uint16 _scid, uint16 _dcid)
 {
-	
+
 	net_buffer* _m = gBufferModule->create(sizeof(struct _cmd_rej));
 	if ((_m) == NULL)
 		return NULL;
@@ -110,7 +129,7 @@ l2cap_cmd_rej(uint8 _ident, uint16 _reason, uint16 _mtu, uint16 _scid, uint16 _d
 		bufferHeader->data.cid.dcid = htole16((_dcid));
 		bufferHeader->hdr.length += sizeof(bufferHeader->data.cid);
 	}
-	
+
 	_m->size = sizeof(bufferHeader->hdr) + bufferHeader->hdr.length;	 /* TODO: needed ?*/
 
 	bufferHeader->hdr.length = htole16(bufferHeader->hdr.length);
@@ -122,13 +141,13 @@ l2cap_cmd_rej(uint8 _ident, uint16 _reason, uint16 _mtu, uint16 _scid, uint16 _d
 
 
 /* L2CAP_ConnectReq */
-static inline net_buffer*
+net_buffer*
 l2cap_con_req(uint8 _ident, uint16 _psm, uint16 _scid)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _con_req));
 	if ((_m) == NULL)
-		return NULL;						
+		return NULL;
 
 	NetBufferPrepend<struct _con_req> bufferHeader(_m);
 	status_t status = bufferHeader.Status();
@@ -136,7 +155,7 @@ l2cap_con_req(uint8 _ident, uint16 _psm, uint16 _scid)
 		/* TODO free the buffer */
 		return NULL;
 	}
-	
+
 	bufferHeader->hdr.code = L2CAP_CON_REQ;
 	bufferHeader->hdr.ident = (_ident);
 	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
@@ -151,13 +170,13 @@ l2cap_con_req(uint8 _ident, uint16 _psm, uint16 _scid)
 
 
 /* L2CAP_ConnectRsp */
-static inline net_buffer*
+net_buffer*
 l2cap_con_rsp(uint8 _ident, uint16 _dcid, uint16 _scid, uint16 _result, uint16 _status)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _con_rsp));
 	if ((_m) == NULL)
-		return NULL;						
+		return NULL;
 
 	NetBufferPrepend<struct _con_rsp> bufferHeader(_m);
 	status_t status = bufferHeader.Status();
@@ -182,14 +201,14 @@ l2cap_con_rsp(uint8 _ident, uint16 _dcid, uint16 _scid, uint16 _result, uint16 _
 
 
 /* L2CAP_ConfigReq */
-static inline net_buffer*
+net_buffer*
 l2cap_cfg_req(uint8 _ident, uint16 _dcid, uint16 _flags, net_buffer* _data)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _cfg_req));
 	if ((_m) == NULL){
 		/* TODO free the _data buffer? */
-		return NULL;	
+		return NULL;
 	}
 
 	(_m)->size = sizeof(struct _cfg_req);	  /* check if needed */
@@ -211,6 +230,7 @@ l2cap_cfg_req(uint8 _ident, uint16 _dcid, uint16 _flags, net_buffer* _data)
 	bufferHeader.Sync();
 
 	/* Add the given data */
+	// TODO: given data can be freed... merge does it?
 	gBufferModule->merge(_m, _data, true);
 
 	return _m;
@@ -218,14 +238,14 @@ l2cap_cfg_req(uint8 _ident, uint16 _dcid, uint16 _flags, net_buffer* _data)
 
 
 /* L2CAP_ConfigRsp */
-static inline net_buffer*
+net_buffer*
 l2cap_cfg_rsp(uint8 _ident, uint16 _scid, uint16 _flags, uint16 _result, net_buffer* _data)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _cfg_rsp));
 	if ((_m) == NULL){
 		/* TODO free the _data buffer */
-		return NULL;	
+		return NULL;
 	}
 
 	NetBufferPrepend<struct _cfg_rsp> bufferHeader(_m);
@@ -246,20 +266,20 @@ l2cap_cfg_rsp(uint8 _ident, uint16 _scid, uint16 _flags, uint16 _result, net_buf
 	bufferHeader.Sync();
 
 	gBufferModule->merge(_m, _data, true);
-	
+
 	return _m;
 
 }
 
 
 /* L2CAP_DisconnectReq */
-static inline net_buffer*
+net_buffer*
 l2cap_discon_req(uint8 _ident, uint16 _dcid, uint16 _scid)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _discon_req));
 	if ((_m) == NULL){
-		return NULL;	
+		return NULL;
 	}
 
 	NetBufferPrepend<struct _discon_req> bufferHeader(_m);
@@ -283,13 +303,13 @@ l2cap_discon_req(uint8 _ident, uint16 _dcid, uint16 _scid)
 
 
 /* L2CA_DisconnectRsp */
-static inline net_buffer*
+net_buffer*
 l2cap_discon_rsp(uint8 _ident, uint16 _dcid, uint16 _scid)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _discon_rsp));
 	if ((_m) == NULL){
-		return NULL;	
+		return NULL;
 	}
 
 	NetBufferPrepend<struct _discon_rsp> bufferHeader(_m);
@@ -313,27 +333,16 @@ l2cap_discon_rsp(uint8 _ident, uint16 _dcid, uint16 _scid)
 
 
 /* L2CAP_EchoReq */
-static inline net_buffer*
+net_buffer*
 l2cap_echo_req(uint8 _ident, void* _data, size_t _size)
 {
 	net_buffer* _m = gBufferModule->create(sizeof(l2cap_cmd_hdr_t));
 	if ((_m) == NULL){
 		/* TODO free the _data buffer */
-		return NULL;	
-	}
-
-	NetBufferPrepend<l2cap_cmd_hdr_t> bufferHeader(_m);
-	status_t status = bufferHeader.Status();
-	if (status < B_OK) {
-		/* TODO free the buffer */
 		return NULL;
 	}
 
-	bufferHeader->code = L2CAP_ECHO_REQ;
-	bufferHeader->ident = (_ident);
-	bufferHeader->length = htole16(0);
 
-	bufferHeader.Sync();	
 
 	if ((_data) != NULL) {
 	   	gBufferModule->append(_m, _data, _size);
@@ -344,13 +353,13 @@ l2cap_echo_req(uint8 _ident, void* _data, size_t _size)
 
 
 /* L2CAP_InfoReq */
-static inline net_buffer*
+net_buffer*
 l2cap_info_req(uint8 _ident, uint16 _type)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _info_req));
 	if ((_m) == NULL){
-		return NULL;	
+		return NULL;
 	}
 
 	NetBufferPrepend<struct _info_req> bufferHeader(_m);
@@ -373,13 +382,13 @@ l2cap_info_req(uint8 _ident, uint16 _type)
 
 
 /* L2CAP_InfoRsp */
-static inline net_buffer*
+net_buffer*
 l2cap_info_rsp(uint8 _ident, uint16 _type, uint16 _result, uint16 _mtu)
 {
 
 	net_buffer* _m = gBufferModule->create(sizeof(struct _info_rsp));
 	if ((_m) == NULL){
-		return NULL;	
+		return NULL;
 	}
 
 	NetBufferPrepend<struct _info_rsp> bufferHeader(_m);
@@ -401,10 +410,10 @@ l2cap_info_rsp(uint8 _ident, uint16 _type, uint16 _result, uint16 _mtu)
 		case L2CAP_CONNLESS_MTU:
 			bufferHeader->data.mtu.mtu = htole16((_mtu));
 			bufferHeader->hdr.length += sizeof((bufferHeader->data.mtu.mtu));
-			break;				
-		}						   
-	}							   
-									
+			break;
+		}
+	}
+
 	(_m)->size = sizeof(bufferHeader->hdr) + bufferHeader->hdr.length;
 
 	bufferHeader->hdr.length = htole16(bufferHeader->hdr.length);
@@ -420,12 +429,81 @@ l2cap_info_rsp(uint8 _ident, uint16 _type, uint16 _result, uint16 _mtu)
 #endif
 
 
+#define _ng_l2cap_build_cfg_options(_m, _mtu, _flush_timo, _flow)	\
+
 /* Build configuration options  */
-static inline net_buffer*
+net_buffer*
 l2cap_build_cfg_options(uint16* _mtu, uint16* _flush_timo, l2cap_flow_t* _flow)
 {
-	//TODO:
+	size_t requestedSize = 0;
 
-	return NULL;
+	if (_mtu != NULL)
+		requestedSize+=sizeof(*_mtu);
+
+
+	if (_flush_timo != NULL)
+		requestedSize+=sizeof(*_flush_timo);
+
+	if (_flow != NULL)
+		requestedSize+=sizeof(*_flow);
+	
+	net_buffer* _m = gBufferModule->create(sizeof(requestedSize));
+
+	if (_m == NULL)
+		return NULL;
+
+	if (_mtu != NULL) {
+		NetBufferPrepend<struct _cfg_opt_mtu> bufferHeader(_m);
+		status_t status = bufferHeader.Status();
+		if (status < B_OK) {
+			/* TODO free the buffer ?? */
+			return NULL;
+		}
+
+		bufferHeader->hdr.type = L2CAP_OPT_MTU;
+		bufferHeader->hdr.length = sizeof(bufferHeader->val);
+		bufferHeader->val = htole16(*(uint16 *)(_mtu));
+
+		delete &bufferHeader;
+	}
+
+	if (_flush_timo != NULL) {
+
+		NetBufferPrepend<struct _cfg_opt_flush> bufferHeader(_m);
+		status_t status = bufferHeader.Status();
+		if (status < B_OK) {
+			/* TODO free the buffer ?? */
+			return NULL;
+		}
+
+		bufferHeader->hdr.type = L2CAP_OPT_FLUSH_TIMO;
+		bufferHeader->hdr.length = sizeof(bufferHeader->val);
+		bufferHeader->val = htole16(*(int16 *)(_flush_timo));
+
+		delete &bufferHeader;
+	}
+
+	if (_flow != NULL) {
+
+		NetBufferPrepend<struct _cfg_opt_flow> bufferHeader(_m);
+		status_t status = bufferHeader.Status();
+		if (status < B_OK) {
+			/* TODO free the buffer ?? */
+			return NULL;
+		}
+
+		bufferHeader->hdr.type = L2CAP_OPT_QOS;
+		bufferHeader->hdr.length = sizeof(bufferHeader->val);
+		bufferHeader->val.flags = _flow->flags;
+		bufferHeader->val.service_type = _flow->service_type;
+		bufferHeader->val.token_rate = htole32(_flow->token_rate);
+		bufferHeader->val.token_bucket_size = htole32(_flow->token_bucket_size);
+		bufferHeader->val.peak_bandwidth = htole32(_flow->peak_bandwidth);
+		bufferHeader->val.latency = htole32(_flow->latency);
+		bufferHeader->val.delay_variation = htole32(_flow->delay_variation);
+
+		delete &bufferHeader;
+	}
+
+	return _m;
 }
-
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.h b/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.h
index 97c832709c..fe4afca915 100644
--- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.h
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_command.h
@@ -1,46 +1,45 @@
-/*
- * Copyright 2007 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+/* 
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
  * All rights reserved. Distributed under the terms of the MIT License.
  */
 #ifndef _L2CAP_CMDS_H_
 #define _L2CAP_CMDS_H_
 
-#include <net_buffer.h>
+#include <NetBufferUtilities.h>
 
 #include <l2cap.h>
 
 
 extern net_buffer_module_info *gBufferModule;
 
-static inline net_buffer*
+net_buffer*
 l2cap_cmd_rej(uint8 _ident, uint16 _reason, uint16 _mtu, uint16 _scid, uint16 _dcid);
 
-static inline net_buffer*
+net_buffer*
 l2cap_con_req(uint8 _ident, uint16 _psm, uint16 _scid);
 
-static inline net_buffer*
+net_buffer*
 l2cap_con_rsp(uint8 _ident, uint16 _dcid, uint16 _scid, uint16 _result, uint16 _status);
 
-static inline net_buffer*
+net_buffer*
 l2cap_cfg_req(uint8 _ident, uint16 _dcid, uint16 _flags, net_buffer* _data);
 
-static inline net_buffer*
+net_buffer*
 l2cap_cfg_rsp(uint8 _ident, uint16 _scid, uint16 _flags, uint16 _result, net_buffer* _data);
 
-static inline net_buffer*
+net_buffer*
 l2cap_discon_req(uint8 _ident, uint16 _dcid, uint16 _scid);
 
-static inline net_buffer*
+net_buffer*
 l2cap_discon_rsp(uint8 _ident, uint16 _dcid, uint16 _scid);
 
-static inline net_buffer*
+net_buffer*
 l2cap_echo_req(uint8 _ident, void* _data, size_t _size);
 
-static inline net_buffer*
+net_buffer*
 l2cap_info_req(uint8 _ident, uint16 _type);
 
-static inline net_buffer*
+net_buffer*
 l2cap_info_rsp(uint8 _ident, uint16 _type, uint16 _result, uint16 _mtu);
 
-#endif /* L2CAP_CMDS_H_ */
-
+#endif
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_internal.h b/src/add-ons/kernel/network/protocols/l2cap/l2cap_internal.h
new file mode 100644
index 0000000000..4260a365ec
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_internal.h
@@ -0,0 +1,17 @@
+/* 
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+ * All rights reserved. Distributed under the terms of the MIT License.
+ */
+#ifndef L2CAP_INTERNAL_H
+#define L2CAP_INTERNAL_H
+
+#include <BTCoreData.h>
+#include <net_protocol.h>
+#include <net_stack.h>
+
+extern bluetooth_core_data_module_info *btCoreData;
+extern net_buffer_module_info *gBufferModule;
+extern net_stack_module_info *gStackModule;
+extern net_socket_module_info *gSocketModule;
+
+#endif	// L2CAP_ADDRESS_H
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp
new file mode 100644
index 0000000000..1163965bee
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp
@@ -0,0 +1,222 @@
+/*
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+ * All rights reserved. Distributed under the terms of the MIT License.
+ *
+ */
+/*-
+ * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+*/
+#include <KernelExport.h>
+#include <string.h>
+
+#include <NetBufferUtilities.h>
+
+#include <bluetooth/HCI/btHCI_transport.h>
+
+#include <l2cap.h>
+#include "l2cap_internal.h"
+#include "l2cap_signal.h"
+
+#define BT_DEBUG_THIS_MODULE
+#define SUBMODULE_NAME "lower"
+#define SUBMODULE_COLOR 36
+#include <btDebug.h>
+
+status_t
+l2cap_receive(HciConnection* conn, net_buffer* buffer)
+{
+	status_t    error = B_OK;
+	uint16		dcid;
+	uint16		length;
+	
+	/* Check packet */
+	if (buffer->size < sizeof(l2cap_hdr_t)) {
+		debugf("invalid L2CAP packet. Packet too small, len=%ld\n", buffer->size);
+		gBufferModule->free(buffer);
+		return EMSGSIZE;
+
+	}
+
+	/* Get L2CAP header */
+	NetBufferHeaderReader<l2cap_hdr_t> bufferHeader(buffer);
+	status_t status = bufferHeader.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	length = bufferHeader->length = le16toh(bufferHeader->length);
+	dcid = bufferHeader->dcid = le16toh(bufferHeader->dcid);
+
+	bufferHeader.Remove(); /* pulling */
+
+	/* Check payload size */
+	if (length != buffer->size ) {
+		debugf("invalid L2CAP packet. Payload length mismatch, packetlen=%d, nebufferlen=%ld\n",
+			   length, buffer->size);
+		gBufferModule->free(buffer);
+		return EMSGSIZE;
+	}
+
+	/* Process packet */
+	switch (dcid) {
+		case L2CAP_SIGNAL_CID: /* L2CAP command */
+			error = l2cap_process_signal_cmd(conn, buffer);
+		break;
+
+		case L2CAP_CLT_CID: /* Connectionless packet
+			error = l2cap_cl_receive(buffer);*/
+		break;
+
+		default: /* Data packet
+			error = l2cap_co_receive(buffer);*/
+		break;
+	}
+
+	return (error);
+
+}
+
+#if 0
+#pragma - thread conn sched - 
+#endif
+
+static thread_id sConnectionThread;
+
+
+void
+purge_connection(HciConnection* conn)
+{
+
+	L2capFrame* frame;
+
+	debugf("handle=%d\n",conn->handle);
+
+	DoublyLinkedQueue<L2capFrame>::Iterator iterator = conn->OutGoingFrames.GetIterator();
+	while (iterator.HasNext()) {
+
+		frame = iterator.Next();
+
+		if (frame->type == L2CAP_C_FRAME) {
+			btCoreData->TimeoutSignal(frame, bluetooth_l2cap_rtx_timeout);
+			conn->ExpectedResponses.Add(frame);
+		}
+
+		//TODO: This operation should be atomic ACL Segments should be sent 
+		//lower->Send(frame->buffer);
+		flowf("tolower!");
+		conn->OutGoingFrames.Remove(frame);
+	}
+	
+}
+
+
+static status_t
+connection_thread(void *)
+{
+	int32 code;
+	ssize_t	ssizePort;
+	ssize_t	ssizeRead;
+
+	HciConnection* conn = NULL;
+	
+	// TODO: Keep this a static var
+	port_id fPort = find_port(BLUETOOTH_CONNECTION_SCHED_PORT);
+	if ( fPort == B_NAME_NOT_FOUND ) 
+	{
+		panic("BT Connection port has been deleted");
+	}
+
+	while ((ssizePort = port_buffer_size(fPort)) != B_BAD_PORT_ID) { 
+
+		 if (ssizePort <= 0) {
+		 	debugf("Error %s\n", strerror(ssizePort));
+		 	snooze(1*1000*1000);
+		    continue;
+		 }
+		 
+		if (ssizePort > (ssize_t) sizeof(conn)) {
+		 	debugf("Message too big %ld\n", ssizePort);
+		 	snooze(1*1000*1000);
+			continue;
+		}
+
+		ssizeRead = read_port(fPort, &code, &conn, ssizePort);		 		 
+
+		if (ssizeRead != ssizePort) {
+		 	debugf("Missmatch size port=%ld read=%ld\n", ssizePort, ssizeRead);
+		 	snooze(1*1000*1000);
+		    continue;
+		}
+		
+		purge_connection(conn);
+	}
+
+	return B_OK;
+}
+
+
+status_t
+InitializeConnectionPurgeThread()
+{
+
+	port_id fPort = find_port(BLUETOOTH_CONNECTION_SCHED_PORT);
+	if ( fPort == B_NAME_NOT_FOUND ) 
+	{
+		fPort = create_port(16, BLUETOOTH_CONNECTION_SCHED_PORT);
+		debugf("Connection purge port created %ld\n",fPort);
+	}
+	
+	// This thread has to catch up connections before first package is sent.
+	sConnectionThread = spawn_kernel_thread(connection_thread,
+				"bluetooth connection purge", B_URGENT_DISPLAY_PRIORITY, NULL);
+
+	if (sConnectionThread >= B_OK)
+		return resume_thread(sConnectionThread);
+	else
+		return B_ERROR;
+}
+
+
+status_t
+QuitConnectionPurgeThread()
+{
+	status_t status;
+
+	port_id fPort = find_port(BLUETOOTH_CONNECTION_SCHED_PORT);
+	if ( fPort != B_NAME_NOT_FOUND ) 
+		close_port(fPort);
+
+	flowf("Connection port deleted\n");
+	wait_for_thread(sConnectionThread, &status);
+	return status;
+}
+
+
+void
+SchedConnectionPurgeThread(HciConnection* conn) 
+{
+	port_id port = find_port(BLUETOOTH_CONNECTION_SCHED_PORT);
+
+	HciConnection* temp = conn;
+
+	if (port == B_NAME_NOT_FOUND) 
+		panic("BT Connection Port Deleted");
+
+	status_t error = write_port(port, (uint32) conn, &temp, sizeof(conn));
+	
+	//debugf("error post %s port=%ld size=%ld\n", strerror(error), port, sizeof(conn));
+
+	if (error != B_OK)
+		panic("BT Connection sched failed");
+
+}
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.h b/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.h
new file mode 100644
index 0000000000..f86c9a3ddb
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.h
@@ -0,0 +1,15 @@
+/* 
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+ * All rights reserved. Distributed under the terms of the MIT License.
+ */
+#ifndef L2CAP_LOWER_H
+#define L2CAP_LOWER_H
+
+status_t l2cap_receive(HciConnection* conn, net_buffer* buffer);
+
+
+status_t InitializeConnectionPurgeThread();
+status_t QuitConnectionPurgeThread();
+void SchedConnectionPurgeThread(HciConnection* conn);
+
+#endif
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp
new file mode 100644
index 0000000000..237e66077b
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp
@@ -0,0 +1,1090 @@
+/*
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+ * All rights reserved. Distributed under the terms of the MIT License.
+ */
+/*-
+ * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+*/
+
+#include <KernelExport.h>
+#include <string.h>
+
+#include <NetBufferUtilities.h>
+
+
+#include <l2cap.h>
+#include "l2cap_internal.h"
+#include "l2cap_signal.h"
+#include "l2cap_command.h"
+#include "l2cap_upper.h"
+
+#define BT_DEBUG_THIS_MODULE
+#define SUBMODULE_NAME "signal"
+#define SUBMODULE_COLOR 36
+#include <btDebug.h>
+
+
+typedef enum _option_status {
+	OPTION_NOT_PRESENT = 0,
+	OPTION_PRESENT = 1,
+	HEADER_TOO_SHORT = -1,
+	BAD_OPTION_LENGTH = -2,
+	OPTION_UNKNOWN = -3
+} option_status;
+
+
+static status_t
+l2cap_process_con_req(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_con_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_cfg_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_discon_req(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_echo_req(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_echo_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_info_req(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+static status_t
+l2cap_process_info_rsp(HciConnection *conn, uint8 ident, net_buffer* buffer);
+
+static status_t
+l2cap_process_cmd_rej(HciConnection *conn, uint8 ident, net_buffer *buffer);
+
+
+/*
+ * Process L2CAP signaling command. We already know that destination channel ID
+ * is 0x1 that means we have received signaling command from peer's L2CAP layer.
+ * So get command header, decode and process it.
+ *
+ * XXX do we need to check signaling MTU here?
+ */
+
+status_t
+l2cap_process_signal_cmd(HciConnection* conn, net_buffer* buffer)
+{
+	net_buffer* 		m = buffer;
+	flowf("\n");
+
+	while (m != NULL) {
+
+		/* Verify packet length */
+		if (buffer->size < sizeof(l2cap_cmd_hdr_t)) {
+			debugf("small L2CAP signaling command len=%ld\n", buffer->size);
+			gBufferModule->free(buffer);
+			return EMSGSIZE;
+		}
+
+		/* Get COMMAND header */
+		NetBufferHeaderReader<l2cap_cmd_hdr_t> commandHeader(buffer);
+		status_t status = commandHeader.Status();
+		if (status < B_OK) {
+			return ENOBUFS;
+		}
+
+		commandHeader->length = le16toh(commandHeader->length);
+
+		/* Verify command length */
+		if (buffer->size < commandHeader->length) {
+			debugf("invalid L2CAP signaling command, code=%#x, ident=%d, length=%d, buffer size=%ld\n",
+				    commandHeader->code, commandHeader->ident, commandHeader->length, buffer->size);
+			gBufferModule->free(buffer);
+			return (EMSGSIZE);
+		}
+
+		uint8 processingCode = commandHeader->code;
+		uint16 processingIdent = commandHeader->ident;
+
+		commandHeader.Remove(); // pulling the header of the command
+
+		/* Process command processors responsible to delete the command*/
+		switch (processingCode) {
+		case L2CAP_CMD_REJ:
+			l2cap_process_cmd_rej(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_CON_REQ:
+			l2cap_process_con_req(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_CON_RSP:
+			l2cap_process_con_rsp(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_CFG_REQ:
+			l2cap_process_cfg_req(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_CFG_RSP:
+			l2cap_process_cfg_rsp(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_DISCON_REQ:
+			l2cap_process_discon_req(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_DISCON_RSP:
+			l2cap_process_discon_rsp(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_ECHO_REQ:
+			l2cap_process_echo_req(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_ECHO_RSP:
+			l2cap_process_echo_rsp(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_INFO_REQ:
+			l2cap_process_info_req(conn, processingIdent, buffer);
+		break;
+
+		case L2CAP_INFO_RSP:
+			l2cap_process_info_rsp(conn, processingIdent, buffer);
+		break;
+
+		default:
+			debugf("unknown L2CAP signaling command, code=%#x, ident=%d\n",
+						processingCode, processingIdent);
+
+			// Send L2CAP_CommandRej. Do not really care about the result
+			// ORD: Remiaining commands in the same packet are also to be rejected
+/*			send_l2cap_reject(con, processingIdent, L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);*/
+			gBufferModule->free(m);
+			break;
+		}
+
+		// Is there still remaining size? processors should have pulled its content...
+		if (m->size == 0) {
+			// free the buffer
+			gBufferModule->free(m);
+			m = NULL;
+		}
+	}
+
+	return B_OK;
+}
+
+
+#if 0
+#pragma mark - Processing Incoming signals
+#endif
+
+
+/* Process L2CAP_ConnectReq command */
+static status_t
+l2cap_process_con_req(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capChannel*		ch;
+	uint16				dcid, psm;
+
+	/* Get con req data */
+	NetBufferHeaderReader<l2cap_con_req_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	psm = le16toh(command->psm);
+	dcid = le16toh(command->scid);
+
+	command.Remove(); // pull the command body
+
+	/*	Create new channel and send L2CA_ConnectInd
+	    notification to the upper layer protocol.
+	 */
+	ch = btCoreData->AddChannel(conn, psm /*, dcid, ident*/);
+	if (ch == NULL) {
+		flowf("No resources to create channel\n");
+		return (send_l2cap_con_rej(conn, ident, 0, dcid, L2CAP_NO_RESOURCES));
+	} else {
+		debugf("New channel created scid=%d\n", ch->scid);
+	}
+
+	ch->dcid = dcid;
+	ch->ident = ident;
+		
+	status_t indicationStatus = l2cap_l2ca_con_ind(ch);
+
+	if ( indicationStatus == B_OK ) {
+		ch->state = L2CAP_CHAN_W4_L2CA_CON_RSP;
+
+	} else if (indicationStatus == B_NO_MEMORY) {
+		/* send_l2cap_con_rej(con, ident, ch->scid, dcid, L2CAP_NO_RESOURCES);*/
+		btCoreData->RemoveChannel(conn, ch->scid);
+
+	} else {
+		send_l2cap_con_rej(conn, ident, ch->scid, dcid, L2CAP_PSM_NOT_SUPPORTED);
+		btCoreData->RemoveChannel(conn, ch->scid);
+	}
+
+	return (indicationStatus);
+}
+
+
+static status_t
+l2cap_process_con_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capFrame	*cmd = NULL;
+	uint16 		scid, dcid, result, status;
+	status_t		error = 0;
+
+	/* Get command parameters */
+	NetBufferHeaderReader<l2cap_con_rsp_cp> command(buffer);
+	if (command.Status() < B_OK) {
+		return ENOBUFS;
+	}
+
+	dcid = le16toh(command->dcid);
+	scid = le16toh(command->scid);
+	result = le16toh(command->result);
+	status = le16toh(command->status);
+
+	command.Remove(); // pull the command body
+	/* Check if we have pending command descriptor */
+	cmd = btCoreData->SignalByIdent(conn, ident);
+	if (cmd == NULL) {
+		debugf("unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", ident,	conn->handle);
+		return ENOENT;
+	}
+
+	/* Verify channel state, if invalid - do nothing */
+	if (cmd->channel->state != L2CAP_CHAN_W4_L2CAP_CON_RSP) {
+		debugf("unexpected L2CAP_ConnectRsp. Invalid channel state, cid=%d, state=%d\n", scid,	cmd->channel->state);
+		goto reject;
+	}
+
+	/* Verify CIDs and send reject if does not match */
+	if (cmd->channel->scid != scid) {
+		debugf("unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", cmd->channel->scid, scid);
+		goto reject;
+	}
+
+	/*
+	 * Looks good. We got confirmation from our peer. Now process
+	 * it. First disable RTX timer. Then check the result and send
+	 * notification to the upper layer. If command timeout already
+	 * happened then ignore response.
+	 */
+
+	if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0)
+		return (error);
+
+	if (result == L2CAP_PENDING) {
+		/*
+		 * Our peer wants more time to complete connection. We shall
+		 * start ERTX timer and wait. Keep command in the list.
+		 */
+
+		cmd->channel->dcid = dcid;
+		btCoreData->TimeoutSignal(cmd, bluetooth_l2cap_ertx_timeout);
+
+		//INDICATION error = ng_l2cap_l2ca_con_rsp(cmd->channel, cmd->token,	result, status);
+		if (error != B_OK)
+			btCoreData->RemoveChannel(conn, cmd->channel->scid);
+
+	} else {
+
+		if (result == L2CAP_SUCCESS) {
+			/*
+			 * Channel is open. Complete command and move to CONFIG
+			 * state. Since we have sent positive confirmation we
+			 * expect to receive L2CA_Config request from the upper
+			 * layer protocol.
+			 */
+
+			cmd->channel->dcid = dcid;
+			cmd->channel->state = L2CAP_CHAN_CONFIG;
+		} else {
+			/* There was an error, so close the channel */
+			debugf("failed to open L2CAP channel, result=%d, status=%d\n", result, status);
+		}
+
+		error = l2cap_upper_con_rsp(conn, cmd->channel);
+
+		/* XXX do we have to remove the channel on error? */
+		if (error != 0 || result != L2CAP_SUCCESS)
+			btCoreData->RemoveChannel(conn, cmd->channel->scid);
+
+		btCoreData->AcknowledgeSignal(cmd);
+	}
+
+	return (error);
+
+reject:
+	/* Send reject. Do not really care about the result */
+	send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, scid, dcid);
+
+	return (0);
+}
+
+
+static option_status
+getNextSignalOption(net_buffer *nbuf, size_t *off, l2cap_cfg_opt_t *hdr, l2cap_cfg_opt_val_t *val)
+{
+	int	hint;
+	size_t	len = nbuf->size - (*off);
+
+	if (len == 0)
+		return (OPTION_NOT_PRESENT);
+	if (len < 0 || len < sizeof(*hdr))
+		return HEADER_TOO_SHORT;
+
+	gBufferModule->write(nbuf, *off, hdr, sizeof(*hdr));
+	*off += sizeof(*hdr);
+	len  -= sizeof(*hdr);
+
+	hint = L2CAP_OPT_HINT(hdr->type);
+	hdr->type &= L2CAP_OPT_HINT_MASK;
+
+	switch (hdr->type) {
+		case L2CAP_OPT_MTU:
+			if (hdr->length != L2CAP_OPT_MTU_SIZE || len < hdr->length)
+				return BAD_OPTION_LENGTH;
+
+			gBufferModule->write(nbuf, *off, val, L2CAP_OPT_MTU_SIZE);
+			val->mtu = le16toh(val->mtu);
+			*off += L2CAP_OPT_MTU_SIZE;
+		break;
+
+		case L2CAP_OPT_FLUSH_TIMO:
+			if (hdr->length != L2CAP_OPT_FLUSH_TIMO_SIZE ||
+			    len < hdr->length)
+				return BAD_OPTION_LENGTH;
+
+			gBufferModule->write(nbuf, *off, val, L2CAP_OPT_FLUSH_TIMO_SIZE);
+			val->flush_timo = le16toh(val->flush_timo);
+			*off += L2CAP_OPT_FLUSH_TIMO_SIZE;
+		break;
+
+		case L2CAP_OPT_QOS:
+			if (hdr->length != L2CAP_OPT_QOS_SIZE || len < hdr->length)
+				return BAD_OPTION_LENGTH;
+
+			gBufferModule->write(nbuf, *off, val, L2CAP_OPT_QOS_SIZE);
+			val->flow.token_rate = le32toh(val->flow.token_rate);
+			val->flow.token_bucket_size =	le32toh(val->flow.token_bucket_size);
+			val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth);
+			val->flow.latency = le32toh(val->flow.latency);
+			val->flow.delay_variation = le32toh(val->flow.delay_variation);
+			*off += L2CAP_OPT_QOS_SIZE;
+		break;
+
+		default:
+			if (hint)
+				*off += hdr->length;
+			else
+				return OPTION_UNKNOWN;
+		break;
+	}
+
+	return OPTION_PRESENT;
+}
+
+
+static status_t
+l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capChannel		*channel = NULL;
+	uint16  dcid;
+	uint16  respond;
+	uint16  result;
+
+	l2cap_cfg_opt_t	 hdr;
+	l2cap_cfg_opt_val_t	 val;
+
+	size_t			 off;
+	status_t		 error = 0;
+
+	/* Get command parameters */
+    NetBufferHeaderReader<l2cap_cfg_req_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	dcid = le16toh(command->dcid);
+	respond = L2CAP_OPT_CFLAG(le16toh(command->flags));
+
+
+	/* Check if we have this channel and it is in valid state */
+	channel = btCoreData->ChannelBySourceID(conn, dcid);
+	if (channel == NULL) {
+		debugf("unexpected L2CAP_ConfigReq command. Channel does not exist, cid=%d\n", dcid);
+		goto reject;
+	}
+
+	/* Verify channel state */
+	if (channel->state != L2CAP_CHAN_CONFIG && channel->state != L2CAP_CHAN_OPEN) {
+		debugf("unexpected L2CAP_ConfigReq. Invalid channel state, cid=%d, state=%d\n", dcid, channel->state);
+		goto reject;
+	}
+
+	if (channel->state == L2CAP_CHAN_OPEN) { /* Re-configuration */
+		channel->state = L2CAP_CHAN_CONFIG;
+	}
+
+    command.Remove(); // pull configuration header
+
+	for (result = 0, off = 0; ; ) {
+		error = getNextSignalOption(buffer, &off, &hdr, &val);
+		if (error == OPTION_NOT_PRESENT) { /* We done with this packet */
+			// TODO: configurations should have been pulled
+			break;
+		} else if (error > 0) { /* Got option */
+			switch (hdr.type) {
+			case L2CAP_OPT_MTU:
+				channel->omtu = val.mtu;
+				break;
+
+			case L2CAP_OPT_FLUSH_TIMO:
+				channel->flush_timo = val.flush_timo;
+				break;
+
+			case L2CAP_OPT_QOS:
+				memcpy(&val.flow, &channel->iflow, sizeof(channel->iflow));
+				break;
+
+			default: /* Ignore unknown hint option */
+				break;
+			}
+		} else { /* Oops, something is wrong */
+			respond = 1;
+
+			if (error == OPTION_UNKNOWN) {
+				// TODO: Remote to get the next possible option
+				/*m_adj(m, off - sizeof(hdr));
+				m->m_pkthdr.len = sizeof(hdr) + hdr.length;
+				result = L2CAP_UNKNOWN_OPTION;
+				*/
+			} else {
+				/* XXX FIXME Send other reject codes? */
+				gBufferModule->free(buffer);
+				result = L2CAP_REJECT;
+			}
+
+			break;
+		}
+	}
+
+	/*
+	 * Now check and see if we have to respond. If everything was OK then
+	 * respond contain "C flag" and (if set) we will respond with empty
+	 * packet and will wait for more options.
+	 *
+	 * Other case is that we did not like peer's options and will respond
+	 * with L2CAP_Config response command with Reject error code.
+	 *
+	 * When "respond == 0" than we have received all options and we will
+	 * sent L2CA_ConfigInd event to the upper layer protocol.
+	 */
+
+	if (respond) {
+		error = send_l2cap_cfg_rsp(conn, ident, channel->dcid, result, buffer);
+		if (error != 0) {
+			//INDICATION ng_l2cap_l2ca_discon_ind(ch);
+			btCoreData->RemoveChannel(conn, channel->scid);
+		}
+	} else {
+		/* Send L2CA_ConfigInd event to the upper layer protocol */
+		//TODO: channel->ident = ident;
+		//INDICATION error = ng_l2cap_l2ca_cfg_ind(ch);
+		if (error != 0)
+			btCoreData->RemoveChannel(conn, channel->scid);
+	}
+
+	return error;
+
+reject:
+	/* Send reject. Do not really care about the result */
+	gBufferModule->free(buffer);
+
+	send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, 0, dcid);
+
+	return B_OK;
+}
+
+
+/* Process L2CAP_ConfigRsp command */
+static status_t
+l2cap_process_cfg_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capFrame     *cmd = NULL;
+	uint16  		 scid, cflag, result;
+
+	l2cap_cfg_opt_t	 hdr;
+	l2cap_cfg_opt_val_t	 val;
+
+	size_t			 off;
+	status_t		 error = 0;
+
+    NetBufferHeaderReader<l2cap_cfg_rsp_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	scid = le16toh(command->scid);
+	cflag = L2CAP_OPT_CFLAG(le16toh(command->flags));
+	result = le16toh(command->result);
+
+	/* Check if we have this command */
+	cmd = btCoreData->SignalByIdent(conn, ident);
+	if (cmd == NULL) {
+		debugf("unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", ident, conn->handle);
+		gBufferModule->free(buffer);
+		return ENOENT;
+	}
+
+	/* Verify CIDs and send reject if does not match */
+	if (cmd->channel->scid != scid) {
+		debugf("unexpected L2CAP_ConfigRsp.Channel ID does not match, scid=%d(%d)\n", cmd->channel->scid,	scid);
+		goto reject;
+	}
+
+	/* Verify channel state and reject if invalid */
+	if (cmd->channel->state != L2CAP_CHAN_CONFIG) {
+		debugf("unexpected L2CAP_ConfigRsp. Invalid channel state, scid=%d, state=%d\n", cmd->channel->scid,	cmd->channel->state);
+		goto reject;
+	}
+
+	/*
+	 * Looks like it is our response, so process it. First parse options,
+	 * then verify C flag. If it is set then we shall expect more
+	 * configuration options from the peer and we will wait. Otherwise we
+	 * have received all options and we will send L2CA_ConfigRsp event to
+	 * the upper layer protocol. If command timeout already happened then
+	 * ignore response.
+	 */
+
+	if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) {
+		gBufferModule->free(buffer);
+		return error;
+	}
+
+	for (off = 0; ; ) {
+		error = getNextSignalOption(buffer, &off, &hdr, &val);
+		// TODO: pull the option
+
+		if (error == OPTION_NOT_PRESENT) /* We done with this packet */
+			break;
+		else if (error > 0) { /* Got option */
+			switch (hdr.type) {
+			case L2CAP_OPT_MTU:
+				cmd->channel->imtu = val.mtu;
+			break;
+
+			case L2CAP_OPT_FLUSH_TIMO:
+				cmd->channel->flush_timo = val.flush_timo;
+				break;
+
+			case L2CAP_OPT_QOS:
+				memcpy(&val.flow, &cmd->channel->oflow, sizeof(cmd->channel->oflow));
+			break;
+
+			default: /* Ignore unknown hint option */
+				break;
+			}
+		} else {
+			/*
+			 * XXX FIXME What to do here?
+			 *
+			 * This is really BAD :( options packet was broken, or
+			 * peer sent us option that we did not understand. Let
+			 * upper layer know and do not wait for more options.
+			 */
+
+			debugf("failed to parse configuration options, error=%ld\n", error);
+            //INDICATION
+			result = L2CAP_UNKNOWN;
+			cflag = 0;
+
+			break;
+		}
+	}
+
+	command.Remove();
+
+	if (cflag) /* Restart timer and wait for more options */
+		btCoreData->TimeoutSignal(cmd, bluetooth_l2cap_rtx_timeout);
+	else {
+		/* Send L2CA_Config response to the upper layer protocol */
+		//INDICATION error = ng_l2cap_l2ca_cfg_rsp(cmd->channel, cmd->token, result);
+		if (error != 0) {
+			/*
+			 * XXX FIXME what to do here? we were not able to send
+			 * response to the upper layer protocol, so for now
+			 * just close the channel. Send L2CAP_Disconnect to
+			 * remote peer?
+			 */
+
+			debugf("failed to send L2CA_Config response, error=%ld\n", error);
+
+			btCoreData->RemoveChannel(conn, cmd->channel->scid);
+		}
+
+		btCoreData->AcknowledgeSignal(cmd);
+	}
+
+	return (error);
+
+reject:
+	/* Send reject. Do not really care about the result */
+	gBufferModule->free(buffer);
+	send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, scid, 0);
+
+	return B_OK;
+}
+
+
+/* Process L2CAP_DisconnectReq command */
+static status_t
+l2cap_process_discon_req(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capChannel *ch = NULL;
+	L2capFrame *cmd = NULL;
+	uint16 scid;
+	uint16 dcid;
+
+	NetBufferHeaderReader<l2cap_discon_req_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	dcid = le16toh(command->dcid);
+	scid = le16toh(command->scid);
+
+	command.Remove();
+// NG_FREE_M(con->rx_pkt);
+
+	/* Check if we have this channel and it is in valid state */
+	ch = btCoreData->ChannelBySourceID(conn, dcid);
+	if (ch == NULL) {
+		debugf("unexpected L2CAP_DisconnectReq message.Channel does not exist, cid=%d\n", dcid);
+		goto reject;
+	}
+
+	/* XXX Verify channel state and reject if invalid -- is that true? */
+	if (ch->state != L2CAP_CHAN_OPEN && ch->state != L2CAP_CHAN_CONFIG &&
+		ch->state != L2CAP_CHAN_W4_L2CAP_DISCON_RSP) {
+		debugf("unexpected L2CAP_DisconnectReq. Invalid channel state, cid=%d, state=%d\n", dcid, ch->state);
+		goto reject;
+	}
+
+	/* Match destination channel ID */
+	if (ch->dcid != scid || ch->scid != dcid) {
+		debugf("unexpected L2CAP_DisconnectReq. Channel IDs does not match, channel: scid=%d, dcid=%d,
+		request: scid=%d, dcid=%d\n", ch->scid, ch->dcid, scid, dcid);
+		goto reject;
+	}
+
+	/*
+	 * Looks good, so notify upper layer protocol that channel is about
+	 * to be disconnected and send L2CA_DisconnectInd message. Then respond
+	 * with L2CAP_DisconnectRsp.
+	 */
+
+	if (ch->state != L2CAP_CHAN_W4_L2CAP_DISCON_RSP) {
+		//INDICATION ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
+		btCoreData->RemoveChannel(conn, ch->scid);
+	}
+
+	/* Send L2CAP_DisconnectRsp */
+	cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_DISCON_RSP);
+	if (cmd == NULL)
+		return ENOMEM;
+
+	cmd->buffer = l2cap_discon_rsp(ident, dcid, scid);
+	if (cmd->buffer == NULL) {
+		btCoreData->AcknowledgeSignal(cmd);
+		return ENOBUFS;
+	}
+
+	/* Link command to the queue */
+	//TODO l2cap_sched(conn);
+
+	return B_OK;
+
+reject:
+	/* Send reject. Do not really care about the result */
+	send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, scid, dcid);
+
+	return B_OK;
+}
+
+
+/* Process L2CAP_DisconnectRsp command */
+static status_t
+l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer *buffer)
+{
+	L2capFrame *cmd = NULL;
+	int16 scid, dcid;
+	status_t error = 0;
+
+	/* Get command parameters */
+	NetBufferHeaderReader<l2cap_discon_rsp_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	dcid = le16toh(command->dcid);
+	scid = le16toh(command->scid);
+
+	command.Remove();
+	//? NG_FREE_M(con->rx_pkt);
+
+	/* Check if we have pending command descriptor */
+	cmd = btCoreData->SignalByIdent(conn, ident);
+	if (cmd == NULL) {
+		debugf("unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", ident, conn->handle);
+		goto out;
+	}
+
+	/* Verify channel state, do nothing if invalid */
+	if (cmd->channel->state != L2CAP_CHAN_W4_L2CAP_DISCON_RSP) {
+		debugf("unexpected L2CAP_DisconnectRsp. Invalid channel state, cid=%d, state=%d\n", scid, cmd->channel->state);
+		goto out;
+	}
+
+	/* Verify CIDs and send reject if does not match */
+	if (cmd->channel->scid != scid || cmd->channel->dcid != dcid) {
+		debugf("unexpected L2CAP_DisconnectRsp. Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n",
+			cmd->channel->scid, scid, cmd->channel->dcid, dcid);
+		goto out;
+	}
+
+	/*
+	* Looks like we have successfuly disconnected channel, so notify
+	* upper layer. If command timeout already happened then ignore
+	* response.
+	*/
+
+	if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0)
+		goto out;
+
+	//INDICATION = ng_l2cap_l2ca_discon_rsp(cmd->channel, cmd->token, NG_L2CAP_SUCCESS);
+	btCoreData->RemoveChannel(conn, scid); /* this will free commands too */
+
+out:
+	return (error);
+}
+
+
+static status_t
+l2cap_process_echo_req(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capFrame      *cmd = NULL;
+
+	cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_ECHO_RSP);
+	if (cmd == NULL) {
+		gBufferModule->free(buffer);
+		return ENOMEM;
+	}
+
+	cmd->buffer = l2cap_echo_req(ident, NULL, 0);
+
+	/* Attach data and link command to the queue */
+	//TODO l2cap_sched(conn);
+
+	return B_OK;
+}
+
+
+/* Process L2CAP_EchoRsp command */
+static status_t
+l2cap_process_echo_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capFrame	*cmd = NULL;
+	status_t    error = 0;
+
+	/* Check if we have this command */
+	cmd = btCoreData->SignalByIdent(conn, ident);
+	if (cmd != NULL) {
+		/* If command timeout already happened then ignore response */
+		if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) {
+			return error;
+		}
+
+
+		//INDICATION error = ng_l2cap_l2ca_ping_rsp(cmd->conn, cmd->token, L2CAP_SUCCESS, conn->rx_pkt);
+		btCoreData->AcknowledgeSignal(cmd);
+
+	} else {
+		debugf("unexpected L2CAP_EchoRsp command. Requested ident does not exist, ident=%d\n", ident);
+		gBufferModule->free(buffer);
+		error = B_ERROR;
+	}
+
+	return error;
+}
+
+
+/* Process L2CAP_InfoReq command */
+static status_t
+l2cap_process_info_req(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capFrame	*cmd = NULL;
+	uint16      type;
+
+	/* Get command parameters */
+    NetBufferHeaderReader<l2cap_info_req_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	//command->type = le16toh(mtod(conn->rx_pkt, ng_l2cap_info_req_cp *)->type);
+    type = command->type;
+
+	command.Remove();
+
+	cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_INFO_RSP);
+	if (cmd == NULL)
+		return ENOMEM;
+
+	switch (type) {
+	    case L2CAP_CONNLESS_MTU:
+		    cmd->buffer = l2cap_info_rsp(ident, L2CAP_CONNLESS_MTU, L2CAP_SUCCESS, L2CAP_MTU_DEFAULT);
+		break;
+
+	    default:
+		    cmd->buffer = l2cap_info_rsp(ident, type, L2CAP_NOT_SUPPORTED, 0);
+		break;
+	}
+
+	if (cmd->buffer == NULL) {
+		btCoreData->AcknowledgeSignal(cmd);
+		return ENOBUFS;
+	}
+
+	/* Link command to the queue */
+	//TODO l2cap_sched(conn);
+
+	return B_OK;
+}
+
+
+/* Process L2CAP_InfoRsp command */
+static status_t
+l2cap_process_info_rsp(HciConnection *conn, uint8 ident, net_buffer* buffer)
+{
+	l2cap_info_rsp_cp   *cp = NULL;
+	L2capFrame         *cmd = NULL;
+	status_t            error = B_OK;
+
+	/* Get command parameters */
+    NetBufferHeaderReader<l2cap_info_rsp_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	command->type = le16toh(command->type);
+	command->result = le16toh(command->result);
+
+	command.Remove();
+	
+	/* Check if we have pending command descriptor */
+	cmd = btCoreData->SignalByIdent(conn, ident);
+	if (cmd == NULL) {
+		debugf("unexpected L2CAP_InfoRsp command. Requested ident does not exist, ident=%d\n", ident);
+		gBufferModule->free(buffer);
+		return ENOENT;
+	}
+
+	/* If command timeout already happened then ignore response */
+	if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) {
+		gBufferModule->free(buffer);
+		return error;
+	}
+
+	if (command->result == L2CAP_SUCCESS) {
+		switch (command->type) {
+
+    		case L2CAP_CONNLESS_MTU:
+			/*
+				// TODO: Check specs ??
+	    	    if (conn->rx_pkt->m_pkthdr.len == sizeof(uint16)) {
+				    *mtod(conn->rx_pkt, uint16 *) = le16toh(*mtod(conn->rx_pkt,uint16 *));
+			    } else {
+				    cp->result = L2CAP_UNKNOWN;  XXX
+				    debugf("invalid L2CAP_InfoRsp command. Bad connectionless MTU parameter, len=%d\n", conn->rx_pkt->m_pkthdr.len);
+			    }*/
+			break;
+
+		default:
+			debugf("invalid L2CAP_InfoRsp command. Unknown info type=%d\n", cp->type);
+			break;
+		}
+	}
+
+	//INDICATION error = ng_l2cap_l2ca_get_info_rsp(cmd->conn, cmd->token, cp->result, conn->rx_pkt);
+	btCoreData->AcknowledgeSignal(cmd);
+
+	return error;
+}
+
+
+static status_t
+l2cap_process_cmd_rej(HciConnection *conn, uint8 ident, net_buffer *buffer)
+{
+	L2capFrame		*cmd = NULL;
+
+	/* TODO: review this command... Get command data */
+	NetBufferHeaderReader<l2cap_cmd_rej_cp> command(buffer);
+	status_t status = command.Status();
+	if (status < B_OK) {
+		return ENOBUFS;
+	}
+
+	command->reason = le16toh(command->reason);
+	
+	command.Remove();
+	
+	/* Check if we have pending command descriptor */
+	cmd = btCoreData->SignalByIdent(conn, ident);
+	if (cmd != NULL) {
+		/* If command timeout already happened then ignore reject */
+		if (btCoreData->UnTimeoutSignal(cmd) != 0) {
+			gBufferModule->free(buffer);
+			return ETIMEDOUT;
+		}
+
+
+		switch (cmd->code) {
+			case L2CAP_CON_REQ:
+				//INDICATION l2cap_l2ca_con_rsp(cmd->channel, cmd->token, cp->reason, 0);
+				btCoreData->RemoveChannel(conn, cmd->channel->scid);
+			break;
+
+			case L2CAP_CFG_REQ:
+				//INDICATION l2cap_l2ca_cfg_rsp(cmd->channel, cmd->token, cp->reason);
+			break;
+
+			case L2CAP_DISCON_REQ:
+				//INDICATION l2cap_l2ca_discon_rsp(cmd->channel,cmd->token,cp->reason);
+				btCoreData->RemoveChannel(conn, cmd->channel->scid);
+			break;
+
+			case L2CAP_ECHO_REQ:
+				//INDICATION l2cap_l2ca_ping_rsp(cmd->con, cmd->token, cp->reason, NULL);
+			break;
+
+			case L2CAP_INFO_REQ:
+				//INDICATION l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, cp->reason, NULL);
+			break;
+
+		default:
+			debugf("unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", cmd->code);
+			break;
+		}
+
+		btCoreData->AcknowledgeSignal(cmd);
+
+	} else
+		debugf("unexpected L2CAP_CommandRej command. Requested ident does not exist, ident=%d\n", ident);
+
+	return B_OK;
+}
+
+
+#if 0
+#pragma mark - Queuing Outgoing signals
+#endif
+
+
+status_t
+send_l2cap_reject(HciConnection *conn, uint8 ident, uint16 reason,
+		uint16 mtu, uint16 scid, uint16 dcid)
+{
+	L2capFrame	*cmd = NULL;
+
+	cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_CMD_REJ);
+	if (cmd == NULL)
+		return ENOMEM;
+
+	cmd->buffer = l2cap_cmd_rej(cmd->ident, reason, mtu, scid, dcid);
+	if (cmd->buffer == NULL) {
+		btCoreData->AcknowledgeSignal(cmd);
+		return ENOBUFS;
+	}
+
+	/* Link command to the queue */
+	//TODO l2cap_sched(conn);
+
+	return B_OK;
+}
+
+
+status_t
+send_l2cap_con_rej(HciConnection *conn, uint8 ident, uint16 scid, uint16 dcid, uint16 result)
+{
+	L2capFrame	*cmd = NULL;
+
+	cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_CON_RSP);
+	if (cmd == NULL)
+		return ENOMEM;
+
+	cmd->buffer = l2cap_con_rsp(cmd->ident, dcid, scid, result, 0);
+	if (cmd->buffer == NULL) {
+		btCoreData->AcknowledgeSignal(cmd);
+		return ENOBUFS;
+	}
+
+	/* Link command to the queue */
+	//TODO l2cap_sched(conn);
+
+	return B_OK;
+}
+
+
+status_t
+send_l2cap_cfg_rsp(HciConnection *conn, uint8 ident, uint16 scid, uint16 result, net_buffer *opt)
+{
+	L2capFrame	*cmd = NULL;
+
+	cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_CFG_RSP);
+	if (cmd == NULL) {
+		gBufferModule->free(opt);
+
+		return ENOMEM;
+	}
+
+	cmd->buffer = l2cap_cfg_rsp(cmd->ident, scid, 0, result, opt);
+	if (cmd->buffer == NULL) {
+		btCoreData->AcknowledgeSignal(cmd);
+		return ENOBUFS;
+	}
+
+	/* Link command to the queue */
+	//TODO l2cap_sched(conn);
+
+	return B_OK;
+}
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.h b/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.h
new file mode 100644
index 0000000000..7190738692
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.h
@@ -0,0 +1,26 @@
+/* 
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+ * All rights reserved. Distributed under the terms of the MIT License.
+ */
+#ifndef L2CAP_SIGNAL_H
+#define L2CAP_SIGNAL_H
+
+// Processing signals
+status_t l2cap_process_signal_cmd(HciConnection* conn, net_buffer* buffer);
+
+// Queuing commands
+
+
+#if 0
+#pragma - Signals Responses
+#endif
+
+status_t
+send_l2cap_reject(HciConnection *conn, uint8 ident, uint16 reason, uint16 mtu, uint16 scid, uint16 dcid);
+status_t
+send_l2cap_con_rej(HciConnection *conn, uint8 ident, uint16 scid, uint16 dcid, uint16 result);
+status_t
+send_l2cap_cfg_rsp(HciConnection *conn, uint8 ident, uint16 scid, uint16 result, net_buffer *opt);
+
+
+#endif
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.cpp
new file mode 100644
index 0000000000..60a252d12c
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.cpp
@@ -0,0 +1,64 @@
+/*
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+ * All rights reserved. Distributed under the terms of the MIT License.
+ *
+ */
+
+#include <KernelExport.h>
+
+#include <NetBufferUtilities.h>
+
+
+#include <l2cap.h>
+#include "l2cap_internal.h"
+#include "l2cap_signal.h"
+#include "l2cap_command.h"
+#include "l2cap_lower.h"
+
+#include "L2capEndpoint.h"
+
+#define BT_DEBUG_THIS_MODULE
+#define SUBMODULE_NAME "upper"
+#define SUBMODULE_COLOR 36
+#include <btDebug.h>
+
+
+status_t
+l2cap_l2ca_con_ind(L2capChannel* channel)
+{
+	L2capEndpoint* endpoint = L2capEndpoint::ForPsm(channel->psm);
+	
+	if (endpoint == NULL) { //refuse connection no endpoint bound
+		debugf("No endpoint bound for psm %d\n", channel->psm);
+		return B_ERROR;
+	}
+
+	channel->endpoint = endpoint;
+	debugf("Endpoint %p bound for psm %d, schannel %x dchannel %x\n",endpoint, channel->psm, channel->scid, channel->dcid);
+	
+	
+	L2capFrame	*cmd = NULL;
+
+	cmd = btCoreData->SpawnSignal(channel->conn, channel, NULL, channel->ident, L2CAP_CON_RSP);
+	if (cmd == NULL)
+		return ENOMEM;
+
+	cmd->buffer = l2cap_con_rsp(cmd->ident, channel->dcid, channel->scid, L2CAP_SUCCESS, L2CAP_NO_INFO);
+	if (cmd->buffer == NULL) {
+		btCoreData->AcknowledgeSignal(cmd);
+		return ENOBUFS;
+	}
+
+	/* Link command to the queue */
+	SchedConnectionPurgeThread(channel->conn);
+	return B_OK;
+}
+
+
+status_t
+l2cap_upper_con_rsp(HciConnection* conn, L2capChannel* channel)
+{
+	flowf("\n");
+
+	return B_OK;
+}
diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h b/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h
new file mode 100644
index 0000000000..4d74b20b3b
--- /dev/null
+++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h
@@ -0,0 +1,13 @@
+/* 
+ * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
+ * All rights reserved. Distributed under the terms of the MIT License.
+ */
+#ifndef L2CAP_UPPER_H
+#define L2CAP_UPPER_H
+
+#include "l2cap_internal.h"
+
+status_t l2cap_l2ca_con_ind(L2capChannel* channel);
+status_t l2cap_upper_con_rsp(HciConnection* conn, L2capChannel* channel);
+
+#endif