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