l2cap.c

blue tooth 核心协议栈在linux上的实现

/****************** INCLUDE FILES SECTION ***********************************/

#define __NO_VERSION__ /* don't define kernel_version in module.h */

#ifdef __KERNEL__
#include <linux/config.h>
#include <linux/bluetooth/sysdep-2.1.h>
#include <linux/malloc.h>
#include <linux/bluetooth/l2cap.h>
#include <linux/bluetooth/hci.h>
#include <linux/bluetooth/hci_internal.h>
#include <linux/bluetooth/rfcomm.h>
#include <linux/bluetooth/sdp.h>
#include <linux/bluetooth/test.h>
#include <linux/bluetooth/btmem.h>
#include <linux/bluetooth/l2cap_internal.h>
#include <linux/bluetooth/l2cap_con.h>
#include <linux/bluetooth/l2cap_sec.h>
#include <linux/bluetooth/bt_errno.h>
#include <asm/byteorder.h>
#include <asm/unaligned.h>
#else /* user mode */
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <signal.h>
#include <errno.h>
#include <asm/unaligned.h>

#include "include/l2cap.h"
#include "include/hci.h"
#include "include/hci_internal.h"
#include "include/l2cap_internal.h"
#include "include/l2cap_con.h"
#include "include/l2cap_sec.h"
#include "include/rfcomm.h"
#include "include/sdp.h"
#include "include/test.h"
#include "include/btmem.h"
#include "include/local.h"
#include "include/bt_errno.h"
#endif

/****************** DEBUG CONSTANT AND MACRO SECTION ************************/

#if L2CAP_DEBUG_STATE
/* State machine */
#define D_STATE(fmt...) printk(L2CAP_DBG_STR fmt)
#define PRINTSTATE(con) (printk(L2CAP_DBG_STR "Current state of (%d:%d) is %s\n", con->local_cid, con->remote_cid, state_name[con->current_state]))
#else
/* State machine */
#define D_STATE(fmt...)
#define PRINTSTATE(con)
#endif

#if L2CAP_DEBUG_TIMER
#define D_TIM(fmt...) printk(L2CAP_DBG_STR fmt)
#else
#define D_TIM(fmt...)
#endif

#if L2CAP_DEBUG_CON
/* Connection manager */
#define D_CON(fmt...) printk(L2CAP_DBG_STR fmt)

#define SHOW_CON(str, con) show_con(str, con)
#define SHOW_LIST() show_list()
#else
#define D_CON(fmt...)
#define SHOW_CON(str, con)
#define SHOW_LIST()
#endif

#if L2CAP_DEBUG_RCV
/* Receive data */
#define D_RCV(fmt...) printk(L2CAP_DBG_STR fmt)
#else
#define D_RCV(fmt...)
#endif

#if L2CAP_DEBUG_XMIT
/* Send data */
#define D_XMIT(fmt...) printk(L2CAP_DBG_STR fmt)
#else
#define D_XMIT(fmt...)
#endif

#if L2CAP_DEBUG_MISC
/* Misc */
#define D_MISC(fmt...) printk(L2CAP_DBG_STR fmt)
#else
#define D_MISC(fmt...)
#endif

#if L2CAP_DEBUG_DATA
#define PRINTPKT(str, data, len) print_data(str, data, len)
#else
#define PRINTPKT(str, data, len)
#endif

/****************** CONSTANT AND MACRO SECTION ******************************/

#define CON_LESS 0
#define CON_ORIENTED 1

#define MIN_CID 0x0040
#define MAX_CID 0xffff

/* EVEN => REQUEST */ 
#define ISREQUEST(x) (!(x%2))
#define SET_L2CAP_HDR(frame, len, cid) {\
        (*(u8 *)(frame)) = (u8)(((len) & 0xff)); \
        (*(u8 *)(frame + 1)) = (u8)((len) >> 8); \
        (*(u8 *)(frame + 2)) = (u8)(((cid) & 0xff)); \
        (*(u8 *)(frame + 3)) = (u8)((cid) >> 8); \
}

#define ENTERSTATE(con, state) (con->current_state = state)

#define RCID_NOT_SET 0
#define RCID_SET 1

#define CMDREJ_NOTUNDERSTOOD 0x0
#define CMDREJ_MTUEXCEEDED 0x1
#define CMDREJ_INVALIDCID 0x2

#define CONFREQ_NO_OPTIONS 0x11

const u8* psm2str(u16 psm)
{
	switch (psm) {
	case 1:
		return "SDP";
		break;

	case 3:
		return "RFCOMM";
		break;

	case 5:
		return "TCS";
		break;

	case 0x1231:
		return "TEST";
		break;

	case 0x1233:
		return "TEST-2";
		break;

	case 0x4561:
		return "TEST-3";
		break;

	default :
		return "unknown";
	}
}

const u8* cmdrej_reason[] = {
	"Command not understood",
	"Signalling MTU exceeded",
	"Invalid CID in request",
};

/****************** TYPE DEFINITION SECTION *********************************/

/****************** LOCAL FUNCTION DECLARATION SECTION **********************/

/* MISC */
static u8 set_id(l2cap_con* con);
static u8 get_id(void);
static s32 id_matched(l2cap_con* con, u8 rcv_id);

static s32 parse_options(l2cap_con* con, u8 *data, u32 len);
static void print_flow(flow *f);
static s32 l2cap_cmdrej(s32 hci_hdl, u8 reason, u8 *opt_data, s32 opt_len);

static s32 insert_upper(protocol_layer *upper_layer);
static protocol_layer* get_upper(u32 psm);
static void remove_all_upper(void);

#ifdef CONFIG_BLUETOOTH_L2CAP_USE_TIMERS

/* fixme -- calculate this using flush timeout */
#define MAX_NO_RTX 3
#define RTX_TIMEOUT 2 /* sec */
#define ERTX_TIMEOUT 60 /* sec */


#ifdef __KERNEL__
static void l2cap_rtx_timeout(unsigned long ptr);
static void l2cap_ertx_timeout(unsigned long ptr);
#else /* usermode stack */
static l2cap_con *timeout_con = NULL;
static s32 timer_cancelled = 0;
static void l2cap_rtx_timeout(void);
static void l2cap_ertx_timeout(unsigned long ptr);
#endif

static void start_rtx(l2cap_con *con, unsigned short timeout, unsigned short action);
static void disable_rtx(l2cap_con *con);

static void start_ertx(l2cap_con *con, unsigned short timeout);
static void disable_ertx(l2cap_con *con);

#endif

/****************** GLOBAL VARIABLE DECLARATION SECTION *********************/

/****************** LOCAL VARIABLE DECLARATION SECTION **********************/

static l2cap_layer l2capmain; /* Main structure */
static l2cap_layer* l2cap;
static struct protocol_layer default_protocol;
extern int bt_max_connections;
extern hci_controller hci_ctrl;

/****************** FUNCTION DEFINITION SECTION *****************************/
  
/*********************************************************************/
/*---------------------- INIT AND SHUTDOWN --------------------------*/
/*********************************************************************/

s32
l2cap_init(void)
{
	s32 i;
	u8 bd_name[20];
	DSYS("Initialising L2CAP\n");
	l2cap = &l2capmain;
	init_con_list();
        l2cap->cid_count = MIN_CID; /* Moved from init_con_list */

	/* Set all upper layers to default */

	l2cap_protocol_default(&default_protocol);
	
	l2cap->id_count = 1;
	l2cap->mtu = MTU_DEFAULT ;
	l2cap->flush_timeout = FLUSHTIMEOUT_DEFAULT; /* Always retransmit */
	l2cap->upper_layers = NULL;
	init_flow(&l2cap->qos);

	/* get local bd address */
	hci_read_local_bd(l2cap->my_bd);

	/* check result... */
	if (!l2cap->my_bd[0] && !l2cap->my_bd[1] && !l2cap->my_bd[2] &&
	    !l2cap->my_bd[3] && !l2cap->my_bd[4] && !l2cap->my_bd[5])
		D_ERR(__FUNCTION__ ": Failed to get local BD addr\n");
	else {
		i = l2cap_sprint_bd(bd_name, l2cap->my_bd);
		bd_name[i] = 0;
		DSYS("Local bd [%s]\n", bd_name);
	}

#ifdef CONFIG_BLUETOOTH_USE_SECURITY_MANAGER
        /* Initialize security */
        l2cap_sec_man_init();
#endif
        
#if L2CAP_SELFTEST
	test_conlist();
#endif
#ifdef CONFIG_BLUETOOTH_L2CAP_CONNECTIONLESS
	l2cap->allow_conless = 1;
#else
	l2cap->allow_conless = 0;
#endif
	l2cap->initiated = 1;
	return 0;
}

void 
l2cap_protocol_default(struct protocol_layer *prot)
{
	prot->psm = 0;
	prot->con_pnd = not_registered_cfm;
	prot->con_cfm = not_registered_cfm;
	prot->conf_cfm = not_registered_cfm;
	prot->disc_cfm = not_registered_cfm_noparams;

	prot->con_ind = not_registered_ind;
	prot->conf_ind = not_registered_ind;
	prot->disc_ind = not_registered_ind;

	prot->receive_data = not_registered_rcv;
	prot->next_layer = NULL;
}

void 
l2cap_register_default_upper(struct protocol_layer *prot)
{
	memcpy(&default_protocol, prot, sizeof default_protocol);
}

s32 
l2cap_shutdown(void)
{
	DSYS("Shutting down L2CAP\n");
	if (!l2cap->initiated) {
		D_ERR(__FUNCTION__ ": L2CAP not initiated\n");
		return -1;
	}
	
	/* fixme -- experimental */
#if 0
	{
		l2cap_con *con;
		con = get_first_con();
		while (con != NULL) {
			if (l2ca_disconnect_req(con) != 0)
				break;
			con = get_first_con();
		}
	}
#endif

	free_con_list();
	remove_all_upper();

	/* ALWAYS SUCCESS */
	return 0;
}

/* is called with a struct of functions to handle incoming data */


s32 
l2cap_register_upper(u16 psm, struct protocol_layer *prot) 
{
        if (((psm % 2) == 0) || (prot == NULL)) {
		D_ERR(__FUNCTION__ ": incorrect parameters\n");
		return -EINVAL;
        }
    
	D_MISC(__FUNCTION__ ": psm 0x%x\n", psm);

        if ((psm > MAX_PSM) && (psm < MIN_DYNAMIC_PSM)) {
		D_ERR(__FUNCTION__ ": value of psm reserved\n");
		return -EINVAL;
        }

        if (psm > MAX_DYNAMIC_PSM) {
		D_ERR(__FUNCTION__ ": psm not valid!\n");
		return -EINVAL;
        }

	prot->psm = psm;
	return insert_upper(prot);
}

/* Inserts the function pointers to a new upper layer in the list */

s32
insert_upper(protocol_layer *upper_layer)
{
	protocol_layer *new_layer;
	D_MISC(__FUNCTION__ ": Inserting layer psm:0x%x\n", upper_layer->psm);

	if (!(new_layer = kmalloc(sizeof *new_layer, GFP_ATOMIC)))
		return -ENOMEM;

	memcpy(new_layer, upper_layer, sizeof *new_layer);

	/* Add to head of list of upper layers */
	new_layer->next_layer = l2cap->upper_layers;
	l2cap->upper_layers = new_layer;

	return 0;
}
 
protocol_layer*
get_upper(u32 psm)
{
	protocol_layer *tmp_layer;
	
	/* Check PSM value */
        if((psm % 2) == 0) {
		D_ERR(__FUNCTION__ ": incorrect psm\n");
		return &default_protocol;
        }
        if((psm > MAX_PSM) && (psm < MIN_DYNAMIC_PSM)) {
		D_ERR(__FUNCTION__ ": value of psm reserved\n");
		return &default_protocol;
        }
        if(psm > MAX_DYNAMIC_PSM) {
		D_ERR(__FUNCTION__ ": psm not valid!\n");
		return &default_protocol;
        }

	D_MISC(__FUNCTION__ ": Try to retrieve psm 0x%x\n",psm);
	tmp_layer = l2cap->upper_layers;
	while (tmp_layer != NULL) {
		if (tmp_layer->psm == psm) {
			D_MISC(__FUNCTION__ ": Actually got psm:0x%x\n",
			       tmp_layer->psm);
			return tmp_layer;
		}
		tmp_layer = tmp_layer->next_layer;
	}
	D_MISC(__FUNCTION__ ": Didn't get any layer, returning default\n");
	return &default_protocol;
}

void
remove_all_upper(void)
{
	protocol_layer *tmp_layer;

	D_MISC(__FUNCTION__ ": Freeing all upper layers\n");

	while (l2cap->upper_layers != NULL) {
		tmp_layer = l2cap->upper_layers;
		l2cap->upper_layers = tmp_layer->next_layer;
		kfree(tmp_layer);
	}
}

/* needed ? */
void 
not_registered_cfm(l2cap_con *con, s32 result)
{
	DSYS("No upper layer confirm function defined (psm:0x%x)\n", con->psm);
	DSYS("silent discard...\n");
}

void 
not_registered_cfm_noparams(l2cap_con *con)
{
	DSYS("No upper layer confirm function defined (psm:0x%x)\n", con->psm);
	DSYS("silent discard...\n");
}

void 
not_registered_ind(l2cap_con *con)
{
	DSYS("No upper layer ind function defined (psm:0x%x)\n", con->psm);
	DSYS("silent discard...\n");
}

void 
not_registered_rcv(l2cap_con *con, u8 *data, u32 len)
{
	DSYS("No upper layer receive_data function defined (psm:0x%x)\n", 
	     con->psm);
	DSYS("silent discard...\n");
}

/****************************************************************/
/*---------------------- STATE MACHINE -------------------------*/
/****************************************************************/



/* If l2cap_len is 0 then we know we have a new frame. Parse the length field
   and set l2cap_len and compare len to this value each time new data arrives.
   When equal a whole frame is received and we can parse it */  

void 
l2cap_receive_data(u8 *data,  u32 len, u16 hci_handle, /*u8 pb_flag,*/  
                   /*u8 bc_flag,*/ u32 *l2cap_len)
{
	l2cap_packet *pkt = NULL;
	l2cap_con *con;
	u16 pkt_len;
	CID pkt_cid;

	D_RCV(__FUNCTION__ ": got %d bytes on hci_handle : %d\n", len, hci_handle);
	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 */