123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580 |
- #include "sdk_common.h"
- #if NRF_MODULE_ENABLED(NRF_QUEUE)
- #include "nrf_queue.h"
- #include "app_util_platform.h"
- #if NRF_QUEUE_CONFIG_LOG_ENABLED
- #define NRF_LOG_LEVEL NRF_QUEUE_CONFIG_LOG_LEVEL
- #define NRF_LOG_INIT_FILTER_LEVEL NRF_QUEUE_CONFIG_LOG_INIT_FILTER_LEVEL
- #define NRF_LOG_INFO_COLOR NRF_QUEUE_CONFIG_INFO_COLOR
- #define NRF_LOG_DEBUG_COLOR NRF_QUEUE_CONFIG_DEBUG_COLOR
- #else
- #define NRF_LOG_LEVEL 0
- #endif
- #include "nrf_log.h"
- NRF_SECTION_DEF(nrf_queue, nrf_queue_t);
- #if NRF_QUEUE_CLI_CMDS && NRF_CLI_ENABLED
- #include "nrf_cli.h"
- static void nrf_queue_status(nrf_cli_t const * p_cli, size_t argc, char **argv)
- {
- UNUSED_PARAMETER(argv);
- if (nrf_cli_help_requested(p_cli))
- {
- nrf_cli_help_print(p_cli, NULL, 0);
- return;
- }
- if (argc > 1)
- {
- nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Bad argument count");
- return;
- }
- uint32_t num_of_instances = NRF_SECTION_ITEM_COUNT(nrf_queue, nrf_queue_t);
- uint32_t i;
- for (i = 0; i < num_of_instances; i++)
- {
- const nrf_queue_t * p_instance = NRF_SECTION_ITEM_GET(nrf_queue, nrf_queue_t, i);
- uint32_t element_size = p_instance->element_size;
- uint32_t size = p_instance->size;
- uint32_t max_util = nrf_queue_max_utilization_get(p_instance);
- uint32_t util = nrf_queue_utilization_get(p_instance);
- const char * p_name = p_instance->p_name;
- nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL,
- "%s\r\n\t- Element size:\t%d\r\n"
- "\t- Usage:\t%u%% (%u out of %u elements)\r\n"
- "\t- Maximum:\t%u%% (%u out of %u elements)\r\n"
- "\t- Mode:\t\t%s\r\n\r\n",
- p_name, element_size,
- 100ul * util/size, util,size,
- 100ul * max_util/size, max_util,size,
- (p_instance->mode == NRF_QUEUE_MODE_OVERFLOW) ? "Overflow" : "No overflow");
- }
- }
- NRF_CLI_CREATE_STATIC_SUBCMD_SET(nrf_queue_commands)
- {
- NRF_CLI_CMD(status, NULL, "Print status of queue instances.", nrf_queue_status),
- NRF_CLI_SUBCMD_SET_END
- };
- NRF_CLI_CMD_REGISTER(queue, &nrf_queue_commands, "Commands for BALLOC management", nrf_queue_status);
- #endif
- __STATIC_INLINE size_t circullar_buffer_size_get(nrf_queue_t const * p_queue)
- {
- static const uint8_t full_queue_indicator = 1;
-
- return p_queue->size + full_queue_indicator;
- }
- __STATIC_INLINE size_t nrf_queue_next_idx(nrf_queue_t const * p_queue, size_t idx)
- {
- ASSERT(p_queue != NULL);
- return (idx < p_queue->size) ? (idx + 1) : 0;
- }
- __STATIC_INLINE size_t queue_utilization_get(nrf_queue_t const * p_queue)
- {
- size_t front = p_queue->p_cb->front;
- size_t back = p_queue->p_cb->back;
- return (back >= front) ? (back - front) :
- (circullar_buffer_size_get(p_queue) - front + back);
- }
- bool nrf_queue_is_full(nrf_queue_t const * p_queue)
- {
- ASSERT(p_queue != NULL);
- size_t front = p_queue->p_cb->front;
- size_t back = p_queue->p_cb->back;
- return (nrf_queue_next_idx(p_queue, back) == front);
- }
- ret_code_t nrf_queue_push(nrf_queue_t const * p_queue, void const * p_element)
- {
- ret_code_t status = NRF_SUCCESS;
- ASSERT(p_queue != NULL);
- ASSERT(p_element != NULL);
- CRITICAL_REGION_ENTER();
- bool is_full = nrf_queue_is_full(p_queue);
- if (!is_full || (p_queue->mode == NRF_QUEUE_MODE_OVERFLOW))
- {
-
- size_t write_pos = p_queue->p_cb->back;
- p_queue->p_cb->back = nrf_queue_next_idx(p_queue, p_queue->p_cb->back);
- if (is_full)
- {
-
- NRF_LOG_INST_WARNING(p_queue->p_log, "Queue full. Overwriting oldest element.");
- p_queue->p_cb->front = nrf_queue_next_idx(p_queue, p_queue->p_cb->front);
- }
-
- switch (p_queue->element_size)
- {
- case sizeof(uint8_t):
- ((uint8_t *)p_queue->p_buffer)[write_pos] = *((uint8_t *)p_element);
- break;
- case sizeof(uint16_t):
- ((uint16_t *)p_queue->p_buffer)[write_pos] = *((uint16_t *)p_element);
- break;
- case sizeof(uint32_t):
- ((uint32_t *)p_queue->p_buffer)[write_pos] = *((uint32_t *)p_element);
- break;
- case sizeof(uint64_t):
- ((uint64_t *)p_queue->p_buffer)[write_pos] = *((uint64_t *)p_element);
- break;
- default:
- memcpy((void *)((size_t)p_queue->p_buffer + write_pos * p_queue->element_size),
- p_element,
- p_queue->element_size);
- break;
- }
-
- size_t utilization = queue_utilization_get(p_queue);
- if (p_queue->p_cb->max_utilization < utilization)
- {
- p_queue->p_cb->max_utilization = utilization;
- }
- }
- else
- {
- status = NRF_ERROR_NO_MEM;
- }
- CRITICAL_REGION_EXIT();
- NRF_LOG_INST_DEBUG(p_queue->p_log, "pushed element 0x%08X, status:%d", p_element, status);
- return status;
- }
- ret_code_t nrf_queue_generic_pop(nrf_queue_t const * p_queue,
- void * p_element,
- bool just_peek)
- {
- ret_code_t status = NRF_SUCCESS;
- ASSERT(p_queue != NULL);
- ASSERT(p_element != NULL);
- CRITICAL_REGION_ENTER();
- if (!nrf_queue_is_empty(p_queue))
- {
-
- size_t read_pos = p_queue->p_cb->front;
-
- if (!just_peek)
- {
- p_queue->p_cb->front = nrf_queue_next_idx(p_queue, p_queue->p_cb->front);
- }
-
- switch (p_queue->element_size)
- {
- case sizeof(uint8_t):
- *((uint8_t *)p_element) = ((uint8_t *)p_queue->p_buffer)[read_pos];
- break;
- case sizeof(uint16_t):
- *((uint16_t *)p_element) = ((uint16_t *)p_queue->p_buffer)[read_pos];
- break;
- case sizeof(uint32_t):
- *((uint32_t *)p_element) = ((uint32_t *)p_queue->p_buffer)[read_pos];
- break;
- case sizeof(uint64_t):
- *((uint64_t *)p_element) = ((uint64_t *)p_queue->p_buffer)[read_pos];
- break;
- default:
- memcpy(p_element,
- (void const *)((size_t)p_queue->p_buffer + read_pos * p_queue->element_size),
- p_queue->element_size);
- break;
- }
- }
- else
- {
- status = NRF_ERROR_NOT_FOUND;
- }
- CRITICAL_REGION_EXIT();
- NRF_LOG_INST_DEBUG(p_queue->p_log, "%s element 0x%08X, status:%d",
- just_peek ? "peeked" : "popped", p_element, status);
- return status;
- }
- static size_t continous_items_get(nrf_queue_t const * p_queue, bool write)
- {
- size_t front = p_queue->p_cb->front;
- size_t back = p_queue->p_cb->back;
-
- if (write)
- {
- return (back >= front) ? circullar_buffer_size_get(p_queue) - back : front - back;
- }
- else
- {
- return (back >= front) ? back - front : circullar_buffer_size_get(p_queue) - front;
- }
- }
- static void queue_write(nrf_queue_t const * p_queue, void const * p_data, uint32_t element_count)
- {
- size_t prev_available = nrf_queue_available_get(p_queue);
- size_t continuous = continous_items_get(p_queue, true);
- void * p_write_ptr = (void *)((size_t)p_queue->p_buffer
- + p_queue->p_cb->back * p_queue->element_size);
- if (element_count <= continuous)
- {
- memcpy(p_write_ptr,
- p_data,
- element_count * p_queue->element_size);
- p_queue->p_cb->back = ((p_queue->p_cb->back + element_count) <= p_queue->size)
- ? (p_queue->p_cb->back + element_count)
- : 0;
- }
- else
- {
- size_t first_write_length = continuous * p_queue->element_size;
- memcpy(p_write_ptr,
- p_data,
- first_write_length);
- size_t elements_left = element_count - continuous;
- memcpy(p_queue->p_buffer,
- (void const *)((size_t)p_data + first_write_length),
- elements_left * p_queue->element_size);
- p_queue->p_cb->back = elements_left;
- if (prev_available < element_count)
- {
-
- p_queue->p_cb->front = nrf_queue_next_idx(p_queue, p_queue->p_cb->back);
- }
- }
-
- size_t utilization = queue_utilization_get(p_queue);
- if (p_queue->p_cb->max_utilization < utilization)
- {
- p_queue->p_cb->max_utilization = utilization;
- }
- }
- ret_code_t nrf_queue_write(nrf_queue_t const * p_queue,
- void const * p_data,
- size_t element_count)
- {
- ret_code_t status = NRF_SUCCESS;
- ASSERT(p_queue != NULL);
- ASSERT(p_data != NULL);
- ASSERT(element_count <= p_queue->size);
- if (element_count == 0)
- {
- return NRF_SUCCESS;
- }
- CRITICAL_REGION_ENTER();
- if ((nrf_queue_available_get(p_queue) >= element_count)
- || (p_queue->mode == NRF_QUEUE_MODE_OVERFLOW))
- {
- queue_write(p_queue, p_data, element_count);
- }
- else
- {
- status = NRF_ERROR_NO_MEM;
- }
- CRITICAL_REGION_EXIT();
- NRF_LOG_INST_DEBUG(p_queue->p_log, "Write %d elements (start address: 0x%08X), status:%d",
- element_count, p_data, status);
- return status;
- }
- size_t nrf_queue_in(nrf_queue_t const * p_queue,
- void const * p_data,
- size_t element_count)
- {
- ASSERT(p_queue != NULL);
- ASSERT(p_data != NULL);
- size_t req_element_count = element_count;
- if (element_count == 0)
- {
- return 0;
- }
- CRITICAL_REGION_ENTER();
- if (p_queue->mode == NRF_QUEUE_MODE_OVERFLOW)
- {
- element_count = MIN(element_count, p_queue->size);
- }
- else
- {
- size_t available = nrf_queue_available_get(p_queue);
- element_count = MIN(element_count, available);
- }
- queue_write(p_queue, p_data, element_count);
- CRITICAL_REGION_EXIT();
- NRF_LOG_INST_DEBUG(p_queue->p_log, "Put in %d elements (start address: 0x%08X), requested :%d",
- element_count, p_data, req_element_count);
- return element_count;
- }
- static void queue_read(nrf_queue_t const * p_queue, void * p_data, uint32_t element_count)
- {
- size_t front = p_queue->p_cb->front;
- size_t continuous = continous_items_get(p_queue, false);
- void const * p_read_ptr = (void const *)((size_t)p_queue->p_buffer
- + front * p_queue->element_size);
- if (element_count <= continuous)
- {
- memcpy(p_data,
- p_read_ptr,
- element_count * p_queue->element_size);
- p_queue->p_cb->front = ((front + element_count) <= p_queue->size)
- ? (front + element_count)
- : 0;
- }
- else
- {
- size_t first_read_length = continuous * p_queue->element_size;
- memcpy(p_data,
- p_read_ptr,
- first_read_length);
- size_t elements_left = element_count - continuous;
- memcpy((void *)((size_t)p_data + first_read_length),
- p_queue->p_buffer,
- elements_left * p_queue->element_size);
- p_queue->p_cb->front = elements_left;
- }
- }
- ret_code_t nrf_queue_read(nrf_queue_t const * p_queue,
- void * p_data,
- size_t element_count)
- {
- ret_code_t status = NRF_SUCCESS;
- ASSERT(p_queue != NULL);
- ASSERT(p_data != NULL);
- if (element_count == 0)
- {
- return NRF_SUCCESS;
- }
- CRITICAL_REGION_ENTER();
- if (element_count <= queue_utilization_get(p_queue))
- {
- queue_read(p_queue, p_data, element_count);
- }
- else
- {
- status = NRF_ERROR_NOT_FOUND;
- }
- CRITICAL_REGION_EXIT();
- NRF_LOG_INST_DEBUG(p_queue->p_log, "Read %d elements (start address: 0x%08X), status :%d",
- element_count, p_data, status);
- return status;
- }
- size_t nrf_queue_out(nrf_queue_t const * p_queue,
- void * p_data,
- size_t element_count)
- {
- ASSERT(p_queue != NULL);
- ASSERT(p_data != NULL);
- size_t req_element_count = element_count;
- if (element_count == 0)
- {
- return 0;
- }
- CRITICAL_REGION_ENTER();
- size_t utilization = queue_utilization_get(p_queue);
- element_count = MIN(element_count, utilization);
- queue_read(p_queue, p_data, element_count);
- CRITICAL_REGION_EXIT();
- NRF_LOG_INST_DEBUG(p_queue->p_log, "Out %d elements (start address: 0x%08X), requested :%d",
- element_count, p_data, req_element_count);
- return element_count;
- }
- void nrf_queue_reset(nrf_queue_t const * p_queue)
- {
- ASSERT(p_queue != NULL);
- CRITICAL_REGION_ENTER();
- memset(p_queue->p_cb, 0, sizeof(nrf_queue_cb_t));
- CRITICAL_REGION_EXIT();
- NRF_LOG_INST_DEBUG(p_queue->p_log, "Reset");
- }
- size_t nrf_queue_utilization_get(nrf_queue_t const * p_queue)
- {
- size_t utilization;
- ASSERT(p_queue != NULL);
- CRITICAL_REGION_ENTER();
- utilization = queue_utilization_get(p_queue);
- CRITICAL_REGION_EXIT();
- return utilization;
- }
- bool nrf_queue_is_empty(nrf_queue_t const * p_queue)
- {
- ASSERT(p_queue != NULL);
- size_t front = p_queue->p_cb->front;
- size_t back = p_queue->p_cb->back;
- return (front == back);
- }
- size_t nrf_queue_available_get(nrf_queue_t const * p_queue)
- {
- ASSERT(p_queue != NULL);
- return p_queue->size - nrf_queue_utilization_get(p_queue);
- }
- size_t nrf_queue_max_utilization_get(nrf_queue_t const * p_queue)
- {
- ASSERT(p_queue != NULL);
- return p_queue->p_cb->max_utilization;
- }
- void nrf_queue_max_utilization_reset(nrf_queue_t const * p_queue)
- {
- ASSERT(p_queue != NULL);
- p_queue->p_cb->max_utilization = 0;
- }
- #endif
|