LUFA/MemoryAllocator/DynAlloc.c

   1 /*
   2              LUFA Library
   3      Copyright (C) Dean Camera, 2009.
   4
   5   dean [at] fourwalledcubicle [dot] com
   6       www.fourwalledcubicle.com
   7 */
   8
   9 /*
  10   Copyright 2009  Dean Camera (dean [at] fourwalledcubicle [dot] com)
  11
  12   Permission to use, copy, modify, and distribute this software
  13   and its documentation for any purpose and without fee is hereby
  14   granted, provided that the above copyright notice appear in all
  15   copies and that both that the copyright notice and this
  16   permission notice and warranty disclaimer appear in supporting
  17   documentation, and that the name of the author not be used in
  18   advertising or publicity pertaining to distribution of the
  19   software without specific, written prior permission.
  20
  21   The author disclaim all warranties with regard to this
  22   software, including all implied warranties of merchantability
  23   and fitness.  In no event shall the author be liable for any
  24   special, indirect or consequential damages or any damages
  25   whatsoever resulting from loss of use, data or profits, whether
  26   in an action of contract, negligence or other tortious action,
  27   arising out of or in connection with the use or performance of
  28   this software.
  29 */
  30
  31 #define  INCLUDE_FROM_DYNALLOC_C
  32 #include "DynAlloc.h"
  33
  34 struct
  35 {
  36         char    Mem_Heap[NUM_BLOCKS * BLOCK_SIZE];
  37         void*   Mem_Handles[NUM_HANDLES];
  38         uint8_t Mem_Block_Flags[(NUM_BLOCKS / 4) + ((NUM_BLOCKS % 4) ? 1 : 0)];
  39         uint8_t FlagMaskLookupMask[4];
  40         uint8_t FlagMaskLookupNum[4];
  41 } Mem_MemData = {FlagMaskLookupMask:  {(3 << 0), (3 << 2), (3 << 4), (3 << 6)},
  42                  FlagMaskLookupNum:   {      0,        2,        4,        6}};
  43
  44 static uint8_t Mem_GetBlockFlags(const Block_Number_t BlockNum)
  45 {
  46         const Block_Number_t BlockIndex    = (BlockNum >> 2);
  47         const uint8_t        FlagMask      = Mem_MemData.FlagMaskLookupMask[BlockNum & 0x03];
  48         const uint8_t        FlagMaskShift = Mem_MemData.FlagMaskLookupNum[BlockNum & 0x03];
  49
  50         return ((Mem_MemData.Mem_Block_Flags[BlockIndex] & FlagMask) >> FlagMaskShift);
  51 }
  52
  53 static void Mem_SetBlockFlags(const Block_Number_t BlockNum, const uint8_t Flags)
  54 {
  55         const Block_Number_t BlockIndex    = (BlockNum >> 2);
  56         const uint8_t        FlagMask      = Mem_MemData.FlagMaskLookupMask[BlockNum & 0x03];
  57         const uint8_t        FlagMaskShift = Mem_MemData.FlagMaskLookupNum[BlockNum & 0x03];
  58
  59         Mem_MemData.Mem_Block_Flags[BlockIndex] &= ~FlagMask;
  60         Mem_MemData.Mem_Block_Flags[BlockIndex] |= (Flags << FlagMaskShift);
  61 }
  62
  63 static inline void Mem_Defrag(void)
  64 {
  65         Block_Number_t FreeStartBlock = 0;
  66         char*          FreeStartPtr   = NULL;
  67         char*          UsedStartPtr   = NULL;
  68         Block_Number_t CurrBlock;
  69
  70         for (CurrBlock = 0; CurrBlock < NUM_BLOCKS; CurrBlock++)
  71         {
  72                 if (!(Mem_GetBlockFlags(CurrBlock) & BLOCK_USED_MASK))
  73                 {
  74                         FreeStartPtr   = &Mem_MemData.Mem_Heap[CurrBlock * BLOCK_SIZE];
  75                         FreeStartBlock = CurrBlock;
  76                         break;
  77                 }
  78         }
  79
  80         if (FreeStartPtr == NULL)
  81           return;
  82
  83         while (++CurrBlock < NUM_BLOCKS)
  84         {
  85                 uint8_t CurrBlockFlags = Mem_GetBlockFlags(CurrBlock);
  86
  87                 if (CurrBlockFlags & BLOCK_USED_MASK)
  88                 {
  89                         UsedStartPtr = &Mem_MemData.Mem_Heap[CurrBlock * BLOCK_SIZE];
  90
  91                         for (Handle_Number_t HandleNum = 0; HandleNum < NUM_HANDLES; HandleNum++)
  92                         {
  93                                 if (Mem_MemData.Mem_Handles[HandleNum] == UsedStartPtr)
  94                                 {
  95                                         Mem_MemData.Mem_Handles[HandleNum] = FreeStartPtr;
  96                                         break;
  97                                 }
  98                         }
  99
 100                         memcpy(FreeStartPtr, UsedStartPtr, BLOCK_SIZE);
 101                         FreeStartPtr += BLOCK_SIZE;
 102
 103                         Mem_SetBlockFlags(FreeStartBlock++, CurrBlockFlags);
 104                         Mem_SetBlockFlags(CurrBlock, 0);
 105                 }
 106         }
 107 }
 108
 109 static inline bool Mem_FindFreeBlocks(Block_Number_t* const RetStartPtr, const Block_Number_t Blocks)
 110 {
 111         Block_Number_t FreeInCurrSec = 0;
 112
 113         for (Block_Number_t CurrBlock = 0; CurrBlock < NUM_BLOCKS; CurrBlock++)
 114         {
 115                 if (Mem_GetBlockFlags(CurrBlock) & BLOCK_USED_MASK)
 116                   FreeInCurrSec = 0;
 117                 else
 118                   FreeInCurrSec++;
 119
 120                 if (FreeInCurrSec >= Blocks)
 121                 {
 122                         *RetStartPtr = CurrBlock;
 123                         return true;
 124                 }
 125         }
 126
 127         return false;
 128 }
 129
 130 Mem_Handle_t Mem_Alloc(const Alloc_Size_t Bytes)
 131 {
 132         Block_Number_t ReqBlocks = (Bytes / BLOCK_SIZE);
 133         Block_Number_t StartBlock;
 134
 135         if (Bytes % BLOCK_SIZE)
 136           ReqBlocks++;
 137
 138         if (!(Mem_FindFreeBlocks(&StartBlock, ReqBlocks)))
 139         {
 140                 Mem_Defrag();
 141
 142                 if (!(Mem_FindFreeBlocks(&StartBlock, ReqBlocks)))
 143                   return NULL;
 144         }
 145
 146         for (Block_Number_t UsedBlock = 0; UsedBlock < (ReqBlocks - 1); UsedBlock++)
 147           Mem_SetBlockFlags((StartBlock + UsedBlock), (BLOCK_USED_MASK | BLOCK_LINKED_MASK));
 148
 149         Mem_SetBlockFlags((StartBlock + (ReqBlocks - 1)), BLOCK_USED_MASK);
 150
 151         for (Handle_Number_t AllocEntry = 0; AllocEntry < NUM_HANDLES; AllocEntry++)
 152         {
 153                 Mem_Handle_t CurrHdl = (Mem_Handle_t)&Mem_MemData.Mem_Handles[AllocEntry];
 154
 155                 if (DEREF(CurrHdl, void*) == NULL)
 156                 {
 157                         DEREF(CurrHdl, void*) = &Mem_MemData.Mem_Heap[StartBlock * BLOCK_SIZE];
 158                         return CurrHdl;
 159                 }
 160         }
 161
 162         return NULL;
 163 }
 164
 165 Mem_Handle_t Mem_Realloc(Mem_Handle_t CurrAllocHdl, const Alloc_Size_t Bytes)
 166 {
 167         Mem_Free(CurrAllocHdl);
 168         return Mem_Alloc(Bytes);
 169 }
 170
 171 Mem_Handle_t Mem_Calloc(const Alloc_Size_t Bytes)
 172 {
 173         Mem_Handle_t AllocHdl = Mem_Alloc(Bytes);
 174
 175         if (AllocHdl != NULL)
 176           memset(DEREF(AllocHdl, void*), 0x00, Bytes);
 177
 178         return AllocHdl;
 179 }
 180
 181 void Mem_Free(Mem_Handle_t CurrAllocHdl)
 182 {
 183         char*          MemBlockPtr = DEREF(CurrAllocHdl, char*);
 184         Block_Number_t CurrBlock   = ((uint16_t)(MemBlockPtr - Mem_MemData.Mem_Heap) / BLOCK_SIZE);
 185         uint8_t        CurrBlockFlags;
 186
 187         if ((CurrAllocHdl == NULL) || (MemBlockPtr == NULL))
 188           return;
 189
 190         do
 191         {
 192                 CurrBlockFlags = Mem_GetBlockFlags(CurrBlock);
 193                 Mem_SetBlockFlags(CurrBlock, 0);
 194
 195                 CurrBlock++;
 196         }
 197         while (CurrBlockFlags & BLOCK_LINKED_MASK);
 198
 199         DEREF(CurrAllocHdl, void*) = NULL;
 200 }
 201
 202 Block_Number_t Mem_TotalFreeBlocks(void)
 203 {
 204         Block_Number_t FreeBlocks = 0;
 205
 206         for (Block_Number_t CurrBlock = 0; CurrBlock < NUM_BLOCKS; CurrBlock++)
 207         {
 208                 if (!(Mem_GetBlockFlags(CurrBlock) & BLOCK_USED_MASK))
 209                   FreeBlocks++;
 210         }
 211
 212         return FreeBlocks;
 213 }
 214
 215 Handle_Number_t Mem_TotalFreeHandles(void)
 216 {
 217         Handle_Number_t FreeHandles = 0;
 218
 219         for (Handle_Number_t CurrHandle = 0; CurrHandle < NUM_HANDLES; CurrHandle++)
 220         {
 221                 if (Mem_MemData.Mem_Handles[CurrHandle] == NULL)
 222                   FreeHandles++;
 223         }
 224
 225         return FreeHandles;
 226 }