l2cap.c

蓝牙协议源代码 bluetooth stack for lwip

	pcb->dcid = ((u16_t *)p->payload)[0];
	  
	/* Remove signal from unresponded list and deallocate it */
	L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
	pbuf_free(sig->p);
	lwbt_memp_free(MEMP_L2CAP_SIG, sig);
	  
	/* Configure connection */
	pcb->state = L2CAP_CONFIG;

	/* If initiator send a configuration request */
	if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
	  l2ca_config_req(pcb);
	  pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
	}
	break;
      case L2CAP_CONN_PND:
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_pnd, status %d\n", status));
	  
	/* Disable rtx and enable ertx */
	sig->rtx = 0;
	sig->ertx = L2CAP_ERTX;
	break;
      default:
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_neg, result %d\n", result));
	/* Remove signal from unresponded list and deallocate it */
	L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
	pbuf_free(sig->p);
	lwbt_memp_free(MEMP_L2CAP_SIG, sig);

	L2CA_ACTION_CONN_CFM(pcb,result,status,ret);
	break;
      }
      break;
    case L2CAP_CFG_REQ:
      siglen = sighdr->len;
      dcid = ((u16_t *)p->payload)[0];
      flags = ((u16_t *)p->payload)[1];
      siglen -= 4;
      pbuf_header(p, -4);


      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Congfiguration request, flags = %d\n", flags));
	
      /* Find PCB with matching cid */
      for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: dcid = 0x%x, pcb->scid = 0x%x, pcb->dcid = 0x%x\n\n", dcid, pcb->scid, pcb->dcid));
	if(pcb->scid == dcid) {
	  /* Matching cid found */
	  break;
	}
      }
      /* If no matching cid was found, send a cmd reject (Invalid cid) */
      if(pcb == NULL) {
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Cfg req: no matching cid was found\n"));
	/* Alloc size of reason in cmd rej + data (dcid + scid) */
	if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
	  ((u16_t *)data->payload)[0] = L2CAP_INVALID_CID;
	  ((u16_t *)data->payload)[1] = dcid; /* Requested local cid */
	  ((u16_t *)data->payload)[2] = L2CAP_NULL_CID; /* Remote cid not known */
	  
	  ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
	}
      } else { /* Handle config request */
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Handle configuration request\n"));
	pcb->ursp_id = sighdr->id; /* Set id of request to respond to */

	/* Parse options and add to pcb */
	while(siglen > 0) {
	  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Siglen = %d\n", siglen));
	  opthdr = p->payload;
	  /* Check if type of action bit indicates a non-hint. Hints are ignored */
	  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Type of action bit = %d\n", L2CAP_OPTH_TOA(opthdr)));
	  if(L2CAP_OPTH_TOA(opthdr) == 0) {
	    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Type = %d\n", L2CAP_OPTH_TYPE(opthdr)));
	    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Length = %d\n", opthdr->len));
	    switch(L2CAP_OPTH_TYPE(opthdr)) {
	    case L2CAP_CFG_MTU:
	      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Out MTU = %d\n", ((u16_t *)p->payload)[1]));
	      pcb->cfg.outmtu = ((u16_t *)p->payload)[1];
	      break;
	    case L2CAP_FLUSHTO:
	      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: In flush timeout = %d\n", ((u16_t *)p->payload)[1]));
	      pcb->cfg.influshto = ((u16_t *)p->payload)[1];
	      break;
	    case L2CAP_QOS:
	      /* If service type is Best Effort or No Traffic the remainder fields will be ignored */
	      if(((u8_t *)p->payload)[3] == L2CAP_QOS_GUARANTEED) {
		LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: This implementation does not support the guaranteed QOS service type"));
		if(rspstate == L2CAP_CFG_SUCCESS) {
		  rspstate = L2CAP_CFG_UNACCEPT;
		  if(pcb->cfg.opt != NULL) {
		    pbuf_free(pcb->cfg.opt);
		    pcb->cfg.opt = NULL;
		  }
		}
		s = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
		memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
		if(pcb->cfg.opt == NULL) {
		  pcb->cfg.opt = s;
		} else {
		  pbuf_chain(pcb->cfg.opt, s);
		  pbuf_free(s);
		}
	      }
	      break;
	    default:
	      if(rspstate != L2CAP_CFG_REJ) {
		/* Unknown option. Add to unknown option type buffer */
		if(rspstate != L2CAP_CFG_UNKNOWN) {
		  rspstate = L2CAP_CFG_UNKNOWN;
		  if(pcb->cfg.opt != NULL) {
		    pbuf_free(pcb->cfg.opt);
		    pcb->cfg.opt = NULL;
		  }
		}
		s = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
		memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
		if(pcb->cfg.opt == NULL) {
		  pcb->cfg.opt = s;
		} else {
		  pbuf_chain(pcb->cfg.opt, s);
		  pbuf_free(s);
		}
	      }
	      break; 
	    } /* switch */
	  } /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
	  pbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
	  siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
	} /* while */
	  
	/* If continuation flag is set we don't send the final response just yet */
	if((flags & 0x0001) == 1) {
	  /* Send success result with no options until the full request has been received */
	  if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) == NULL) {
	    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate memory for pbuf\n"));
	    break;
	  }
	  ((u16_t *)data->payload)[0] = pcb->dcid;
	  ((u16_t *)data->payload)[1] = 0;
	  ((u16_t *)data->payload)[2] = L2CAP_CFG_SUCCESS;
	  ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
	  break;
	}

	/* Send a configure request for outgoing link if it hasnt been configured */
	if(!(pcb->cfg.l2capcfg & L2CAP_CFG_IR) && !(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_REQ)) {
	  l2ca_config_req(pcb);
	  pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
	}

	/* Send response to configuration request */
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Send response to configuration request\n"));
	if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) != NULL) {
	  ((u16_t *)data->payload)[0] = pcb->dcid;
	  ((u16_t *)data->payload)[1] = 0; /* Flags (No continuation) */
	  ((u16_t *)data->payload)[2] = rspstate; /* Result */
	  if(pcb->cfg.opt != NULL) {
	    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: pcb->cfg.opt->len = %d\n", pcb->cfg.opt->len));
	    pbuf_chain(data, pcb->cfg.opt); /* Add option type buffer to data buffer */
	    pbuf_free(pcb->cfg.opt);
	    pcb->cfg.opt = NULL;
	  }
	  ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
	}

	if(rspstate == L2CAP_CFG_SUCCESS) {
	  pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_SUCCESS;
	  /* L2CAP connection established if a successful configuration response has been sent */
	  if(pcb->cfg.l2capcfg & L2CAP_CFG_IN_SUCCESS) {
	    /* IPCP connection established, notify upper layer that connection is open */
	    pcb->state = L2CAP_OPEN;
	    if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
	      L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
	    } else {
	      L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
	    }
	  }
	}
      } /* else */
      break;
    case L2CAP_CFG_RSP:
      if(pcb == NULL) {
	/* A response without a matching request is silently discarded */
	break;
      }

      /* Remove signal from unresponded list and deallocate it */
      L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
      pbuf_free(sig->p);
      lwbt_memp_free(MEMP_L2CAP_SIG, sig);

      LWIP_ASSERT(("l2cap_process_sig: cfg rsp, active pcb->state == L2CAP_CONFIG\n"),
		  pcb->state == L2CAP_CONFIG);
	
      siglen = sighdr->len;
      scid = ((u16_t *)p->payload)[0];
      flags = ((u16_t *)p->payload)[1];
      result = ((u16_t *)p->payload)[2];
      siglen -= 6;
      pbuf_header(p, -6);
      
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Outgoing configuration result == %d continuation flag == %d\n", result, flags));

      /* Handle config request */
      switch(result) {
      case L2CAP_CFG_SUCCESS:
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Successfull outgoing configuration\n"));
	pcb->cfg.l2capcfg |= L2CAP_CFG_IN_SUCCESS; /* Local side of the connection
						      has been configured for outgoing data */
	pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset configuration timeout */
	
	if(pcb->cfg.outflushto != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
	  lp_write_flush_timeout(&pcb->remote_bdaddr, pcb->cfg.outflushto);
	}

	/* L2CAP connection established if a successful configuration response has been sent */
	if(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_SUCCESS) {
	  pcb->state = L2CAP_OPEN;
	  if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
	    L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
	  } else {
	    L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
	  }
	}
	break;
      case L2CAP_CFG_UNACCEPT:
	/* Parse and add options to pcb */
	while(siglen > 0) {
	  opthdr = p->payload;
	  /* Check if type of action bit indicates a non-hint. Hints are ignored */
	  if(L2CAP_OPTH_TOA(opthdr) == 0) {
	    switch(L2CAP_OPTH_TYPE(opthdr)) {
	    case L2CAP_CFG_MTU:
	      if(L2CAP_MTU > ((u16_t *)p->payload)[1]) {
		pcb->cfg.outmtu = ((u16_t *)p->payload)[1];
	      } else {
		LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Configuration of MTU failed\n"));
		l2ca_disconnect_req(pcb, NULL);
		return;
	      }
	      break;
	    case L2CAP_FLUSHTO:
	      pcb->cfg.influshto = ((u16_t *)p->payload)[1];
	      break;
	    case L2CAP_QOS:
	      /* If service type Best Effort is not accepted we will close the connection */
	      if(((u8_t *)p->payload)[3] != L2CAP_QOS_BEST_EFFORT) {
		LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Unsupported service type\n"));
		l2ca_disconnect_req(pcb, NULL);
		return;
	      }
	      break;
	    default:
	      /* Should not happen, skip option */
	      break; 
	    } /* switch */
	  } /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
	  pbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
	  siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
	} /* while */
	
	/* Send out a new configuration request if the continuation flag isn't set */
	if((flags & 0x0001) == 0) {
	  l2ca_config_req(pcb);
	}
	break;
      case L2CAP_CFG_REJ:
	/* Fallthrough */
      case L2CAP_CFG_UNKNOWN:
	/* Fallthrough */
      default:
	if((flags & 0x0001) == 0) {
	  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Configuration failed\n"));
	  l2ca_disconnect_req(pcb, NULL);
	  return;
	}
	break;
      } /* switch(result) */  

      /* If continuation flag is set we must send a NULL configuration request */
      if((flags & 0x0001) == 1) {
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Continuation flag is set. Send empty (default) config request signal\n"));
	if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
	  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate memory for pbuf\n"));
	  return;
	}
	/* Assemble config request packet */
	((u16_t *)data->payload)[0] = pcb->scid;
	((u16_t *)data->payload)[2] = 0; 
	l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), data);
      }
      break;
    case L2CAP_DISCONN_REQ:
      siglen = sighdr->len;
      dcid = ((u16_t *)p->payload)[0];
      siglen = siglen - 2;
      flags = ((u16_t *)p->payload)[1];
      siglen = siglen - 2;
      pbuf_header(p, -4);
	
      /* Find PCB with matching cid */
      for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
	if(pcb->scid == dcid) {
	  /* Matching cid found */
	  break;
	}
      }
      /* If no matching cid was found, send a cmd reject (Invalid cid) */
      if(pcb == NULL) {
	/* Alloc size of reason in cmd rej + data (dcid + scid) */
	if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
	  ((u16_t *)data->payload)[0] = L2CAP_INVALID_CID;
	  ((u16_t *)data->payload)[1] = dcid; /* Requested local cid */
	  ((u16_t *)data->payload)[2] = L2CAP_NULL_CID; /* Remote cid not known */
	
	  ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
	}
      } else { /* Handle disconnection request */
	if((data = pbuf_alloc(PBUF_RAW, L2CAP_DISCONN_RSP_SIZE, PBUF_RAM)) != NULL) {
	  ((u16_t *)data->payload)[0] = pcb->scid;
	  ((u16_t *)data->payload)[1] = pcb->dcid;
	  ret = l2cap_signal(pcb, L2CAP_DISCONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
	  
	  /* Give upper layer indication */
	  pcb->state = L2CAP_CLOSED;
	  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Disconnection request\n"));
	  L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);  
	}	  
      }
      break;
    case L2CAP_DISCONN_RSP:
      if(pcb == NULL) {
	/* A response without a matching request is silently discarded */
	break;
      }
      /* Remove signal from unresponded list and deallocate it */
      L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
      pbuf_free(sig->p);
      lwbt_memp_free(MEMP_L2CAP_SIG, sig);

      L2CA_ACTION_DISCONN_CFM(pcb,ret); /* NOTE: Application should
					   now close the connection */
      break;
    case L2CAP_ECHO_REQ:
      pcb->ursp_id = sighdr->id; 
      ret = l2cap_signal(pcb, L2CAP_ECHO_RSP, sighdr->id, &(pcb->remote_bdaddr), NULL);
      break;
    case L2CAP_ECHO_RSP:
      if(pcb == NULL) {
	/* A response without a matching request is silently discarded */
	break;
      }
      /* Remove signal from unresponded list and deallocate it */
      L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
      pbuf_free(sig->p);
      lwbt_memp_free(MEMP_L2CAP_SIG, sig);
	
      /* Remove temporary pcb from active list */
      L2CAP_RMV(&l2cap_active_pcbs, pcb);
      L2CA_ACTION_PING_CFM(pcb,L2CAP_ECHO_RCVD,ret);
      break;
    default:
      /* Alloc size of reason in cmd rej */
      if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE, PBUF_RAM)) != NULL) {
	((u16_t *)data->payload)[0] = L2CAP_CMD_NOT_UNDERSTOOD;
      
	ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
      }
      break;
    } /* switch */
    len = len - (sighdr->len + L2CAP_SIGHDR_LEN);
    pbuf_header(p, -(sighdr->len));
  } /* while */
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_input():
 * 
 * Called by the lower layer. Reassembles the packet, parses the header and forward
 * it to the upper layer or the signal handler.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_input(struct pbuf *p, struct bd_addr *bdaddr)
{
  struct l2cap_seg *inseg;
  struct hci_acl_hdr *aclhdr;
  struct pbuf *data;
  err_t ret;

  pbuf_header(p, HCI_ACL_HDR_LEN);
  aclhdr = p->payload;