419 lines
11 KiB
C
419 lines
11 KiB
C
#include <fifo.h>
|
|
#include <os_macros.h>
|
|
#include <ctype.h> // isspace
|
|
#include <limits.h> // ULONG_MAX
|
|
|
|
/* ------------------------------------------------------------------------------------------------------------------ */
|
|
/* */
|
|
|
|
|
|
// 在尾部写入数据
|
|
os_err_t fifo_put(fifo_t* self, uint8_t data){
|
|
|
|
os_size_t write_idx = self->write_idx;
|
|
|
|
os_size_t next_write_idx = fifo_next(self, self->write_idx);
|
|
|
|
if(next_write_idx == self->read_idx){
|
|
return OS_ERR_FULL;
|
|
}
|
|
|
|
self->buffer[write_idx] = data;
|
|
self->write_idx = next_write_idx;
|
|
|
|
return OS_ERR_OK;
|
|
}
|
|
|
|
// 从头部获取数据
|
|
os_err_t fifo_get(fifo_t* self, uint8_t* data){
|
|
os_size_t read_idx = self->read_idx;
|
|
if(read_idx==self->write_idx){
|
|
return OS_ERR_EMPTY;
|
|
}
|
|
|
|
if(data){
|
|
*data = self->buffer[read_idx];
|
|
}
|
|
|
|
self->read_idx = fifo_next(self, self->read_idx);
|
|
return OS_ERR_OK;
|
|
}
|
|
|
|
// 批量写入数据
|
|
os_size_t fifo_write(fifo_t* self, uint8_t* data, os_size_t size){
|
|
os_size_t i;
|
|
for(i=0; i<size; i++){
|
|
if(fifo_put(self, data[i])!=OS_ERR_OK){
|
|
break;
|
|
}
|
|
}
|
|
return i;
|
|
}
|
|
|
|
// 批量读取数据
|
|
os_size_t fifo_read(fifo_t* self, uint8_t* buf, os_size_t size){
|
|
os_size_t i;
|
|
for(i=0; i<size; i++){
|
|
if(fifo_get(self, buf++)!=OS_ERR_OK){
|
|
break;
|
|
}
|
|
}
|
|
return i;
|
|
}
|
|
|
|
os_size_t fifo_write_fast(fifo_t* rb, const uint8_t * data, os_size_t len){
|
|
if (rb == NULL || data == NULL || len == 0) {
|
|
return 0;
|
|
}
|
|
|
|
/* 如果写入数据超过剩余空间，则只写入剩余空间部分（或者可以选择报错/覆盖，这里选择截断） */
|
|
os_size_t free_sp = fifo_space(rb);
|
|
if (len > free_sp) {
|
|
len = free_sp;
|
|
}
|
|
|
|
if (len == 0) {
|
|
// 没有空间了
|
|
return 0;
|
|
}
|
|
|
|
// 分段写入
|
|
// buf_size: 8
|
|
// 0, 1, 2, 3, 4, 5, 6, 7
|
|
// r w
|
|
// w r
|
|
os_size_t write_idx = rb->write_idx;
|
|
os_size_t read_idx = rb->read_idx;
|
|
os_size_t size = rb->buffer_size;
|
|
|
|
if(write_idx > read_idx){
|
|
os_size_t first_part = size - write_idx;
|
|
if(first_part >= len){
|
|
// 一次就可以拷贝完成
|
|
memcpy(&rb->buffer[write_idx], data, first_part);
|
|
}else{
|
|
// 分成两个部分
|
|
memcpy(&rb->buffer[write_idx], data, first_part);
|
|
os_size_t second_part = len - first_part;
|
|
memcpy(&rb->buffer, &data[first_part], second_part);
|
|
}
|
|
}else{
|
|
// 这里不会有 write_idx == read_idx 的情况，这种情况，free_sp=0
|
|
// 前面已经计算过 len 就是最多可以存下的字节数，因此这里直接拷贝
|
|
memcpy(&rb->buffer[write_idx], data, len);
|
|
}
|
|
|
|
rb->write_idx = (write_idx + len) % size;
|
|
|
|
return len;
|
|
}
|
|
|
|
os_size_t fifo_read_fast(fifo_t* rb, uint8_t * data, os_size_t len){
|
|
if (rb == NULL || data == NULL || len == 0) {
|
|
return 0;
|
|
}
|
|
|
|
/* 如果读取长度超过有效数据长度，则只读取有效部分 */
|
|
os_size_t data_sz = fifo_count(rb);
|
|
if (len > data_sz) {
|
|
len = data_sz;
|
|
}
|
|
|
|
if (len == 0) {
|
|
return 0;
|
|
}
|
|
|
|
os_size_t read_idx = rb->read_idx;
|
|
os_size_t write_idx = rb->write_idx;
|
|
os_size_t size = rb->buffer_size;
|
|
|
|
// 分段读取
|
|
// buf_size: 8
|
|
// 0, 1, 2, 3, 4, 5, 6, 7
|
|
// r w
|
|
// w r
|
|
|
|
if(write_idx > read_idx){
|
|
memcpy(data, &rb->buffer[read_idx], len);
|
|
}else{
|
|
os_size_t first_part = size - read_idx;
|
|
if(first_part >= len){
|
|
// 一次就可以拷贝完成
|
|
memcpy(data, &rb->buffer[read_idx], first_part);
|
|
}else{
|
|
// 分成两个部分
|
|
memcpy(data, &rb->buffer[read_idx], first_part);
|
|
os_size_t second_part = len - first_part;
|
|
memcpy(&data[first_part], &rb->buffer, second_part);
|
|
}
|
|
}
|
|
|
|
rb->read_idx = (read_idx + len) % size;
|
|
|
|
return len;
|
|
}
|
|
|
|
os_err_t fifo_peek_at(fifo_t* self, os_size_t offset, uint8_t * data){
|
|
os_size_t data_sz = fifo_count(self);
|
|
if(offset >= data_sz){
|
|
return OS_ERR_FULL;
|
|
}
|
|
if(data){
|
|
os_size_t index = (self->read_idx + offset) % self->buffer_size;
|
|
*data = self->buffer[index];
|
|
}
|
|
return OS_ERR_OK;
|
|
}
|
|
|
|
os_size_t fifo_peek_bulk(fifo_t* rb, os_size_t offset, uint8_t* data, os_size_t len){
|
|
if (rb == NULL || data == NULL || len == 0) {
|
|
return 0;
|
|
}
|
|
|
|
/* 如果读取长度超过有效数据长度，则只读取有效部分 */
|
|
os_size_t data_sz = fifo_count(rb);
|
|
|
|
if(offset >= data_sz){
|
|
return 0;
|
|
}
|
|
|
|
if (len > (data_sz - offset)) {
|
|
len = data_sz - offset;
|
|
}
|
|
|
|
if (len == 0) {
|
|
return 0;
|
|
}
|
|
|
|
os_size_t size = rb->buffer_size;
|
|
os_size_t read_idx = (rb->read_idx + offset) % size;
|
|
os_size_t write_idx = rb->write_idx;
|
|
|
|
// 分段读取
|
|
// buf_size: 8
|
|
// 0, 1, 2, 3, 4, 5, 6, 7
|
|
// r w
|
|
// w r
|
|
|
|
if(write_idx > read_idx){
|
|
memcpy(data, &rb->buffer[read_idx], len);
|
|
}else{
|
|
os_size_t first_part = size - read_idx;
|
|
if(first_part >= len){
|
|
// 一次就可以拷贝完成
|
|
memcpy(data, &rb->buffer[read_idx], first_part);
|
|
}else{
|
|
// 分成两个部分
|
|
memcpy(data, &rb->buffer[read_idx], first_part);
|
|
os_size_t second_part = len - first_part;
|
|
memcpy(&data[first_part], &rb->buffer, second_part);
|
|
}
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
int fifo_memcmp(fifo_t *rb, os_size_t offset, const void *ptr, os_size_t len) {
|
|
os_size_t data_sz = fifo_count(rb);
|
|
OS_ASSERT(offset < data_sz);
|
|
OS_ASSERT(ptr);
|
|
|
|
if (len > (data_sz - offset)) {
|
|
// 如果请求比较的长度超过缓冲区现有数据量，可根据需求返回错误或只比较现有部分
|
|
// 这里假设调用者确保len <= fifo_count，或者我们只比较有效部分
|
|
len = data_sz - offset;
|
|
}
|
|
|
|
uint8_t rb_byte=0xff;
|
|
const uint8_t *external_ptr = (const uint8_t *)ptr;
|
|
for (os_size_t i = 0; i < len; i++) {
|
|
fifo_peek_at(rb, i + offset, &rb_byte);
|
|
if (rb_byte != external_ptr[i]) {
|
|
return (int)rb_byte - (int)external_ptr[i];
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* 在FIFO中查找特定模式
|
|
* @return: 找到则返回模式起始位置相对于head的偏移量，未找到返回-1
|
|
*/
|
|
os_size_t fifo_find(fifo_t *rb, os_size_t offset, const void *pattern, os_size_t pattern_len) {
|
|
if (!rb || !pattern || pattern_len == 0) {
|
|
return FIFO_INVALID_POS;
|
|
}
|
|
|
|
os_size_t data_size = fifo_count(rb);
|
|
if(offset > data_size){
|
|
return FIFO_INVALID_POS; // 开始查找位置错误了
|
|
}
|
|
|
|
if (pattern_len > (data_size - offset) ) {
|
|
return FIFO_INVALID_POS; // 模式长度超过缓冲区数据量，不可能找到
|
|
}
|
|
|
|
// 遍历缓冲区中所有可能的起始位置
|
|
// 最大起始偏移为 count - pattern_len
|
|
os_size_t max_offset = data_size - offset - pattern_len;
|
|
|
|
for (os_size_t i = 0; i <= max_offset; i++) {
|
|
// 构造一个临时的“视图”或使用辅助函数比较从offset开始的pattern_len个字节
|
|
// 由于fifo_memcmp是从head开始比较，我们需要一种方法从任意offset开始比较
|
|
|
|
// 方法：逐字节比较
|
|
os_bool_t match = OS_TRUE;
|
|
const uint8_t *pat = (const uint8_t *)pattern;
|
|
|
|
for (os_size_t j = 0; j < pattern_len; j++) {
|
|
uint8_t rb_byte;
|
|
if (fifo_peek_at(rb, i + offset + j, &rb_byte)!=OS_ERR_OK) {
|
|
match = OS_FALSE;
|
|
break;
|
|
}
|
|
if (rb_byte != pat[j]) {
|
|
match = OS_FALSE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (match) {
|
|
return i;
|
|
}
|
|
}
|
|
|
|
return FIFO_INVALID_POS;
|
|
}
|
|
|
|
unsigned long fifo_strtoul(fifo_t* self, os_size_t* endptr /*从哪里开始转换*/, int base) {
|
|
os_size_t idx = endptr?(*endptr):0;
|
|
unsigned long acc = 0;
|
|
uint8_t c = 0xff;
|
|
unsigned long cutoff;
|
|
int neg = 0, any, cutlim;
|
|
((void)neg);
|
|
|
|
fifo_peek_at(self, idx, &c);
|
|
// 1. 跳过前导空白字符
|
|
while (isspace((unsigned char)c)) {
|
|
idx++;
|
|
}
|
|
|
|
// 2. 处理可选的正负号 (strtoul 忽略负号，但会消耗它)
|
|
if (/* *s == '-'*/ fifo_offset_is(self, idx, '-')) {
|
|
neg = 1;
|
|
idx++;
|
|
} else if (/* *s == '+' */ fifo_offset_is(self, idx, '+')) {
|
|
idx++;
|
|
}
|
|
|
|
// 3. 确定基数
|
|
if (base == 0) {
|
|
if (/* *s == '0' */ fifo_offset_is(self, idx, '0')) {
|
|
if (/* *(s + 1) == 'x' || *(s + 1) == 'X' */
|
|
fifo_offset_is(self, idx+1, 'x')
|
|
|| fifo_offset_is(self, idx+1, 'X') )
|
|
{
|
|
base = 16;
|
|
idx += 2; // 跳过 0x
|
|
} else {
|
|
base = 8;
|
|
idx++; // 跳过 0
|
|
}
|
|
} else {
|
|
base = 10;
|
|
}
|
|
} else if (base == 16) {
|
|
if (/* *s == '0' && (*(s + 1) == 'x' || *(s + 1) == 'X') */
|
|
fifo_offset_is(self, idx, '0')
|
|
&& (fifo_offset_is(self, idx+1, 'x') || fifo_offset_is(self, idx+1, 'X')) )
|
|
{
|
|
idx += 2;
|
|
}
|
|
}
|
|
|
|
// 4. 预计算溢出检查边界
|
|
cutoff = ULONG_MAX / (unsigned long)base;
|
|
cutlim = ULONG_MAX % (unsigned long)base;
|
|
|
|
// 5. 转换数字
|
|
any = 0; // 标记是否已读取到有效数字
|
|
for (; ; idx++) {
|
|
// c = (unsigned char)*s;
|
|
fifo_peek_at(self, idx, &c);
|
|
|
|
// 将字符转换为数值
|
|
if (c >= '0' && c <= '9') {
|
|
c -= '0';
|
|
} else if (c >= 'A' && c <= 'Z') {
|
|
c -= 'A' - 10;
|
|
} else if (c >= 'a' && c <= 'z') {
|
|
c -= 'a' - 10;
|
|
} else {
|
|
break; // 非数字字符，停止解析
|
|
}
|
|
|
|
// 检查字符是否在当前基数范围内
|
|
if (c >= base) {
|
|
break;
|
|
}
|
|
|
|
// 检查溢出
|
|
if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim)) {
|
|
any = -1;
|
|
break;
|
|
} else {
|
|
any = 1;
|
|
acc *= base;
|
|
acc += c;
|
|
}
|
|
}
|
|
|
|
// 6. 处理结果和错误
|
|
if (any < 0) {
|
|
acc = ULONG_MAX;
|
|
// errno = ERANGE;
|
|
} else if (!any) {
|
|
// 没有进行任何转换
|
|
// if (endptr) {
|
|
// *endptr = (char *)nptr;
|
|
// }
|
|
return 0;
|
|
}
|
|
|
|
// 设置 endptr 指向第一个未转换的字符
|
|
if (endptr) {
|
|
*endptr = idx;
|
|
}
|
|
|
|
return acc;
|
|
}
|
|
|
|
// Returns the virtual index relative to tail where the match starts, or -1 if not found
|
|
os_size_t fifo_strstr(fifo_t *self, const char *search) {
|
|
os_size_t search_len = strlen(search);
|
|
os_size_t count = fifo_count(self);
|
|
if (search_len == 0 || count < search_len) return FIFO_INVALID_POS;
|
|
|
|
// Loop through every possible starting position in the ring buffer
|
|
for (os_size_t i = 0; i <= (count - search_len); i++) {
|
|
bool match = true;
|
|
|
|
// Check if the substring matches starting at virtual position i
|
|
for (os_size_t j = 0; j < search_len; j++) {
|
|
os_size_t rd_index = (self->read_idx + i + j) % self->buffer_size;
|
|
if (self->buffer[rd_index] != search[j]) {
|
|
match = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (match) {
|
|
return i; // Return the position relative to the current tail
|
|
}
|
|
}
|
|
return FIFO_INVALID_POS;
|
|
}
|
|
|