pinebuds/utils/cqueue/cqueue.c

421 lines
8.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.
*
****************************************************************************/
/***
* cqueue.c - c circle queue c file
*/
#include "cqueue.h"
#include <stdio.h>
#include "cmsis.h"
#include "hal_uart.h"
#include <string.h>
//#define DEBUG_CQUEUE 1
CQ_FUNC_ATTR
int InitCQueue(CQueue *Q, unsigned int size, CQItemType *buf)
{
Q->size = size;
Q->base = buf;
Q->len = 0;
if (!Q->base)
return CQ_ERR;
Q->read = Q->write = 0;
return CQ_OK;
}
CQ_FUNC_ATTR
int IsEmptyCQueue(CQueue *Q)
{
if (Q->len == 0)
return CQ_OK;
else
return CQ_ERR;
}
CQ_FUNC_ATTR
int LengthOfCQueue(CQueue *Q)
{
return Q->len;
}
CQ_FUNC_ATTR
int AvailableOfCQueue(CQueue *Q)
{
return (Q->size - Q->len);
}
#if 1
CQ_FUNC_ATTR
int EnCQueue(CQueue *Q, CQItemType *e, unsigned int len)
{
if (AvailableOfCQueue(Q) < len) {
return CQ_ERR;
}
Q->len += len;
uint32_t bytesToTheEnd = Q->size - Q->write;
if (bytesToTheEnd > len)
{
memcpy((uint8_t *)&Q->base[Q->write], (uint8_t *)e, len);
Q->write += len;
}
else
{
memcpy((uint8_t *)&Q->base[Q->write], (uint8_t *)e, bytesToTheEnd);
memcpy((uint8_t *)&Q->base[0], (((uint8_t *)e)+bytesToTheEnd), len-bytesToTheEnd);
Q->write = len-bytesToTheEnd;
}
return CQ_OK;
}
int EnCQueue_AI(CQueue *Q, CQItemType *e, unsigned int len)
{
uint32_t bytesToTheEnd = Q->size - Q->write;
uint32_t readBytesToTheEnd = Q->size - Q->read;
if(AvailableOfCQueue(Q) < len) {
if(readBytesToTheEnd > len) {
Q->read += len;
} else {
Q->read = len - readBytesToTheEnd;
}
} else {
Q->len += len;
}
if (bytesToTheEnd > len) {
memcpy((uint8_t *)&Q->base[Q->write], (uint8_t *)e, len);
Q->write += len;
} else {
memcpy((uint8_t *)&Q->base[Q->write], (uint8_t *)e, bytesToTheEnd);
memcpy((uint8_t *)&Q->base[0], (((uint8_t *)e)+bytesToTheEnd), len-bytesToTheEnd);
Q->write = len-bytesToTheEnd;
}
return CQ_OK;
}
#else
static inline void memcpy_u8(void *dst, void *src, unsigned int len)
{
unsigned int i = 0;
unsigned int *d8, *s8;
d8 = dst;
s8 = src;
for(i=0;i < len;i++)
d8[i] = s8[i];
}
static inline void memcpy_u32(void *dst, void *src, unsigned int len)
{
unsigned int i = 0;
unsigned int *d32, *s32;
d32 = dst;
s32 = src;
for(i = 0;i < len;i++)
d32[i] = s32[i];
}
CQ_FUNC_ATTR
int EnCQueue(CQueue *Q, CQItemType *e, unsigned int len)
{
unsigned char *src = e;
unsigned char *dst = &(Q->base[Q->write]);
unsigned int cnt_u8;
unsigned int cnt_u32;
unsigned int cnt_u32_res;
unsigned int front_u8;
unsigned int front_u32;
unsigned int front_u32_res;
unsigned int end_u8;
unsigned int end_u32;
unsigned int end_u32_res;
bool unaligned_en;
if (AvailableOfCQueue(Q) < len) {
return CQ_ERR;
}
Q->len += len;
end_u8 = Q->size - Q->write;
end_u32 = end_u8 / 4;
end_u32_res = end_u8 % 4;
cnt_u8 = len;
cnt_u32 = cnt_u8 / 4;
cnt_u32_res = cnt_u8 % 4;
unaligned_en = config_unaligned_access(true);
if(cnt_u8 <= end_u8)
{
memcpy_u32(dst, src, cnt_u32);
if(cnt_u32_res)
{
src += cnt_u32 * 4;
dst += cnt_u32 * 4;
memcpy_u8(dst, src, cnt_u32_res);
}
// Deal with Q->write
Q->write += cnt_u8;
}
else
{
front_u8 = len - end_u8;
front_u32 = front_u8 / 4;
front_u32_res = front_u8 % 4;
// Deal with end data
memcpy_u32(dst, src, end_u32);
src += end_u32 * 4;
dst += end_u32 * 4;
memcpy_u8(dst, src, end_u32_res);
src += end_u32_res;
dst = &(Q->base[0]);
// Deal with front data
memcpy_u32(dst, src, front_u32);
if(front_u32_res)
{
src += front_u32 * 4;
dst += front_u32 * 4;
memcpy_u8(dst, src, front_u32_res);
}
// Deal with Q->write
Q->write = front_u8;
}
config_unaligned_access(unaligned_en);
return CQ_OK;
}
void memcpy_fast(char *dst, char *src, unsigned int len)
{
int size = len/4;
int mod = len%4;
int *Dst = (int *)dst;
int *Src = (int *)src;
int dst_offset = (int)Dst - (int)dst;
int src_offset = (int)Src -(int)src;
if(!dst_offset && !src_offset){
memcpy_u32(Dst, Src, size);
if(mod){
memcpy_u8(Dst + size, Src + size, mod);
}
}else{
memcpy_u8(dst, src, len);
}
}
#endif
CQ_FUNC_ATTR
int EnCQueueFront(CQueue *Q, CQItemType *e, unsigned int len)
{
if (AvailableOfCQueue(Q) < len) {
return CQ_ERR;
}
Q->len += len;
/* walk to last item , revert write */
e = e + len - 1;
if(Q->read == 0)
Q->read = Q->size - 1;
else
Q->read--;
while(len > 0) {
Q->base[Q->read] = *e;
--Q->read;
--e;
--len;
if(Q->read < 0)
Q->read = Q->size - 1;
}
/* we walk one more, walk back */
if(Q->read == Q->size - 1)
Q->read = 0;
else
++Q->read;
return CQ_OK;
}
CQ_FUNC_ATTR
int DeCQueue(CQueue *Q, CQItemType *e, unsigned int len)
{
if(LengthOfCQueue(Q) < len)
return CQ_ERR;
Q->len -= len;
if(e != NULL)
{
uint32_t bytesToTheEnd = Q->size - Q->read;
if (bytesToTheEnd > len)
{
memcpy((uint8_t *)e, (uint8_t *)&Q->base[Q->read], len);
Q->read += len;
}
else
{
memcpy((uint8_t *)e, (uint8_t *)&Q->base[Q->read], bytesToTheEnd);
memcpy((((uint8_t *)e)+bytesToTheEnd), (uint8_t *)&Q->base[0], len-bytesToTheEnd);
Q->read = len-bytesToTheEnd;
}
}
else
{
if (0 < Q->size)
{
Q->read = (Q->read+len)%Q->size;
}
else
{
Q->read = 0;
}
}
return CQ_OK;
}
CQ_FUNC_ATTR
int PeekCQueue(CQueue *Q, unsigned int len_want, CQItemType **e1, unsigned int *len1, CQItemType **e2, unsigned int *len2)
{
if(LengthOfCQueue(Q) < len_want) {
return CQ_ERR;
}
*e1 = &(Q->base[Q->read]);
if((Q->write > Q->read) || (Q->size - Q->read >= len_want)) {
*len1 = len_want;
*e2 = NULL;
*len2 = 0;
return CQ_OK;
}
else {
*len1 = Q->size - Q->read;
*e2 = &(Q->base[0]);
*len2 = len_want - *len1;
return CQ_OK;
}
return CQ_ERR;
}
int PeekCQueueToBuf(CQueue *Q, CQItemType *e, unsigned int len)
{
int status = CQ_OK;
unsigned char *e1 = NULL, *e2 = NULL;
unsigned int len1 = 0, len2 = 0;
status = PeekCQueue(Q, len, &e1, &len1, &e2, &len2);
if(status == CQ_OK) {
if (len == (len1 + len2)) {
memcpy(e, e1, len1);
memcpy(e + len1, e2, len2);
} else {
status = CQ_ERR;
}
}
return status;
}
int PullCQueue(CQueue *Q, CQItemType *e, unsigned int len)
{
int status = CQ_OK;
unsigned char *e1 = NULL, *e2 = NULL;
unsigned int len1 = 0, len2 = 0;
status = PeekCQueue(Q, len, &e1, &len1, &e2, &len2);
if(status == CQ_OK){
if (len == (len1 + len2)){
memcpy(e, e1, len1);
memcpy(e + len1, e2, len2);
DeCQueue(Q, 0, len);
}else{
status = CQ_ERR;
}
}
return status;
}
#ifdef DEBUG_CQUEUE
int DumpCQueue(CQueue *Q)
{
CQItemType e;
int len = 0, i = 0;
len = LengthOfCQueue(Q);
if(len <= 0)
return CQ_ERR;
i = Q->read;
while(len > 0) {
e = Q->base[i];
TRACE(stderr, "-0x%x", e);
TRACE(stderr, "-%c", e);
++i;
--len;
if(i == Q->size)
i = 0;
}
return CQ_ERR;
}
#endif
void ResetCQueue(CQueue *Q)
{
Q->len = 0;
Q->read = Q->write = 0;
}