From f49e5e2442da3634eea1a065ecd236c415f4d6b0 Mon Sep 17 00:00:00 2001
From: koalo <koalo@koalo.de>
Date: Wed, 21 Aug 2019 19:56:53 +0200
Subject: [PATCH] feat(bhi160): Add I2C locks for BHI160
---
epicardium/modules/bhi.c | 28 ++++++++++++++++++++++++++++
1 file changed, 28 insertions(+)
diff --git a/epicardium/modules/bhi.c b/epicardium/modules/bhi.c
index a0b9c0ed..6aa5f3fb 100644
--- a/epicardium/modules/bhi.c
+++ b/epicardium/modules/bhi.c
@@ -116,6 +116,11 @@ int epic_bhi160_enable_sensor(
return -ENODEV;
}
+ int lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
+ if (lockret < 0) {
+ return lockret;
+ }
+
if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) == pdTRUE) {
struct stream_info *stream = &bhi160_streams[sensor_type];
stream->item_size = bhi160_lookup_data_size(sensor_type);
@@ -125,6 +130,7 @@ int epic_bhi160_enable_sensor(
);
if (stream->queue == NULL) {
xSemaphoreGive(bhi160_mutex);
+ hwlock_release(HWLOCK_I2C);
return -ENOMEM;
}
@@ -141,9 +147,11 @@ int epic_bhi160_enable_sensor(
);
xSemaphoreGive(bhi160_mutex);
} else {
+ hwlock_release(HWLOCK_I2C);
return -EBUSY;
}
+ hwlock_release(HWLOCK_I2C);
return 0;
}
@@ -154,6 +162,11 @@ int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
return -ENODEV;
}
+ int ret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
+ if (ret < 0) {
+ return ret;
+ }
+
if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) == pdTRUE) {
struct stream_info *stream = &bhi160_streams[sensor_type];
stream_deregister(bhi160_lookup_sd(sensor_type), stream);
@@ -163,9 +176,11 @@ int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
xSemaphoreGive(bhi160_mutex);
} else {
+ hwlock_release(HWLOCK_I2C);
return -EBUSY;
}
+ hwlock_release(HWLOCK_I2C);
return 0;
}
/* }}} */
@@ -237,7 +252,13 @@ static int bhi160_fetch_fifo(void)
/* Number of bytes left in BHI160's FIFO buffer */
uint16_t bytes_left_in_fifo = 1;
+ int lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
+ if (lockret < 0) {
+ return lockret;
+ }
+
if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
+ hwlock_release(HWLOCK_I2C);
return -EBUSY;
}
@@ -283,6 +304,7 @@ static int bhi160_fetch_fifo(void)
}
xSemaphoreGive(bhi160_mutex);
+ hwlock_release(HWLOCK_I2C);
return 0;
}
@@ -310,6 +332,11 @@ void vBhi160Task(void *pvParameters)
bhi160_task_id = xTaskGetCurrentTaskHandle();
bhi160_mutex = xSemaphoreCreateMutexStatic(&bhi160_mutex_data);
+ int lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
+ if (lockret < 0) {
+ return;
+ }
+
/* Take Mutex during initialization, just in case */
if (xSemaphoreTake(bhi160_mutex, 0) != pdTRUE) {
LOG_CRIT("bhi160", "Failed to acquire BHI160 mutex!");
@@ -356,6 +383,7 @@ void vBhi160Task(void *pvParameters)
bhy_set_sic_matrix(bhi160_sic_array);
xSemaphoreGive(bhi160_mutex);
+ hwlock_release(HWLOCK_I2C);
/* ----------------------------------------- */
--
GitLab