rtl_malloc.c

最新rtlinux内核源码

/*
 * Two Levels Segregate Fit memory allocator (TLSF)
 * Version 1.1
 *
 * Written by Miguel Masmano Tello <[e-mail]>
 * Copyright (C) Dec, 2002 OCERA Consortium
 * Release under the terms of the GNU General Public License Version 2
 *
 */

#include "rtl_malloc.h"
#include <arch/bits.h>

// linux/types.h includes definitions
// for the standard types
// #include <linux/types.h>

#if !defined (__RTL__) && defined (__KERNEL__)
#error "This modules can only be used by RTLinux or by user space"
#endif

#if defined (__RTL__)

#include <linux/config.h>

#define INIT_THREAD_MUTEX()
#define THREAD_LOCK() __asm__ __volatile__ ("cli");
#define THREAD_UNLOCK() __asm__ __volatile__ ("sti");

MODULE_AUTHOR("Miguel Masmano Tello <[e-mail]>");
MODULE_DESCRIPTION("Three Levels Segregate Fit memory allocator (TLSF) V1.0");

MODULE_LICENSE("GPL");

static int max_size = 1024; // 1 MByte

MODULE_PARM(max_size, "i");
MODULE_PARM_DESC(max_size, "Memory pool size in Kb (default=1024)");

static unsigned char *ptr_mem;

/* This TLSF version only can be used with BIGPHYSAREA patch */
#if defined(CONFIG_BIGPHYSAREA) || defined(CONFIG_BIGPHYS_AREA)
#include <linux/bigphysarea.h>
#else
#error BIGPHYSAREA LINUX KERNEL PATCH MUST BE INSTALLED
#endif

int init_module(void){
  // First of all we reserve the memory that will be used later
  ptr_mem = (unsigned char *) bigphysarea_alloc(max_size * 1024);
  if (ptr_mem == NULL) { 
    rtl_printf ("rtl_malloc PANIC: Error reserving memory!!");
    return -1; 
  }
  if (init_memory_pool (max_size, 5, ptr_mem) < 0) return -1;
  associate_buffer (ptr_mem);
  
  return 0;
}

void cleanup_module(void){
  destroy_memory_pool(ptr_mem);
  bigphysarea_free((caddr_t) ptr_mem, 0);
}

#else // #if defined (__RTL__)

#define INIT_THREAD_MUTEX()
#define THREAD_LOCK()
#define THREAD_UNLOCK()

#endif // #if defined (__RTL__)

/*
 * The following  TAD will be a  double level indexed  array, the most
 * important thing is that the time  is bounded, the reason of this is
 * because TLSF has been designed to be used by real time programs.
 *
 *     First level       Second level
 *     it is indexed     it is indexed by 2**n+(2**n/m*index_number)
 *     by power of 2
 *                            0             1     m-1        m
 *                            ----> NULL   --> NULL          ----> NUL
 *                            |            |                 |
 *       -------         ---------------------...---------------------
 *  2**n |  n  |  -----> |2**n+(2**n/m*0)|...|   |...|2**n+(2**n/m*m)|
 *       |-----|         ---------------------...---------------------
 * 2**n-1| n-1 |  -----> ....                      |
 *       |-----|                                   --->NULL
 *        .....
 */

/* 
 * Some defaults values;
 * please, don't touch these macros
 */

#define TLSF_WORD_SIZE 4 //(sizeof(__u32))
#define LOG2_TLSF_WORD_SIZE 2 //(TLSF_fls(TLSF_WORD_SIZE)) // TLSF_WORD size



#define MIN_LOG2_SIZE 2 // 16 bytes
#define MIN_SIZE 4 //(1 << MIN_LOG2_SIZE)

#define TLSF_WORDS2BYTES(x) ((x) << LOG2_TLSF_WORD_SIZE)
#define BYTES2TLSF_WORDS(x) ((x) >> LOG2_TLSF_WORD_SIZE)

/* 
 * The following structure defines the pointers used 
 * by the header to know the position of the free blocks 
 */

typedef struct free_ptr_struct {
  struct block_header_struct *prev;
  struct block_header_struct *next;

  /* 
   * first_index and second_index are used to store
   * mapping_function results, that's how we get some extra
   * nanoseconds 
   */
  __u8 first_index;
  __u8 second_index;
} free_ptr_t;

/*
 * USED_BLOCK must be used like mask with the magic number
 * i.e. 
 * if ((magic_number & USED_BLOCK) = USED_BLOCK) then
 *   return USED;
 * else
 *   return FREE
 * end if;
 */

#define USED_BLOCK 0x80000000
#define FREE_BLOCK ~USED_BLOCK //0x7FFFFFFF

#define LAST_BLOCK 0x40000000
#define NOT_LAST_BLOCK ~LAST_BLOCK //0xBFFFFFFF

#define IS_USED_BLOCK(x) ((x -> size & USED_BLOCK) == USED_BLOCK)
#define IS_LAST_BLOCK(x) ((x -> size & LAST_BLOCK) == LAST_BLOCK)
#define GET_BLOCK_SIZE(x) (x -> size & FREE_BLOCK & NOT_LAST_BLOCK)
#define SET_USED_BLOCK(x) (x -> size |= USED_BLOCK)
#define SET_FREE_BLOCK(x) (x -> size &= FREE_BLOCK)
#define SET_LAST_BLOCK(x) (x -> size |= LAST_BLOCK)
#define SET_NOT_LAST_BLOCK(x) (x -> size &= NOT_LAST_BLOCK)

#define MAGIC_NUMBER 0x2A59FA59

typedef struct block_header_struct {
  /* 
   * size codifies the block size in TLSF_BYTES and it also codifies if 
   * the block is used or free 
   */
  
  __u32 size;
  
  /* The following pointer points to the previous physical block */
  struct block_header_struct *prev_phys_block;

  union {
    struct free_ptr_struct free_ptr;
    __u8 buffer[sizeof(struct free_ptr_struct)];
  } ptr;
  
} block_header_t;

/* first level index array */
typedef struct fl_array_struct {
  /* ptr is pointer to next level */
  block_header_t **sl_array;
  
  /* bitmapSL is the second level bitmap */
  __u32 bitmapSL;
} fl_array_t;

/* 
 * When the user calls init_memory_pool, he must give a block of memory
 * block, this block will be used to store the TLSF structure
 */

typedef struct TLSF_struct {
  __u32 magic_number;
  /* 
   * max_fl_index, max_sl_index and max_sl_log2_index
   * must be __u8 but the compiler assigns 32 bits by efficiency, 
   * we also do that
   */
  __u32 max_fl_index;
  __u32 max_fl_pow2_index;
  __u32 max_sl_index;
  __u32 max_sl_log2_index;
  /* bitmapFL is the bitmap of the first level */
  __u32 bitmapFL;
  /* first_bh will be our first memory block allocated by MALLOC function */
  block_header_t *first_bh;

  /* 
   * fl_array will be our first level array,
   * it will be an array of [max_fl_index] elements
   */
  fl_array_t *fl_array;
} TLSF_t;

/*
 * header_overhead has size of blocks_header - 2 * ptr to next free block
 */
static __s32 beg_header_overhead = 0;

#define TLSF__set_bit(num, mem) mem |= (1 << num)
#define TLSF__clear_bit(num, mem) mem &= ~(1 << num)

char *main_buffer = NULL;

/*
 * log2size ()  returns cell  of log2 (len)  it does a  search between
 * MIN_SIZE and MAX_SIZE values in order to find the log2 of the size.
 * Theorically  we have obtained  that this  method is  more efficient
 * than the asm instruction which does the same operation
 */

static inline __s32 log2size (size_t size, size_t *new_size) {
  
  __s32 i;

  /* Other way quicker and also more machine dependent that the first one */
  /*------------------------------*/
  i = TLSF_fls(size);
  
    
  *new_size = (1 << i);

  return i;
}

/*
 * mapping_function () returns first and second level index
 *
 * first level index function is:
 * fl = log2 (size)
 *
 * and second level index function is:
 * sl = (size - 2**(log2size (size))) / (log2 (size) - log2 (MAX_SL_INDEX))
 *
 */

static inline __s32 mapping_function (size_t size, __s32 *fl, __s32 *sl,
				      TLSF_t *ptr_TLSF){
  __s32 aux, new_len;
   
  /* This is a new way to calculate first level index 
     and second level index, it is quicker than older one */
  
  *fl = log2size (size, &new_len);
  aux = (*fl - ptr_TLSF -> max_sl_log2_index);
  *sl = ((size ^ new_len) >> aux);
  
  return 0;
}

/* 
   Max_size is in Kbytes, On success this function returns the free size
 */


int init_memory_pool (int max_size,
		      int max_sl_log2_index, char *block_ptr) {
  __s32 n, free_mem = 0, total_size;
  block_header_t *initial_block_ptr;
  __s32 size_fl_sl_array, i, fl, sl;
  TLSF_t *ptr_TLSF;
  
  if (!(max_size > 0)) {
    PRINT_MSG ("ERROR: size must be > 0\n");
    return -1;
  }
  
  if (max_sl_log2_index > 5 || max_sl_log2_index < 1) {
    PRINT_MSG ("ERROR max_sl_log2_index must be >=1 or <= 5\n");
    return -1;
  }
  
  if (max_sl_log2_index <= 0) {
    PRINT_MSG ("ERROR: max_sl_log2_index (%d) must be >= 0\n", 
  	       max_sl_log2_index);
    return -1;
  }

  if ((((__u32) block_ptr >> LOG2_TLSF_WORD_SIZE) << LOG2_TLSF_WORD_SIZE) 
      != (__u32) block_ptr) {
    PRINT_MSG ("ERROR block_ptr must be aligned\n");
    return -1;
  }
  
  memset ((char *) block_ptr, 0x00, max_size * 1024);

  INIT_THREAD_MUTEX();
  ptr_TLSF = (TLSF_t *) block_ptr;
  
  ptr_TLSF -> magic_number = MAGIC_NUMBER;
  
  /* Total size of the block, TLSF_struct + free_memory */
  
  total_size = BYTES2TLSF_WORDS(max_size * 1024);

  ptr_TLSF -> max_sl_log2_index = max_sl_log2_index;
  ptr_TLSF -> max_sl_index = (1 << ptr_TLSF -> max_sl_log2_index);
  
  size_fl_sl_array = BYTES2TLSF_WORDS((__s32) sizeof (fl_array_t) +
    ((__s32) sizeof (block_header_t *) * (__s32) ptr_TLSF -> max_sl_index));

  free_mem = total_size - BYTES2TLSF_WORDS(sizeof (TLSF_t)) - size_fl_sl_array;
    
  n = MIN_LOG2_SIZE + 1;
  
  while ((int) TLSF_WORDS2BYTES(free_mem) > 
	 (1 << (n + LOG2_TLSF_WORD_SIZE)) && n < MAX_FL_INDEX) {
    n ++;
    free_mem -= size_fl_sl_array;
  }
  
  if (free_mem < 0) return -1;

  // max_fl_index never will be greater than 32 (4 Gbytes)
  
  ptr_TLSF -> max_fl_index = n;
  ptr_TLSF -> max_fl_pow2_index = (1 << ptr_TLSF -> max_fl_index);
  
  n -= MIN_LOG2_SIZE;
  
  /* max_fl_index will never be greater than MAX_FL_INDEX */
  if (ptr_TLSF -> max_fl_index < 0 || MAX_FL_INDEX < 0) return -1;
  
  ptr_TLSF -> fl_array = ( fl_array_t *) 
    ((__u32) &(ptr_TLSF -> fl_array) 
     + (__u32) sizeof (ptr_TLSF -> fl_array));
  
  for (i = 0 ; i < n; i ++)
    ptr_TLSF -> fl_array [i] .sl_array = (block_header_t **) 
      (((__s32) ptr_TLSF -> fl_array + ((__s32) sizeof (fl_array_t) * n)) +
       ((__s32) sizeof (block_header_t *) *
	(__s32) ptr_TLSF -> max_sl_index * i));
  
  initial_block_ptr = (block_header_t *) 
    ((__u32) ptr_TLSF -> fl_array + 
     (TLSF_WORDS2BYTES(size_fl_sl_array) * n));
  
  ptr_TLSF -> first_bh = initial_block_ptr;
  
  beg_header_overhead = BYTES2TLSF_WORDS((int) initial_block_ptr -> ptr.buffer
					 - (int) initial_block_ptr);

  ptr_TLSF -> bitmapFL = 0;
 
  
  initial_block_ptr -> size =
    free_mem - beg_header_overhead;

  SET_LAST_BLOCK (initial_block_ptr);

  initial_block_ptr -> ptr.free_ptr.prev = NULL;
  initial_block_ptr -> ptr.free_ptr.next = NULL;
  initial_block_ptr -> prev_phys_block = NULL;
  
  if (TLSF_WORDS2BYTES(GET_BLOCK_SIZE(initial_block_ptr)) 
      <= MIN_SIZE) {
    return -1;
  } else {
    mapping_function (GET_BLOCK_SIZE(initial_block_ptr), &fl, &sl, 
		      ptr_TLSF);
    fl -= MIN_LOG2_SIZE;
  }

  ptr_TLSF -> fl_array [fl].sl_array [sl] = initial_block_ptr;
  TLSF__set_bit (sl, ptr_TLSF -> fl_array[fl].bitmapSL);
  TLSF__set_bit (fl, ptr_TLSF -> bitmapFL);
  
  return TLSF_WORDS2BYTES(GET_BLOCK_SIZE(initial_block_ptr));
}

void destroy_memory_pool (char *block_ptr) {
  TLSF_t *ptr_TLSF;
  ptr_TLSF = (TLSF_t *) block_ptr;
  ptr_TLSF -> magic_number = 0;
} 

/* see man malloc */
/*
 * malloc searchs a free block of size 'size',
 * after the free block will be splitted in two new blocks,
 * one of these new blocks will be given to the user and the
 * other will be inserted into TLSF structure
 *
 * The cost of this operation is
 *      best case: (K) = (1)
 *      worst case: (MAX_FL_LOG2_INDEX - MIN_FL_LOG2_INDEX + MAX_SL_INDEX + K)
 *                   = (1)
 * where K is a constant integer
 */

#ifdef WITH_RTL_PREFIX
void *rtl_malloc_ex (size_t size, char *block_ptr) {
#else
void *malloc_ex (size_t size, char *block_ptr) {
#endif
  TLSF_t *ptr_TLSF;
  int fl, sl, n, found = 0, old_size = BYTES2TLSF_WORDS(size), last_block;
  block_header_t *bh = NULL, *bh2 = NULL, *bh3 = NULL;
  
  ptr_TLSF = (TLSF_t *) block_ptr;

  if (ptr_TLSF == NULL || ptr_TLSF -> magic_number != MAGIC_NUMBER) {
    PRINT_MSG ("FATAL ERROR: TLSF structure not initialized\n");
    return NULL;
  }

  if (!(size > 0)) {
    PRINT_MSG ("ERROR: Memory pool exhausted!!!\n");
    return NULL;
  }

  if (old_size < MIN_SIZE) {
    size = MIN_SIZE;
    fl = 0;
    sl = 0;
  } else {
    mapping_function (old_size, &fl, &sl, ptr_TLSF);

    if (++sl == ptr_TLSF -> max_sl_index) {
      fl ++;
      sl = 0;
    }
    
    /*
     * This is the reason of the internal fragmentation
     * The block given is greater that the size demanded
     */
    
    /* size can be smaller that maximum SLI, in this case the mapping function
       has problems calculating fl and sl values */
 
    if (old_size >= 
	BYTES2TLSF_WORDS(ptr_TLSF -> max_sl_index << 2)) {
      size = (1 << fl);
      size += (sl << (fl - ptr_TLSF -> max_sl_log2_index));
    } else {
      size = old_size + 1;
      sl = old_size - MIN_SIZE;
    }
    
    fl -= MIN_LOG2_SIZE;
  }


  /*----------------------------------------*/
  /* The search for a free block begins now */
  /*----------------------------------------*/
  
  /*
   * Our first try, we take the first free block 
   * from fl_array or its buddy
   */
  
  THREAD_LOCK();
  sl = ptr_TLSF -> fl_array[fl].bitmapSL & ((~0) << sl);
  if (sl != 0) {
    sl = TLSF_fls(sl);
    bh =  ptr_TLSF -> fl_array [fl].sl_array [sl];
    ptr_TLSF -> fl_array [fl].sl_array [sl] = bh -> ptr.free_ptr.next;
    if (ptr_TLSF -> fl_array [fl].sl_array [sl] != NULL)
      ptr_TLSF ->fl_array [fl].sl_array [sl] -> ptr.free_ptr.prev = NULL;
    else {
      TLSF__clear_bit (sl, ptr_TLSF -> fl_array[fl].bitmapSL);
      if (!ptr_TLSF -> fl_array[fl].bitmapSL)
        TLSF__clear_bit (fl, ptr_TLSF -> bitmapFL);
    }