void ds3wiibt_disconnect(struct ds3wiibt_context *ctx)
{
if (ctx->status == DS3WIIBT_STATUS_CONNECTED) {
//We have only to request the disconnection of the DATA channel
l2ca_disconnect_req(ctx->data_pcb, l2ca_disconnect_cfm_cb);
ctx->status = DS3WIIBT_STATUS_DISCONNECTING;
}
}
/*
* sdp_attributes_recv():
*
* Can be used as a callback by SDP when a response to a service attribute request or
* a service search attribute request was received.
* Disconnects the L2CAP SDP channel and connects to the RFCOMM one.
* If no RFCOMM channel was found it initializes a search for other devices.
*/
void sdp_attributes_recv(void *arg, struct sdp_pcb *sdppcb, u16_t attribl_bc, struct pbuf *p)
{
struct l2cap_pcb *l2cappcb;
l2ca_disconnect_req(sdppcb->l2cappcb, l2cap_disconnected_cfm);
/* Get the RFCOMM channel identifier from the protocol descriptor list */
if((bt_spp_state.cn = get_rfcomm_cn(attribl_bc, p)) != 0) {
if((l2cappcb = l2cap_new()) == NULL) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("sdp_attributes_recv: Could not alloc L2CAP pcb\n"));
return;
}
LWIP_DEBUGF(BT_SPP_DEBUG, ("sdp_attributes_recv: RFCOMM channel: %d\n", bt_spp_state.cn));
l2ca_connect_req(l2cappcb, &(sdppcb->l2cappcb->remote_bdaddr), RFCOMM_PSM, HCI_ALLOW_ROLE_SWITCH, l2cap_connected);
} else {
bt_spp_start();
}
sdp_free(sdppcb);
}
void
l2cap_tmr(void)
{
struct l2cap_sig *sig;
struct l2cap_pcb *pcb;
err_t ret;
/* Step through all of the active pcbs */
for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
/* Step through any unresponded signals */
for(sig = pcb->unrsp_sigs; sig != NULL; sig = sig->next) {
/* Check if channel is not reliable */
if(pcb->cfg.outflushto < 0xFFFF) {
/* Check if rtx is active. Otherwise ertx is active */
if(sig->rtx > 0) {
/* Adjust rtx timer */
--sig->rtx;
/* Check if rtx has expired */
if(sig->rtx == 0) {
if(sig->nrtx == 0) {
/* Move pcb to closed state */
pcb->state = L2CAP_CLOSED;
/* Indicate disconnect to upper layer */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_tmr: Max number of retransmissions (rtx) has expired\n"));
L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
} else {
--sig->nrtx;
/* Indicate timeout to upper layer */
L2CA_ACTION_TO_IND(pcb,ERR_OK,ret);
/* Retransmitt signal w timeout doubled */
sig->rtx += sig->rtx;
ret = l2cap_rexmit_signal(pcb, sig);
}
} /* if */
} else {
/* Adjust ertx timer */
--sig->ertx;
/* Check if ertx has expired */
if(sig->ertx == 0) {
if(sig->nrtx == 0) {
/* Move pcb to closed state */
pcb->state = L2CAP_CLOSED;
/* Indicate disconnect to upper layer */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_tmr: Max number of retransmissions (ertx) has expired\n"));
L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
} else {
--sig->nrtx;
/* Indicate timeout to upper layer */
L2CA_ACTION_TO_IND(pcb,ERR_OK,ret);
/* Disable ertx, activate rtx and retransmitt signal */
sig->ertx = 0;
sig->rtx = L2CAP_RTX;
ret = l2cap_rexmit_signal(pcb, sig);
}
} /* if */
} /* else */
} /* if */
} /* for */
/* Check configuration timer */
if(pcb->state == L2CAP_CONFIG) {
/* Check if configuration timer is active */
if(pcb->cfg.cfgto > 0) {
--pcb->cfg.cfgto;
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_tmr: Configuration timer = %d\n", pcb->cfg.cfgto));
/* Check if config timer has expired */
if(pcb->cfg.cfgto == 0) {
/* Connection attempt failed. Disconnect */
l2ca_disconnect_req(pcb, NULL);
/* Notify the application that the connection attempt failed */
if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_CFG_TO, 0x0000, ret);
} else {
L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
}
pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset timer */
}
}
}
} /* for */
}
void
l2cap_process_sig(struct pbuf *q, struct l2cap_hdr *l2caphdr, struct bd_addr *bdaddr)
{
struct l2cap_sig_hdr *sighdr;
struct l2cap_sig *sig = NULL;
struct l2cap_pcb *pcb = NULL;
struct l2cap_pcb_listen *lpcb;
struct l2cap_cfgopt_hdr *opthdr;
u16_t result, status, flags, psm, dcid, scid;
u16_t len;
u16_t siglen;
struct pbuf *p, *r = NULL, *s = NULL, *data;
err_t ret;
u8_t i;
u16_t rspstate = L2CAP_CFG_SUCCESS;
if(q->len != q->tot_len) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Fragmented packet received. Reassemble into one buffer\n"));
if((p = pbuf_alloc(PBUF_RAW, q->tot_len, PBUF_RAM)) != NULL) {
i = 0;
for(r = q; r != NULL; r = r->next) {
memcpy(((u8_t *)p->payload) + i, r->payload, r->len);
i += r->len;
}
} else {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate buffer for fragmented packet\n"));
return;
}
} else {
p = q;
}
len = l2caphdr->len;
while(len > 0) {
/* Set up signal header */
sighdr = p->payload;
pbuf_header(p, -L2CAP_SIGHDR_LEN);
/* Check if this is a response/reject signal, and if so, find the matching request */
if(sighdr->code % 2) { /* if odd this is a resp/rej signal */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Response/reject signal received id = %d code = %d\n",
sighdr->id, sighdr->code));
for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
for(sig = pcb->unrsp_sigs; sig != NULL; sig = sig->next) {
if(sig->sigid == sighdr->id) {
break; /* found */
}
}
if(sig != NULL) {
break;
}
}
} else {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Request signal received id = %d code = %d\n",
sighdr->id, sighdr->code));
}
/* Reject packet if length exceeds MTU */
if(l2caphdr->len > L2CAP_MTU) {
/* Alloc size of reason in cmd rej + MTU */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+2, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_MTU_EXCEEDED;
((u16_t *)data->payload)[1] = L2CAP_MTU;
l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
break;
}
switch(sighdr->code) {
case L2CAP_CMD_REJ:
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Our command was rejected so we disconnect\n"));
l2ca_disconnect_req(pcb, NULL);
break;
case L2CAP_CONN_REQ:
psm = ((u16_t *)p->payload)[0];
/* Search for a listening pcb */
for(lpcb = l2cap_listen_pcbs; lpcb != NULL; lpcb = lpcb->next) {
if(lpcb->psm == psm) {
/* Found a listening pcb with the correct PSM */
break;
}
}
/* If no matching pcb was found, send a connection rsp neg (PSM) */
if(lpcb == NULL) {
/* Alloc size of data in conn rsp signal */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_CONN_REF_PSM;
((u16_t *)data->payload)[1] = 0; /* No further info available */
ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
}
} else {
/* Initiate a new active pcb */
pcb = l2cap_new();
if(pcb == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: could not allocate PCB\n"));
/* Send a connection rsp neg (no resources available) and alloc size of data in conn rsp
signal */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_CONN_REF_RES;
((u16_t *)data->payload)[1] = 0; /* No further info available */
ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
}
}
bd_addr_set(&(pcb->remote_bdaddr),bdaddr);
pcb->scid = l2cap_cid_alloc();
pcb->dcid = ((u16_t *)p->payload)[1];
pcb->psm = psm;
pcb->callback_arg = lpcb->callback_arg;
pcb->l2ca_connect_ind = lpcb->l2ca_connect_ind;
pcb->state = L2CAP_CONFIG;
L2CAP_REG(&l2cap_active_pcbs, pcb);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: A connection request was received. Send a response\n"));
data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM);
if(data == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_connect_rsp: Could not allocate memory for pbuf\n"));
break;
}
((u16_t *)data->payload)[0] = pcb->scid;
((u16_t *)data->payload)[1] = pcb->dcid;
((u16_t *)data->payload)[2] = L2CAP_CONN_SUCCESS;
((u16_t *)data->payload)[3] = 0x0000; /* No further information available */
/* Send the response */
ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
}
break;
case L2CAP_CONN_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
LWIP_ASSERT("l2cap_process_sig: conn rsp, active pcb->state == W4_L2CAP_CONNECT_RSP\n",
pcb->state == W4_L2CAP_CONNECT_RSP);
result = ((u16_t *)p->payload)[2];
status = ((u16_t *)p->payload)[3];
switch(result) {
case L2CAP_CONN_SUCCESS:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_sucess, status %d\n", status));
LWIP_ASSERT("l2cap_process_sig: conn rsp success, pcb->scid == ((u16_t *)p->payload)[1]\n",
pcb->scid == ((u16_t *)p->payload)[1]);
/* Set destination connection id */
pcb->dcid = ((u16_t *)p->payload)[0];
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
/* Configure connection */
pcb->state = L2CAP_CONFIG;
/* If initiator send a configuration request */
if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
l2ca_config_req(pcb);
pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
}
break;
case L2CAP_CONN_PND:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_pnd, status %d\n", status));
/* Disable rtx and enable ertx */
sig->rtx = 0;
sig->ertx = L2CAP_ERTX;
break;
default:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_neg, result %d\n", result));
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
L2CA_ACTION_CONN_CFM(pcb,result,status,ret);
break;
}
break;
case L2CAP_CFG_REQ:
siglen = sighdr->len;
dcid = ((u16_t *)p->payload)[0];
flags = ((u16_t *)p->payload)[1];
siglen -= 4;
pbuf_header(p, -4);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Congfiguration request, flags = %d\n", flags));
/* Find PCB with matching cid */
for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: dcid = 0x%x, pcb->scid = 0x%x, pcb->dcid = 0x%x\n\n", dcid, pcb->scid, pcb->dcid));
if(pcb->scid == dcid) {
/* Matching cid found */
break;
}
}
/* If no matching cid was found, send a cmd reject (Invalid cid) */
if(pcb == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Cfg req: no matching cid was found\n"));
/* Alloc size of reason in cmd rej + data (dcid + scid) */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_INVALID_CID;
((u16_t *)data->payload)[1] = dcid; /* Requested local cid */
((u16_t *)data->payload)[2] = L2CAP_NULL_CID; /* Remote cid not known */
ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
} else { /* Handle config request */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Handle configuration request\n"));
pcb->ursp_id = sighdr->id; /* Set id of request to respond to */
/* Parse options and add to pcb */
while(siglen > 0) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Siglen = %d\n", siglen));
opthdr = p->payload;
/* Check if type of action bit indicates a non-hint. Hints are ignored */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Type of action bit = %d\n", L2CAP_OPTH_TOA(opthdr)));
if(L2CAP_OPTH_TOA(opthdr) == 0) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Type = %d\n", L2CAP_OPTH_TYPE(opthdr)));
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Length = %d\n", opthdr->len));
switch(L2CAP_OPTH_TYPE(opthdr)) {
case L2CAP_CFG_MTU:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Out MTU = %d\n", ((u16_t *)p->payload)[1]));
pcb->cfg.outmtu = ((u16_t *)p->payload)[1];
break;
case L2CAP_FLUSHTO:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: In flush timeout = %d\n", ((u16_t *)p->payload)[1]));
pcb->cfg.influshto = ((u16_t *)p->payload)[1];
break;
case L2CAP_QOS:
/* If service type is Best Effort or No Traffic the remainder fields will be ignored */
if(((u8_t *)p->payload)[3] == L2CAP_QOS_GUARANTEED) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: This implementation does not support the guaranteed QOS service type"));
if(rspstate == L2CAP_CFG_SUCCESS) {
rspstate = L2CAP_CFG_UNACCEPT;
if(pcb->cfg.opt != NULL) {
pbuf_free(pcb->cfg.opt);
pcb->cfg.opt = NULL;
}
}
s = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
if(pcb->cfg.opt == NULL) {
pcb->cfg.opt = s;
} else {
pbuf_chain(pcb->cfg.opt, s);
pbuf_free(s);
}
}
break;
default:
if(rspstate != L2CAP_CFG_REJ) {
/* Unknown option. Add to unknown option type buffer */
if(rspstate != L2CAP_CFG_UNKNOWN) {
rspstate = L2CAP_CFG_UNKNOWN;
if(pcb->cfg.opt != NULL) {
pbuf_free(pcb->cfg.opt);
pcb->cfg.opt = NULL;
}
}
s = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
if(pcb->cfg.opt == NULL) {
pcb->cfg.opt = s;
} else {
pbuf_chain(pcb->cfg.opt, s);
pbuf_free(s);
}
}
break;
} /* switch */
} /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
pbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
} /* while */
/* If continuation flag is set we don't send the final response just yet */
if((flags & 0x0001) == 1) {
/* Send success result with no options until the full request has been received */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate memory for pbuf\n"));
break;
}
((u16_t *)data->payload)[0] = pcb->dcid;
((u16_t *)data->payload)[1] = 0;
((u16_t *)data->payload)[2] = L2CAP_CFG_SUCCESS;
ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
break;
}
/* Send a configure request for outgoing link if it hasnt been configured */
if(!(pcb->cfg.l2capcfg & L2CAP_CFG_IR) && !(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_REQ)) {
l2ca_config_req(pcb);
pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
}
/* Send response to configuration request */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Send response to configuration request\n"));
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = pcb->dcid;
((u16_t *)data->payload)[1] = 0; /* Flags (No continuation) */
((u16_t *)data->payload)[2] = rspstate; /* Result */
if(pcb->cfg.opt != NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: pcb->cfg.opt->len = %d\n", pcb->cfg.opt->len));
pbuf_chain(data, pcb->cfg.opt); /* Add option type buffer to data buffer */
pbuf_free(pcb->cfg.opt);
pcb->cfg.opt = NULL;
}
ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
}
if(rspstate == L2CAP_CFG_SUCCESS) {
pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_SUCCESS;
/* L2CAP connection established if a successful configuration response has been sent */
if(pcb->cfg.l2capcfg & L2CAP_CFG_IN_SUCCESS) {
/* IPCP connection established, notify upper layer that connection is open */
pcb->state = L2CAP_OPEN;
if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
} else {
L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
}
}
}
} /* else */
break;
case L2CAP_CFG_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
LWIP_ASSERT(("l2cap_process_sig: cfg rsp, active pcb->state == L2CAP_CONFIG\n"),
pcb->state == L2CAP_CONFIG);
siglen = sighdr->len;
scid = ((u16_t *)p->payload)[0];
flags = ((u16_t *)p->payload)[1];
result = ((u16_t *)p->payload)[2];
siglen -= 6;
pbuf_header(p, -6);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Outgoing configuration result == %d continuation flag == %d\n", result, flags));
/* Handle config request */
switch(result) {
case L2CAP_CFG_SUCCESS:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Successfull outgoing configuration\n"));
pcb->cfg.l2capcfg |= L2CAP_CFG_IN_SUCCESS; /* Local side of the connection
has been configured for outgoing data */
pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset configuration timeout */
if(pcb->cfg.outflushto != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
lp_write_flush_timeout(&pcb->remote_bdaddr, pcb->cfg.outflushto);
}
/* L2CAP connection established if a successful configuration response has been sent */
if(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_SUCCESS) {
pcb->state = L2CAP_OPEN;
if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
} else {
L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
}
}
break;
case L2CAP_CFG_UNACCEPT:
/* Parse and add options to pcb */
while(siglen > 0) {
opthdr = p->payload;
/* Check if type of action bit indicates a non-hint. Hints are ignored */
if(L2CAP_OPTH_TOA(opthdr) == 0) {
switch(L2CAP_OPTH_TYPE(opthdr)) {
case L2CAP_CFG_MTU:
if(L2CAP_MTU > ((u16_t *)p->payload)[1]) {
pcb->cfg.outmtu = ((u16_t *)p->payload)[1];
} else {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Configuration of MTU failed\n"));
l2ca_disconnect_req(pcb, NULL);
return;
}
break;
case L2CAP_FLUSHTO:
pcb->cfg.influshto = ((u16_t *)p->payload)[1];
break;
case L2CAP_QOS:
/* If service type Best Effort is not accepted we will close the connection */
if(((u8_t *)p->payload)[3] != L2CAP_QOS_BEST_EFFORT) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Unsupported service type\n"));
l2ca_disconnect_req(pcb, NULL);
return;
}
break;
default:
/* Should not happen, skip option */
break;
} /* switch */
} /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
pbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
} /* while */
/* Send out a new configuration request if the continuation flag isn't set */
if((flags & 0x0001) == 0) {
l2ca_config_req(pcb);
}
break;
case L2CAP_CFG_REJ:
/* Fallthrough */
case L2CAP_CFG_UNKNOWN:
/* Fallthrough */
default:
if((flags & 0x0001) == 0) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Configuration failed\n"));
l2ca_disconnect_req(pcb, NULL);
return;
}
break;
} /* switch(result) */
/* If continuation flag is set we must send a NULL configuration request */
if((flags & 0x0001) == 1) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Continuation flag is set. Send empty (default) config request signal\n"));
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate memory for pbuf\n"));
return;
}
/* Assemble config request packet */
((u16_t *)data->payload)[0] = pcb->scid;
((u16_t *)data->payload)[2] = 0;
l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), data);
}
break;
case L2CAP_DISCONN_REQ:
siglen = sighdr->len;
dcid = ((u16_t *)p->payload)[0];
siglen = siglen - 2;
flags = ((u16_t *)p->payload)[1];
siglen = siglen - 2;
pbuf_header(p, -4);
/* Find PCB with matching cid */
for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
if(pcb->scid == dcid) {
/* Matching cid found */
break;
}
}
/* If no matching cid was found, send a cmd reject (Invalid cid) */
if(pcb == NULL) {
/* Alloc size of reason in cmd rej + data (dcid + scid) */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_INVALID_CID;
((u16_t *)data->payload)[1] = dcid; /* Requested local cid */
((u16_t *)data->payload)[2] = L2CAP_NULL_CID; /* Remote cid not known */
ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
} else { /* Handle disconnection request */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_DISCONN_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = pcb->scid;
((u16_t *)data->payload)[1] = pcb->dcid;
ret = l2cap_signal(pcb, L2CAP_DISCONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
/* Give upper layer indication */
pcb->state = L2CAP_CLOSED;
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Disconnection request\n"));
L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
}
}
break;
case L2CAP_DISCONN_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
L2CA_ACTION_DISCONN_CFM(pcb,ret); /* NOTE: Application should
now close the connection */
break;
case L2CAP_ECHO_REQ:
if( pcb != NULL)
{
pcb->ursp_id = sighdr->id;
ret = l2cap_signal(pcb, L2CAP_ECHO_RSP, sighdr->id, &(pcb->remote_bdaddr), p);
} else {
ret = l2cap_signal(NULL, L2CAP_ECHO_RSP, sighdr->id, bdaddr, p);
}
pbuf_free(p);
break;
case L2CAP_ECHO_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
/* Remove temporary pcb from active list */
L2CAP_RMV(&l2cap_active_pcbs, pcb);
L2CA_ACTION_PING_CFM(pcb,L2CAP_ECHO_RCVD,ret);
break;
default:
/* Alloc size of reason in cmd rej */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_CMD_NOT_UNDERSTOOD;
ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
break;
} /* switch */
len = len - (sighdr->len + L2CAP_SIGHDR_LEN);
pbuf_header(p, -(sighdr->len));
} /* while */
}
