lwbt/btmemb.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 "btmemb.h"
   9
  10 void btmemb_init(struct memb_blks *blk)
  11 {
  12         MEMSET(blk->mem,0,(MEM_ALIGN_SIZE(blk->size)+sizeof(u32))*blk->num);
  13 }
  14
  15 void* btmemb_alloc(struct memb_blks *blk)
  16 {
  17         s32 i;
  18         u32 *ptr;
  19         u32 level;
  20         void *p;
  21
  22         _CPU_ISR_Disable(level);
  23         ptr = (u32*)blk->mem;
  24         for(i=0;i<blk->num;i++) {
  25                 if(*ptr==0) {
  26                         ++(*ptr);
  27                         p = (ptr+1);
  28                         _CPU_ISR_Restore(level);
  29                         return p;
  30                 }
  31                 ptr = (u32*)(u8*)ptr+(MEM_ALIGN_SIZE(blk->size)+sizeof(u32));
  32         }
  33         _CPU_ISR_Restore(level);
  34         return NULL;
  35 }
  36
  37 u8 btmemb_free(struct memb_blks *blk,void *ptr)
  38 {
  39         u8 ref;
  40         s32 i;
  41         u32 level;
  42         u32 *ptr2,*ptr1;
  43
  44         _CPU_ISR_Disable(level);
  45         ptr1 = ptr;
  46         ptr2 = (u32*)blk->mem;
  47         for(i=0;i<blk->num;i++) {
  48                 if(ptr2==(ptr1 - 1)) {
  49                         ref = --(*ptr2);
  50                         _CPU_ISR_Restore(level);
  51                         return ref;
  52                 }
  53                 ptr2 = (u32*)(u8*)ptr2+(MEM_ALIGN_SIZE(blk->size)+sizeof(u32));
  54         }
  55         _CPU_ISR_Restore(level);
  56         return -1;
  57 }
  58
  59 u8 btmemb_ref(struct memb_blks *blk,void *ptr)
  60 {
  61         u8 ref;
  62         u32 *pref;
  63         u32 level;
  64
  65         _CPU_ISR_Disable(level);
  66         pref = ptr-sizeof(u32);
  67         ref = ++(*pref);
  68         _CPU_ISR_Restore(level);
  69         return ref;
  70 }