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
os_mempool_poison(void * start,int sz)35 static void os_mempool_poison(void *start, int sz)
36 {
37 int i;
38 char *p = start;
39
40 for (i = sizeof(struct os_memblock); i < sz;
41 i = i + sizeof(os_mem_poison)) {
42 memcpy(p + i, &os_mem_poison, min(sizeof(os_mem_poison), sz - i));
43 }
44 }
45
os_mempool_poison_check(void * start,int sz)46 static void os_mempool_poison_check(void *start, int sz)
47 {
48 int i;
49 char *p = start;
50
51 for (i = sizeof(struct os_memblock); i < sz;
52 i = i + sizeof(os_mem_poison)) {
53 assert(!memcmp(p + i, &os_mem_poison,
54 min(sizeof(os_mem_poison), sz - i)));
55 }
56 }
57 #else
58 #define os_mempool_poison(start, sz)
59 #define os_mempool_poison_check(start, sz)
60 #endif
61
os_mempool_reset(void)62 void os_mempool_reset(void)
63 {
64 STAILQ_INIT(&g_os_mempool_list);
65 }
66
os_mempool_init(struct os_mempool * mp,uint16_t blocks,uint32_t block_size,void * membuf,char * name)67 os_error_t os_mempool_init(struct os_mempool *mp, uint16_t blocks, uint32_t block_size,
68 void *membuf, char *name)
69 {
70 int true_block_size;
71 uint8_t *block_addr;
72 struct os_memblock *block_ptr;
73
74 /* Check for valid parameters */
75 if (!mp || (block_size == 0)) {
76 return OS_INVALID_PARM;
77 }
78
79 if ((!membuf) && (blocks != 0)) {
80 return OS_INVALID_PARM;
81 }
82
83 if (membuf != NULL) {
84 /* Blocks need to be sized properly and memory buffer should be
85 * aligned
86 */
87 if (((uintptr_t)membuf & (OS_ALIGNMENT - 1)) != 0) {
88 return OS_MEM_NOT_ALIGNED;
89 }
90 }
91
92 true_block_size = OS_MEM_TRUE_BLOCK_SIZE(block_size);
93 /* Initialize the memory pool structure */
94 mp->mp_block_size = block_size;
95 mp->mp_num_free = blocks;
96 mp->mp_min_free = blocks;
97 mp->mp_flags = 0;
98 mp->mp_num_blocks = blocks;
99 mp->mp_membuf_addr = (uintptr_t)membuf;
100 mp->name = name;
101 os_mempool_poison(membuf, true_block_size);
102 SLIST_FIRST(mp) = membuf;
103 /* Chain the memory blocks to the free list */
104 block_addr = (uint8_t *)membuf;
105 block_ptr = (struct os_memblock *)block_addr;
106
107 while (blocks > 1) {
108 block_addr += true_block_size;
109 os_mempool_poison(block_addr, true_block_size);
110 SLIST_NEXT(block_ptr, mb_next) = (struct os_memblock *)block_addr;
111 block_ptr = (struct os_memblock *)block_addr;
112 --blocks;
113 }
114
115 /* Last one in the list should be NULL */
116 SLIST_NEXT(block_ptr, mb_next) = NULL;
117 STAILQ_INSERT_TAIL(&g_os_mempool_list, mp, mp_list);
118 return OS_OK;
119 }
120
os_mempool_ext_init(struct os_mempool_ext * mpe,uint16_t blocks,uint32_t block_size,void * membuf,char * name)121 os_error_t os_mempool_ext_init(struct os_mempool_ext *mpe, uint16_t blocks,
122 uint32_t block_size, void *membuf, char *name)
123 {
124 int rc;
125 rc = os_mempool_init(&mpe->mpe_mp, blocks, block_size, membuf, name);
126 if (rc != 0) {
127 return rc;
128 }
129
130 mpe->mpe_mp.mp_flags = OS_MEMPOOL_F_EXT;
131 mpe->mpe_put_cb = NULL;
132 mpe->mpe_put_arg = NULL;
133 return 0;
134 }
135
os_mempool_clear(struct os_mempool * mp)136 os_error_t os_mempool_clear(struct os_mempool *mp)
137 {
138 struct os_memblock *block_ptr;
139 int true_block_size;
140 uint8_t *block_addr;
141 uint16_t blocks;
142
143 if (!mp) {
144 return OS_INVALID_PARM;
145 }
146
147 true_block_size = OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size);
148 /* cleanup the memory pool structure */
149 mp->mp_num_free = mp->mp_num_blocks;
150 mp->mp_min_free = mp->mp_num_blocks;
151 os_mempool_poison((void *)mp->mp_membuf_addr, true_block_size);
152 SLIST_FIRST(mp) = (void *)mp->mp_membuf_addr;
153 /* Chain the memory blocks to the free list */
154 block_addr = (uint8_t *)mp->mp_membuf_addr;
155 block_ptr = (struct os_memblock *)block_addr;
156 blocks = mp->mp_num_blocks;
157 while (blocks > 1) {
158 block_addr += true_block_size;
159 os_mempool_poison(block_addr, true_block_size);
160 SLIST_NEXT(block_ptr, mb_next) = (struct os_memblock *)block_addr;
161 block_ptr = (struct os_memblock *)block_addr;
162 --blocks;
163 }
164
165 /* Last one in the list should be NULL */
166 SLIST_NEXT(block_ptr, mb_next) = NULL;
167 return OS_OK;
168 }
169
os_mempool_is_sane(const struct os_mempool * mp)170 bool os_mempool_is_sane(const struct os_mempool *mp)
171 {
172 struct os_memblock *block;
173 /* Verify that each block in the free list belongs to the mempool. */
174 SLIST_FOREACH(block, mp, mb_next) {
175 if (!os_memblock_from(mp, block)) {
176 return false;
177 }
178
179 os_mempool_poison_check(block, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
180 }
181 return true;
182 }
183
os_memblock_from(const struct os_mempool * mp,const void * block_addr)184 int os_memblock_from(const struct os_mempool *mp, const void *block_addr)
185 {
186 uintptr_t true_block_size;
187 uintptr_t baddr_ptr;
188 uintptr_t end;
189 _Static_assert(sizeof block_addr == sizeof baddr_ptr,
190 "Pointer to void must be native word size.");
191 baddr_ptr = (uintptr_t)block_addr;
192 true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp);
193 if (true_block_size == 0) {
194 return -1;
195 }
196 end = mp->mp_membuf_addr + (mp->mp_num_blocks * true_block_size);
197
198 /* Check that the block is in the memory buffer range. */
199 if ((baddr_ptr < mp->mp_membuf_addr) || (baddr_ptr >= end)) {
200 return 0;
201 }
202
203 /* All freed blocks should be on true block size boundaries! */
204 if (((baddr_ptr - mp->mp_membuf_addr) % true_block_size) != 0) {
205 return 0;
206 }
207
208 return 1;
209 }
210
os_memblock_get(struct os_mempool * mp)211 void *os_memblock_get(struct os_mempool *mp)
212 {
213 os_sr_t sr;
214 struct os_memblock *block;
215 /* Check to make sure they passed in a memory pool (or something) */
216 block = NULL;
217
218 if (mp) {
219 OS_ENTER_CRITICAL(sr);
220
221 /* Check for any free */
222 if (mp->mp_num_free) {
223 /* Get a free block */
224 block = SLIST_FIRST(mp);
225 /* Set new free list head */
226 SLIST_FIRST(mp) = SLIST_NEXT(block, mb_next);
227 /* Decrement number free by 1 */
228 mp->mp_num_free--;
229
230 if (mp->mp_min_free > mp->mp_num_free) {
231 mp->mp_min_free = mp->mp_num_free;
232 }
233 }
234
235 OS_EXIT_CRITICAL(sr);
236
237 if (block) {
238 os_mempool_poison_check(block, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
239 }
240 }
241
242 return (void *)block;
243 }
244
os_memblock_put_from_cb(struct os_mempool * mp,void * block_addr)245 os_error_t os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr)
246 {
247 os_sr_t sr;
248 struct os_memblock *block;
249 os_mempool_poison(block_addr, OS_MEMPOOL_TRUE_BLOCK_SIZE(mp));
250 block = (struct os_memblock *)block_addr;
251 OS_ENTER_CRITICAL(sr);
252 /* Chain current free list pointer to this block; make this block head */
253 SLIST_NEXT(block, mb_next) = SLIST_FIRST(mp);
254 SLIST_FIRST(mp) = block;
255 /* XXX: Should we check that the number free <= number blocks? */
256 /* Increment number free */
257 mp->mp_num_free++;
258 OS_EXIT_CRITICAL(sr);
259 return OS_OK;
260 }
261
os_memblock_put(struct os_mempool * mp,void * block_addr)262 os_error_t os_memblock_put(struct os_mempool *mp, void *block_addr)
263 {
264 struct os_mempool_ext *mpe;
265
266 #if MYNEWT_VAL(OS_MEMPOOL_CHECK)
267 struct os_memblock *block;
268 #endif
269
270 /* Make sure parameters are valid */
271 if ((mp == NULL) || (block_addr == NULL)) {
272 return OS_INVALID_PARM;
273 }
274
275 #if MYNEWT_VAL(OS_MEMPOOL_CHECK)
276 /* Check that the block we are freeing is a valid block! */
277 assert(os_memblock_from(mp, block_addr));
278 /*
279 * Check for duplicate free.
280 */
281 SLIST_FOREACH(block, mp, mb_next) {
282 assert(block != (struct os_memblock *)block_addr);
283 }
284 #endif
285
286 /* If this is an extended mempool with a put callback, call the callback
287 * instead of freeing the block directly.
288 */
289 if (mp->mp_flags & OS_MEMPOOL_F_EXT) {
290 mpe = (struct os_mempool_ext *)mp;
291
292 if (mpe->mpe_put_cb != NULL) {
293 int rc = mpe->mpe_put_cb(mpe, block_addr, mpe->mpe_put_arg);
294 return rc;
295 }
296 }
297
298 /* No callback; free the block. */
299 return os_memblock_put_from_cb(mp, block_addr);
300 }
301
os_mempool_info_get_next(struct os_mempool * mp,struct os_mempool_info * omi)302 struct os_mempool *os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi)
303 {
304 struct os_mempool *cur;
305
306 if (mp == NULL) {
307 cur = STAILQ_FIRST(&g_os_mempool_list);
308 } else {
309 cur = STAILQ_NEXT(mp, mp_list);
310 }
311
312 if (cur == NULL) {
313 return (NULL);
314 }
315
316 omi->omi_block_size = cur->mp_block_size;
317 omi->omi_num_blocks = cur->mp_num_blocks;
318 omi->omi_num_free = cur->mp_num_free;
319 omi->omi_min_free = cur->mp_min_free;
320 strncpy(omi->omi_name, cur->name, sizeof(omi->omi_name));
321 return (cur);
322 }