pinebuds/tests/anc_usb/safe_queue.c

162 lines
3.4 KiB
C

/***************************************************************************
*
* Copyright 2015-2019 BES.
* All rights reserved. All unpublished rights reserved.
*
* No part of this work may be used or reproduced in any form or by any
* means, or stored in a database or retrieval system, without prior written
* permission of BES.
*
* Use of this work is governed by a license granted by BES.
* This work contains confidential and proprietary information of
* BES. which is protected by copyright, trade secret,
* trademark and other intellectual property rights.
*
****************************************************************************/
#include "cmsis.h"
#include "safe_queue.h"
#include "hal_trace.h"
void safe_queue_init(struct SAFE_QUEUE_T *queue, uint32_t *items, uint32_t size)
{
// queue->first points to the first valid item
queue->first = 0;
// queue->last points to the first empty slot (beyond the last valid item)
queue->last = 0;
queue->count = 0;
queue->size = size;
queue->watermark = 0;
queue->items = items;
}
int safe_queue_put(struct SAFE_QUEUE_T *queue, uint32_t item)
{
int ret = 0;
uint32_t lock;
lock = int_lock();
if (queue->count >= queue->size) {
ret = 1;
} else {
queue->items[queue->last] = item;
queue->last++;
if (queue->last >= queue->size) {
queue->last = 0;
}
queue->count++;
if (queue->count > queue->watermark) {
queue->watermark = queue->count;
}
}
int_unlock(lock);
return ret;
}
int safe_queue_get(struct SAFE_QUEUE_T *queue, uint32_t *itemp)
{
int ret = 0;
uint32_t lock;
lock = int_lock();
if (queue->count == 0) {
ret = 1;
} else {
if (itemp) {
*itemp = queue->items[queue->first];
}
queue->first++;
if (queue->first >= queue->size) {
queue->first = 0;
}
queue->count--;
}
int_unlock(lock);
return ret;
}
int safe_queue_pop(struct SAFE_QUEUE_T *queue, uint32_t *itemp)
{
int ret = 0;
uint32_t lock;
lock = int_lock();
if (queue->count == 0) {
ret = 1;
} else {
if (queue->last) {
queue->last--;
} else {
queue->last = queue->size - 1;
}
if (itemp) {
*itemp = queue->items[queue->last];
}
queue->count--;
}
int_unlock(lock);
return ret;
}
int safe_queue_peek(const struct SAFE_QUEUE_T *queue, int offset, uint32_t *itemp)
{
int ret = 0;
uint32_t lock;
uint8_t idx;
lock = int_lock();
if (offset >= 0 && offset < queue->count) {
idx = queue->first + offset;
} else if (offset < 0 && -offset <= queue->count) {
idx = queue->size + queue->last + offset;
} else {
ret = 1;
}
if (ret == 0) {
if (idx >= queue->size) {
idx -= queue->size;
}
*itemp = queue->items[idx];
}
int_unlock(lock);
return ret;
}
uint32_t safe_queue_watermark(const struct SAFE_QUEUE_T *queue)
{
return queue->watermark;
}
int safe_queue_dump(const struct SAFE_QUEUE_T *queue)
{
uint32_t cnt;
int i;
uint32_t item;
cnt = queue->count;
for (i = 0; i < cnt; i++) {
if (safe_queue_peek(queue, i, &item)) {
break;
}
TRACE_IMM(3,"SAFE-QUEUE-DUMP: [%02d/%02d] 0x%08x", i, cnt, item);
}
return 0;
}