l2cap.c

蓝牙协议源代码 bluetooth stack for lwip

    ret = l2cap_signal(pcb, L2CAP_CONN_REQ, 0, &(pcb->remote_bdaddr), data); /* Send l2cap_conn_req signal */
    
    pcb->state = W4_L2CAP_CONNECT_RSP;
  }
  
  L2CAP_REG(&l2cap_active_pcbs, pcb);
  
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2ca_config_req():
 * 
 * Requests the initial configuration (or reconfiguration) of a channel to a new set 
 * of channel parameters. Input parameters are the local CID endpoint, new incoming 
 * receivable MTU (InMTU), new outgoing flow specification, and flush and link 
 * timeouts. Also specify the function to be called when a confirm has been received.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2ca_config_req(struct l2cap_pcb *pcb)
{
  struct pbuf *p, *q;
  struct l2cap_cfgopt_hdr *opthdr;
  err_t ret;

  switch(pcb->state) {
  case L2CAP_OPEN:
     LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_config_req: state = L2CAP_OPEN. Suspend transmission\n"));
     /* Note: Application should have suspended data transmission, otherwise outgoing data will be 
	dropped */
    pcb->state = L2CAP_CONFIG;
    /* Fallthrough */
  case L2CAP_CONFIG:
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_config_req: state = L2CAP_CONFIG\n"));

    if((p = pbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_config_req: Could not allocate memory for pbuf\n"));
      return ERR_MEM;
    }

    /* Assemble config request packet. Only options that has to be changed will be
       sent */
    ((u16_t *)p->payload)[0] = pcb->dcid;
    /* In this implementation we do not send multiple cmds in one
       signal packet. Therefore we will never send a config_req packet
       that will cause the signal to be larger than the minimum L2CAP MTU
       48 bytes. Hence, this flag will always be cleared */
    ((u16_t *)p->payload)[1] = 0; 
    
    /* Add MTU and out flush timeout to cfg packet if not default value. QoS (Best effort) is always 
       set to default and can be skipped */
    if(pcb->cfg.inmtu != L2CAP_CFG_DEFAULT_INMTU) {
      if((q = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + L2CAP_MTU_LEN, PBUF_RAM)) == NULL) {
	pbuf_free(p);
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_config_req: Could not allocate memory for pbuf\n"));
	return ERR_MEM;
      }
      opthdr = q->payload;
      opthdr->type = L2CAP_CFG_MTU;
      opthdr->len = L2CAP_MTU_LEN;
      ((u16_t *)q->payload)[1] = pcb->cfg.inmtu;
      pbuf_chain(p, q);
      pbuf_free(q);
    }

    if(L2CAP_OUT_FLUSHTO != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
      if((q = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + L2CAP_FLUSHTO_LEN, PBUF_RAM)) == NULL) {
	pbuf_free(p);
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_config_req: Could not allocate memory for pbuf\n"));
	return ERR_MEM;
      }
      opthdr = q->payload;
      opthdr->type = L2CAP_FLUSHTO;
      opthdr->len = L2CAP_FLUSHTO_LEN;
      ((u16_t *)q->payload)[1] = pcb->cfg.outflushto = L2CAP_OUT_FLUSHTO;
      pbuf_chain(p, q);
      pbuf_free(q);
    }

    /* Send config request signal */
    ret = l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), p);
    break;
  default:
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_config_req: state = L2CAP_?. Invalid state\n"));
    return ERR_CONN; /* Invalid state. Connection is not in OPEN or CONFIG state */
  }
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2ca_disconnect_req():
 * 
 * Requests the disconnection of the channel. Also specify the function to be called
 * when a confirm is received
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2ca_disconnect_req(struct l2cap_pcb *pcb, err_t (* l2ca_disconnect_cfm)(void *arg, struct l2cap_pcb *pcb))
{
  struct pbuf *data;
  err_t ret;

  if(pcb->state == L2CAP_OPEN || pcb->state == L2CAP_CONFIG) {
    if((data = pbuf_alloc(PBUF_RAW, L2CAP_DISCONN_REQ_SIZE, PBUF_RAM)) == NULL) {
     LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_disconnect_req: Could not allocate memory for pbuf\n")); 
      return ERR_MEM;
    }
    pcb->l2ca_disconnect_cfm = l2ca_disconnect_cfm;

    ((u16_t *)data->payload)[0] = pcb->dcid;
    ((u16_t *)data->payload)[1] = pcb->scid;

    ret = l2cap_signal(pcb, L2CAP_DISCONN_REQ, 0, &(pcb->remote_bdaddr), data);
  
    if(ret == ERR_OK) {
      pcb->state = W4_L2CAP_DISCONNECT_RSP;  
    }
  } else {
    return ERR_CONN; /* Signal not supported in this state */
  }

  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2ca_datawrite():
 * 
 * Transfers data across the channel.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2ca_datawrite(struct l2cap_pcb *pcb, struct pbuf *p)
{
  err_t ret;
  struct l2cap_hdr *l2caphdr;
  struct pbuf *q;

  if(pcb->state != L2CAP_OPEN) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_datawrite: State != L2CAP_OPEN. Dropping data\n"));
    return ERR_CONN;
  }

  /* Build L2CAP header */
  if((q = pbuf_alloc(PBUF_RAW, L2CAP_HDR_LEN, PBUF_RAM)) == NULL) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_datawrite: Could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }
  pbuf_chain(q, p);

  l2caphdr = q->payload;
  l2caphdr->cid = pcb->dcid;
  
  /* If length of the data exceeds the OutMTU then only the first OutMTU bytes are sent */
  if(p->tot_len > pcb->cfg.outmtu) {
    /* Send peer L2CAP data */
    l2caphdr->len = pcb->cfg.outmtu;
    if((ret = l2cap_write(&(pcb->remote_bdaddr), q, pcb->cfg.outmtu + L2CAP_HDR_LEN)) == ERR_OK) {
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_datawrite: Length of data exceeds the OutMTU p->tot_len = %d\n", p->tot_len));
      ret = ERR_BUF; /* Length of data exceeds the OutMTU */ 
    }
  } else {
    /* Send peer L2CAP data */
    l2caphdr->len = p->tot_len;
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_datawrite: q->tot_len = %d\n", q->tot_len));
    ret = l2cap_write(&(pcb->remote_bdaddr), q, q->tot_len);
  }

  /* Free L2CAP header. Higher layers will handle rest of packet */
  p = pbuf_dechain(q);
  pbuf_free(q);

  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2ca_ping():
 * 
 * Sends an empty L2CAP echo request message. Also specify the function that should
 * be called when a L2CAP echo reply has been received.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2ca_ping(struct bd_addr *bdaddr, struct l2cap_pcb *tpcb,
	  err_t (* l2ca_pong)(void *arg, struct l2cap_pcb *pcb, u8_t result))
{
  err_t ret;
  
  if(!lp_is_connected(bdaddr)) {
    return ERR_CONN;
  }

  bd_addr_set(&(tpcb->remote_bdaddr), bdaddr);
  tpcb->l2ca_pong = l2ca_pong;
  
  L2CAP_REG(&l2cap_active_pcbs, tpcb);
  
  ret = l2cap_signal(tpcb, L2CAP_ECHO_REQ, 0, &(tpcb->remote_bdaddr), NULL); /* Send l2cap_echo_req signal */
  
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* Lower-Layer to L2CAP signaling events
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* 
 * lp_connect_cfm():
 * 
 * Confirms the request to establish a lower layer (Baseband) connection.
 */
/*-----------------------------------------------------------------------------------*/
void
lp_connect_cfm(struct bd_addr *bdaddr, u8_t encrypt_mode, err_t err)
{
  struct l2cap_pcb *pcb;
  struct pbuf *data;
  err_t ret;
  
  for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
    if(bd_addr_cmp(&(pcb->remote_bdaddr), bdaddr)) {
      break;
    }
  }
  if(pcb == NULL) {
    /* Silently discard */
    LWIP_DEBUGF(L2CAP_DEBUG, ("lp_connect_cfm: Silently discard\n")); 
  } else {
    if(err == ERR_OK) {
      pcb->encrypt = encrypt_mode;
      /* Send l2cap_conn_req signal if no error */
      if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_REQ_SIZE, PBUF_RAM)) != NULL) {
	((u16_t *)data->payload)[0] = pcb->psm;
	((u16_t *)data->payload)[1] = pcb->scid;
	if((ret = l2cap_signal(pcb, L2CAP_CONN_REQ, 0, &(pcb->remote_bdaddr), data)) == ERR_OK) {
	  pcb->state = W4_L2CAP_CONNECT_RSP;
	} else {
	  L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available? */
	}
	LWIP_DEBUGF(L2CAP_DEBUG, ("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)); 
      } else {
	LWIP_DEBUGF(L2CAP_DEBUG, ("lp_connect_cfm: No resources available\n")); 
	L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available */
      }
    } else {
      LWIP_DEBUGF(L2CAP_DEBUG, ("lp_connect_cfm: Connection falied\n")); 
      L2CA_ACTION_CONN_CFM(pcb,L2CAP_CONN_REF_RES,0x0000,ret); /* No resources available */
    }
  }
}
/*-----------------------------------------------------------------------------------*/
/* 
 * lp_connect_ind():
 * 
 * Indicates the lower protocol has successfully established a connection.
 */
/*-----------------------------------------------------------------------------------*/
void
lp_connect_ind(struct bd_addr *bdaddr)
{
  LWIP_DEBUGF(L2CAP_DEBUG, ("lp_connect_ind\n")); 
}
/*-----------------------------------------------------------------------------------*/
/* 
 * lp_disconnect_ind():
 * 
 * Indicates the lower protocol (Baseband) has been shut down by LMP commands or a 
 * timeout event..
 */
/*-----------------------------------------------------------------------------------*/
void
lp_disconnect_ind(struct bd_addr *bdaddr)
{
  struct l2cap_pcb *pcb, *tpcb;
  err_t ret;
  
  for(pcb = l2cap_active_pcbs; pcb != NULL;) {
    tpcb = pcb->next;
    LWIP_DEBUGF(L2CAP_DEBUG, ("lp_disconnect_ind: Find a pcb with a matching Bluetooth address\n"));
    /* All PCBs with matching Bluetooth address have been disconnected */
    if(bd_addr_cmp(&(pcb->remote_bdaddr), bdaddr)) {// && pcb->state != L2CAP_CLOSED) {
      pcb->state = L2CAP_CLOSED;
      LWIP_DEBUGF(L2CAP_DEBUG, ("lp_disconnect_ind: Notify application\n"));
      L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
    }
    pcb = tpcb;
  }
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_next_sigid():
 * 
 * Issues a signal identifier that helps matching a request with the reply.
 */
/*-----------------------------------------------------------------------------------*/
u8_t
l2cap_next_sigid(void)
{
  ++sigid_nxt;
  if(sigid_nxt == 0) {
    sigid_nxt = 1;
  }
  return sigid_nxt;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_arg():
 * 
 * Used to specify the argument that should be passed callback functions.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_arg(struct l2cap_pcb *pcb, void *arg)
{
  pcb->callback_arg = arg;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_connect_ind():
 * 
 * Set the state of the connection to be LISTEN, which means that it is able to accept 
 * incoming connections. The protocol control block is reallocated in order to consume 
 * less memory. Setting the connection to LISTEN is an irreversible process. Also 
 * specify the function that should be called when the channel has received a 
 * connection request.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
l2cap_connect_ind(struct l2cap_pcb *npcb, u8_t psm,
		  err_t (* l2ca_connect_ind)(void *arg, struct l2cap_pcb *pcb, err_t err))
{
  struct l2cap_pcb_listen *lpcb;

  lpcb = lwbt_memp_malloc(MEMP_L2CAP_PCB_LISTEN);
  if(lpcb == NULL) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_connect_ind: Could not allocate memory for lpcb\n"));
    return ERR_MEM;
  }
  lpcb->psm = psm;
  lpcb->l2ca_connect_ind = l2ca_connect_ind;
  lpcb->state = L2CAP_LISTEN;
  lpcb->callback_arg = npcb->callback_arg;
  lwbt_memp_free(MEMP_L2CAP_PCB, npcb);
  L2CAP_REG((struct l2cap_pcb **)&l2cap_listen_pcbs, (struct l2cap_pcb *)lpcb);
  return ERR_OK;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_disconnect_ind():
 * 
 * Used to specify the a function to be called when a disconnection request has been 
 * received from a remote device or the remote device has been disconnected because it 
 * has failed to respond to a signalling request.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_disconnect_ind(struct l2cap_pcb *pcb,
		     err_t (* l2ca_disconnect_ind)(void *arg, struct l2cap_pcb *newpcb, err_t err))
{
  pcb->l2ca_disconnect_ind = l2ca_disconnect_ind;
}
/*-----------------------------------------------------------------------------------*/
/*
 * l2cap_timeout_ind():
 * 
 * Used to specify the function to be called when RTX or ERTX timer has expired.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_timeout_ind(struct l2cap_pcb *pcb,
		  err_t (* l2ca_timeout_ind)(void *arg, struct l2cap_pcb *newpcb, err_t err))
{
  pcb->l2ca_timeout_ind = l2ca_timeout_ind;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_recv():
 * 
 * Used to specify the function that should be called when a L2CAP connection receives 
 * data.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_recv(struct l2cap_pcb *pcb,
	   err_t (* l2ca_recv)(void *arg, struct l2cap_pcb *pcb, struct pbuf *p, err_t err))
{
  pcb->l2ca_recv = l2ca_recv;
}
/*-----------------------------------------------------------------------------------*/