API tools faq
paste
Advertisement
xerpi

l2cap.c LOG->printf

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