Michael Spencer / Smoothie

Fork of Smoothie to port to mbed non-LPC targets.

Dependencies:   mbed

Fork of Smoothie by Stéphane Cachat

libs/MemoryPool.cpp@3:f151d08d335c, 2014-03-02 (annotated)

Committer:
Bigcheese
Date:
Sun Mar 02 06:33:08 2014 +0000
Revision:
3:f151d08d335c
Parent:
2:1df0b61d3b5a 
Bunch of stuff. Need to locally merge in updated USB changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Michael J. Spencer 2:1df0b61d3b5a 1 #include "MemoryPool.h"
Michael J. Spencer 2:1df0b61d3b5a 2
Michael J. Spencer 2:1df0b61d3b5a 3 #include <cstdio>
Michael J. Spencer 2:1df0b61d3b5a 4
Michael J. Spencer 2:1df0b61d3b5a 5 #define offset(x) (((uint8_t*) x) - ((uint8_t*) this->base))
Michael J. Spencer 2:1df0b61d3b5a 6
Michael J. Spencer 2:1df0b61d3b5a 7 typedef struct __attribute__ ((packed))
Michael J. Spencer 2:1df0b61d3b5a 8 {
Michael J. Spencer 2:1df0b61d3b5a 9 uint32_t next :31;
Michael J. Spencer 2:1df0b61d3b5a 10 uint32_t used :1;
Michael J. Spencer 2:1df0b61d3b5a 11
Michael J. Spencer 2:1df0b61d3b5a 12 uint8_t data[];
Michael J. Spencer 2:1df0b61d3b5a 13 } _poolregion;
Michael J. Spencer 2:1df0b61d3b5a 14
Michael J. Spencer 2:1df0b61d3b5a 15 MemoryPool* MemoryPool::first = NULL;
Michael J. Spencer 2:1df0b61d3b5a 16
Michael J. Spencer 2:1df0b61d3b5a 17 MemoryPool::MemoryPool(void* base, uint16_t size)
Michael J. Spencer 2:1df0b61d3b5a 18 {
Michael J. Spencer 2:1df0b61d3b5a 19 this->base = base;
Michael J. Spencer 2:1df0b61d3b5a 20 this->size = size;
Michael J. Spencer 2:1df0b61d3b5a 21
Michael J. Spencer 2:1df0b61d3b5a 22 ((_poolregion*) base)->used = 0;
Michael J. Spencer 2:1df0b61d3b5a 23 ((_poolregion*) base)->next = size;
Michael J. Spencer 2:1df0b61d3b5a 24
Michael J. Spencer 2:1df0b61d3b5a 25 // insert ourselves into head of LL
Michael J. Spencer 2:1df0b61d3b5a 26 next = first;
Michael J. Spencer 2:1df0b61d3b5a 27 first = this;
Michael J. Spencer 2:1df0b61d3b5a 28 }
Michael J. Spencer 2:1df0b61d3b5a 29
Michael J. Spencer 2:1df0b61d3b5a 30 MemoryPool::~MemoryPool()
Michael J. Spencer 2:1df0b61d3b5a 31 {
Michael J. Spencer 2:1df0b61d3b5a 32 MDEBUG("Pool %p destroyed: region %p (%d)\n", this, base, size);
Michael J. Spencer 2:1df0b61d3b5a 33
Michael J. Spencer 2:1df0b61d3b5a 34 // remove ourselves from the LL
Michael J. Spencer 2:1df0b61d3b5a 35 if (first == this)
Michael J. Spencer 2:1df0b61d3b5a 36 { // special case: we're first
Michael J. Spencer 2:1df0b61d3b5a 37 first = this->next;
Michael J. Spencer 2:1df0b61d3b5a 38 return;
Michael J. Spencer 2:1df0b61d3b5a 39 }
Michael J. Spencer 2:1df0b61d3b5a 40
Michael J. Spencer 2:1df0b61d3b5a 41 // otherwise search the LL for the previous pool
Michael J. Spencer 2:1df0b61d3b5a 42 MemoryPool* m = first;
Michael J. Spencer 2:1df0b61d3b5a 43 while (m)
Michael J. Spencer 2:1df0b61d3b5a 44 {
Michael J. Spencer 2:1df0b61d3b5a 45 if (m->next == this)
Michael J. Spencer 2:1df0b61d3b5a 46 {
Michael J. Spencer 2:1df0b61d3b5a 47 m->next = next;
Michael J. Spencer 2:1df0b61d3b5a 48 return;
Michael J. Spencer 2:1df0b61d3b5a 49 }
Michael J. Spencer 2:1df0b61d3b5a 50 m = m->next;
Michael J. Spencer 2:1df0b61d3b5a 51 }
Michael J. Spencer 2:1df0b61d3b5a 52 }
Michael J. Spencer 2:1df0b61d3b5a 53
Michael J. Spencer 2:1df0b61d3b5a 54 void* MemoryPool::alloc(size_t nbytes)
Michael J. Spencer 2:1df0b61d3b5a 55 {
Michael J. Spencer 2:1df0b61d3b5a 56 // nbytes = ceil(nbytes / 4) * 4
Michael J. Spencer 2:1df0b61d3b5a 57 if (nbytes & 3)
Michael J. Spencer 2:1df0b61d3b5a 58 nbytes += 4 - (nbytes & 3);
Michael J. Spencer 2:1df0b61d3b5a 59
Michael J. Spencer 2:1df0b61d3b5a 60 // start at the start
Michael J. Spencer 2:1df0b61d3b5a 61 _poolregion* p = ((_poolregion*) base);
Michael J. Spencer 2:1df0b61d3b5a 62
Michael J. Spencer 2:1df0b61d3b5a 63 // find the allocation size including our metadata
Michael J. Spencer 2:1df0b61d3b5a 64 uint16_t nsize = nbytes + sizeof(_poolregion);
Michael J. Spencer 2:1df0b61d3b5a 65
Michael J. Spencer 2:1df0b61d3b5a 66 MDEBUG("\tallocate %d bytes from %p\n", nsize, base);
Michael J. Spencer 2:1df0b61d3b5a 67
Michael J. Spencer 2:1df0b61d3b5a 68 // now we walk the list, looking for a sufficiently large free block
Michael J. Spencer 2:1df0b61d3b5a 69 do {
Michael J. Spencer 2:1df0b61d3b5a 70 MDEBUG("\t\tchecking %p (%s, %db)\n", p, (p->used?"used":"free"), p->next);
Michael J. Spencer 2:1df0b61d3b5a 71 if ((p->used == 0) && (p->next >= nsize))
Michael J. Spencer 2:1df0b61d3b5a 72 { // we found a free space that's big enough
Michael J. Spencer 2:1df0b61d3b5a 73 MDEBUG("\t\tFOUND free block at %p (%+d) with %d bytes\n", p, offset(p), p->next);
Michael J. Spencer 2:1df0b61d3b5a 74 // mark it as used
Michael J. Spencer 2:1df0b61d3b5a 75 p->used = 1;
Michael J. Spencer 2:1df0b61d3b5a 76
Michael J. Spencer 2:1df0b61d3b5a 77 // if there's free space at the end of this block
Michael J. Spencer 2:1df0b61d3b5a 78 if (p->next > nsize)
Michael J. Spencer 2:1df0b61d3b5a 79 {
Michael J. Spencer 2:1df0b61d3b5a 80 // q = p->next
Michael J. Spencer 2:1df0b61d3b5a 81 _poolregion* q = (_poolregion*) (((uint8_t*) p) + nsize);
Michael J. Spencer 2:1df0b61d3b5a 82
Michael J. Spencer 2:1df0b61d3b5a 83 MDEBUG("\t\twriting header to %p (%+d) (%d)\n", q, offset(q), p->next - nsize);
Michael J. Spencer 2:1df0b61d3b5a 84 // write a new block header into q
Michael J. Spencer 2:1df0b61d3b5a 85 q->used = 0;
Michael J. Spencer 2:1df0b61d3b5a 86 q->next = p->next - nsize;
Michael J. Spencer 2:1df0b61d3b5a 87
Michael J. Spencer 2:1df0b61d3b5a 88 // set our next to point to it
Michael J. Spencer 2:1df0b61d3b5a 89 p->next = nsize;
Michael J. Spencer 2:1df0b61d3b5a 90
Michael J. Spencer 2:1df0b61d3b5a 91 // sanity check
Michael J. Spencer 2:1df0b61d3b5a 92 if (offset(q) >= size)
Michael J. Spencer 2:1df0b61d3b5a 93 {
Michael J. Spencer 2:1df0b61d3b5a 94 // captain, we have a problem!
Michael J. Spencer 2:1df0b61d3b5a 95 // this can only happen if something has corrupted our heap, since we should simply fail to find a free block if it's full
Bigcheese 3:f151d08d335c 96 //__debugbreak();
Michael J. Spencer 2:1df0b61d3b5a 97 }
Michael J. Spencer 2:1df0b61d3b5a 98 }
Michael J. Spencer 2:1df0b61d3b5a 99
Michael J. Spencer 2:1df0b61d3b5a 100 // then return the data region for the block
Michael J. Spencer 2:1df0b61d3b5a 101 return &p->data;
Michael J. Spencer 2:1df0b61d3b5a 102 }
Michael J. Spencer 2:1df0b61d3b5a 103
Michael J. Spencer 2:1df0b61d3b5a 104 // p = p->next
Michael J. Spencer 2:1df0b61d3b5a 105 p = (_poolregion*) (((uint8_t*) p) + p->next);
Michael J. Spencer 2:1df0b61d3b5a 106
Michael J. Spencer 2:1df0b61d3b5a 107 // make sure we don't walk off the end
Michael J. Spencer 2:1df0b61d3b5a 108 } while (p <= (_poolregion*) (((uint8_t*)base) + size));
Michael J. Spencer 2:1df0b61d3b5a 109
Michael J. Spencer 2:1df0b61d3b5a 110 // fell off the end of the region!
Michael J. Spencer 2:1df0b61d3b5a 111 return NULL;
Michael J. Spencer 2:1df0b61d3b5a 112 }
Michael J. Spencer 2:1df0b61d3b5a 113
Michael J. Spencer 2:1df0b61d3b5a 114 void MemoryPool::dealloc(void* d)
Michael J. Spencer 2:1df0b61d3b5a 115 {
Michael J. Spencer 2:1df0b61d3b5a 116 _poolregion* p = (_poolregion*) (((uint8_t*) d) - sizeof(_poolregion));
Michael J. Spencer 2:1df0b61d3b5a 117 p->used = 0;
Michael J. Spencer 2:1df0b61d3b5a 118
Michael J. Spencer 2:1df0b61d3b5a 119 MDEBUG("\tdeallocating %p (%+d, %db)\n", p, offset(p), p->next);
Michael J. Spencer 2:1df0b61d3b5a 120
Michael J. Spencer 2:1df0b61d3b5a 121 // combine next block if it's free
Michael J. Spencer 2:1df0b61d3b5a 122 _poolregion* q = (_poolregion*) (((uint8_t*) p) + p->next);
Michael J. Spencer 2:1df0b61d3b5a 123 if (q->used == 0)
Michael J. Spencer 2:1df0b61d3b5a 124 {
Michael J. Spencer 2:1df0b61d3b5a 125 MDEBUG("\t\tCombining with next free region at %p, new size is %d\n", q, p->next + q->next);
Michael J. Spencer 2:1df0b61d3b5a 126
Michael J. Spencer 2:1df0b61d3b5a 127 // sanity check
Michael J. Spencer 2:1df0b61d3b5a 128 if (offset(q) > size)
Michael J. Spencer 2:1df0b61d3b5a 129 {
Michael J. Spencer 2:1df0b61d3b5a 130 // captain, we have a problem!
Michael J. Spencer 2:1df0b61d3b5a 131 // this can only happen if something has corrupted our heap, since we should simply fail to find a free block if it's full
Bigcheese 3:f151d08d335c 132 //__debugbreak();
Michael J. Spencer 2:1df0b61d3b5a 133 }
Michael J. Spencer 2:1df0b61d3b5a 134
Michael J. Spencer 2:1df0b61d3b5a 135 p->next += q->next;
Michael J. Spencer 2:1df0b61d3b5a 136 }
Michael J. Spencer 2:1df0b61d3b5a 137
Michael J. Spencer 2:1df0b61d3b5a 138 // walk the list to find previous block
Michael J. Spencer 2:1df0b61d3b5a 139 q = (_poolregion*) base;
Michael J. Spencer 2:1df0b61d3b5a 140 do {
Michael J. Spencer 2:1df0b61d3b5a 141 // check if q is the previous block
Michael J. Spencer 2:1df0b61d3b5a 142 if ((((uint8_t*) q) + q->next) == (uint8_t*) p) {
Michael J. Spencer 2:1df0b61d3b5a 143 // q is the previous block.
Michael J. Spencer 2:1df0b61d3b5a 144 if (q->used == 0)
Michael J. Spencer 2:1df0b61d3b5a 145 { // if q is free
Michael J. Spencer 2:1df0b61d3b5a 146 MDEBUG("\t\tCombining with previous free region at %p, new size is %d\n", q, p->next + q->next);
Michael J. Spencer 2:1df0b61d3b5a 147
Michael J. Spencer 2:1df0b61d3b5a 148 // combine!
Michael J. Spencer 2:1df0b61d3b5a 149 q->next += p->next;
Michael J. Spencer 2:1df0b61d3b5a 150
Michael J. Spencer 2:1df0b61d3b5a 151 // sanity check
Michael J. Spencer 2:1df0b61d3b5a 152 if ((offset(p) + p->next) >= size)
Michael J. Spencer 2:1df0b61d3b5a 153 {
Michael J. Spencer 2:1df0b61d3b5a 154 // captain, we have a problem!
Michael J. Spencer 2:1df0b61d3b5a 155 // this can only happen if something has corrupted our heap, since we should simply fail to find a free block if it's full
Bigcheese 3:f151d08d335c 156 //__debugbreak();
Michael J. Spencer 2:1df0b61d3b5a 157 }
Michael J. Spencer 2:1df0b61d3b5a 158 }
Michael J. Spencer 2:1df0b61d3b5a 159
Michael J. Spencer 2:1df0b61d3b5a 160 // we found previous block, return
Michael J. Spencer 2:1df0b61d3b5a 161 return;
Michael J. Spencer 2:1df0b61d3b5a 162 }
Michael J. Spencer 2:1df0b61d3b5a 163
Michael J. Spencer 2:1df0b61d3b5a 164 // return if last block
Michael J. Spencer 2:1df0b61d3b5a 165 if (offset(q) + q->next >= size)
Michael J. Spencer 2:1df0b61d3b5a 166 return;
Michael J. Spencer 2:1df0b61d3b5a 167
Michael J. Spencer 2:1df0b61d3b5a 168 // q = q->next
Michael J. Spencer 2:1df0b61d3b5a 169 q = (_poolregion*) (((uint8_t*) q) + q->next);
Michael J. Spencer 2:1df0b61d3b5a 170
Michael J. Spencer 2:1df0b61d3b5a 171 // if some idiot deallocates our memory region while we're using it, strange things can happen.
Michael J. Spencer 2:1df0b61d3b5a 172 // avoid an infinite loop in that case, however we'll still leak memory and may corrupt things
Michael J. Spencer 2:1df0b61d3b5a 173 if (q->next == 0)
Michael J. Spencer 2:1df0b61d3b5a 174 return;
Michael J. Spencer 2:1df0b61d3b5a 175
Michael J. Spencer 2:1df0b61d3b5a 176 // make sure we don't walk off the end
Michael J. Spencer 2:1df0b61d3b5a 177 } while (q < (_poolregion*) (((uint8_t*) base) + size));
Michael J. Spencer 2:1df0b61d3b5a 178 }
Michael J. Spencer 2:1df0b61d3b5a 179
Michael J. Spencer 2:1df0b61d3b5a 180 void MemoryPool::debug(StreamOutput* str)
Michael J. Spencer 2:1df0b61d3b5a 181 {
Michael J. Spencer 2:1df0b61d3b5a 182 _poolregion* p = (_poolregion*) base;
Michael J. Spencer 2:1df0b61d3b5a 183 uint32_t tot = 0;
Michael J. Spencer 2:1df0b61d3b5a 184 uint32_t free = 0;
Michael J. Spencer 2:1df0b61d3b5a 185 str->printf("Start: %ub MemoryPool at %p\n", size, p);
Michael J. Spencer 2:1df0b61d3b5a 186 do {
Michael J. Spencer 2:1df0b61d3b5a 187 str->printf("\tChunk at %p (%+4d): %s, %lu bytes\n", p, offset(p), (p->used?"used":"free"), p->next);
Michael J. Spencer 2:1df0b61d3b5a 188 tot += p->next;
Michael J. Spencer 2:1df0b61d3b5a 189 if (p->used == 0)
Michael J. Spencer 2:1df0b61d3b5a 190 free += p->next;
Michael J. Spencer 2:1df0b61d3b5a 191 if ((offset(p) + p->next >= size) || (p->next <= sizeof(_poolregion)))
Michael J. Spencer 2:1df0b61d3b5a 192 {
Michael J. Spencer 2:1df0b61d3b5a 193 str->printf("End: total %lub, free: %lub\n", tot, free);
Michael J. Spencer 2:1df0b61d3b5a 194 return;
Michael J. Spencer 2:1df0b61d3b5a 195 }
Michael J. Spencer 2:1df0b61d3b5a 196 p = (_poolregion*) (((uint8_t*) p) + p->next);
Michael J. Spencer 2:1df0b61d3b5a 197 } while (1);
Michael J. Spencer 2:1df0b61d3b5a 198 }
Michael J. Spencer 2:1df0b61d3b5a 199
Michael J. Spencer 2:1df0b61d3b5a 200 bool MemoryPool::has(void* p)
Michael J. Spencer 2:1df0b61d3b5a 201 {
Michael J. Spencer 2:1df0b61d3b5a 202 return ((p >= base) && (p < (void*) (((uint8_t*) base) + size)));
Michael J. Spencer 2:1df0b61d3b5a 203 }
Michael J. Spencer 2:1df0b61d3b5a 204
Michael J. Spencer 2:1df0b61d3b5a 205 uint32_t MemoryPool::free()
Michael J. Spencer 2:1df0b61d3b5a 206 {
Michael J. Spencer 2:1df0b61d3b5a 207 uint32_t free = 0;
Michael J. Spencer 2:1df0b61d3b5a 208
Michael J. Spencer 2:1df0b61d3b5a 209 _poolregion* p = (_poolregion*) base;
Michael J. Spencer 2:1df0b61d3b5a 210
Michael J. Spencer 2:1df0b61d3b5a 211 do {
Michael J. Spencer 2:1df0b61d3b5a 212 if (p->used == 0)
Michael J. Spencer 2:1df0b61d3b5a 213 free += p->next;
Michael J. Spencer 2:1df0b61d3b5a 214 if (offset(p) + p->next >= size)
Michael J. Spencer 2:1df0b61d3b5a 215 return free;
Michael J. Spencer 2:1df0b61d3b5a 216 if (p->next <= sizeof(_poolregion))
Michael J. Spencer 2:1df0b61d3b5a 217 return free;
Michael J. Spencer 2:1df0b61d3b5a 218 p = (_poolregion*) (((uint8_t*) p) + p->next);
Michael J. Spencer 2:1df0b61d3b5a 219 } while (1);
Michael J. Spencer 2:1df0b61d3b5a 220 }