123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258 |
- #include "nrf_memobj.h"
- #include "nrf_atomic.h"
- #include "nrf_assert.h"
- typedef struct memobj_elem_s memobj_elem_t;
- typedef struct
- {
- memobj_elem_t * p_next;
- } memobj_header_t;
- typedef struct
- {
- uint8_t user_cnt;
- uint8_t chunk_cnt;
- uint16_t chunk_size;
- } memobj_head_header_fields_t;
- typedef struct
- {
- union
- {
- nrf_atomic_u32_t atomic_user_cnt;
- memobj_head_header_fields_t fields;
- } data;
- } memobj_head_header_t;
- typedef struct
- {
- memobj_header_t header;
- memobj_head_header_t head_header;
- uint8_t data[1];
- } memobj_head_t;
- STATIC_ASSERT(sizeof(memobj_header_t) == NRF_MEMOBJ_STD_HEADER_SIZE);
- struct memobj_elem_s
- {
- memobj_header_t header;
- uint8_t data[1];
- };
- ret_code_t nrf_memobj_pool_init(nrf_memobj_pool_t const * p_pool)
- {
- return nrf_balloc_init((nrf_balloc_t const *)p_pool);
- }
- nrf_memobj_t * nrf_memobj_alloc(nrf_memobj_pool_t const * p_pool,
- size_t size)
- {
- uint32_t bsize = (uint32_t)NRF_BALLOC_ELEMENT_SIZE((nrf_balloc_t const *)p_pool) - sizeof(memobj_header_t);
- uint8_t num_of_chunks = (uint8_t)CEIL_DIV(size + sizeof(memobj_head_header_t), bsize);
- memobj_head_t * p_head = nrf_balloc_alloc((nrf_balloc_t const *)p_pool);
- if (p_head == NULL)
- {
- return NULL;
- }
- p_head->head_header.data.fields.user_cnt = 0;
- p_head->head_header.data.fields.chunk_cnt = 1;
- p_head->head_header.data.fields.chunk_size = bsize;
- memobj_header_t * p_prev = (memobj_header_t *)p_head;
- memobj_header_t * p_curr;
- uint32_t i;
- uint32_t chunk_less1 = (uint32_t)num_of_chunks - 1;
- p_prev->p_next = (memobj_elem_t *)p_pool;
- for (i = 0; i < chunk_less1; i++)
- {
- p_curr = (memobj_header_t *)nrf_balloc_alloc((nrf_balloc_t const *)p_pool);
- if (p_curr)
- {
- (p_head->head_header.data.fields.chunk_cnt)++;
- p_prev->p_next = (memobj_elem_t *)p_curr;
- p_curr->p_next = (memobj_elem_t *)p_pool;
- p_prev = p_curr;
- }
- else
- {
-
- nrf_memobj_free((nrf_memobj_t *)p_head);
- return NULL;
- }
- }
- return (nrf_memobj_t *)p_head;
- }
- void nrf_memobj_free(nrf_memobj_t * p_obj)
- {
- memobj_head_t * p_head = (memobj_head_t *)p_obj;
- uint8_t chunk_cnt = p_head->head_header.data.fields.chunk_cnt;
- uint32_t i;
- memobj_header_t * p_curr = (memobj_header_t *)p_obj;
- memobj_header_t * p_next;
- uint32_t chunk_less1 = (uint32_t)chunk_cnt - 1;
- for (i = 0; i < chunk_less1; i++)
- {
- p_curr = (memobj_header_t *)p_curr->p_next;
- }
- nrf_balloc_t const * p_pool2 = (nrf_balloc_t const *)p_curr->p_next;
- p_curr = (memobj_header_t *)p_obj;
- for (i = 0; i < chunk_cnt; i++)
- {
- p_next = (memobj_header_t *)p_curr->p_next;
- nrf_balloc_free(p_pool2, p_curr);
- p_curr = p_next;
- }
- }
- void nrf_memobj_get(nrf_memobj_t const * p_obj)
- {
- memobj_head_t * p_head = (memobj_head_t *)p_obj;
- (void)nrf_atomic_u32_add(&p_head->head_header.data.atomic_user_cnt, 1);
- }
- void nrf_memobj_put(nrf_memobj_t * p_obj)
- {
- memobj_head_t * p_head = (memobj_head_t *)p_obj;
- uint32_t user_cnt = nrf_atomic_u32_sub(&p_head->head_header.data.atomic_user_cnt, 1);
- memobj_head_header_fields_t * p_fields = (memobj_head_header_fields_t *)&user_cnt;
- if (p_fields->user_cnt == 0)
- {
- nrf_memobj_free(p_obj);
- }
- }
- static void memobj_op(nrf_memobj_t * p_obj,
- void * p_data,
- size_t * p_len,
- size_t offset,
- bool read)
- {
- ASSERT(p_obj);
- memobj_head_t * p_head = (memobj_head_t *)p_obj;
- memobj_elem_t * p_curr_chunk = (memobj_elem_t *)p_obj;
- size_t obj_capacity;
- size_t chunk_size;
- size_t chunk_idx;
- size_t chunk_offset;
- size_t len;
- obj_capacity = (p_head->head_header.data.fields.chunk_size *
- p_head->head_header.data.fields.chunk_cnt) -
- sizeof(memobj_head_header_fields_t);
- ASSERT(offset < obj_capacity);
- chunk_size = p_head->head_header.data.fields.chunk_size;
- chunk_idx = (offset + sizeof(memobj_head_header_fields_t)) / chunk_size;
- chunk_offset = (offset + sizeof(memobj_head_header_fields_t)) % chunk_size;
- len = ((*p_len + offset) > obj_capacity) ? obj_capacity - offset : *p_len;
-
- *p_len = len;
-
- while (chunk_idx > 0)
- {
- p_curr_chunk = p_curr_chunk->header.p_next;
- chunk_idx--;
- }
- size_t user_mem_offset = 0;
- size_t curr_cpy_size = chunk_size - chunk_offset;
- curr_cpy_size = curr_cpy_size > len ? len : curr_cpy_size;
- while (len)
- {
- void * p_user_mem = &((uint8_t *)p_data)[user_mem_offset];
- void * p_obj_mem = &p_curr_chunk->data[chunk_offset];
- if (read)
- {
- memcpy(p_user_mem, p_obj_mem, curr_cpy_size);
- }
- else
- {
- memcpy(p_obj_mem, p_user_mem, curr_cpy_size);
- }
- chunk_offset = 0;
- p_curr_chunk = p_curr_chunk->header.p_next;
- len -= curr_cpy_size;
- user_mem_offset += curr_cpy_size;
- curr_cpy_size = (chunk_size > len) ? len : chunk_size;
- }
- }
- void nrf_memobj_write(nrf_memobj_t * p_obj,
- void * p_data,
- size_t len,
- size_t offset)
- {
- size_t op_len = len;
- memobj_op(p_obj, p_data, &op_len, offset, false);
- ASSERT(op_len == len);
- }
- void nrf_memobj_read(nrf_memobj_t * p_obj,
- void * p_data,
- size_t len,
- size_t offset)
- {
- size_t op_len = len;
- memobj_op(p_obj, p_data, &op_len, offset, true);
- ASSERT(op_len == len);
- }
|