Commit 713a76b2 authored by Rahix's avatar Rahix

chore(bhi160): Port to new mutex & hwlock APIs

Port the bhi160 mutex and hwlock usage in bhi.c to the new mutex API.
This will hopefully increase system stability.
Signed-off-by: Rahix's avatarRahix <rahix@rahix.de>
parent eed9ffda
......@@ -9,7 +9,6 @@
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"
#include "api/interrupt-sender.h"
......@@ -18,9 +17,6 @@
#include "modules/modules.h"
#include "modules/stream.h"
/* Ticks to wait when trying to acquire lock */
#define LOCK_WAIT pdMS_TO_TICKS(BHI160_MUTEX_WAIT_MS)
/* BHI160 Firmware Blob. Contents are defined in libcard10. */
extern uint8_t bhy1_fw[];
......@@ -57,8 +53,7 @@ static size_t start_index = 0;
static TaskHandle_t bhi160_task_id = NULL;
/* BHI160 Mutex */
static StaticSemaphore_t bhi160_mutex_data;
static SemaphoreHandle_t bhi160_mutex = NULL;
static struct mutex bhi160_mutex = { 0 };
/* Streams */
static struct stream_info bhi160_streams[10];
......@@ -135,15 +130,8 @@ int epic_bhi160_enable_sensor(
return -ENODEV;
}
result = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
if (result < 0) {
return result;
}
if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
result = -EBUSY;
goto out_free_i2c;
}
mutex_lock(&bhi160_mutex);
hwlock_acquire(HWLOCK_I2C);
struct stream_info *stream = &bhi160_streams[sensor_type];
stream->item_size = bhi160_lookup_data_size(sensor_type);
......@@ -152,12 +140,12 @@ int epic_bhi160_enable_sensor(
xQueueCreate(config->sample_buffer_len, stream->item_size);
if (stream->queue == NULL) {
result = -ENOMEM;
goto out_free_both;
goto out_free;
}
result = stream_register(bhi160_lookup_sd(sensor_type), stream);
if (result < 0) {
goto out_free_both;
goto out_free;
}
result = bhy_enable_virtual_sensor(
......@@ -170,16 +158,16 @@ int epic_bhi160_enable_sensor(
config->dynamic_range /* dynamic range is sensor dependent */
);
if (result != BHY_SUCCESS) {
goto out_free_both;
goto out_free;
}
bhi160_sensor_active[sensor_type] = true;
result = bhi160_lookup_sd(sensor_type);
/* Return the sensor stream descriptor */
result = bhi160_lookup_sd(sensor_type);
out_free_both:
xSemaphoreGive(bhi160_mutex);
out_free_i2c:
out_free:
hwlock_release(HWLOCK_I2C);
mutex_unlock(&bhi160_mutex);
return result;
}
......@@ -192,36 +180,28 @@ int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
return -ENODEV;
}
result = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
if (result < 0) {
return result;
}
if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
result = -EBUSY;
goto out_free_i2c;
}
mutex_lock(&bhi160_mutex);
hwlock_acquire(HWLOCK_I2C);
struct stream_info *stream = &bhi160_streams[sensor_type];
result = stream_deregister(bhi160_lookup_sd(sensor_type), stream);
if (result < 0) {
goto out_free_both;
goto out_free;
}
vQueueDelete(stream->queue);
stream->queue = NULL;
result = bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
if (result < 0) {
goto out_free_both;
goto out_free;
}
bhi160_sensor_active[sensor_type] = false;
result = 0;
out_free_both:
xSemaphoreGive(bhi160_mutex);
out_free_i2c:
out_free:
hwlock_release(HWLOCK_I2C);
mutex_unlock(&bhi160_mutex);
return result;
}
......@@ -347,15 +327,8 @@ static int bhi160_fetch_fifo(void)
/* Number of bytes left in BHI160's FIFO buffer */
uint16_t bytes_left_in_fifo = 1;
result = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
if (result < 0) {
return result;
}
if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
result = -EBUSY;
goto out_free_i2c;
}
mutex_lock(&bhi160_mutex);
hwlock_acquire(HWLOCK_I2C);
while (bytes_left_in_fifo) {
/* Fill local FIFO buffer with as many bytes as possible */
......@@ -398,9 +371,8 @@ static int bhi160_fetch_fifo(void)
start_index = bytes_left;
}
xSemaphoreGive(bhi160_mutex);
out_free_i2c:
hwlock_release(HWLOCK_I2C);
mutex_unlock(&bhi160_mutex);
return result;
}
......@@ -426,24 +398,10 @@ void vBhi160Task(void *pvParameters)
int ret;
bhi160_task_id = xTaskGetCurrentTaskHandle();
bhi160_mutex = xSemaphoreCreateMutexStatic(&bhi160_mutex_data);
/*
* Wait a little before initializing BHI160.
*/
vTaskDelay(pdMS_TO_TICKS(3));
int lockret = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
if (lockret < 0) {
LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
vTaskDelay(portMAX_DELAY);
}
/* Take Mutex during initialization, just in case */
if (xSemaphoreTake(bhi160_mutex, 0) != pdTRUE) {
LOG_CRIT("bhi160", "Failed to acquire BHI160 mutex!");
vTaskDelay(portMAX_DELAY);
}
mutex_create(&bhi160_mutex);
mutex_lock(&bhi160_mutex);
hwlock_acquire(HWLOCK_I2C);
memset(bhi160_streams, 0x00, sizeof(bhi160_streams));
......@@ -469,11 +427,7 @@ void vBhi160Task(void *pvParameters)
/* Wait for first interrupt */
hwlock_release(HWLOCK_I2C);
ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
lockret = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
if (lockret < 0) {
LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
vTaskDelay(portMAX_DELAY);
}
hwlock_acquire(HWLOCK_I2C);
/* Remap axes to match card10 layout */
/* Due to a known issue (#133) the first call to
......@@ -494,8 +448,8 @@ void vBhi160Task(void *pvParameters)
/* Set "SIC" matrix. TODO: Find out what this is about */
bhy_set_sic_matrix(bhi160_sic_array);
xSemaphoreGive(bhi160_mutex);
hwlock_release(HWLOCK_I2C);
mutex_unlock(&bhi160_mutex);
/* ----------------------------------------- */
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment