l2cap.c

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

/*******************************************************************/
/* (A2) l2cap to l2cap signalling */
/**********************************/

/****************************/
/* l2cap request functions  */
/****************************/

s32 
l2cap_connect_req(l2cap_con *con, u16 psm)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_conreq *req;
	u16 payload_len;

        D_XMIT(__FUNCTION__ ": Connecting %s (rcid:%d)\n",psm2str(psm), 
               con->remote_cid);
	
	if (con->current_state != W4_L2CAP_CONNECT_RSP) {
		D_ERR(__FUNCTION__ ": Invalid state\n");
		return -MSGCODE(MSG_LAYER_L2CAP, L2CAP_INVALID_STATE);
	}

	payload_len = SIGCMD_HDRSIZE + CON_REQSIZE;  /*2 x 4*/

	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	}

	l2cap_buf = (l2cap_tx_buf *)(tx->data);
  
	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	req = (sig_conreq*)(l2cap_buf->frame + L2CAP_HDRSIZE + SIGCMD_HDRSIZE);

	/* Now fill in header fields */  
	req->psm = cpu_to_le16(con->psm);
	req->src_cid = cpu_to_le16(con->local_cid);
  
	cmd->code = SIG_CONREQ;

        /* Don't increment ID if retransmission */
        if (con->timer.rtx_no == 0)
                cmd->id = set_id(con); /* S
ets sig_id_sent in l2cap_con */
        else
                DSYS("RTX, use same ID\n");

	cmd->len = cpu_to_le16(CON_REQSIZE);

	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);

	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = con->hci_hdl;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len; /* Increased as lower layers
						      add header data */  
	
#ifdef CONFIG_BLUETOOTH_L2CAP_USE_TIMERS
	/* start retransmission timer */
	start_rtx(con, RTX_TIMEOUT, RTX_ACTION_START_ERTX);
#endif
	return hci_send_data(tx);
}

/* only specify inparam if it differs from default value */
s32 
l2cap_config_req(l2cap_con *con, u16 in_mtu, flow *outflow,
		 u16 flush_timeout, u16 link_timeout)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_confreq *req;
	u16 payload_len;
	s32 opt_len = 0;
	struct l2cap_option *opt;

	D_XMIT(__FUNCTION__ ": rcid :%d\n", con->remote_cid);

	/******************************************************************/
	/* | len2 | CID2 || code1 | id1 || destcid2 | flags1 | options? | */
	/* -------------------------------------------------------------- */
	/* | l2cap hdr   || sig cmd hdr ||             data             | */
	/******************************************************************/

	D_STATE(__FUNCTION__ ": inmtu : %d, local mtu : %d\n",
		in_mtu, con->local_mtu);

	if (in_mtu != 0) {
		DSYS("Setting local mtu to %d\n", in_mtu);
		con->local_mtu = in_mtu;
		opt_len += 4;
	}

	if (flush_timeout != 0) {
		/* We inform peer about our flush timeout */
		
		/* FIXME -- who controls this ? Upper layer ? */
		opt_len +=4;
	}
	
	if (outflow != NULL) {
		/* We inform peer about our QOS settings */
		opt_len += sizeof *outflow + 2; /* include type/len */
	}
	
	payload_len = SIGCMD_HDRSIZE + CONF_REQSIZE + opt_len;  
 
	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	}

	l2cap_buf = (l2cap_tx_buf *)(tx->data);
  
	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	req = (sig_confreq*)(l2cap_buf->frame + L2CAP_HDRSIZE + SIGCMD_HDRSIZE);
	
	if (opt_len > 0) {
		/* if the negotiated mtu differs from the current, send it */
          
		opt = (struct l2cap_option*)(l2cap_buf->frame+L2CAP_HDRSIZE + 
			SIGCMD_HDRSIZE + CONF_REQSIZE);
 
		if (in_mtu != 0) {
			DSYS("Sending in_mtu %d\n", in_mtu);
			opt->type = OPT_MTU;
			opt->len = 2;
			*(opt->option_data) = (u8)(in_mtu & 0x00ff);
			*(opt->option_data+1) = (u8)(in_mtu >> 8) & 0x00ff;
	
			/* forward pointer for other options */
			opt = (struct l2cap_option*)((char *)opt + 4);
		}
		
		if (flush_timeout != 0) {
			DSYS("Sending flush_timeout %d\n", flush_timeout);
			opt->type = OPT_FLUSH;
			opt->len = 2;
			*(opt->option_data) = (u8)(flush_timeout & 0x00ff);
			*(opt->option_data+1) = (u8)(flush_timeout >> 8) &0x00ff;

			/* forward pointer for other options */
			opt = (struct l2cap_option*)((char *)opt + 4);
		}
		
		if (outflow != NULL) { 
			DSYS("Sending conf req outflow\n");
			print_flow(outflow);
			opt->type = OPT_QOS;
			opt->len = sizeof *outflow;
			memcpy((char*)opt+2, outflow, sizeof *outflow);
		}
	}

	/* Request header */
	req->dst_cid = cpu_to_le16(con->remote_cid); /* Sending end */
	req->flags = 0; /* Negotiate same as remote */

	/* Signalling header */
	cmd->code = SIG_CONFREQ;

        /* Don't increment ID if retransmission */
        if (con->timer.rtx_no == 0)
                cmd->id = set_id(con); /* Sets sig_id_sent in l2cap_con */
        else
                DSYS("RTX, use same ID\n");

	/* fixme -- wrong len field should be 2 more */

	cmd->len = cpu_to_le16(CONF_REQSIZE + opt_len);

	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);

	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = con->hci_hdl;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len; /* Increased when lower
						      layers add header data */

	opt = (struct l2cap_option*)(l2cap_buf->frame+L2CAP_HDRSIZE + 
				     SIGCMD_HDRSIZE + CONF_REQSIZE);
	
	print_data("l2cap options: ", (char*)opt , opt_len);
#ifdef CONFIG_BLUETOOTH_L2CAP_USE_TIMERS
	/* start retransmission timer */
	start_rtx(con, RTX_TIMEOUT, RTX_ACTION_START_ERTX);
#endif
	return hci_send_data(tx);
}

s32 
l2cap_disconnect_req(l2cap_con *con)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_discreq *req;
	u16 payload_len;
 
	D_XMIT(__FUNCTION__ ": rcid %d\n", con->remote_cid);

	payload_len = SIGCMD_HDRSIZE + DISC_REQSIZE;  

	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	}

	l2cap_buf = (l2cap_tx_buf *)(tx->data);

	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	req = (sig_discreq*)(l2cap_buf->frame + L2CAP_HDRSIZE + SIGCMD_HDRSIZE);

	/* Now fill in header fields */  
	req->dst_cid = cpu_to_le16(con->remote_cid);
	req->src_cid = cpu_to_le16(con->local_cid);

	cmd->code = SIG_DISCREQ;
        
        /* Don't increment ID if retransmission */
        if (con->timer.rtx_no == 0)
                cmd->id = set_id(con); /* Sets sig_id_sent in l2cap_con */
        else
                DSYS("RTX, use same ID\n");
        
	cmd->len = cpu_to_le16(DISC_REQSIZE);

	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);
	tx->hci_hdl = con->hci_hdl;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len;

#ifdef CONFIG_BLUETOOTH_L2CAP_USE_TIMERS
	/* start retransmission timer */
	start_rtx(con, RTX_TIMEOUT, RTX_ACTION_TERMINATE);
#endif
	return hci_send_data(tx);
}

s32 
l2cap_echo_req(l2cap_con *con, u8 *opt_data, u16 opt_len)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_echo_pkt *req;
	u16 payload_len;
	u16 hci_hdl = con->hci_hdl;

	payload_len = SIGCMD_HDRSIZE + sizeof(sig_echo_pkt) + opt_len;

	D_XMIT(__FUNCTION__ "\n");
	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	}

	l2cap_buf = (l2cap_tx_buf *)(tx->data);
  
	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	req = (sig_echo_pkt*)(l2cap_buf->frame + L2CAP_HDRSIZE +SIGCMD_HDRSIZE);
    
	/* Now fill in header fields */
	if (opt_len)
		memcpy(req->data, opt_data, opt_len);
  
	cmd->code = SIG_ECHOREQ;
	cmd->id = get_id();
  
	cmd->len = cpu_to_le16(opt_len);

	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);

	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = hci_hdl;
	tx->bc_flag = NO_BROADCAST;

        /* Increased when lower layers add header data */
	tx->cur_len = L2CAP_HDRSIZE + payload_len;

#ifdef CONFIG_BLUETOOTH_L2CAP_USE_TIMERS
	start_rtx(con, RTX_TIMEOUT, RTX_ACTION_DISCONNECT);
#endif
        return hci_send_data(tx);
}

s32 
l2cap_echo_rsp(s32 hci_hdl, s32 id, u8 *opt_data, u16 opt_len)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_echo_pkt *rsp;
	u16 payload_len;

	D_XMIT(__FUNCTION__ "\n");

	payload_len = SIGCMD_HDRSIZE + opt_len;

	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	} 

	l2cap_buf = (l2cap_tx_buf *)(tx->data);

	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	rsp = (sig_echo_pkt*)(l2cap_buf->frame + L2CAP_HDRSIZE +SIGCMD_HDRSIZE);
    
	/* Now fill in header fields */
	if (opt_len)
		memcpy(rsp->data, opt_data, opt_len);
  
	cmd->code = SIG_ECHORSP;
	cmd->id = id;
	cmd->len = cpu_to_le16(opt_len);

	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);

	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = hci_hdl;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len; /* Increased as lower layers
						      add header data */  
	return hci_send_data(tx);
}

s32
l2cap_info_req(l2cap_con *con, u16 info_type)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_info_req *req;
	s32 payload_len;
	u16 hci_hdl = con->hci_hdl;

	payload_len = SIGCMD_HDRSIZE + sizeof(sig_info_req);

	D_XMIT(__FUNCTION__ "\n");

	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	}

	l2cap_buf = (l2cap_tx_buf *)(tx->data);
  
	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	req = (sig_info_req*)(l2cap_buf->frame + L2CAP_HDRSIZE +SIGCMD_HDRSIZE);
	req->type = cpu_to_le16(info_type);
    
	cmd->code = SIG_INFOREQ;
	cmd->id = get_id();
	cmd->len = cpu_to_le16(sizeof(sig_info_req));

	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);

	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = hci_hdl;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len; /* Increased when lower
						      layers add header data */
#ifdef CONFIG_BLUETOOTH_L2CAP_USE_TIMERS
	start_rtx(con, RTX_TIMEOUT, RTX_ACTION_TERMINATE);
#endif
	return hci_send_data(tx);
}
 
s32 
l2cap_info_rsp(s32 hci_hdl, s32 id, u16 info_type, u8 *info_data, 
	       s32 info_len, s32 result)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_info_rsp *rsp;
	s32 payload_len;

	D_XMIT(__FUNCTION__ "\n");

	payload_len = SIGCMD_HDRSIZE + sizeof(sig_info_rsp) + info_len;

	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE + payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	} 

	l2cap_buf = (l2cap_tx_buf *)(tx->data);

	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	rsp = (sig_info_rsp*)(l2cap_buf->frame + L2CAP_HDRSIZE +SIGCMD_HDRSIZE);

	rsp->type = cpu_to_le16(info_type);
	rsp->result = cpu_to_le16(result);	

	/* Now fill in header fields */
	if (info_len) {
		memcpy(rsp->infodata, info_data, info_len);
	}

	cmd->code = SIG_INFORSP;
	cmd->id = id;
	cmd->len = cpu_to_le16(sizeof(sig_info_rsp) + info_len);

	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);

	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = hci_hdl;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len; /* Increased as lower layers
						      add header data */  
	return hci_send_data(tx);
}

s32 
l2cap_cmdrej(s32 hci_hdl, u8 reason, u8 *opt_data, s32 opt_len)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_cmdreject *cmdrej;
	u16 payload_len;

	payload_len = SIGCMD_HDRSIZE + sizeof(sig_cmdreject) + opt_len;

	D_XMIT(__FUNCTION__ ": %s\n", cmdrej_reason[reason]);

	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	}  

	l2cap_buf = (l2cap_tx_buf *)(tx->data);

	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	cmdrej = (sig_cmdreject*)(l2cap_buf->frame
				  + L2CAP_HDRSIZE + SIGCMD_HDRSIZE);
    
	/* Now fill in header fields */
	if (opt_len)
		memcpy(cmdrej->data, opt_data, opt_len);
  
	cmd->code = SIG_CMDREJECT;
	cmd->id = get_id();
	cmd->len = cpu_to_le16(sizeof(sig_cmdreject) + opt_len);

	/* FIXME: set the reason parameter */
	/* Ok. Does this work? (gjm) */
	cmdrej->reason = cpu_to_le16((u16)reason);
	SET_L2CAP_HDR(l2cap_buf->frame, payload_len, CIDSIG);

	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = hci_hdl;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len; /* Increased when lower
						      layers add header data */
	return hci_send_data(tx);
}

/****************************/
/* l2cap response functions */
/****************************/

s32 
l2cap_connect_rsp(l2cap_con* con, u16 response, u16 status)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_conrsp *rsp;
	u16 payload_len;
  
	D_XMIT(__FUNCTION__ ": rcid:%d lcid:%d result:%d status:%d \n",
	       con->remote_cid, con->local_cid, response, status);

	payload_len = SIGCMD_HDRSIZE + CON_RSPSIZE;
  
	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR(__FUNCTION__ ": didn't get a valid tx buf\n");
		return -ENOMEM;
	}

	l2cap_buf = (l2cap_tx_buf *)(tx->data);

	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	rsp = (sig_conrsp*)(l2cap_buf->frame + L2CAP_HDRSIZE + SIGCMD_HDRSIZE);
  
	/* Now fill in header fields */
	rsp->dst_cid = cpu_to_le16(con->local_cid); /* sending end */
	rsp->src_cid = cpu_to_le16(con->remote_cid); /* receiving end */

	if ((rsp->result = cpu_to_le16(response)) == RES_PENDING)
		rsp->status = cpu_to_le16(status);
	else {
		rsp->status = cpu_to_le16(STAT_NOINFO);
	}
	
	cmd->code = SIG_CONRSP;
	cmd->id = con->sig_id_rcv; /* Send back same id as received