l2cap.c

蓝牙协议源代码 bluetooth stack for lwip

  pbuf_header(p, -HCI_ACL_HDR_LEN);

  pbuf_realloc(p, aclhdr->len);

  for(inseg = l2cap_insegs; inseg != NULL; inseg = inseg->next) {
    if(bd_addr_cmp(bdaddr, &(inseg->bdaddr))) {
      break;
    }
  }

  /* Reassembly procedures */
  /* Check if continuing fragment or start of L2CAP packet */
  if(((aclhdr->conhdl_pb_bc >> 12) & 0x03)== L2CAP_ACL_CONT) { /* Continuing fragment */
    if(inseg == NULL)  {
      /* Discard packet */
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Continuing fragment. Discard packet\n"));
      pbuf_free(p);
      return;
    } else if(inseg->p->tot_len + p->tot_len > inseg->len) { /* Check if length of
								segment exceeds
								l2cap header length */
      /* Discard packet */
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Continuing fragment. Length exceeds L2CAP hdr length. Discard packet\n"));
      pbuf_free(inseg->p);
      L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
      lwbt_memp_free(MEMP_L2CAP_SEG, inseg);

      pbuf_free(p);
      return;
    }
    /* Add pbuf to segement */
    pbuf_chain(inseg->p, p);
    pbuf_free(p);
    
  } else if(((aclhdr->conhdl_pb_bc >> 12) & 0x03) == L2CAP_ACL_START) { /* Start of L2CAP packet */
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Start of L2CAP packet p->len = %d, p->tot_len = %d\n", 
			 p->len, p->tot_len));
    if(inseg != NULL) { /* Check if there are segments missing in a previous packet */
      /* Discard previous packet */
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Start of L2CAP packet. Discard previous packet\n"));
      pbuf_free(inseg->p);
    } else {
      inseg = lwbt_memp_malloc(MEMP_L2CAP_SEG);
      bd_addr_set(&(inseg->bdaddr), bdaddr);
      L2CAP_SEG_REG(&(l2cap_insegs), inseg);
    }
    inseg->p = p;
    inseg->l2caphdr = p->payload;
    inseg->len = inseg->l2caphdr->len + L2CAP_HDR_LEN;
    for(inseg->pcb = l2cap_active_pcbs; inseg->pcb != NULL; inseg->pcb = inseg->pcb->next) {
      if(inseg->pcb->scid == inseg->l2caphdr->cid) {
	break; /* found */
      }
    }
  } else {
    /* Discard packet */
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Discard packet\n"));
    pbuf_free(inseg->p);
    L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
    lwbt_memp_free(MEMP_L2CAP_SEG, inseg);

    pbuf_free(p);
    return;
  }
  if(inseg->p->tot_len < inseg->len) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Get continuing segments\n"));
    return; /* Get continuing segments */
  }

  /* Handle packet */
  switch(inseg->l2caphdr->cid) {
  case L2CAP_NULL_CID:
    /* Illegal */
    LWIP_DEBUGF(L2CAP_DEBUG,("l2cap_input: Illegal null cid\n"));
    pbuf_free(inseg->p);
    break;
  case L2CAP_SIG_CID:
    pbuf_header(inseg->p, -L2CAP_HDR_LEN);
    l2cap_process_sig(inseg->p, inseg->l2caphdr, bdaddr);
    pbuf_free(inseg->p);
    break;
  case L2CAP_CONNLESS_CID:
    /* Not needed by PAN, LAN access or DUN profiles */
    pbuf_free(inseg->p);
    break;
  default:
    if(inseg->l2caphdr->cid < 0x0040 || inseg->pcb == NULL) {
      /* Reserved for specific L2CAP functions or channel does not exist */
      /* Alloc size of reason in cmd rej */
      if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
	((u16_t *)data->payload)[0] = L2CAP_INVALID_CID;
	((u16_t *)data->payload)[1] = inseg->l2caphdr->cid;
	((u16_t *)data->payload)[2] = L2CAP_NULL_CID;
      
	ret = l2cap_signal(NULL, L2CAP_CMD_REJ, l2cap_next_sigid(), bdaddr, data);
      }
      pbuf_free(inseg->p);
      break;
    }

    pbuf_header(inseg->p, -L2CAP_HDR_LEN);

    /* Forward packet to higher layer */
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Forward packet to higher layer\n"));
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Remote BD address: 0x%x:0x%x:0x%x:0x%x:0x%x:0x%x\n",
		       inseg->pcb->remote_bdaddr.addr[5],
		       inseg->pcb->remote_bdaddr.addr[4],
		       inseg->pcb->remote_bdaddr.addr[3],
		       inseg->pcb->remote_bdaddr.addr[2],
		       inseg->pcb->remote_bdaddr.addr[1],
		       inseg->pcb->remote_bdaddr.addr[0]));
    
    L2CA_ACTION_RECV(inseg->pcb,inseg->p,ERR_OK,ret);
    break;
  }
  
  /* Remove input segment */
  L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
  lwbt_memp_free(MEMP_L2CAP_SEG, inseg);

}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_cid_alloc():
 * 
 * Allocates a channel identifier (CID). They are local names representing a logical 
 * channel endpoint on the device.
 */
/*-----------------------------------------------------------------------------------*/
static u16_t
l2cap_cid_alloc(void)
{
  u16_t cid;
  struct l2cap_pcb *pcb;
  
  for (cid = L2CAP_MIN_CID; cid < L2CAP_MAX_CID; ++cid) {
    for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
      if(pcb->scid == cid) {
	break;
      }
    }
    if(pcb == NULL) {
      return cid;
    }
  }
  return 0;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_new():
 * 
 * Creates a new L2CAP protocol control block but doesn't place it on
 * any of the L2CAP PCB lists.
 */
/*-----------------------------------------------------------------------------------*/
struct l2cap_pcb *
l2cap_new(void)
{
  struct l2cap_pcb *pcb;
  
  pcb = lwbt_memp_malloc(MEMP_L2CAP_PCB);
  if(pcb != NULL) {
    memset(pcb, 0, sizeof(struct l2cap_pcb));
    pcb->state = L2CAP_CLOSED;

    /* Initialize configuration parameter options with default values */
    
    /* Maximum Transmission Unit */
    pcb->cfg.inmtu = L2CAP_MTU; /* The MTU that this implementation support */
    pcb->cfg.outmtu = 672; /* Default MTU. Two Baseband DH5 packets minus the Baseband ACL headers and 
			      L2CAP header. This can be set here since we will never send any signals
			      larger than the L2CAP sig MTU (48 bytes) before L2CAP has been configured 
			   */

    /* Flush Timeout */
    pcb->cfg.influshto = 0xFFFF;
    pcb->cfg.outflushto = 0xFFFF;

    pcb->cfg.cfgto = L2CAP_CFG_TO; /* Maximum time before terminating a negotiation.
				      Cfg shall not last more than 120s */
    pcb->cfg.opt = NULL;
    return pcb;
  }
  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_new: Could not allocate memory for pcb\n"));
  return NULL;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_close():
 * 
 * Closes the L2CAP protocol control block.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2cap_close(struct l2cap_pcb *pcb)
{
  struct l2cap_sig *tmpsig;

  if(pcb->state == L2CAP_LISTEN) {
    L2CAP_RMV((struct l2cap_pcb **)&l2cap_listen_pcbs, pcb);
    lwbt_memp_free(MEMP_L2CAP_PCB_LISTEN, pcb);
  } else {
    L2CAP_RMV(&l2cap_active_pcbs, pcb);
    /* Free any unresponded signals */
    while(pcb->unrsp_sigs != NULL) {
      tmpsig = pcb->unrsp_sigs;
      pcb->unrsp_sigs = pcb->unrsp_sigs->next;
      lwbt_memp_free(MEMP_L2CAP_SIG, tmpsig);
    }

    lwbt_memp_free(MEMP_L2CAP_PCB, pcb);
  }
  pcb = NULL;
  return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_reset_all():
 *
 * Closes all active and listening L2CAP protocol control blocks.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_reset_all(void)
{ 
  struct l2cap_pcb *pcb, *tpcb;
  struct l2cap_pcb_listen *lpcb, *tlpcb;
  struct l2cap_seg *seg, *tseg;

  for(pcb = l2cap_active_pcbs; pcb != NULL;) {
    tpcb = pcb->next;
    l2cap_close(pcb);
    pcb = tpcb;
  }

  for(lpcb = l2cap_listen_pcbs; lpcb != NULL;) {
    tlpcb = lpcb->next;
    l2cap_close((struct l2cap_pcb *)lpcb);
    lpcb = tlpcb;
  }

  for(seg = l2cap_insegs; seg != NULL;) {
    tseg = seg->next;
    L2CAP_SEG_RMV(&(l2cap_insegs), seg);
    lwbt_memp_free(MEMP_L2CAP_SEG, seg);
    seg = tseg;
  }

  l2cap_init();
}
/*-----------------------------------------------------------------------------------*/
/* L2CAP to L2CAP signalling events
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_signal():
 * 
 * Assembles the signalling packet and passes it to the lower layer.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2cap_signal(struct l2cap_pcb *pcb, u8_t code, u16_t ursp_id, struct bd_addr *remote_bdaddr, 
	     struct pbuf *data)
{
  struct l2cap_sig *sig;
  struct l2cap_sig_hdr *sighdr;
  struct l2cap_hdr *hdr;
  err_t ret;

  /* Alloc a new signal */
  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_signal: Allocate memory for l2cap_sig. Code = 0x%x\n", code));
  if((sig = lwbt_memp_malloc(MEMP_L2CAP_SIG)) == NULL) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_signal: could not allocate memory for l2cap_sig\n"));
    return ERR_MEM;
  }

  /* Alloc a pbuf for signal */
  if((sig->p = pbuf_alloc(PBUF_RAW, L2CAP_HDR_LEN+L2CAP_SIGHDR_LEN, PBUF_RAM)) == NULL) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_signal: could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }

  /* Setup signal header and leave room for l2cap hdr */
  sighdr = (struct l2cap_sig_hdr *)(((u8_t *)sig->p->payload)+L2CAP_HDR_LEN);

  /* Chain data to signal and set length of signal data */
  if(data == NULL) {
    sighdr->len = 0;
  } else {
    pbuf_chain(sig->p, data);
    pbuf_free(data);
    sighdr->len = data->tot_len;
  }
  
  sighdr->code = code;
  
  if(sighdr->code % 2) { /* If odd this is a resp/rej signal */
    sig->sigid = ursp_id; /* Get id */
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_signal: Sending response/reject signal with id = %d code = %d\n", 
			 sig->sigid, sighdr->code));
  } else {
    sig->sigid = l2cap_next_sigid(); /* Alloc id */
    sig->rtx = L2CAP_RTX; /* Set Response Timeout Expired timer (in seconds)
			     should be at least as large as the BB flush timeout */
    sig->nrtx = L2CAP_MAXRTX; /* Set max number of retransmissions */
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_signal: Sending request signal with id = %d code = %d\n", 
			 sig->sigid, sighdr->code));
  }
  sighdr->id = sig->sigid; /* Set id */
  
  /* Set up L2CAP hdr */
  hdr = sig->p->payload;
  hdr->len = sig->p->tot_len - L2CAP_HDR_LEN;
  hdr->cid = L2CAP_SIG_CID; /* 0x0001 */
  
  ret = l2cap_write(remote_bdaddr, sig->p, sig->p->tot_len); /* Send peer L2CAP signal */
  
  /* Put signal on unresponded list if it's a request signal, else deallocate it */
  if(ret == ERR_OK && (sighdr->code % 2) == 0) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_signal: Registering sent request signal with id = %d code = %d\n", 
			 sig->sigid, sighdr->code));
    L2CAP_SIG_REG(&(pcb->unrsp_sigs), sig);
  } else {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_signal: Deallocating sent response/reject signal with id = %d code = %d\n", 
			 sig->sigid, sighdr->code));
    pbuf_free(sig->p);
    sig->p = NULL;
    lwbt_memp_free(MEMP_L2CAP_SIG, sig);
  }
  
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_rexmit_signal():
 * 
 * Called by the l2cap timer. Retransmitts a signal.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2cap_rexmit_signal(struct l2cap_pcb *pcb, struct l2cap_sig *sig)
{
  err_t ret;
  
  /* Set up L2CAP hdr */
  ret = l2cap_write(&(pcb->remote_bdaddr), sig->p, sig->p->tot_len); /* Send peer L2CAP signal */

  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* Upper-Layer to L2CAP signaling events
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* 
 * l2ca_connect_req():
 * 
 * Initiates the sending of a connect request message. Requests the creation of a 
 * channel representing a logicalconnection to a physical address. Input parameters 
 * are the target protocol(PSM) and remote devices 48-bit address (BD_ADDR). Also 
 * specify the function to be called when a confirm has been received.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2ca_connect_req(struct l2cap_pcb *pcb, struct bd_addr *bdaddr, u16_t psm, 
                 u8_t role_switch, err_t (* l2ca_connect_cfm)(void *arg, struct l2cap_pcb *lpcb,
					    u16_t result, u16_t status))
{
  err_t ret;
  struct pbuf *data;
  
  if(bdaddr != NULL) {
    bd_addr_set(&(pcb->remote_bdaddr),bdaddr);
  } else {
    return ERR_VAL;
  }
  
  pcb->psm = psm;
  pcb->l2ca_connect_cfm = l2ca_connect_cfm;
  pcb->scid = l2cap_cid_alloc();
  LWIP_ASSERT("l2ca_connect_req: out of CIDs\n", pcb->scid != 0);

  pcb->cfg.l2capcfg |= L2CAP_CFG_IR; /* We are the initiator of this connection */

  if(!lp_is_connected(bdaddr)) {
    ret = lp_connect_req(bdaddr, role_switch); /* Create ACL link w pcb state == CLOSED */
  } else {
    if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_REQ_SIZE, PBUF_RAM)) == NULL) {
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_connect_req: Could not allocate memory for pbuf\n"));
      return ERR_MEM;
    }
    ((u16_t *)data->payload)[0] = psm;
    ((u16_t *)data->payload)[1] = pcb->scid;