lwbt/btmemr.c

   1 #include <stdlib.h>
   2 #include <string.h>
   3
   4 #include "asm.h"
   5 #include "processor.h"
   6
   7 #include "bt.h"
   8 #include "btmemr.h"
   9
  10 #define MIN_SIZE                                12
  11 #define SIZEOF_STRUCT_MEM               (sizeof(struct mem)+(((sizeof(struct mem)%MEM_ALIGNMENT)==0)?0:(4-(sizeof(struct mem)%MEM_ALIGNMENT))))
  12
  13 struct mem {
  14         u32 next,prev;
  15         u32 used;
  16 };
  17
  18 static struct mem *ram_free;
  19 static struct mem *ram_end;
  20 static u8 ram_block[sizeof(struct mem)+MEM_SIZE+MEM_ALIGNMENT];
  21
  22 static void plug_holes(struct mem *rmem)
  23 {
  24         struct mem *nmem;
  25         struct mem *pmem;
  26
  27         nmem = (struct mem*)&ram_block[rmem->next];
  28         if(rmem!=nmem && nmem->used==0 && (u8_t*)nmem!=(u8_t*)ram_end) {
  29                 if(ram_free==nmem) ram_free = rmem;
  30
  31                 rmem->next = nmem->next;
  32                 ((struct mem*)&ram_block[nmem->next])->prev = (u8_t*)rmem -     ram_block;
  33         }
  34
  35         pmem = (struct mem*)&ram_block[rmem->prev];
  36         if(pmem!=rmem && pmem->used==0) {
  37                 if(ram_free==rmem) ram_free = pmem;
  38                 pmem->next = rmem->next;
  39                 ((struct mem*)&ram_block[rmem->next])->prev = (u8_t*)pmem - ram_block;
  40         }
  41 }
  42
  43 void btmemr_init()
  44 {
  45         u32 level;
  46         struct mem *rmem;
  47
  48         MEMSET(ram_block,0,MEM_SIZE);
  49
  50         _CPU_ISR_Disable(level);
  51         rmem = (struct mem*)ram_block;
  52         rmem->next = MEM_SIZE;
  53         rmem->prev = 0;
  54         rmem->used = 0;
  55
  56         ram_end = (struct mem*)&ram_block[MEM_SIZE];
  57         ram_end->used = 1;
  58         ram_end->prev = MEM_SIZE;
  59         ram_end->next = MEM_SIZE;
  60
  61         ram_free = (struct mem*)ram_block;
  62         _CPU_ISR_Restore(level);
  63 }
  64
  65 void* btmemr_malloc(u32 size)
  66 {
  67         u32 level;
  68         u32 ptr,ptr2;
  69         struct mem *rmem,*rmem2;
  70
  71         if(size==0) return NULL;
  72
  73         if(size%MEM_ALIGNMENT) size += MEM_ALIGNMENT - ((size+SIZEOF_STRUCT_MEM)%SIZEOF_STRUCT_MEM);
  74         if(size>MEM_SIZE) return NULL;
  75
  76         _CPU_ISR_Disable(level);
  77         for(ptr = (u8_t*)ram_free - ram_block;ptr<MEM_SIZE;ptr=((struct mem*)&ram_block[ptr])->next) {
  78                 rmem = (struct mem*)&ram_block[ptr];
  79                 if(!rmem->used && rmem->next - (ptr + SIZEOF_STRUCT_MEM)>=size + SIZEOF_STRUCT_MEM) {
  80                         ptr2 = ptr + SIZEOF_STRUCT_MEM + size;
  81                         rmem2 = (struct mem*)&ram_block[ptr2];
  82
  83                         rmem2->prev = ptr;
  84                         rmem2->next = rmem->next;
  85                         rmem->next = ptr2;
  86                         if(rmem->next!=MEM_SIZE) ((struct mem*)&ram_block[rmem2->next])->prev = ptr2;
  87
  88                         rmem2->used = 0;
  89                         rmem->used = 1;
  90
  91                         if(rmem==ram_free) {
  92                                 while(ram_free->used && ram_free!=ram_end) ram_free = (struct mem*)&ram_block[ram_free->next];
  93                         }
  94
  95                         _CPU_ISR_Restore(level);
  96                         return (u8_t*)rmem+SIZEOF_STRUCT_MEM;
  97                 }
  98         }
  99         _CPU_ISR_Restore(level);
 100         return NULL;
 101 }
 102
 103 void btmemr_free(void *ptr)
 104 {
 105         u32 level;
 106         struct mem *rmem;
 107
 108         if(ptr==NULL) return;
 109         if((u8_t*)ptr<(u8_t*)ram_block || (u8_t*)ptr>=(u8_t*)ram_end) return;
 110
 111         _CPU_ISR_Disable(level);
 112         rmem = (struct mem*)((u8_t*)ptr - SIZEOF_STRUCT_MEM);
 113         rmem->used = 0;
 114
 115         if(rmem<ram_free) ram_free = rmem;
 116
 117         plug_holes(rmem);
 118         _CPU_ISR_Restore(level);
 119 }
 120
 121 void* btmemr_realloc(void *ptr,u32 newsize)
 122 {
 123         u32 level;
 124         u32 size,ptr1,ptr2;
 125         struct mem *rmem,*rmem2;
 126
 127         if(newsize%MEM_ALIGNMENT) newsize += MEM_ALIGNMENT - ((newsize + SIZEOF_STRUCT_MEM)%MEM_ALIGNMENT);
 128         if(newsize>MEM_SIZE) return NULL;
 129         if((u8_t*)ptr<(u8_t*)ram_block || (u8_t*)ptr>=(u8_t*)ram_end) {
 130                 ERROR("memr_realloc: illegal memory.\n");
 131                 return ptr;
 132         }
 133
 134         _CPU_ISR_Disable(level);
 135         rmem = (struct mem*)((u8_t*)ptr - SIZEOF_STRUCT_MEM);
 136         ptr1 = (u8_t*)rmem - ram_block;
 137         size = rmem->next - ptr1 - SIZEOF_STRUCT_MEM;
 138
 139         if(newsize+SIZEOF_STRUCT_MEM+MIN_SIZE<size) {
 140                 ptr2 = ptr1 + SIZEOF_STRUCT_MEM + newsize;
 141                 rmem2 = (struct mem*)&ram_block[ptr2];
 142                 rmem2->used = 0;
 143                 rmem2->next = rmem->next;
 144                 rmem2->prev = ptr1;
 145                 rmem->next = ptr2;
 146                 if(rmem2->next!=MEM_SIZE) ((struct mem*)&ram_block[rmem2->next])->prev = ptr2;
 147
 148                 plug_holes(rmem2);
 149         }
 150         _CPU_ISR_Restore(level);
 151
 152         return ptr;
 153 }
 154
 155 void* btmemr_reallocm(void *ptr,u32 newsize)
 156 {
 157         void *nmem;
 158
 159         nmem = btmemr_malloc(newsize);
 160         if(nmem==NULL) return btmemr_realloc(ptr,newsize);
 161
 162         MEMCPY(nmem,ptr,newsize);
 163         btmemr_free(ptr);
 164
 165         return nmem;
 166 }