1 /*
2 * Copyright (c) 2016 Nordic Semiconductor ASA
3 * Copyright (c) 2016 Vinayak Kariappa Chettimada
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/types.h>
9 #include <string.h>
10
11 #include "util.h"
12
13 #include "mem.h"
14
mem_init(void * mem_pool,uint16_t mem_size,uint16_t mem_count,void ** mem_head)15 void mem_init(void *mem_pool, uint16_t mem_size, uint16_t mem_count,
16 void **mem_head)
17 {
18 *mem_head = mem_pool;
19
20 /* Store free mem_count after the list's next pointer at an 32-bit
21 * aligned memory location to ensure atomic read/write (in ARM for now).
22 */
23 *((uint16_t *)MROUND((uint8_t *)mem_pool + sizeof(mem_pool))) = mem_count;
24
25 /* Initialize next pointers to form a free list,
26 * next pointer is stored in the first 32-bit of each block
27 */
28 (void)memset(((uint8_t *)mem_pool + (mem_size * (--mem_count))), 0,
29 sizeof(mem_pool));
30 while (mem_count--) {
31 uint32_t next;
32
33 next = (uint32_t)((uint8_t *) mem_pool +
34 (mem_size * (mem_count + 1)));
35 memcpy(((uint8_t *)mem_pool + (mem_size * mem_count)),
36 (void *)&next, sizeof(next));
37 }
38 }
39
mem_acquire(void ** mem_head)40 void *mem_acquire(void **mem_head)
41 {
42 if (*mem_head) {
43 uint16_t free_count;
44 void *head;
45 void *mem;
46
47 /* Get the free count from the list and decrement it */
48 free_count = *((uint16_t *)MROUND((uint8_t *)*mem_head +
49 sizeof(mem_head)));
50 free_count--;
51
52 mem = *mem_head;
53 memcpy(&head, mem, sizeof(head));
54
55 /* Store free mem_count after the list's next pointer */
56 if (head) {
57 *((uint16_t *)MROUND((uint8_t *)head + sizeof(head))) =
58 free_count;
59 }
60
61 *mem_head = head;
62 return mem;
63 }
64
65 return NULL;
66 }
67
mem_release(void * mem,void ** mem_head)68 void mem_release(void *mem, void **mem_head)
69 {
70 uint16_t free_count = 0U;
71
72 /* Get the free count from the list and increment it */
73 if (*mem_head) {
74 free_count = *((uint16_t *)MROUND((uint8_t *)*mem_head +
75 sizeof(mem_head)));
76 }
77 free_count++;
78
79 memcpy(mem, mem_head, sizeof(mem));
80
81 /* Store free mem_count after the list's next pointer */
82 *((uint16_t *)MROUND((uint8_t *)mem + sizeof(mem))) = free_count;
83
84 *mem_head = mem;
85 }
86
mem_free_count_get(void * mem_head)87 uint16_t mem_free_count_get(void *mem_head)
88 {
89 uint16_t free_count = 0U;
90
91 /* Get the free count from the list */
92 if (mem_head) {
93 free_count = *((uint16_t *)MROUND((uint8_t *)mem_head +
94 sizeof(mem_head)));
95 }
96
97 return free_count;
98 }
99
mem_get(const void * mem_pool,uint16_t mem_size,uint16_t index)100 void *mem_get(const void *mem_pool, uint16_t mem_size, uint16_t index)
101 {
102 return ((void *)((uint8_t *)mem_pool + (mem_size * index)));
103 }
104
mem_index_get(const void * mem,const void * mem_pool,uint16_t mem_size)105 uint16_t mem_index_get(const void *mem, const void *mem_pool, uint16_t mem_size)
106 {
107 return ((uint8_t *)mem - (uint8_t *)mem_pool) / mem_size;
108 }
109
110 /**
111 * @brief Copy bytes in reverse
112 * @details Example: [ 0x11 0x22 0x33 ] -> [ 0x33 0x22 0x11 ]
113 */
mem_rcopy(uint8_t * dst,uint8_t const * src,uint16_t len)114 void mem_rcopy(uint8_t *dst, uint8_t const *src, uint16_t len)
115 {
116 src += len;
117 while (len--) {
118 *dst++ = *--src;
119 }
120 }
121
122 /**
123 * @brief Determine if src[0..len-1] contains one or more non-zero bytes
124 * @return 0 if all bytes are zero; otherwise 1
125 */
mem_nz(uint8_t * src,uint16_t len)126 uint8_t mem_nz(uint8_t *src, uint16_t len)
127 {
128 while (len--) {
129 if (*src++) {
130 return 1;
131 }
132 }
133
134 return 0;
135 }
136
137 /**
138 * @brief Unit test
139 */
mem_ut(void)140 uint32_t mem_ut(void)
141 {
142 #define BLOCK_SIZE MROUND(10)
143 #define BLOCK_COUNT 10
144 uint8_t MALIGN(4) pool[BLOCK_COUNT][BLOCK_SIZE];
145 void *mem_free;
146 void *mem_used;
147 uint16_t mem_free_count;
148 void *mem;
149
150 mem_init(pool, BLOCK_SIZE, BLOCK_COUNT, &mem_free);
151
152 mem_free_count = mem_free_count_get(mem_free);
153 if (mem_free_count != BLOCK_COUNT) {
154 return 1;
155 }
156
157 mem_used = 0;
158 while (mem_free_count--) {
159 uint16_t mem_free_count_current;
160
161 mem = mem_acquire(&mem_free);
162 mem_free_count_current = mem_free_count_get(mem_free);
163 if (mem_free_count != mem_free_count_current) {
164 return 2;
165 }
166
167 memcpy(mem, &mem_used, sizeof(mem));
168 mem_used = mem;
169 }
170
171 mem = mem_acquire(&mem_free);
172 if (mem) {
173 return 3;
174 }
175
176 while (++mem_free_count < BLOCK_COUNT) {
177 uint16_t mem_free_count_current;
178
179 mem = mem_used;
180 memcpy(&mem_used, mem, sizeof(void *));
181 mem_release(mem, &mem_free);
182
183 mem_free_count_current = mem_free_count_get(mem_free);
184 if ((mem_free_count + 1) != mem_free_count_current) {
185 return 4;
186 }
187 }
188
189 if (mem != mem_free) {
190 return 5;
191 }
192
193 if (mem_free_count_get(mem_free) != BLOCK_COUNT) {
194 return 6;
195 }
196
197 return 0;
198 }
199
