u16 yPeekContinuousFifoEx(yFifoBuf *buf, u8 **ptr, u16 startofs)
{
u8 *lptr;
if(startofs >= buf->datasize) {
return 0;
}
lptr = buf->head + startofs;
if(lptr >= YFIFOEND(buf)) {
// wrap
if(ptr) {
*ptr = lptr - buf->buffsize;
}
return buf->datasize - startofs;
} else {
// no wrap
u16 toend = (u16)(YFIFOEND(buf) - lptr);
if(ptr) {
*ptr = lptr;
}
return (toend < buf->datasize ? toend : buf->datasize);
}
}
u16 yPeekFifoEx(yFifoBuf *buf, u8 *data, u16 datalen, u16 startofs)
{
u8 *ptr ;
if(startofs > buf->datasize){
return 0;
}
if (datalen + startofs > buf->datasize)
datalen = buf->datasize - startofs;
ptr=buf->head+startofs;
if(ptr >= YFIFOEND(buf)){
ptr -= buf->buffsize;
}
if (ptr + datalen > YFIFOEND(buf)) {
//need to handle wrap
u16 firstpart = (u16)(YFIFOEND(buf) - ptr);
if (data) {
memcpy(data, ptr, firstpart);
memcpy(data + firstpart, buf->buff, datalen - firstpart);
}
} else {
if (data) {
memcpy(data, ptr, datalen);
}
}
return datalen;
}
u16 yPopFifoEx(yFifoBuf *buf, u8 *data, u16 datalen)
{
if (datalen > buf->datasize)
datalen = buf->datasize;
if (buf->head + datalen > YFIFOEND(buf)) {
//need to handle wrap
u16 firstpart = (u16)(YFIFOEND(buf) - buf->head);
if (data) {
memcpy(data, buf->head, firstpart);
memcpy(data + firstpart, buf->buff, datalen - firstpart);
}
buf->head = buf->buff + (datalen - firstpart);
} else {
if (data) {
memcpy(data, buf->head, datalen);
}
buf->head += datalen;
if (buf->head == YFIFOEND(buf))
buf->head -= buf->buffsize;
}
// This must remain very last, so that buffsize-datasize always
// remain the number of bytes really free in buffer (may be polled
// from within interrupt handlers)
buf->datasize -= datalen;
#ifdef DEBUG_FIFO
buf->totalPopded += datalen;
#endif
return datalen;
}
u16 yPushFifoEx(yFifoBuf *buf, const u8 *data, u16 datalen)
{
u16 freespace = buf->buffsize - buf->datasize;
if (datalen > freespace) {
// not enough space available in
// buffer to handle correctly this packet
// (we do not handle partial packet since usb packet are
// max 64byte long)
return 0;
}
//append data to our fifo buffer
if (buf->tail + datalen <= YFIFOEND(buf)) {
memcpy(buf->tail, data, datalen);
buf->tail += datalen;
if (buf->tail == YFIFOEND(buf))
buf->tail = buf->buff;
} else {
u16 cplen = (u16)(YFIFOEND(buf) - buf->tail);
memcpy(buf->tail, data, cplen);
memcpy(buf->buff, data + cplen, datalen - cplen);
buf->tail = buf->buff + (datalen - cplen);
}
// This must remain very last, so that datasize always remain
// the number of bytes really available in buffer (may be polled
// from within interrupt handlers)
buf->datasize += datalen;
#ifdef DEBUG_FIFO
buf->totalPushed += datalen;
#endif
return datalen;
}
u16 yPeekContinuousFifoEx(yFifoBuf *buf, u8 **ptr,u16 *len, u16 startofs)
{
u8 *lptr;
if(startofs > buf->datasize) {
*len =0;
return 0;
}
lptr = buf->head + startofs;
if(lptr >= YFIFOEND(buf)) {
// wrap
*len =buf->datasize - startofs;
if(ptr)
*ptr = lptr - buf->buffsize;
return 0;
} else {
// no wrap
u16 toend = (u16)(YFIFOEND(buf) - lptr);
if(toend < buf->datasize)
*len = toend;
else
*len = buf->datasize;
if(ptr)
*ptr = lptr;
return buf->datasize - *len;
}
}
u16 ySeekFifoEx(yFifoBuf *buf, const u8* pattern, u16 patlen, u16 startofs, u16 searchlen, u8 bTextCompare)
{
u8 *ptr;
u16 patidx;
u16 firstmatch = 0xffff;
// pattern bigger than our buffer size -> not found
if (startofs + patlen > buf->datasize) {
return 0xffff;
}
// ajust searchlen and ptr to our buffer
// size and position
if (searchlen == 0 || searchlen > buf->datasize - startofs)
searchlen = buf->datasize - startofs;
ptr = buf->head + startofs;
if (ptr >= YFIFOEND(buf))
ptr -= buf->buffsize;
patidx = 0;
while (searchlen > 0 && patidx < patlen) {
u8 bletter = *ptr;
u8 match = 0;
if (!bTextCompare){
if(pattern[patidx] == bletter) {
match = 1;
}
} else {
if (pattern[patidx] >= 'a' && pattern[patidx] <= 'z'){
bletter = tolower(bletter);
}else{
bletter = toupper(bletter);
}
if (bletter == pattern[patidx])
match = 1;
}
if (match) {
if(patidx == 0) {
firstmatch = startofs;
}
patidx++;
} else if(patidx > 0) {
// rescan this character as first pattern character
patidx = 0;
continue;
}
startofs++;
searchlen--;
ptr++;
if (ptr >= YFIFOEND(buf))
ptr -= buf->buffsize;
}
if (patidx == patlen) {
return firstmatch;
}
return 0xffff;
}
