libogc/lwp_heap.c

   1 #include <stdlib.h>
   2 #include <system.h>
   3 #include <processor.h>
   4 #include <sys_state.h>
   5 #include <lwp_config.h>
   6
   7 #include "lwp_heap.h"
   8
   9
  10 u32 __lwp_heap_init(heap_cntrl *theheap,void *start_addr,u32 size,u32 pg_size)
  11 {
  12         u32 dsize,level;
  13         heap_block *block;
  14
  15         if(!__lwp_heap_pgsize_valid(pg_size) || size<HEAP_MIN_SIZE) return 0;
  16
  17         _CPU_ISR_Disable(level);
  18         theheap->pg_size = pg_size;
  19         dsize = (size - HEAP_OVERHEAD);
  20
  21         block = (heap_block*)start_addr;
  22         block->back_flag = HEAP_DUMMY_FLAG;
  23         block->front_flag = dsize;
  24         block->next     = __lwp_heap_tail(theheap);
  25         block->prev = __lwp_heap_head(theheap);
  26
  27         theheap->start = block;
  28         theheap->first = block;
  29         theheap->perm_null = NULL;
  30         theheap->last = block;
  31
  32         block = __lwp_heap_nextblock(block);
  33         block->back_flag = dsize;
  34         block->front_flag = HEAP_DUMMY_FLAG;
  35         theheap->final = block;
  36         _CPU_ISR_Restore(level);
  37
  38         return (dsize - HEAP_BLOCK_USED_OVERHEAD);
  39 }
  40
  41 void* __lwp_heap_allocate(heap_cntrl *theheap,u32 size)
  42 {
  43         u32 excess;
  44         u32 dsize;
  45         heap_block *block;
  46         heap_block *next_block;
  47         heap_block *tmp_block;
  48         void *ptr;
  49         u32 offset,level;
  50
  51
  52         if(size>=(-1-HEAP_BLOCK_USED_OVERHEAD)) return NULL;
  53
  54         _CPU_ISR_Disable(level);
  55         excess = (size % theheap->pg_size);
  56         dsize = (size + theheap->pg_size + HEAP_BLOCK_USED_OVERHEAD);
  57
  58         if(excess)
  59                 dsize += (theheap->pg_size - excess);
  60
  61         if(dsize<sizeof(heap_block)) dsize = sizeof(heap_block);
  62
  63         for(block=theheap->first;;block=block->next) {
  64                 if(block==__lwp_heap_tail(theheap)) {
  65                         _CPU_ISR_Restore(level);
  66                         return NULL;
  67                 }
  68                 if(block->front_flag>=dsize) break;
  69         }
  70
  71         if((block->front_flag-dsize)>(theheap->pg_size+HEAP_BLOCK_USED_OVERHEAD)) {
  72                 block->front_flag -= dsize;
  73                 next_block = __lwp_heap_nextblock(block);
  74                 next_block->back_flag = block->front_flag;
  75
  76                 tmp_block = __lwp_heap_blockat(next_block,dsize);
  77                 tmp_block->back_flag = next_block->front_flag = __lwp_heap_buildflag(dsize,HEAP_BLOCK_USED);
  78
  79                 ptr = __lwp_heap_startuser(next_block);
  80         } else {
  81                 next_block = __lwp_heap_nextblock(block);
  82                 next_block->back_flag = __lwp_heap_buildflag(block->front_flag,HEAP_BLOCK_USED);
  83
  84                 block->front_flag = next_block->back_flag;
  85                 block->next->prev = block->prev;
  86                 block->prev->next = block->next;
  87
  88                 ptr = __lwp_heap_startuser(block);
  89         }
  90
  91         offset = (theheap->pg_size - ((u32)ptr&(theheap->pg_size-1)));
  92         ptr += offset;
  93         *(((u32*)ptr)-1) = offset;
  94         _CPU_ISR_Restore(level);
  95
  96         return ptr;
  97 }
  98
  99 BOOL __lwp_heap_free(heap_cntrl *theheap,void *ptr)
 100 {
 101         heap_block *block;
 102         heap_block *next_block;
 103         heap_block *new_next;
 104         heap_block *prev_block;
 105         heap_block *tmp_block;
 106         u32 dsize,level;
 107
 108         _CPU_ISR_Disable(level);
 109
 110         block = __lwp_heap_usrblockat(ptr);
 111         if(!__lwp_heap_blockin(theheap,block) || __lwp_heap_blockfree(block)) {
 112                 _CPU_ISR_Restore(level);
 113                 return FALSE;
 114         }
 115
 116         dsize = __lwp_heap_blocksize(block);
 117         next_block = __lwp_heap_blockat(block,dsize);
 118
 119         if(!__lwp_heap_blockin(theheap,next_block) || (block->front_flag!=next_block->back_flag)) {
 120                 _CPU_ISR_Restore(level);
 121                 return FALSE;
 122         }
 123
 124         if(__lwp_heap_prev_blockfree(block)) {
 125                 prev_block = __lwp_heap_prevblock(block);
 126                 if(!__lwp_heap_blockin(theheap,prev_block)) {
 127                         _CPU_ISR_Restore(level);
 128                         return FALSE;
 129                 }
 130
 131                 if(__lwp_heap_blockfree(next_block)) {
 132                         prev_block->front_flag += next_block->front_flag+dsize;
 133                         tmp_block = __lwp_heap_nextblock(prev_block);
 134                         tmp_block->back_flag = prev_block->front_flag;
 135                         next_block->next->prev = next_block->prev;
 136                         next_block->prev->next = next_block->next;
 137                 } else {
 138                         prev_block->front_flag = next_block->back_flag = prev_block->front_flag+dsize;
 139                 }
 140         } else if(__lwp_heap_blockfree(next_block)) {
 141                 block->front_flag = dsize+next_block->front_flag;
 142                 new_next = __lwp_heap_nextblock(block);
 143                 new_next->back_flag = block->front_flag;
 144                 block->next = next_block->next;
 145                 block->prev = next_block->prev;
 146                 next_block->prev->next = block;
 147                 next_block->next->prev = block;
 148
 149                 if(theheap->first==next_block) theheap->first = block;
 150         } else {
 151                 next_block->back_flag = block->front_flag = dsize;
 152                 block->prev = __lwp_heap_head(theheap);
 153                 block->next = theheap->first;
 154                 theheap->first = block;
 155                 block->next->prev = block;
 156         }
 157         _CPU_ISR_Restore(level);
 158
 159         return TRUE;
 160 }
 161
 162 u32 __lwp_heap_getinfo(heap_cntrl *theheap,heap_iblock *theinfo)
 163 {
 164         u32 not_done = 1;
 165         heap_block *theblock = NULL;
 166         heap_block *nextblock = NULL;
 167
 168         theinfo->free_blocks = 0;
 169         theinfo->free_size = 0;
 170         theinfo->used_blocks = 0;
 171         theinfo->used_size = 0;
 172
 173         if(!__sys_state_up(__sys_state_get())) return 1;
 174
 175         theblock = theheap->start;
 176         if(theblock->back_flag!=HEAP_DUMMY_FLAG) return 2;
 177
 178         while(not_done) {
 179                 if(__lwp_heap_blockfree(theblock)) {
 180                         theinfo->free_blocks++;
 181                         theinfo->free_size += __lwp_heap_blocksize(theblock);
 182                 } else {
 183                         theinfo->used_blocks++;
 184                         theinfo->used_size += __lwp_heap_blocksize(theblock);
 185                 }
 186
 187                 if(theblock->front_flag!=HEAP_DUMMY_FLAG) {
 188                         nextblock = __lwp_heap_nextblock(theblock);
 189                         if(theblock->front_flag!=nextblock->back_flag) return 2;
 190                 }
 191
 192                 if(theblock->front_flag==HEAP_DUMMY_FLAG)
 193                         not_done = 0;
 194                 else
 195                         theblock = nextblock;
 196         }
 197         return 0;
 198 }