?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
silence some warnings (dhewg)
[libogc.git] / lwbt / l2cap.c
blobf0c72bcf90037a471ba853646515c6e0e826bc37
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
4 #include <malloc.h>
5 #include <ogcsys.h>
6 #include <gccore.h>
7
8 #include "hci.h"
9 #include "l2cap.h"
10 #include "btmemb.h"
11 #include "btpbuf.h"
12
13 /* Next Identifier to be sent */
14 u8_t sigid_nxt;
15
16 /* The L2CAP PCB lists. */
17 struct l2cap_pcb_listen *l2cap_listen_pcbs = NULL; /* List of all L2CAP PCBs in CLOSED state
18 but awaiting an incoming conn req */
19 struct l2cap_pcb *l2cap_active_pcbs; /* List of all L2CAP PCBs that are in a
20 state in which they accept or send
21 data */
22 struct l2cap_pcb *l2cap_tmp_pcb = NULL;
23
24 /* Temp signal */
25 struct l2cap_sig *l2cap_tmp_sig = NULL;
26
27 /* Global variable involved in input processing of l2cap data segements */
28 struct l2cap_seg *l2cap_insegs = NULL;
29 struct l2cap_seg *l2cap_tmp_inseg = NULL;
30
31 /* Global Baseband disconnect callback. */
32 static void (*l2cap_disconnect_bb_cb)(struct bd_addr *bdaddr,u8_t reason) = NULL;
33
34 /* Forward declarations */
35 static u16_t l2cap_cid_alloc(void);
36
37 MEMB(l2cap_pcbs,sizeof(struct l2cap_pcb),MEMB_NUM_L2CAP_PCB);
38 MEMB(l2cap_listenpcbs,sizeof(struct l2cap_pcb_listen),MEMB_NUM_L2CAP_PCB_LISTEN);
39 MEMB(l2cap_sigs,sizeof(struct l2cap_sig),MEMB_NUM_L2CAP_SIG);
40 MEMB(l2cap_segs,sizeof(struct l2cap_seg),MEMB_NUM_L2CAP_SEG);
41
42 /*-----------------------------------------------------------------------------------*/
43 /*
44 * l2cap_init():
45 *
46 * Initializes the L2CAP layer.
47 */
48 /*-----------------------------------------------------------------------------------*/
49 void l2cap_init()
50 {
51 btmemb_init(&l2cap_pcbs);
52 btmemb_init(&l2cap_listenpcbs);
53 btmemb_init(&l2cap_sigs);
54 btmemb_init(&l2cap_segs);
55
56 /* Clear globals */
57 l2cap_listen_pcbs = NULL;
58 l2cap_active_pcbs = NULL;
59 l2cap_tmp_pcb = NULL;
60 l2cap_tmp_sig = NULL;
61 l2cap_insegs = NULL;
62 l2cap_tmp_inseg = NULL;
63 l2cap_disconnect_bb_cb = NULL;
64
65 /* Initialize the signal identifier (0x00 shall never be used) */
66 sigid_nxt = 0x00;
67 }
68
69 /*-----------------------------------------------------------------------------------*/
70 /*
71 * l2cap_tmr():
72 *
73 * Called every 1s and implements the retransmission timer that
74 * removes a channel if it has been waiting for a request enough
75 * time. It also includes a configuration timer.
76 */
77 /*-----------------------------------------------------------------------------------*/
78 void l2cap_tmr()
79 {
80 struct l2cap_sig *sig;
81 struct l2cap_pcb *pcb;
82 err_t ret;
83
84 (void) ret;
85
86 /* Step through all of the active pcbs */
87 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
88 /* Step through any unresponded signals */
89 for(sig = pcb->unrsp_sigs; sig != NULL; sig = sig->next) {
90 /* Check if channel is not reliable */
91 if(pcb->cfg.outflushto < 0xFFFF) {
92 /* Check if rtx is active. Otherwise ertx is active */
93 if(sig->rtx > 0) {
94 /* Adjust rtx timer */
95 --sig->rtx;
96 /* Check if rtx has expired */
97 if(sig->rtx == 0) {
98 if(sig->nrtx == 0) {
99 /* Move pcb to closed state */
100 pcb->state = L2CAP_CLOSED;
101 /* Indicate disconnect to upper layer */
102 LOG("l2cap_tmr: Max number of retransmissions (rtx) has expired\n");
103 L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
104 } else {
105 --sig->nrtx;
106 /* Indicate timeout to upper layer */
107 L2CA_ACTION_TO_IND(pcb,ERR_OK,ret);
108 /* Retransmitt signal w timeout doubled */
109 sig->rtx += sig->rtx;
110 ret = l2cap_rexmit_signal(pcb, sig);
111 }
112 } /* if */
113 } else {
114 /* Adjust ertx timer */
115 --sig->ertx;
116 /* Check if ertx has expired */
117 if(sig->ertx == 0) {
118 if(sig->nrtx == 0) {
119 /* Move pcb to closed state */
120 pcb->state = L2CAP_CLOSED;
121 /* Indicate disconnect to upper layer */
122 LOG("l2cap_tmr: Max number of retransmissions (ertx) has expired\n");
123 L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
124 } else {
125 --sig->nrtx;
126 /* Indicate timeout to upper layer */
127 L2CA_ACTION_TO_IND(pcb,ERR_OK,ret);
128 /* Disable ertx, activate rtx and retransmitt signal */
129 sig->ertx = 0;
130 sig->rtx = L2CAP_RTX;
131 ret = l2cap_rexmit_signal(pcb, sig);
132 }
133 } /* if */
134 } /* else */
135 } /* if */
136 } /* for */
137
138 /* Check configuration timer */
139 if(pcb->state == L2CAP_CONFIG) {
140 /* Check if configuration timer is active */
141 if(pcb->cfg.cfgto > 0) {
142 --pcb->cfg.cfgto;
143 //LOG("l2cap_tmr: Configuration timer = %d\n", pcb->cfg.cfgto);
144 /* Check if config timer has expired */
145 if(pcb->cfg.cfgto == 0) {
146 /* Connection attempt failed. Disconnect */
147 l2ca_disconnect_req(pcb, NULL);
148 /* Notify the application that the connection attempt failed */
149 if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
150 L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_CFG_TO, 0x0000, ret);
151 } else {
152 L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
153 }
154 pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset timer */
155 }
156 }
157 }
158 } /* for */
159 }
160
161 /*-----------------------------------------------------------------------------------*/
162 /*
163 * l2cap_write():
164 *
165 * Output L2CAP data to the lower layers. Segments the packet in to PDUs.
166 */
167 /*-----------------------------------------------------------------------------------*/
168 err_t l2cap_write(struct bd_addr *bdaddr, struct pbuf *p, u16_t len)
169 {
170 u8_t pb = L2CAP_ACL_START;
171 u16_t maxsize;
172 u16_t outsize;
173 err_t ret = ERR_OK;
174 struct pbuf *q;
175 u16_t i = 0;
176
177 /*u16_t i;
178 struct pbuf *q;
179 for(q = p; q != NULL; q = q->next) {
180 for(i = 0; i < q->len; ++i) {
181 LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: 0x%x\n", ((u8_t *)q->payload)[i]));
182 }
183 LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: *\n"));
184 }
185 */
186
187 maxsize = lp_pdu_maxsize();
188 q = p;
189
190 while(len && ret == ERR_OK) {
191 //LOG("l2cap_write: len %d maxsize %d p->len %d\n", len, maxsize, p->len);
192 if(len > maxsize) {
193 ret = lp_acl_write(bdaddr, q, maxsize, pb);
194 len -= maxsize;
195 outsize = maxsize;
196 //LOG("l2cap_write: Outsize before %d\n", outsize);
197 while(q->len < outsize) {
198 outsize -= q->len;
199 q = q->next;
200 }
201 //LOG("l2cap_write: Outsize after %d\n", outsize);
202 if(outsize) {
203 btpbuf_header(q, -outsize);
204 i += outsize;
205 }
206 pb = L2CAP_ACL_CONT;
207 LOG("l2cap_write: FRAG\n");
208 } else {
209 ret = lp_acl_write(bdaddr, q, len, pb);
210 len = 0;
211 }
212 }
213 btpbuf_header(q, i);
214 LOG("l2cap_write: DONE\n");
215 return ret;
216 }
217
218 /*-----------------------------------------------------------------------------------*/
219 /*
220 * l2cap_process_sig():
221 *
222 * Parses the received message handles it.
223 */
224 /*-----------------------------------------------------------------------------------*/
225 void l2cap_process_sig(struct pbuf *q, struct l2cap_hdr *l2caphdr, struct bd_addr *bdaddr)
226 {
227 struct l2cap_sig_hdr *sighdr;
228 struct l2cap_sig *sig = NULL;
229 struct l2cap_pcb *pcb = NULL;
230 struct l2cap_pcb_listen *lpcb;
231 struct l2cap_cfgopt_hdr *opthdr;
232 u16_t result, status, flags, psm, dcid;
233 u16_t len;
234 u16_t siglen;
235 struct pbuf *p, *r = NULL, *s = NULL, *data;
236 err_t ret;
237 u8_t i;
238 u16_t rspstate = L2CAP_CFG_SUCCESS;
239
240 (void)ret;
241
242 if(q->len != q->tot_len) {
243 LOG("l2cap_process_sig: Fragmented packet received. Reassemble into one buffer\n");
244 if((p = btpbuf_alloc(PBUF_RAW, q->tot_len, PBUF_RAM)) != NULL) {
245 i = 0;
246 for(r = q; r != NULL; r = r->next) {
247 memcpy(((u8_t *)p->payload) + i, r->payload, r->len);
248 i += r->len;
249 }
250 } else {
251 ERROR("l2cap_process_sig: Could not allocate buffer for fragmented packet\n");
252 return;
253 }
254 } else {
255 p = q;
256 }
257
258 len = l2caphdr->len;
259
260 while(len > 0) {
261 /* Set up signal header */
262 sighdr = p->payload;
263 btpbuf_header(p, -L2CAP_SIGHDR_LEN);
264
265 /* Check if this is a response/reject signal, and if so, find the matching request */
266 if(sighdr->code % 2) { /* if odd this is a resp/rej signal */
267 LOG("l2cap_process_sig: Response/reject signal received id = %d code = %d\n", sighdr->id, sighdr->code);
268 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
269 for(sig = pcb->unrsp_sigs; sig != NULL; sig = sig->next) {
270 if(sig->sigid == sighdr->id) {
271 break; /* found */
272 }
273 }
274 if(sig != NULL) {
275 break;
276 }
277 }
278 } else {
279 LOG("l2cap_process_sig: Request signal received id = %d code = %d\n", sighdr->id, sighdr->code);
280 }
281
282 /* Reject packet if length exceeds MTU */
283 if(l2caphdr->len > L2CAP_MTU) {
284 /* Alloc size of reason in cmd rej + MTU */
285 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+2, PBUF_RAM)) != NULL) {
286 ((u16_t *)data->payload)[0] = htole16(L2CAP_MTU_EXCEEDED);
287 ((u16_t *)data->payload)[1] = htole16(L2CAP_MTU);
288
289 l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
290 }
291 break;
292 }
293
294 switch(sighdr->code) {
295 case L2CAP_CMD_REJ:
296 /* Remove signal from unresponded list and deallocate it */
297 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
298 btpbuf_free(sig->p);
299 btmemb_free(&l2cap_sigs, sig);
300 LOG("l2cap_process_sig: Our command was rejected so we disconnect\n");
301 l2ca_disconnect_req(pcb, NULL);
302 break;
303 case L2CAP_CONN_REQ:
304 psm = le16toh(((u16_t *)p->payload)[0]);
305 /* Search for a listening pcb */
306 for(lpcb = l2cap_listen_pcbs; lpcb != NULL; lpcb = lpcb->next) {
307 if(bd_addr_cmp(&(lpcb->bdaddr),bdaddr) && lpcb->psm == psm) {
308 /* Found a listening pcb with the correct PSM & BD Address */
309 break;
310 }
311 }
312
313 //printf("l2cap_process_sig(L2CAP_CONN_REQ): psm = %04x, lpcb = %p\n",psm,lpcb);
314 /* If no matching pcb was found, send a connection rsp neg (PSM) */
315 if(lpcb == NULL) {
316 /* Alloc size of data in conn rsp signal */
317 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
318 ((u16_t *)data->payload)[0] = htole16(L2CAP_CONN_REF_PSM);
319 ((u16_t *)data->payload)[1] = 0; /* No further info available */
320 ret = l2cap_signal(NULL, L2CAP_CONN_RSP, sighdr->id, bdaddr, data);
321 }
322 } else {
323 /* Initiate a new active pcb */
324 pcb = l2cap_new();
325 if(pcb == NULL) {
326 LOG("l2cap_process_sig: could not allocate PCB\n");
327 /* Send a connection rsp neg (no resources available) and alloc size of data in conn rsp
328 signal */
329 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
330 ((u16_t *)data->payload)[0] = htole16(L2CAP_CONN_REF_RES);
331 ((u16_t *)data->payload)[1] = 0; /* No further info available */
332 ret = l2cap_signal(NULL, L2CAP_CONN_RSP, sighdr->id, bdaddr, data);
333 }
334 }
335 bd_addr_set(&(pcb->remote_bdaddr),bdaddr);
336
337 pcb->scid = l2cap_cid_alloc();
338 pcb->dcid = le16toh(((u16_t *)p->payload)[1]);
339 pcb->psm = psm;
340 pcb->callback_arg = lpcb->callback_arg;
341 pcb->l2ca_connect_ind = lpcb->l2ca_connect_ind;
342
343 pcb->state = L2CAP_CONFIG;
344 L2CAP_REG(&l2cap_active_pcbs, pcb);
345
346 LOG("l2cap_process_sig: A connection request was received. Send a response\n");
347 data = btpbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM);
348 if(data == NULL) {
349 ERROR("l2cap_connect_rsp: Could not allocate memory for pbuf\n");
350 break;
351 }
352 ((u16_t *)data->payload)[0] = htole16(pcb->scid);
353 ((u16_t *)data->payload)[1] = htole16(pcb->dcid);
354 ((u16_t *)data->payload)[2] = htole16(L2CAP_CONN_SUCCESS);
355 ((u16_t *)data->payload)[3] = 0x0000; /* No further information available */
356
357 /* Send the response */
358 ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
359 }
360 break;
361 case L2CAP_CONN_RSP:
362 if(pcb == NULL) {
363 /* A response without a matching request is silently discarded */
364 break;
365 }
366 LOG("l2cap_process_sig: conn rsp, active pcb->state == W4_L2CAP_CONNECT_RSP\n");
367 result = le16toh(((u16_t *)p->payload)[2]);
368 status = le16toh(((u16_t *)p->payload)[3]);
369 switch(result) {
370 case L2CAP_CONN_SUCCESS:
371 LOG("l2cap_process_sig: Conn_rsp_sucess, status %d\n", status);
372 LOG("l2cap_process_sig: conn rsp success, pcb->scid == %04x\n", ((u16_t *)p->payload)[1]);
373
374 /* Set destination connection id */
375 pcb->dcid = le16toh(((u16_t *)p->payload)[0]);
376
377 /* Remove signal from unresponded list and deallocate it */
378 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
379 btpbuf_free(sig->p);
380 btmemb_free(&l2cap_sigs, sig);
381
382 /* Configure connection */
383 pcb->state = L2CAP_CONFIG;
384
385 /* If initiator send a configuration request */
386 if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
387 l2ca_config_req(pcb);
388 pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
389 }
390 break;
391 case L2CAP_CONN_PND:
392 LOG("l2cap_process_sig: Conn_rsp_pnd, status %d\n", status);
393
394 /* Disable rtx and enable ertx */
395 sig->rtx = 0;
396 sig->ertx = L2CAP_ERTX;
397 break;
398 default:
399 LOG("l2cap_process_sig: Conn_rsp_neg, result %d\n", result);
400 /* Remove signal from unresponded list and deallocate it */
401 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
402 btpbuf_free(sig->p);
403 btmemb_free(&l2cap_sigs, sig);
404
405 L2CA_ACTION_CONN_CFM(pcb,result,status,ret);
406 break;
407 }
408 break;
409 case L2CAP_CFG_REQ:
410 siglen = le16toh(sighdr->len);
411 dcid = le16toh(((u16_t *)p->payload)[0]);
412 flags = le16toh(((u16_t *)p->payload)[1]);
413 siglen -= 4;
414 btpbuf_header(p, -4);
415
416
417 LOG("l2cap_process_sig: Congfiguration request, flags = %d\n", flags);
418
419 /* Find PCB with matching cid */
420 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
421 //LOG("l2cap_process_sig: dcid = 0x%x, pcb->scid = 0x%x, pcb->dcid = 0x%x\n\n", dcid, pcb->scid, pcb->dcid);
422 if(pcb->scid == dcid) {
423 /* Matching cid found */
424 break;
425 }
426 }
427 /* If no matching cid was found, send a cmd reject (Invalid cid) */
428 if(pcb == NULL) {
429 LOG("l2cap_process_sig: Cfg req: no matching cid was found\n");
430 /* Alloc size of reason in cmd rej + data (dcid + scid) */
431 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
432 ((u16_t *)data->payload)[0] = htole16(L2CAP_INVALID_CID);
433 ((u16_t *)data->payload)[1] = htole16(dcid); /* Requested local cid */
434 ((u16_t *)data->payload)[2] = htole16(L2CAP_NULL_CID); /* Remote cid not known */
435
436 ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
437 }
438 } else { /* Handle config request */
439 LOG("l2cap_process_sig: Handle configuration request\n");
440 pcb->ursp_id = sighdr->id; /* Set id of request to respond to */
441
442 /* Parse options and add to pcb */
443 while(siglen > 0) {
444 LOG("l2cap_process_sig: Siglen = %d\n", siglen);
445 opthdr = p->payload;
446 /* Check if type of action bit indicates a non-hint. Hints are ignored */
447 LOG("l2cap_process_sig: Type of action bit = %d\n", L2CAP_OPTH_TOA(opthdr));
448 if(L2CAP_OPTH_TOA(opthdr) == 0) {
449 LOG("l2cap_process_sig: Type = %d\n", L2CAP_OPTH_TYPE(opthdr));
450 LOG("l2cap_process_sig: Length = %d\n", opthdr->len);
451 switch(L2CAP_OPTH_TYPE(opthdr)) {
452 case L2CAP_CFG_MTU:
453 LOG("l2cap_process_sig: Out MTU = %d\n", le16toh(((u16_t *)p->payload)[1]));
454 pcb->cfg.outmtu = le16toh(((u16_t *)p->payload)[1]);
455 break;
456 case L2CAP_FLUSHTO:
457 LOG("l2cap_process_sig: In flush timeout = %d\n", ((u16_t *)p->payload)[1]);
458 pcb->cfg.influshto = le16toh(((u16_t *)p->payload)[1]);
459 break;
460 case L2CAP_QOS:
461 /* If service type is Best Effort or No Traffic the remainder fields will be ignored */
462 if(((u8_t *)p->payload)[3] == L2CAP_QOS_GUARANTEED) {
463 LOG("l2cap_process_sig: This implementation does not support the guaranteed QOS service type");
464 if(rspstate == L2CAP_CFG_SUCCESS) {
465 rspstate = L2CAP_CFG_UNACCEPT;
466 if(pcb->cfg.opt != NULL) {
467 btpbuf_free(pcb->cfg.opt);
468 pcb->cfg.opt = NULL;
469 }
470 }
471 s = btpbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
472 memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
473 if(pcb->cfg.opt == NULL) {
474 pcb->cfg.opt = s;
475 } else {
476 btpbuf_chain(pcb->cfg.opt, s);
477 btpbuf_free(s);
478 }
479 }
480 break;
481 default:
482 if(rspstate != L2CAP_CFG_REJ) {
483 /* Unknown option. Add to unknown option type buffer */
484 if(rspstate != L2CAP_CFG_UNKNOWN) {
485 rspstate = L2CAP_CFG_UNKNOWN;
486 if(pcb->cfg.opt != NULL) {
487 btpbuf_free(pcb->cfg.opt);
488 pcb->cfg.opt = NULL;
489 }
490 }
491 s = btpbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
492 memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
493 if(pcb->cfg.opt == NULL) {
494 pcb->cfg.opt = s;
495 } else {
496 btpbuf_chain(pcb->cfg.opt, s);
497 btpbuf_free(s);
498 }
499 }
500 break;
501 } /* switch */
502 } /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
503 btpbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
504 siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
505 } /* while */
506
507 /* If continuation flag is set we don't send the final response just yet */
508 if((flags & 0x0001) == 1) {
509 /* Send success result with no options until the full request has been received */
510 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) == NULL) {
511 ERROR("l2cap_process_sig: Could not allocate memory for pbuf\n");
512 break;
513 }
514 ((u16_t *)data->payload)[0] = htole16(pcb->dcid);
515 ((u16_t *)data->payload)[1] = 0;
516 ((u16_t *)data->payload)[2] = htole16(L2CAP_CFG_SUCCESS);
517 ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
518 break;
519 }
520
521 /* Send a configure request for outgoing link if it hasnt been configured */
522 if(!(pcb->cfg.l2capcfg & L2CAP_CFG_IR) && !(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_REQ)) {
523 l2ca_config_req(pcb);
524 pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
525 }
526
527 /* Send response to configuration request */
528 LOG("l2cap_process_sig: Send response to configuration request\n");
529 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) != NULL) {
530 ((u16_t *)data->payload)[0] = htole16(pcb->dcid);
531 ((u16_t *)data->payload)[1] = 0; /* Flags (No continuation) */
532 ((u16_t *)data->payload)[2] = htole16(rspstate); /* Result */
533 if(pcb->cfg.opt != NULL) {
534 LOG("l2cap_process_sig: pcb->cfg.opt->len = %d\n", pcb->cfg.opt->len);
535 btpbuf_chain(data, pcb->cfg.opt); /* Add option type buffer to data buffer */
536 btpbuf_free(pcb->cfg.opt);
537 pcb->cfg.opt = NULL;
538 }
539 ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
540 }
541
542 if(rspstate == L2CAP_CFG_SUCCESS) {
543 pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_SUCCESS;
544 /* L2CAP connection established if a successful configuration response has been sent */
545 if(pcb->cfg.l2capcfg & L2CAP_CFG_IN_SUCCESS) {
546 /* IPCP connection established, notify upper layer that connection is open */
547 pcb->state = L2CAP_OPEN;
548 if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
549 L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
550 } else {
551 L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
552 }
553 }
554 }
555 } /* else */
556 break;
557 case L2CAP_CFG_RSP:
558 if(pcb == NULL) {
559 /* A response without a matching request is silently discarded */
560 LOG("l2cap_process_sig: discarded response without matching request\n");
561 break;
562 }
563
564 /* Remove signal from unresponded list and deallocate it */
565 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
566 btpbuf_free(sig->p);
567 btmemb_free(&l2cap_sigs, sig);
568
569 siglen = le16toh(sighdr->len);
570 //scid = le16toh(((u16_t *)p->payload)[0]);
571 flags = le16toh(((u16_t *)p->payload)[1]);
572 result = le16toh(((u16_t *)p->payload)[2]);
573 siglen -= 6;
574 btpbuf_header(p, -6);
575
576 LOG("l2cap_process_sig: Outgoing configuration result == %d continuation flag == %d\n", result, flags);
577
578 /* Handle config request */
579 switch(result) {
580 case L2CAP_CFG_SUCCESS:
581 LOG("l2cap_process_sig: Successfull outgoing configuration\n");
582 pcb->cfg.l2capcfg |= L2CAP_CFG_IN_SUCCESS; /* Local side of the connection
583 has been configured for outgoing data */
584 pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset configuration timeout */
585
586 if(pcb->cfg.outflushto != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
587 lp_write_flush_timeout(&pcb->remote_bdaddr, pcb->cfg.outflushto);
588 }
589
590 /* L2CAP connection established if a successful configuration response has been sent */
591 if(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_SUCCESS) {
592 pcb->state = L2CAP_OPEN;
593 if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
594 L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
595 } else {
596 L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
597 }
598 }
599 break;
600 case L2CAP_CFG_UNACCEPT:
601 /* Parse and add options to pcb */
602 while(siglen > 0) {
603 opthdr = p->payload;
604 /* Check if type of action bit indicates a non-hint. Hints are ignored */
605 if(L2CAP_OPTH_TOA(opthdr) == 0) {
606 switch(L2CAP_OPTH_TYPE(opthdr)) {
607 case L2CAP_CFG_MTU:
608 if(L2CAP_MTU > le16toh(((u16_t *)p->payload)[1])) {
609 pcb->cfg.outmtu = le16toh(((u16_t *)p->payload)[1]);
610 } else {
611 ERROR("l2cap_process_sig: Configuration of MTU failed\n");
612 l2ca_disconnect_req(pcb, NULL);
613 return;
614 }
615 break;
616 case L2CAP_FLUSHTO:
617 pcb->cfg.influshto = le16toh(((u16_t *)p->payload)[1]);
618 break;
619 case L2CAP_QOS:
620 /* If service type Best Effort is not accepted we will close the connection */
621 if(((u8_t *)p->payload)[3] != L2CAP_QOS_BEST_EFFORT) {
622 ERROR("l2cap_process_sig: Unsupported service type\n");
623 l2ca_disconnect_req(pcb, NULL);
624 return;
625 }
626 break;
627 default:
628 /* Should not happen, skip option */
629 break;
630 } /* switch */
631 } /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
632 btpbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
633 siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
634 } /* while */
635
636 /* Send out a new configuration request if the continuation flag isn't set */
637 if((flags & 0x0001) == 0) {
638 l2ca_config_req(pcb);
639 }
640 break;
641 case L2CAP_CFG_REJ:
642 /* Fallthrough */
643 case L2CAP_CFG_UNKNOWN:
644 /* Fallthrough */
645 default:
646 if((flags & 0x0001) == 0) {
647 LOG("l2cap_process_sig: Configuration failed\n");
648 l2ca_disconnect_req(pcb, NULL);
649 return;
650 }
651 break;
652 } /* switch(result) */
653
654 /* If continuation flag is set we must send a NULL configuration request */
655 if((flags & 0x0001) == 1) {
656 LOG("l2cap_process_sig: Continuation flag is set. Send empty (default) config request signal\n");
657 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
658 ERROR("l2cap_process_sig: Could not allocate memory for pbuf\n");
659 return;
660 }
661 /* Assemble config request packet */
662 ((u16_t *)data->payload)[0] = htole16(pcb->scid);
663 ((u16_t *)data->payload)[2] = 0;
664 l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), data);
665 }
666 break;
667 case L2CAP_DISCONN_REQ:
668 siglen = le16toh(sighdr->len);
669 dcid = le16toh(((u16_t *)p->payload)[0]);
670 siglen = siglen - 2;
671 flags = le16toh(((u16_t *)p->payload)[1]);
672 siglen = siglen - 2;
673 btpbuf_header(p, -4);
674
675 /* Find PCB with matching cid */
676 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
677 if(pcb->scid == dcid) {
678 /* Matching cid found */
679 break;
680 }
681 }
682 /* If no matching cid was found, send a cmd reject (Invalid cid) */
683 if(pcb == NULL) {
684 /* Alloc size of reason in cmd rej + data (dcid + scid) */
685 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
686 ((u16_t *)data->payload)[0] = htole16(L2CAP_INVALID_CID);
687 ((u16_t *)data->payload)[1] = htole16(dcid); /* Requested local cid */
688 ((u16_t *)data->payload)[2] = htole16(L2CAP_NULL_CID); /* Remote cid not known */
689
690 ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
691 }
692 } else { /* Handle disconnection request */
693 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_DISCONN_RSP_SIZE, PBUF_RAM)) != NULL) {
694 ((u16_t *)data->payload)[0] = htole16(pcb->scid);
695 ((u16_t *)data->payload)[1] = htole16(pcb->dcid);
696 ret = l2cap_signal(pcb, L2CAP_DISCONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
697
698 /* Give upper layer indication */
699 pcb->state = L2CAP_CLOSED;
700 LOG("l2cap_process_sig: Disconnection request\n");
701 L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
702 }
703 }
704 break;
705 case L2CAP_DISCONN_RSP:
706 if(pcb == NULL) {
707 /* A response without a matching request is silently discarded */
708 break;
709 }
710 /* Remove signal from unresponded list and deallocate it */
711 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
712 btpbuf_free(sig->p);
713 btmemb_free(&l2cap_sigs, sig);
714
715 L2CA_ACTION_DISCONN_CFM(pcb,ret); /* NOTE: Application should
716 now close the connection */
717 break;
718 case L2CAP_ECHO_REQ:
719 pcb->ursp_id = sighdr->id;
720 ret = l2cap_signal(pcb, L2CAP_ECHO_RSP, sighdr->id, &(pcb->remote_bdaddr), NULL);
721 break;
722 case L2CAP_ECHO_RSP:
723 if(pcb == NULL) {
724 /* A response without a matching request is silently discarded */
725 break;
726 }
727 /* Remove signal from unresponded list and deallocate it */
728 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
729 btpbuf_free(sig->p);
730 btmemb_free(&l2cap_sigs, sig);
731
732 /* Remove temporary pcb from active list */
733 L2CAP_RMV(&l2cap_active_pcbs, pcb);
734 L2CA_ACTION_PING_CFM(pcb,L2CAP_ECHO_RCVD,ret);
735 break;
736 default:
737 /* Alloc size of reason in cmd rej */
738 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE, PBUF_RAM)) != NULL) {
739 ((u16_t *)data->payload)[0] = htole16(L2CAP_CMD_NOT_UNDERSTOOD);
740
741 ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
742 }
743 break;
744 } /* switch */
745 len = len - (le16toh(sighdr->len) + L2CAP_SIGHDR_LEN);
746 btpbuf_header(p, -(le16toh(sighdr->len)));
747 } /* while */
748 }
749
750 /*-----------------------------------------------------------------------------------*/
751 /*
752 * l2cap_input():
753 *
754 * Called by the lower layer. Reassembles the packet, parses the header and forward
755 * it to the upper layer or the signal handler.
756 */
757 /*-----------------------------------------------------------------------------------*/
758 void l2cap_input(struct pbuf *p, struct bd_addr *bdaddr)
759 {
760 struct l2cap_seg *inseg;
761 struct hci_acl_hdr *aclhdr;
762 struct pbuf *data;
763 err_t ret;
764
765 (void)ret;
766
767 btpbuf_header(p, HCI_ACL_HDR_LEN);
768 aclhdr = p->payload;
769 btpbuf_header(p, -HCI_ACL_HDR_LEN);
770
771 btpbuf_realloc(p, aclhdr->len);
772
773 for(inseg = l2cap_insegs; inseg != NULL; inseg = inseg->next) {
774 if(bd_addr_cmp(bdaddr, &(inseg->bdaddr))) {
775 break;
776 }
777 }
778
779 aclhdr->connhdl_pb_bc = le16toh(aclhdr->connhdl_pb_bc);
780 aclhdr->len = le16toh(aclhdr->len);
781 /* Reassembly procedures */
782 /* Check if continuing fragment or start of L2CAP packet */
783 if(((aclhdr->connhdl_pb_bc >> 12) & 0x03)== L2CAP_ACL_CONT) { /* Continuing fragment */
784 if(inseg == NULL) {
785 /* Discard packet */
786 LOG("l2cap_input: Continuing fragment. Discard packet\n");
787 btpbuf_free(p);
788 return;
789 } else if(inseg->p->tot_len + p->tot_len > inseg->len) { /* Check if length of
790 segment exceeds
791 l2cap header length */
792 /* Discard packet */
793 LOG("l2cap_input: Continuing fragment. Length exceeds L2CAP hdr length. Discard packet\n");
794 btpbuf_free(inseg->p);
795 L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
796 btmemb_free(&l2cap_segs, inseg);
797
798 btpbuf_free(p);
799 return;
800 }
801 /* Add pbuf to segement */
802 btpbuf_chain(inseg->p, p);
803 btpbuf_free(p);
804
805 } else if(((aclhdr->connhdl_pb_bc >> 12) & 0x03) == L2CAP_ACL_START) { /* Start of L2CAP packet */
806 //LOG("l2cap_input: Start of L2CAP packet p->len = %d, p->tot_len = %d\n", p->len, p->tot_len);
807 if(inseg != NULL) { /* Check if there are segments missing in a previous packet */
808 /* Discard previous packet */
809 LOG("l2cap_input: Start of L2CAP packet. Discard previous packet\n");
810 btpbuf_free(inseg->p);
811 } else {
812 inseg = btmemb_alloc(&l2cap_segs);
813 bd_addr_set(&(inseg->bdaddr), bdaddr);
814 L2CAP_SEG_REG(&(l2cap_insegs), inseg);
815 }
816 inseg->p = p;
817 inseg->l2caphdr = p->payload;
818 inseg->l2caphdr->cid = le16toh(inseg->l2caphdr->cid);
819 inseg->l2caphdr->len = le16toh(inseg->l2caphdr->len);
820
821 inseg->len = inseg->l2caphdr->len + L2CAP_HDR_LEN;
822 for(inseg->pcb = l2cap_active_pcbs; inseg->pcb != NULL; inseg->pcb = inseg->pcb->next) {
823 if(inseg->pcb->scid == inseg->l2caphdr->cid) {
824 break; /* found */
825 }
826 }
827 } else {
828 /* Discard packet */
829 LOG("l2cap_input: Discard packet\n");
830 btpbuf_free(inseg->p);
831 L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
832 btmemb_free(&l2cap_segs, inseg);
833
834 btpbuf_free(p);
835 return;
836 }
837 if(inseg->p->tot_len < inseg->len) {
838 LOG("l2cap_input: Get continuing segments\n");
839 return; /* Get continuing segments */
840 }
841
842 /* Handle packet */
843 switch(inseg->l2caphdr->cid) {
844 case L2CAP_NULL_CID:
845 /* Illegal */
846 LOG("l2cap_input: Illegal null cid\n");
847 btpbuf_free(inseg->p);
848 break;
849 case L2CAP_SIG_CID:
850 btpbuf_header(inseg->p, -L2CAP_HDR_LEN);
851 l2cap_process_sig(inseg->p, inseg->l2caphdr, bdaddr);
852 btpbuf_free(inseg->p);
853 break;
854 case L2CAP_CONNLESS_CID:
855 /* Not needed by PAN, LAN access or DUN profiles */
856 btpbuf_free(inseg->p);
857 break;
858 default:
859 if(inseg->l2caphdr->cid < 0x0040 || inseg->pcb == NULL) {
860 /* Reserved for specific L2CAP functions or channel does not exist */
861 /* Alloc size of reason in cmd rej */
862 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
863 ((u16_t *)data->payload)[0] = htole16(L2CAP_INVALID_CID);
864 ((u16_t *)data->payload)[1] = htole16(inseg->l2caphdr->cid);
865 ((u16_t *)data->payload)[2] = htole16(L2CAP_NULL_CID);
866
867 ret = l2cap_signal(NULL, L2CAP_CMD_REJ, l2cap_next_sigid(), bdaddr, data);
868 }
869 btpbuf_free(inseg->p);
870 break;
871 }
872
873 btpbuf_header(inseg->p, -L2CAP_HDR_LEN);
874
875 /* Forward packet to higher layer */
876 LOG("l2cap_input: Forward packet to higher layer\n");
877 /*
878 LOG("l2cap_input: Remote BD address: 0x%x:0x%x:0x%x:0x%x:0x%x:0x%x\n",
879 inseg->pcb->remote_bdaddr.addr[5],
880 inseg->pcb->remote_bdaddr.addr[4],
881 inseg->pcb->remote_bdaddr.addr[3],
882 inseg->pcb->remote_bdaddr.addr[2],
883 inseg->pcb->remote_bdaddr.addr[1],
884 inseg->pcb->remote_bdaddr.addr[0]));
885 */
886 L2CA_ACTION_RECV(inseg->pcb,inseg->p,ERR_OK,ret);
887 break;
888 }
889
890 /* Remove input segment */
891 L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
892 btmemb_free(&l2cap_segs, inseg);
893 }
894
895 /*-----------------------------------------------------------------------------------*/
896 /*
897 * l2cap_cid_alloc():
898 *
899 * Allocates a channel identifier (CID). They are local names representing a logical
900 * channel endpoint on the device.
901 */
902 /*-----------------------------------------------------------------------------------*/
903 static u16_t l2cap_cid_alloc(void)
904 {
905 u16_t cid;
906 struct l2cap_pcb *pcb;
907
908 for (cid = L2CAP_MIN_CID; cid < L2CAP_MAX_CID; ++cid) {
909 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
910 if(pcb->scid == cid) {
911 break;
912 }
913 }
914 if(pcb == NULL) {
915 return cid;
916 }
917 }
918 return 0;
919 }
920
921 /*-----------------------------------------------------------------------------------*/
922 /*
923 * l2cap_new():
924 *
925 * Creates a new L2CAP protocol control block but doesn't place it on
926 * any of the L2CAP PCB lists.
927 */
928 /*-----------------------------------------------------------------------------------*/
929 struct l2cap_pcb* l2cap_new(void)
930 {
931 struct l2cap_pcb *pcb;
932
933 pcb = btmemb_alloc(&l2cap_pcbs);
934 if(pcb != NULL) {
935 memset(pcb, 0, sizeof(struct l2cap_pcb));
936 pcb->state = L2CAP_CLOSED;
937
938 /* Initialize configuration parameter options with default values */
939
940 /* Maximum Transmission Unit */
941 pcb->cfg.inmtu = L2CAP_MTU; /* The MTU that this implementation support */
942 pcb->cfg.outmtu = 672; /* Default MTU. Two Baseband DH5 packets minus the Baseband ACL headers and
943 L2CAP header. This can be set here since we will never send any signals
944 larger than the L2CAP sig MTU (48 bytes) before L2CAP has been configured
945 */
946
947 /* Flush Timeout */
948 pcb->cfg.influshto = 0xFFFF;
949 pcb->cfg.outflushto = 0xFFFF;
950
951 pcb->cfg.cfgto = L2CAP_CFG_TO; /* Maximum time before terminating a negotiation.
952 Cfg shall not last more than 120s */
953 pcb->cfg.opt = NULL;
954 return pcb;
955 }
956 ERROR("l2cap_new: Could not allocate memory for pcb\n");
957 return NULL;
958 }
959
960 /*-----------------------------------------------------------------------------------*/
961 /*
962 * l2cap_close():
963 *
964 * Closes the L2CAP protocol control block.
965 */
966 /*-----------------------------------------------------------------------------------*/
967 err_t l2cap_close(struct l2cap_pcb *pcb)
968 {
969 struct l2cap_sig *tmpsig;
970
971 if(pcb->state == L2CAP_LISTEN) {
972 L2CAP_RMV((struct l2cap_pcb**)((void*)&(l2cap_listen_pcbs)), pcb);
973 btmemb_free(&l2cap_listenpcbs, pcb);
974 } else {
975 L2CAP_RMV(&(l2cap_active_pcbs), pcb);
976 /* Free any unresponded signals */
977 while(pcb->unrsp_sigs != NULL) {
978 tmpsig = pcb->unrsp_sigs;
979 pcb->unrsp_sigs = pcb->unrsp_sigs->next;
980 btmemb_free(&l2cap_sigs, tmpsig);
981 }
982
983 btmemb_free(&l2cap_pcbs, pcb);
984 }
985 pcb = NULL;
986 return ERR_OK;
987 }
988 /*-----------------------------------------------------------------------------------*/
989 /*
990 * l2cap_reset_all():
991 *
992 * Closes all active and listening L2CAP protocol control blocks.
993 */
994 /*-----------------------------------------------------------------------------------*/
995 void l2cap_reset_all(void)
996 {
997 struct l2cap_pcb *pcb, *tpcb;
998 struct l2cap_pcb_listen *lpcb, *tlpcb;
999 struct l2cap_seg *seg, *tseg;
1000
1001 for(pcb = l2cap_active_pcbs; pcb != NULL;) {
1002 tpcb = pcb->next;
1003 l2cap_close(pcb);
1004 pcb = tpcb;
1005 }
1006
1007 for(lpcb = l2cap_listen_pcbs; lpcb != NULL;) {
1008 tlpcb = lpcb->next;
1009 l2cap_close((struct l2cap_pcb *)lpcb);
1010 lpcb = tlpcb;
1011 }
1012
1013 for(seg = l2cap_insegs; seg != NULL;) {
1014 tseg = seg->next;
1015 L2CAP_SEG_RMV(&(l2cap_insegs), seg);
1016 btmemb_free(&l2cap_segs, seg);
1017 seg = tseg;
1018 }
1019
1020 l2cap_init();
1021 }
1022
1023 /*-----------------------------------------------------------------------------------*/
1024 /* L2CAP to L2CAP signalling events
1025 */
1026 /*-----------------------------------------------------------------------------------*/
1027 /*-----------------------------------------------------------------------------------*/
1028 /*
1029 * l2cap_signal():
1030 *
1031 * Assembles the signalling packet and passes it to the lower layer.
1032 */
1033 /*-----------------------------------------------------------------------------------*/
1034 err_t l2cap_signal(struct l2cap_pcb *pcb, u8_t code, u16_t ursp_id, struct bd_addr *remote_bdaddr, struct pbuf *data)
1035 {
1036 struct l2cap_sig *sig;
1037 struct l2cap_sig_hdr *sighdr;
1038 struct l2cap_hdr *hdr;
1039 err_t ret;
1040
1041 /* Alloc a new signal */
1042 LOG("l2cap_signal: Allocate memory for l2cap_sig. Code = 0x%x\n", code);
1043 if((sig = btmemb_alloc(&l2cap_sigs)) == NULL) {
1044 ERROR("l2cap_signal: could not allocate memory for l2cap_sig\n");
1045 return ERR_MEM;
1046 }
1047
1048 /* Alloc a pbuf for signal */
1049 if((sig->p = btpbuf_alloc(PBUF_RAW, L2CAP_HDR_LEN+L2CAP_SIGHDR_LEN, PBUF_RAM)) == NULL) {
1050 ERROR("l2cap_signal: could not allocate memory for pbuf\n");
1051 return ERR_MEM;
1052 }
1053
1054 /* Setup signal header and leave room for l2cap hdr */
1055 sighdr = (struct l2cap_sig_hdr *)(((u8_t *)sig->p->payload)+L2CAP_HDR_LEN);
1056
1057 /* Chain data to signal and set length of signal data */
1058 if(data == NULL) {
1059 sighdr->len = 0;
1060 } else {
1061 btpbuf_chain(sig->p, data);
1062 btpbuf_free(data);
1063 sighdr->len = htole16(data->tot_len);
1064 }
1065
1066 sighdr->code = code;
1067
1068 if(sighdr->code % 2) { /* If odd this is a resp/rej signal */
1069 sig->sigid = ursp_id; /* Get id */
1070 LOG("l2cap_signal: Sending response/reject signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1071 } else {
1072 sig->sigid = l2cap_next_sigid(); /* Alloc id */
1073 sig->rtx = L2CAP_RTX; /* Set Response Timeout Expired timer (in seconds)
1074 should be at least as large as the BB flush timeout */
1075 sig->nrtx = L2CAP_MAXRTX; /* Set max number of retransmissions */
1076 LOG("l2cap_signal: Sending request signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1077 }
1078 sighdr->id = sig->sigid; /* Set id */
1079
1080 /* Set up L2CAP hdr */
1081 hdr = sig->p->payload;
1082 hdr->len = htole16((sig->p->tot_len - L2CAP_HDR_LEN));
1083 hdr->cid = htole16(L2CAP_SIG_CID); /* 0x0001 */
1084
1085 ret = l2cap_write(remote_bdaddr, sig->p, sig->p->tot_len); /* Send peer L2CAP signal */
1086
1087 /* Put signal on unresponded list if it's a request signal, else deallocate it */
1088 if(ret == ERR_OK && (sighdr->code % 2) == 0) {
1089 LOG("l2cap_signal: Registering sent request signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1090 L2CAP_SIG_REG(&(pcb->unrsp_sigs), sig);
1091 } else {
1092 LOG("l2cap_signal: Deallocating sent response/reject signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1093 btpbuf_free(sig->p);
1094 sig->p = NULL;
1095 btmemb_free(&l2cap_sigs, sig);
1096 }
1097
1098 return ret;
1099 }
1100
1101 /*-----------------------------------------------------------------------------------*/
1102 /*
1103 * l2cap_rexmit_signal():
1104 *
1105 * Called by the l2cap timer. Retransmitts a signal.
1106 */
1107 /*-----------------------------------------------------------------------------------*/
1108 err_t l2cap_rexmit_signal(struct l2cap_pcb *pcb, struct l2cap_sig *sig)
1109 {
1110 err_t ret;
1111
1112 /* Set up L2CAP hdr */
1113 ret = l2cap_write(&(pcb->remote_bdaddr), sig->p, sig->p->tot_len); /* Send peer L2CAP signal */
1114
1115 return ret;
1116 }
1117 /*-----------------------------------------------------------------------------------*/
1118 /* Upper-Layer to L2CAP signaling events
1119 */
1120 /*-----------------------------------------------------------------------------------*/
1121 /*-----------------------------------------------------------------------------------*/
1122 /*
1123 * l2ca_connect_req():
1124 *
1125 * Initiates the sending of a connect request message. Requests the creation of a
1126 * channel representing a logicalconnection to a physical address. Input parameters
1127 * are the target protocol(PSM) and remote devices 48-bit address (BD_ADDR). Also
1128 * specify the function to be called when a confirm has been received.
1129 */
1130 /*-----------------------------------------------------------------------------------*/
1131 err_t l2ca_connect_req(struct l2cap_pcb *pcb, struct bd_addr *bdaddr, u16_t psm,
1132 u8_t role_switch, err_t (* l2ca_connect_cfm)(void *arg, struct l2cap_pcb *lpcb,
1133 u16_t result, u16_t status))
1134 {
1135 err_t ret;
1136 struct pbuf *data;
1137
1138 if(bdaddr != NULL) {
1139 bd_addr_set(&(pcb->remote_bdaddr),bdaddr);
1140 } else {
1141 return ERR_VAL;
1142 }
1143
1144 pcb->psm = psm;
1145 pcb->l2ca_connect_cfm = l2ca_connect_cfm;
1146 pcb->scid = l2cap_cid_alloc();
1147
1148 pcb->cfg.l2capcfg |= L2CAP_CFG_IR; /* We are the initiator of this connection */
1149
1150 if(!lp_is_connected(bdaddr)) {
1151 ret = lp_connect_req(bdaddr, role_switch); /* Create ACL link w pcb state == CLOSED */
1152 } else {
1153 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CONN_REQ_SIZE, PBUF_RAM)) == NULL) {
1154 ERROR("l2cap_connect_req: Could not allocate memory for pbuf\n");
1155 return ERR_MEM;
1156 }
1157 ((u16_t *)data->payload)[0] = htole16(psm);
1158 ((u16_t *)data->payload)[1] = htole16(pcb->scid);
1159 ret = l2cap_signal(pcb, L2CAP_CONN_REQ, 0, &(pcb->remote_bdaddr), data); /* Send l2cap_conn_req signal */
1160
1161 pcb->state = W4_L2CAP_CONNECT_RSP;
1162 }
1163
1164 L2CAP_REG(&(l2cap_active_pcbs), pcb);
1165
1166 return ret;
1167 }
1168
1169 /*-----------------------------------------------------------------------------------*/
1170 /*
1171 * l2ca_config_req():
1172 *
1173 * Requests the initial configuration (or reconfiguration) of a channel to a new set
1174 * of channel parameters. Input parameters are the local CID endpoint, new incoming
1175 * receivable MTU (InMTU), new outgoing flow specification, and flush and link
1176 * timeouts. Also specify the function to be called when a confirm has been received.
1177 */
1178 /*-----------------------------------------------------------------------------------*/
1179 err_t l2ca_config_req(struct l2cap_pcb *pcb)
1180 {
1181 struct pbuf *p, *q;
1182 struct l2cap_cfgopt_hdr *opthdr;
1183 err_t ret;
1184
1185 switch(pcb->state) {
1186 case L2CAP_OPEN:
1187 LOG("l2cap_config_req: state = L2CAP_OPEN. Suspend transmission\n");
1188 /* Note: Application should have suspended data transmission, otherwise outgoing data will be
1189 dropped */
1190 pcb->state = L2CAP_CONFIG;
1191 /* Fallthrough */
1192 case L2CAP_CONFIG:
1193 LOG("l2cap_config_req: state = L2CAP_CONFIG\n");
1194
1195 if((p = btpbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
1196 ERROR("l2cap_config_req: Could not allocate memory for pbuf\n");
1197 return ERR_MEM;
1198 }
1199
1200 /* Assemble config request packet. Only options that has to be changed will be
1201 sent */
1202 ((u16_t *)p->payload)[0] = htole16(pcb->dcid);
1203 /* In this implementation we do not send multiple cmds in one
1204 signal packet. Therefore we will never send a config_req packet
1205 that will cause the signal to be larger than the minimum L2CAP MTU
1206 48 bytes. Hence, this flag will always be cleared */
1207 ((u16_t *)p->payload)[1] = 0;
1208
1209 /* Add MTU and out flush timeout to cfg packet if not default value. QoS (Best effort) is always
1210 set to default and can be skipped */
1211 if(pcb->cfg.inmtu != L2CAP_CFG_DEFAULT_INMTU) {
1212 if((q = btpbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + L2CAP_MTU_LEN, PBUF_RAM)) == NULL) {
1213 ERROR("l2cap_config_req: Could not allocate memory for pbuf\n");
1214 btpbuf_free(p);
1215 return ERR_MEM;
1216 }
1217 opthdr = q->payload;
1218 opthdr->type = L2CAP_CFG_MTU;
1219 opthdr->len = L2CAP_MTU_LEN;
1220 ((u16_t *)q->payload)[1] = htole16(pcb->cfg.inmtu);
1221 btpbuf_chain(p, q);
1222 btpbuf_free(q);
1223 }
1224
1225 if(L2CAP_OUT_FLUSHTO != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
1226 if((q = btpbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + L2CAP_FLUSHTO_LEN, PBUF_RAM)) == NULL) {
1227 ERROR("l2cap_config_req: Could not allocate memory for pbuf\n");
1228 btpbuf_free(p);
1229 return ERR_MEM;
1230 }
1231 opthdr = q->payload;
1232 opthdr->type = L2CAP_FLUSHTO;
1233 opthdr->len = L2CAP_FLUSHTO_LEN;
1234 pcb->cfg.outflushto = L2CAP_OUT_FLUSHTO;
1235 ((u16_t *)q->payload)[1] = htole16(pcb->cfg.outflushto);
1236 btpbuf_chain(p, q);
1237 btpbuf_free(q);
1238 }
1239
1240 /* Send config request signal */
1241 ret = l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), p);
1242 break;
1243 default:
1244 ERROR("l2cap_config_req: state = L2CAP_?. Invalid state\n");
1245 return ERR_CONN; /* Invalid state. Connection is not in OPEN or CONFIG state */
1246 }
1247 return ret;
1248 }
1249 /*-----------------------------------------------------------------------------------*/
1250 /*
1251 * l2ca_disconnect_req():
1252 *
1253 * Requests the disconnection of the channel. Also specify the function to be called
1254 * when a confirm is received
1255 */
1256 /*-----------------------------------------------------------------------------------*/
1257 err_t l2ca_disconnect_req(struct l2cap_pcb *pcb, err_t (* l2ca_disconnect_cfm)(void *arg, struct l2cap_pcb *pcb))
1258 {
1259 struct pbuf *data;
1260 err_t ret;
1261
1262 if(pcb->state == L2CAP_OPEN || pcb->state == L2CAP_CONFIG) {
1263 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_DISCONN_REQ_SIZE, PBUF_RAM)) == NULL) {
1264 ERROR("l2cap_disconnect_req: Could not allocate memory for pbuf\n");
1265 return ERR_MEM;
1266 }
1267 pcb->l2ca_disconnect_cfm = l2ca_disconnect_cfm;
1268
1269 ((u16_t *)data->payload)[0] = htole16(pcb->dcid);
1270 ((u16_t *)data->payload)[1] = htole16(pcb->scid);
1271
1272 ret = l2cap_signal(pcb, L2CAP_DISCONN_REQ, 0, &(pcb->remote_bdaddr), data);
1273
1274 if(ret == ERR_OK) {
1275 pcb->state = W4_L2CAP_DISCONNECT_RSP;
1276 }
1277 } else {
1278 return ERR_CONN; /* Signal not supported in this state */
1279 }
1280
1281 return ret;
1282 }
1283 /*-----------------------------------------------------------------------------------*/
1284 /*
1285 * l2ca_datawrite():
1286 *
1287 * Transfers data across the channel.
1288 */
1289 /*-----------------------------------------------------------------------------------*/
1290 err_t l2ca_datawrite(struct l2cap_pcb *pcb, struct pbuf *p)
1291 {
1292 err_t ret;
1293 struct l2cap_hdr *l2caphdr;
1294 struct pbuf *q;
1295
1296 if(pcb->state != L2CAP_OPEN) {
1297 ERROR("l2cap_datawrite: State != L2CAP_OPEN. Dropping data\n");
1298 return ERR_CONN;
1299 }
1300
1301 /* Build L2CAP header */
1302 if((q = btpbuf_alloc(PBUF_RAW, L2CAP_HDR_LEN, PBUF_RAM)) == NULL) {
1303 ERROR("l2cap_datawrite: Could not allocate memory for pbuf\n");
1304 return ERR_MEM;
1305 }
1306 btpbuf_chain(q, p);
1307
1308 l2caphdr = q->payload;
1309 l2caphdr->cid = htole16(pcb->dcid);
1310
1311 /* If length of the data exceeds the OutMTU then only the first OutMTU bytes are sent */
1312 if(p->tot_len > pcb->cfg.outmtu) {
1313 /* Send peer L2CAP data */
1314 l2caphdr->len = htole16(pcb->cfg.outmtu);
1315 if((ret = l2cap_write(&(pcb->remote_bdaddr), q, pcb->cfg.outmtu + L2CAP_HDR_LEN)) == ERR_OK) {
1316 //LOG("l2cap_datawrite: Length of data exceeds the OutMTU p->tot_len = %d\n", p->tot_len);
1317 ret = ERR_BUF; /* Length of data exceeds the OutMTU */
1318 }
1319 } else {
1320 /* Send peer L2CAP data */
1321 l2caphdr->len = htole16(p->tot_len);
1322 //LOG("l2cap_datawrite: q->tot_len = %d\n", q->tot_len);
1323 ret = l2cap_write(&(pcb->remote_bdaddr), q, q->tot_len);
1324 }
1325
1326 /* Free L2CAP header. Higher layers will handle rest of packet */
1327 p = btpbuf_dechain(q);
1328 btpbuf_free(q);
1329
1330 return ret;
1331 }
1332 /*-----------------------------------------------------------------------------------*/
1333 /*
1334 * l2ca_ping():
1335 *
1336 * Sends an empty L2CAP echo request message. Also specify the function that should
1337 * be called when a L2CAP echo reply has been received.
1338 */
1339 /*-----------------------------------------------------------------------------------*/
1340 err_t l2ca_ping(struct bd_addr *bdaddr, struct l2cap_pcb *tpcb,
1341 err_t (* l2ca_pong)(void *arg, struct l2cap_pcb *pcb, u8_t result))
1342 {
1343 err_t ret;
1344
1345 if(!lp_is_connected(bdaddr)) {
1346 return ERR_CONN;
1347 }
1348
1349 bd_addr_set(&(tpcb->remote_bdaddr), bdaddr);
1350 tpcb->l2ca_pong = l2ca_pong;
1351
1352 L2CAP_REG(&(l2cap_active_pcbs), tpcb);
1353
1354 ret = l2cap_signal(tpcb, L2CAP_ECHO_REQ, 0, &(tpcb->remote_bdaddr), NULL); /* Send l2cap_echo_req signal */
1355
1356 return ret;
1357 }
1358 /*-----------------------------------------------------------------------------------*/
1359 /* Lower-Layer to L2CAP signaling events
1360 */
1361 /*-----------------------------------------------------------------------------------*/
1362 /*-----------------------------------------------------------------------------------*/
1363 /*
1364 * lp_connect_cfm():
1365 *
1366 * Confirms the request to establish a lower layer (Baseband) connection.
1367 */
1368 /*-----------------------------------------------------------------------------------*/
1369 void lp_connect_cfm(struct bd_addr *bdaddr, u8_t encrypt_mode, err_t err)
1370 {
1371 struct l2cap_pcb *pcb;
1372 struct pbuf *data;
1373 err_t ret;
1374
1375 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
1376 if(bd_addr_cmp(&(pcb->remote_bdaddr), bdaddr)) {
1377 break;
1378 }
1379 }
1380 if(pcb == NULL) {
1381 /* Silently discard */
1382 LOG("lp_connect_cfm: Silently discard\n");
1383 } else {
1384 if(err == ERR_OK) {
1385 pcb->encrypt = encrypt_mode;
1386 /* Send l2cap_conn_req signal if no error */
1387 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CONN_REQ_SIZE, PBUF_RAM)) != NULL) {
1388 ((u16_t *)data->payload)[0] = htole16(pcb->psm);
1389 ((u16_t *)data->payload)[1] = htole16(pcb->scid);
1390 if((ret = l2cap_signal(pcb, L2CAP_CONN_REQ, 0, &(pcb->remote_bdaddr), data)) == ERR_OK) {
1391 pcb->state = W4_L2CAP_CONNECT_RSP;
1392 } else {
1393 L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available? */
1394 }
1395 //LOG("lp_connect_cfm: l2cap_conn_req signal sent. err = %d\nPSM = 0x%x\nscid = 0x%x\nencrypt mode = 0x%x\n", err, pcb->psm, pcb->scid, pcb->encrypt);
1396 } else {
1397 ERROR("lp_connect_cfm: No resources available\n");
1398 L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available */
1399 }
1400 } else {
1401 ERROR("lp_connect_cfm: Connection falied\n");
1402 L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available */
1403 }
1404 }
1405 }
1406 /*-----------------------------------------------------------------------------------*/
1407 /*
1408 * lp_connect_ind():
1409 *
1410 * Indicates the lower protocol has successfully established a connection.
1411 */
1412 /*-----------------------------------------------------------------------------------*/
1413 void lp_connect_ind(struct bd_addr *bdaddr)
1414 {
1415 LOG("lp_connect_ind\n");
1416 }
1417 /*-----------------------------------------------------------------------------------*/
1418 /*
1419 * lp_disconnect_ind():
1420 *
1421 * Indicates the lower protocol (Baseband) has been shut down by LMP commands or a
1422 * timeout event..
1423 */
1424 /*-----------------------------------------------------------------------------------*/
1425 void lp_disconnect_ind(struct bd_addr *bdaddr,u8_t reason)
1426 {
1427 struct l2cap_pcb *pcb, *tpcb;
1428 err_t ret;
1429
1430 (void)ret;
1431
1432 for(pcb = l2cap_active_pcbs; pcb != NULL;) {
1433 tpcb = pcb->next;
1434 LOG("lp_disconnect_ind: Find a pcb with a matching Bluetooth address\n");
1435 /* All PCBs with matching Bluetooth address have been disconnected */
1436 if(bd_addr_cmp(&(pcb->remote_bdaddr), bdaddr)) {// && pcb->state != L2CAP_CLOSED) {
1437 pcb->state = L2CAP_CLOSED;
1438 LOG("lp_disconnect_ind: Notify application\n");
1439 L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
1440 }
1441 pcb = tpcb;
1442 }
1443 if(l2cap_disconnect_bb_cb) l2cap_disconnect_bb_cb(bdaddr,reason);
1444 }
1445
1446 /*-----------------------------------------------------------------------------------*/
1447 /*
1448 * l2cap_disconnect_bb():
1449 *
1450 * Register a callback to obtain the disconnection reason from the baseband
1451 */
1452 /*-----------------------------------------------------------------------------------*/
1453 void (*l2cap_disconnect_bb(void (*l2ca_disconnect_bb)(struct bd_addr *bdaddr,u8_t reason)))(struct bd_addr *bdaddr,u8_t reason)
1454 {
1455 void (*oldcb)(struct bd_addr *bdaddr,u8_t reason) = NULL;
1456 oldcb = l2cap_disconnect_bb_cb;
1457 l2cap_disconnect_bb_cb = l2ca_disconnect_bb;
1458 return oldcb;
1459 }
1460
1461 /*-----------------------------------------------------------------------------------*/
1462 /*
1463 * l2cap_next_sigid():
1464 *
1465 * Issues a signal identifier that helps matching a request with the reply.
1466 */
1467 /*-----------------------------------------------------------------------------------*/
1468 u8_t l2cap_next_sigid(void)
1469 {
1470 ++sigid_nxt;
1471 if(sigid_nxt == 0) {
1472 sigid_nxt = 1;
1473 }
1474 return sigid_nxt;
1475 }
1476 /*-----------------------------------------------------------------------------------*/
1477 /*
1478 * l2cap_arg():
1479 *
1480 * Used to specify the argument that should be passed callback functions.
1481 */
1482 /*-----------------------------------------------------------------------------------*/
1483 void l2cap_arg(struct l2cap_pcb *pcb, void *arg)
1484 {
1485 pcb->callback_arg = arg;
1486 }
1487
1488 /*-----------------------------------------------------------------------------------*/
1489 /*
1490 * l2cap_connect_ind():
1491 *
1492 * Set the state of the connection to be LISTEN, which means that it is able to accept
1493 * incoming connections. The protocol control block is reallocated in order to consume
1494 * less memory. Setting the connection to LISTEN is an irreversible process. Also
1495 * specify the function that should be called when the channel has received a
1496 * connection request.
1497 */
1498 /*-----------------------------------------------------------------------------------*/
1499 err_t l2cap_connect_ind(struct l2cap_pcb *npcb, struct bd_addr *bdaddr, u16_t psm,err_t (* l2ca_connect_ind)(void *arg, struct l2cap_pcb *pcb, err_t err))
1500 {
1501 struct l2cap_pcb_listen *lpcb;
1502
1503 lpcb = btmemb_alloc(&l2cap_listenpcbs);
1504 if(lpcb == NULL) {
1505 ERROR("l2cap_connect_ind: Could not allocate memory for lpcb\n");
1506 return ERR_MEM;
1507 }
1508
1509 bd_addr_set(&(lpcb->bdaddr),bdaddr);
1510 lpcb->psm = psm;
1511 lpcb->l2ca_connect_ind = l2ca_connect_ind;
1512 lpcb->state = L2CAP_LISTEN;
1513 lpcb->callback_arg = npcb->callback_arg;
1514 btmemb_free(&l2cap_pcbs, npcb);
1515 L2CAP_REG(&(l2cap_listen_pcbs), lpcb);
1516 return ERR_OK;
1517 }
1518
1519 /*-----------------------------------------------------------------------------------*/
1520 /*
1521 * l2cap_disconnect_ind():
1522 *
1523 * Used to specify the a function to be called when a disconnection request has been
1524 * received from a remote device or the remote device has been disconnected because it
1525 * has failed to respond to a signalling request.
1526 */
1527 /*-----------------------------------------------------------------------------------*/
1528 void l2cap_disconnect_ind(struct l2cap_pcb *pcb, err_t (* l2ca_disconnect_ind)(void *arg, struct l2cap_pcb *newpcb, err_t err))
1529 {
1530 pcb->l2ca_disconnect_ind = l2ca_disconnect_ind;
1531 }
1532 /*-----------------------------------------------------------------------------------*/
1533 /*
1534 * l2cap_timeout_ind():
1535 *
1536 * Used to specify the function to be called when RTX or ERTX timer has expired.
1537 */
1538 /*-----------------------------------------------------------------------------------*/
1539 void l2cap_timeout_ind(struct l2cap_pcb *pcb,err_t (* l2ca_timeout_ind)(void *arg, struct l2cap_pcb *newpcb, err_t err))
1540 {
1541 pcb->l2ca_timeout_ind = l2ca_timeout_ind;
1542 }
1543 /*-----------------------------------------------------------------------------------*/
1544 /*
1545 * l2cap_recv():
1546 *
1547 * Used to specify the function that should be called when a L2CAP connection receives
1548 * data.
1549 */
1550 /*-----------------------------------------------------------------------------------*/
1551 void l2cap_recv(struct l2cap_pcb *pcb, err_t (* l2ca_recv)(void *arg, struct l2cap_pcb *pcb, struct pbuf *p, err_t err))
1552 {
1553 pcb->l2ca_recv = l2ca_recv;
1554 }
1555 /*-----------------------------------------------------------------------------------*/
inofficial libogc fork