diff --git a/epicardium/modules/bhi.c b/epicardium/modules/bhi.c index a0b9c0eda83029bd5f41b8811d2ffcad714e6991..6aa5f3fba1aac9e1929cbbe017d66e34ccf5f637 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); /* ----------------------------------------- */