rfcomm.c

蓝牙协议源代码 bluetooth stack for lwip

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

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

  ((u8_t *)r->payload)[0] = pcb->uihpf_out_fcs;
  pbuf_chain(p, r);
  pbuf_free(r);

  /* Increase remote credits */
  pcb->rk += credits;

  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_uih_credits: p->tot_len = %d pcb->k = %d pcb->rk = %d\n", p->tot_len, pcb->k, pcb->rk));

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

  /* Free RFCOMM header. Higher layers will handle rest of packet */
  if(q != NULL) {
    pbuf_dechain(p);
    pbuf_realloc(q, q->tot_len-1); /* Remove FCS from packet */
  }
  pbuf_free(p);

  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_process_msg():
 *
 * Parses the received RFCOMM message and handles it.
 */
/*-----------------------------------------------------------------------------------*/
void
rfcomm_process_msg(struct rfcomm_pcb *pcb, struct rfcomm_hdr *rfcommhdr, struct l2cap_pcb *l2cappcb,
		   struct pbuf *p)
{
  struct rfcomm_msg_hdr *cmdhdr, *rsphdr;
  struct rfcomm_pn_msg *pnreq;
  struct rfcomm_msc_msg *mscreq;
  struct rfcomm_rpn_msg *rpnreq;
  struct rfcomm_pcb *tpcb; /* Temp pcb */
  struct rfcomm_pcb_listen *lpcb; /* Listen pcb */
  struct pbuf *q;
  err_t ret;

  cmdhdr = p->payload;
  pbuf_header(p, -RFCOMM_MSGHDR_LEN);

  switch(cmdhdr->type) {
  case RFCOMM_PN_CMD:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM PN command\n"));
    pnreq = p->payload;

    /* Check if the DLC is already established */
    tpcb = rfcomm_get_active_pcb((pnreq->dlci >> 1), &pcb->l2cappcb->remote_bdaddr);
    
    if(tpcb == NULL) {
      /* Check if the server channel exists */
      for(lpcb = rfcomm_listen_pcbs; lpcb != NULL; lpcb = lpcb->next) {
	if(lpcb->cn == (pnreq->dlci >> 1)) {
	  break;
	}
      }
      if(lpcb != NULL) {
	/* Found a listening pcb with a matching server channel number, now initiate a new PCB 
	   with default configuration */
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: Allocate RFCOMM PCB for CN %d******************************\n", lpcb->cn));
	if((tpcb = rfcomm_new(pcb->l2cappcb)) == NULL) {
	  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: could not allocate PCB\n"));
	  return;
	}
	tpcb->cn = lpcb->cn;
	tpcb->callback_arg = lpcb->callback_arg;
	tpcb->accept = lpcb->accept;
	tpcb->state = RFCOMM_CFG;

	RFCOMM_REG(&rfcomm_active_pcbs, tpcb);
      } else {
	/* Channel does not exist, refuse connection with DM frame */
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: Channel does not exist, refuse connection with DM frame CN %d\n", (pnreq->dlci >> 1)));
	rfcomm_dm(pcb->l2cappcb, rfcommhdr);
	break;
      }
    }
    /* Get suggested parameters */
    tpcb->cl = pnreq->i_cl >> 4;
    tpcb->p = pnreq->p;
    if(tpcb->n > pnreq->n) {
      tpcb->n = pnreq->n;
    }
    tpcb->k = pnreq->k;
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM_PN_CMD. tpcb->k = %d\n", tpcb->k));
  
    /* Send PN response */
    cmdhdr->type = cmdhdr->type & 0xFD; /* Set C/R to response */
    if(tpcb->cl == 0xF) {
      pnreq->i_cl = 0 | (0xE << 4); /* Credit based flow control */
    } else {
      pnreq->i_cl = 0; /* Remote device conforms to bluetooth version 1.0B. No flow control */
    }
    pnreq->p = tpcb->p;
    pnreq->n = tpcb->n;
    pnreq->k = tpcb->k;
    pbuf_header(p, RFCOMM_MSGHDR_LEN);

    rfcomm_uih(pcb, 0, p);
    break;
  case RFCOMM_PN_RSP:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM PN response\n"));
    pcb->to = 0; /* Reset response timer */
    pnreq = p->payload;
    /* Find PCB with matching server channel number and bluetooth address */
    tpcb = rfcomm_get_active_pcb((pnreq->dlci >> 1), &pcb->l2cappcb->remote_bdaddr);
      
    if(tpcb != NULL) {
      /* Use negotiated settings that may have changed from the default ones */
      if((pnreq->i_cl >> 4) == 0xE) {
	tpcb->cl = 0xF; /* Credit based flow control */
	tpcb->k = pnreq->k; /* Inital credit value */
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM_PN_RSP. tpcb->k = %d\n", tpcb->k));
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: Credit based flow control is used for outgoing packets 0x%x %d %d\n", (pnreq->i_cl >> 4), pnreq->k, pnreq->n));
      } else {
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: No flow control used for outgoing packets 0x%x\n", (pnreq->i_cl >> 4)));
	tpcb->cl = 0; /* Remote device conform to bluetooth version 1.0B. No flow control */
      }
      tpcb->n = pnreq->n; /* Maximum frame size */
      
      if(tpcb->state == W4_RFCOMM_MULTIPLEXER) {
	rfcomm_connect(tpcb, tpcb->cn, tpcb->connected); /* Create a connection for a channel that 
							    waits for the multiplexer connection to
							    be established */
      }

      pcb->state = RFCOMM_OPEN;
      RFCOMM_EVENT_PN_RSP(tpcb,ERR_OK,ret); 
    } /* else silently discard */
    break; 
  case RFCOMM_TEST_CMD:
    /* Send TEST response */
    cmdhdr->type = cmdhdr->type & 0xBF; /* Set C/R to response */
    pbuf_header(p, RFCOMM_MSGHDR_LEN);
 
    rfcomm_uih(pcb, 0, p);
    break;
  case RFCOMM_TEST_RSP:
    pcb->to = 0; /* Reset response timer */
    RFCOMM_EVENT_TEST(pcb,ERR_OK,ret);
    break;
  case RFCOMM_FCON_CMD:
    /* Enable transmission of data on all channels in session except cn 0 */
    for(tpcb = rfcomm_active_pcbs; tpcb != NULL; tpcb = tpcb->next) {
      if(bd_addr_cmp(&(tpcb->l2cappcb->remote_bdaddr), &(l2cappcb->remote_bdaddr)) &&
	 tpcb->cn != 0) {
	tpcb->rfcommcfg |= RFCOMM_CFG_FC;
      }
    }
    /* Send FC_ON response */
    cmdhdr->type = cmdhdr->type & 0xBF; /* Set C/R to response */
    pbuf_header(p, RFCOMM_MSGHDR_LEN);
 
    rfcomm_uih(pcb, 0, p);
    break;
  case RFCOMM_FCON_RSP:
    break;
  case RFCOMM_FCOFF_CMD:
    /* Disable transmission of data on all channels in session except cn 0 */
    for(tpcb = rfcomm_active_pcbs; tpcb != NULL; tpcb = tpcb->next) {
      if(bd_addr_cmp(&(tpcb->l2cappcb->remote_bdaddr), &(l2cappcb->remote_bdaddr)) &&
	 tpcb->cn != 0) {
	tpcb->rfcommcfg &= ~RFCOMM_CFG_FC;
      }
    }
    /* Send FC_OFF response */
    cmdhdr->type = cmdhdr->type & 0xBF; /* Set C/R to response */
    pbuf_header(p, RFCOMM_MSGHDR_LEN);
 
    rfcomm_uih(pcb, 0, p);
    break;
  case RFCOMM_FCOFF_RSP:
    break;
  case RFCOMM_MSC_CMD:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM_MSC_CMD\n"));
    mscreq = p->payload;    
    /* Find DLC */
    tpcb = rfcomm_get_active_pcb((mscreq->dlci >> 3), &pcb->l2cappcb->remote_bdaddr);

    if(tpcb != NULL) {
      /* Set flow control bit. Ignore remaining fields in the MSC since this is a type 1 
	 device */
      if((mscreq->rs232 >> 1) & 0x01) {
	tpcb->rfcommcfg |= RFCOMM_CFG_FC;
      } else {
	tpcb->rfcommcfg &= ~RFCOMM_CFG_FC;
      }

      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcommm_process_msg: fc bit = %d\n", (mscreq->rs232 >> 1) & 0x01));

      /* Send MSC response */
      cmdhdr->type = cmdhdr->type & 0xFD; /* Set C/R to response */
      pbuf_header(p, RFCOMM_MSGHDR_LEN);
      
      if(!(tpcb->rfcommcfg & RFCOMM_CFG_IR) && !(tpcb->rfcommcfg & RFCOMM_CFG_MSC_IN)) { /* We are the responder and should send a MSC command before responding to one */
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcommm_process_msg: We are the responder and should send a MSC command before responding to one\n"));
	rfcomm_msc(tpcb, 0, NULL);
      }

      rfcomm_uih(pcb, 0, p);

      tpcb->rfcommcfg |= RFCOMM_CFG_MSC_OUT;
     
      if(tpcb->rfcommcfg & RFCOMM_CFG_MSC_IN && tpcb->state != RFCOMM_OPEN) {
	tpcb->state = RFCOMM_OPEN;
	if(tpcb->rfcommcfg & RFCOMM_CFG_IR) {
	  RFCOMM_EVENT_CONNECTED(tpcb,ERR_OK,ret);
	} else {
	  RFCOMM_EVENT_ACCEPT(tpcb,ERR_OK,ret);
	}
      }
    } /* else silently discard */
    break;
  case RFCOMM_MSC_RSP:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM_MSC_RSP\n"));
    /* Information received in response is only a copy of the signals that where sent in the command */
    pcb->to = 0; /* Reset response timer */
    
    mscreq = p->payload;
    
    /* Find PCB with matching server channel number and Bluetooth address */
    tpcb = rfcomm_get_active_pcb((mscreq->dlci >> 3), &pcb->l2cappcb->remote_bdaddr);
 
    if(tpcb != NULL) {
      
      if(tpcb->rfcommcfg & RFCOMM_CFG_MSC_IN) {
	RFCOMM_EVENT_MSC(tpcb,ERR_OK,ret); /* We have sent a MSC after initial configuration of 
					      the connection was done */
      } else {
	tpcb->rfcommcfg |= RFCOMM_CFG_MSC_IN;
	if(tpcb->rfcommcfg & RFCOMM_CFG_MSC_OUT) {
	  tpcb->state = RFCOMM_OPEN;
	  if(tpcb->rfcommcfg & RFCOMM_CFG_IR) {
	    RFCOMM_EVENT_CONNECTED(tpcb,ERR_OK,ret);
	  } else {
	    RFCOMM_EVENT_ACCEPT(tpcb,ERR_OK,ret);
	  }
	}
      }
    } /* else silently discard */
    break;
  case RFCOMM_RPN_CMD:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM_RPN_CMD\n"));
    /* Send RPN response */
    if(cmdhdr->len == 8) {
      /* RPN command was a request to set up the link's parameters */
      /* All parameters accepted since this is a type 1 device */
      cmdhdr->type = cmdhdr->type & 0xBF; /* Set C/R to response */
      pbuf_header(p, RFCOMM_MSGHDR_LEN);
      //rfcomm_uih(pcb->l2cappcb, rfcommhdr, p);
      rfcomm_uih(pcb, 0, p);
    } else if(cmdhdr->len == 1) {
      /* RPN command was a request for the link's parameters */
      q = pbuf_alloc(PBUF_RAW, RFCOMM_RPNMSG_LEN+RFCOMM_MSGHDR_LEN, PBUF_RAM);
      rsphdr = q->payload;
      rsphdr->type = cmdhdr->type & 0xBF; /* Set C/R to response */
      rsphdr->len = RFCOMM_RPNMSG_LEN;
      pbuf_header(q, -RFCOMM_MSGHDR_LEN);
      rpnreq = q->payload;
      rpnreq->dlci = ((u8_t *)p->payload)[0]; /* Copy DLCI from command to response */
      rpnreq->br = RFCOMM_COM_BR; /* Default baud rate */
      rpnreq->cfg = RFCOMM_COM_CFG; /* Default data bits, stop bits, parity and parity type */
      rpnreq->fc = RFCOMM_COM_FC; /* Default flow control */
      rpnreq->xon = RFCOMM_COM_XON; /* Default */
      rpnreq->xoff = RFCOMM_COM_XOFF; /* Default */
      rpnreq->mask = 0xFFFF; /* All parameters are valid */
      pbuf_header(q, RFCOMM_MSGHDR_LEN);
      
      rfcomm_uih(pcb, 0, q);
      pbuf_free(q);
    } else {
      //SHOULD NOT HAPPEN. LENGTH SHOULD ALWAYS BE 1 OR 8
    }
    break;
  case RFCOMM_RPN_RSP:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM_RPN_CMD\n"));
    pcb->to = 0; /* Reset response timer */
    rpnreq = p->payload;
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_msc: rpn response received 0x%x\n", rpnreq->br));

    /* Find PCB with matching server channel number and bluetooth address */
    tpcb = rfcomm_get_active_pcb((rpnreq->dlci >> 3), &pcb->l2cappcb->remote_bdaddr);
    
    if(tpcb != NULL) {
      RFCOMM_EVENT_RPN(tpcb,ERR_OK,ret);
    }
    break;
  case RFCOMM_RLS_CMD:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_process_msg: RFCOMM_RLS_CMD\n"));
    /* Send RLS response */
    cmdhdr->type = cmdhdr->type & 0xBF; /* Set C/R to response */
    pbuf_header(p, RFCOMM_MSGHDR_LEN);
  
    rfcomm_uih(pcb, 0, p);
    break;
  case RFCOMM_RLS_RSP:
    break;
  case RFCOMM_NSC_RSP:
    break;
  default:
    /* Send NSC response */
    q = pbuf_alloc(PBUF_RAW, RFCOMM_MSGHDR_LEN, PBUF_RAM);
    rsphdr = q->payload; 
    rsphdr->type = ((cmdhdr->type & 0x03) << 0) | (RFCOMM_NSC_RSP << 2);
    rsphdr->len = 0;
 
    rfcomm_uih(pcb, 0, q);
    pbuf_free(q);
    break;
  }
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_input():
 *
 * Called by the lower layer. Does a frame check, parses the header and forward it to 
 * the upper layer or handle the command frame.
 */
/*-----------------------------------------------------------------------------------*/
err_t
rfcomm_input(void *arg, struct l2cap_pcb *l2cappcb, struct pbuf *p, err_t err)
{
  struct rfcomm_hdr rfcommhdr;
  struct rfcomm_pcb *pcb, *tpcb;
  struct rfcomm_pcb_listen *lpcb;

  s16_t len = 0;
  u8_t hdrlen;
  u8_t fcs;
  
  struct pbuf *q;
  err_t ret;

  rfcommhdr.addr = *((u8_t *)p->payload);