l2cap.c

linux系统下的关于蓝牙模块的源代码！十分的经典的程序！

	PRINTPKT(__FUNCTION__ ": ", data, len);

	if (*l2cap_len == 0) {
		/* Start of a new frame received, 
		   parse header and set l2cap_len */
		
		if (len < 4) {
			D_RCV(__FUNCTION__ ": Incomplete frame header!\n");
			return;
		}

		pkt = (l2cap_packet *)data;

		pkt_len = le16_to_cpu(pkt->len);
		pkt_cid = le16_to_cpu(pkt->cid);

		/* Do some sanity checks */
		if (pkt_len > HCI_IN_SIZE)
                {
                  /* Packet won't fit in inbuffers */
                  D_ERR(__FUNCTION__": packet too big [%d], discard packet\n", 
                        pkt_len);
                  hci_clear_buffer(hci_handle);
                  return;
                }

		if ((pkt_cid != 1) && (pkt_cid != 2) &&
                    ((pkt_cid < 0x0040) || (pkt_cid > 0xffff)))
                {
                  D_ERR(__FUNCTION__": invalid CID [%d], discard packet\n", 
                        pkt_cid);
                  hci_clear_buffer(hci_handle);
                  return;
                }

		/* l2cap_len is checked in hci, when l2cap_len bytes 
		   has been received in hci this function is called again */
		*l2cap_len = pkt_len + L2CAP_HDRSIZE;
    
		D_RCV(__FUNCTION__ ": New frame len:%d cid:%d\n", pkt_len, pkt_cid);

		/* check length */
		if (!(pkt_len == (len - L2CAP_HDRSIZE)))
			return;
	} else if (len != *l2cap_len) {
		/* Not recieved full frame yet or BIG packet */
		
		if (len > *l2cap_len) {
			DSYS(__FUNCTION__ ": BIG PACKET ! (%d bytes) discard\n", len);
			hci_clear_buffer(hci_handle);
		}
		return;
	}  

	/* A whole frame is received */
	pkt = (l2cap_packet *)data;
	pkt_len = le16_to_cpu(pkt->len);
	pkt_cid = le16_to_cpu(pkt->cid);

	switch (pkt_cid) {
	case CIDSIG:
		/* Signalling channel */
		D_RCV(__FUNCTION__ ": Signal data !\n");
		
		signal_handler(hci_handle, data + L2CAP_HDRSIZE, 
			       len-L2CAP_HDRSIZE);
		break;
    
	case CIDRCVCONLESS:
		if (!l2cap->allow_conless) {
			D_WARN("Connection less data not allowed\n");
			hci_clear_buffer(hci_handle);
			return;
		}
		
                D_RCV(__FUNCTION__ ": Connectionless data\n");

		/* FIXME: Move data 2 bytes ahead since there is a psm value
		   in the connection less packet */
		con = get_con_hcihdl(hci_handle);
		
		get_upper(le16_to_cpu(get_unaligned((u16 *)&pkt->data[0])))->
			receive_data(con, pkt->data + 2, pkt_len - 2);
		break;
    
	default:
		/* Data channel */
		con = get_lcon(pkt_cid);
    
		if (con == NULL) {
			D_WARN(__FUNCTION__": No connection object found\n");
			hci_clear_buffer(hci_handle);
			return;
		}

		if (con->current_state == OPEN ) {      
			process_frame(con, pkt->data, pkt_len);
		} else {
			D_ERR(__FUNCTION__ ": not OPEN yet, discard data\n");
		}
		break;
	}

	/* free hci inbuffer */
	hci_clear_buffer(hci_handle); 
}

/* Signalling between two l2cap entities on remote devices */

void 
signal_handler(u16 hci_handle, u8 *data, 
	       u32 len)
{
	sig_cmd *cmd;
	s32 pos = 0; /* position in packet */

	cmd = (struct sig_cmd *)(data + pos);
  
	cmd->len = le16_to_cpu(cmd->len);

	D_RCV(__FUNCTION__ ": received %d bytes\n", len);
	PRINTPKT(__FUNCTION__ ": data", data, len);

	if (len < (cmd->len + 4)) {
		D_ERR(__FUNCTION__ ": Length doesn't match\n");
		return; 
	} else if (len > (cmd->len + 4)) {  
		D_RCV(__FUNCTION__ ": Multiple commands !\n");  
	} else {
		D_RCV(__FUNCTION__ ": Single command\n");
	}  

	while (pos < len) {
		D_RCV(__FUNCTION__ ": got packet (%d bytes) with ID : %d\n",
		      cmd->len, cmd->id);
    
		if (ISREQUEST(cmd->code)) { 
			process_request(hci_handle, cmd);
		} else {
			process_response(hci_handle, cmd);
		}
    
		pos += (cmd->len + 4); /* General command header 4 bytes */
		
		/* parse next command header if more cmds left in packet */
		if (pos < len) {
			cmd = (struct sig_cmd *)(data + pos);
			cmd->len = le16_to_cpu(cmd->len);
			DSYS("another command in same packet...(%d bytes)\n",
			     cmd->len);
		}
	} 
}


void process_request(u16 hci_handle, struct sig_cmd *req)
{
	sig_conreq *conreq;
	sig_confreq *confreq;
	sig_discreq *discreq;
	l2cap_con *con = NULL;
	D_STATE(__FUNCTION__ ": Got request : 0x%x id:%d\n", req->code, req->id);
	
	/* FIXME -- Add check for cmd len in each request type 
	   (used to detect corrupt packets) */

	switch (req->code) {
	case SIG_CONREQ:
                /* Request for connection */
		conreq = (sig_conreq *)req->data;
		conreq->psm = le16_to_cpu(conreq->psm);
		conreq->src_cid = le16_to_cpu(conreq->src_cid);
		
		D_STATE(__FUNCTION__ ": Connection request\n");
		D_STATE(__FUNCTION__ ": id:%d len:%d PSM 0x%x src_cid:%d\n",
			req->id, req->len, conreq->psm, conreq->src_cid);
                /* 
		   Two cases :

		   1) This is the first connection  request on 
		   this hci handle
		
		   2) We already have a hci handle and wants 
		   another connection over the same physical
		   link
		*/

		/* FIXMARK -- use new function which looks in hci link list 
		   if found */
		
		if ((con = check_remote_cid(hci_handle, 
					    conreq->src_cid)) == NULL) {
			D_ERR(__FUNCTION__ ": couldn't find l2cap connection\n");
			l2cap_cmdrej(hci_handle, CMDREJ_INVALIDCID,
				     "Invalid CID", 13);
			return;
		}
		
		if (PARANOIA_CHECKCON(con)) {
			D_ERR(__FUNCTION__ ": Paranoia check failed\n");
			return;
		}
                
		ENTERSTATE(con, W4_L2CA_CONNECT_RSP);
		PRINTSTATE(con);
		
		con->psm = conreq->psm;

		con->sig_id_rcv = req->id;

		/* Other side initiated (default) */
		con->initiator = FALSE; 
		
		/* we must send a configure request */
		con->conf_req_ready=FALSE;
		con->conf_rsp_ready=FALSE;

#ifdef CONFIG_BLUETOOTH_USE_SECURITY_MANAGER                
                l2cap_check_allowed_security(con->psm, con->remote_bd, con);
#else
                l2ca_connect_ind(con);
#endif
		break; /* SIG_CONREQ*/
	
	case SIG_CONFREQ:
                /* Request for configuration */
		confreq = (sig_confreq *)req->data;
		confreq->dst_cid = le16_to_cpu(confreq->dst_cid);
		confreq->flags = le16_to_cpu(confreq->flags);
		
		D_STATE(__FUNCTION__ ": config request cid:%d flags: 0x%x\n", 
			confreq->dst_cid, confreq->flags);
		
		if ((con = get_lcon(confreq->dst_cid)) == NULL) {
			D_ERR(__FUNCTION__ ": Couldn't find local CID\n");
			/* send back response ? */
			l2cap_cmdrej(hci_handle, CMDREJ_INVALIDCID, NULL, 0);
			return;
		}
		
		switch (con->current_state) {
		case CLOSED:
			/* FIXME - send peer 
			   'l2cap_config_rsp_neg' */
			break;
			
		case CONFIG: {
			s32 result = CONF_SUCCESS;
			/* Parse remote options and store it in con */

			if ((req->len - 4) > 0) {
				result = parse_options(con, confreq->options,
						       req->len-4);
			}

			/* Are we expecting more config requests ? */
			con->remote_flags = confreq->flags;

			if (result != CONF_SUCCESS) {
				/* we didn't like the options... */
				DSYS("parse_options failed, send neg rsp\n");

				/* send back negative response */
				if (l2ca_config_rsp(con, result, STAT_NOINFO, 
						    CONF_FAILURE) < 0) {
					D_ERR(__FUNCTION__ ": Couldn't send conf rsp\n");
				}
				
				return;
			}
			
			con->sig_id_rcv = req->id;

			l2ca_config_ind(con);
			break;
		}
		
		case OPEN: {	
			s32 result = CONF_SUCCESS;

			/* 
			   FIXME - Suspend data transmission at a 
			   convenient point 
			*/
			
			D_STATE(__FUNCTION__ "Got conf req in OPEN, renegotiate !\n");

			ENTERSTATE(con, CONFIG);
			PRINTSTATE(con);
			
			if ((req->len - 4) > 0) {
				result = parse_options(con, confreq->options,
						       req->len-4);
			} else {
				D_ERR(__FUNCTION__": Remote peer tried to renegotiate but cinf req contained no options !\n");
				return;
			}
			
			/* Are we expecting more config requests ? */
			con->remote_flags = confreq->flags;

			if (result != RES_SUCCESS) {
				
				/* we didn't like the options... */
				
				/* send back negative response */
				if (l2ca_config_rsp(con, result, 
						    STAT_NOINFO, 
						    CONF_FAILURE) < 0) {
					D_ERR(__FUNCTION__": Couldnt send conf rsp\n");
				}
				return;
			}

			con->sig_id_rcv = req->id;

			/* we haven't replied pos on other side's 
			   config request yet */
			
			con->conf_rsp_ready=FALSE;
			con->conf_req_ready=FALSE;
			
			l2ca_config_ind(con);
			break;
		}
           
		default:
			D_ERR(__FUNCTION__": Got config req in invalid state! [%s]\n",
			      state_name[con->current_state]);
			PRINTSTATE(con);
			break;
		}  
		break; /* SIG_CONFREQ*/
			
	case SIG_DISCREQ:
		discreq = (sig_discreq *)req->data;
		discreq->dst_cid = le16_to_cpu(discreq->dst_cid);
		discreq->src_cid = le16_to_cpu(discreq->src_cid);

		D_STATE(__FUNCTION__ ": disconnection request id %d\n", 
			req->id);

		if ((con = get_lcon(discreq->dst_cid)) == NULL) {
			DSYS(__FUNCTION__"Disconnecting NULL object!\n");
			return;
		}

		SHOW_CON("disc req\n", con);

		con->sig_id_rcv = req->id;

		/* if already closed, simply acknowledge */
		if ((con->current_state == CLOSED)) {
			D_STATE(__FUNCTION__ ": connection closed, send disc rsp\n");
			l2cap_disconnect_rsp(con);
			return;
		}

		/* else notify upper layers */
		ENTERSTATE(con, W4_L2CA_DISCONNECT_RSP);
		PRINTSTATE(con);
         
		l2ca_disconnect_ind(con);          
		break; /* SIG_DISCREQ */
       
	case SIG_ECHOREQ: {
		sig_echo_pkt* echo;
		D_STATE(__FUNCTION__ ": Echo request\n");
		
		echo = (sig_echo_pkt *)(req->data);		

		l2cap_echo_rsp(hci_handle, req->id, 
			       req->data, req->len - sizeof(sig_echo_pkt));
		break;
	}
       
	case SIG_INFOREQ: {
		/* Implementation specific */
 		sig_info_req *info;
		D_STATE(__FUNCTION__ ": Info request\n");
		
		info = (sig_info_req*)(req->data);
		info->type = le16_to_cpu(info->type);
		
		switch(info->type) {
		case INFO_CONNLESS_MTU:
			DSYS("Request for connectionless MTU\n");
			
			l2cap_info_rsp(hci_handle, req->id, info->type,
				       NULL, 0, 1 /* not supported */);
			break;

		default:
			D_ERR(__FUNCTION__ ": Unknown info request: type 0x%x\n", info->type);
			l2cap_cmdrej(hci_handle, CMDREJ_NOTUNDERSTOOD,
				     NULL, 0);
			break;		
		}
		break;
	}    
       
	case SIG_RESERVED:
		D_STATE(__FUNCTION__ ": Reserved!\n");
		break;
        
	default:
		/* Not a valid command */
		DSYS(__FUNCTION__ ": Invalid command (code 0x%x)s\n", req->code);
		l2cap_cmdrej(hci_handle, CMDREJ_NOTUNDERSTOOD, NULL, 0);
		break;
	}
}

void process_response(u16 hci_handle, struct sig_cmd *rsp)
{
	sig_conrsp *conrsp;
	sig_confrsp *confrsp;
	sig_discrsp *discrsp;
	sig_echo_pkt *echo;
	sig_info_rsp *info;
	sig_cmdreject *cmdreject;
	l2cap_con *con = NULL;
	s32 failure = 0;
	u16 opt_len = 0;

	D_STATE(__FUNCTION__ ": Got response: 0x%x id:%d\n",
		rsp->code, rsp->id);

	/* FIXME -- Add check for cmd len in each response type 
	   (used to detect corrupt packets) */

	switch (rsp->code) {
	case SIG_CMDREJECT:
		D_STATE(__FUNCTION__ ": Command reject - \n");
		DSYS("Got command reject\n");
		cmdreject = (sig_cmdreject*)rsp->data;
		cmdreject->reason = le16_to_cpu(cmdreject->reason);
		opt_len = rsp->len - sizeof(sig_cmdreject);
		switch (cmdreject->reason) {
		case 0:
			D_ERR(__FUNCTION__ ": Command not understood\n"); 
			break;
           
		case 1:
			D_ERR(__FUNCTION__ ": Signalling MTU exceeded\n");
			break;
           
		case 2:
			D_ERR(__FUNCTION__ ": Invalid CID in request\n");
			break;
           
		default:
			D_ERR(__FUNCTION__ ": Unrecognized cmd reject "\
			      "reason\n");
			break;
		}
		
		if (opt_len > 0)
			print_data(__FUNCTION__ ": optional data : ", 
				 cmdreject->data, rsp->len-2);

		/* find connection object using id field */
		
		con = get_first_con();

		while (con && (con->sig_id_sent != rsp->id))
			con = get_next_con(con);

		if (con) {                  
			/* fixme -- set 'real' reason code */
			con->c_status = CSTATUS_CMDREJECT;
			
			l2ca_wakeup("l2cap cmd reject", con);
			
#ifdef CONFIG_BLUETOOTH_L2CAP_USE_TIMERS
			disable_rtx(con);
#endif
		} else
			D_ERR(__FUNCTION__ ": Could not find an l2cap "\
			      "connection for this ID\n");
		break;
       
	case SIG_CONRSP:
		/* client */
		D_STATE(__FUNCTION__ ": Got connection response\n");
		conrsp = (sig_conrsp *)rsp->data;
		conrsp->src_cid = le16_to_cpu(conrsp->src_cid);
		conrsp->dst_cid = le16_to_cpu(conrsp->dst_cid);
		conrsp->result  = le16_to_cpu(conrsp->result);
		conrsp->status  = le16_to_cpu(conrsp->status);

		/* find connection */
		if ((con = get_lcon(conrsp->src_cid)) == NULL) {
			D_ERR(__FUNCTION__ ": con rsp, NO CONNECTION FOUND\n");
			return;
		}

		con->remote_cid = conrsp->dst_cid;

		/* match id with request */
		if (!id_matched(con, rsp->id)) {
			D_ERR(__FUNCTION__ ": ID doesn't match ! [%d:%d]\n", con->sig_id_sent, rsp->id);
			return;
		}
         
		con->c_status = conrsp->result;		

		switch (conrsp->result) {
		case RES_SUCCESS:

			con->c_result = RES_SUCCESS;
			l2ca_wakeup("l2cap con rsp", con);