?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
patch to make enabling 64B L2 cache optional (tueidj)
[libogc.git] / lwbt / l2cap.c
blob573037976e494f34ca4e2571e566847520fcbeb2
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");
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 == %04x\n", ((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 LOG("l2cap_process_sig: discarded response without matching request\n");
557 break;
558 }
559
560 /* Remove signal from unresponded list and deallocate it */
561 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
562 btpbuf_free(sig->p);
563 btmemb_free(&l2cap_sigs, sig);
564
565 siglen = le16toh(sighdr->len);
566 scid = le16toh(((u16_t *)p->payload)[0]);
567 flags = le16toh(((u16_t *)p->payload)[1]);
568 result = le16toh(((u16_t *)p->payload)[2]);
569 siglen -= 6;
570 btpbuf_header(p, -6);
571
572 LOG("l2cap_process_sig: Outgoing configuration result == %d continuation flag == %d\n", result, flags);
573
574 /* Handle config request */
575 switch(result) {
576 case L2CAP_CFG_SUCCESS:
577 LOG("l2cap_process_sig: Successfull outgoing configuration\n");
578 pcb->cfg.l2capcfg |= L2CAP_CFG_IN_SUCCESS; /* Local side of the connection
579 has been configured for outgoing data */
580 pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset configuration timeout */
581
582 if(pcb->cfg.outflushto != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
583 lp_write_flush_timeout(&pcb->remote_bdaddr, pcb->cfg.outflushto);
584 }
585
586 /* L2CAP connection established if a successful configuration response has been sent */
587 if(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_SUCCESS) {
588 pcb->state = L2CAP_OPEN;
589 if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
590 L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
591 } else {
592 L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
593 }
594 }
595 break;
596 case L2CAP_CFG_UNACCEPT:
597 /* Parse and add options to pcb */
598 while(siglen > 0) {
599 opthdr = p->payload;
600 /* Check if type of action bit indicates a non-hint. Hints are ignored */
601 if(L2CAP_OPTH_TOA(opthdr) == 0) {
602 switch(L2CAP_OPTH_TYPE(opthdr)) {
603 case L2CAP_CFG_MTU:
604 if(L2CAP_MTU > le16toh(((u16_t *)p->payload)[1])) {
605 pcb->cfg.outmtu = le16toh(((u16_t *)p->payload)[1]);
606 } else {
607 ERROR("l2cap_process_sig: Configuration of MTU failed\n");
608 l2ca_disconnect_req(pcb, NULL);
609 return;
610 }
611 break;
612 case L2CAP_FLUSHTO:
613 pcb->cfg.influshto = le16toh(((u16_t *)p->payload)[1]);
614 break;
615 case L2CAP_QOS:
616 /* If service type Best Effort is not accepted we will close the connection */
617 if(((u8_t *)p->payload)[3] != L2CAP_QOS_BEST_EFFORT) {
618 ERROR("l2cap_process_sig: Unsupported service type\n");
619 l2ca_disconnect_req(pcb, NULL);
620 return;
621 }
622 break;
623 default:
624 /* Should not happen, skip option */
625 break;
626 } /* switch */
627 } /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
628 btpbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
629 siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
630 } /* while */
631
632 /* Send out a new configuration request if the continuation flag isn't set */
633 if((flags & 0x0001) == 0) {
634 l2ca_config_req(pcb);
635 }
636 break;
637 case L2CAP_CFG_REJ:
638 /* Fallthrough */
639 case L2CAP_CFG_UNKNOWN:
640 /* Fallthrough */
641 default:
642 if((flags & 0x0001) == 0) {
643 LOG("l2cap_process_sig: Configuration failed\n");
644 l2ca_disconnect_req(pcb, NULL);
645 return;
646 }
647 break;
648 } /* switch(result) */
649
650 /* If continuation flag is set we must send a NULL configuration request */
651 if((flags & 0x0001) == 1) {
652 LOG("l2cap_process_sig: Continuation flag is set. Send empty (default) config request signal\n");
653 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
654 ERROR("l2cap_process_sig: Could not allocate memory for pbuf\n");
655 return;
656 }
657 /* Assemble config request packet */
658 ((u16_t *)data->payload)[0] = htole16(pcb->scid);
659 ((u16_t *)data->payload)[2] = 0;
660 l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), data);
661 }
662 break;
663 case L2CAP_DISCONN_REQ:
664 siglen = le16toh(sighdr->len);
665 dcid = le16toh(((u16_t *)p->payload)[0]);
666 siglen = siglen - 2;
667 flags = le16toh(((u16_t *)p->payload)[1]);
668 siglen = siglen - 2;
669 btpbuf_header(p, -4);
670
671 /* Find PCB with matching cid */
672 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
673 if(pcb->scid == dcid) {
674 /* Matching cid found */
675 break;
676 }
677 }
678 /* If no matching cid was found, send a cmd reject (Invalid cid) */
679 if(pcb == NULL) {
680 /* Alloc size of reason in cmd rej + data (dcid + scid) */
681 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
682 ((u16_t *)data->payload)[0] = htole16(L2CAP_INVALID_CID);
683 ((u16_t *)data->payload)[1] = htole16(dcid); /* Requested local cid */
684 ((u16_t *)data->payload)[2] = htole16(L2CAP_NULL_CID); /* Remote cid not known */
685
686 ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
687 }
688 } else { /* Handle disconnection request */
689 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_DISCONN_RSP_SIZE, PBUF_RAM)) != NULL) {
690 ((u16_t *)data->payload)[0] = htole16(pcb->scid);
691 ((u16_t *)data->payload)[1] = htole16(pcb->dcid);
692 ret = l2cap_signal(pcb, L2CAP_DISCONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
693
694 /* Give upper layer indication */
695 pcb->state = L2CAP_CLOSED;
696 LOG("l2cap_process_sig: Disconnection request\n");
697 L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
698 }
699 }
700 break;
701 case L2CAP_DISCONN_RSP:
702 if(pcb == NULL) {
703 /* A response without a matching request is silently discarded */
704 break;
705 }
706 /* Remove signal from unresponded list and deallocate it */
707 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
708 btpbuf_free(sig->p);
709 btmemb_free(&l2cap_sigs, sig);
710
711 L2CA_ACTION_DISCONN_CFM(pcb,ret); /* NOTE: Application should
712 now close the connection */
713 break;
714 case L2CAP_ECHO_REQ:
715 pcb->ursp_id = sighdr->id;
716 ret = l2cap_signal(pcb, L2CAP_ECHO_RSP, sighdr->id, &(pcb->remote_bdaddr), NULL);
717 break;
718 case L2CAP_ECHO_RSP:
719 if(pcb == NULL) {
720 /* A response without a matching request is silently discarded */
721 break;
722 }
723 /* Remove signal from unresponded list and deallocate it */
724 L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
725 btpbuf_free(sig->p);
726 btmemb_free(&l2cap_sigs, sig);
727
728 /* Remove temporary pcb from active list */
729 L2CAP_RMV(&l2cap_active_pcbs, pcb);
730 L2CA_ACTION_PING_CFM(pcb,L2CAP_ECHO_RCVD,ret);
731 break;
732 default:
733 /* Alloc size of reason in cmd rej */
734 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE, PBUF_RAM)) != NULL) {
735 ((u16_t *)data->payload)[0] = htole16(L2CAP_CMD_NOT_UNDERSTOOD);
736
737 ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
738 }
739 break;
740 } /* switch */
741 len = len - (le16toh(sighdr->len) + L2CAP_SIGHDR_LEN);
742 btpbuf_header(p, -(le16toh(sighdr->len)));
743 } /* while */
744 }
745
746 /*-----------------------------------------------------------------------------------*/
747 /*
748 * l2cap_input():
749 *
750 * Called by the lower layer. Reassembles the packet, parses the header and forward
751 * it to the upper layer or the signal handler.
752 */
753 /*-----------------------------------------------------------------------------------*/
754 void l2cap_input(struct pbuf *p, struct bd_addr *bdaddr)
755 {
756 struct l2cap_seg *inseg;
757 struct hci_acl_hdr *aclhdr;
758 struct pbuf *data;
759 err_t ret;
760
761 btpbuf_header(p, HCI_ACL_HDR_LEN);
762 aclhdr = p->payload;
763 btpbuf_header(p, -HCI_ACL_HDR_LEN);
764
765 btpbuf_realloc(p, aclhdr->len);
766
767 for(inseg = l2cap_insegs; inseg != NULL; inseg = inseg->next) {
768 if(bd_addr_cmp(bdaddr, &(inseg->bdaddr))) {
769 break;
770 }
771 }
772
773 aclhdr->connhdl_pb_bc = le16toh(aclhdr->connhdl_pb_bc);
774 aclhdr->len = le16toh(aclhdr->len);
775 /* Reassembly procedures */
776 /* Check if continuing fragment or start of L2CAP packet */
777 if(((aclhdr->connhdl_pb_bc >> 12) & 0x03)== L2CAP_ACL_CONT) { /* Continuing fragment */
778 if(inseg == NULL) {
779 /* Discard packet */
780 LOG("l2cap_input: Continuing fragment. Discard packet\n");
781 btpbuf_free(p);
782 return;
783 } else if(inseg->p->tot_len + p->tot_len > inseg->len) { /* Check if length of
784 segment exceeds
785 l2cap header length */
786 /* Discard packet */
787 LOG("l2cap_input: Continuing fragment. Length exceeds L2CAP hdr length. Discard packet\n");
788 btpbuf_free(inseg->p);
789 L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
790 btmemb_free(&l2cap_segs, inseg);
791
792 btpbuf_free(p);
793 return;
794 }
795 /* Add pbuf to segement */
796 btpbuf_chain(inseg->p, p);
797 btpbuf_free(p);
798
799 } else if(((aclhdr->connhdl_pb_bc >> 12) & 0x03) == L2CAP_ACL_START) { /* Start of L2CAP packet */
800 //LOG("l2cap_input: Start of L2CAP packet p->len = %d, p->tot_len = %d\n", p->len, p->tot_len);
801 if(inseg != NULL) { /* Check if there are segments missing in a previous packet */
802 /* Discard previous packet */
803 LOG("l2cap_input: Start of L2CAP packet. Discard previous packet\n");
804 btpbuf_free(inseg->p);
805 } else {
806 inseg = btmemb_alloc(&l2cap_segs);
807 bd_addr_set(&(inseg->bdaddr), bdaddr);
808 L2CAP_SEG_REG(&(l2cap_insegs), inseg);
809 }
810 inseg->p = p;
811 inseg->l2caphdr = p->payload;
812 inseg->l2caphdr->cid = le16toh(inseg->l2caphdr->cid);
813 inseg->l2caphdr->len = le16toh(inseg->l2caphdr->len);
814
815 inseg->len = inseg->l2caphdr->len + L2CAP_HDR_LEN;
816 for(inseg->pcb = l2cap_active_pcbs; inseg->pcb != NULL; inseg->pcb = inseg->pcb->next) {
817 if(inseg->pcb->scid == inseg->l2caphdr->cid) {
818 break; /* found */
819 }
820 }
821 } else {
822 /* Discard packet */
823 LOG("l2cap_input: Discard packet\n");
824 btpbuf_free(inseg->p);
825 L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
826 btmemb_free(&l2cap_segs, inseg);
827
828 btpbuf_free(p);
829 return;
830 }
831 if(inseg->p->tot_len < inseg->len) {
832 LOG("l2cap_input: Get continuing segments\n");
833 return; /* Get continuing segments */
834 }
835
836 /* Handle packet */
837 switch(inseg->l2caphdr->cid) {
838 case L2CAP_NULL_CID:
839 /* Illegal */
840 LOG("l2cap_input: Illegal null cid\n");
841 btpbuf_free(inseg->p);
842 break;
843 case L2CAP_SIG_CID:
844 btpbuf_header(inseg->p, -L2CAP_HDR_LEN);
845 l2cap_process_sig(inseg->p, inseg->l2caphdr, bdaddr);
846 btpbuf_free(inseg->p);
847 break;
848 case L2CAP_CONNLESS_CID:
849 /* Not needed by PAN, LAN access or DUN profiles */
850 btpbuf_free(inseg->p);
851 break;
852 default:
853 if(inseg->l2caphdr->cid < 0x0040 || inseg->pcb == NULL) {
854 /* Reserved for specific L2CAP functions or channel does not exist */
855 /* Alloc size of reason in cmd rej */
856 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
857 ((u16_t *)data->payload)[0] = htole16(L2CAP_INVALID_CID);
858 ((u16_t *)data->payload)[1] = htole16(inseg->l2caphdr->cid);
859 ((u16_t *)data->payload)[2] = htole16(L2CAP_NULL_CID);
860
861 ret = l2cap_signal(NULL, L2CAP_CMD_REJ, l2cap_next_sigid(), bdaddr, data);
862 }
863 btpbuf_free(inseg->p);
864 break;
865 }
866
867 btpbuf_header(inseg->p, -L2CAP_HDR_LEN);
868
869 /* Forward packet to higher layer */
870 LOG("l2cap_input: Forward packet to higher layer\n");
871 /*
872 LOG("l2cap_input: Remote BD address: 0x%x:0x%x:0x%x:0x%x:0x%x:0x%x\n",
873 inseg->pcb->remote_bdaddr.addr[5],
874 inseg->pcb->remote_bdaddr.addr[4],
875 inseg->pcb->remote_bdaddr.addr[3],
876 inseg->pcb->remote_bdaddr.addr[2],
877 inseg->pcb->remote_bdaddr.addr[1],
878 inseg->pcb->remote_bdaddr.addr[0]));
879 */
880 L2CA_ACTION_RECV(inseg->pcb,inseg->p,ERR_OK,ret);
881 break;
882 }
883
884 /* Remove input segment */
885 L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
886 btmemb_free(&l2cap_segs, inseg);
887 }
888
889 /*-----------------------------------------------------------------------------------*/
890 /*
891 * l2cap_cid_alloc():
892 *
893 * Allocates a channel identifier (CID). They are local names representing a logical
894 * channel endpoint on the device.
895 */
896 /*-----------------------------------------------------------------------------------*/
897 static u16_t l2cap_cid_alloc(void)
898 {
899 u16_t cid;
900 struct l2cap_pcb *pcb;
901
902 for (cid = L2CAP_MIN_CID; cid < L2CAP_MAX_CID; ++cid) {
903 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
904 if(pcb->scid == cid) {
905 break;
906 }
907 }
908 if(pcb == NULL) {
909 return cid;
910 }
911 }
912 return 0;
913 }
914
915 /*-----------------------------------------------------------------------------------*/
916 /*
917 * l2cap_new():
918 *
919 * Creates a new L2CAP protocol control block but doesn't place it on
920 * any of the L2CAP PCB lists.
921 */
922 /*-----------------------------------------------------------------------------------*/
923 struct l2cap_pcb* l2cap_new(void)
924 {
925 struct l2cap_pcb *pcb;
926
927 pcb = btmemb_alloc(&l2cap_pcbs);
928 if(pcb != NULL) {
929 memset(pcb, 0, sizeof(struct l2cap_pcb));
930 pcb->state = L2CAP_CLOSED;
931
932 /* Initialize configuration parameter options with default values */
933
934 /* Maximum Transmission Unit */
935 pcb->cfg.inmtu = L2CAP_MTU; /* The MTU that this implementation support */
936 pcb->cfg.outmtu = 672; /* Default MTU. Two Baseband DH5 packets minus the Baseband ACL headers and
937 L2CAP header. This can be set here since we will never send any signals
938 larger than the L2CAP sig MTU (48 bytes) before L2CAP has been configured
939 */
940
941 /* Flush Timeout */
942 pcb->cfg.influshto = 0xFFFF;
943 pcb->cfg.outflushto = 0xFFFF;
944
945 pcb->cfg.cfgto = L2CAP_CFG_TO; /* Maximum time before terminating a negotiation.
946 Cfg shall not last more than 120s */
947 pcb->cfg.opt = NULL;
948 return pcb;
949 }
950 ERROR("l2cap_new: Could not allocate memory for pcb\n");
951 return NULL;
952 }
953
954 /*-----------------------------------------------------------------------------------*/
955 /*
956 * l2cap_close():
957 *
958 * Closes the L2CAP protocol control block.
959 */
960 /*-----------------------------------------------------------------------------------*/
961 err_t l2cap_close(struct l2cap_pcb *pcb)
962 {
963 struct l2cap_sig *tmpsig;
964
965 if(pcb->state == L2CAP_LISTEN) {
966 L2CAP_RMV((struct l2cap_pcb**)((void*)&(l2cap_listen_pcbs)), pcb);
967 btmemb_free(&l2cap_listenpcbs, pcb);
968 } else {
969 L2CAP_RMV(&(l2cap_active_pcbs), pcb);
970 /* Free any unresponded signals */
971 while(pcb->unrsp_sigs != NULL) {
972 tmpsig = pcb->unrsp_sigs;
973 pcb->unrsp_sigs = pcb->unrsp_sigs->next;
974 btmemb_free(&l2cap_sigs, tmpsig);
975 }
976
977 btmemb_free(&l2cap_pcbs, pcb);
978 }
979 pcb = NULL;
980 return ERR_OK;
981 }
982 /*-----------------------------------------------------------------------------------*/
983 /*
984 * l2cap_reset_all():
985 *
986 * Closes all active and listening L2CAP protocol control blocks.
987 */
988 /*-----------------------------------------------------------------------------------*/
989 void l2cap_reset_all(void)
990 {
991 struct l2cap_pcb *pcb, *tpcb;
992 struct l2cap_pcb_listen *lpcb, *tlpcb;
993 struct l2cap_seg *seg, *tseg;
994
995 for(pcb = l2cap_active_pcbs; pcb != NULL;) {
996 tpcb = pcb->next;
997 l2cap_close(pcb);
998 pcb = tpcb;
999 }
1000
1001 for(lpcb = l2cap_listen_pcbs; lpcb != NULL;) {
1002 tlpcb = lpcb->next;
1003 l2cap_close((struct l2cap_pcb *)lpcb);
1004 lpcb = tlpcb;
1005 }
1006
1007 for(seg = l2cap_insegs; seg != NULL;) {
1008 tseg = seg->next;
1009 L2CAP_SEG_RMV(&(l2cap_insegs), seg);
1010 btmemb_free(&l2cap_segs, seg);
1011 seg = tseg;
1012 }
1013
1014 l2cap_init();
1015 }
1016
1017 /*-----------------------------------------------------------------------------------*/
1018 /* L2CAP to L2CAP signalling events
1019 */
1020 /*-----------------------------------------------------------------------------------*/
1021 /*-----------------------------------------------------------------------------------*/
1022 /*
1023 * l2cap_signal():
1024 *
1025 * Assembles the signalling packet and passes it to the lower layer.
1026 */
1027 /*-----------------------------------------------------------------------------------*/
1028 err_t l2cap_signal(struct l2cap_pcb *pcb, u8_t code, u16_t ursp_id, struct bd_addr *remote_bdaddr, struct pbuf *data)
1029 {
1030 struct l2cap_sig *sig;
1031 struct l2cap_sig_hdr *sighdr;
1032 struct l2cap_hdr *hdr;
1033 err_t ret;
1034
1035 /* Alloc a new signal */
1036 LOG("l2cap_signal: Allocate memory for l2cap_sig. Code = 0x%x\n", code);
1037 if((sig = btmemb_alloc(&l2cap_sigs)) == NULL) {
1038 ERROR("l2cap_signal: could not allocate memory for l2cap_sig\n");
1039 return ERR_MEM;
1040 }
1041
1042 /* Alloc a pbuf for signal */
1043 if((sig->p = btpbuf_alloc(PBUF_RAW, L2CAP_HDR_LEN+L2CAP_SIGHDR_LEN, PBUF_RAM)) == NULL) {
1044 ERROR("l2cap_signal: could not allocate memory for pbuf\n");
1045 return ERR_MEM;
1046 }
1047
1048 /* Setup signal header and leave room for l2cap hdr */
1049 sighdr = (struct l2cap_sig_hdr *)(((u8_t *)sig->p->payload)+L2CAP_HDR_LEN);
1050
1051 /* Chain data to signal and set length of signal data */
1052 if(data == NULL) {
1053 sighdr->len = 0;
1054 } else {
1055 btpbuf_chain(sig->p, data);
1056 btpbuf_free(data);
1057 sighdr->len = htole16(data->tot_len);
1058 }
1059
1060 sighdr->code = code;
1061
1062 if(sighdr->code % 2) { /* If odd this is a resp/rej signal */
1063 sig->sigid = ursp_id; /* Get id */
1064 LOG("l2cap_signal: Sending response/reject signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1065 } else {
1066 sig->sigid = l2cap_next_sigid(); /* Alloc id */
1067 sig->rtx = L2CAP_RTX; /* Set Response Timeout Expired timer (in seconds)
1068 should be at least as large as the BB flush timeout */
1069 sig->nrtx = L2CAP_MAXRTX; /* Set max number of retransmissions */
1070 LOG("l2cap_signal: Sending request signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1071 }
1072 sighdr->id = sig->sigid; /* Set id */
1073
1074 /* Set up L2CAP hdr */
1075 hdr = sig->p->payload;
1076 hdr->len = htole16((sig->p->tot_len - L2CAP_HDR_LEN));
1077 hdr->cid = htole16(L2CAP_SIG_CID); /* 0x0001 */
1078
1079 ret = l2cap_write(remote_bdaddr, sig->p, sig->p->tot_len); /* Send peer L2CAP signal */
1080
1081 /* Put signal on unresponded list if it's a request signal, else deallocate it */
1082 if(ret == ERR_OK && (sighdr->code % 2) == 0) {
1083 LOG("l2cap_signal: Registering sent request signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1084 L2CAP_SIG_REG(&(pcb->unrsp_sigs), sig);
1085 } else {
1086 LOG("l2cap_signal: Deallocating sent response/reject signal with id = %d code = %d\n", sig->sigid, sighdr->code);
1087 btpbuf_free(sig->p);
1088 sig->p = NULL;
1089 btmemb_free(&l2cap_sigs, sig);
1090 }
1091
1092 return ret;
1093 }
1094
1095 /*-----------------------------------------------------------------------------------*/
1096 /*
1097 * l2cap_rexmit_signal():
1098 *
1099 * Called by the l2cap timer. Retransmitts a signal.
1100 */
1101 /*-----------------------------------------------------------------------------------*/
1102 err_t l2cap_rexmit_signal(struct l2cap_pcb *pcb, struct l2cap_sig *sig)
1103 {
1104 err_t ret;
1105
1106 /* Set up L2CAP hdr */
1107 ret = l2cap_write(&(pcb->remote_bdaddr), sig->p, sig->p->tot_len); /* Send peer L2CAP signal */
1108
1109 return ret;
1110 }
1111 /*-----------------------------------------------------------------------------------*/
1112 /* Upper-Layer to L2CAP signaling events
1113 */
1114 /*-----------------------------------------------------------------------------------*/
1115 /*-----------------------------------------------------------------------------------*/
1116 /*
1117 * l2ca_connect_req():
1118 *
1119 * Initiates the sending of a connect request message. Requests the creation of a
1120 * channel representing a logicalconnection to a physical address. Input parameters
1121 * are the target protocol(PSM) and remote devices 48-bit address (BD_ADDR). Also
1122 * specify the function to be called when a confirm has been received.
1123 */
1124 /*-----------------------------------------------------------------------------------*/
1125 err_t l2ca_connect_req(struct l2cap_pcb *pcb, struct bd_addr *bdaddr, u16_t psm,
1126 u8_t role_switch, err_t (* l2ca_connect_cfm)(void *arg, struct l2cap_pcb *lpcb,
1127 u16_t result, u16_t status))
1128 {
1129 err_t ret;
1130 struct pbuf *data;
1131
1132 if(bdaddr != NULL) {
1133 bd_addr_set(&(pcb->remote_bdaddr),bdaddr);
1134 } else {
1135 return ERR_VAL;
1136 }
1137
1138 pcb->psm = psm;
1139 pcb->l2ca_connect_cfm = l2ca_connect_cfm;
1140 pcb->scid = l2cap_cid_alloc();
1141
1142 pcb->cfg.l2capcfg |= L2CAP_CFG_IR; /* We are the initiator of this connection */
1143
1144 if(!lp_is_connected(bdaddr)) {
1145 ret = lp_connect_req(bdaddr, role_switch); /* Create ACL link w pcb state == CLOSED */
1146 } else {
1147 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CONN_REQ_SIZE, PBUF_RAM)) == NULL) {
1148 ERROR("l2cap_connect_req: Could not allocate memory for pbuf\n");
1149 return ERR_MEM;
1150 }
1151 ((u16_t *)data->payload)[0] = htole16(psm);
1152 ((u16_t *)data->payload)[1] = htole16(pcb->scid);
1153 ret = l2cap_signal(pcb, L2CAP_CONN_REQ, 0, &(pcb->remote_bdaddr), data); /* Send l2cap_conn_req signal */
1154
1155 pcb->state = W4_L2CAP_CONNECT_RSP;
1156 }
1157
1158 L2CAP_REG(&(l2cap_active_pcbs), pcb);
1159
1160 return ret;
1161 }
1162
1163 /*-----------------------------------------------------------------------------------*/
1164 /*
1165 * l2ca_config_req():
1166 *
1167 * Requests the initial configuration (or reconfiguration) of a channel to a new set
1168 * of channel parameters. Input parameters are the local CID endpoint, new incoming
1169 * receivable MTU (InMTU), new outgoing flow specification, and flush and link
1170 * timeouts. Also specify the function to be called when a confirm has been received.
1171 */
1172 /*-----------------------------------------------------------------------------------*/
1173 err_t l2ca_config_req(struct l2cap_pcb *pcb)
1174 {
1175 struct pbuf *p, *q;
1176 struct l2cap_cfgopt_hdr *opthdr;
1177 err_t ret;
1178
1179 switch(pcb->state) {
1180 case L2CAP_OPEN:
1181 LOG("l2cap_config_req: state = L2CAP_OPEN. Suspend transmission\n");
1182 /* Note: Application should have suspended data transmission, otherwise outgoing data will be
1183 dropped */
1184 pcb->state = L2CAP_CONFIG;
1185 /* Fallthrough */
1186 case L2CAP_CONFIG:
1187 LOG("l2cap_config_req: state = L2CAP_CONFIG\n");
1188
1189 if((p = btpbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
1190 ERROR("l2cap_config_req: Could not allocate memory for pbuf\n");
1191 return ERR_MEM;
1192 }
1193
1194 /* Assemble config request packet. Only options that has to be changed will be
1195 sent */
1196 ((u16_t *)p->payload)[0] = htole16(pcb->dcid);
1197 /* In this implementation we do not send multiple cmds in one
1198 signal packet. Therefore we will never send a config_req packet
1199 that will cause the signal to be larger than the minimum L2CAP MTU
1200 48 bytes. Hence, this flag will always be cleared */
1201 ((u16_t *)p->payload)[1] = 0;
1202
1203 /* Add MTU and out flush timeout to cfg packet if not default value. QoS (Best effort) is always
1204 set to default and can be skipped */
1205 if(pcb->cfg.inmtu != L2CAP_CFG_DEFAULT_INMTU) {
1206 if((q = btpbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + L2CAP_MTU_LEN, PBUF_RAM)) == NULL) {
1207 ERROR("l2cap_config_req: Could not allocate memory for pbuf\n");
1208 btpbuf_free(p);
1209 return ERR_MEM;
1210 }
1211 opthdr = q->payload;
1212 opthdr->type = L2CAP_CFG_MTU;
1213 opthdr->len = L2CAP_MTU_LEN;
1214 ((u16_t *)q->payload)[1] = htole16(pcb->cfg.inmtu);
1215 btpbuf_chain(p, q);
1216 btpbuf_free(q);
1217 }
1218
1219 if(L2CAP_OUT_FLUSHTO != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
1220 if((q = btpbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + L2CAP_FLUSHTO_LEN, PBUF_RAM)) == NULL) {
1221 ERROR("l2cap_config_req: Could not allocate memory for pbuf\n");
1222 btpbuf_free(p);
1223 return ERR_MEM;
1224 }
1225 opthdr = q->payload;
1226 opthdr->type = L2CAP_FLUSHTO;
1227 opthdr->len = L2CAP_FLUSHTO_LEN;
1228 pcb->cfg.outflushto = L2CAP_OUT_FLUSHTO;
1229 ((u16_t *)q->payload)[1] = htole16(pcb->cfg.outflushto);
1230 btpbuf_chain(p, q);
1231 btpbuf_free(q);
1232 }
1233
1234 /* Send config request signal */
1235 ret = l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), p);
1236 break;
1237 default:
1238 ERROR("l2cap_config_req: state = L2CAP_?. Invalid state\n");
1239 return ERR_CONN; /* Invalid state. Connection is not in OPEN or CONFIG state */
1240 }
1241 return ret;
1242 }
1243 /*-----------------------------------------------------------------------------------*/
1244 /*
1245 * l2ca_disconnect_req():
1246 *
1247 * Requests the disconnection of the channel. Also specify the function to be called
1248 * when a confirm is received
1249 */
1250 /*-----------------------------------------------------------------------------------*/
1251 err_t l2ca_disconnect_req(struct l2cap_pcb *pcb, err_t (* l2ca_disconnect_cfm)(void *arg, struct l2cap_pcb *pcb))
1252 {
1253 struct pbuf *data;
1254 err_t ret;
1255
1256 if(pcb->state == L2CAP_OPEN || pcb->state == L2CAP_CONFIG) {
1257 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_DISCONN_REQ_SIZE, PBUF_RAM)) == NULL) {
1258 ERROR("l2cap_disconnect_req: Could not allocate memory for pbuf\n");
1259 return ERR_MEM;
1260 }
1261 pcb->l2ca_disconnect_cfm = l2ca_disconnect_cfm;
1262
1263 ((u16_t *)data->payload)[0] = htole16(pcb->dcid);
1264 ((u16_t *)data->payload)[1] = htole16(pcb->scid);
1265
1266 ret = l2cap_signal(pcb, L2CAP_DISCONN_REQ, 0, &(pcb->remote_bdaddr), data);
1267
1268 if(ret == ERR_OK) {
1269 pcb->state = W4_L2CAP_DISCONNECT_RSP;
1270 }
1271 } else {
1272 return ERR_CONN; /* Signal not supported in this state */
1273 }
1274
1275 return ret;
1276 }
1277 /*-----------------------------------------------------------------------------------*/
1278 /*
1279 * l2ca_datawrite():
1280 *
1281 * Transfers data across the channel.
1282 */
1283 /*-----------------------------------------------------------------------------------*/
1284 err_t l2ca_datawrite(struct l2cap_pcb *pcb, struct pbuf *p)
1285 {
1286 err_t ret;
1287 struct l2cap_hdr *l2caphdr;
1288 struct pbuf *q;
1289
1290 if(pcb->state != L2CAP_OPEN) {
1291 ERROR("l2cap_datawrite: State != L2CAP_OPEN. Dropping data\n");
1292 return ERR_CONN;
1293 }
1294
1295 /* Build L2CAP header */
1296 if((q = btpbuf_alloc(PBUF_RAW, L2CAP_HDR_LEN, PBUF_RAM)) == NULL) {
1297 ERROR("l2cap_datawrite: Could not allocate memory for pbuf\n");
1298 return ERR_MEM;
1299 }
1300 btpbuf_chain(q, p);
1301
1302 l2caphdr = q->payload;
1303 l2caphdr->cid = htole16(pcb->dcid);
1304
1305 /* If length of the data exceeds the OutMTU then only the first OutMTU bytes are sent */
1306 if(p->tot_len > pcb->cfg.outmtu) {
1307 /* Send peer L2CAP data */
1308 l2caphdr->len = htole16(pcb->cfg.outmtu);
1309 if((ret = l2cap_write(&(pcb->remote_bdaddr), q, pcb->cfg.outmtu + L2CAP_HDR_LEN)) == ERR_OK) {
1310 //LOG("l2cap_datawrite: Length of data exceeds the OutMTU p->tot_len = %d\n", p->tot_len);
1311 ret = ERR_BUF; /* Length of data exceeds the OutMTU */
1312 }
1313 } else {
1314 /* Send peer L2CAP data */
1315 l2caphdr->len = htole16(p->tot_len);
1316 //LOG("l2cap_datawrite: q->tot_len = %d\n", q->tot_len);
1317 ret = l2cap_write(&(pcb->remote_bdaddr), q, q->tot_len);
1318 }
1319
1320 /* Free L2CAP header. Higher layers will handle rest of packet */
1321 p = btpbuf_dechain(q);
1322 btpbuf_free(q);
1323
1324 return ret;
1325 }
1326 /*-----------------------------------------------------------------------------------*/
1327 /*
1328 * l2ca_ping():
1329 *
1330 * Sends an empty L2CAP echo request message. Also specify the function that should
1331 * be called when a L2CAP echo reply has been received.
1332 */
1333 /*-----------------------------------------------------------------------------------*/
1334 err_t l2ca_ping(struct bd_addr *bdaddr, struct l2cap_pcb *tpcb,
1335 err_t (* l2ca_pong)(void *arg, struct l2cap_pcb *pcb, u8_t result))
1336 {
1337 err_t ret;
1338
1339 if(!lp_is_connected(bdaddr)) {
1340 return ERR_CONN;
1341 }
1342
1343 bd_addr_set(&(tpcb->remote_bdaddr), bdaddr);
1344 tpcb->l2ca_pong = l2ca_pong;
1345
1346 L2CAP_REG(&(l2cap_active_pcbs), tpcb);
1347
1348 ret = l2cap_signal(tpcb, L2CAP_ECHO_REQ, 0, &(tpcb->remote_bdaddr), NULL); /* Send l2cap_echo_req signal */
1349
1350 return ret;
1351 }
1352 /*-----------------------------------------------------------------------------------*/
1353 /* Lower-Layer to L2CAP signaling events
1354 */
1355 /*-----------------------------------------------------------------------------------*/
1356 /*-----------------------------------------------------------------------------------*/
1357 /*
1358 * lp_connect_cfm():
1359 *
1360 * Confirms the request to establish a lower layer (Baseband) connection.
1361 */
1362 /*-----------------------------------------------------------------------------------*/
1363 void lp_connect_cfm(struct bd_addr *bdaddr, u8_t encrypt_mode, err_t err)
1364 {
1365 struct l2cap_pcb *pcb;
1366 struct pbuf *data;
1367 err_t ret;
1368
1369 for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
1370 if(bd_addr_cmp(&(pcb->remote_bdaddr), bdaddr)) {
1371 break;
1372 }
1373 }
1374 if(pcb == NULL) {
1375 /* Silently discard */
1376 LOG("lp_connect_cfm: Silently discard\n");
1377 } else {
1378 if(err == ERR_OK) {
1379 pcb->encrypt = encrypt_mode;
1380 /* Send l2cap_conn_req signal if no error */
1381 if((data = btpbuf_alloc(PBUF_RAW, L2CAP_CONN_REQ_SIZE, PBUF_RAM)) != NULL) {
1382 ((u16_t *)data->payload)[0] = htole16(pcb->psm);
1383 ((u16_t *)data->payload)[1] = htole16(pcb->scid);
1384 if((ret = l2cap_signal(pcb, L2CAP_CONN_REQ, 0, &(pcb->remote_bdaddr), data)) == ERR_OK) {
1385 pcb->state = W4_L2CAP_CONNECT_RSP;
1386 } else {
1387 L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available? */
1388 }
1389 //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);
1390 } else {
1391 ERROR("lp_connect_cfm: No resources available\n");
1392 L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available */
1393 }
1394 } else {
1395 ERROR("lp_connect_cfm: Connection falied\n");
1396 L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available */
1397 }
1398 }
1399 }
1400 /*-----------------------------------------------------------------------------------*/
1401 /*
1402 * lp_connect_ind():
1403 *
1404 * Indicates the lower protocol has successfully established a connection.
1405 */
1406 /*-----------------------------------------------------------------------------------*/
1407 void lp_connect_ind(struct bd_addr *bdaddr)
1408 {
1409 LOG("lp_connect_ind\n");
1410 }
1411 /*-----------------------------------------------------------------------------------*/
1412 /*
1413 * lp_disconnect_ind():
1414 *
1415 * Indicates the lower protocol (Baseband) has been shut down by LMP commands or a
1416 * timeout event..
1417 */
1418 /*-----------------------------------------------------------------------------------*/
1419 void lp_disconnect_ind(struct bd_addr *bdaddr,u8_t reason)
1420 {
1421 struct l2cap_pcb *pcb, *tpcb;
1422 err_t ret;
1423
1424 for(pcb = l2cap_active_pcbs; pcb != NULL;) {
1425 tpcb = pcb->next;
1426 LOG("lp_disconnect_ind: Find a pcb with a matching Bluetooth address\n");
1427 /* All PCBs with matching Bluetooth address have been disconnected */
1428 if(bd_addr_cmp(&(pcb->remote_bdaddr), bdaddr)) {// && pcb->state != L2CAP_CLOSED) {
1429 pcb->state = L2CAP_CLOSED;
1430 LOG("lp_disconnect_ind: Notify application\n");
1431 L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
1432 }
1433 pcb = tpcb;
1434 }
1435 if(l2cap_disconnect_bb_cb) l2cap_disconnect_bb_cb(bdaddr,reason);
1436 }
1437
1438 /*-----------------------------------------------------------------------------------*/
1439 /*
1440 * l2cap_disconnect_bb():
1441 *
1442 * Register a callback to obtain the disconnection reason from the baseband
1443 */
1444 /*-----------------------------------------------------------------------------------*/
1445 void (*l2cap_disconnect_bb(void (*l2ca_disconnect_bb)(struct bd_addr *bdaddr,u8_t reason)))(struct bd_addr *bdaddr,u8_t reason)
1446 {
1447 void (*oldcb)(struct bd_addr *bdaddr,u8_t reason) = NULL;
1448 oldcb = l2cap_disconnect_bb_cb;
1449 l2cap_disconnect_bb_cb = l2ca_disconnect_bb;
1450 return oldcb;
1451 }
1452
1453 /*-----------------------------------------------------------------------------------*/
1454 /*
1455 * l2cap_next_sigid():
1456 *
1457 * Issues a signal identifier that helps matching a request with the reply.
1458 */
1459 /*-----------------------------------------------------------------------------------*/
1460 u8_t l2cap_next_sigid(void)
1461 {
1462 ++sigid_nxt;
1463 if(sigid_nxt == 0) {
1464 sigid_nxt = 1;
1465 }
1466 return sigid_nxt;
1467 }
1468 /*-----------------------------------------------------------------------------------*/
1469 /*
1470 * l2cap_arg():
1471 *
1472 * Used to specify the argument that should be passed callback functions.
1473 */
1474 /*-----------------------------------------------------------------------------------*/
1475 void l2cap_arg(struct l2cap_pcb *pcb, void *arg)
1476 {
1477 pcb->callback_arg = arg;
1478 }
1479
1480 /*-----------------------------------------------------------------------------------*/
1481 /*
1482 * l2cap_connect_ind():
1483 *
1484 * Set the state of the connection to be LISTEN, which means that it is able to accept
1485 * incoming connections. The protocol control block is reallocated in order to consume
1486 * less memory. Setting the connection to LISTEN is an irreversible process. Also
1487 * specify the function that should be called when the channel has received a
1488 * connection request.
1489 */
1490 /*-----------------------------------------------------------------------------------*/
1491 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))
1492 {
1493 struct l2cap_pcb_listen *lpcb;
1494
1495 lpcb = btmemb_alloc(&l2cap_listenpcbs);
1496 if(lpcb == NULL) {
1497 ERROR("l2cap_connect_ind: Could not allocate memory for lpcb\n");
1498 return ERR_MEM;
1499 }
1500
1501 bd_addr_set(&(lpcb->bdaddr),bdaddr);
1502 lpcb->psm = psm;
1503 lpcb->l2ca_connect_ind = l2ca_connect_ind;
1504 lpcb->state = L2CAP_LISTEN;
1505 lpcb->callback_arg = npcb->callback_arg;
1506 btmemb_free(&l2cap_pcbs, npcb);
1507 L2CAP_REG(&(l2cap_listen_pcbs), lpcb);
1508 return ERR_OK;
1509 }
1510
1511 /*-----------------------------------------------------------------------------------*/
1512 /*
1513 * l2cap_disconnect_ind():
1514 *
1515 * Used to specify the a function to be called when a disconnection request has been
1516 * received from a remote device or the remote device has been disconnected because it
1517 * has failed to respond to a signalling request.
1518 */
1519 /*-----------------------------------------------------------------------------------*/
1520 void l2cap_disconnect_ind(struct l2cap_pcb *pcb, err_t (* l2ca_disconnect_ind)(void *arg, struct l2cap_pcb *newpcb, err_t err))
1521 {
1522 pcb->l2ca_disconnect_ind = l2ca_disconnect_ind;
1523 }
1524 /*-----------------------------------------------------------------------------------*/
1525 /*
1526 * l2cap_timeout_ind():
1527 *
1528 * Used to specify the function to be called when RTX or ERTX timer has expired.
1529 */
1530 /*-----------------------------------------------------------------------------------*/
1531 void l2cap_timeout_ind(struct l2cap_pcb *pcb,err_t (* l2ca_timeout_ind)(void *arg, struct l2cap_pcb *newpcb, err_t err))
1532 {
1533 pcb->l2ca_timeout_ind = l2ca_timeout_ind;
1534 }
1535 /*-----------------------------------------------------------------------------------*/
1536 /*
1537 * l2cap_recv():
1538 *
1539 * Used to specify the function that should be called when a L2CAP connection receives
1540 * data.
1541 */
1542 /*-----------------------------------------------------------------------------------*/
1543 void l2cap_recv(struct l2cap_pcb *pcb, err_t (* l2ca_recv)(void *arg, struct l2cap_pcb *pcb, struct pbuf *p, err_t err))
1544 {
1545 pcb->l2ca_recv = l2ca_recv;
1546 }
1547 /*-----------------------------------------------------------------------------------*/
inofficial libogc fork