/* 
 * l2cap.c --  Implementation of Bluetooth Logical Link Control 
 *             and Adaption Protocol (L2CAP)          
 *
 * Copyright (C) 2000  Axis Communications AB
 *
 * Mattias Agren <mattias.agren@axis.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 * $Id: l2cap.c,v 1.59 2000/10/26 13:52:10 mattias Exp $
 *
 */

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


#ifdef __KERNEL__
#include <linux/malloc.h>

#include <linux/bluetooth/l2cap.h>
#include <linux/bluetooth/hci.h>
#include <linux/bluetooth/rfcomm.h>
#include <linux/bluetooth/sdp.h>
#include <linux/bluetooth/test.h>
#include <linux/bluetooth/btmem.h>
#else /* user mode */
#include <stdlib.h>
#include <string.h>
#include "include/l2cap.h"
#include "include/hci.h"
#include "include/rfcomm.h"
#include "include/sdp.h"
#include "include/test.h"
#include "include/btmem.h"
#endif

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

#if L2CAP_DEBUG_STATE
/* State machine */
#define D_STATE(fmt, args...) printk(L2CAP_DBG_STR fmt, ## args)
#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, args...)
#define PRINTSTATE(con)
#endif

#if L2CAP_DEBUG_CON
/* Connection manager */
#define D_CON(fmt, args...) printk(L2CAP_DBG_STR fmt, ## args)
#else
#define D_CON(fmt, args...)
#endif

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

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

#if L2CAP_DEBUG_MISC
/* Misc */
#define D_MISC(fmt, args...) printk(L2CAP_DBG_STR fmt, ## args)
#else
#define D_MISC(fmt, args...)
#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

/* Double linked dynamic list */
#define MAX_CONLISTSIZE 255 /* for now */
#define MIN_CID 0x0040
#define MAX_CID 0xffff

/* EVEN => REQUEST */ 
#define ISREQUEST(x) (!(x%2))
#define SET_L2CAP_HDR(frame, len, cid) \
(*(u32*)frame=((u32)(((cid&0xffff)<<16) | (len & 0xffff))))

#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

const u8* state_name[] = {
	"CLOSED",
	"W4_L2CAP_CONNECT_RSP",
	"W4_L2CA_CONNECT_RSP",
	"CONFIG",
	"OPEN", 
	"W4_L2CAP_DISCONNECT_RSP",
	"W4_L2CA_DISCONNECT_RSP",
	"ANY_STATE"
};

const u8* bool2str[] = {
	"no",
	"yes", 
};

const u8* psm_name[] = {
	"","SDP", 
	"","RFCOMM",
	"","TCS"
};


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;  
      
  default :
    return "unknown";
  }
}


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

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

typedef struct l2cap_con_list {
	l2cap_con *first;
	l2cap_con *last;
	l2cap_con *cur;
	u32 count;
} l2cap_con_list;

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

/* CONNECTION MANAGER */
static l2cap_con* create_con(u16 HCI_con_handle, CID lcid, CID rcid);
static s32 delete_con(l2cap_con *con);
static l2cap_con* get_rcon(CID rcid);
static l2cap_con* get_lcon(CID lcid);
static l2cap_con* get_con(BD_ADDR bd, s32 STATE);
static l2cap_con* get_con_hcihdl(u16 hdl);
static l2cap_con* check_remote_cid(u16 hdl, CID remote_cid);
static void insert_con(l2cap_con *con);
static void free_list(void);
static  u16 get_cid(void);
static void show_con(const u8* head, l2cap_con* con);
static void show_list(void);
static  s32 count_con(u16 hci_hdl);
static void reset_con(l2cap_con* con);

/* LINKED LIST */
static void init_con_list(void);

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

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

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

/* internal test of connection manager */
#define L2CAP_SELFTEST 0

#if L2CAP_SELFTEST
static void test_conlist(void);
static s32 remove_rcid(CID rcid);
#endif

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

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

static l2cap_layer l2capmain; /* Main structure */
static l2cap_layer* l2cap;
static l2cap_con_list con_list;
static struct protocol_layer default_protocol;

/****************** 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();

	/* 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 */
	read_bd_addr(l2cap->my_bd);

	/* check result... */
	if ((l2cap->my_bd[0]==0) && (l2cap->my_bd[1]==0) && 
	    (l2cap->my_bd[2]==0) && (l2cap->my_bd[3]==0) &&
	    (l2cap->my_bd[4]==0) && (l2cap->my_bd[5]==0))
		D_ERR("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);
	}

#if L2CAP_SELFTEST
	test_conlist();
#endif
	l2cap->allow_conless = DEF_ALLOWCONLESS;
	l2cap->initiated = 1;
	return 0;
}

void 
l2cap_protocol_default(struct protocol_layer *prot)
{
	prot->psm = 0;
	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;
}

s32 
l2cap_shutdown(void)
{
	DSYS("Shutting down L2CAP\n");
	if (!l2cap->initiated)
	{
		D_ERR("L2CAP not initiated\n");
		return -1;
	}
	
	/* FIXME - Disconnect all open connections */

	free_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("l2cap_register_upper : incorrect parameters\n");
		return -1;
        }
    
	D_MISC("l2cap_register_upper : psm 0x%x\n", psm);

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

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

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

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

void
insert_upper(protocol_layer *upper_layer)
{
	protocol_layer *tmp_layer, *new_layer;
	D_MISC("insert_upper: Inserting layer psm:0x%x\n",upper_layer->psm);
	
	new_layer = (protocol_layer*) kmalloc(sizeof(protocol_layer),
					      GFP_ATOMIC);
	memcpy(new_layer, upper_layer, sizeof(protocol_layer));
	new_layer->next_layer = NULL;
	
	tmp_layer = l2cap->upper_layers;
	if (tmp_layer != NULL) {
		while (tmp_layer->next_layer != NULL) {
			tmp_layer = tmp_layer->next_layer;
		}
		tmp_layer->next_layer = new_layer;
	} else {
		l2cap->upper_layers = new_layer;
	}
}
 
protocol_layer*
get_upper(u32 psm)
{
	protocol_layer *tmp_layer;
	
	/* Check PSM value */
        if((psm % 2) == 0) {
		D_ERR("get_upper : incorrect psm\n");
		return &default_protocol;
        }
        if((psm > MAX_PSM) && (psm < MIN_DYNAMIC_PSM)) {
		D_ERR("get_upper : value of psm reserved\n");
		return &default_protocol;
        }
        if(psm > MAX_DYNAMIC_PSM) {
		D_ERR("get_upper : psm not valid!\n");
		return &default_protocol;
        }

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

void
remove_all_upper(void)
{
	protocol_layer *tmp_layer;

	D_MISC("remove_all_upper: 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");
}

void 
init_flow(flow *f)
{
	D_MISC("init_flow\n");
	f->type = 0x3;
	f->len = 22;
	f->flags = 0;
	f->service = 0x01;
	f->token_rate = 0x00000000; /* No token rate specified, default */
	f->bucket_size = 0x00000000; /* No bucket needed, default */
	f->peak = 0x00000000; /* Unknown maximum bandwidth, default*/
	f->latency = 0xffffffff; /* Don't care. default */
	f->delay = 0xffffffff; /* Don't care. default */
}

/****************************************************************/
/*---------------------- 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)
{
#define FNC "l2cap_receive_data : "
	l2cap_packet *pkt;
	l2cap_con *con;

	D_RCV(FNC" got %d bytes on hci_handle : %d\n", len, hci_handle);
	PRINTPKT("l2cap_receive_data : ", data, len);

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

		pkt = (l2cap_packet *)data;

		/* 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(FNC"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(FNC"BIG PACKET ! (%d bytes) discard\n", len);
			hci_clear_buffer(hci_handle);
		}
		return;
	}  

	/* A whole frame is received */
	pkt = (l2cap_packet *)data;

	switch (pkt->cid) {
	case CIDSIG:
	
		/* Signalling channel */
		D_RCV("l2cap_receive_data : 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");
			return;
		}
		
                D_RCV("l2cap_receive_data: 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(CHAR2INT16(pkt->data[1], pkt->data[0]))->
			receive_data(con, pkt->data + 2, pkt->len - 2);
		break;
    
	default:
		
		/* Data channel */
		con = get_lcon(pkt->cid);
    
		if (con == NULL)
			return;

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

	/* free hci inbuffer */
	hci_clear_buffer(hci_handle); 
#undef FNC
}

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

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

	cmd = (struct sig_cmd *)(data + pos);
  
	D_RCV(FNC"received %d bytes\n", len);
	PRINTPKT(FNC"data", data, len);

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

	while (pos < (len)) {
    
		D_RCV(FNC"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);
			D_RCV("another command in same packet...(%d bytes)\n",
			      cmd->len);	
		}
	} 
#undef FNC		
}


void process_request(u16 hci_handle, struct sig_cmd *req)
{
#define FNC "process_request : "

	sig_conreq *conreq;
	sig_confreq *confreq;
	sig_discreq *discreq;
	l2cap_con *con = NULL;
	
	D_STATE(FNC"Got request : 0x%x id:%d\n",
		req->code, req->id);
	
	switch (req->code) {
		
	case SIG_CONREQ: {
                
                /* Request for connection */
		conreq = (sig_conreq *)req->data;
		
		D_STATE(FNC"Connection request\n");
		D_STATE(FNC"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
		*/
		
		if ((con = check_remote_cid(hci_handle, 
					  conreq->src_cid)) == NULL) {
			D_ERR(FNC"couldn't find l2cap connection\n");
			return;
		}
		
		if (PARANOIA_CHECKCON(con)) {
			D_ERR("%s l.%d NULL/magic failed\n", 
			      __FILE__, __LINE__);
			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;

		l2ca_connect_ind(con);
                
		/* 
		   FIXME - optionally send l2cap_connect_rsp_pnd
		*/
	}
	break; /* SIG_CONREQ*/
	
	case SIG_CONFREQ: {

                /* Request for configuration */
		confreq = (sig_confreq *)req->data;
		
		D_STATE(FNC"config request cid:%d flags: 0x%x\n", 
			confreq->dst_cid, confreq->flags);
		
		if ((con = get_lcon(confreq->dst_cid)) == NULL)
			return;
		
		switch (con->current_state) {
		case CLOSED:

			/* FIXME - send peer 
			   'l2cap_config_rsp_neg' */
		  
		  break;
			
		case CONFIG:

			/* Parse & set options in l2cap con */          
			parse_options(con, confreq->options,
				      req->len-4);

			con->sig_id_rcv = req->id;

			l2ca_config_ind(con);           
			break;

		case OPEN:

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

			ENTERSTATE(con, CONFIG);
			PRINTSTATE(con);
			
			parse_options(con, confreq->options,
					req->len-4);
			
			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("SIG_CONFREQ invalid state!\n");
			PRINTSTATE(con);
			break;
		}  
	}        
	break; /* SIG_CONFREQ*/
			
	case SIG_DISCREQ: {
         
		discreq = (sig_discreq *)req->data;

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

		if ((con = get_rcon(discreq->src_cid)) == NULL) {
			D_STATE("not connected... discard!\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(FNC"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(FNC"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: {
		D_STATE(FNC"info request \n");
		/* IMPLEMENTATION SPECIFIC */
	}    
	break;
       
	case SIG_RESERVED:
		D_STATE(FNC"Reserved !\n");
		break;
        
	default: {
			/* Not a valid command */
			DSYS(FNC"Invalid command (code 0x%x)s\n", req->code);
			l2cap_cmdrej(con->hci_con_handle, CMDREJ_NOTUNDERSTOOD,
				     NULL, 0);
		}
		break;
	}
#undef FNC
}

void process_response(u16 hci_handle, struct sig_cmd *rsp)
{
#define FNC "process_response :"

	sig_conrsp *conrsp;
	sig_confrsp *confrsp;
	sig_discrsp *discrsp;
	sig_echo_pkt *echo;
	sig_cmdreject *cmdreject;
	l2cap_con *con = NULL;
	s32 failure = 0;


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

	switch (rsp->code) {
	case SIG_CMDREJECT: {

		D_STATE(FNC"Command reject - ");
         
		cmdreject = (sig_cmdreject*)rsp->data;
		switch  (cmdreject->reason) {
		case 0:
			D_STATE(FNC"Command not understood\n"); 
			break;
           
		case 1:
			D_STATE(FNC"Signalling MTU exceeded\n");
			break;
           
		case 2:
			D_STATE(FNC"Invalid CID in request\n");
			break;
           
		default:
			D_STATE(FNC"Not recognized cmd reject reason\n");
			break;
		}

		if (rsp->len>2)
			PRINTPKT(FNC"optional data : ", 
				 cmdreject->data, rsp->len-2);
	}
	break;
       
	case SIG_CONRSP: { 

		/* client */
		D_STATE(FNC"Got connection response\n");
		conrsp = (sig_conrsp *)rsp->data;
         
		/* find connection */
		if ((con = get_lcon(conrsp->src_cid)) == NULL) {
			D_ERR(FNC"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(FNC"ID doesn't match !\n");
			return;
		}
         
		/* FIXME - disable RTX timer */
         
		switch (conrsp->result) {
		case RES_SUCCESS:
			ENTERSTATE(con, CONFIG);
			PRINTSTATE(con);
			l2ca_connect_cfm(con, conrsp->result);
			break;
           
		case RES_PENDING:

			/* FIXME - 
			   Send l2ca_connect_rsp_pnd()
			   Disable RTX timer and start ERTX
			*/
			D_STATE(FNC"connection pending (fixme)\n");
			break;
           
		case RES_PSMNEG:
			/* FIXME */
			D_STATE(FNC"connection refused, psm 0x%x not supp\n", 
				con->psm);
			failure = 1;
			break;
           
		case RES_SECNEG:
			/* FIXME */
			D_STATE(FNC"connection refused, security block\n");
			failure = 1;
			break;
           
		case RES_NOSRC:
			/* FIXME */
			D_STATE(FNC"connection refused, no resources\n");
			failure = 1;
			break;
           
		default:
			D_ERR(FNC"SIG_CONRSP\n");
			break;
		}
         
		if (failure) {

			/* Connection refused 
			   FIXME -
			   Disable RTX/ERTX timers
			   (New state : CLOSED)
			*/
			l2ca_connect_cfm(con, conrsp->result);
			
			ENTERSTATE(con, CLOSED);
			delete_con(con);
		}
	}      
	break;
       
	case SIG_CONFRSP: {

		D_STATE(FNC"Got configuration response\n");
		confrsp = (sig_confrsp *)rsp->data;
		PRINTPKT(FNC"config response", rsp->data, rsp->len);

		/* check that remote CID is in list */
		if ((con = get_lcon(confrsp->src_cid)) == NULL)
			return;

		if (con->current_state != CONFIG) {
			D_ERR(FNC"SIG_CONFRSP invalid state\n");
			PRINTSTATE(con);
			return; 
		}
			       
		/* match id with request */
		if (!id_matched(con, rsp->id)) {
			D_ERR(FNC"ID doesn't match !\n");
			return;
		}

		switch (confrsp->result) {
	
		case CONF_SUCCESS:
         
			/* FIXME - disable RTX timer */
			/* FIXME - should check that we have received 
			   l2cap_config_req & replied pos. If we are server 
			   we can now proceed to OPEN (will do for now) */
           
			con->conf_req_ready = TRUE;

  			if (!con->conf_rsp_ready)
  			  {
  			    /* Upper layers still haven't replied pos */
			    DSYS("Still haven't replied pos on other sides conf req\n");
			    return ;
  			  }
			else {				
					
				ENTERSTATE(con, OPEN);
				PRINTSTATE(con);
				DSYS("l2cap channel (%d,%d) [%s] connected\n", 
				     con->local_cid, con->remote_cid, 
				     psm2str(con->psm));
				
				/* notify upper layers that we successfully
				   opened a connection ! */
				l2ca_config_cfm(con, confrsp->result);
			}
			return;
           
		case CONF_FAILURE:
			D_STATE(FNC"config failure, unacceptable params\n");
			break;
           
		case CONF_REJ:
			D_STATE(FNC"connection refused, no reason\n");
			break;
           
		case CONF_UNKNOWN:
			D_STATE(FNC"connection refused, unknown options\n");
			break;
            
		default:
			D_ERR(FNC"SIG_CONFRSP\n");
			break;
		}
     
		/* Failure */
		/* FIXME - disable RTX timer */
		if (con->current_state != CONFIG)
			D_ERR(FNC"SIG_CONFRSP invalid state\n");
		PRINTSTATE(con);

		/* FIXME - For now we simply disconnect if params failed */
		l2ca_config_cfm(con, confrsp->result);
		l2ca_disconnect_req(con);         

		/* FIXME - should stay in CONFIG state and renegotiate 
		   according to spec */
	}
	break;
        
	case SIG_DISCRSP: {
	
		D_STATE(FNC"Got disconnect response\n");
		discrsp = (sig_discrsp *)rsp->data;
		PRINTPKT(FNC"disconnect response", rsp->data, rsp->len);
			       
		/* find connection */
		if ((con = get_lcon(discrsp->src_cid)) == NULL)
			return;
         
		if (con->current_state != W4_L2CAP_DISCONNECT_RSP) {
			D_ERR(FNC"SIG_DISCRSP invalid state\n");
			PRINTSTATE(con);
			return; 
		}
	       	       
		/* match id with request */
		if (!id_matched(con, rsp->id)) {
			D_ERR(FNC"ID doesn't match !\n");
			return;
		}

		/* FIXME - disable RTX */
		l2ca_disconnect_cfm(con);

	}
	break;
       
	case SIG_ECHORSP: { 
		l2cap_con *tempcon;
		
		D_STATE(FNC"Got echo response\n");		

		if ((tempcon = get_con_hcihdl(hci_handle))==NULL) {
			D_STATE("Echo rsp : could not find connection\n");
			return;
		}
		
		echo = (sig_echo_pkt *)(rsp->data);
         
		if (rsp->len >sizeof(sig_echo_pkt))
			PRINTPKT(FNC"optional data ", 
				 echo->data, rsp->len-sizeof(sig_echo_pkt));
		l2ca_ping_rsp(echo->data,  
			      rsp->len-sizeof(sig_echo_pkt),
			      tempcon->remote_bd);

		/* for now we disconnect every time  a successful 
		   ping was made if no others are using this handle */
		
		if (count_con(hci_handle)==1) {
			printk("disconnect after doing ping...\n");
			lp_disconnect(hci_handle);
		}
		
	}    
	break;
       
	case SIG_INFORSP: {  

		/* FIXME */
		D_STATE(FNC"Got info response (fixme)\n");
	}    
	break;
       
	default: {
        
		/* Not a valid command */
		DSYS(FNC"Invalid command 0x%x\n", rsp->code);
		l2cap_cmdrej(con->hci_con_handle, CMDREJ_NOTUNDERSTOOD,
			     NULL, 0);
	}
       
	break;
	}
	
#undef FNC
}




/*******************************************************************/
/*-------------------------- EVENTS -------------------------------*/
/*******************************************************************/

/*

  Events are all incoming messages to the L2CA layer along with the
  timeouts. Events are partitioned into five categories :
  1. Indications and confirms from lower layers
  2. Signal requests and responses from peers
  3. L2CAP to L2CAP Data
  4. Requests and responses from upper layers
  5. Events caused by timers

 */

/*******************************************************************/
/* (E1) Lower layer to l2cap */
/****************************/

/* Indicates the lower protocol has successfully established connection */
void 
lp_connect_ind(BD_ADDR bd_addr)
{
	PRINTPKT("lp_connect_ind from :",bd_addr, 6);

        /* Check BD_ADDR */
        if ((bd_addr[0]==0) && (bd_addr[1]==0) && (bd_addr[2]==0) &&
            (bd_addr[3]==0) && (bd_addr[4]==0) && (bd_addr[5]==0)) {
		D_ERR("lp_connect_ind : no BD addr\n");
		return;
        }
 
	/* FIXME - add check with control block if this bd_addr is allowed */

	/* We are server and creates an l2cap connection object 
	   which we set hci handle when we received lp_connect_cfm */
  
	D_CON("lp_connect_ind: Accepting connection\n");
	l2cap_create_con(bd_addr);
	lp_connect_rsp(bd_addr,1);
}

/* is called when we accept a _new_ baseband connection */
void 
l2cap_create_con(BD_ADDR bd)
{
	l2cap_con *con;
	D_RCV("l2cap_create_con\n");	
	PRINTPKT("l2cap_create_con : bd ", bd, 6);

	/* create a new l2cap connection obj and insert it in list */

	/* HACK! - remote CID = 0 indicates that remote cid has not '
	   been set yet */

	con = create_con(0 /* not yet set*/ , get_cid(), 0/*not yet set*/);
	
	/* Check connection */
        if (con == NULL) {
		D_ERR("l2cap_create_con : no connection created");
		return;
        }
        
	con->link_up = TRUE;
	memcpy(con->remote_bd, bd, 6);

	/* have not received l2cap connection req yet */
	ENTERSTATE(con, CLOSED); 
	insert_con(con);
	return;
}


/* 
   Confirms the request to establish a baseband connection
*/

/* FIXME - add passing authentication challenge if auth is required 
   to establish the physical link */
  
void 
lp_connect_cfm(u8 *bd_addr, u32 status, u16 con_hdl)
{
	l2cap_con *con;
	D_STATE("lp_connect_cfm: %s (hci_handle : %d)\n", 
		get_err_msg(status), con_hdl);
	
	/* search for the corresponding l2cap connection */
	if ((con = get_con(bd_addr, CLOSED)) == NULL) {
		D_ERR("lp_connect_cfm : couldn't find l2cap con!\n");
		return;
	}
	
	if (status == 0) {
		
		/* pos cfm */
		con->hci_con_handle = con_hdl;
		con->link_up = TRUE;

		/* Check if a ping is to be sent */
		if (con->ping_sent) {
			D_STATE("baseband up, now send ping\n");
			l2cap_echo_req(con->hci_con_handle, NULL, 0);
			con->ping_sent=0; /* only send one / request  */
			return; /* don't try to connect l2cap... */
		}
		
		if (!(con->initiator)) {		
			
			D_STATE("we are server\n");
			/* now wait for a connection request */
		} else {
			
			D_STATE("we are client\n");
			PRINTPKT("lp_connect_cfm : HCI connected to ", 
				 bd_addr,6);
      
			ENTERSTATE(con, W4_L2CAP_CONNECT_RSP);
			PRINTSTATE(con);
			
#ifdef BTD_USERSTACK
			/* wait for 3coms HW while they are doing 
			   write_link_policy ?!? */
			printk("sleeping 2 sec... (3com fix)\n");	
			sleep(2);
#endif
			/* baseband is up, now initiate l2cap */
			l2cap_connect_req(con, con->psm);
		}
	} else {
		
			/* neg cfm */
		D_STATE("lp_connect_cfm : (neg) %s\n",get_err_msg(status));
		con->link_up = FALSE;
		
		/* don't notify upper layers if cfm originated from a ping */
		if (con->ping_sent==1) {
			/* Remove connection object */
			delete_con(con);
			return;
		}

		if (con->initiator) {
			/* fail any outstanding upper service connection requests */			
			l2ca_connect_cfm(con, status);
			/* Remove connection object */
			delete_con(con);
		}
	}
}
 
/* Indicates that one of the baseband connections has been shutdown */
void 
lp_disconnect_ind(u32 con_hdl)
{
	l2cap_con *con; 

	/* temp link down */
	DSYS("lp_disconnect_ind : Connection handle %d disconnected\n",
	     con_hdl);

	/* find & notify/remove l2cap connection(s) on this hci handle */

	show_list();

	/* FIXME - should flag phys link down here  */
	
	while (((con = get_con_hcihdl(con_hdl)) != NULL)) {

		D_STATE("l2cap connection (%d:%d) found on handle %d\n", 
			con->local_cid, con->remote_cid, con_hdl);
		D_STATE("now closing it...\n");
		
		PRINTSTATE(con);
    
		/* flag phys link as down */
		con->link_up = FALSE;
		
		/* if not connected yet simply remove it */
		if (con->current_state == CLOSED) {      
			D_STATE("connection closed, remove it!\n");
			delete_con(con);
		} else {
			ENTERSTATE(con, W4_L2CA_DISCONNECT_RSP);
			DSYS("closing l2cap con (%d,%d)\n",
			     con->local_cid, con->remote_cid);

			/* fixme up3 */

			/* notify upper layers that phys link is down */
			get_upper(con->psm)->disc_ind(con);
		}
	}
   
	D_CON("lp_disconnect_ind : no more l2cap cons on this handle\n");

	/* flush old buffers waiting to be sent on this handle */
	btmem_flushhandle((u16)con_hdl);
	//btmem_reset();
	return;
}

/* FIXME - lp_qos_violation_ind() */
/* FIXME - lp_qos_cfm() */

 
/********************************************************************/
/* (E2) l2cap to l2cap signalling */
/*********************************/

/* is handled in signal_handler() */

/******************************************************************/
/* (E3) L2CAP to L2CAP Data*/
/**************************/


void 
process_frame(l2cap_con *con, u8 *data, u32 len)
{  
	PRINTPKT("process_frame : ", data, len);

	
	if (len > (con->local_mtu)) {
		DSYS("l2cap process_frame : len > local_mtu (%d/%d)\n", 
		     len- L2CAP_HDRSIZE, con->local_mtu);
		l2cap_cmdrej(con->hci_con_handle, CMDREJ_MTUEXCEEDED,
			     NULL, 0);
		return;
	} 

	get_upper(con->psm)->receive_data(con, data, len);
}


/**************************************************************/
/* (E4) Upper layers to l2cap */
/*****************************/

s32 
l2ca_connect_req(BD_ADDR bd, u16 psm)
{
	l2cap_con *con;
	l2cap_con *tmpcon;
	s32 i;
	u8 rev_bd[6];

	D_RCV("l2cap_connect_req\n");
	PRINTPKT("l2ca_connect_req : sent to bd ",bd, 6);

	/* Check bd_addr */
	if ((bd[0]==0) && (bd[1]==0) && (bd[2]==0) && (bd[3]==0) &&
	    (bd[4]==0) && (bd[5]==0)) {
		D_ERR("l2ca_connect req : No BD-addr\n");
		return -1;
	}

	/* Check PSM */
	if ((psm % 2) == 0) {
		D_ERR("l2ca_connect_req : incorrect PSM value\n");
		return -1;
	}
	if ((psm > MAX_PSM) && (psm < MIN_DYNAMIC_PSM)) {
		D_ERR("l2ca_connect_req : value of PSM preserved\n");
		return -1;
	}
	if (psm > MAX_DYNAMIC_PSM) {
		D_ERR("l2ca_connect_req : PSM value not valid\n");
		return -1;
	}
	

/* bd is big endian, reverse byte order to little endian */
	for (i = 0;i<6;i++) {
		rev_bd[5-i] = bd[i];
	}

	tmpcon = get_con(rev_bd, ANY_STATE);
  
	/* Create connection object, hci_handle is set from lp_connect_cfm */
	con = create_con(0/* not yet set */, get_cid(), 0/* not yet set */);
	/* Check connection */
        if (con == NULL) {
		D_ERR("l2ca_connect_req : no connection created\n");
		return -1;
        }

	memcpy(con->remote_bd, rev_bd, 6);

	/* we are client */
	con->initiator = 1; 

	/* we must send and receive a configure req */
	con->conf_req_ready=0;
	con->conf_rsp_ready=0;

	con->psm = psm;
	con->flush_timeout = FLUSHTIMEOUT_DEFAULT;

	/* is set when receiving a pos lp_connect_cfm */
	con->link_up = 0; 
  
	insert_con(con);

	/* if this physical bd link exist for another l2cap con, use that!*/
	if (tmpcon) {

		D_STATE("l2ca_connect_req : hci handle already exist.\n");
		con->hci_con_handle = tmpcon->hci_con_handle;
		con->link_up = 1;
		ENTERSTATE(con, W4_L2CAP_CONNECT_RSP);
		PRINTSTATE(con);

		l2cap_connect_req(con, psm);

		return 0;
	} else {
		D_STATE("l2ca_connect_req : create new baseband link\n");
		/* create baseband connection, l2cap_connect_req is called 
		   from lp_connect_cfm */
		return lp_connect_req(con->remote_bd);  
	}
}

/* params determines how we would like to receive data */
s32 
l2ca_config_req(l2cap_con *con, u16 in_mtu, flow *outflow, 
                u16 flush_timeout, u16 link_to)
{
  
	if (PARANOIA_CHECKCON(con)) {
		D_ERR("%s l.%d NULL/magic failed\n", __FILE__, __LINE__);
		return -1;
	}

	D_STATE("l2ca_config_req remote cid %d, in_mtu %d\n", 
		con->remote_cid, in_mtu);

	/* should already be in CONFIG state */

	if (con->current_state == CONFIG) {  

		/* local mtu is set in l2cap_config_req */

		return l2cap_config_req(con, in_mtu, outflow, 
					flush_timeout, link_to);  

	} else if (con->current_state == CLOSED) {

		l2ca_config_cfm(con, CONF_REJ);
		return -1;
	} else if (con->current_state == OPEN) {

		printk("l2ca_config_req: state OPEN, reconfiguration !\n");

		/* Fixme - 
		   Is data automatically suspended while reconfiguring  ?
		*/

		ENTERSTATE(con, CONFIG);
		PRINTSTATE(con);
		
		/* local mtu is set in l2cap_config_req */

		con->conf_req_ready = FALSE;
		con->conf_rsp_ready = FALSE;
		
		return l2cap_config_req(con, in_mtu, outflow, 
					flush_timeout, link_to);  
		return -1;
	} else {

		D_ERR("l2cap_config_req : invalid state\n");
		PRINTSTATE(con);
		return -1;
	}
}

s32 
l2ca_disconnect_req(l2cap_con *con)
{
	s32 result = -1;

	if (PARANOIA_CHECKCON(con)) {
		D_ERR("%s l.%d NULL/magic failed\n", __FILE__, __LINE__);
		return -1;
	}

	D_STATE("l2ca_disconnect_req remote cid : %d\n", con->remote_cid);

	if (con->current_state == OPEN || con->current_state == CONFIG) {

		result = l2cap_disconnect_req(con);

		/*FIXME - start RTX timer */

		ENTERSTATE(con, W4_L2CAP_DISCONNECT_RSP);
		PRINTSTATE(con);
	} else {
		D_ERR("l2ca_disconnect_req : Invalid state !\n");
		return -1;    
	}
	return result;
}


/* Response from upper layer to the indication of a channel to a remote 
   device */
s32 
l2ca_connect_rsp(l2cap_con* con, u16 response, u16 status)
{
	s32 result;

	D_STATE("l2ca_connect_rsp\n");

	if (PARANOIA_CHECKCON(con)) {
		D_ERR("%s l.%d NULL/magic failed\n", __FILE__, __LINE__);
		return -1;
	}

	/* optionally send l2cap_connect_rsp_pnd() */  

	if (con->current_state != W4_L2CA_CONNECT_RSP) {
		D_ERR("l2ca_connect_rsp : invalid state \n");
		PRINTSTATE(con);
		return -1;
	}
  
	result = l2cap_connect_rsp(con, response, status);
	
	if (response == RES_SUCCESS) {  

		ENTERSTATE(con, CONFIG);
		PRINTSTATE(con);

	} else if (response >= RES_PSMNEG) {

		/* 'l2ca_connect_rsp_neg()' */
		ENTERSTATE(con, CLOSED);
		PRINTSTATE(con);

		/* remove connection */
		delete_con(con);
	}  
	return result;
}


/* fixme -- use inparams... */
s32 
l2ca_config_rsp(l2cap_con* con, u32 out_mtu, flow *in_flow, s32 ok)
{
#define FNC "l2ca_config_rsp : "
	s32 result;

	if (PARANOIA_CHECKCON(con)) {
		D_ERR("%s l.%d NULL/magic failed\n", __FILE__, __LINE__);
		return -1;
	}

	D_STATE(FNC"remote cid %d remote mtu %d\n", 
		con->remote_cid, con->remote_mtu);

	if (con->current_state != CONFIG) {
		D_ERR("l2ca_config_rsp : invalid state\n");
		PRINTSTATE(con);
		return -1;
	}

	if (ok) {
		/* upper layers responded OK */
		con->conf_rsp_ready = TRUE;

		result = l2cap_config_rsp(con, out_mtu, in_flow);

		/* check if we have sent a configure request yet */
		if (con->conf_req_ready) {
			/* all done, proceed to OPEN */
			ENTERSTATE(con, OPEN);
			PRINTSTATE(con);
			DSYS("l2cap channel (%d,%d) [%s] connected\n", 
			     con->local_cid, con->remote_cid, 
			     psm2str(con->psm));
			
			/* notify upper layers that we are opened */
			l2ca_config_cfm(con, RES_SUCCESS);

		} else {
		  DSYS(FNC"Not yet a pos rsp on config_req\n");
		}
	} else {
		/* FIXME - send l2cap_config_rsp_neg */
		D_STATE(FNC"params not accepted by us (fixme) !\n");
		return -1;
	}
	return result;
#undef FNC
}

s32 
l2ca_disconnect_rsp(l2cap_con* con)
{
	s32 result = -1;
	D_STATE("l2ca_disconnect_rsp\n");

	if (PARANOIA_CHECKCON(con)) {
		D_ERR("%s l.%d NULL/magic failed\n", __FILE__, __LINE__);
		return -1;
	}

	show_con("disconnecting : ", con);

	if (con->current_state == W4_L2CA_DISCONNECT_RSP) {

		/* if we got a link down unexpectedly we cannot send 
		   back a response, send nothing... */
		if (con->link_up)
			result = l2cap_disconnect_rsp(con);
		else {
			/* tell upper layers that disconnection was 
			   'successful' */
			result=0;
			D_STATE("baseband link down, cannot send rsp\n");
		}
		
		ENTERSTATE(con, CLOSED);		

		PRINTSTATE(con);
 
		DSYS("l2cap channel (%d,%d) [%s] disconnected\n", 
		     con->local_cid, con->remote_cid, psm2str(con->psm));

		if (count_con(con->hci_con_handle) > 1) {
			delete_con(con);     
		}
		/* If this con is the last with this hci_hdl, it is deleted 
		   when the baseband link goes down */
	} else {
		D_ERR("l2ca_disconnect_rsp : invalid state !\n\n");
		PRINTSTATE(con);
	}
	return result;    
}

/*******************************************************************/
/* (E5) Timer events */
/********************/

/* FIXME !*/
/* RTX */
/* ERTX */

/*******************************************************************/
/*-------------------------- ACTIONS ------------------------------*/
/*******************************************************************/

/*
  
  Actions are partitioned into five categories:
    1. l2cap to lower layers
    2. l2cap to l2cap signalling
    3. l2cap to l2cap data transmission
    4. l2cap to upper layers
    5. setting timers
    
 */

/*******************************************************************/
/* (A1) l2cap to lower layers */
/*****************************/

/* FIXME - lp_qos_req() */

/*******************************************************************/
/* (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;
	s32 payload_len;

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

	payload_len = SIGCMD_HDRSIZE + CON_REQSIZE;  

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

	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 = con->psm;
	req->src_cid = con->local_cid;
  
	cmd->code = SIG_CONREQ;
	cmd->id = set_id(con); /* Sets sig_id_sent in l2cap_con */
	cmd->len = CON_REQSIZE;

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

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

	/*FIXME - start RTX timer */

#ifndef L2CAP_USETIMERS
	return hci_send_data(tx);
#else
	return 0;
#endif

}

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;
	s32 payload_len;
	s32 opt_len=0;
	struct l2cap_option *opt;

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

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

	/* FIXME - use function parameters in packet data... */
	
	D_STATE("l2cap_config_req: inmtu : %d, local mtu : %d\n",
		in_mtu, con->local_mtu);

	if (in_mtu != con->local_mtu) {
		printk("Reconfigure our mtu to %d...\n", in_mtu);	
		con->local_mtu = in_mtu;
		opt_len+=4;
	}

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

	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) {

		printk("sending options...\n");
		/* if the negotiated mtu differs from the current, send it */
          
		opt=(struct l2cap_option*)(l2cap_buf->frame+L2CAP_HDRSIZE + 
			SIGCMD_HDRSIZE + CONF_REQSIZE);

		opt->type = 1; 
		opt->len = 2;
		*(opt->option_data) = (u8)(in_mtu & 0x00ff);
		*(opt->option_data+1) = (u8)(in_mtu>>8) & 0x00ff;
	}

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

	/* Signalling header */
	cmd->code = SIG_CONFREQ;
	cmd->id = set_id(con); /* Sets sig_id_sent in l2cap_con */
	cmd->len = 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_con_handle;
	tx->bc_flag = NO_BROADCAST;
	tx->cur_len = L2CAP_HDRSIZE + payload_len; /* Increased when lower
						      layers add header data */
	tx->flushed = 0;

	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;
	s32 payload_len;
 
	D_XMIT("l2cap_disconnect_req : 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("l2cap_disconnect_req : didn't get a valid tx buf\n");
		return -1;
	}

	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 = con->remote_cid;
	req->src_cid = con->local_cid;
  
	cmd->code = SIG_DISCREQ;
	cmd->id = set_id(con); /* Sets sig_id_sent in l2cap_con */
	cmd->len = DISC_REQSIZE;

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

	return hci_send_data(tx);

}

s32 
l2cap_echo_rsp(s32 hci_hdl, s32 id, 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_echo_pkt *rsp;
	s32 payload_len;

	D_XMIT("l2cap_echo_rsp\n");

	payload_len = SIGCMD_HDRSIZE + opt_len;

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

	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 = 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 */  
	tx->flushed = 0;

	return hci_send_data(tx);
}

s32 
l2cap_echo_req(s32 hci_hdl, 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_echo_pkt *req;
	s32 payload_len;

	payload_len = SIGCMD_HDRSIZE + sizeof(sig_echo_pkt) + opt_len;

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

	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 = 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 when lower
						      layers add header data */
	tx->flushed = 0;

	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;
	s32 payload_len;

	payload_len = SIGCMD_HDRSIZE + sizeof(sig_cmdreject) + opt_len;

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

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

	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 = sizeof(sig_cmdreject) + opt_len;

	/* FIXME: set the reason parameter */

	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 */
	tx->flushed = 0;

	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;
	s32 payload_len;
  
	D_XMIT("l2cap_connect_rsp : 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("l2cap_connect_rsp : didn't get a valid tx buf\n");
		return -1;
	}

	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 = con->local_cid; /* sending end */
	rsp->src_cid = con->remote_cid; /* receiving end */
	rsp->result = response;
	rsp->status = status;
  
	cmd->code = SIG_CONRSP;
	cmd->id = con->sig_id_rcv; /* Send back same id as received on request*/
	cmd->len = CON_RSPSIZE;

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

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

	return hci_send_data(tx);  
};
 
s32 
l2cap_config_rsp(l2cap_con* con, u32 out_mtu, 
		 flow *in_flow)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_confrsp *rsp;
	s32 payload_len;

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

	/* FIXME - No options for now, i.e we don't negotiate anything */  

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

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

	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	rsp = (sig_confrsp*)(l2cap_buf->frame + L2CAP_HDRSIZE + SIGCMD_HDRSIZE);
  
	/* Now fill in header fields */  
	rsp->src_cid = con->remote_cid;

	/* FIXME - WHERE ARE THIS DETERMINED ??? */
	rsp->flags = 0; /* No more config responses to follow */

	rsp->result = CONF_SUCCESS;
  
	cmd->code = SIG_CONFRSP;
	cmd->id = con->sig_id_rcv; /* Send back same id as received on request*/
	cmd->len = CONF_RSPSIZE;


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

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

	return hci_send_data(tx);

};

s32 
l2cap_disconnect_rsp(l2cap_con* con /*FIXME - , u16 status*/)
{
	bt_tx_buf *tx; 
	l2cap_tx_buf *l2cap_buf;  /* Entire l2cap frame + lower layer hdrs */
	sig_cmd *cmd;
	sig_discrsp *rsp;
	s32 payload_len;
 
	payload_len = SIGCMD_HDRSIZE + sizeof(sig_discrsp);

	D_XMIT("l2cap_disconnect_rsp : rcid %d\n", con->remote_cid);
	tx = subscribe_bt_buf(sizeof(l2cap_tx_buf) + L2CAP_HDRSIZE+payload_len);
	if (!tx) {
		D_ERR("l2cap_disconnect_rsp : didn't get a valid tx buf\n");
		return -1;
	}

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

	cmd = (sig_cmd*)(l2cap_buf->frame + L2CAP_HDRSIZE);
	rsp = (sig_discrsp*)(l2cap_buf->frame + L2CAP_HDRSIZE + SIGCMD_HDRSIZE);
    
	/* Now fill in header fields */
	rsp->dst_cid = con->local_cid;
	rsp->src_cid = con->remote_cid;
  
	cmd->code = SIG_DISCRSP;
	cmd->id = con->sig_id_rcv; /* Send back same id as received on req */
	cmd->len = sizeof(sig_discrsp);

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

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

	return hci_send_data(tx);
}


 
/*******************************************************************/
/* (A3) l2cap to l2cap data transmission */
/*****************************************/

s32 
l2cap_send_data(bt_tx_buf *tx, l2cap_con *con)
{
	l2cap_tx_buf *l2cap_buf;
  
	D_XMIT("l2cap_send_data : hdl : %d, rcid : %d, len:%d \n",
	       con->hci_con_handle, con->remote_cid, tx->cur_len);
	
	if (PARANOIA_CHECKCON(con))
	{
		D_ERR("%s l.%d NULL/magic failed\n", __FILE__, __LINE__);
		return -1;
	}

	if (tx->cur_len > con->remote_mtu)
	{
		D_ERR("%s tries to send more than remote mtu, ignore\n", 
		      psm2str(con->psm));
	    
		D_ERR("SETTING FLUSHED ON THIS BUFFER !!!\n");
		tx->flushed=1;
	    
		return -1;
	}

	/* Only send on an OPEN channel */
	if (con->current_state!=OPEN)
	{
		D_ERR("l2cap_send_data : not in open state\n");
		D_ERR("SETTING FLUSHED ON THIS BUFFER !!!\n");
		tx->flushed=1;
		return -1;
	}
	
	l2cap_buf = (l2cap_tx_buf *)(tx->data);

	SET_L2CAP_HDR(l2cap_buf->frame, tx->cur_len, con->remote_cid);
  
	/* pb_flag is set from hci_send_data */
	tx->hci_hdl = con->hci_con_handle;
	tx->bc_flag = NO_BROADCAST;

	tx->cur_len += L2CAP_HDRSIZE; /* At this stage cur_len indicates
				       payload len of l2cap i.e an entire
				       frame of upper layers*/
	tx->flushed = 0;

	PRINTPKT("l2cap_send_data : ", l2cap_buf->frame, tx->cur_len);

	return hci_send_data(tx);
}

/*******************************************************************/
/* (A4) l2cap to upper layers */
/*****************************/

void 
l2ca_connect_ind(l2cap_con *con) 
{
	D_STATE("l2ca_connect_ind : remote cid : %d psm 0x%x\n", 
		con->remote_cid, con->psm);


	if (get_upper(con->psm) == &default_protocol) {
		D_STATE("l2ca_connect_ind : PSM not registered\n");
		l2ca_connect_rsp(con, RES_PSMNEG, STAT_NOINFO);
		return;
	}
  
	get_upper(con->psm)->con_ind(con);
}

void 
l2ca_connect_cfm(l2cap_con *con, s32 result)
{
	D_STATE("l2ca_connect_cfm : rCID %d, result %d\n", 
		con->remote_cid, result);

	if (con->psm > MAX_PSM)
		return;
  
	if (!con->initiator) {
		D_ERR(" l2ca_connect_cfm : server should not receive this message\n");
		return;
	}

	get_upper(con->psm)->con_cfm(con, result); 
}


void 
l2ca_config_ind(l2cap_con* con)
{  
	D_STATE("l2ca_config_ind : remote cid : %d\n", con->remote_cid); 
  
	get_upper(con->psm)->conf_ind(con); 
}


/* according to spec this could be sent to upper layers after 
   receiving a config req but some stacks send their conf req 
   before sending their response we don't send cfm upwards
   until we really reached OPEN state. */
void 
l2ca_config_cfm(l2cap_con *con, s32 result)
{
	D_STATE("l2ca_config_cfm : remote cid : %d result %d\n", 
		con->remote_cid, result);

	get_upper(con->psm)->conf_cfm(con, result); 
}

void 
l2ca_disconnect_ind(l2cap_con *con)
{
	D_STATE("l2ca_disconnect_ind \n");

	get_upper(con->psm)->disc_ind(con);
}


void 
l2ca_disconnect_cfm(l2cap_con *con)
{
	int tmp_hdl;

	D_STATE("l2ca_disconnect_cfm : remote cid : %d\n", con->remote_cid);
  
	/* tell upper layers that connection is down */
	get_upper(con->psm)->disc_cfm(con);

	tmp_hdl=con->hci_con_handle;

	ENTERSTATE(con, CLOSED);
	PRINTSTATE(con);

	DSYS("l2cap channel (%d,%d) [%s] disconnected\n", 
	     con->local_cid, con->remote_cid, psm2str(con->psm));

	/* remove l2cap connection */
	delete_con(con);

	if (count_con(tmp_hdl) == 0) {
		DSYS("l2ca_disconnect_cfm : (C) no more l2cap connections\n");
		DSYS("shutdown baseband\n");
		lp_disconnect(tmp_hdl);
	}
}

s32 l2ca_ping(BD_ADDR bd)
{
	l2cap_con *con;
	s32 i ;
	u8 rev_bd[6];
	
	D_XMIT("Sending ping\n");
	PRINTPKT("Ping destination : bd ", bd, 6) ;
	
/* up3 --remove */
	/* bd is big endian, reverse byte order to little endian */
	for (i = 0;i<6;i++) {
		rev_bd[5-i] = bd[i];
	}
	
	con = get_con(rev_bd, ANY_STATE);

	if (!con)
	{
		D_STATE("l2ca_ping : create new baseband link\n");
		con = create_con(0/* not yet set */, 
				 get_cid(), 0/* not yet set */);
                if (con == NULL)
                {
			D_ERR("l2ca_ping : no connection created");
			return -1;
                }

		memcpy(con->remote_bd, rev_bd, 6);
		con->link_up = 0;
		con->ping_sent = 1;
		con->initiator = 1;
		insert_con(con);
		/* l2cap_echo_req is called in lp_connect_cfn */
		return lp_connect_req(con->remote_bd);
	}
	else
	{
		/* use existing handle */
		D_STATE("l2ca_ping : using existing handle\n");
		con->ping_sent=1;
		l2cap_echo_req(con->hci_con_handle, NULL, 0);
		return 0 ;
	}
}

void l2ca_ping_rsp(u8 *data, u32 len, BD_ADDR remote_bd)
{
#ifdef BTD_USERSTACK
	printf("Ping : got response from %02x:%02x:%02x:%02x:%02x:%02x\n\n", 
	       remote_bd[5], remote_bd[4], 
	       remote_bd[3], remote_bd[2], 
	       remote_bd[1], remote_bd[0]);
	if (len>0)
		print_data("ping data : ", data, len);
#else
	/* Fixme - indicate to usermode requester */
	D_STATE("ping : got response\n\n");
#endif
}


/*******************************************************************/
/* (A5) setting timers */
/***********************/

#ifdef L2CAP_USETIMERS

#ifdef __KERNEL__

/* used to resend lost l2cap commands */
void do_cmd_backup(l2cap_con *con, u8 type, u16 in_mtu, 
		   u16 flush_timeout, u16 link_to, flow *outflow)
{
	con->last_cmd.type = type; /* type of request */ 
	con->last_cmd.nbr_resends = 0;
	con->last_cmd.in_mtu = in_mtu;
	con->last_cmd.flush_timeout = flush_timeout;
	con->last_cmd.link_to = link_to;
	con->last_cmd.outflow = outflow;
}

/* change name to l2ca_timeoutind() ??!?! */

void
l2cap_rtx_timeout(unsigned long ptr)
{
  l2cap_con *con;
  printk("l2cap_rtx_timeout\n");
  /* now resend last command, where is this stored ? */
  con = (l2cap_con*)ptr;
  /* do some paranoia checks */

  if (PARANOIA_CHECKCON(con)) {
	  D_ERR("%s l.%d NULL/magic failed\n", __FILE__, __LINE__);
	  return;
  }
  
  con->last_cmd.nbr_resends++;

  /* now resend command */
  switch(con->last_cmd.type) {

  case SIG_CONREQ:
	  printk("resending connection request\n");
	  l2cap_connect_req(con, con->psm);
	  break;
	  
  case SIG_CONFREQ:
	  printk("resending configure request\n");
	  l2cap_config_req(con, 
			   con->last_cmd.in_mtu, 
			   con->last_cmd.outflow,
			   con->last_cmd.flush_timeout, 
			   con->last_cmd.link_to);
	  break;
	  
  case SIG_DISCREQ:
	  printk("resending disconnection request\n");
	  l2cap_disconnect_req(con);
	  break;
	  
  default:
	  printk("l2cap_rtx_timeout : unknown !\n");
	  break;
  }
}

void
start_rtx(l2cap_con *con, unsigned short timeout)
{
  printk("Starting RTX timer (%d sec)\n", timeout);

  init_timer(&con->rtx_timer);
  con->rtx_timer.function = l2cap_rtx_timeout;
  con->rtx_timer.data = (unsigned long)con;
  con->rtx_timer.expires = jiffies + timeout*HZ;
  add_timer(&con->rtx_timer);  
}



#if 0
void
start_ertx(l2cap_con *con, unsigned short timeout)
{
  printk("Starting ERTX timer (%s sec)\n");
  
  init_timer(&con->ertx_timer);
  hci_receive_timer.function = l2cap_ertx_timeout;
  hci_receive_timer.data = (unsigned long)con;
  hci_receive_timer.expires = jiffies + timeout*HZ;
  add_timer(&con->ertx_timer);  
}
void
l2cap_ertx_timeout(unsigned long ptr)
{
    printk("l2cap_ertx_timeout\n");
    /* now ...  */
    con = (l2cap_con*)ptr;
    /* do some paranoia checks */
    PARANOIA_CHECKCON(con);
}
#endif

#endif

#endif

/***************************************************************/
/*--------------------- MISCELLANEOUS ------------------------ */
/***************************************************************/

int l2ca_opened(l2cap_con *con)
{
	return (con->current_state==OPEN);
}

int l2ca_remote_conf_done(l2cap_con *con)
{
	return con->conf_rsp_ready;
}

int l2ca_local_conf_done(l2cap_con *con)
{
	return con->conf_req_ready;
}

/* Parse and set remote options in l2cap con */
void 
parse_options(l2cap_con *con, u8 *data, u32 len)
{
	u32 pos = 0;
	l2cap_option *opt;
  
	D_RCV("parse_options\n");

	print_data("parse_options : ", data, len);
	
	while (pos<len) {
		opt = (l2cap_option*)(data + pos);
		switch (opt->type) {
		case OPT_MTU:
			con->remote_mtu = *((u16*)opt->option_data);
			D_RCV("Setting remote mtu (%d:%d) to %d bytes\n",
			      con->local_cid, con->remote_cid, 
			      con->remote_mtu);

			/* FIXME - Set this as our MTU aswell... for now ! */
			D_RCV("setting local MTU to %d\n", con->remote_mtu);
			l2cap->mtu = con->remote_mtu;
			break;
      
		case OPT_FLUSH:
			con->flush_timeout = *((u16*)opt->option_data);
			D_RCV("parse_options : flush timeout %d ms\n", 
			      con->flush_timeout);
			break;
      
		case OPT_QOS:
			D_RCV("parse_options : QOS NOT SET IN STACK !!!!\n");
			print_flow((flow*)(opt->option_data));
			break;
      
		default:
			D_RCV("parse_options : Invalid type !\n");
			break;
		}
		pos += opt->len + 2; /* 2 bytes header */
	}
}

/* 
   Enter ID in IDlist when sending the request 
   When receiving response check id against registered l2cap_con->sig_id_sent
*/

/* Sets id and returns value */
u8 
set_id(l2cap_con* con)
{
	if (l2cap->id_count > 255) /*Wrap*/
		l2cap->id_count = 0;
	return con->sig_id_sent = l2cap->id_count++;
};

s32 
id_matched(l2cap_con* con, u8 rcv_id)
{
	return (con->sig_id_sent == rcv_id);
}

/* Get id and returns value */
u8 get_id()
{
	if (l2cap->id_count > 255) /*Wrap*/
		l2cap->id_count = 0;
	return l2cap->id_count++;
};


/*************************************/
/* Connection manager list functions */
/**************************************/

void 
init_con_list(void)
{
	con_list.first = 0;
	con_list.last = 0;
	con_list.cur = 0;
	con_list.count = 0;
	l2cap->cid_count = MIN_CID;
}

void 
free_list(void)
{
	D_CON("free_list : Freeing connection list\n");
	while (con_list.count)
		delete_con(con_list.first);
	D_CON("free_list : Connection list removed\n");
} 

l2cap_con* 
create_con(u16 hci_con_handle, CID lcid, CID rcid)
{ 
	l2cap_con* con;
	D_CON("Create_con rcid %d\n", rcid);
	if (con_list.count == MAX_CONLISTSIZE) {
		D_ERR("Connection list full!\n");
		return NULL;
	}
	/* Add check if already in list ?? or rely on that otherwhere ?*/
  
	/* Allocate new element */
	if ((con = (l2cap_con*)kmalloc(sizeof(l2cap_con), GFP_ATOMIC))<0)
	{
		D_ERR("create_con : could not allocate new l2cap con\n");
		return NULL;
	}
	
	reset_con(con);

	con->hci_con_handle = hci_con_handle;
	con->local_cid = lcid;
	con->remote_cid = rcid;	
	con->ping_sent = 0;

	con->next = con;
	con->prev = con;

	init_flow(&con->qos);

	return con;
}

void 
reset_con(l2cap_con* con)
{
	con->local_cid = 0;
	con->remote_cid = 0;
	con->local_mtu = MTU_DEFAULT;
	con->remote_mtu = MTU_DEFAULT;
	con->flush_timeout = FLUSHTIMEOUT_DEFAULT;
	con->current_state = CLOSED;
	con->conf_req_ready = TRUE; /* haven't started anything yet */
	con->conf_rsp_ready = TRUE; /* haven't started anything yet */
	con->initiator = FALSE; /* Other side initiated (default) */ 
	con->magic = L2CAP_CON_MAGIC;
	con->ping_sent=0;
}



/* Searches list for remote bd addr and returns connection if found
   and if the connection is in state STATE */

l2cap_con* get_con(BD_ADDR bd, s32 STATE)
{
	s32 i = 0;
  
	D_CON("get_con : look for connections in state %s (%d)\n", 
	      state_name[STATE], STATE);
	PRINTPKT("get_con : bd ", bd, 6);
  
	while (i<con_list.count) { 
		if (memcmp(bd, con_list.cur->remote_bd, 6) == 0) {

			if (STATE == ANY_STATE) {
				/* simply look for all connections with
				   BD address bd*/
				PRINTPKT("get_con : con_list bd ", 
					 con_list.cur->remote_bd, 6);

				if (PARANOIA_CHECKCON(con_list.cur)) {
					D_ERR("%s l.%d NULL/magic failed\n", 
					      __FILE__, __LINE__);
					return NULL;
				}  
				return con_list.cur;
			} else if (con_list.cur->current_state == STATE) {
				PRINTPKT("get_con : con_list bd ", 
					 con_list.cur->remote_bd, 6);
				return con_list.cur; 
			}
		}
    
		/* Not yet found, keep on searching */
		con_list.cur = con_list.cur->next;
		i++;
	}
	D_CON("get_con : connection not found\n");
	return NULL;
}


/*
  Searches list based on hci handle, if found it sets remote cid 
  and returns the connection. If hci handle is found but connection 
  not yet 'active' it simply returns the found con.
  If previously in use, a new l2cap con is created over the same 
  hci handle. */

l2cap_con* 
check_remote_cid(u16 hdl, CID remote_cid)
{
#define FNC "check_remote_cid : "
	s32 i = 0;
	l2cap_con *new_con;
  
	D_CON(FNC"hdl %d remote_cid %d\n", hdl, remote_cid);

	while (i<con_list.count) {
		D_CON(FNC"con_list.cur->hci_con_handle : %d\n", 
		      con_list.cur->hci_con_handle);
		if (con_list.cur->hci_con_handle == hdl) {

			D_CON(FNC"con handle found...\n");
			if (con_list.cur->remote_cid == 0) {

				/* This is the first l2cap connection over 
				   this hci handle (remote_cid is only set 
				   to 0 when receiving a new baseband 
				   connection) */

				D_CON(FNC"single connection / handle \n");
				con_list.cur->remote_cid = remote_cid;
				show_list();
				return con_list.cur;
			} else if ((con_list.cur->remote_cid >= 0x0040) 
				   && (con_list.cur->remote_cid <= 0xffff)) {

				/* at least one l2cap connection with this hci
				   handle exist now let's create another 
				   connection over same hci handle */

				D_CON(FNC"multiple connections / handle \n");
				new_con = create_con(hdl, get_cid(),remote_cid);
                                if (new_con == NULL) {
					D_ERR("l2cap_create_con : no connection created\n");
					return NULL;
                                }
                               
				new_con->link_up=TRUE;
				new_con->initiator=FALSE;
				/* set bd address in new connection */
				memcpy(new_con->remote_bd, 
				       con_list.cur->remote_bd, 6);

				insert_con(new_con);
				show_list();
				return new_con;
			}
      
		} 

		/* Not yet found, keep on searching */
		con_list.cur = con_list.cur->next;
		i++;
	}
  
	D_CON(FNC"connection not found\n");
	return NULL;
#undef FNC
}

/* Searches list for hci handle and returns first connection found */
l2cap_con* 
get_con_hcihdl(u16 hdl)
{
	s32 i = 0;
	D_CON("get_con_hcihdl : hdl %d\n", hdl);
  
	while (i<con_list.count) { 
		if (con_list.cur->hci_con_handle == hdl) {
			if (PARANOIA_CHECKCON(con_list.cur)) {
				D_ERR("%s l.d%d NULL/magic failed\n", 
				      __FILE__, __LINE__);
				return NULL;
			}
			return con_list.cur;
		}
    
		/* Not yet found, keep on searching */
		con_list.cur = con_list.cur->next;
		i++;
	}
	D_CON("get_con_hcihdl : no more connections on this handle\n");
	return NULL;  
}
 


/* Searches list for local cid and returns connection if found */
l2cap_con* 
get_lcon(CID lcid)
{
	s32 i = 0;
	D_CON("get_lcon : lcid %d con_list.count = %d\n", 
	      lcid, con_list.count);
  
	while (i<con_list.count) { 
		if (lcid == con_list.cur->local_cid) {
			
			if (PARANOIA_CHECKCON(con_list.cur)) {
				D_ERR("%s l.%d NULL/magic failed\n", 
				      __FILE__, __LINE__);
				return NULL;
			}
			return con_list.cur;
		}
		
		/* Not yet found, keep on searching */
		con_list.cur = con_list.cur->next;
		i++;
	}
	D_CON("get_lcon : connection lcid : %d not found\n", lcid);
	return NULL;
}

/* Searches list for remote cid and returns connection if found */
l2cap_con* 
get_rcon(CID rcid)
{
	s32 i = 0;
	D_CON("get_rcon : rcid %d con_list.count = %d\n", 
	      rcid, con_list.count);
  
	while (i<con_list.count) { 
		if (rcid == con_list.cur->remote_cid) {
			if (PARANOIA_CHECKCON(con_list.cur)) {
				D_ERR("%s l.%d NULL/magic failed\n", 
				      __FILE__, __LINE__);
				return NULL;
			}
			return con_list.cur;
		}
    
		/* Not yet found, keep on searching */
		con_list.cur = con_list.cur->next;
		i++;
	}
	D_CON("get_rcon : connection rcid : %d not found\n", rcid);
	return NULL;
}

void 
insert_con(l2cap_con *newcon)
{
	l2cap_con *oldcon;
	oldcon = con_list.first; /* mark oldcon first */
	show_con("insert_con()", newcon);
	if (!con_list.count) {
		/* Empty list */
		con_list.first = newcon; 
		con_list.cur = newcon;
		con_list.last = newcon;
		con_list.count++;
		D_CON("insert_con : now %d connections\n", con_list.count);
		newcon->next = con_list.first; /* Pos32 at itself */
		newcon->prev = con_list.first;
		return;
	}
  
	/* Not empty */
  
	newcon->next = oldcon;
	con_list.last->next = newcon;
	oldcon->prev = newcon;
	newcon->prev = con_list.last;
  
	con_list.cur = newcon; /* Set current to new element */
	con_list.first = newcon; /* Set first to new element */
	con_list.count++;
	D_CON("insert_con : now %d connections\n", con_list.count);
}

s32 
delete_con(l2cap_con* con)
{  
	if (PARANOIA_CHECKCON(con)) {
		D_ERR("%s l.%d NULL/magic failed\n",  __FILE__, __LINE__);
		return -1;
	}

	show_con("delete_con : ", con);

	/* Issue a warning if connection is not in CLOSED */
	if (con->current_state!=CLOSED)
		D_ERR("Trying to delete an active connection\n");

	if (con_list.count == 1) {
		/* Last element */
		con_list.count--;
		con_list.first = 0;
		con_list.cur = 0;
		con_list.last = 0;
		kfree(con);
		D_CON("Now connection list is empty !\n");
		return 0;
	}
  
	(con->prev)->next = con->next; /* Take it out */
	(con->next)->prev = con->prev;
  
	if (con_list.first == con) /* Update first pointer */
		con_list.first = con->next;
  
	if (con_list.last == con) /* Update last pointer */
		con_list.last = con->prev;
  
	con_list.cur = con->next; /* Update cur pointer */
	con_list.count--;
  
	kfree(con); 
  
	return 0;  
}

s32 
count_con(u16 hci_hdl)
{
	s32 i = 0;
	s32 sum = 0;
  
	while (i < con_list.count) { 
		if (con_list.cur->hci_con_handle == hci_hdl) {
			sum += 1;
		}
		con_list.cur = con_list.cur->next;
		i++;
	}
	D_CON("count_con : %d l2cap_con's found with hci_hdl:%d\n",sum,hci_hdl);
	return sum;
}

#if L2CAP_SELFTEST


s32 
remove_rcid(CID rcid) /* Searches for remote_cid */
{
	D_CON("remove_rcid\n");
  
	if (!con_list.count) {
		D_CON("del_con() no connections in list\n");
		return -1;
	}
	delete_con(get_rcon(rcid));
	D_CON("remote cid %d removed\n", rcid);
	return 0;
}

void 
test_conlist(void)
{
	/* Fixme - make it better... */

	insert_con(create_con(0, 0, 0));
	show_list();
	insert_con(create_con(1, 1, 1));
	show_list();
	insert_con(create_con(2, 2, 2));
	insert_con(create_con(3, 3, 3));
	insert_con(create_con(4, 4, 4));
	insert_con(create_con(5, 5, 5));
	insert_con(create_con(6, 6, 6));
	show_list();
	remove_rcid((u16)3);
	show_list();
	remove_rcid((u16)5);
	show_list();
	free_list();
	show_list();
	/* ... */
}
#endif

void 
show_con(const u8* head, l2cap_con* con)
{
	if (con) { 
		D_CON("%s (%d:%d) [%s] [%s] r_mtu[%d] C[%s] link_up[%s]\n",
		      head, con->local_cid, con->remote_cid,
		      state_name[con->current_state], psm2str(con->psm),
		      con->remote_mtu, bool2str[con->initiator], 
		      bool2str[con->link_up]);		
	} else {
		D_CON("show_con : NULL\n");
	}
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

/* Status functions for /proc files */

s32 l2cap_sprint_bd(u8 *buf, BD_ADDR bd) 
{
	/* stored in little endian format */
	return sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x", 
		       bd[5], bd[4], bd[3], bd[2], bd[1], bd[0]);
}

s32 l2cap_sprint_con(u8 *buf, l2cap_con* con)
{
	u16 pos=0;
	if (con) {
		pos += sprintf(buf + pos, "\nr_bd[");
		pos += l2cap_sprint_bd(buf + pos, con->remote_bd);
		pos += sprintf(buf + pos, " ]\n");

		pos += sprintf(buf + pos, 
			       "lcid[%d] rcid[%d] state[%s] psm[%s]\n",
			       con->local_cid, con->remote_cid, 
			       state_name[con->current_state], 
			       psm2str(con->psm));

		pos += sprintf(buf + pos,
			       "remote_mtu[%d] clnt[%s] link_up[%s]\n",
			       con->remote_mtu, bool2str[con->initiator], 
			       bool2str[con->link_up]);
	}
	return pos;
}

s32 l2cap_sprint_active(u8 *buf)
{
	u16 i;
	u16 pos=0;
	l2cap_con *tmp_con;  
	tmp_con = con_list.first;

	for (i = 0; i < con_list.count; i++) {
		pos+=l2cap_sprint_con(buf+pos, tmp_con);
		pos+=sprintf(buf+pos, "\n");
		tmp_con = tmp_con->next;
	}
	return pos;
}

s32 l2cap_sprint_status(u8 *buf)
{
	u16 pos=0;

	pos+=sprintf(buf+pos, "\n[L2CAP]\n\n");
	/* This device bd address */
	pos+=sprintf(buf+pos, "local bd :[");
	pos+=l2cap_sprint_bd(buf+pos, l2cap->my_bd);
	pos+=sprintf(buf+pos, "]\n");
	/* print active connections */
	pos+=l2cap_sprint_active(buf+pos);
	pos+=sprintf(buf+pos,"\n");

	/* other stuff ... */

	return pos;
}


/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

void 
show_list(void)
{
	s32 i;
	l2cap_con *tmp;
  
	tmp = con_list.first;
	D_CON("----------- show_list count :%d -----------\n", con_list.count);
  
	for (i = 0; i < con_list.count; i++) {
		show_con(" - ",tmp);
		tmp = tmp->next;
	}
  
	D_CON("-------------------------------------------\n\n\n");
}

void 
print_flow(flow *f)
{
	D_MISC("-------------------------------------------------------\n");
	D_MISC("flow:\n");
	D_MISC("type:%d flags:%d service:%d token_rate:%d\n",
	       f->type, f->flags, f->service, (int) f->token_rate);
	D_MISC("bucket_size:%d bytes peak:%d bps latency:%d ms delay:%d ms\n",
	       (int)f->bucket_size, (int)f->peak, (int)f->latency, 
	       (int)f->delay);
	D_MISC("-------------------------------------------------------\n");
}

/* Returns a free cid in range 0x0040 - 0xffff */
/* FIXME - search from MIN_CID every time ? */
u16 get_cid(void)
{
	CID cid;
	s32 i = 0;

	while (1) {
		if (l2cap->cid_count == MAX_CID)
			l2cap->cid_count = MIN_CID;

		cid = l2cap->cid_count++;
		if (get_lcon(cid) == NULL) /* free CID */
			return cid;  
		else if (i == MAX_CID)
			return 0;
		i++;
	}
}

/****************** END OF FILE l2cap.c *************************************/
