From 7cc7cada6a78b0f0a38da5dea553de57de73b899 Mon Sep 17 00:00:00 2001 From: Oliver Ruiz Dorantes Date: Thu, 27 Nov 2008 19:12:33 +0000 Subject: [PATCH] - Implement signaling req/rsp - Deliver Connection Oriented packets - Complete the l2cap upper layers - Basic interaction with sockets - Implement thead at this layer for TX git-svn-id: file:///srv/svn/repos/haiku/haiku/trunk@28738 a95241bf-73f2-0310-859d-f6bbb57e9c96 --- .../network/protocols/l2cap/L2capEndpoint.cpp | 49 +++- .../network/protocols/l2cap/L2capEndpoint.h | 18 +- .../kernel/network/protocols/l2cap/l2cap.cpp | 32 +-- .../network/protocols/l2cap/l2cap_internal.h | 4 +- .../network/protocols/l2cap/l2cap_lower.cpp | 73 ++++- .../network/protocols/l2cap/l2cap_signal.cpp | 264 +++++++++--------- .../network/protocols/l2cap/l2cap_signal.h | 4 - .../network/protocols/l2cap/l2cap_upper.cpp | 106 ++++++- .../network/protocols/l2cap/l2cap_upper.h | 4 + 9 files changed, 353 insertions(+), 201 deletions(-) diff --git a/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp index 751ad60a81..5f4818d6d4 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp +++ b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.cpp @@ -1,19 +1,15 @@ #include "L2capEndpoint.h" - #include #include #include - -#include "l2cap_internal.h" #include "l2cap_address.h" #include #define BT_DEBUG_THIS_MODULE #include - static inline bigtime_t absolute_timeout(bigtime_t timeout) { @@ -33,7 +29,19 @@ L2capEndpoint::L2capEndpoint(net_socket* socket) { debugf("[%ld] %p->L2capEndpoint::L2capEndpoint()\n", find_thread(NULL), this); + /* Set MTU and flow control settings to defaults */ + configuration.imtu = L2CAP_MTU_DEFAULT; + memcpy(&configuration.iflow, &default_qos , sizeof(l2cap_flow_t) ); + configuration.omtu = L2CAP_MTU_DEFAULT; + memcpy(&configuration.oflow, &default_qos , sizeof(l2cap_flow_t) ); + + configuration.flush_timo = L2CAP_FLUSH_TIMO_DEFAULT; + configuration.link_timo = L2CAP_LINK_TIMO_DEFAULT; + + configurationSet = false; + + gStackModule->init_fifo(&fReceivingFifo, "l2cap recvfifo", L2CAP_MTU_DEFAULT); } @@ -162,7 +170,7 @@ 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, + debugf("[%ld] %p->L2capEndpoint::Connect(\"%s\")\n", find_thread(NULL), this, ConstSocketAddress(&gL2cap4AddressModule, _address).AsString().Data()); const sockaddr_l2cap* address = (const sockaddr_l2cap*)_address; @@ -177,7 +185,7 @@ L2capEndpoint::Connect(const struct sockaddr *_address) status_t L2capEndpoint::Accept(net_socket **_acceptedSocket) { - debugf("[%ld] %p->UnixEndpoint::Accept()\n", find_thread(NULL), this); + debugf("[%ld] %p->L2capEndpoint::Accept()\n", find_thread(NULL), this); bigtime_t timeout = absolute_timeout(socket->receive.timeout); @@ -190,7 +198,7 @@ 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), + debugf("[%ld] %p->L2capEndpoint::Send(%p, %ld, %p)\n", find_thread(NULL), this, vecs, vecCount, ancillaryData); return B_OK; @@ -202,7 +210,7 @@ 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), + debugf("[%ld] %p->L2capEndpoint::Receive(%p, %ld)\n", find_thread(NULL), this, vecs, vecCount); @@ -210,10 +218,19 @@ L2capEndpoint::Receive(const iovec *vecs, size_t vecCount, } +ssize_t +L2capEndpoint::ReadData(size_t numBytes, uint32 flags, net_buffer** _buffer) +{ + + return gStackModule->fifo_dequeue_buffer(&fReceivingFifo, flags, B_INFINITE_TIMEOUT, _buffer); + +} + + ssize_t L2capEndpoint::Sendable() { - debugf("[%ld] %p->UnixEndpoint::Sendable()\n", find_thread(NULL), this); + debugf("[%ld] %p->L2capEndpoint::Sendable()\n", find_thread(NULL), this); return 0; @@ -223,7 +240,7 @@ L2capEndpoint::Sendable() ssize_t L2capEndpoint::Receivable() { - debugf("[%ld] %p->UnixEndpoint::Receivable()\n", find_thread(NULL), this); + debugf("[%ld] %p->L2capEndpoint::Receivable()\n", find_thread(NULL), this); return 0; } @@ -242,10 +259,20 @@ L2capEndpoint::ForPsm(uint16 psm) endpoint = iterator.Next(); if (((struct sockaddr_l2cap*)&endpoint->socket->address)->l2cap_psm == psm && endpoint->fState == LISTEN) { - // TODO endpoint ocupied, lock it! + // TODO endpoint ocupied, lock it! define a channel for it return endpoint; } } return NULL; } + + +void +L2capEndpoint::BindToChannel(L2capChannel* channel) +{ + + this->channel = channel; + channel->configuration = &configuration; + +} diff --git a/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h index 54d77a9c37..83c42c0e3f 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h +++ b/src/add-ons/kernel/network/protocols/l2cap/L2capEndpoint.h @@ -12,6 +12,9 @@ #include #include +#include "l2cap_internal.h" + +extern net_stack_module_info* gStackModule; class L2capEndpoint : public net_protocol, public ProtocolSocket, @@ -50,7 +53,7 @@ public: ssize_t Receive(const iovec *vecs, size_t vecCount, ancillary_data_container **_ancillaryData, struct sockaddr *_address, socklen_t *_addressLength); - + ssize_t ReadData(size_t numBytes, uint32 flags, net_buffer** _buffer); ssize_t Sendable(); ssize_t Receivable(); @@ -59,8 +62,16 @@ public: status_t Shutdown(int direction); + void BindToChannel(L2capChannel* channel); + static L2capEndpoint* ForPsm(uint16 psm); + ChannelConfiguration configuration; + L2capChannel* channel; + + bool configurationSet; + net_fifo fReceivingFifo; + private: typedef enum { // establishing a connection @@ -80,13 +91,12 @@ private: CLOSING, TIME_WAIT } State; - -private: + mutex fLock; State fState; L2capEndpoint* fPeerEndpoint; sem_id fAcceptSemaphore; - + }; diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp index 7bda9f93e9..11885cf8b0 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp +++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap.cpp @@ -37,7 +37,6 @@ #include "L2capEndpoint.h" #include -#include #include #define BT_DEBUG_THIS_MODULE @@ -49,20 +48,13 @@ typedef NetBufferField AclLenField; DoublyLinkedList 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; -net_stack_module_info *sStackModule; +net_stack_module_info *gStackModule; net_socket_module_info *gSocketModule; static struct net_domain *sDomain; @@ -167,7 +159,9 @@ l2cap_setsockopt(net_protocol *protocol, int level, int option, const void *value, int length) { flowf("\n"); - + + ((L2capEndpoint*)protocol)->configurationSet = true; + /* return protocol->next->module->setsockopt(protocol->next, level, option, value, length); */ return EOPNOTSUPP; } @@ -238,8 +232,8 @@ l2cap_read_data(net_protocol *protocol, size_t numBytes, uint32 flags, net_buffer **_buffer) { flowf("\n"); - - return B_ERROR; + + return ((L2capEndpoint*)protocol)->ReadData(numBytes, flags, _buffer); } @@ -315,20 +309,20 @@ l2cap_std_ops(int32 op, ...) switch (op) { case B_MODULE_INIT: { - error = sStackModule->register_domain_protocols(AF_BLUETOOTH, SOCK_STREAM, BLUETOOTH_PROTO_L2CAP, + error = gStackModule->register_domain_protocols(AF_BLUETOOTH, SOCK_STREAM, BLUETOOTH_PROTO_L2CAP, "network/protocols/l2cap/v1", NULL); if (error != B_OK) { return error; } - error = sStackModule->register_domain_receiving_protocol(AF_BLUETOOTH, BLUETOOTH_PROTO_L2CAP, + error = gStackModule->register_domain_receiving_protocol(AF_BLUETOOTH, BLUETOOTH_PROTO_L2CAP, "network/protocols/l2cap/v1"); if (error != B_OK) { return error; } - error = sStackModule->register_domain(AF_BLUETOOTH, "l2cap", &gL2CAPModule, + error = gStackModule->register_domain(AF_BLUETOOTH, "l2cap", &gL2CAPModule, &gL2cap4AddressModule, &sDomain); if (error != B_OK) { return error; @@ -344,8 +338,8 @@ l2cap_std_ops(int32 op, ...) case B_MODULE_UNINIT: error = QuitConnectionPurgeThread(); - - sStackModule->unregister_domain(sDomain); + gStackModule->unregister_domain(sDomain); + return B_OK; default: @@ -390,7 +384,7 @@ net_protocol_module_info gL2CAPModule = { }; module_dependency module_dependencies[] = { - {NET_STACK_MODULE_NAME, (module_info **)&sStackModule}, + {NET_STACK_MODULE_NAME, (module_info **)&gStackModule}, {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_internal.h b/src/add-ons/kernel/network/protocols/l2cap/l2cap_internal.h index 4260a365ec..53de22f093 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_internal.h +++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_internal.h @@ -5,10 +5,12 @@ #ifndef L2CAP_INTERNAL_H #define L2CAP_INTERNAL_H -#include +#include #include #include +extern l2cap_flow_t default_qos; + extern bluetooth_core_data_module_info *btCoreData; extern net_buffer_module_info *gBufferModule; extern net_stack_module_info *gStackModule; diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp index 1163965bee..87dee50209 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp +++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_lower.cpp @@ -23,9 +23,12 @@ #include +#include #include + #include "l2cap_internal.h" #include "l2cap_signal.h" +#include "l2cap_upper.h" #define BT_DEBUG_THIS_MODULE #define SUBMODULE_NAME "lower" @@ -75,10 +78,11 @@ l2cap_receive(HciConnection* conn, net_buffer* buffer) case L2CAP_CLT_CID: /* Connectionless packet error = l2cap_cl_receive(buffer);*/ + flowf("CL FRAME!!\n"); break; - default: /* Data packet - error = l2cap_co_receive(buffer);*/ + default: /* Data packet */ + error = l2cap_co_receive(conn, buffer, dcid); break; } @@ -86,8 +90,11 @@ l2cap_receive(HciConnection* conn, net_buffer* buffer) } + +struct net_device_module_info* btDevices = NULL; + #if 0 -#pragma - thread conn sched - +#pragma mark - thread conn sched - #endif static thread_id sConnectionThread; @@ -101,21 +108,57 @@ purge_connection(HciConnection* conn) debugf("handle=%d\n",conn->handle); - DoublyLinkedQueue::Iterator iterator = conn->OutGoingFrames.GetIterator(); - while (iterator.HasNext()) { - - frame = iterator.Next(); + frame = conn->OutGoingFrames.RemoveHead(); +// while ( frame != NULL) { + // Here is the place to decide how many l2cap signals we want to have + // per l2cap packet. 1 ATM 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); - } + // Add the l2cap header + if (frame->buffer == NULL) + panic("Malformed frame in ongoing queue"); + + NetBufferPrepend bufferHeader(frame->buffer); + status_t status = bufferHeader.Status(); + if (status < B_OK) { + // free the buffer +//! continue; + } + + // fill + bufferHeader->length = htole16(frame->buffer->size - sizeof(l2cap_hdr_t)); + switch (frame->type) { + case L2CAP_C_FRAME: + bufferHeader->dcid = L2CAP_SIGNAL_CID; + break; + case L2CAP_G_FRAME: + bufferHeader->dcid = L2CAP_CLT_CID; + break; + default: + bufferHeader->dcid = frame->channel->dcid; + break; + + } + + bufferHeader.Sync(); + + if (btDevices == NULL) + if (get_module(NET_BLUETOOTH_DEVICE_NAME,(module_info**)&btDevices) != B_OK) { + panic("l2cap: cannot get dev module"); + } // TODO: someone put it + + + debugf("dev %p frame %p tolower\n", conn->ndevice, frame->buffer); + + frame->buffer->type = conn->handle; + btDevices->send_data(conn->ndevice, frame->buffer); + +// frame = conn->OutGoingFrames.RemoveHead(); +// } } @@ -140,13 +183,13 @@ connection_thread(void *) if (ssizePort <= 0) { debugf("Error %s\n", strerror(ssizePort)); - snooze(1*1000*1000); + snooze(500*1000); continue; } if (ssizePort > (ssize_t) sizeof(conn)) { debugf("Message too big %ld\n", ssizePort); - snooze(1*1000*1000); + snooze(500*1000); continue; } @@ -154,7 +197,7 @@ connection_thread(void *) if (ssizeRead != ssizePort) { debugf("Missmatch size port=%ld read=%ld\n", ssizePort, ssizeRead); - snooze(1*1000*1000); + snooze(500*1000); continue; } diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp b/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp index 237e66077b..6ce14bca28 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp +++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.cpp @@ -15,7 +15,6 @@ * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. */ - #include #include @@ -27,12 +26,14 @@ #include "l2cap_signal.h" #include "l2cap_command.h" #include "l2cap_upper.h" +#include "l2cap_lower.h" #define BT_DEBUG_THIS_MODULE #define SUBMODULE_NAME "signal" #define SUBMODULE_COLOR 36 #include +#include typedef enum _option_status { OPTION_NOT_PRESENT = 0, @@ -43,38 +44,49 @@ typedef enum _option_status { } option_status; -static status_t -l2cap_process_con_req(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_flow_t default_qos = { + /* flags */ 0x0, + /* service_type */ HCI_SERVICE_TYPE_BEST_EFFORT, + /* token_rate */ 0xffffffff, /* maximum */ + /* token_bucket_size */ 0xffffffff, /* maximum */ + /* peak_bandwidth */ 0x00000000, /* maximum */ + /* latency */ 0xffffffff, /* don't care */ + /* delay_variation */ 0xffffffff /* don't care */ +}; + static status_t -l2cap_process_con_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_process_con_req(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_process_con_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_cfg_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_process_cfg_req(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_discon_req(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_process_cfg_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer *buffer); +l2cap_process_discon_req(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_echo_req(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_echo_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_process_echo_req(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_info_req(HciConnection *conn, uint8 ident, net_buffer *buffer); +l2cap_process_echo_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_info_rsp(HciConnection *conn, uint8 ident, net_buffer* buffer); +l2cap_process_info_req(HciConnection* conn, uint8 ident, net_buffer* buffer); static status_t -l2cap_process_cmd_rej(HciConnection *conn, uint8 ident, net_buffer *buffer); +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); /* @@ -89,7 +101,7 @@ status_t l2cap_process_signal_cmd(HciConnection* conn, net_buffer* buffer) { net_buffer* m = buffer; - flowf("\n"); + debugf("Signal size=%ld\n", buffer->size); while (m != NULL) { @@ -198,10 +210,10 @@ l2cap_process_signal_cmd(HciConnection* conn, net_buffer* buffer) /* Process L2CAP_ConnectReq command */ static status_t -l2cap_process_con_req(HciConnection *conn, uint8 ident, net_buffer *buffer) +l2cap_process_con_req(HciConnection* conn, uint8 ident, net_buffer* buffer) { - L2capChannel* ch; - uint16 dcid, psm; + L2capChannel* ch; + uint16 dcid, psm; /* Get con req data */ NetBufferHeaderReader command(buffer); @@ -232,7 +244,7 @@ l2cap_process_con_req(HciConnection *conn, uint8 ident, net_buffer *buffer) status_t indicationStatus = l2cap_l2ca_con_ind(ch); if ( indicationStatus == B_OK ) { - ch->state = L2CAP_CHAN_W4_L2CA_CON_RSP; + //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);*/ @@ -248,11 +260,11 @@ 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) +l2cap_process_con_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) { - L2capFrame *cmd = NULL; + L2capFrame* cmd = NULL; uint16 scid, dcid, result, status; - status_t error = 0; + status_t error = 0; /* Get command parameters */ NetBufferHeaderReader command(buffer); @@ -293,7 +305,7 @@ l2cap_process_con_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) */ if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) - return (error); + return error; if (result == L2CAP_PENDING) { /* @@ -334,28 +346,28 @@ l2cap_process_con_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) btCoreData->AcknowledgeSignal(cmd); } - return (error); + 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); + return 0; } static option_status -getNextSignalOption(net_buffer *nbuf, size_t *off, l2cap_cfg_opt_t *hdr, l2cap_cfg_opt_val_t *val) +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); + size_t len = nbuf->size - (*off); if (len == 0) - return (OPTION_NOT_PRESENT); + return OPTION_NOT_PRESENT; if (len < 0 || len < sizeof(*hdr)) return HEADER_TOO_SHORT; - gBufferModule->write(nbuf, *off, hdr, sizeof(*hdr)); + gBufferModule->read(nbuf, *off, hdr, sizeof(*hdr)); *off += sizeof(*hdr); len -= sizeof(*hdr); @@ -367,9 +379,10 @@ getNextSignalOption(net_buffer *nbuf, size_t *off, l2cap_cfg_opt_t *hdr, l2cap_c if (hdr->length != L2CAP_OPT_MTU_SIZE || len < hdr->length) return BAD_OPTION_LENGTH; - gBufferModule->write(nbuf, *off, val, L2CAP_OPT_MTU_SIZE); + gBufferModule->read(nbuf, *off, val, L2CAP_OPT_MTU_SIZE); val->mtu = le16toh(val->mtu); *off += L2CAP_OPT_MTU_SIZE; + flowf("mtu specified\n"); break; case L2CAP_OPT_FLUSH_TIMO: @@ -377,8 +390,9 @@ getNextSignalOption(net_buffer *nbuf, size_t *off, l2cap_cfg_opt_t *hdr, l2cap_c len < hdr->length) return BAD_OPTION_LENGTH; - gBufferModule->write(nbuf, *off, val, L2CAP_OPT_FLUSH_TIMO_SIZE); + gBufferModule->read(nbuf, *off, val, L2CAP_OPT_FLUSH_TIMO_SIZE); val->flush_timo = le16toh(val->flush_timo); + flowf("flush specified\n"); *off += L2CAP_OPT_FLUSH_TIMO_SIZE; break; @@ -386,13 +400,14 @@ getNextSignalOption(net_buffer *nbuf, size_t *off, l2cap_cfg_opt_t *hdr, l2cap_c if (hdr->length != L2CAP_OPT_QOS_SIZE || len < hdr->length) return BAD_OPTION_LENGTH; - gBufferModule->write(nbuf, *off, val, L2CAP_OPT_QOS_SIZE); + gBufferModule->read(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; + flowf("qos specified\n"); break; default: @@ -408,18 +423,21 @@ getNextSignalOption(net_buffer *nbuf, size_t *off, l2cap_cfg_opt_t *hdr, l2cap_c static status_t -l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer) +l2cap_process_cfg_req(HciConnection* conn, uint8 ident, net_buffer* buffer) { - L2capChannel *channel = NULL; + L2capChannel* channel = NULL; uint16 dcid; uint16 respond; uint16 result; l2cap_cfg_opt_t hdr; - l2cap_cfg_opt_val_t val; + l2cap_cfg_opt_val_t val; - size_t off; - status_t error = 0; + size_t off; + status_t error = 0; + + + debugf("configuration=%ld\n", buffer->size); /* Get command parameters */ NetBufferHeaderReader command(buffer); @@ -431,7 +449,8 @@ l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer) dcid = le16toh(command->dcid); respond = L2CAP_OPT_CFLAG(le16toh(command->flags)); - + command.Remove(); // pull configuration header + /* Check if we have this channel and it is in valid state */ channel = btCoreData->ChannelBySourceID(conn, dcid); if (channel == NULL) { @@ -449,25 +468,24 @@ l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer) 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; + channel->configuration->omtu = val.mtu; break; case L2CAP_OPT_FLUSH_TIMO: - channel->flush_timo = val.flush_timo; + channel->configuration->flush_timo = val.flush_timo; break; case L2CAP_OPT_QOS: - memcpy(&val.flow, &channel->iflow, sizeof(channel->iflow)); + memcpy(&val.flow, &channel->configuration->iflow, sizeof(channel->configuration->iflow)); break; default: /* Ignore unknown hint option */ @@ -475,13 +493,9 @@ l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer) } } 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); @@ -492,6 +506,9 @@ l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer) } } + debugf("Pulled %ld of configuration fields respond=%d remaining=%ld\n", off, respond, buffer->size); + gBufferModule->remove_header(buffer, off); + /* * 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 @@ -505,15 +522,18 @@ l2cap_process_cfg_req(HciConnection *conn, uint8 ident, net_buffer *buffer) */ if (respond) { + flowf("Refusing cfg\n"); error = send_l2cap_cfg_rsp(conn, ident, channel->dcid, result, buffer); if (error != 0) { + // TODO: //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); + channel->cfgState |= L2CAP_CFG_IN; + channel->ident = ident; // sent ident to reply + error = l2cap_l2ca_cfg_rsp_ind(channel); if (error != 0) btCoreData->RemoveChannel(conn, channel->scid); } @@ -553,6 +573,8 @@ l2cap_process_cfg_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) cflag = L2CAP_OPT_CFLAG(le16toh(command->flags)); result = le16toh(command->result); + command.Remove(); + /* Check if we have this command */ cmd = btCoreData->SignalByIdent(conn, ident); if (cmd == NULL) { @@ -596,15 +618,16 @@ l2cap_process_cfg_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) else if (error > 0) { /* Got option */ switch (hdr.type) { case L2CAP_OPT_MTU: - cmd->channel->imtu = val.mtu; + cmd->channel->configuration->imtu = val.mtu; break; case L2CAP_OPT_FLUSH_TIMO: - cmd->channel->flush_timo = val.flush_timo; + cmd->channel->configuration->flush_timo = val.flush_timo; break; case L2CAP_OPT_QOS: - memcpy(&val.flow, &cmd->channel->oflow, sizeof(cmd->channel->oflow)); + memcpy(&val.flow, &cmd->channel->configuration->oflow, + sizeof(cmd->channel->configuration->oflow)); break; default: /* Ignore unknown hint option */ @@ -628,8 +651,6 @@ l2cap_process_cfg_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) } } - command.Remove(); - if (cflag) /* Restart timer and wait for more options */ btCoreData->TimeoutSignal(cmd, bluetooth_l2cap_rtx_timeout); else { @@ -647,11 +668,14 @@ l2cap_process_cfg_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) btCoreData->RemoveChannel(conn, cmd->channel->scid); } - + cmd->channel->cfgState |= L2CAP_CFG_OUT; + if ((cmd->channel->cfgState & L2CAP_CFG_BOTH) == L2CAP_CFG_BOTH) { + cmd->channel->state = L2CAP_CHAN_OPEN; + } btCoreData->AcknowledgeSignal(cmd); } - return (error); + return error; reject: /* Send reject. Do not really care about the result */ @@ -664,10 +688,11 @@ reject: /* Process L2CAP_DisconnectReq command */ static status_t -l2cap_process_discon_req(HciConnection *conn, uint8 ident, net_buffer *buffer) +l2cap_process_discon_req(HciConnection* conn, uint8 ident, net_buffer* buffer) { - L2capChannel *ch = NULL; - L2capFrame *cmd = NULL; + L2capChannel* ch = NULL; + L2capFrame* cmd = NULL; + net_buffer* buff = NULL; uint16 scid; uint16 dcid; @@ -681,7 +706,6 @@ l2cap_process_discon_req(HciConnection *conn, uint8 ident, net_buffer *buffer) 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); @@ -710,28 +734,28 @@ l2cap_process_discon_req(HciConnection *conn, uint8 ident, net_buffer *buffer) * with L2CAP_DisconnectRsp. */ + flowf("Responding\n"); + /* Send L2CAP_DisconnectRsp */ + buff = l2cap_discon_rsp(ident, dcid, scid); + cmd = btCoreData->SpawnSignal(conn, ch, buff, ident, L2CAP_DISCON_RSP); + if (cmd == NULL) + return ENOMEM; + + /* Link command to the queue */ + SchedConnectionPurgeThread(conn); + + + ch->state = L2CAP_CHAN_W4_L2CA_DISCON_RSP; + 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: + flowf("Rejecting\n"); /* Send reject. Do not really care about the result */ send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, scid, dcid); @@ -741,7 +765,7 @@ reject: /* Process L2CAP_DisconnectRsp command */ static status_t -l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer *buffer) +l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) { L2capFrame *cmd = NULL; int16 scid, dcid; @@ -793,7 +817,7 @@ l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer *buffer) btCoreData->RemoveChannel(conn, scid); /* this will free commands too */ out: - return (error); + return error; } @@ -802,16 +826,14 @@ l2cap_process_echo_req(HciConnection *conn, uint8 ident, net_buffer *buffer) { L2capFrame *cmd = NULL; - cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_ECHO_RSP); + cmd = btCoreData->SpawnSignal(conn, NULL, l2cap_echo_req(ident, NULL, 0), 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); + SchedConnectionPurgeThread(conn); return B_OK; } @@ -819,9 +841,9 @@ l2cap_process_echo_req(HciConnection *conn, uint8 ident, net_buffer *buffer) /* Process L2CAP_EchoRsp command */ static status_t -l2cap_process_echo_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) +l2cap_process_echo_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) { - L2capFrame *cmd = NULL; + L2capFrame* cmd = NULL; status_t error = 0; /* Check if we have this command */ @@ -848,9 +870,10 @@ l2cap_process_echo_rsp(HciConnection *conn, uint8 ident, net_buffer *buffer) /* Process L2CAP_InfoReq command */ static status_t -l2cap_process_info_req(HciConnection *conn, uint8 ident, net_buffer *buffer) +l2cap_process_info_req(HciConnection* conn, uint8 ident, net_buffer* buffer) { - L2capFrame *cmd = NULL; + L2capFrame* cmd = NULL; + net_buffer* buf = NULL; uint16 type; /* Get command parameters */ @@ -861,31 +884,26 @@ l2cap_process_info_req(HciConnection *conn, uint8 ident, net_buffer *buffer) } //command->type = le16toh(mtod(conn->rx_pkt, ng_l2cap_info_req_cp *)->type); - type = command->type; + type = le16toh(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); + buf = 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); + buf = l2cap_info_rsp(ident, type, L2CAP_NOT_SUPPORTED, 0); break; } - if (cmd->buffer == NULL) { - btCoreData->AcknowledgeSignal(cmd); - return ENOBUFS; - } + cmd = btCoreData->SpawnSignal(conn, NULL, buf, ident, L2CAP_INFO_RSP); + if (cmd == NULL) + return ENOMEM; /* Link command to the queue */ - //TODO l2cap_sched(conn); + SchedConnectionPurgeThread(conn); return B_OK; } @@ -893,11 +911,11 @@ l2cap_process_info_req(HciConnection *conn, uint8 ident, net_buffer *buffer) /* Process L2CAP_InfoRsp command */ static status_t -l2cap_process_info_rsp(HciConnection *conn, uint8 ident, net_buffer* buffer) +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; + l2cap_info_rsp_cp* cp = NULL; + L2capFrame* cmd = NULL; + status_t error = B_OK; /* Get command parameters */ NetBufferHeaderReader command(buffer); @@ -953,9 +971,9 @@ 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) +l2cap_process_cmd_rej(HciConnection* conn, uint8 ident, net_buffer* buffer) { - L2capFrame *cmd = NULL; + L2capFrame* cmd = NULL; /* TODO: review this command... Get command data */ NetBufferHeaderReader command(buffer); @@ -1021,70 +1039,52 @@ l2cap_process_cmd_rej(HciConnection *conn, uint8 ident, net_buffer *buffer) status_t -send_l2cap_reject(HciConnection *conn, uint8 ident, uint16 reason, +send_l2cap_reject(HciConnection* conn, uint8 ident, uint16 reason, uint16 mtu, uint16 scid, uint16 dcid) { - L2capFrame *cmd = NULL; + L2capFrame* cmd = NULL; - cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_CMD_REJ); + cmd = btCoreData->SpawnSignal(conn, NULL, l2cap_cmd_rej(cmd->ident, reason, mtu, scid, dcid), 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); + SchedConnectionPurgeThread(conn); return B_OK; } status_t -send_l2cap_con_rej(HciConnection *conn, uint8 ident, uint16 scid, uint16 dcid, uint16 result) +send_l2cap_con_rej(HciConnection* conn, uint8 ident, uint16 scid, uint16 dcid, uint16 result) { - L2capFrame *cmd = NULL; + L2capFrame* cmd = NULL; - cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_CON_RSP); + cmd = btCoreData->SpawnSignal(conn, NULL, l2cap_con_rsp(cmd->ident, scid, dcid, result, 0), 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); + SchedConnectionPurgeThread(conn); return B_OK; } status_t -send_l2cap_cfg_rsp(HciConnection *conn, uint8 ident, uint16 scid, uint16 result, net_buffer *opt) +send_l2cap_cfg_rsp(HciConnection* conn, uint8 ident, uint16 scid, uint16 result, net_buffer* opt) { - L2capFrame *cmd = NULL; + L2capFrame* cmd = NULL; - cmd = btCoreData->SpawnSignal(conn, NULL, NULL, ident, L2CAP_CFG_RSP); + cmd = btCoreData->SpawnSignal(conn, NULL, l2cap_cfg_rsp(cmd->ident, scid, 0, result, opt), + 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); + SchedConnectionPurgeThread(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 index 7190738692..15e3da72af 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.h +++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_signal.h @@ -8,9 +8,6 @@ // Processing signals status_t l2cap_process_signal_cmd(HciConnection* conn, net_buffer* buffer); -// Queuing commands - - #if 0 #pragma - Signals Responses #endif @@ -22,5 +19,4 @@ send_l2cap_con_rej(HciConnection *conn, uint8 ident, uint16 scid, uint16 dcid, u 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 index 60a252d12c..9459440856 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.cpp +++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.cpp @@ -3,13 +3,9 @@ * All rights reserved. Distributed under the terms of the MIT License. * */ - #include - #include - -#include #include "l2cap_internal.h" #include "l2cap_signal.h" #include "l2cap_command.h" @@ -21,6 +17,7 @@ #define SUBMODULE_NAME "upper" #define SUBMODULE_COLOR 36 #include +#include status_t @@ -28,26 +25,25 @@ l2cap_l2ca_con_ind(L2capChannel* channel) { L2capEndpoint* endpoint = L2capEndpoint::ForPsm(channel->psm); - if (endpoint == NULL) { //refuse connection no endpoint bound + if (endpoint == NULL) { //TODO: refuse connection no endpoint bound debugf("No endpoint bound for psm %d\n", channel->psm); return B_ERROR; } + // Pair Channel with endpoint channel->endpoint = endpoint; + endpoint->BindToChannel(channel); + 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) + net_buffer* buf = l2cap_con_rsp(channel->ident, channel->scid, channel->dcid, L2CAP_SUCCESS, L2CAP_NO_INFO); + L2capFrame* cmd = btCoreData->SpawnSignal(channel->conn, channel, buf, channel->ident, L2CAP_CON_RSP); + if (cmd == NULL) { + gBufferModule->free(buf); 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; } + // we can move to configuration... + channel->state = L2CAP_CHAN_CONFIG; /* Link command to the queue */ SchedConnectionPurgeThread(channel->conn); @@ -55,6 +51,64 @@ l2cap_l2ca_con_ind(L2capChannel* channel) } +status_t +l2cap_l2ca_cfg_rsp_ind(L2capChannel* channel) +{ + // if our configuration has not been yet send .. + if(!(channel->cfgState & L2CAP_CFG_OUT_SENT)) { + // send config_rsp + + net_buffer* buf = l2cap_cfg_rsp(channel->ident, channel->dcid, 0, L2CAP_SUCCESS, NULL); + L2capFrame* cmd = btCoreData->SpawnSignal(channel->conn, channel, buf, channel->ident, L2CAP_CFG_RSP); + if (cmd == NULL) { + gBufferModule->free(buf); + channel->state = L2CAP_CHAN_CLOSED; + return ENOMEM; + } + + flowf("Sending cfg resp\n"); + /* Link command to the queue */ + SchedConnectionPurgeThread(channel->conn); + + // set status + channel->cfgState |= L2CAP_CFG_OUT_SENT; + + } else { + + + } + + + if ((channel->cfgState & L2CAP_CFG_BOTH) == L2CAP_CFG_BOTH) { + // Channel can be declared open + channel->state = L2CAP_CHAN_OPEN; + } else { + // send configuration Request by our side + if (channel->endpoint->configurationSet) { + // TODO: define complete configuration packet + + } else { + // nothing special requested + net_buffer* buf = l2cap_cfg_req(channel->ident, channel->dcid, 0, NULL); + L2capFrame* cmd = btCoreData->SpawnSignal(channel->conn, channel, buf, channel->ident, L2CAP_CFG_REQ); + if (cmd == NULL) { + gBufferModule->free(buf); + channel->state = L2CAP_CHAN_CLOSED; + return ENOMEM; + } + + flowf("Sending cfg req\n"); + // Link command to the queue + SchedConnectionPurgeThread(channel->conn); + + } + channel->cfgState |= L2CAP_CFG_IN_SENT; + } + + return B_OK; +} + + status_t l2cap_upper_con_rsp(HciConnection* conn, L2capChannel* channel) { @@ -62,3 +116,25 @@ l2cap_upper_con_rsp(HciConnection* conn, L2capChannel* channel) return B_OK; } + + +status_t +l2cap_co_receive(HciConnection* conn, net_buffer* buffer, uint16 dcid) +{ + + L2capChannel* channel = btCoreData->ChannelBySourceID(conn, dcid); + + if (channel == NULL) { + debugf("dcid %d does not exist for handle %d\n", dcid, conn->handle); + return B_ERROR; + } + + if (channel->endpoint == NULL) { + debugf("dcid %d not bound to endpoint\n", dcid); + return B_ERROR; + } + + flowf("Enqueue to fifo\n"); + return gStackModule->fifo_enqueue_buffer(&channel->endpoint->fReceivingFifo, buffer); + +} diff --git a/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h b/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h index 4d74b20b3b..75723a809e 100644 --- a/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h +++ b/src/add-ons/kernel/network/protocols/l2cap/l2cap_upper.h @@ -9,5 +9,9 @@ status_t l2cap_l2ca_con_ind(L2capChannel* channel); status_t l2cap_upper_con_rsp(HciConnection* conn, L2capChannel* channel); +status_t l2cap_l2ca_cfg_rsp_ind(L2capChannel* channel); + + +status_t l2cap_co_receive(HciConnection* conn, net_buffer* buffer, uint16 dcid); #endif