/* 
 * btmem.c --  Memory management in Bluetooth stack
 *
 * 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: btmem.c,v 1.30 2000/10/26 15:56:57 mattias Exp $
 *
 */

#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 <string.h> /* memset */
#endif


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

#if BTMEM_DEBUG
#define D_MEM(fmt, args...) printk(BTMEM_DBG_STR fmt, ## args)
#define PRINTPKT(str, data, len) print_data(str, data, len)
#else
#define D_MEM(fmt, args...) 
#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))

/* The total buffer size */
#define BT_BUF_SIZE 2500 

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

/****************** 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 */
	kfree(bt_buf.head);
}

/* 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;
}


/* Flush all buffers waiting to be sent on handle hdl */

void btmem_flushhandle(u16 hdl)
{
	u8 *pos = bt_buf.head;
	bt_tx_buf *tx;
	u32 i=0;
	D_MEM("btmem_flushhandle\n");

	/* 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 */
		return;
	
	D_MEM("btmem_flushhandle : flushed %d buffers\n", i);

	/* unsubscribe until a non-flushed buffer comes up... */
	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)
{
#define FNC "subscribe_bt_buf : "
	bt_tx_buf *tx;
	s32 buf_len; /* Total size of bt_tx_object */
	u32 head_free;
	u32 tail_free; /* Number bytes available */

	buf_len = send_len + BT_TX_HDRSIZE;  
	D_MEM(FNC"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(FNC"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(FNC"Cannot subscribe %d bytes !\n", send_len);
			D_ERR(FNC"Only %d available (non - fragmented)\n",
			      buf_write_room());

#if PANIC_AT_ERROR
			btmem_get_status(NULL);
			panic("lets stop here...\n");
#endif
			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(FNC"Cannot subscribe %d bytes !\n", send_len); 
			  D_ERR(FNC"Only %d available (non - fragmented)\n",
				buf_write_room());
			  btmem_get_status(NULL);
			  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(FNC"Cannot subscribe requested size (%d) !\n", 
			      send_len);
			D_ERR(FNC"Only %d available (non - fragmented)\n",
			      buf_write_room());
			btmem_get_status(NULL);
#if PANIC_AT_ERROR
			panic("lets stop here... (WRAP)\n");
#endif
			return NULL;
		}
	}  

	tx->magic = 0x4321;
	tx->subscr_len = send_len;
	tx->flushed = 0;

	return tx;
#undef FNC
}

/* 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()
{
	return bt_buf.count;
}


/* Returns unfragmented buffer space */ 

#define BTMEM_EXTRASPACE BT_BUF_SIZE/10 /* always an extra 10 % marginal
					   in case the subscriber doesn't
					   check available space before 
					   doing the subscribe (only in 
					   connect/disconnect phase ) */

s32 buf_write_room()
{
	s32 space_left;  

	if (bt_buf.free > bt_buf.send) 
		space_left = MAX((bt_buf.tail - bt_buf.free),
				 (bt_buf.send - bt_buf.head));
	else if (bt_buf.free < bt_buf.send)
		space_left  = (bt_buf.send - bt_buf.free);
	else {
		/* subscribe_bt_buf will reset buffer if buffer is empty 
		   don't count a tossed tail ! */
		space_left =  (BT_BUF_SIZE - 
			       (bt_buf.tail - bt_buf.toss_tail) - bt_buf.count);
  
	}

	space_left  -= (BT_TX_HDRSIZE+BTMEM_EXTRASPACE);

	if (space_left < 0)
		space_left=0;

//	btmem_show_size(space_left);
	
	return space_left;
}


/* Fetch pointer to next transmission chunk */

bt_tx_buf* get_bt_buf(void)
{
	bt_tx_buf *tx;

	if (bt_buf.count == 0){
		D_MEM("get_bt_buf : no data in buffer\n");
		return NULL;
	}

	/* Find the next send buf, first check toss_tail */
	if (bt_buf.send == bt_buf.toss_tail) {
		D_MEM("get_bt_buf : toss tail !\n");
		bt_buf.send = bt_buf.head;
		bt_buf.toss_tail = bt_buf.tail;
	}

	/* add check that cur_len < subscr_len ? */

	tx = (bt_tx_buf *)(bt_buf.send);

	if (tx->magic != 0x4321)
	{
		D_ERR("get_bt_buf : wrong magic!\n");
#if PANIC_AT_ERROR
		panic("lets stop here...\n");
#endif
		return NULL;
	}
  
	/* Check if chunk is valid */
	if (tx->flushed) {
		/* This buffer is flushed */
		DSYS("get_bt_buf : Flushing this buffer\n");
#if BTMEM_FLUSH_ENABLED
		unsubscribe_bt_buf(tx);
		return get_bt_buf(); /* Yeehaa, more recursion :) */
#else
		return NULL;
#endif
	}
	D_MEM("get_bt_buf : returning %d\n", (bt_buf.send - bt_buf.head) );
  
	return tx;
}


/* Unsubscribes memory area so other functions can reuse that memory. */

void unsubscribe_bt_buf(bt_tx_buf *tx)
{
#define FNC "unsubscribe_bt_buf : "
	s32 tail_free; /* for debug */
	s32 head_free;
  
  
	D_MEM(FNC"%d bytes (not incl hdrs) at pos %d\n", 
	      tx->subscr_len, (u8 *)tx - bt_buf.head);
  
	/* Check that tx is valid for unssubscribe. We must process the 
	   buffers in fifo order ! */

	if (tx && (bt_buf.send == (u8*)tx)) {

		if ((u8*)(tx->data + tx->subscr_len) <= bt_buf.tail) {
      
			/* don't touch free, only subscribe_bt_buf change free */
			bt_buf.send += (tx->subscr_len + BT_TX_HDRSIZE);
			bt_buf.count  -= (tx->subscr_len + BT_TX_HDRSIZE);
			bt_buf.nbr_bufs--;      
		} else {
			D_ERR(FNC"failed, out of boundary\n");
		}
	} else if (bt_buf.count == 0)
		D_MEM(FNC"no data in buffer\n");
	else {
		D_ERR(FNC"invalid data segment!\n");
#ifdef __KERNEL__

		D_ERR("pid : %d (tx:0x%x - send:0x%x=%d) send:%d free:%d\n",
		      current->pid, (uint)tx, (uint)bt_buf.send,
		      ((int)tx - (int)bt_buf.send),(bt_buf.send -bt_buf.head), 
		      (bt_buf.free - bt_buf.head));
		
		D_ERR("count:%d nbr_bufs:%d count:%d\n" ,
		      bt_buf.count, bt_buf.nbr_bufs, bt_buf.count);

#if PANIC_AT_ERROR
		panic("Lets stop here...\n");
#endif

#endif
	}

	/* If this buffer has flushed flag set it is simply unsubscribed 
	   just as any other buffer, get_bt_buf is responsible for calling 
	   this function. However, it is unsubscribe_bt_buf:s responsibility 
	   to toss the tail */
  
	/* Check if next buffer is to be tossed or not */

	/* Is also checked done in get_bt_buf */
	if (bt_buf.send == bt_buf.toss_tail) {
		D_MEM(FNC"toss_tail, now send pos is 0\n");
		bt_buf.send = bt_buf.head;
		bt_buf.toss_tail = bt_buf.tail;

		if (bt_buf.count == 0)
			bt_buf.free = bt_buf.head;
	}
  
	tail_free = (bt_buf.tail - bt_buf.free);
	head_free = (bt_buf.send - bt_buf.head);
#if BTMEM_SHOW_GRAPH
	btmem_show_size(BT_BUF_SIZE - buf_write_room());
#endif

}

/*===================================================================*/

/* Debug tools */


/* Displays unfragmented buffer usage */

#define BUFFERMETERRESOLUTION 40

u8 buffermeter[BUFFERMETERRESOLUTION + 1];

/* Graphical view of buffer size */
void btmem_show_size(s32 count)
{
	u32 i;
	i =  ((count*BUFFERMETERRESOLUTION)/BT_BUF_SIZE);
	memset(buffermeter, '|', i);
	buffermeter[i+1] = 0; /* null terminate */ 
	printk("%s  (%d)\n", buffermeter, count);
}

void show_bt_buf(bt_tx_buf *tx, s32 no_data)
{
	printk("buf (%d) - magic 0x%x, hci_hdl 0x%x\n",
	      ((u8*)tx - bt_buf.head), tx->magic, 
	      tx->hci_hdl);
	
	printk("          pb flag : %d bc flag : %d\n", 
	      tx->pb_flag, tx->bc_flag);
	
	printk("          cur_len %d, subscr_len %d, flushed %d\n\n",
	      tx->cur_len, tx->subscr_len, tx->flushed);
	
	if (!no_data)
		print_data("data :", tx->data, tx->subscr_len); 
}

void btmem_get_status(bt_tx_buf *current_buf)
{
	u8 *pos = bt_buf.head;
	bt_tx_buf *tx;
	printk("*** buffer info ***\n");
	printk("current buffer at : 0x%x\n", (int)current_buf);
	printk("head : 0x%x, tail : 0x%x\n", (int)bt_buf.head, 
	       (int)bt_buf.tail);
	printk("size : %d count : %d nbr_bufs : %d\n", 
	       bt_buf.size, bt_buf.count, bt_buf.nbr_bufs);
	printk("free : %d, send : %d, toss_tail : %d\n", 
	       bt_buf.free - bt_buf.head, bt_buf.send - bt_buf.head,
	       bt_buf.toss_tail - bt_buf.head);
	printk("------------------------------------------\n");

	if (bt_buf.free > bt_buf.send) {
		/* read from send -> free */
		pos = bt_buf.send;
		printk("SEND <-> FREE\n");
		while (pos < bt_buf.free) {
			tx = (bt_tx_buf *)(pos);
			show_bt_buf(tx, 1);
			pos += BT_TX_HDRSIZE + tx->subscr_len;
		}
	} else {
		/* read send -> toss_tail + read from head -> free */
		pos = bt_buf.send;
		printk("SEND <-> TOSS_TAIL\n");
		while (pos < bt_buf.toss_tail) {
			tx = (bt_tx_buf *)(pos);
			show_bt_buf(tx, 1);
			pos += BT_TX_HDRSIZE + tx->subscr_len;
		}
    
		printk("HEAD <-> FREE\n");
		pos = bt_buf.head; 
		while (pos < bt_buf.free) {
			tx = (bt_tx_buf*)(pos);
			show_bt_buf(tx, 1);
			pos += BT_TX_HDRSIZE + tx->subscr_len;
		}
	}
	printk("------------------------------------------\n");
}

#if BTMEM_TEST
void btmem_test(void)
{
           s32 i;
           s32 count = 10000;
           u32 cursize = 1;
           bt_tx_buf *tx;
   
           for (i = 0; i < count; i++) {

	     if (cursize > buf_write_room())
	       return;
	     
	     printk("storing %d bytes packets\n", cursize);
	     while (buf_write_room() >= cursize) {
	       
	       if (!(tx = subscribe_bt_buf(cursize))) {
		 return ;
	       }
	     }

	     printk("emptying %d bytes packets\n", cursize);
	     while (tx = get_bt_buf()) {
	       unsubscribe_bt_buf(get_bt_buf());
	     }
	     cursize++;
	   }
           return;
}

#endif
/****************** END OF FILE btmem.c *************************************/
