/**
* @brief Low level memory manager initialization.
*
* @notapi
*/
void _core_init(void) {
#if CH_MEMCORE_SIZE == 0
extern uint8_t __heap_base__[];
extern uint8_t __heap_end__[];
nextmem = (uint8_t *)MEM_ALIGN_NEXT(__heap_base__);
endmem = (uint8_t *)MEM_ALIGN_PREV(__heap_end__);
#else
static stkalign_t buffer[MEM_ALIGN_NEXT(CH_MEMCORE_SIZE)/MEM_ALIGN_SIZE];
nextmem = (uint8_t *)&buffer[0];
endmem = (uint8_t *)&buffer[MEM_ALIGN_NEXT(CH_MEMCORE_SIZE)/MEM_ALIGN_SIZE];
#endif
}
/**
* @brief Creates a new thread into a static memory area.
* @details The new thread is initialized but not inserted in the ready list,
* the initial state is @p CH_STATE_WTSTART.
* @post The initialized thread can be subsequently started by invoking
* @p chThdStart(), @p chThdStartI() or @p chSchWakeupS()
* depending on the execution context.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note Threads created using this function do not obey to the
* @p CH_DBG_FILL_THREADS debug option because it would keep
* the kernel locked for too much time.
*
* @param[out] tdp pointer to the thread descriptor
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
*
* @iclass
*/
thread_t *chThdCreateSuspendedI(const thread_descriptor_t *tdp) {
thread_t *tp;
chDbgCheckClassI();
chDbgCheck(tdp != NULL);
chDbgCheck(MEM_IS_ALIGNED(tdp->wbase, PORT_WORKING_AREA_ALIGN) &&
MEM_IS_ALIGNED(tdp->wend, PORT_STACK_ALIGN) &&
(tdp->wend > tdp->wbase) &&
((size_t)((tdp->wend - tdp->wbase) *
sizeof (stkalign_t)) >= THD_WORKING_AREA_SIZE(0)));
chDbgCheck((tdp->prio <= HIGHPRIO) && (tdp->funcp != NULL));
/* The thread structure is laid out in the upper part of the thread
workspace. The thread position structure is aligned to the required
stack alignment because it represents the stack top.*/
tp = (thread_t *)((uint8_t *)tdp->wend -
MEM_ALIGN_NEXT(sizeof (thread_t), PORT_STACK_ALIGN));
/* Initial state.*/
tp->state = CH_STATE_WTSTART;
/* Stack boundary.*/
tp->stklimit = tdp->wbase;
/* Setting up the port-dependent part of the working area.*/
PORT_SETUP_CONTEXT(tp, tdp->wbase, tp, tdp->funcp, tdp->arg);
/* The driver object is initialized but not started.*/
return _thread_init(tp, tdp->name, tdp->prio);
}
void chCoreGrowI(size_t size) {
chDbgCheckClassI();
size = MEM_ALIGN_NEXT(size);
chDbgCheck(endmem + size <= realendmem, "chCoreGrowI");
endmem += size;
}
/**
* @brief Initializes an empty memory pool.
* @note The size is internally aligned to be a multiple of the
* @p stkalign_t type size.
*
* @param[out] mp pointer to a @p MemoryPool structure
* @param[in] size the size of the objects contained in this memory pool,
* the minimum accepted size is the size of a pointer to
* void.
* @param[in] provider memory provider function for the memory pool or
* @p NULL if the pool is not allowed to grow
* automatically
*
* @init
*/
void chPoolInit(MemoryPool *mp, size_t size, memgetfunc_t provider) {
chDbgCheck((mp != NULL) && (size >= sizeof(void *)), "chPoolInit");
mp->mp_next = NULL;
mp->mp_object_size = MEM_ALIGN_NEXT(size);
mp->mp_provider = provider;
}
/**
* @brief Allocates a block of memory from the heap by using the first-fit
* algorithm.
* @details The allocated block is guaranteed to be properly aligned for a
* pointer data type (@p stkalign_t).
*
* @param[in] heapp pointer to a heap descriptor or @p NULL in order to
* access the default heap.
* @param[in] size the size of the block to be allocated. Note that the
* allocated block may be a bit bigger than the requested
* size for alignment and fragmentation reasons.
* @return A pointer to the allocated block.
* @retval NULL if the block cannot be allocated.
*
* @api
*/
void *chHeapAlloc(memory_heap_t *heapp, size_t size) {
union heap_header *qp, *hp, *fp;
if (heapp == NULL) {
heapp = &default_heap;
}
size = MEM_ALIGN_NEXT(size);
qp = &heapp->h_free;
H_LOCK(heapp);
while (qp->h.u.next != NULL) {
hp = qp->h.u.next;
if (hp->h.size >= size) {
if (hp->h.size < (size + sizeof(union heap_header))) {
/* Gets the whole block even if it is slightly bigger than the
requested size because the fragment would be too small to be
useful.*/
qp->h.u.next = hp->h.u.next;
}
else {
/* Block bigger enough, must split it.*/
/*lint -save -e9087 [11.3] Safe cast.*/
fp = (void *)((uint8_t *)(hp) + sizeof(union heap_header) + size);
/*lint -restore*/
fp->h.u.next = hp->h.u.next;
fp->h.size = (hp->h.size - sizeof(union heap_header)) - size;
qp->h.u.next = fp;
hp->h.size = size;
}
hp->h.u.heap = heapp;
H_UNLOCK(heapp);
/*lint -save -e9087 [11.3] Safe cast.*/
return (void *)(hp + 1);
/*lint -restore*/
}
qp = hp;
}
H_UNLOCK(heapp);
/* More memory is required, tries to get it from the associated provider
else fails.*/
if (heapp->h_provider != NULL) {
hp = heapp->h_provider(size + sizeof(union heap_header));
if (hp != NULL) {
hp->h.u.heap = heapp;
hp->h.size = size;
hp++;
/*lint -save -e9087 [11.3] Safe cast.*/
return (void *)hp;
/*lint -restore*/
}
}
return NULL;
}
/**
* @brief Low level memory manager initialization.
*
* @notapi
*/
void _core_init(void) {
#if CH_CFG_MEMCORE_SIZE == 0
extern uint8_t __heap_base__[];
extern uint8_t __heap_end__[];
/*lint -save -e9033 [10.8] Required cast operations.*/
nextmem = (uint8_t *)MEM_ALIGN_NEXT(__heap_base__);
endmem = (uint8_t *)MEM_ALIGN_PREV(__heap_end__);
/*lint restore*/
#else
static stkalign_t buffer[MEM_ALIGN_NEXT(CH_CFG_MEMCORE_SIZE) /
MEM_ALIGN_SIZE];
nextmem = (uint8_t *)&buffer[0];
endmem = (uint8_t *)&buffer[MEM_ALIGN_NEXT(CH_CFG_MEMCORE_SIZE) /
MEM_ALIGN_SIZE];
#endif
}
void *chCoreUnreserveI(size_t size) {
chDbgCheckClassI();
size = MEM_ALIGN_NEXT(size);
if ((size_t)(realendmem - endmem) < size)
return NULL;
endmem += size;
return endmem;
}
void *chCoreReserveI(size_t size) {
chDbgCheckClassI();
size = MEM_ALIGN_NEXT(size);
if ((size_t)(endmem - nextmem) < size)
return NULL;
endmem -= size;
return endmem;
}
/**
* @brief Allocates a memory block.
* @details The size of the returned block is aligned to the alignment
* type so it is not possible to allocate less than
* <code>MEM_ALIGN_SIZE</code>.
*
* @param[in] size the size of the block to be allocated.
* @return A pointer to the allocated memory block.
* @retval NULL allocation failed, core memory exhausted.
*
* @iclass
*/
void *chCoreAllocI(size_t size) {
void *p;
chDbgCheckClassI();
size = MEM_ALIGN_NEXT(size);
if ((size_t)(endmem - nextmem) < size)
return NULL;
p = nextmem;
nextmem += size;
return p;
}
/**
* @brief Allocates a memory block.
* @details The size of the returned block is aligned to the alignment
* type so it is not possible to allocate less than
* <code>MEM_ALIGN_SIZE</code>.
*
* @param[in] size the size of the block to be allocated.
* @return A pointer to the allocated memory block.
* @retval NULL allocation failed, core memory exhausted.
*
* @iclass
*/
void *chCoreAllocI(size_t size) {
void *p;
chDbgCheckClassI();
size = MEM_ALIGN_NEXT(size);
/*lint -save -e9033 [10.8] The cast is safe.*/
if ((size_t)(endmem - nextmem) < size) {
/*lint -restore*/
return NULL;
}
p = nextmem;
nextmem += size;
return p;
}
/**
* @brief Creates a new thread into a static memory area.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
*
* @param[out] wsp pointer to a working area dedicated to the thread stack
* @param[in] size size of the working area
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
*
* @api
*/
thread_t *chThdCreateStatic(void *wsp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
thread_t *tp;
chDbgCheck((wsp != NULL) &&
MEM_IS_ALIGNED(wsp, PORT_WORKING_AREA_ALIGN) &&
(size >= THD_WORKING_AREA_SIZE(0)) &&
MEM_IS_ALIGNED(size, PORT_STACK_ALIGN) &&
(prio <= HIGHPRIO) && (pf != NULL));
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
/* The thread structure is laid out in the upper part of the thread
workspace. The thread position structure is aligned to the required
stack alignment because it represents the stack top.*/
tp = (thread_t *)((uint8_t *)wsp + size -
MEM_ALIGN_NEXT(sizeof (thread_t), PORT_STACK_ALIGN));
/* Stack boundary.*/
tp->stklimit = (stkalign_t *)wsp;
/* Setting up the port-dependent part of the working area.*/
PORT_SETUP_CONTEXT(tp, wsp, tp, pf, arg);
tp = _thread_init(tp, "noname", prio);
/* Starting the thread immediately.*/
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
