rfcomm.c

蓝牙协议源代码 bluetooth stack for lwip

{
  struct pbuf *p;
  struct rfcomm_hdr *rfcommhdr;
  err_t ret;
  
  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_ua\n"));

  if((p = pbuf_alloc(PBUF_RAW, RFCOMM_UA_LEN, PBUF_RAM)) == NULL) {
    /* Could not allocate memory for pbuf */
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_ua: Could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }

  rfcommhdr = p->payload;
  rfcommhdr->addr = hdr->addr & 0xFB; /* Set direction bit to 0 for the response */
  rfcommhdr->ctrl = RFCOMM_UA;
  rfcommhdr->len = 1; /* EA bit set to 1 to indicate a 7 bit length field */
  ((u8_t *)p->payload)[RFCOMM_HDR_LEN_1] = fcs8_crc_calc(p, RFCOMM_CRC_CHECK_LEN);
  
  ret = l2ca_datawrite(pcb, p);
  pbuf_free(p);
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_pn():
 *
 * Sends a RFCOMM parameter negotiation multiplexer frame to negotiate the parameters
 * of a data link connection. Also specify the function to be called when a PN 
 * response is received
 */
/*-----------------------------------------------------------------------------------*/
err_t
rfcomm_pn(struct rfcomm_pcb *pcb, 
	  err_t (* pn_rsp)(void *arg, struct rfcomm_pcb *pcb, err_t err))
{
  struct pbuf *p;
  struct rfcomm_msg_hdr *cmdhdr;
  struct rfcomm_pn_msg *pnmsg;
  err_t ret;
  struct rfcomm_pcb *opcb;
  
  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_pn\n"));

  opcb = rfcomm_get_active_pcb(0, &pcb->l2cappcb->remote_bdaddr);

  if((p = pbuf_alloc(PBUF_RAW, RFCOMM_MSGHDR_LEN + RFCOMM_PNMSG_LEN, PBUF_RAM)) == NULL) {
    /* Could not allocate memory for pbuf */
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_pn: Could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }
  
  /* Set multiplexer parameter negotiation command header */
  cmdhdr = p->payload;
  cmdhdr->type = RFCOMM_PN_CMD;
  cmdhdr->len = 1 | (RFCOMM_PNMSG_LEN << 1);

  /* Set multiplexer parameter negotiation command paramenters */
  pnmsg = (void *)(((u8_t *)p->payload) + RFCOMM_MSGHDR_LEN);
  pnmsg->dlci = (((opcb->rfcommcfg & RFCOMM_CFG_IR) ^ 1) << 0) | (pcb->cn << 1);
  pnmsg->i_cl = 0 | (RFCOMM_CL << 4);
  pnmsg->p = 0;
  pnmsg->t = 0;
  pnmsg->n = RFCOMM_N;
  pnmsg->na = 0;
  pnmsg->k = RFCOMM_K;
  
  if((ret = rfcomm_uih(opcb, 0, p)) == ERR_OK) {
    pcb->pn_rsp = pn_rsp;
    opcb->to = RFCOMM_TO; /* Set acknowledgement timer, 10-60s */
  }
  pbuf_free(p);
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_test():
 *
 * .
 */
/*-----------------------------------------------------------------------------------*/
err_t
rfcomm_test(struct rfcomm_pcb *pcb, err_t (* test_rsp)(void *arg, struct rfcomm_pcb *tpcb, err_t err)) 
{
  struct pbuf *p;
  struct rfcomm_msg_hdr *cmdhdr;
  err_t ret;
  struct rfcomm_pcb *opcb;
  
  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_test\n"));

  opcb = rfcomm_get_active_pcb(0, &pcb->l2cappcb->remote_bdaddr);

  if((p = pbuf_alloc(PBUF_RAW, RFCOMM_MSGHDR_LEN, PBUF_RAM)) == NULL) {
    /* Could not allocate memory for pbuf */
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_test: Could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }

  /* Set multiplexer modem status command header */
  cmdhdr = p->payload;
  cmdhdr->type = RFCOMM_TEST_CMD;
  cmdhdr->len = 1 | (0 << 1);
  
  if((ret = rfcomm_uih(opcb, 0, p)) == ERR_OK) {
    opcb->test_rsp = test_rsp;
    opcb->to = RFCOMM_TO; /* Set acknowledgement timer, 10-60s */
  }
  pbuf_free(p);
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_msc():
 *
 * Sends a RFCOMM modem status multiplexer frame. Also specify the function to be 
 * called when a MSC response is received.
 */
/*-----------------------------------------------------------------------------------*/
err_t
rfcomm_msc(struct rfcomm_pcb *pcb, u8_t fc, 
	   err_t (* msc_rsp)(void *arg, struct rfcomm_pcb *pcb, err_t err))
{
  struct pbuf *p;
  struct rfcomm_msg_hdr *cmdhdr;
  struct rfcomm_msc_msg *mscmsg;
  err_t ret;
  struct rfcomm_pcb *opcb;

  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_msc\n"));

  opcb = rfcomm_get_active_pcb(0, &pcb->l2cappcb->remote_bdaddr);

  if((p = pbuf_alloc(PBUF_RAW, RFCOMM_MSGHDR_LEN + RFCOMM_MSCMSG_LEN, PBUF_RAM)) == NULL) {
    /* Could not allocate memory for pbuf */
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_msc: Could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }

  /* Set multiplexer modem status command header */
  cmdhdr = p->payload;
  cmdhdr->type = RFCOMM_MSC_CMD;
  cmdhdr->len = 1 | (RFCOMM_MSCMSG_LEN << 1);

  /* Set multiplexer parameter negotiation command paramenters */
  mscmsg = (void *)(((u8_t *)p->payload) + RFCOMM_MSGHDR_LEN);
  // mscmsg->dlci = (1 << 0) | (1 << 1) | (((pcb->rfcommcfg & RFCOMM_CFG_IR) ^ 1) << 2) | (pcb->cn << 3);
  mscmsg->dlci = (1 << 0) | (1 << 1) | (0 << 2) | (pcb->cn << 3);
  mscmsg->rs232 = (1 << 0) | (fc << 1) | (0x23 << 2);

  if((ret = rfcomm_uih(opcb, 0, p)) == ERR_OK) {
    pcb->msc_rsp = msc_rsp;
    opcb->to = RFCOMM_TO; /* Set acknowledgement timer, 10-60s */
  }
  pbuf_free(p);
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_rpn():
 *
 * Sends a RFCOMM remote port negotiation multiplexer frame to set communication 
 * settings at the remote end of the data link connection.
 */
/*-----------------------------------------------------------------------------------*/
err_t //INITIATOR
rfcomm_rpn(struct rfcomm_pcb *pcb, u8_t br,
	   err_t (* rpn_rsp)(void *arg, struct rfcomm_pcb *pcb, err_t err))
{
  struct pbuf *p;
  struct rfcomm_msg_hdr *cmdhdr;
  struct rfcomm_rpn_msg *rpnmsg;
  err_t ret;
  struct rfcomm_pcb *opcb;

  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_rpn\n"));

  opcb = rfcomm_get_active_pcb(0, &pcb->l2cappcb->remote_bdaddr);

  if((p = pbuf_alloc(PBUF_RAW, RFCOMM_MSGHDR_LEN + RFCOMM_RPNMSG_LEN, PBUF_RAM)) == NULL) {
    /* Could not allocate memory for pbuf */
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_rpn: Could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }
  
  /* Set remote port negotiation command header */
  cmdhdr = p->payload;
  cmdhdr->type = RFCOMM_RPN_CMD;
  cmdhdr->len = 1 | (RFCOMM_RPNMSG_LEN << 1);

  /* Set remote port negotiation command paramenters */
  rpnmsg = (void *)(((u8_t *)p->payload) + RFCOMM_MSGHDR_LEN);
  rpnmsg->dlci = (1 << 0) | (1 << 1) | (((opcb->rfcommcfg & RFCOMM_CFG_IR) ^ 1) << 2) | (pcb->cn << 3);
  rpnmsg->br = br;
  rpnmsg->mask = 1;
  
  if((ret = rfcomm_uih(opcb, 0, p)) == ERR_OK) {
    pcb->rpn_rsp = rpn_rsp;
    opcb->to = RFCOMM_TO; /* Set acknowledgement timer, 10-60s */
    
  }
  pbuf_free(p);
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_uih():
 *
 * Sends a RFCOMM unnumbered information frame with header check.
 */
/*-----------------------------------------------------------------------------------*/
err_t //RESPONDER & INITIATOR
rfcomm_uih(struct rfcomm_pcb *pcb, u8_t cn, struct pbuf *q) 
{
  struct pbuf *p, *r;
  err_t ret;
  u16_t tot_len = 0;

  /* Decrease local credits */
  if(pcb->cl == 0xF && pcb->state == RFCOMM_OPEN && pcb->cn != 0) {
    if(pcb->k != 0) {
      --pcb->k;
    } else {
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih: Out of local credits\n"));
#if RFCOMM_FLOW_QUEUEING
      if(q != NULL) {
	/* Packet can be queued? */
	if(pcb->buf != NULL) {
	  return ERR_OK; /* Drop packet */
	} else {
	  /* Copy PBUF_REF referenced payloads into PBUF_RAM */
	  q = pbuf_take(q);
	  /* Remember pbuf to queue, if any */
	  pcb->buf = q;
	  /* Pbufs are queued, increase the reference count */
	  pbuf_ref(q);
	  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih: Queued packet %p on channel %d\n", (void *)q, pcb->cn));
	}
      }
#else
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih: Dropped packet.\n"));
      return ERR_OK; /* Drop packet */
#endif /* RFCOMM_FLOW_QUEUEING */      
    }
  }

  if(q != NULL) {
    tot_len = q->tot_len;
  }

  /* Size of information must be less than maximum frame size */
  if(tot_len > pcb->n) {
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih: Size of information must be less than maximum frame size\n"));
    return ERR_MEM;
  }

  if(tot_len < 127) {
    if((p = pbuf_alloc(PBUF_RAW, RFCOMM_UIH_LEN, PBUF_RAM)) == NULL) {
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih: Could not allocate memory for pbuf\n"));
      return ERR_MEM; /* Could not allocate memory for pbuf */
    }
  } else {
    if((p = pbuf_alloc(PBUF_RAW, RFCOMM_UIH_LEN+1, PBUF_RAM)) == NULL) {
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih: Could not allocate memory for pbuf\n")); 
      return ERR_MEM; /* Could not allocate memory for pbuf */
    }
  }

  /* Setup RFCOMM header */
  if(cn == 0) {
    ((u8_t *)p->payload)[0] = (1 << 0) | ((pcb->rfcommcfg & RFCOMM_CFG_IR) << 1) | (0 << 2) | (0 << 3);
  } else {
    ((u8_t *)p->payload)[0] = (1 << 0) | ((pcb->rfcommcfg & RFCOMM_CFG_IR) << 1) | (0 << 2) | (cn << 3);
  }
  ((u8_t *)p->payload)[1] = RFCOMM_UIH;
  if(q != NULL) {
    if(q->tot_len < 127) {
      ((u8_t *)p->payload)[2] = (1 << 0) | (q->tot_len << 1);
    } else {
      ((u16_t *)p->payload)[1] = (0 << 0) | (q->tot_len << 1);
    }
    /* Add information data to pbuf */
    pbuf_chain(p, q);
  } else {
    ((u8_t *)p->payload)[2] = (1 << 0) | (0 << 1); /* Empty UIH frame */
  }
  /* Add information FCS to pbuf */
  if((r = pbuf_alloc(PBUF_RAW, 1, PBUF_RAM)) == NULL) { 
    pbuf_free(p);
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih: Could not allocate memory for pbuf\n"));
    return ERR_MEM; /* Could not allocate memory for pbuf */
  }

  if(cn == 0) {
    ((u8_t *)r->payload)[0] = pcb->uih0_out_fcs;
  } else {
    ((u8_t *)r->payload)[0] = pcb->uih_out_fcs;
  }
  
  pbuf_chain(p, r);
  pbuf_free(r);

  ret = l2ca_datawrite(pcb->l2cappcb, p);

  /* Dealloc the RFCOMM header. Lower layers will handle rest of packet */
  if(q != NULL) {
    pbuf_dechain(p); /* Have q point to information + FCS */
    pbuf_realloc(q, q->tot_len-1); /* Remove FCS from packet */
  }
  pbuf_free(p);

  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_uih_credits():
 *
 * Sends a RFCOMM unnumbered information frame with header check and credit based 
 * flow control.
 */
/*-----------------------------------------------------------------------------------*/
err_t //RESPONDER & INITIATOR
rfcomm_uih_credits(struct rfcomm_pcb *pcb, u8_t credits, struct pbuf *q) 
{
  struct pbuf *p, *r;
  err_t ret;
  u16_t tot_len = 0;

  /* Decrease local credits */
  if(pcb->cl == 0xF && pcb->state == RFCOMM_OPEN && pcb->cn != 0) {
    if(pcb->k != 0) {
      --pcb->k;
    } else {
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih_credits: Out of local credits\n"));
#if RFCOMM_FLOW_QUEUEING
      if(q != NULL) {
	/* Packet can be queued? */
	if(pcb->buf != NULL) {
	  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih_credits: Buffer full. Dropped packet\n"));
	  return ERR_OK; /* Drop packet */
	} else {
	  /* Copy PBUF_REF referenced payloads into PBUF_RAM */
	  q = pbuf_take(q);
	  /* Remember pbuf to queue, if any */
	  pcb->buf = q;
	  /* Pbufs are queued, increase the reference count */
	  pbuf_ref(q);
	  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih_credits: Queued packet %p on channel %d\n", (void *)q, pcb->cn));
	}
      }
#else
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih_credits: Dropped packet\n"));
#endif /* RFCOMM_FLOW_QUEUEING */
      return ERR_OK;
    }
  }

  if(q != NULL) {
    tot_len = q->tot_len;
  }

  /* Size of information must be less than maximum frame size */
  if(tot_len > pcb->n) {
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih_credits: Size of information must be less than maximum frame size = %d Packet lenght = %d\n", pcb->n, q->tot_len));
    return ERR_MEM;
  }
  
  if(tot_len < 127) {
    if((p = pbuf_alloc(PBUF_RAW, RFCOMM_UIHCRED_LEN, PBUF_RAM)) == NULL) {
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih_credits: Could not allocate memory for pbuf\n"));