rfcomm.c

蓝牙协议源代码 bluetooth stack for lwip

  rfcommhdr.ctrl = ((u8_t *)p->payload)[1];

  /* Find PCB with matching server channel number and bluetooth address */
  pcb = rfcomm_get_active_pcb((rfcommhdr.addr >> 3), &l2cappcb->remote_bdaddr);

  if(pcb == NULL && rfcommhdr.ctrl != RFCOMM_SABM) {
    /* Channel does not exist */
    if(rfcommhdr.ctrl != RFCOMM_DM_PF && rfcommhdr.ctrl != RFCOMM_DM) {
      /* Send a DM response */
      LWIP_DEBUGF(RFCOMM_DEBUG,("Send a DM response to CN %d rfcomm.ctrl == 0x%x\n", (rfcommhdr.addr >> 3), rfcommhdr.ctrl));
      rfcomm_dm(l2cappcb, &rfcommhdr);
    } /* else silently discard packet */ 
    pbuf_free(p);
    return ERR_OK;
  }

  /* Check if length field is 1 or 2 bytes long and remove EA bit */
  if((((u8_t *)p->payload)[2] & 0x01) == 1) {
    hdrlen = RFCOMM_HDR_LEN_1;
    rfcommhdr.len = (((u8_t *)p->payload)[2] >> 1) & 0x007F;
  } else {
    hdrlen = RFCOMM_HDR_LEN_2;
    rfcommhdr.len = (((u16_t *)p->payload)[1] >> 1) & 0x7FFF;
  }

  if(rfcommhdr.ctrl == RFCOMM_UIH_PF) {
    if(pcb->cl == 0xF) {
      rfcommhdr.k = ((u8_t *)p->payload)[hdrlen++];
    }
  }

  /* Frame check */
  for(q = p; q != NULL; q = q->next) {
    len += q->len;
    if(len > (rfcommhdr.len + hdrlen)) {
      len -= q->len;
      len = rfcommhdr.len - len;
      len += hdrlen;
      break;
    }
  }

  fcs = ((u8_t *)q->payload)[len];
  if(rfcommhdr.ctrl == RFCOMM_UIH) {
    if(pcb->cn == 0) {
      if(fcs != pcb->uih0_in_fcs) { /* Check against the precalculated fcs */
	//if(fcs8_crc_check(p, RFCOMM_UIHCRC_CHECK_LEN, fcs) != 0) {
	/* Packet discarded due to failing frame check sequence */
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: UIH packet discarded due to failing frame check sequence\n"));
	pbuf_free(p);
	return ERR_OK;
      }
    }
  } else if(rfcommhdr.ctrl == RFCOMM_UIH) {
    if(fcs != pcb->uih_in_fcs) { /* Check against the precalculated fcs */
      //if(fcs8_crc_check(p, RFCOMM_UIHCRC_CHECK_LEN, fcs) != 0) {
      /* Packet discarded due to failing frame check sequence */
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: UIH packet discarded due to failing frame check sequence\n"));
      pbuf_free(p);
      return ERR_OK;
    }
  } else if(rfcommhdr.ctrl == RFCOMM_UIH_PF) {
    if(fcs != pcb->uihpf_in_fcs) { /* Check against the precalculated fcs */
      //if(fcs8_crc_check(p, RFCOMM_UIHCRC_CHECK_LEN, fcs) != 0) {
      /* Packet discarded due to failing frame check sequence */
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: UIH_PF packet discarded due to failing frame check sequence RFCS = 0x%x LFCS = 0x%x\n", 
			    fcs, pcb->uihpf_in_fcs));
      pbuf_free(p);
      return ERR_OK;
    }
  } else {
    if(fcs8_crc_check(p, hdrlen, fcs) != 0) {
      /* Packet discarded due to failing frame check sequence */
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Packet discarded due to failing frame check sequence\n"));
      pbuf_free(p);
      return ERR_OK;
    }
  }

  pbuf_header(p, -hdrlen); /* Adjust information pointer */
  pbuf_realloc(p, rfcommhdr.len); /* Remove fcs from packet */
  
  switch(rfcommhdr.ctrl) {
  case RFCOMM_SABM:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: RFCOMM_SABM\n"));
    if(pcb == NULL) {
      /* Check if the server channel exists */
      lpcb = NULL;
      if(rfcommhdr.addr >> 3 == 0) { /* Only the multiplexer channel can be connected without first 
					configuring it */
	for(lpcb = rfcomm_listen_pcbs; lpcb != NULL; lpcb = lpcb->next) {
	  if(lpcb->cn == (rfcommhdr.addr >> 3)) {
	    break;
	  }
	}
      }
      if(lpcb != NULL) {
	/* Found a listening pcb with a matching server channel number, now initiate a new active PCB 
	   with default configuration */
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Allocate RFCOMM PCB for CN %d******************************\n", lpcb->cn));
	if((pcb = rfcomm_new(l2cappcb)) == NULL) {
	  /* No memory to allocate PCB. Refuse connection attempt */
	  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: No memory to allocate PCB. Refuse connection attempt CN %d******************************\n", lpcb->cn));
	  rfcomm_dm(l2cappcb, &rfcommhdr);
	  pbuf_free(p);
	  return ERR_OK;
	}
	pcb->cn = lpcb->cn;
	pcb->callback_arg = lpcb->callback_arg;
	pcb->accept = lpcb->accept;
	  
	RFCOMM_REG(&rfcomm_active_pcbs, pcb);
      } else {
	/* Channel does not exist or multiplexer is not connected, refuse connection with DM frame */
	LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Channel does not exist, refuse connection with DM frame CN %d\n", (rfcommhdr.addr >> 3)));
	rfcomm_dm(l2cappcb, &rfcommhdr);
	pbuf_free(p);
	break;
      }
    }
    /* Set role to responder */
    pcb->rfcommcfg &= ~RFCOMM_CFG_IR;
    
    /* Send UA frame as response to SABM frame */
    rfcomm_ua(l2cappcb, &rfcommhdr);
    
    /* FCS precalculation for UIH frames */
    pbuf_header(p, hdrlen); /* Reuse the buffer for the current header */

    /* Change header values to refelct an UIH frame sent to the initiator */
    *((u8_t *)p->payload) &= 0xFB; /* Set direction bit to 0. We are the responder */
    *((u8_t *)p->payload) &= 0xFD; /* Set C/R bit to 0. We are the responder */
    ((u8_t *)p->payload)[1] = RFCOMM_UIH;
    pcb->uih_out_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);
    ((u8_t *)p->payload)[1] = RFCOMM_UIH_PF;
    pcb->uihpf_out_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);

    /* Change header values to refelct an UIH frame from to the initiator */
    //*((u8_t *)p->payload) |= 0x04; /* Set direction bit to 1. We are the responder */
    *((u8_t *)p->payload) &= 0xFB; /* Set direction bit to 0. We are the responder */
    *((u8_t *)p->payload) |= 0x02; /* Set C/R bit to 1. We are the responder */
    ((u8_t *)p->payload)[1] = RFCOMM_UIH;
    pcb->uih_in_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);
    ((u8_t *)p->payload)[1] = RFCOMM_UIH_PF;
    pcb->uihpf_in_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);

    /* UIH frame received on the control channel */
    *((u8_t *)p->payload) &= 0xFB; /* Set direction bit to 0 */
    ((u8_t *)p->payload)[1] = RFCOMM_UIH;
    pcb->uih0_in_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);

    /* Change header values to reflect an UIH frame sent on the control channel */
    *((u8_t *)p->payload) &= 0xF9; /* Set C/R bit and direction bit to 0 */
    ((u8_t *)p->payload)[1] = RFCOMM_UIH;
    pcb->uih0_out_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);
    
    pbuf_free(p);
    break;
  case RFCOMM_UA:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: RFCOMM_UA\n"));
    pcb->to = 0;
    if(pcb->state ==  W4_RFCOMM_SABM_RSP) {
      pcb->state = RFCOMM_CFG;
      /* FCS precalculation for UIH frames */
      pbuf_header(p, hdrlen); /* Reuse the buffer for the current header */
      /* Change header values to refelct an UIH frame sent to the responder */
      *((u8_t *)p->payload) &= 0xFB; /* Set direction bit to 0. We are the initiator */
      *((u8_t *)p->payload) |= 0x02; /* Set C/R bit to 1. We are the intitiator */
      ((u8_t *)p->payload)[1] = RFCOMM_UIH;
      pcb->uih_out_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);
      ((u8_t *)p->payload)[1]= RFCOMM_UIH_PF;
      pcb->uihpf_out_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);

      /* Change header values to reflect an UIH frame sent to the responder */
      *((u8_t *)p->payload) &= 0xFB; /* Set direction bit to 0. We are the intitiator */
      *((u8_t *)p->payload) &= 0xFD; /* Set C/R bit to 0. We are the initiator */
      ((u8_t *)p->payload)[1] = RFCOMM_UIH;
      pcb->uih_in_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);
      ((u8_t *)p->payload)[1] = RFCOMM_UIH_PF;
      pcb->uihpf_in_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);
      
      /* UIH frame sent on the control channel */
      *((u8_t *)p->payload) &= 0xFB; /* Set direction bit to 0 */
      *((u8_t *)p->payload) |= 0x02; /* Set C/R bit to 1. We are the intitiator */
      ((u8_t *)p->payload)[1] = RFCOMM_UIH;
      pcb->uih0_out_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);

      /* Change header values to reflect an UIH frame received on the control channel */
      *((u8_t *)p->payload) &= 0xF9; /* Set C/R bit and direction bit to 0 */
      ((u8_t *)p->payload)[1] = RFCOMM_UIH;
      pcb->uih0_in_fcs = fcs8_crc_calc(p, RFCOMM_UIHCRC_CHECK_LEN);

      if(pcb->cn == 0) {	
	for(tpcb = rfcomm_active_pcbs; tpcb != NULL; tpcb = tpcb->next) {
	  if(bd_addr_cmp(&(tpcb->l2cappcb->remote_bdaddr), &(pcb->l2cappcb->remote_bdaddr)) &&
	     tpcb->state == W4_RFCOMM_MULTIPLEXER) {
	    rfcomm_pn(tpcb, NULL); /* Send a parameter negotiation command to negotiate the 
						     connection settings for a channel that waits for the
						     multiplexer connection to be established */
	    break;
	  }
	}
      } else {
	rfcomm_msc(pcb, 0, NULL); /* Send a modem status command to set V.24 control signals for
				     the RFCOMM connection */
      }
    } else if (pcb->state == W4_RFCOMM_DISC_RSP) {
      //RFCOMM_RMV(&rfcomm_active_pcbs, pcb);
      pcb->state = RFCOMM_CLOSED;
      RFCOMM_EVENT_DISCONNECTED(pcb,ERR_OK,ret);
    } else {
      /* A response without an outstanding request is silently discarded */
    }
    pbuf_free(p);
    break;
  case RFCOMM_DM_PF:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: RFCOMM_DM_PF\n"));
  case RFCOMM_DM:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: RFCOMM_DM\n"));
    pcb->to = 0;
    //RFCOMM_RMV(&rfcomm_active_pcbs, pcb);
    if(pcb->state ==  W4_RFCOMM_SABM_RSP) {
      pcb->state = RFCOMM_CLOSED;
      RFCOMM_EVENT_CONNECTED(pcb,ERR_CONN,ret);
    } else {
      pcb->state = RFCOMM_CLOSED;
      RFCOMM_EVENT_DISCONNECTED(pcb,ERR_CONN,ret);
    }
    pbuf_free(p);
    break;
  case RFCOMM_DISC:
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: RFCOMM_DISC\n"));
    //RFCOMM_RMV(&rfcomm_active_pcbs, pcb);
    /* Send UA frame as response to DISC frame */
    ret = rfcomm_ua(l2cappcb, &rfcommhdr);
    pcb->state = RFCOMM_CLOSED;
    RFCOMM_EVENT_DISCONNECTED(pcb,ERR_OK,ret);
    pbuf_free(p);
    break;
  case RFCOMM_UIH_PF: 
    if((rfcommhdr.addr >> 3) == 0) {
      /* Process multiplexer command/response */
      rfcomm_process_msg(pcb, &rfcommhdr, l2cappcb, p);
      pbuf_free(p);
    } else if(pcb->cl == 0xF) {
      /* Process credit based frame */
      if(pcb->rk != 0) {
      --pcb->rk; /* Decrease remote credits */
      }
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Received local credits: %d Existing local credits: %d\n", rfcommhdr.k, pcb->k));
      if((pcb->k + rfcommhdr.k) < 255) {
	pcb->k += rfcommhdr.k; /* Increase local credits */
#if RFCOMM_FLOW_QUEUEING
	q = pcb->buf;
        /* Queued packet present? */
        if (q != NULL) {
          /* NULL attached buffer immediately */
          pcb->buf = NULL;
          LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: sending queued packet.\n"));
          /* Send the queued packet */
          rfcomm_uih(pcb, pcb->cn, q); 
          /* Free the queued packet */
          pbuf_free(q);
        }
#endif /* RFCOMM_FLOW_QUEUEING */	
      } else {
	pcb->k = 255;
      }
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Forward RFCOMM_UIH_PF credit packet to higher layer\n"));
      RFCOMM_EVENT_RECV(pcb,ERR_OK,p,ret); /* Process information. Application must free pbuf */
    } else {
      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Forward RFCOMM_UIH_PF non credit packet to higher layer\n"));
      RFCOMM_EVENT_RECV(pcb,ERR_OK,p,ret); /* Process information. Application must free pbuf */
    }
    break;
  case RFCOMM_UIH:
    if((rfcommhdr.addr >> 3) == 0) {
      /* Process multiplexer command/response */
      rfcomm_process_msg(pcb, &rfcommhdr, l2cappcb, p);
      pbuf_free(p);
    } else {
      if(pcb->rk != 0) {
	--pcb->rk; /* Decrease remote credits */
      }

      LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Forward RFCOMM_UIH packet to higher layer\n"));
      RFCOMM_EVENT_RECV(pcb,ERR_OK,p,ret); /* Process information. Application must free pbuf */
    }
    break;
  default:
    /* Unknown or illegal frame type. Throw it away! */
    pbuf_free(p);
    break;
  }
  return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_arg():
 *
 * Used to specify the argument that should be passed callback functions.
 */
/*-----------------------------------------------------------------------------------*/
void
rfcomm_arg(struct rfcomm_pcb *pcb, void *arg)
{
  pcb->callback_arg = arg;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_recv():
 * 
 * Used to specify the function that should be called when a RFCOMM connection 
 * receives data.
 */
/*-----------------------------------------------------------------------------------*/
void
rfcomm_recv(struct rfcomm_pcb *pcb, 
	    err_t (* recv)(void *arg, struct rfcomm_pcb *pcb, struct pbuf *p, err_t err))
{
  pcb->recv = recv;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_disc():
 * 
 * Used to specify the function that should be called when a RFCOMM channel is 
 * disconnected
 */
/*-----------------------------------------------------------------------------------*/
void
rfcomm_disc(struct rfcomm_pcb *pcb, 
	    err_t (* disc)(void *arg, struct rfcomm_pcb *pcb, err_t err))
{
  pcb->disconnected = disc;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * rfcomm_listen():
 * 
 * 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 been connected.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
rfcomm_listen(struct rfcomm_pcb *npcb, u8_t cn, 
	      err_t (* accept)(void *arg, struct rfcomm_pcb *pcb, err_t err))
{
  struct rfcomm_pcb_listen *lpcb;

  if((lpcb = lwbt_memp_malloc(MEMP_RFCOMM_PCB_LISTEN)) == NULL) {
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_listen: Could not allocate memory for pcb\n"));
    return ERR_MEM;
  }
  lpcb->cn = cn;
  lpcb->callback_arg = npcb->callback_arg;
  lpcb->accept = accept;
  lpcb->state = RFCOMM_LISTEN;

  lwbt_memp_free(MEMP_RFCOMM_PCB, npcb);
  RFCOMM_REG((struct rfcomm_pcb **)&rfcomm_listen_pcbs, (struct rfcomm_pcb *)lpcb);
  return ERR_OK;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/