mirror of https://github.com/rusefi/bldc.git
178 lines
4.1 KiB
C
178 lines
4.1 KiB
C
/*
|
|
Copyright 2022 Benjamin Vedder benjamin@vedder.se
|
|
|
|
This file is part of the VESC firmware.
|
|
|
|
The VESC firmware is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
The VESC firmware is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "rb.h"
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
// Private functions (without locks)
|
|
static unsigned int get_item_count(rb_t *rb);
|
|
static unsigned int get_free_space(rb_t *rb);
|
|
static bool is_empty(rb_t *rb);
|
|
static bool pop(rb_t *rb, void *data);
|
|
static bool insert(rb_t *rb, const void *data);
|
|
|
|
void rb_init(rb_t *rb, void *buffer, int item_size, int item_count) {
|
|
rb->mutex = VESC_IF->mutex_create();
|
|
rb->data = buffer;
|
|
rb->item_size = item_size;
|
|
rb->item_count = item_count;
|
|
rb->head = 0;
|
|
rb->tail = 0;
|
|
rb->full = false;
|
|
}
|
|
|
|
void rb_init_alloc(rb_t *rb, int item_size, int item_count) {
|
|
void *buffer = VESC_IF->malloc(item_size * item_count);
|
|
rb_init(rb, buffer, item_size, item_count);
|
|
}
|
|
|
|
void rb_free(rb_t *rb) {
|
|
VESC_IF->free(rb->mutex);
|
|
VESC_IF->free(rb->data);
|
|
}
|
|
|
|
void rb_flush(rb_t *rb) {
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
rb->head = 0;
|
|
rb->tail = 0;
|
|
rb->full = false;
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
}
|
|
|
|
bool rb_insert(rb_t *rb, const void *data) {
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
bool res = insert(rb, data);
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return res;
|
|
}
|
|
|
|
unsigned int rb_insert_multi(rb_t *rb, const void *data, unsigned int count) {
|
|
unsigned int cnt = 0;
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
while (!rb->full && cnt < count) {
|
|
insert(rb, (char*)data + rb->item_size * cnt);
|
|
cnt++;
|
|
}
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return cnt;
|
|
}
|
|
|
|
bool rb_pop(rb_t *rb, void *data) {
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
bool res = pop(rb, data);
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return res;
|
|
}
|
|
|
|
unsigned int rb_pop_multi(rb_t *rb, void *data, unsigned int count) {
|
|
unsigned int cnt = 0;
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
while (!is_empty(rb) && cnt < count) {
|
|
if (data) {
|
|
pop(rb, (char*)data + rb->item_size * cnt);
|
|
} else {
|
|
pop(rb, 0);
|
|
}
|
|
cnt++;
|
|
}
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return cnt;
|
|
}
|
|
|
|
bool rb_is_full(rb_t *rb) {
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
bool res = rb->full;
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return res;
|
|
}
|
|
|
|
bool rb_is_empty(rb_t *rb) {
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
bool res = is_empty(rb);
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return res;
|
|
}
|
|
|
|
unsigned int rb_get_item_count(rb_t *rb) {
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
unsigned int res = get_item_count(rb);
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return res;
|
|
}
|
|
|
|
unsigned int rb_get_free_space(rb_t *rb) {
|
|
VESC_IF->mutex_lock(rb->mutex);
|
|
unsigned int res = get_free_space(rb);
|
|
VESC_IF->mutex_unlock(rb->mutex);
|
|
return res;
|
|
}
|
|
|
|
// Private function implementations
|
|
|
|
static unsigned int get_item_count(rb_t *rb) {
|
|
unsigned int res = rb->item_count;
|
|
|
|
if (!rb->full) {
|
|
if (rb->head >= rb->tail) {
|
|
res = rb->head - rb->tail;
|
|
} else {
|
|
res = rb->item_count - rb->tail + rb->head;
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static unsigned int get_free_space(rb_t *rb) {
|
|
return rb->item_count - get_item_count(rb);
|
|
}
|
|
|
|
static bool is_empty(rb_t *rb) {
|
|
return rb->head == rb->tail && !rb->full;
|
|
}
|
|
|
|
static bool pop(rb_t *rb, void *data) {
|
|
if (is_empty(rb)) {
|
|
return false;
|
|
}
|
|
|
|
// Null will just advance the tail and discard the data
|
|
if (data) {
|
|
memcpy(data, (char*)(rb->data) + rb->tail * rb->item_size, rb->item_size);
|
|
}
|
|
|
|
rb->tail = (rb->tail + 1) % rb->item_count;
|
|
rb->full = false;
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool insert(rb_t *rb, const void *data) {
|
|
if (rb->full) {
|
|
return false;
|
|
}
|
|
|
|
memcpy((char*)(rb->data) + rb->head * rb->item_size, data, rb->item_size);
|
|
rb->head = (rb->head + 1) % rb->item_count;
|
|
rb->full = rb->head == rb->tail;
|
|
|
|
return true;
|
|
}
|