btmem.c

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

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

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

#ifdef __KERNEL__
#include <linux/malloc.h>
#include <linux/bluetooth/btmem.h>
#else
#include <stdlib.h>
#include "include/btmem.h"
#include "include/btcommon.h"
#include <string.h> /* memset */
#endif

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

#if BTMEM_DEBUG
#define D_MEM(fmt...) printk(BTMEM_DBG_STR fmt)
#define PRINTPKT(str, data, len) print_data(str, data, len)
#else
#define D_MEM(fmt...) 
#define PRINTPKT(str, data, len)
#endif

 /* Execute test function in init */
#define BTMEM_TEST 0

 /* Displays buffer usage */
#define BTMEM_SHOW_GRAPH 0

/* Panic if something goes wrong */
#define PANIC_AT_ERROR 0

/* discard all marked buffers */
#define BTMEM_FLUSH_ENABLED 1

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

#define BUFFER_USAGE_LEVEL(usage) ((usage*100)/(int)(BT_BUF_SIZE))

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

typedef struct bt_buf_main
{
	u8 *head; /* Start of data buffer */
	u8 *tail; /* End of data buffer */
	u8 *toss_tail; /* Toss tail when getting subscribed mem */
	u8 *free; /* After this pointer new data can be subscribed */
	u8 *send; /* This is where send function consumes data */
	s32 size; /* Total size of buffer */
	s32 count; /* Total number bytes subscribed */
	s32 nbr_bufs;
} bt_buf_main;

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

/* Finds a free area and subscribes it */
//static bt_tx_buf* subscribe(s32 head_free, s32 tail_free, s32 len, s32 wrap);

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

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

static bt_buf_main bt_buf = {NULL, NULL, NULL, NULL, NULL, 0, 0, 0};

/****************** FUNCTION DEFINITION SECTION *****************************/

void btmem_init(void)
{
	/* Allocate memory area */
	DSYS("Initialising BTMEM [%d bytes]\n", BT_BUF_SIZE);

	bt_buf.head = (u8*) kmalloc(BT_BUF_SIZE,GFP_KERNEL);
  
	/* Initiate main buffer object */
	bt_buf.tail = bt_buf.head + BT_BUF_SIZE;
	bt_buf.toss_tail = bt_buf.tail; /* Used when reading wrapped data */ 
	bt_buf.free = bt_buf.head;
	bt_buf.send = bt_buf.head; 
	bt_buf.size = BT_BUF_SIZE;
	bt_buf.count = 0;
	bt_buf.nbr_bufs = 0;
#if BTMEM_TEST
	btmem_test();
#endif
}
 
/* Returns success if deallocation was successful */

void btmem_shutdown(void)
{
	DSYS("Shutting down BTMEM\n");
	/* FIXME - Check that the stack is not in use */
	/* Deallocate memory area */
	if (bt_buf.head)
		kfree(bt_buf.head);
	bt_buf.head = NULL;
}

/* Is run whenever buffer is empty */

void 
btmem_reset()
{
	if (bt_buf.count)
		D_ERR("Resetting buffer and not empty !\n");
  
	bt_buf.toss_tail = bt_buf.tail; /* Used when reading out wrapped data*/
	bt_buf.free = bt_buf.head;
	bt_buf.send = bt_buf.head; 
	bt_buf.count = 0;
	bt_buf.nbr_bufs = 0;
}


/* Sets the flush flag for all buffers waiting to be sent on handle hdl.
   Later when calling get_bt_buf(), if the buffer found has flushed flag set
   the function unsubscribes it and fetches the next unflushed instead. */
void btmem_flushhandle(u16 hdl)
{
	u8 *pos = bt_buf.head;
	bt_tx_buf *tx;
	u32 i=0;
	D_MEM("btmem_flushhandle\n");

	cli();
	/* non wrapped buffer */
	if (bt_buf.free > bt_buf.send) { 
		/* read from send -> free */
		pos = bt_buf.send;
		
		while (pos < bt_buf.free) {
			tx = (bt_tx_buf *)(pos);

			if (tx->hci_hdl==hdl) {
				i++;
				tx->flushed=1;
			}
			pos += BT_TX_HDRSIZE + tx->subscr_len;
		}
	} else if (bt_buf.free!=bt_buf.send) { /* wrapped buffer */
	  	 /* send -> toss_tail, read from head -> free */
		pos = bt_buf.send;
		while (pos < bt_buf.toss_tail) {
			tx = (bt_tx_buf *)(pos);

			if (tx->hci_hdl==hdl) {
				i++;
				tx->flushed=1;
			}
			pos += BT_TX_HDRSIZE + tx->subscr_len;
		}
		pos = bt_buf.head; 
		while (pos < bt_buf.free) {
			tx = (bt_tx_buf*)(pos);

			if (tx->hci_hdl==hdl) {
				i++;
				tx->flushed=1;
			}
			pos += BT_TX_HDRSIZE + tx->subscr_len;
		}
	} else { /*empty buffer */
		sti();
		return;
	}
	
	D_MEM("btmem_flushhandle : flushed %d buffers\n", i);

	sti();
	/* get_bt_buf removes flushed buffers if located in 
	   queue head */
	get_bt_buf();
}


/* Subscribes one bt_tx_buf with send_len bytes in the data segment */

bt_tx_buf* subscribe_bt_buf(s32 send_len)
{
	bt_tx_buf *tx;
	s32 buf_len; /* Total size of bt_tx_object */
	u32 head_free;
	u32 tail_free; /* Number bytes available */

	cli();
	buf_len = send_len + BT_TX_HDRSIZE;  
	D_MEM(__FUNCTION__ ": buf_len %d\n", buf_len);

	/*  
	    'Normal' case
	    send        free
	    |            |
	    ---------------------------------------------
	    | head_free |XXXXXXXXXXXX|    tail_free       |
	    ---------------------------------------------
	    head                                           tail
	*/

	/* check for flushed buffers */

	get_bt_buf();

	if (bt_buf.free > bt_buf.send) {
		tail_free = (bt_buf.tail - bt_buf.free);
		head_free = (bt_buf.send - bt_buf.head);

		if (tail_free >= buf_len) {
			D_MEM(__FUNCTION__ ": subscribe in tail at pos %d\n", 
			      bt_buf.free - bt_buf.head);
			tx = (bt_tx_buf *)bt_buf.free;
			/* Don't touch send, only update free and count */
			bt_buf.free += buf_len;
			bt_buf.count += buf_len;
			bt_buf.nbr_bufs++;
		} else if (head_free >= buf_len) {  
			D_MEM("No room in tail, subscribe at head (WRAP!)\n");
			tx = (bt_tx_buf *)bt_buf.head;
			
			/* Toss rest of tail to avoid fragmenting */    
			bt_buf.toss_tail = bt_buf.free; 

			bt_buf.free = bt_buf.head+buf_len; /*Now send > free */
			bt_buf.nbr_bufs++;
			bt_buf.count += buf_len;
		} else {
			D_ERR(__FUNCTION__ ": Cannot subscribe %d bytes !\n", send_len);
			D_ERR(__FUNCTION__ ": Only %d available (non - fragmented)\n",
			      buf_write_room());

#if PANIC_AT_ERROR
			btmem_get_status(NULL);
			panic("lets stop here...\n");
#endif
			sti();
			return NULL;
		}
	} else if (bt_buf.free <= bt_buf.send) {
		/* 'Wrap' case */
		tail_free = (bt_buf.send - bt_buf.free);
		head_free = 0; /* head_free is not interesting since we cannot 
				  put data here even if it would fit since we 
				  already have put data in the start of the 
				  buffer! (Fragmentation) */
    
		if (tail_free >= buf_len) {
			D_MEM("Wrapped buffer, subscribe at free (pos %d)\n", 
			      bt_buf.free - bt_buf.head);
			tx = (bt_tx_buf *)bt_buf.free;
			bt_buf.free += buf_len;
			bt_buf.count += buf_len;
			bt_buf.nbr_bufs++;
		} else if (!bt_buf.count) {
			/* If buffer empty, reset buffer */          
			btmem_reset();
			
			if (buf_len > bt_buf.size) {
				D_ERR(__FUNCTION__ ": Cannot subscribe %d bytes !\n", send_len); 
				D_ERR(__FUNCTION__ ": Only %d available (non - fragmented)\n",
				      buf_write_room());
				btmem_get_status(NULL);
				sti();
				return NULL;
			}
			
			D_MEM("Buffer empty, reset buffer and subsc at %d\n ",
			      bt_buf.free - bt_buf.head);
			tx = (bt_tx_buf *)bt_buf.free;
			bt_buf.free += buf_len;
			bt_buf.count += buf_len;
			bt_buf.nbr_bufs++;
		} else {
			D_ERR(__FUNCTION__ ": Cannot subscribe requested size (%d) !\n", 
			      send_len);
			D_ERR(__FUNCTION__ ": Only %d available (non - fragmented)\n",
			      buf_write_room());
			btmem_get_status(NULL);
#if PANIC_AT_ERROR
			panic("lets stop here... (WRAP)\n");
#endif
			sti();
			return NULL;
		}
	}

	tx->magic = 0x4321;
	tx->subscr_len = send_len;
	tx->flushed = 0;
	tx->line = -1;
	
	sti();
	return tx;
}

/* Returns number of buffers */

s32 buf_count(void)
{
	return bt_buf.nbr_bufs;
}

/* Returns total number of bytes in buffer (fragmented) */

s32 buf_byte_count(s32 line)
{
	u8 *pos = bt_buf.head;
	bt_tx_buf *tx;
	u32 sum = 0;

	if (line < 0) {
		//printk(__FUNCTION__": Total bytes in buffer %d\n", bt_buf.count);