1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one
3 * or more contributor license agreements. See the NOTICE file
4 * distributed with this work for additional information
5 * regarding copyright ownership. The ASF licenses this file
6 * to you under the Apache License, Version 2.0 (the
7 * "License"); you may not use this file except in compliance
8 * with the License. You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing,
13 * software distributed under the License is distributed on an
14 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
15 * KIND, either express or implied. See the License for the
16 * specific language governing permissions and limitations
17 * under the License.
18 */
19
20 #include "os/os.h"
21
22 #include <string.h>
23 #include <assert.h>
24 #include <stdbool.h>
25
26 #define OS_MEM_TRUE_BLOCK_SIZE(bsize) OS_ALIGN(bsize, OS_ALIGNMENT)
27 #define OS_MEMPOOL_TRUE_BLOCK_SIZE(mp) OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)
28
29 STAILQ_HEAD(, os_mempool) g_os_mempool_list =
30 STAILQ_HEAD_INITIALIZER(g_os_mempool_list);
31
32 #if MYNEWT_VAL(OS_MEMPOOL_POISON)
33 static uint32_t os_mem_poison = 0xde7ec7ed;
34
35 static void
os_mempool_poison(void * start,int sz)36 os_mempool_poison(void *start, int sz)
37 {
38 int i;
39 char *p = start;
40
41 for (i = sizeof(struct os_memblock); i < sz;
42 i = i + sizeof(os_mem_poison)) {
43 memcpy(p + i, &os_mem_poison, min(sizeof(os_mem_poison), sz - i));
44 }
45 }
46
47 static void
os_mempool_poison_check(void * start,int sz)48 os_mempool_poison_check(void *start, int sz)
49 {
50 int i;
51 char *p = start;
52
53 for (i = sizeof(struct os_memblock); i < sz;
54 i = i + sizeof(os_mem_poison)) {
55 assert(!memcmp(p + i, &os_mem_poison,
56 min(sizeof(os_mem_poison), sz - i)));
57 }
58 }
59 #else
60 #define os_mempool_poison(start, sz)
61 #define os_mempool_poison_check(start, sz)
62 #endif
63
64 os_error_t
os_mempool_init(struct os_mempool * mp,uint16_t blocks,uint32_t block_size,void * membuf,char * name)65 os_mempool_init(struct os_mempool *mp, uint16_t blocks, uint32_t block_size,
66 void *membuf, char *name)
67 {
68 int true_block_size;
69 uint8_t *block_addr;
70 struct os_memblock *block_ptr;
71
72 /* Check for valid parameters */
73 if (!mp || (blocks < 0) || (block_size <= 0)) {
74 return OS_INVALID_PARM;
75 }
76
77 if ((!membuf) && (blocks != 0)) {
78 return OS_INVALID_PARM;
79 }
80
81 if (membuf != NULL) {
82 /* Blocks need to be sized properly and memory buffer should be
83 * aligned
84 */
85 if (((uintptr_t)membuf & (OS_ALIGNMENT - 1)) != 0) {
86 return OS_MEM_NOT_ALIGNED;
87 }
88 }
89 true_block_size = OS_MEM_TRUE_BLOCK_SIZE(block_size);
90
91 /* Initialize the memory pool structure */
92 mp->mp_block_size = block_size;
93 mp->mp_num_free = blocks;
94 mp->mp_min_free = blocks;
95 mp->mp_flags = 0;
96 mp->mp_num_blocks = blocks;
97 mp->mp_membuf_addr = (uintptr_t)membuf;
98 mp->name = name;
99 os_mempool_poison(membuf, true_block_size);
100 SLIST_FIRST(mp) = membuf;
101
102 /* Chain the memory blocks to the free list */
103 block_addr = (uint8_t *)membuf;
104 block_ptr = (struct os_memblock *)block_addr;
105 while (blocks > 1) {
106 block_addr += true_block_size;
107 os_mempool_poison(block_addr, true_block_size);
108 SLIST_NEXT(block_ptr, mb_next) = (struct os_memblock *)block_addr;
109 block_ptr = (struct os_memblock *)block_addr;
110 --blocks;
111 }
112
113 /* Last one in the list should be NULL */
114 SLIST_NEXT(block_ptr, mb_next) = NULL;
115
116 STAILQ_INSERT_TAIL(&g_os_mempool_list, mp, mp_list);
117
118 return OS_OK;
119 }
120
121 os_error_t
os_mempool_ext_init(struct os_mempool_ext * mpe,uint16_t blocks,uint32_t block_size,void * membuf,char * name)122 os_mempool_ext_init(struct os_mempool_ext *mpe, uint16_t blocks,
123 uint32_t block_size, void *membuf, char *name)
124 {
125 int rc;
126
127 rc = os_mempool_init(&mpe->mpe_mp, blocks, block_size, membuf, name);
128 if (rc != 0) {
129 return rc;
130 }
131
132 mpe->mpe_mp.mp_flags = OS_MEMPOOL_F_EXT;
133 mpe->mpe_put_cb = NULL;
134 mpe->mpe_put_arg = NULL;
135
136 return 0;
137 }
138
139 os_error_t
os_mempool_clear(struct os_mempool * mp)140 os_mempool_clear(struct os_mempool *mp)
141 {
142 struct os_memblock *block_ptr;
143 int true_block_size;
144 uint8_t *block_addr;
145 uint16_t blocks;
146
147 if (!mp) {
148 return OS_INVALID_PARM;
149 }
150
151 true_block_size = OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size);
152
153 /* cleanup the memory pool structure */
154 mp->mp_num_free = mp->mp_num_blocks;
155 mp->mp_min_free = mp->mp_num_blocks;
156 os_mempool_poison((void *)mp->mp_membuf_addr, true_block_size);
157 SLIST_FIRST(mp) = (void *)mp->mp_membuf_addr;
158
159 /* Chain the memory blocks to the free list */
160 block_addr = (uint8_t *)mp->mp_membuf_addr;
161 block_ptr = (struct os_memblock *)block_addr;
162 blocks = mp->mp_num_blocks;
163
164 while (blocks > 1) {
165 block_addr += true_block_size;
166 os_mempool_poison(block_addr, true_block_size);
167 SLIST_NEXT(block_ptr, mb_next) = (struct os_memblock *)block_addr;
168 block_ptr = (struct os_memblock *)block_addr;
169 --blocks;
170 }
171
172 /* Last one in the list should be NULL */
173 SLIST_NEXT(block_ptr, mb_next) = NULL;
174
175 return OS_OK;
176 }
177
178 bool
os_mempool_is_sane(const struct os_mempool * mp)179 os_mempool_is_sane(const struct os_mempool *mp)
180 {
181 struct os_memblock *block;
182
183 /* Verify that each block in the free list belongs to the mempool. */
184 SLIST_FOREACH(block, mp, mb_next) {
185 if (!os_memblock_from(mp, block)) {
186 return false;
187 }
188 os_mempool_poison_check(block, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
189 }
190
191 return true;
192 }
193
194 int
os_memblock_from(const struct os_mempool * mp,const void * block_addr)195 os_memblock_from(const struct os_mempool *mp, const void *block_addr)
196 {
197 uintptr_t true_block_size;
198 uintptr_t baddr_ptr;
199 uintptr_t end;
200
201 _Static_assert(sizeof block_addr == sizeof baddr_ptr,
202 "Pointer to void must be native word size.");
203
204 baddr_ptr = (uintptr_t)block_addr;
205 true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp);
206 end = mp->mp_membuf_addr + (mp->mp_num_blocks * true_block_size);
207
208 /* Check that the block is in the memory buffer range. */
209 if ((baddr_ptr < mp->mp_membuf_addr) || (baddr_ptr >= end)) {
210 return 0;
211 }
212
213 /* All freed blocks should be on true block size boundaries! */
214 if (((baddr_ptr - mp->mp_membuf_addr) % true_block_size) != 0) {
215 return 0;
216 }
217
218 return 1;
219 }
220
221 void *
os_memblock_get(struct os_mempool * mp)222 os_memblock_get(struct os_mempool *mp)
223 {
224 os_sr_t sr;
225 struct os_memblock *block;
226
227 /* Check to make sure they passed in a memory pool (or something) */
228 block = NULL;
229 if (mp) {
230 OS_ENTER_CRITICAL(sr);
231 /* Check for any free */
232 if (mp->mp_num_free) {
233 /* Get a free block */
234 block = SLIST_FIRST(mp);
235
236 /* Set new free list head */
237 SLIST_FIRST(mp) = SLIST_NEXT(block, mb_next);
238
239 /* Decrement number free by 1 */
240 mp->mp_num_free--;
241 if (mp->mp_min_free > mp->mp_num_free) {
242 mp->mp_min_free = mp->mp_num_free;
243 }
244 }
245 OS_EXIT_CRITICAL(sr);
246
247 if (block) {
248 os_mempool_poison_check(block, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
249 }
250 }
251
252 return (void *)block;
253 }
254
255 os_error_t
os_memblock_put_from_cb(struct os_mempool * mp,void * block_addr)256 os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr)
257 {
258 os_sr_t sr;
259 struct os_memblock *block;
260
261 os_mempool_poison(block_addr, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
262
263 block = (struct os_memblock *)block_addr;
264 OS_ENTER_CRITICAL(sr);
265
266 /* Chain current free list pointer to this block; make this block head */
267 SLIST_NEXT(block, mb_next) = SLIST_FIRST(mp);
268 SLIST_FIRST(mp) = block;
269
270 /* XXX: Should we check that the number free <= number blocks? */
271 /* Increment number free */
272 mp->mp_num_free++;
273
274 OS_EXIT_CRITICAL(sr);
275
276 return OS_OK;
277 }
278
279 os_error_t
os_memblock_put(struct os_mempool * mp,void * block_addr)280 os_memblock_put(struct os_mempool *mp, void *block_addr)
281 {
282 struct os_mempool_ext *mpe;
283 int rc;
284 #if MYNEWT_VAL(OS_MEMPOOL_CHECK)
285 struct os_memblock *block;
286 #endif
287
288 /* Make sure parameters are valid */
289 if ((mp == NULL) || (block_addr == NULL)) {
290 return OS_INVALID_PARM;
291 }
292
293 #if MYNEWT_VAL(OS_MEMPOOL_CHECK)
294 /* Check that the block we are freeing is a valid block! */
295 assert(os_memblock_from(mp, block_addr));
296
297 /*
298 * Check for duplicate free.
299 */
300 SLIST_FOREACH(block, mp, mb_next) {
301 assert(block != (struct os_memblock *)block_addr);
302 }
303 #endif
304
305 /* If this is an extended mempool with a put callback, call the callback
306 * instead of freeing the block directly.
307 */
308 if (mp->mp_flags & OS_MEMPOOL_F_EXT) {
309 mpe = (struct os_mempool_ext *)mp;
310 if (mpe->mpe_put_cb != NULL) {
311 rc = mpe->mpe_put_cb(mpe, block_addr, mpe->mpe_put_arg);
312 return rc;
313 }
314 }
315
316 /* No callback; free the block. */
317 return os_memblock_put_from_cb(mp, block_addr);
318 }
319
320 struct os_mempool *
os_mempool_info_get_next(struct os_mempool * mp,struct os_mempool_info * omi)321 os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi)
322 {
323 struct os_mempool *cur;
324
325 if (mp == NULL) {
326 cur = STAILQ_FIRST(&g_os_mempool_list);
327 } else {
328 cur = STAILQ_NEXT(mp, mp_list);
329 }
330
331 if (cur == NULL) {
332 return (NULL);
333 }
334
335 omi->omi_block_size = cur->mp_block_size;
336 omi->omi_num_blocks = cur->mp_num_blocks;
337 omi->omi_num_free = cur->mp_num_free;
338 omi->omi_min_free = cur->mp_min_free;
339 strncpy(omi->omi_name, cur->name, sizeof(omi->omi_name));
340
341 return (cur);
342 }
343
344
345