ARM7 based quadrocopter
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

186 lines
5.7KB

  1. /***************************************************************************
  2. * sam7fc - dynamic Memory Allocation *
  3. * *
  4. * Copyright (C) 02/2008 by Olaf Rempel *
  5. * razzor@kopf-tisch.de *
  6. * *
  7. * This program is free software; you can redistribute it and/or modify *
  8. * it under the terms of the GNU General Public License as published by *
  9. * the Free Software Foundation; version 2 of the License *
  10. * *
  11. * This program is distributed in the hope that it will be useful, *
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of *
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
  14. * GNU General Public License for more details. *
  15. * *
  16. * You should have received a copy of the GNU General Public License *
  17. * along with this program; if not, write to the *
  18. * Free Software Foundation, Inc., *
  19. * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
  20. ***************************************************************************/
  21. #include <stdint.h>
  22. #include <stdio.h>
  23. #include "AT91SAM7S256.h"
  24. #include "board.h"
  25. #include "memalloc.h"
  26. // TODO: make generic version and find better place
  27. #define ALIGN(x) (((x) +3) & ~3)
  28. /* extern symbols, defined in ldscript */
  29. extern void * __bss_end__;
  30. extern void * __stack_top__;
  31. static uint32_t static_alloc_pos;
  32. void * static_alloc(uint32_t size)
  33. {
  34. if (static_alloc_pos == 0)
  35. static_alloc_pos = ALIGN((uint32_t)&__bss_end__);
  36. uint32_t retval = static_alloc_pos;
  37. size = ALIGN(size);
  38. static_alloc_pos += size;
  39. return (void *)retval;
  40. }
  41. uint32_t static_alloc_used(void)
  42. {
  43. return static_alloc_pos - (uint32_t)&__bss_end__;
  44. }
  45. // TODO: find a better place
  46. uint32_t next_powerof2(uint32_t value)
  47. {
  48. uint32_t i;
  49. value--;
  50. for (i = 1; i < 32; i = i * 2)
  51. value |= value >> i;
  52. return value +1;
  53. }
  54. /*
  55. * (pos->next) - (pos) = size_of_the_chunk
  56. * we use the lower 2 bits of the pointers to store some information.
  57. * (since they point to 4byte aligned structs == are always zero)
  58. */
  59. #define FLAG_CHUNKINUSE 0x01
  60. #define FLAG_ENDOFBLOCK 0x02
  61. #define FLAG_MASK 0x03
  62. #define SET_FLAGS(p, f) ((typeof (p))((uint32_t)(p) | (f)))
  63. #define GET_FLAGS(p) ((uint32_t)((p)->next) & FLAG_MASK)
  64. #define GET_NEXT(p) ((typeof (p))((uint32_t)((p)->next) & ~(FLAG_MASK)))
  65. struct memchunk {
  66. struct memchunk *next;
  67. uint8_t data[0];
  68. };
  69. static struct memchunk first_memchunk;
  70. static struct memchunk *last_endblock;
  71. /* add a new block to the dynamic memory allocator */
  72. static struct memchunk * alloc_add(uint32_t size)
  73. {
  74. size = ALIGN(size);
  75. printf("alloc_add(%ld)\n\r", size);
  76. /* struct at begin of memory block */
  77. struct memchunk *newblock = static_alloc(size);
  78. /* struct at end of memory block */
  79. struct memchunk *endblock = (void *)((uint32_t)newblock + size - sizeof(struct memchunk));
  80. /* link between them */
  81. newblock->next = endblock;
  82. /* next pointer of last block goes to NULL */
  83. endblock->next = SET_FLAGS(NULL, FLAG_ENDOFBLOCK);
  84. /* first block add? */
  85. if (first_memchunk.next == NULL) {
  86. first_memchunk.next = newblock;
  87. last_endblock = endblock;
  88. } else {
  89. /* the old endblock points to the new block, but keeps its flag */
  90. last_endblock->next = SET_FLAGS(newblock, FLAG_ENDOFBLOCK);
  91. last_endblock = endblock;
  92. }
  93. return newblock;
  94. }
  95. void * alloc(uint32_t size)
  96. {
  97. size = ALIGN(size) + sizeof(struct memchunk);
  98. /* found & search are always real pointers (without flags) */
  99. struct memchunk *found = NULL;
  100. struct memchunk *search = GET_NEXT(&first_memchunk);
  101. while (search && GET_NEXT(search)) {
  102. /* already used chunk OR a endofblock structure */
  103. if (GET_FLAGS(search) & (FLAG_CHUNKINUSE | FLAG_ENDOFBLOCK))
  104. goto try_next;
  105. uint32_t nextflags = GET_FLAGS(GET_NEXT(search)) & (FLAG_CHUNKINUSE | FLAG_ENDOFBLOCK);
  106. /* next chunk is unused AND an endofblock structure AND is not the last */
  107. if (nextflags == FLAG_ENDOFBLOCK && GET_NEXT(search) != last_endblock) {
  108. search->next = SET_FLAGS(GET_NEXT(GET_NEXT(search)), 0);
  109. continue;
  110. /* next chunk is unused -> chain them and then retry */
  111. } else if (nextflags == 0) {
  112. search->next = SET_FLAGS(GET_NEXT(GET_NEXT(search)), 0);
  113. continue;
  114. }
  115. uint32_t length = (uint32_t)GET_NEXT(search) - (uint32_t)search;
  116. /* perfect match */
  117. if (length == size) {
  118. found = search;
  119. break;
  120. /* chunk is bigger than needed, remember smallest */
  121. } else if (length >= size) {
  122. if (!found || (((uint32_t)GET_NEXT(found) - (uint32_t)found) > length))
  123. found = search;
  124. }
  125. try_next:
  126. search = GET_NEXT(search);
  127. }
  128. /* nothing useful found, add a new block of memory */
  129. if (!found)
  130. found = alloc_add(MAX(1024, size));
  131. /* if we split the chunk, the remaining piece must be large enough */
  132. if (((uint32_t)GET_NEXT(found) - (uint32_t)found) > (size + 2 * sizeof(struct memchunk))) {
  133. struct memchunk *end = (void *)((uint32_t)found + size);
  134. end->next = SET_FLAGS(GET_NEXT(found), 0);
  135. found->next = SET_FLAGS(end, FLAG_CHUNKINUSE);
  136. } else {
  137. found->next = SET_FLAGS(GET_NEXT(found), FLAG_CHUNKINUSE);
  138. }
  139. return (void *)(found +1);
  140. }
  141. void free(void *p)
  142. {
  143. /* mark the chunk as unused and keep the pointer */
  144. struct memchunk *chunk = (struct memchunk *)p -1;
  145. chunk->next = SET_FLAGS(GET_NEXT(chunk), 0);
  146. }