hci.c

蓝牙协议源代码 bluetooth stack for lwip

  case HCI_UNIT_KEY_USED:
    return("Unit Key Used");
  case HCI_QOS_NOT_SUPPORTED:
    return("QoS is Not Supported");
  case HCI_INSTANT_PASSED:
    return("Instant Passed");
  case HCI_PAIRING_UNIT_KEY_NOT_SUPPORTED:
    return("Pairing with Unit Key Not Supported");
  default:
    return("Error code unknown");
  }
}
#else
u8_t *
hci_get_error_code(u8_t code) 
{
  return 0;
}
#endif /* HCI_EV_DEBUG */
/*-----------------------------------------------------------------------------------*/
/* hci_event_input():
 *
 * Called by the physical bus interface. Parses the received event packet to determine 
 * which event occurred and handles it.
 */
 /*-----------------------------------------------------------------------------------*/
void
hci_event_input(struct pbuf *p)
{
  struct hci_inq_res *inqres;
  struct hci_event_hdr *evhdr;
  struct hci_link *link;
  u8_t i, j;
  struct bd_addr *bdaddr;
  u8_t resp_offset;
  err_t ret;
  u8_t ocf, ogf;

  pbuf_header(p, HCI_EVENT_HDR_LEN);
  evhdr = p->payload;
  pbuf_header(p, -HCI_EVENT_HDR_LEN);

  switch(evhdr->code) {
  case HCI_INQUIRY_COMPLETE:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Inquiry complete, 0x%x %s\n", ((u8_t *)p->payload)[0], hci_get_error_code(((u8_t *)p->payload)[0])));
    HCI_EVENT_INQ_COMPLETE(pcb,((u8_t *)p->payload)[0],ret);
    break;
  case HCI_INQUIRY_RESULT:
    for(i=0;i<((u8_t *)p->payload)[0];i++) {
      resp_offset = i*14;
      LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Inquiry result %d\nBD_ADDR: 0x", i));
      for(i = 0; i < BD_ADDR_LEN; i++) {
	LWIP_DEBUGF(HCI_EV_DEBUG, ("%x",((u8_t *)p->payload)[1+resp_offset+i]));
      }
      LWIP_DEBUGF(HCI_EV_DEBUG, ("\n"));
      LWIP_DEBUGF(HCI_EV_DEBUG, ("Page_Scan_Rep_Mode: 0x%x\n",((u8_t *)p->payload)[7+resp_offset]));
      LWIP_DEBUGF(HCI_EV_DEBUG, ("Page_Scan_Per_Mode: 0x%x\n",((u8_t *)p->payload)[8+resp_offset]));
      LWIP_DEBUGF(HCI_EV_DEBUG, ("Page_Scan_Mode: 0x%x\n",((u8_t *)p->payload)[9+resp_offset]));
      LWIP_DEBUGF(HCI_EV_DEBUG, ("Class_of_Dev: 0x%x 0x%x 0x%x\n",((u8_t *)p->payload)[10+resp_offset],
	     ((u8_t *)p->payload)[11+resp_offset], ((u8_t *)p->payload)[12+resp_offset]));
      LWIP_DEBUGF(HCI_EV_DEBUG, ("Clock_Offset: 0x%x%x\n",((u8_t *)p->payload)[13+resp_offset],
	     ((u8_t *)p->payload)[14+resp_offset]));
      
      bdaddr = (void *)(((u8_t *)p->payload)+(1+resp_offset));
      if((inqres = lwbt_memp_malloc(MEMP_HCI_INQ)) != NULL) {
	bd_addr_set(&(inqres->bdaddr), bdaddr);
	inqres->psrm = ((u8_t *)p->payload)[7+resp_offset];
	inqres->psm = ((u8_t *)p->payload)[9+resp_offset];
	memcpy(inqres->cod, ((u8_t *)p->payload)+10+resp_offset, 3);
	inqres->co = *((u16_t *)(((u8_t *)p->payload)+13+resp_offset));
	HCI_REG(&(pcb->ires), inqres);
      } else {
        LWIP_DEBUGF(HCI_DEBUG, ("hci_event_input: Could not allocate memory for inquiry result\n"));
      }
    }
    break;
  case HCI_CONNECTION_COMPLETE:
    bdaddr = (void *)(((u8_t *)p->payload)+3); /* Get the Bluetooth address */
    link = hci_get_link(bdaddr);
    switch(((u8_t *)p->payload)[0]) {
    case HCI_SUCCESS:
      LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Conn successfully completed\n"));
      if(link == NULL) {
	if((link = hci_new()) == NULL) {
	  /* Could not allocate memory for link. Disconnect */
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Could not allocate memory for link. Disconnect\n"));
	  hci_disconnect(bdaddr, HCI_OTHER_END_TERMINATED_CONN_LOW_RESOURCES);
	  /* Notify L2CAP */
	  lp_disconnect_ind(bdaddr);
	  break;
	}
	bd_addr_set(&(link->bdaddr), bdaddr);
	link->conhdl = *((u16_t *)(((u8_t *)p->payload)+1)); 
	HCI_REG(&(hci_active_links), link);
	HCI_EVENT_CONN_COMPLETE(pcb,bdaddr,ret); /* Allow applicaton to do optional configuration of link */
	LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Calling lp_connect_ind\n"));
	lp_connect_ind(&(link->bdaddr)); /* Notify L2CAP */
      } else {
	link->conhdl = *((u16_t *)(((u8_t *)p->payload)+1));
	HCI_EVENT_CONN_COMPLETE(pcb,bdaddr,ret); /* Allow applicaton to do optional configuration of link */
	LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Calling lp_connect_cfm\n"));
	lp_connect_cfm(bdaddr, ((u8_t *)p->payload)[10], ERR_OK); /* Notify L2CAP */
      }
      //TODO: MASTER SLAVE SWITCH??
      break;
    default:
      LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Conn failed to complete, 0x%x %s\n"
			     , ((u8_t *)p->payload)[0], hci_get_error_code(((u8_t *)p->payload)[0])));
      if(link != NULL) {
        LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Link exists. Notify upper layer\n"));
	hci_close(link);
	lp_connect_cfm(bdaddr, ((u8_t *)p->payload)[10], ERR_CONN);
      } else {
        /* silently discard */
        LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Silently discard. Link does not exist\n"));
      }
      break;
    } /* switch */
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Conn_hdl: 0x%x 0x%x\n", ((u8_t *)p->payload)[1], ((u8_t *)p->payload)[2]));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("BD_ADDR: 0x"));
    for(j=0;j<BD_ADDR_LEN;j++) {
	LWIP_DEBUGF(HCI_EV_DEBUG, ("%x",((u8_t *)p->payload)[3+j]));
    }
    LWIP_DEBUGF(HCI_EV_DEBUG, ("\n"));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Link_type: 0x%x\n",((u8_t *)p->payload)[9]));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Encryption_Mode: 0x%x\n",((u8_t *)p->payload)[10]));
    break;
  case HCI_DISCONNECTION_COMPLETE:
    switch(((u8_t *)p->payload)[0]) {
    case HCI_SUCCESS:
      LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Disconn has occurred\n"));
      for(link = hci_active_links; link != NULL; link = link->next) {
	if(link->conhdl == *((u16_t *)(((u8_t *)p->payload)+1))) {
	  break; /* Found */
	}
      }
      if(link != NULL) {
	lp_disconnect_ind(&(link->bdaddr)); /* Notify upper layer */
	hci_close(link);
      }
      /* else silently discard */
      break;
    default:
      LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Disconn failed to complete, 0x%x %s\n"
			    , ((u8_t *)p->payload)[0], hci_get_error_code(((u8_t *)p->payload)[0])));
      return;
    }
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Conn_hdl: 0x%x%x\n", ((u8_t *)p->payload)[1], ((u8_t *)p->payload)[2]));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Reason: 0x%x %s\n", ((u8_t *)p->payload)[3], hci_get_error_code(((u8_t *)p->payload)[3])));
    break;
  case HCI_ENCRYPTION_CHANGE:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Encryption changed. Status = 0x%x, Encryption enable = 0x%x\n", ((u8_t *)p->payload)[0], ((u8_t *)p->payload)[3]));
    break;
  case HCI_QOS_SETUP_COMPLETE:
     LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: QOS setup complete result = 0x%x\n", ((u8_t *)p->payload)[0]));
    break;
  case HCI_COMMAND_COMPLETE:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Command Complete\n"));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Num_HCI_Command_Packets: 0x%x\n", ((u8_t *)p->payload)[0]));
 
    pcb->numcmd += ((u8_t *)p->payload)[0]; /* Add number of completed command packets to the
					       number of command packets that the BT module 
					       can buffer */
    pbuf_header(p, -1); /* Adjust payload pointer not to cover
			 Num_HCI_Command_Packets parameter */
    ocf = *((u16_t *)p->payload) & 0x03FF;
    ogf = *((u16_t *)p->payload) >> 10;
    
    LWIP_DEBUGF(HCI_EV_DEBUG, ("OCF == 0x%x OGF == 0x%x\n", ocf, ogf));
    
    pbuf_header(p, -2); /* Adjust payload pointer not to cover Command_Opcode
			   parameter */
    if(ogf == HCI_INFO_PARAM) {
      if(ocf == HCI_READ_BUFFER_SIZE) {
	if(((u8_t *)p->payload)[0] == HCI_SUCCESS) {
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Read_Buffer_Size command succeeded\n"));
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("HC_ACL_Data_Packet_Length: 0x%x%x\n", ((u8_t *)p->payload)[1], ((u8_t *)p->payload)[2]));
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("HC_SCO_Data_Packet_Length: 0x%x\n", ((u8_t *)p->payload)[3]));
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("HC_Total_Num_ACL_Data_Packets: %d\n", *((u16_t *)(((u8_t *)p->payload)+4))));
	  pcb->maxsize = *((u16_t *)(((u8_t *)p->payload)+1)); /* Maximum size of an ACL packet 
								  that the BT module is able to 
								  accept */
	  pcb->hc_num_acl = *((u16_t *)(((u8_t *)p->payload)+4)); /* Number of ACL packets that the 
								     BT module can buffer */
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("HC_Total_Num_SCO_Data_Packets: 0x%x%x\n", ((u8_t *)p->payload)[6], ((u8_t *)p->payload)[7]));
	} else {
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Read_Buffer_Size command failed, 0x%x %s\n", ((u8_t *)p->payload)[0], hci_get_error_code(((u8_t *)p->payload)[0])));
	  return;
	}
      }
      if(ocf == HCI_READ_BD_ADDR) {
	if(((u8_t *)p->payload)[0] == HCI_SUCCESS) {
	  bdaddr = (void *)(((u8_t *)p->payload) + 1); /* Get the Bluetooth address */
	  HCI_EVENT_RBD_COMPLETE(pcb, bdaddr, ret); /* Notify application.*/
	}
      }
    }
    if(ogf == HCI_HOST_C_N_BB && ocf == HCI_SET_HC_TO_H_FC) {
      if(((u8_t *)p->payload)[0] == HCI_SUCCESS) {
	pcb->flow = 1;
      }
    }
    if(ogf == HCI_LINK_POLICY) {
      if(ocf == HCI_W_LINK_POLICY) {
	if(((u8_t *)p->payload)[0] == HCI_SUCCESS) {
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Successful HCI_W_LINK_POLICY.\n"));
	  for(link = hci_active_links; link != NULL; link = link->next) {
	    if(link->conhdl == *((u16_t *)(((u8_t *)p->payload)+1))) {
	      break;
	    }
	  }
	  if(link == NULL) {
	    LWIP_DEBUGF(HCI_DEBUG, ("hci_event_input: Connection does not exist\n"));
	    return; /* Connection does not exist */ 
	  }
	  HCI_EVENT_WLP_COMPLETE(pcb, &link->bdaddr, ret); /* Notify application.*/
	} else {
	  LWIP_DEBUGF(HCI_EV_DEBUG, ("Unsuccessful HCI_W_LINK_POLICY.\n"));
	  return;
	}
      }
    }
    
    HCI_EVENT_CMD_COMPLETE(pcb,ogf,ocf,((u8_t *)p->payload)[0],ret);
    break;
  case HCI_COMMAND_STATUS:
    switch(((u8_t *)p->payload)[0]) {
    case HCI_SUCCESS:
      LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Command Status\n"));
      break;
    default:
      LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Command failed, %s\n", hci_get_error_code(((u8_t *)p->payload)[0])));
      pbuf_header(p, -2); /* Adjust payload pointer not to cover
			     Num_HCI_Command_Packets and status parameter */
      ocf = *((u16_t *)p->payload) & 0x03FF;
      ogf = *((u16_t *)p->payload) >> 10;
      pbuf_header(p, -2); /* Adjust payload pointer not to cover Command_Opcode
			   parameter */
      HCI_EVENT_CMD_COMPLETE(pcb,ogf,ocf,((u8_t *)p->payload)[0],ret);
      pbuf_header(p, 4);
      break;
    }
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Num_HCI_Command_Packets: 0x%x\n", ((u8_t *)p->payload)[1]));
    pcb->numcmd += ((u8_t *)p->payload)[1]; /* Add number of completed command packets to the 
					       number of command packets that the BT module 
					       can buffer */
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Command_Opcode: 0x%x 0x%x\n", ((u8_t *)p->payload)[2], ((u8_t *)p->payload)[3]));
    break;
  case HCI_HARDWARE_ERROR:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Hardware Error\n"));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Hardware_code: 0x%x\n\n", ((u8_t *)p->payload)[0]));
    //TODO: IS THIS FATAL??
    break; 
  case HCI_ROLE_CHANGE:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Role change\n"));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Status: 0x%x\n", ((u8_t *)p->payload)[0]));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("New Role: 0x%x\n", ((u8_t *)p->payload)[7]));
    break;
  case HCI_NBR_OF_COMPLETED_PACKETS:
    LWIP_DEBUGF(DBG_OFF, ("hci_event_input: Number Of Completed Packets\n"));
    LWIP_DEBUGF(DBG_OFF, ("Number_of_Handles: 0x%x\n", ((u8_t *)p->payload)[0]));
    for(i=0;i<((u8_t *)p->payload)[0];i++) {
      resp_offset = i*4;
      LWIP_DEBUGF(DBG_OFF, ("Conn_hdl: 0x%x%x\n", ((u8_t *)p->payload)[1+resp_offset], ((u8_t *)p->payload)[2+resp_offset]));
      LWIP_DEBUGF(DBG_OFF, ("HC_Num_Of_Completed_Packets: 0x%x\n",*((u16_t *)(((u8_t *)p->payload)+3+resp_offset))));
      /* Add number of completed ACL packets to the number of ACL packets that the 
	 BT module can buffer */
      pcb->hc_num_acl += *((u16_t *)(((u8_t *)p->payload) + 3 + resp_offset));
#if HCI_FLOW_QUEUEING
      {
	u16_t conhdl = *((u16_t *)(((u8_t *)p->payload) + 1 + resp_offset));
	struct pbuf *q;
	for(link = hci_active_links; link != NULL; link = link->next) {
	  if(link->conhdl == conhdl) {
	    break;
	  }
	}
	q = link->p;
	/* Queued packet present? */
	if (q != NULL) {
	  /* NULL attached buffer immediately */
	  link->p = NULL;
	  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_input: Sending queued packet.\n"));
	  /* Send the queued packet */
	lp_acl_write(&link->bdaddr, q, link->len, link->pb); 
	/* Free the queued packet */
	pbuf_free(q);
	}
      }
#endif /* RFCOMM_FLOW_QUEUEING */	
    }
    break;
  case HCI_MODE_CHANGE:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Mode change\n"));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Status: 0x%x\n", ((u8_t *)p->payload)[0]));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Conn_hdl: 0x%x\n", ((u16_t *)(((u8_t *)p->payload) + 1))[0]));
    break;
  case HCI_DATA_BUFFER_OVERFLOW:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Data Buffer Overflow\n"));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Link_Type: 0x%x\n", ((u8_t *)p->payload)[0]));
    //TODO: IS THIS FATAL????
    break;
  case HCI_MAX_SLOTS_CHANGE:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Max slots changed\n"));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("Conn_hdl: 0x%x\n", ((u16_t *)p->payload)[0]));
    LWIP_DEBUGF(HCI_EV_DEBUG, ("LMP max slots: 0x%x\n", ((u8_t *)p->payload)[2]));
    break; 
  case HCI_PIN_CODE_REQUEST:
    bdaddr = (void *)((u8_t *)p->payload); /* Get the Bluetooth address */
    HCI_EVENT_PIN_REQ(pcb, bdaddr, ret); /* Notify application. If event is not registered, 
					    send a negative reply */
    break;
  case HCI_LINK_KEY_NOTIFICATION:
    bdaddr = (void *)((u8_t *)p->payload); /* Get the Bluetooth address */
    
    HCI_EVENT_LINK_KEY_NOT(pcb, bdaddr, ((u8_t *)p->payload) + 6, ret); /* Notify application.*/
    break;
  default:
    LWIP_DEBUGF(HCI_EV_DEBUG, ("hci_event_input: Undefined event code 0x%x\n", evhdr->code));
    break;
  }/* switch */
}
/*-----------------------------------------------------------------------------------*/
/* HCI Commands */ 
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* hci_cmd_ass():
 *
 * Assemble the command header.
 */
 /*-----------------------------------------------------------------------------------*/
struct pbuf *
hci_cmd_ass(struct pbuf *p, u8_t ocf, u8_t ogf, u8_t len) 
{
  ((u8_t *)p->payload)[0] = HCI_COMMAND_DATA_PACKET; /* cmd packet type */
  ((u8_t *)p->payload)[1] = (ocf & 0xff); /* OCF & OGF */
  ((u8_t *)p->payload)[2] = (ocf >> 8)|(ogf << 2);
  ((u8_t *)p->payload)[3] = len-HCI_CMD_HDR_LEN-1; /* Param len = plen - cmd hdr - ptype */
  if(pcb->numcmd != 0) {
    --pcb->numcmd; /* Reduce number of cmd packets that the host controller can buffer */
  }
  return p;
}
/*-----------------------------------------------------------------------------------*/
/* hci_inquiry():
 *
 * Cause the Host contoller to enter inquiry mode to discovery other nearby Bluetooth 
 * devices.
 */
 /*-----------------------------------------------------------------------------------*/
err_t
hci_inquiry(u32_t lap, u8_t inq_len, u8_t num_resp, 
	    err_t (* inq_complete)(void *arg, struct hci_pcb *pcb, 
				   struct hci_inq_res *ires, u16_t result))
{
  struct pbuf *p;
  struct hci_inq_res *tmpres;

  /* Free any previous inquiry result list */
  while(pcb->ires != NULL) {
    tmpres = pcb->ires;
    pcb->ires = pcb->ires->next;
    lwbt_memp_free(MEMP_HCI_INQ, tmpres);
  }
  
  pcb->inq_complete = inq_complete;
  
  if((p = pbuf_alloc(PBUF_RAW, HCI_INQUIRY_PLEN, PBUF_RAM)) == NULL) {
    LWIP_DEBUGF(HCI_DEBUG, ("hci_inquiry: Could not allocate memory for pbuf\n"));
    return ERR_MEM; /* Could not allocate memory for pbuf */
  }
  /* Assembling command packet */
  p = hci_cmd_ass(p, HCI_INQUIRY_OCF, HCI_LINK_CTRL_OGF, HCI_INQUIRY_PLEN);
  /* Assembling cmd prameters */