rtl_malloc.c

最新rtlinux内核源码

    found = 1;
    goto out;
  }
  
  /*
   * On the last case a free block is searched using a bitmap
   */
  
  fl = TLSF_fls(ptr_TLSF -> bitmapFL & ((~0) << (fl + 1)));
  
  if (fl > 0) {
    sl = TLSF_fls(ptr_TLSF -> fl_array[fl].bitmapSL);
    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);
    }
    found = 1;
    goto out;
  }
  /* end of the search */
  /*------------------------------------------------------------*/
  
 out:
  
  /*
   * HUGGGG, NOT ENOUGHT MEMORY
   * I think that we have done all that we have been able, I'm sorry
   */
  
  if (!found) {
    THREAD_UNLOCK();
    PRINT_MSG ("ERROR: Memory pool exhausted!!!\n");
    return NULL;
  }
  
  /*
   * we can say: YESSSSSSSSSSS, we have enought memory!!!!
   */
  
  /* can bh be splitted? */ 
  
  n = (int)(GET_BLOCK_SIZE(bh) - size - beg_header_overhead);
  if (n >= (int) MIN_SIZE) {
    /*
     * Yes, bh will be splitted
     */

    /* The new block will begin at the end of the current block */
    last_block = IS_LAST_BLOCK(bh)?1:0;
    bh -> size = size;
    SET_USED_BLOCK(bh);
    
    bh2 = (block_header_t *) (__u8 *) (bh -> ptr.buffer + 
				       TLSF_WORDS2BYTES
				       (GET_BLOCK_SIZE(bh)));
    bh2 -> prev_phys_block = bh;
    bh2 -> size = n;
    if (last_block) SET_LAST_BLOCK (bh2);
    mapping_function (GET_BLOCK_SIZE(bh2), &fl, &sl, ptr_TLSF);
    fl -= MIN_LOG2_SIZE;
    bh2 -> ptr.free_ptr.first_index = fl;
    bh2 -> ptr.free_ptr.second_index = sl;
    bh2 -> ptr.free_ptr.prev = NULL;
    bh2 -> ptr.free_ptr.next = ptr_TLSF -> fl_array [fl].sl_array [sl];
    
    if (!IS_LAST_BLOCK (bh2)) {
      bh3 = (block_header_t *) (__u8 *) (bh2 -> ptr.buffer + 
					 TLSF_WORDS2BYTES
					 (GET_BLOCK_SIZE(bh2)));
      bh3 -> prev_phys_block = bh2;
    }
    

    if (ptr_TLSF -> fl_array [fl].sl_array [sl] != NULL)
      ptr_TLSF -> fl_array [fl].sl_array [sl] -> ptr.free_ptr.prev = bh2;
    ptr_TLSF -> fl_array [fl].sl_array [sl] = bh2;
    TLSF__set_bit (sl, ptr_TLSF -> fl_array[fl].bitmapSL);
    TLSF__set_bit (fl, ptr_TLSF -> bitmapFL); 
  }
  
  SET_USED_BLOCK(bh);

  THREAD_UNLOCK();
  
  return (void *)  bh -> ptr.buffer; 
}

/*
 * see man free
 *
 * free () is only guaranteed  to work if ptr is the address
 * of a block allocated by rt_malloc() (and not yet freed).
 */
 
#ifdef WITH_RTL_PREFIX
void rtl_free_ex (void *ptr, char *block_ptr) {
#else
void free_ex (void *ptr, char *block_ptr) {
#endif
  int fl, sl;
  block_header_t *bh = NULL, *bh2 = NULL, *bh3;

  TLSF_t *ptr_TLSF;

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

  bh = (block_header_t *) ((__u8 *) ptr - 
			   TLSF_WORDS2BYTES(beg_header_overhead));
 
  THREAD_LOCK();
  /* now bh is a free block */
  
  SET_FREE_BLOCK (bh);
  bh -> ptr.free_ptr.prev = NULL;
  bh -> ptr.free_ptr.next = NULL;

  /*
   * first of all, we will try to merge bh with the
   * physically contiguos free block and
   * after we will inserte bh into TLSF structure
   */

  /* Now we will try if we can merge bh with the next phys. contiguos block */
  if (!IS_LAST_BLOCK (bh)) {
    /* is it the next block free? */
    /* The next block is easy to found */
    
    bh2 = (block_header_t *) (__u8 *) (bh -> ptr.buffer + 
				       TLSF_WORDS2BYTES
				       (GET_BLOCK_SIZE(bh)));
    if (!IS_USED_BLOCK (bh2)) {
      /* we are lucky, we can merge bh with the following one */  
      if (bh2 -> ptr.free_ptr.next != NULL)
	bh2 -> ptr.free_ptr.next -> ptr.free_ptr.prev =
	  bh2  -> ptr.free_ptr.prev;
      
      if (bh2 -> ptr.free_ptr.prev != NULL)
	bh2 -> ptr.free_ptr.prev -> ptr.free_ptr.next =
	  bh2 -> ptr.free_ptr.next;
      
      fl = bh2 -> ptr.free_ptr.first_index;
      sl = bh2 -> ptr.free_ptr.second_index;
      
      /* bh2 must be deleted from fl_array */
      if (ptr_TLSF -> fl_array [fl].sl_array [sl] == bh2)
	ptr_TLSF -> fl_array [fl].sl_array [sl] = bh2 -> ptr.free_ptr.next;
      
      if (ptr_TLSF -> fl_array [fl].sl_array [sl] == NULL){
	TLSF__clear_bit (sl, ptr_TLSF -> fl_array[fl].bitmapSL);
	if (!ptr_TLSF -> fl_array[fl].bitmapSL) 
	  TLSF__clear_bit (fl, ptr_TLSF -> bitmapFL);
	
      }

      bh -> size += bh2 -> size + beg_header_overhead;
      if (!IS_LAST_BLOCK (bh2)) {
	bh3 = (block_header_t *) (__u8 *) (bh2 -> ptr.buffer + 
					   TLSF_WORDS2BYTES
					   (GET_BLOCK_SIZE(bh2)));
	bh3 -> prev_phys_block = bh;
      }
      
      if (IS_LAST_BLOCK (bh2)) SET_LAST_BLOCK (bh);
    }
  }

  /* is it free the previous physical block? */
  if (bh -> prev_phys_block != NULL) { // This block is not the first block
   
    bh2 = bh -> prev_phys_block;
  
    if (!IS_USED_BLOCK (bh2)) {

      if (bh2 -> ptr.free_ptr.next != NULL)
	bh2 -> ptr.free_ptr.next -> ptr.free_ptr.prev =
	  bh2  -> ptr.free_ptr.prev;
      
      if (bh2 -> ptr.free_ptr.prev != NULL)
	bh2 -> ptr.free_ptr.prev -> ptr.free_ptr.next =
	  bh2 -> ptr.free_ptr.next;
      
      fl = bh2 -> ptr.free_ptr.first_index;
      sl = bh2 -> ptr.free_ptr.second_index;
	
      if (ptr_TLSF -> fl_array [fl].sl_array [sl] == bh2)
	ptr_TLSF -> fl_array [fl].sl_array [sl] = bh2 -> ptr.free_ptr.next;
      
      if (ptr_TLSF -> fl_array[fl].sl_array[sl] == NULL){
	
	TLSF__clear_bit (sl, ptr_TLSF -> fl_array[fl].bitmapSL);
	if (!ptr_TLSF -> fl_array[fl].bitmapSL)
	  TLSF__clear_bit (fl, ptr_TLSF -> bitmapFL);
	
      }     
      bh2 -> size += bh -> size + beg_header_overhead;

      bh = bh2;
      if (!IS_LAST_BLOCK (bh)) {
	bh3 = (block_header_t *) (__u8 *) (bh -> ptr.buffer + 
					   TLSF_WORDS2BYTES
					   (GET_BLOCK_SIZE(bh)));
	bh3 -> prev_phys_block = bh;
      }
    
    }
 
  }

  /*
   * and now we can merge the free block with the initial memory
   */
  mapping_function (GET_BLOCK_SIZE (bh), &fl, &sl, ptr_TLSF);
  fl -= MIN_LOG2_SIZE;
  bh -> ptr.free_ptr.first_index = fl;
  bh -> ptr.free_ptr.second_index = sl;
  bh -> ptr.free_ptr.next = ptr_TLSF -> fl_array [fl].sl_array [sl];
  bh -> ptr.free_ptr.prev = NULL;
 
  if (ptr_TLSF -> fl_array [fl].sl_array [sl] != NULL)
    ptr_TLSF -> fl_array [fl].sl_array [sl] -> ptr.free_ptr.prev = bh;
  ptr_TLSF -> fl_array [fl].sl_array [sl] = bh;

  TLSF__set_bit (sl, ptr_TLSF -> fl_array[fl].bitmapSL);
  TLSF__set_bit (fl, ptr_TLSF -> bitmapFL);    
  THREAD_UNLOCK();
}

/* see man realloc */
#ifdef WITH_RTL_PREFIX
void *rtl_realloc_ex (void *p, size_t new_len, char *block_ptr) {
#else
void *realloc_ex (void *p, size_t new_len, char *block_ptr) {
#endif
  __u8 *ptr_aux;
  block_header_t *b;

  if (p == NULL)
#ifdef WITH_RTL_PREFIX
    return (void *) rtl_malloc_ex (new_len, block_ptr);
#else
    return (void *) malloc_ex (new_len, block_ptr);
#endif
  else if (new_len == 0) {
#ifdef WITH_RTL_PREFIX
    rtl_free_ex (p, block_ptr);
#else
    free_ex (p, block_ptr);
#endif
    return NULL;
  }

#ifdef WITH_RTL_PREFIX  
  ptr_aux = (__u8 *) rtl_malloc_ex (new_len * sizeof (__u8), block_ptr);
#else
  ptr_aux = (__u8 *) malloc_ex (new_len * sizeof (__u8), block_ptr);
#endif
  
  b = (block_header_t *) (((__u8 *) p) - 
			  TLSF_WORDS2BYTES(beg_header_overhead));

  memcpy ((__u8 *) ptr_aux, (__u8 *) b, b -> size);
#ifdef WITH_RTL_PREFIX
  rtl_free_ex (p, block_ptr);
#else
  free_ex (p, block_ptr);
#endif

  return ((void *) ptr_aux);
}

/* see man calloc */
#ifdef WITH_RTL_PREFIX
void *rtl_calloc_ex (size_t nelem, size_t elem_size, char *block_ptr) {
#else
void *calloc_ex (size_t nelem, size_t elem_size, char *block_ptr) {
#endif

  __u8 *p;

  if (nelem <= 0 || elem_size <= 0) return NULL;

#ifdef WITH_RTL_PREFIX
  if ((p = (__u8 *) rtl_malloc_ex (nelem * elem_size, block_ptr)) == NULL)
    return NULL;
#else
  if ((p = (__u8 *) malloc_ex (nelem * elem_size, block_ptr)) == NULL )
    return NULL;
#endif

  memset (p, 0, nelem * elem_size);
  
  return ((void *) p);
}

static void print_block (block_header_t *b){
  if (b == NULL) return;

  PRINT_DBG_C (">>>> Address 0x");
  PRINT_DBG_H (b);
  
  if ((b -> size & USED_BLOCK) != USED_BLOCK)
    PRINT_DBG_C ("\n>>>> Status FREE");
  else
    PRINT_DBG_C ("\n>>>> Status USED");
  
  PRINT_DBG_C (" Block Size ");
  PRINT_DBG_D (TLSF_WORDS2BYTES(b -> size));
  PRINT_DBG_C (" bytes");
  
  if (b -> prev_phys_block == NULL)
    PRINT_DBG_C ("\n>>>> FIRST BLOCK");
  else {
    PRINT_DBG_C ("\n>>>> PREV. PHYS. BLOCK 0x");
    PRINT_DBG_H (b -> prev_phys_block);
  }

  if ((b -> size & LAST_BLOCK) == LAST_BLOCK)
    PRINT_DBG_C (" LAST BLOCK");
  else
    PRINT_DBG_C (" NOT LAST BLOCK");
  
  if ((b -> size & FREE_BLOCK) == FREE_BLOCK){
    PRINT_DBG_C ("\n---- Prev Free 0x");
    PRINT_DBG_H (b -> ptr.free_ptr.prev);
    PRINT_DBG_C (" Next Free 0x");
    PRINT_DBG_H (b -> ptr.free_ptr.next);
  }
  PRINT_DBG_C ("\n\n");
}

/*
 * structure_status () shows the status of all the blocks
 */
void structure_status (char *block_ptr) {
  block_header_t *b;
  TLSF_t *ptr_TLSF;
  int end = 0;
  
  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;
  }
 
  b = ptr_TLSF -> first_bh;
  PRINT_DBG_C ("\nTLSF structure address 0x");
  PRINT_DBG_H (ptr_TLSF);
  PRINT_DBG_C ("\nMax. first level index: ");
  PRINT_DBG_D (ptr_TLSF -> max_fl_index);
  PRINT_DBG_C ("\nMax. second level index: ");
  PRINT_DBG_D (ptr_TLSF -> max_sl_log2_index);
  PRINT_DBG_C ("\n\nALL BLOCKS\n");
  while (!end) {
    print_block (b);
    if (IS_LAST_BLOCK(b))
      end = 1;
    else
      b = (block_header_t *) (__u8 *) (b -> ptr.buffer + 
				       TLSF_WORDS2BYTES
				       (GET_BLOCK_SIZE(b)));
  }

}

/*
 * free_blocks_status () only shows the status
 * of the free blocks
 */
void free_blocks_status (char *block_ptr){
  int i, j;
  block_header_t *b;

  TLSF_t *ptr_TLSF;
  
  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;
  }

  PRINT_DBG_C ("\nTLSF structure address 0x");
  PRINT_DBG_H (ptr_TLSF);
  PRINT_DBG_C ("\nFREE BLOCKS\n\n");
  for (i = ptr_TLSF -> max_fl_index  - 1 - MIN_LOG2_SIZE; i >= 0; i--) {
    if (ptr_TLSF -> fl_array [i].bitmapSL > 0)
      for (j = ptr_TLSF -> max_sl_index - 1; j >= 0; j--) {
        if (ptr_TLSF -> fl_array [i].sl_array[j] != NULL) {
          b = ptr_TLSF -> fl_array [i].sl_array [j];
          PRINT_DBG_C ("[");
          PRINT_DBG_D (i + MIN_LOG2_SIZE);
          PRINT_DBG_C ("] ");
          PRINT_DBG_D (TLSF_WORDS2BYTES(1 << (i + MIN_LOG2_SIZE)));
	  PRINT_DBG_C (" bytes -> Free blocks: 0x");
	  PRINT_DBG_H (ptr_TLSF -> fl_array [i].bitmapSL);
          PRINT_DBG_C ("\n");
	  
          while (b != NULL) {
	    PRINT_DBG_C (">>>> First_Level [");
	    PRINT_DBG_D (i + MIN_LOG2_SIZE);
	    PRINT_DBG_C ("] Second Level [");
	    PRINT_DBG_D (j);
	    PRINT_DBG_C ("] -> ");
	    PRINT_DBG_D (TLSF_WORDS2BYTES((1 << (i + MIN_LOG2_SIZE)) + 
			  ( ((1 << (i + MIN_LOG2_SIZE)) / 
			     ptr_TLSF -> max_sl_index) * j)));

            PRINT_DBG_C (" bytes\n");
            print_block (b);
            b = b -> ptr.free_ptr.next;
          }
        }
      }
  }
}

void dump_memory_region (unsigned char *mem_ptr, unsigned int size) {
  
  unsigned int begin = (unsigned int) mem_ptr;
  unsigned int end = (unsigned int) mem_ptr + size;
  int column = 0;

  begin >>= 2;
  begin <<= 2;
  
  end >>= 2;
  end ++;
  end <<= 2;
  
  PRINT_DBG_C ("\nMemory region dumped: 0x");
  PRINT_DBG_H (begin);
  PRINT_DBG_C (" - ");
  PRINT_DBG_H (end);
  PRINT_DBG_C ("\n\n");
  
  column = 0;
  PRINT_DBG_C ("\t\t+0");
  PRINT_DBG_C ("\t+1");
  PRINT_DBG_C ("\t+2");
  PRINT_DBG_C ("\t+3");
  PRINT_DBG_C ("\t+4");
  PRINT_DBG_C ("\t+5");
  PRINT_DBG_C ("\n0x");
  PRINT_DBG_H (begin);
  PRINT_DBG_C ("\t");

  while (begin < end) {
    if (((unsigned char *) begin) [0] == 0)
      PRINT_DBG_C ("00");
    else
      PRINT_DBG_H (((unsigned char *) begin) [0]);
    if (((unsigned char *) begin) [1] == 0)
      PRINT_DBG_C ("00");
    else
      PRINT_DBG_H (((unsigned char *) begin) [1]);
    PRINT_DBG_C ("\t");
    begin += 2;
    column ++;
    if (column == 6) {
      PRINT_DBG_C ("\n0x");
      PRINT_DBG_H (begin);
      PRINT_DBG_C ("\t");
      column = 0;
    }

  }
  PRINT_DBG_C ("\n\n"); 
}