epicardium/ble/uart.c

+#include "uart.h"
+#include "cccd.h"
+
 #include "modules/modules.h"
+#include "drivers/drivers.h"
 
 #include "wsf_types.h"
 #include "util/bstream.h"
 #include "att_api.h"
+#include "dm_api.h"
+#include "app_api.h"
 
 #include "FreeRTOS.h"
 #include "timers.h"
@@ -11,24 +17,10 @@
 #include <string.h>
 #include <stdbool.h>
 
-#define UART_START_HDL 0x800            /*!< \brief Service start handle. */
-#define UART_END_HDL (UART_MAX_HDL - 1) /*!< \brief Service end handle. */
-
 /**************************************************************************************************
  Handles
 **************************************************************************************************/
 
-/*! \brief UART Service Handles */
-enum { UART_SVC_HDL = UART_START_HDL, /*!< \brief UART service declaration */
-       UART_RX_CH_HDL,                /*!< \brief UART rx characteristic */
-       UART_RX_HDL,                   /*!< \brief UART rx value */
-       UART_TX_CH_HDL,                /*!< \brief UART tx characteristic */
-       UART_TX_HDL,                   /*!< \brief UART tx value */
-       UART_TX_CH_CCC_HDL,            /*!< \brief UART tx CCCD */
-       UART_MAX_HDL                   /*!< \brief Maximum handle. */
-};
-/**@}*/
-
 /* clang-format off */
 static const uint8_t UARTSvc[] = {0x9E,0xCA,0xDC,0x24,0x0E,0xE5,0xA9,0xE0,0x93,0xF3,0xA3,0xB5,0x01,0x00,0x40,0x6E};
 static const uint16_t UARTSvc_len = sizeof(UARTSvc);
@@ -41,7 +33,10 @@ static const uint8_t uartTxCh[] = {ATT_PROP_READ | ATT_PROP_NOTIFY, UINT16_TO_BY
 static const uint16_t uartTxCh_len = sizeof(uartTxCh);
 static const uint8_t attUartTxChUuid[] = {0x9E,0xCA,0xDC,0x24,0x0E,0xE5, 0xA9,0xE0,0x93,0xF3,0xA3,0xB5,0x03,0x00,0x40,0x6E};
 
-static uint8_t ble_uart_tx_buf[128];
+static uint8_t uartValTxChCcc[] = {UINT16_TO_BYTES(0x0000)};
+static const uint16_t uartLenTxChCcc = sizeof(uartValTxChCcc);
+
+static uint8_t ble_uart_tx_buf[20];
 static uint16_t ble_uart_buf_tx_fill = 0;
 /* clang-format on */
 
@@ -97,17 +92,16 @@ static const attsAttr_t uartAttrCfgList[] = {
 	/* UART tx CCC descriptor */
 	{
 		.pUuid    = attCliChCfgUuid,
-		.pValue      = NULL,
-		.pLen        = NULL,
-		.maxLen      = 0,
+		.pValue   = uartValTxChCcc,
+		.pLen     = (uint16_t *)&uartLenTxChCcc,
+		.maxLen   = sizeof(uartValTxChCcc),
 		.settings = ATTS_SET_CCC,
-		.permissions = ATTS_PERMIT_WRITE | ATTS_PERMIT_WRITE_ENC |
-			       ATTS_PERMIT_WRITE_AUTH | ATTS_PERMIT_READ |
-			       ATTS_PERMIT_READ_ENC | ATTS_PERMIT_READ_AUTH,
+		.permissions =
+			(ATTS_PERMIT_READ |
+			 ATTS_PERMIT_WRITE) // How about security?
 	},
-};
 
-dmConnId_t active_connection = 0;
+};
 
 static uint8_t UARTWriteCback(
 	dmConnId_t connId,
@@ -118,62 +112,80 @@ static uint8_t UARTWriteCback(
 	uint8_t *pValue,
 	attsAttr_t *pAttr
 ) {
-	active_connection = connId;
+	static bool was_r = false;
 
-	//printf("UARTWriteCback %d: ", len);
 	int i;
 	for (i = 0; i < len; i++) {
-		//printf("%c", pValue[i]);
+		if (pValue[i] == '\n' && !was_r) {
+			serial_enqueue_char('\r');
+		}
+		was_r = pValue[i] == '\r';
 		serial_enqueue_char(pValue[i]);
 	}
-	serial_enqueue_char('\r');
-	//printf("\n");
-
-#if 0
-  AttsSetAttr(UART_TX_HDL, len, pValue);
-  AttsHandleValueNtf(connId, UART_TX_HDL, len, pValue);
-#endif
 
 	return ATT_SUCCESS;
 }
 
-static int ble_uart_lasttick = 0;
+static bool done;
+static bool again;
 
-void ble_uart_write(uint8_t *pValue, uint8_t len)
+static void ble_uart_flush(void)
 {
-	for (int i = 0; i < len; i++) {
-		if (pValue[i] >= 0x20 && pValue[i] < 0x7f) {
-			ble_uart_tx_buf[ble_uart_buf_tx_fill] = pValue[i];
-			ble_uart_buf_tx_fill++;
+	if (ble_uart_buf_tx_fill == 0) {
+		return;
 	}
 
-		if (ble_uart_buf_tx_fill == 128 || pValue[i] == '\r' ||
-		    pValue[i] == '\n') {
-			if (ble_uart_buf_tx_fill > 0) {
-				if (active_connection) {
-					int x = xTaskGetTickCount() -
-						ble_uart_lasttick;
-					if (x < 100) {
-						/*
-						 * TODO: Ugly hack if we already
-						 *     send something recently.
-						 *     Figure out how fast we
-						 *     can send or use indications.
-						 */
-						vTaskDelay(100 - x);
-					}
+	dmConnId_t connId = AppConnIsOpen();
+	if (connId != DM_CONN_ID_NONE) {
+		if (AttsCccEnabled(connId, UART_TX_CH_CCC_IDX)) {
+			done   = false;
+			again  = true;
+			int t0 = xTaskGetTickCount();
+
+			while (!done && ((xTaskGetTickCount() - t0) < 1000)) {
+				if (again) {
+					again = false;
 					AttsHandleValueNtf(
-						active_connection,
+						connId,
 						UART_TX_HDL,
 						ble_uart_buf_tx_fill,
 						ble_uart_tx_buf
 					);
-					ble_uart_lasttick = xTaskGetTickCount();
+				}
+				/* This function is supposed to only be called
+				 * from the API scheduler with lowest priority.
+				 *
+				 * If that is not the case anymore, use a delay
+				 * instead of the yield. Ideally refactor to avoid
+				 * the delay.
+				 */
+				//vTaskDelay(5);
+				taskYIELD();
+			}
+		}
 	}
 	ble_uart_buf_tx_fill = 0;
 }
+
+static void ble_uart_write_char(uint8_t c)
+{
+	ble_uart_tx_buf[ble_uart_buf_tx_fill] = c;
+	ble_uart_buf_tx_fill++;
+
+	// TODO: increase buffer if configured MTU allows it
+	if (ble_uart_buf_tx_fill == sizeof(ble_uart_tx_buf)) {
+		ble_uart_flush();
 	}
 }
+
+void ble_uart_write(uint8_t *pValue, uint8_t len)
+{
+	for (int i = 0; i < len; i++) {
+		ble_uart_write_char(pValue[i]);
+	}
+
+	// TODO schedule timer in a few ms to flush the buffer
+	ble_uart_flush();
 }
 
 static attsGroup_t uartCfgGroup = {
@@ -188,3 +200,19 @@ void bleuart_init(void)
 	/* Add the UART service */
 	AttsAddGroup(&uartCfgGroup);
 }
+
+void UartProcMsg(bleMsg_t *pMsg)
+{
+	if (pMsg->hdr.event == ATTS_HANDLE_VALUE_CNF) {
+		if (pMsg->att.handle == UART_TX_HDL) {
+			if (pMsg->hdr.status == ATT_SUCCESS) {
+				done = true;
+			} else if (pMsg->hdr.status == ATT_ERR_OVERFLOW) {
+				again = true;
+			}
+		}
+	}
+	if (pMsg->hdr.event == DM_CONN_OPEN_IND) {
+		ble_uart_buf_tx_fill = 0;
+	}
+}

epicardium/ble/uart.h

+#pragma once
+
+#include "ble_api.h"
+
+#define UART_START_HDL 0x800            /*!< \brief Service start handle. */
+#define UART_END_HDL (UART_MAX_HDL - 1) /*!< \brief Service end handle. */
+
+/*! \brief UART Service Handles */
+enum { UART_SVC_HDL = UART_START_HDL, /*!< \brief UART service declaration */
+       UART_RX_CH_HDL,                /*!< \brief UART rx characteristic */
+       UART_RX_HDL,                   /*!< \brief UART rx value */
+       UART_TX_CH_HDL,                /*!< \brief UART tx characteristic */
+       UART_TX_HDL,                   /*!< \brief UART tx value */
+       UART_TX_CH_CCC_HDL,            /*!< \brief UART tx CCCD */
+       UART_MAX_HDL                   /*!< \brief Maximum handle. */
+};
+
+void UartProcMsg(bleMsg_t *pMsg);
+

epicardium/modules/bhi.c → epicardium/drivers/bhi.c

@@ -9,17 +9,14 @@
 
 #include "FreeRTOS.h"
 #include "task.h"
-#include "semphr.h"
 #include "queue.h"
 
-#include "api/interrupt-sender.h"
 #include "epicardium.h"
-#include "modules/log.h"
+#include "os/core.h"
 #include "modules/modules.h"
+#include "drivers/drivers.h"
 #include "modules/stream.h"
-
-/* Ticks to wait when trying to acquire lock */
-#define LOCK_WAIT pdMS_TO_TICKS(BHI160_MUTEX_WAIT_MS)
+#include "user_core/interrupts.h"
 
 /* BHI160 Firmware Blob.  Contents are defined in libcard10. */
 extern uint8_t bhy1_fw[];
@@ -29,9 +26,18 @@ static const gpio_cfg_t bhi160_interrupt_pin = {
 	PORT_0, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP
 };
 
+/* clang-format off */
 /* Axis remapping matrices */
-static int8_t bhi160_mapping_matrix[3 * 3] = { 0, -1, 0, 1, 0, 0, 0, 0, 1 };
-static int8_t bmm150_mapping_matrix[3 * 3] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 };
+static const int8_t bhi160_mapping_matrix[3 * 3] = {
+	 0, -1,  0,
+	 1,  0,  0,
+	 0,  0,  1,
+};
+static const int8_t bmm150_mapping_matrix[3 * 3] = {
+	-1,  0,  0,
+	 0,  1,  0,
+	 0,  0, -1,
+};
 
 /*
  * From the official docs:
@@ -43,22 +49,18 @@ static int8_t bmm150_mapping_matrix[3 * 3] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 };
  *
  * TODO: Get data for card10
  */
-/* clang-format off */
-static float bhi160_sic_array[3 * 3] = { 1.0, 0.0, 0.0,
-                                         0.0, 1.0, 0.0,
-                                         0.0, 0.0, 1.0 };
+static const float bhi160_sic_array[3 * 3] = {
+	1.0f, 0.0f, 0.0f,
+	0.0f, 1.0f, 0.0f,
+	0.0f, 0.0f, 1.0f,
+};
 /* clang-format on */
 
-/* BHI160 Fifo */
-static uint8_t bhi160_fifo[BHI160_FIFO_SIZE];
-static size_t start_index = 0;
-
 /* BHI160 Task ID */
 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];
@@ -66,6 +68,13 @@ static struct stream_info bhi160_streams[10];
 /* Active */
 static bool bhi160_sensor_active[10] = { 0 };
 
+/*
+ * Driver State:  A flag that is set when an unrecoverable error occurred.
+ * Effectively, this means the sensor will be disabled until next reboot and any
+ * API calls will fail immediately.
+ */
+static bool bhi160_driver_b0rked = false;
+
 /* -- Utilities -------------------------------------------------------- {{{ */
 /*
  * Retrieve the data size for a sensor.  This value is needed for the creation
@@ -131,18 +140,16 @@ int epic_bhi160_enable_sensor(
 	int result = 0;
 
 	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
-	if (vs_id < 0) {
+	if (vs_id == (bhy_virtual_sensor_t)-1) {
 		return -ENODEV;
 	}
 
-	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
-	if (result < 0) {
-		return result;
-	}
+	mutex_lock(&bhi160_mutex);
+	hwlock_acquire(HWLOCK_I2C);
 
-	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
-		result = -EBUSY;
-		goto out_free_i2c;
+	if (bhi160_driver_b0rked) {
+		result = -ENODEV;
+		goto out_free;
 	}
 
 	struct stream_info *stream = &bhi160_streams[sensor_type];
@@ -152,12 +159,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 +177,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;
+	/* 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;
 }
 
@@ -188,46 +195,43 @@ int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
 	int result = 0;
 
 	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
-	if (vs_id < 0) {
+	if (vs_id == (bhy_virtual_sensor_t)-1) {
 		return -ENODEV;
 	}
 
-	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
-	if (result < 0) {
-		return result;
-	}
+	mutex_lock(&bhi160_mutex);
+	hwlock_acquire(HWLOCK_I2C);
 
-	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
-		result = -EBUSY;
-		goto out_free_i2c;
+	if (bhi160_driver_b0rked) {
+		result = -ENODEV;
+		goto out_free;
 	}
 
 	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;
 }
 
 void epic_bhi160_disable_all_sensors()
 {
-	for (int i = 0; i < sizeof(bhi160_sensor_active); i++) {
+	for (size_t i = 0; i < sizeof(bhi160_sensor_active); i++) {
 		if (bhi160_sensor_active[i]) {
 			epic_bhi160_disable_sensor(i);
 		}
@@ -319,11 +323,20 @@ bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
 			    bhi160_streams[sensor_type].queue,
 			    &data_vector,
 			    0) != pdTRUE) {
-			LOG_WARN("bhi160", "queue full for %d", sensor_type);
+			if (!bhi160_streams[sensor_type].was_full) {
+				LOG_WARN(
+					"bhi160",
+					"queue full for %d",
+					sensor_type
+				);
+			}
+			bhi160_streams[sensor_type].was_full = true;
+		} else {
+			bhi160_streams[sensor_type].was_full = false;
 		}
 
 		if (wakeup) {
-			api_interrupt_trigger(epic_int);
+			interrupt_trigger(epic_int);
 		}
 		break;
 	default:
@@ -335,7 +348,7 @@ bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
  * Fetch all data available from BHI160's FIFO buffer and handle all packets
  * contained in it.
  */
-static int bhi160_fetch_fifo(void)
+static void bhi160_fetch_fifo(void)
 {
 	/*
 	 * Warning:  The code from the BHy1 docs has some issues.  This
@@ -343,30 +356,51 @@ static int bhi160_fetch_fifo(void)
 	 * You'll probably be best of leaving it as it is ...
 	 */
 
-	int result = 0;
+	/*
+	 * FIFO buffer.  Access to this static variable is safe because this
+	 * function is guarded by the bhi160_mutex.
+	 */
+	static uint8_t bhi160_fifo[BHI160_FIFO_SIZE];
+	static size_t start_index = 0;
+
+	BHY_RETURN_FUNCTION_TYPE result = 0;
 	/* Number of bytes left in BHI160's FIFO buffer */
 	uint16_t bytes_left_in_fifo = 1;
 
-	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
-	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 */
 		uint16_t bytes_read;
-		bhy_read_fifo(
+		result = bhy_read_fifo(
 			&bhi160_fifo[start_index],
 			BHI160_FIFO_SIZE - start_index,
 			&bytes_read,
 			&bytes_left_in_fifo
 		);
 
+		if (result != BHY_SUCCESS) {
+			/*
+			 * Honestly not sure how we should handle these errors.
+			 *
+			 * Needs more digging to find out how bhy_read_fifo()
+			 * behaves in error situations (a quick glance at the
+			 * code shows that the function might have written
+			 * _somethings_ into our buffer so we can't just retry
+			 * blindly ...)
+			 *
+			 * For now, just abort everything and disable the
+			 * sensor.  Won't cause havoc at least ...
+			 */
+			LOG_ERR("bhi160",
+				"Error while reading fifo: %d.  Disabling.",
+				result);
+
+			bhi160_driver_b0rked = true;
+			break;
+		}
+
 		/* Add the bytes left from the last transfer on top */
 		bytes_read += start_index;
 
@@ -386,6 +420,11 @@ static int bhi160_fetch_fifo(void)
 			if (result == BHY_SUCCESS) {
 				bhi160_handle_packet(data_type, &sensor_data);
 			} else {
+				LOG_WARN(
+					"bhi160",
+					"Error in fifo packet: %d.  Ignoring.",
+					result
+				);
 				break;
 			}
 		}
@@ -398,10 +437,8 @@ static int bhi160_fetch_fifo(void)
 		start_index = bytes_left;
 	}
 
-	xSemaphoreGive(bhi160_mutex);
-out_free_i2c:
 	hwlock_release(HWLOCK_I2C);
-	return result;
+	mutex_unlock(&bhi160_mutex);
 }
 
 /*
@@ -412,7 +449,7 @@ static void bhi160_interrupt_callback(void *_)
 {
 	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
 
-	if (bhi160_task_id != NULL) {
+	if (bhi160_task_id != NULL && !bhi160_driver_b0rked) {
 		vTaskNotifyGiveFromISR(
 			bhi160_task_id, &xHigherPriorityTaskWoken
 		);
@@ -426,33 +463,27 @@ 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(HWLOCK_I2C, pdMS_TO_TICKS(100));
-	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));
 
-	/* Install interrupt callback */
+	/*
+	 * The BHI160, coming out of power-on-reset will hold its interrupt line
+	 * high until a firmware image is loaded.  Once that firmware is loaded
+	 * and running, the interrupt line is deasserted and from then on,
+	 * interrupts are signaled using a rising edge.
+	 *
+	 * So, initially we need to configure the IRQ for a falling edge, load
+	 * the firmware and then reconfigure for a rising edge.
+	 */
 	GPIO_Config(&bhi160_interrupt_pin);
 	GPIO_RegisterCallback(
 		&bhi160_interrupt_pin, bhi160_interrupt_callback, NULL
 	);
-	GPIO_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_RISING);
+	GPIO_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_FALLING);
 	GPIO_IntEnable(&bhi160_interrupt_pin);
 	NVIC_SetPriority(
 		(IRQn_Type)MXC_GPIO_GET_IRQ(bhi160_interrupt_pin.port), 2
@@ -462,51 +493,66 @@ void vBhi160Task(void *pvParameters)
 	/* Upload firmware */
 	ret = bhy_driver_init(bhy1_fw);
 	if (ret) {
-		LOG_CRIT("bhi160", "BHy1 init failed!");
-		vTaskDelay(portMAX_DELAY);
+		LOG_CRIT("bhi160", "BHy1 init failed!  Disabling.");
+
+		/* Disable BHI160 until next reboot */
+		bhi160_driver_b0rked = true;
+		hwlock_release(HWLOCK_I2C);
+		mutex_unlock(&bhi160_mutex);
+		vTaskDelete(NULL);
 	}
 
-	/* Wait for first interrupt */
+	/* Wait for first interrupt, a falling edge */
 	hwlock_release(HWLOCK_I2C);
-	ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
-	lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
-	if (lockret < 0) {
-		LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
-		vTaskDelay(portMAX_DELAY);
+	if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(2000)) == 0) {
+		LOG_CRIT(
+			"bhi160",
+			"Sensor firmware was not loaded correctly.  Disabling."
+		);
+
+		/* Disable BHI160 until next reboot */
+		bhi160_driver_b0rked = true;
+		mutex_unlock(&bhi160_mutex);
+		vTaskDelete(NULL);
 	}
+	hwlock_acquire(HWLOCK_I2C);
 
-	/* Remap axes to match card10 layout */
-	/* Due to a known issue (#133) the first call to
-	 * bhy_mapping_matrix_set might fail. */
+	/*
+	 * The firmware is now loaded; as stated above, we now need to
+	 * reconfigure the IRQ for a rising edge.
+	 */
+	GPIO_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_RISING);
+
+	/*
+	 * Remap axes to match card10 layout.
+	 *
+	 * TODO: We set the matrix for the accelerometer twice because on some
+	 * badges, the axis mapping is not applied properly the first time.  We
+	 * should fix this properly at some point.
+	 */
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_ACC, (int8_t *)bhi160_mapping_matrix
 	);
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_ACC, (int8_t *)bhi160_mapping_matrix
 	);
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_MAG, bmm150_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_MAG, (int8_t *)bmm150_mapping_matrix
 	);
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_GYRO, bhi160_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_GYRO, (int8_t *)bhi160_mapping_matrix
 	);
 
 	/* Set "SIC" matrix.  TODO: Find out what this is about */
-	bhy_set_sic_matrix(bhi160_sic_array);
+	bhy_set_sic_matrix((float *)bhi160_sic_array);
 
-	xSemaphoreGive(bhi160_mutex);
 	hwlock_release(HWLOCK_I2C);
+	mutex_unlock(&bhi160_mutex);
 
 	/* ----------------------------------------- */
 
 	while (1) {
-		int ret = bhi160_fetch_fifo();
-		if (ret == -EBUSY) {
-			LOG_WARN("bhi160", "Could not acquire mutex for FIFO?");
-			continue;
-		} else if (ret < 0) {
-			LOG_ERR("bhi160", "Unknown error: %d", -ret);
-		}
+		bhi160_fetch_fifo();
 
 		/*
 		 * Wait for interrupt.  After two seconds, fetch FIFO anyway in

epicardium/drivers/bme680.c

+#include "epicardium.h"
+#include "modules/modules.h"
+#include "os/core.h"
+#include "drivers/drivers.h"
+
+#include "card10.h"
+
+#include "bme680.h"
+#include "bosch.h"
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdio.h>
+
+#define HEATR_TEMP 320
+#define HEATR_DUR 150
+
+static bool initialized;
+static struct bme680_dev bme;
+
+static int convert_error(int8_t error)
+{
+	switch (error) {
+	case BME680_OK:
+		return 0;
+	case BME680_E_NULL_PTR:
+		return EFAULT;
+	case BME680_E_COM_FAIL:
+		return EIO;
+	case BME680_E_DEV_NOT_FOUND:
+		return ENODEV;
+	case BME680_E_INVALID_LENGTH:
+		return EINVAL;
+	default:
+		return 1;
+	}
+}
+
+static int8_t
+i2c_write(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16_t size)
+{
+	int8_t ret;
+	hwlock_acquire(HWLOCK_I2C);
+	ret = card10_bosch_i2c_write(addr, reg, p_buf, size);
+	hwlock_release(HWLOCK_I2C);
+	return ret;
+}
+
+static int8_t i2c_read(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16_t size)
+{
+	int8_t ret;
+	hwlock_acquire(HWLOCK_I2C);
+	ret = card10_bosch_i2c_read(addr, reg, p_buf, size);
+	hwlock_release(HWLOCK_I2C);
+	return ret;
+}
+
+static void delay(uint32_t msec)
+{
+	if (xTaskGetSchedulerState() == taskSCHEDULER_NOT_STARTED) {
+		card10_bosch_delay(msec);
+	} else {
+		vTaskDelay(pdMS_TO_TICKS(msec));
+	}
+}
+
+int epic_bme680_init()
+{
+	int8_t result = BME680_OK;
+
+	if (bsec_active()) {
+		/* If the proprietary Bosch BSEC libary is in use
+		 * we redirect calls to that. It always runs
+		 * in the background
+		 */
+		return 0;
+	}
+	if (initialized) {
+		return 0;
+	}
+
+	bme.dev_id   = BME680_I2C_ADDR_PRIMARY;
+	bme.intf     = BME680_I2C_INTF;
+	bme.read     = i2c_read;
+	bme.write    = i2c_write;
+	bme.delay_ms = delay;
+
+	/*
+	 * amb_temp can be set to 25 prior to configuring the gas sensor
+	 * or by performing a few temperature readings without operating
+	 * the gas sensor.
+	 */
+	bme.amb_temp = 25;
+
+	result = bme680_init(&bme);
+	if (result != BME680_OK) {
+		LOG_ERR("bme680", "bme680_init error: %d\n", result);
+		return -convert_error(result);
+	}
+
+	/*
+	 * Select the power mode.  Must be set before writing the sensor
+	 * configuration
+	 */
+	bme.power_mode = BME680_FORCED_MODE;
+
+	/* Set the temperature, pressure and humidity settings */
+	bme.tph_sett.os_hum  = BME680_OS_2X;
+	bme.tph_sett.os_pres = BME680_OS_4X;
+	bme.tph_sett.os_temp = BME680_OS_8X;
+	bme.tph_sett.filter  = BME680_FILTER_SIZE_0;
+
+	/* Set the remaining gas sensor settings and link the heating profile */
+	bme.gas_sett.run_gas = BME680_ENABLE_GAS_MEAS;
+	/* Create a ramp heat waveform in 3 steps */
+	bme.gas_sett.heatr_temp = HEATR_TEMP; /* degree Celsius */
+	bme.gas_sett.heatr_dur  = HEATR_DUR;  /* milliseconds */
+
+	/* Set the required sensor settings needed */
+	uint16_t settings_sel = BME680_OST_SEL | BME680_OSP_SEL |
+				BME680_OSH_SEL | BME680_FILTER_SEL |
+				BME680_GAS_SENSOR_SEL;
+
+	result = bme680_set_sensor_settings(settings_sel, &bme);
+	if (result != BME680_OK) {
+		LOG_ERR("bme680",
+			"bme680_set_sensor_settings error: %d\n",
+			result);
+		return -convert_error(result);
+	}
+
+	initialized = true;
+	return 0;
+}
+
+int epic_bme680_deinit()
+{
+	/* This is an intentional NO OP to keep the BME680 always initialized.
+	 *
+	 * If it configured to foreced mode, there is no energy consumption
+	 * penalty.
+	 */
+
+	if (bsec_active()) {
+		return 0;
+	}
+
+#if 0
+	if (!initialized) {
+		return 0;
+	}
+
+	int8_t result = bme680_soft_reset(&bme);
+	if (result != BME680_OK) {
+		LOG_ERR("bme680", "bme680_soft_reset error: %d\n", result);
+	}
+
+	initialized = false;
+#endif
+	return 0;
+}
+
+int epic_bme680_read_sensors(struct bme680_sensor_data *data)
+{
+	int8_t result = BME680_OK;
+
+	if (bsec_active()) {
+		return bsec_read_bme680(data);
+	}
+
+	if (!initialized) {
+		LOG_ERR("bme680", "bme680 sensor not initialized");
+		return -EINVAL;
+	}
+
+	if (data == NULL) {
+		return -EFAULT;
+	}
+
+	uint16_t profile_dur = 0;
+	bme680_get_profile_dur(&profile_dur, &bme);
+
+	result = bme680_set_sensor_mode(&bme); /* Trigger a measurement */
+	if (result != BME680_OK) {
+		LOG_ERR("bme680", "bme680_set_sensor_mode error: %d\n", result);
+		return -convert_error(result);
+	}
+
+	/*
+	 * Wait for the measurement to complete.  Release the I2C lock in the
+	 * meantime.
+	 */
+	vTaskDelay(pdMS_TO_TICKS(profile_dur));
+
+	struct bme680_field_data raw_data;
+	result = bme680_get_sensor_data(&raw_data, &bme);
+	if (result != BME680_OK) {
+		LOG_ERR("bme680", "bme680_get_sensor_data error: %d\n", result);
+		return -convert_error(result);
+	}
+
+	data->temperature    = (float)raw_data.temperature / 100.0f;
+	data->humidity       = raw_data.humidity / 1000.0f;
+	data->pressure       = raw_data.pressure / 100.0f;
+	data->gas_resistance = raw_data.gas_resistance;
+
+	return 0;
+}

epicardium/drivers/bsec.c

+/* Adapted from  bsec_iot_example.c and bsec_iot_ulp_plus_example.c */
+
+#include "card10.h"
+#include "bosch.h"
+#include "bsec_integration.h"
+
+#include "ble/ess.h"
+
+#include "epicardium.h"
+#include "modules/modules.h"
+#include "os/config.h"
+#include "os/core.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "max32665.h"
+#include "gcr_regs.h"
+
+#include <string.h>
+#include <stdio.h>
+
+TaskHandle_t bsec_task_id;
+static int64_t last_bme680_timestamp;
+static bool bsec_task_active;
+static bool debug;
+static struct bsec_sensor_data last_bsec_data;
+#define ULP 0
+
+// From generic_18v_3s_4d/bsec_serialized_configurations_iaq.c
+static const uint8_t bsec_config_generic_18v_3s_4d[454] = {
+	0,   8,   4,   1,   61,  0,   0,   0,   0,   0,   0,   0,   174, 1,
+	0,   0,   48,  0,   1,   0,   0,   192, 168, 71,  64,  49,  119, 76,
+	0,   0,   225, 68,  137, 65,  0,   191, 205, 204, 204, 190, 0,   0,
+	64,  191, 225, 122, 148, 190, 0,   0,   0,   0,   216, 85,  0,   100,
+	0,   0,   0,   0,   0,   0,   0,   0,   28,  0,   2,   0,   0,   244,
+	1,   225, 0,   25,  0,   0,   128, 64,  0,   0,   32,  65,  144, 1,
+	0,   0,   112, 65,  0,   0,   0,   63,  16,  0,   3,   0,   10,  215,
+	163, 60,  10,  215, 35,  59,  10,  215, 35,  59,  9,   0,   5,   0,
+	0,   0,   0,   0,   1,   88,  0,   9,   0,   7,   240, 150, 61,  0,
+	0,   0,   0,   0,   0,   0,   0,   28,  124, 225, 61,  52,  128, 215,
+	63,  0,   0,   160, 64,  0,   0,   0,   0,   0,   0,   0,   0,   205,
+	204, 12,  62,  103, 213, 39,  62,  230, 63,  76,  192, 0,   0,   0,
+	0,   0,   0,   0,   0,   145, 237, 60,  191, 251, 58,  64,  63,  177,
+	80,  131, 64,  0,   0,   0,   0,   0,   0,   0,   0,   93,  254, 227,
+	62,  54,  60,  133, 191, 0,   0,   64,  64,  12,  0,   10,  0,   0,
+	0,   0,   0,   0,   0,   0,   0,   229, 0,   254, 0,   2,   1,   5,
+	48,  117, 100, 0,   44,  1,   112, 23,  151, 7,   132, 3,   197, 0,
+	92,  4,   144, 1,   64,  1,   64,  1,   144, 1,   48,  117, 48,  117,
+	48,  117, 48,  117, 100, 0,   100, 0,   100, 0,   48,  117, 48,  117,
+	48,  117, 100, 0,   100, 0,   48,  117, 48,  117, 100, 0,   100, 0,
+	100, 0,   100, 0,   48,  117, 48,  117, 48,  117, 100, 0,   100, 0,
+	100, 0,   48,  117, 48,  117, 100, 0,   100, 0,   44,  1,   44,  1,
+	44,  1,   44,  1,   44,  1,   44,  1,   44,  1,   44,  1,   44,  1,
+	44,  1,   44,  1,   44,  1,   44,  1,   44,  1,   8,   7,   8,   7,
+	8,   7,   8,   7,   8,   7,   8,   7,   8,   7,   8,   7,   8,   7,
+	8,   7,   8,   7,   8,   7,   8,   7,   8,   7,   112, 23,  112, 23,
+	112, 23,  112, 23,  112, 23,  112, 23,  112, 23,  112, 23,  112, 23,
+	112, 23,  112, 23,  112, 23,  112, 23,  112, 23,  255, 255, 255, 255,
+	255, 255, 255, 255, 220, 5,   220, 5,   220, 5,   255, 255, 255, 255,
+	255, 255, 220, 5,   220, 5,   255, 255, 255, 255, 255, 255, 255, 255,
+	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 44,  1,   0,   0,
+	0,   0,   83,  141, 0,   0
+};
+
+/*!
+ * @brief           Capture the system time in microseconds
+ *
+ * @return          system_current_time    current system timestamp in microseconds
+ */
+static int64_t get_timestamp_us()
+{
+	int64_t tick = xTaskGetTickCount();
+	return tick * 1000;
+}
+
+/*!
+ * @brief           Handling of the ready outputs
+ *
+ * @param[in]       timestamp       time in nanoseconds
+ * @param[in]       iaq             IAQ signal
+ * @param[in]       iaq_accuracy    accuracy of IAQ signal
+ * @param[in]       temperature     temperature signal
+ * @param[in]       humidity        humidity signal
+ * @param[in]       pressure        pressure signal
+ * @param[in]       raw_temperature raw temperature signal
+ * @param[in]       raw_humidity    raw humidity signal
+ * @param[in]       gas             raw gas sensor signal
+ * @param[in]       bsec_status     value returned by the bsec_do_steps() call
+ *
+ * @return          none
+ */
+static void output_ready(
+	int64_t timestamp,
+	float iaq,
+	uint8_t iaq_accuracy,
+	float temperature,
+	float humidity,
+	float pressure,
+	float raw_temperature,
+	float raw_humidity,
+	float gas,
+	bsec_library_return_t bsec_status,
+	float static_iaq,
+	float co2_equivalent,
+	float breath_voc_equivalent
+) {
+	last_bsec_data.temperature               = temperature;
+	last_bsec_data.humidity                  = humidity;
+	last_bsec_data.pressure                  = pressure / 100.;
+	last_bsec_data.gas_resistance            = gas;
+	last_bsec_data.timestamp                 = timestamp;
+	last_bsec_data.accuracy                  = iaq_accuracy;
+	last_bsec_data.indoor_air_quality        = iaq;
+	last_bsec_data.static_indoor_air_quality = static_iaq;
+	last_bsec_data.co2_equivalent            = co2_equivalent;
+	last_bsec_data.breath_voc_equivalent     = breath_voc_equivalent;
+
+	__sync_synchronize();
+
+	last_bme680_timestamp = timestamp;
+
+	bleESS_update_from_bsec_data(&last_bsec_data);
+
+	if (debug) {
+		LOG_INFO(
+			"bsec",
+			"time[ms]: %u, IAQ: %u, IAQ ACC[0-3]: %u, T[.1C]: %u, Hum[.1%%]: %u, P[Pa]: %u, Raw T[.1C]: %u, Raw Hum[.1%%]: %u, Gas[Ohm]: %u, Static IAQ: %u, CO2[ppm]: %u, Breath VOC[ppb]: %u",
+			(unsigned int)(timestamp / 1e6),
+			(unsigned int)(iaq),
+			(unsigned int)(iaq_accuracy),
+			(unsigned int)(temperature * 10),
+			(unsigned int)(humidity * 10),
+			(unsigned int)(pressure),
+			(unsigned int)(raw_temperature * 10),
+			(unsigned int)(raw_humidity * 10),
+			(unsigned int)(gas),
+			(unsigned int)(static_iaq),
+			(unsigned int)(co2_equivalent),
+			(unsigned int)(breath_voc_equivalent * 1e3)
+		);
+	}
+}
+
+int epic_bsec_read_sensors(struct bsec_sensor_data *data)
+{
+	if (data == NULL) {
+		return -EFAULT;
+	}
+	if (!bsec_task_active) {
+		return -ENODEV;
+	}
+
+	/* TODO: could also return -EINVAL */
+	while (last_bme680_timestamp == 0)
+		vTaskDelay(pdMS_TO_TICKS(10));
+
+	*data = last_bsec_data;
+	return 0;
+}
+
+static uint32_t bsec_load(char *path, uint8_t *buffer, uint32_t n_buffer)
+{
+	uint32_t len = 0;
+	int fd, res;
+
+	LOG_DEBUG("bsec", "load %s %lu", path, n_buffer);
+
+	if ((fd = epic_file_open(path, "r")) < 0) {
+		LOG_DEBUG("bsec", "Open failed");
+		return 0;
+	}
+
+	uint32_t header;
+	if ((res = epic_file_read(fd, &header, sizeof(header))) !=
+	    sizeof(header)) {
+		LOG_WARN("bsec", "Header failed");
+		goto done;
+	}
+
+	if (header > n_buffer) {
+		LOG_WARN("bsec", "Too large");
+		goto done;
+	}
+
+	if (epic_file_read(fd, buffer, header) != (int)header) {
+		LOG_WARN("bsec", "Read failed");
+		goto done;
+	}
+
+	len = header;
+
+	LOG_DEBUG("bsec", "Success");
+done:
+	epic_file_close(fd);
+	return len;
+}
+/*!
+ * @brief           Load previous library state from non-volatile memory
+ *
+ * @param[in,out]   state_buffer    buffer to hold the loaded state string
+ * @param[in]       n_buffer        size of the allocated state buffer
+ *
+ * @return          number of bytes copied to state_buffer
+ */
+static uint32_t state_load(uint8_t *state_buffer, uint32_t n_buffer)
+{
+	return bsec_load("bsec_iaq.state", state_buffer, n_buffer);
+}
+
+/*!
+ * @brief           Save library state to non-volatile memory
+ *
+ * @param[in]       state_buffer    buffer holding the state to be stored
+ * @param[in]       length          length of the state string to be stored
+ *
+ * @return          none
+ */
+static void state_save(const uint8_t *state_buffer, uint32_t length)
+{
+	int fd, res;
+
+	LOG_DEBUG("bsec", "state_save %d", (int)length);
+
+	if ((fd = epic_file_open("bsec_iaq.state", "w")) < 0) {
+		LOG_WARN("bsec", "Open failed");
+		return;
+	}
+
+	uint32_t header = length;
+	if ((res = epic_file_write(fd, &header, sizeof(header))) !=
+	    sizeof(header)) {
+		LOG_WARN("bsec", "Header failed");
+		goto done;
+	}
+
+	if (epic_file_write(fd, state_buffer, header) != (int)header) {
+		LOG_WARN("bsec", "Write failed");
+		goto done;
+	}
+
+	LOG_DEBUG("bsec", "stack high: %lu", uxTaskGetStackHighWaterMark(NULL));
+done:
+	epic_file_close(fd);
+}
+/*!
+ * @brief           Delete the library state from non-volatile memory
+ *
+ * @return          none
+ */
+static void state_delete(void)
+{
+	LOG_DEBUG("bsec", "state_delete");
+
+	epic_file_unlink("bsec_iaq.state");
+}
+
+static int8_t
+i2c_write(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16_t size)
+{
+	int8_t ret;
+	hwlock_acquire(HWLOCK_I2C);
+	ret = card10_bosch_i2c_write(addr, reg, p_buf, size);
+	hwlock_release(HWLOCK_I2C);
+	return ret;
+}
+
+static int8_t i2c_read(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16_t size)
+{
+	int8_t ret;
+	hwlock_acquire(HWLOCK_I2C);
+	ret = card10_bosch_i2c_read(addr, reg, p_buf, size);
+	hwlock_release(HWLOCK_I2C);
+	return ret;
+}
+
+static void delay(uint32_t msec)
+{
+	if (xTaskGetSchedulerState() == taskSCHEDULER_NOT_STARTED) {
+		/* We need to fall back to hardware waits if not running
+		 * in a task context */
+		card10_bosch_delay(msec);
+	} else {
+		vTaskDelay(pdMS_TO_TICKS(msec));
+	}
+}
+
+/*!
+ * @brief           Load library config from non-volatile memory
+ *
+ * @param[in,out]   config_buffer    buffer to hold the loaded state string
+ * @param[in]       n_buffer        size of the allocated state buffer
+ *
+ * @return          number of bytes copied to config_buffer
+ */
+static uint32_t config_load(uint8_t *config_buffer, uint32_t n_buffer)
+{
+	uint32_t len = bsec_load("bsec_iaq.config", config_buffer, n_buffer);
+
+	if (len == 0) {
+		LOG_INFO("bsec", "Using default bsec_config_generic_18v_3s_4d");
+		len = sizeof(bsec_config_generic_18v_3s_4d);
+		memcpy(config_buffer, bsec_config_generic_18v_3s_4d, len);
+	}
+
+	return len;
+}
+
+#if ULP
+void ulp_plus_trigger_iaq()
+{
+	/* We call bsec_update_subscription() in order to instruct BSEC to perform an extra measurement at the next
+     * possible time slot
+     */
+
+	bsec_sensor_configuration_t requested_virtual_sensors[1];
+	uint8_t n_requested_virtual_sensors = 1;
+	bsec_sensor_configuration_t
+		required_sensor_settings[BSEC_MAX_PHYSICAL_SENSOR];
+	uint8_t n_required_sensor_settings = BSEC_MAX_PHYSICAL_SENSOR;
+	bsec_library_return_t status       = BSEC_OK;
+
+	/* To trigger a ULP plus, we request the IAQ virtual sensor with a specific sample rate code */
+	requested_virtual_sensors[0].sensor_id = BSEC_OUTPUT_IAQ;
+	requested_virtual_sensors[0].sample_rate =
+		BSEC_SAMPLE_RATE_ULP_MEASUREMENT_ON_DEMAND;
+
+	/* Call bsec_update_subscription() to enable/disable the requested virtual sensors */
+	status = bsec_update_subscription(
+		requested_virtual_sensors,
+		n_requested_virtual_sensors,
+		required_sensor_settings,
+		&n_required_sensor_settings
+	);
+
+	/* The status code would tell is if the request was accepted. It will be rejected if the sensor is not already in
+     * ULP mode, or if the time difference between requests is too short, for example. */
+}
+#endif
+
+bool bsec_active(void)
+{
+	return bsec_task_active;
+}
+
+int bsec_read_bme680(struct bme680_sensor_data *data)
+{
+	if (!bsec_task_active) {
+		return BME680_E_COM_FAIL;
+	}
+
+	if (data == NULL) {
+		return -EFAULT;
+	}
+
+	while (last_bme680_timestamp == 0)
+		vTaskDelay(pdMS_TO_TICKS(10));
+
+	data->temperature    = last_bsec_data.temperature;
+	data->humidity       = last_bsec_data.humidity;
+	data->pressure       = last_bsec_data.pressure;
+	data->gas_resistance = last_bsec_data.gas_resistance;
+
+	return BME680_OK;
+}
+
+/**
+ * Checks config and activates the BSEC libary if requested.
+ *
+ * Initializes the BSEC library before starting the task to
+ * reduce the stack size needed for the task by at least 250 bytes
+ */
+int bsec_activate(void)
+{
+	return_values_init ret;
+#if ULP
+	float sample_rate = BSEC_SAMPLE_RATE_ULP;
+#else
+	float sample_rate = BSEC_SAMPLE_RATE_LP;
+#endif
+
+	if (!config_get_boolean_with_default("bsec_enable", false)) {
+		return -1;
+	}
+
+	debug = config_get_boolean_with_default("bsec_debug", false);
+
+	float temperature_offset =
+		config_get_integer_with_default("bsec_offset", -22) / 10.;
+	if (temperature_offset != 0.0) {
+		LOG_INFO(
+			"bsec",
+			"BSEC Temp offset %d/10 K",
+			(int)(temperature_offset * 10)
+		);
+	}
+
+	/* Puts AT LEAST 2 * #BSEC_MAX_PROPERTY_BLOB_SIZE = 2 * 454 = 908 bytes onto the stack */
+	ret = bsec_iot_init(
+		sample_rate,
+		-temperature_offset,
+		i2c_write,
+		i2c_read,
+		delay,
+		state_load,
+		config_load
+	);
+
+	if (ret.bsec_status == BSEC_E_CONFIG_VERSIONMISMATCH) {
+		/* BSEC version changed and old state is not compatible anymore */
+		/* If the config is also not valid anymore, the user will have
+		 * to fix that. */
+		state_delete();
+		ret = bsec_iot_init(
+			sample_rate,
+			-temperature_offset,
+			i2c_write,
+			i2c_read,
+			delay,
+			state_load,
+			config_load
+		);
+	}
+
+	if (ret.bme680_status) {
+		LOG_WARN("bsec", "bme680 init failed: %d", ret.bme680_status);
+		/* Could not initialize BME680 */
+		return -1;
+	} else if (ret.bsec_status) {
+		LOG_WARN("bsec", "bsec init failed: %d", ret.bsec_status);
+		/* Could not initialize BSEC library */
+		return -1;
+	}
+	return 0;
+}
+
+void vBSECTask(void *pvParameters)
+{
+	bsec_task_active = true;
+	bsec_task_id     = xTaskGetCurrentTaskHandle();
+
+#if ULP
+	/* State is saved every 100 samples, which means every 100 * 300 secs = 500 minutes  */
+	const int stat_save_interval = 100;
+#else
+	/* State is saved every 10.000 samples, which means every 10.000 * 3 secs = 500 minutes  */
+	const int stat_save_interval = 10000;
+#endif
+
+	/* Call to endless loop function which reads and processes data based on sensor settings */
+	/* Puts AT LEAST 2 * BSEC_MAX_STATE_BLOB_SIZE + 8 * sizeof(bsec_input_t) =
+	 * 2 * 139 + 8 * 20 = 438 bytes onto the stack */
+	bsec_iot_loop(
+		delay,
+		get_timestamp_us,
+		output_ready,
+		state_save,
+		stat_save_interval
+	);
+}

epicardium/modules/buttons.c → epicardium/drivers/buttons.c

 #include "epicardium.h"
 #include "modules/modules.h"
-#include "modules/log.h"
+#include "os/core.h"
 
 #include "portexpander.h"
 #include "MAX77650-Arduino-Library.h"
@@ -16,20 +16,15 @@ static const uint8_t pin_mask[] = {
 uint8_t epic_buttons_read(uint8_t mask)
 {
 	uint8_t ret = 0;
-	if (portexpander_detected() && (mask & 0x7)) {
-		if (hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100)) < 0) {
-			LOG_ERR("buttons", "Can't acquire I2C bus");
-			return 0;
-		}
 
+	hwlock_acquire(HWLOCK_I2C);
+	if (portexpander_detected() && (mask & 0x7)) {
 		/*
-		 * Not using PB_Get() here as that performs one I2C transcation
+		 * Not using PB_Get() here as that performs one I2C transaction
 		 * per button.
 		 */
 		uint8_t pin_status = ~portexpander_in_get(0xFF);
 
-		hwlock_release(HWLOCK_I2C);
-
 		for (uint8_t m = 1; m < 0x8; m <<= 1) {
 			if (mask & m && pin_status & pin_mask[m]) {
 				ret |= m;
@@ -41,5 +36,6 @@ uint8_t epic_buttons_read(uint8_t mask)
 		ret |= BUTTON_RESET;
 	}
 
+	hwlock_release(HWLOCK_I2C);
 	return ret;
 }

epicardium/drivers/display/api.c

+#include "epicardium.h"
+#include "drivers/display/lcd.h"
+#include "drivers/display/epic_ctx.h"
+
+#include "FreeRTOS.h"
+#include "LCD_Driver.h"
+#include "gpio.h"
+#include "task.h"
+#include "tmr.h"
+#include "tmr_utils.h"
+
+#include <machine/endian.h>
+#include <string.h>
+
+static TaskHandle_t lock = NULL;
+
+static int check_lock()
+{
+	TaskHandle_t task = xTaskGetCurrentTaskHandle();
+	if (task != lock) {
+		return -EBUSY;
+	} else {
+		return 0;
+	}
+}
+
+static uint16_t rgb888_to_rgb565(uint8_t *bytes)
+{
+	return ((bytes[0] & 0b11111000) << 8) | ((bytes[1] & 0b11111100) << 3) |
+	       (bytes[2] >> 3);
+}
+
+static inline void
+rgb565_to_rgb888(uint16_t pixel, uint8_t *red, uint8_t *green, uint8_t *blue)
+{
+	*blue  = (pixel & 31) << 3;
+	*green = ((pixel >> 5) & 63) << 2;
+	*red   = ((pixel >> 11) & 31) << 3;
+}
+
+int epic_disp_print(
+	int16_t posx,
+	int16_t posy,
+	const char *pString,
+	uint16_t fg,
+	uint16_t bg
+) {
+	return epic_disp_print_adv(DISP_FONT20, posx, posy, pString, fg, bg);
+}
+
+static const float font_map[] = {
+	[DISP_FONT8] = 8.0f,   [DISP_FONT12] = 12.0f, [DISP_FONT16] = 16.0f,
+	[DISP_FONT20] = 20.0f, [DISP_FONT24] = 24.0f,
+};
+
+int epic_disp_print_adv(
+	uint8_t font,
+	int16_t posx,
+	int16_t posy,
+	const char *pString,
+	uint16_t fg,
+	uint16_t bg
+) {
+	uint8_t r, g, b;
+	int cl = check_lock();
+	if (cl < 0) {
+		return cl;
+	}
+
+	if (font >= (sizeof(font_map) / sizeof(font_map[0]))) {
+		return -EINVAL;
+	}
+
+	float font_size = font_map[font];
+	ctx_font_size(epicardium_ctx, font_size);
+
+	if (fg != bg) {
+		/* non-transparent background */
+		rgb565_to_rgb888(bg, &r, &g, &b);
+		ctx_rgba8(epicardium_ctx, r, g, b, 255);
+		float width = ctx_text_width(epicardium_ctx, pString);
+		ctx_rectangle(epicardium_ctx, posx, posy, width, font_size);
+		ctx_fill(epicardium_ctx);
+	}
+
+	rgb565_to_rgb888(fg, &r, &g, &b);
+	ctx_rgba8(epicardium_ctx, r, g, b, 255);
+	ctx_move_to(epicardium_ctx, posx, (float)posy + font_size * 0.8f);
+	ctx_text(epicardium_ctx, pString);
+
+	return 0;
+}
+
+int epic_disp_clear(uint16_t color)
+{
+	int cl = check_lock();
+	if (cl < 0) {
+		return cl;
+	}
+
+	/*
+	 * We could use ctx for this but it's much easier to just clear the
+	 * framebuffer directly.
+	 */
+	for (size_t i = 0; i < sizeof(epicardium_ctx_fb); i += 2) {
+		epicardium_ctx_fb[i]     = color >> 8;
+		epicardium_ctx_fb[i + 1] = color & 0xff;
+	}
+
+	return 0;
+}
+
+int epic_disp_pixel(int16_t x, int16_t y, uint16_t color)
+{
+	int cl = check_lock();
+	if (cl < 0) {
+		return cl;
+	}
+
+	uint8_t r, g, b;
+	rgb565_to_rgb888(color, &r, &g, &b);
+	ctx_set_pixel_u8(epicardium_ctx, x, y, r, g, b, 255);
+
+	return 0;
+}
+
+static uint16_t rgb565_pixel_from_buf(
+	uint8_t *img,
+	enum epic_rgb_format format,
+	int16_t width,
+	int16_t x,
+	int16_t y,
+	uint8_t *alpha
+) {
+	uint16_t tmp16;
+	uint8_t rgba[4];
+
+	switch (format) {
+	case EPIC_RGB565:
+		*alpha = 255;
+		memcpy(&tmp16, &img[y * width * 2 + x * 2], 2);
+		return tmp16;
+	case EPIC_RGBA5551:
+		memcpy(&tmp16, &img[y * width * 2 + x * 2], 2);
+		*alpha = (tmp16 & 0x01) ? 255 : 0;
+		return (tmp16 & 0xFFC0) | ((tmp16 & 0x3E) >> 1);
+	case EPIC_RGB8:
+		*alpha = 255;
+		memcpy(rgba, &img[y * width * 3 + x * 3], 3);
+		return rgb888_to_rgb565(rgba);
+	case EPIC_RGBA8:
+		memcpy(rgba, &img[y * width * 4 + x * 4], 4);
+		*alpha = rgba[3];
+		return rgb888_to_rgb565(rgba);
+	default:
+		return 0xFFFF;
+	}
+}
+
+int epic_disp_blit(
+	int16_t pos_x,
+	int16_t pos_y,
+	int16_t width,
+	int16_t height,
+	void *img,
+	enum epic_rgb_format format
+) {
+	int cl = check_lock();
+	if (cl < 0) {
+		return cl;
+	}
+
+	for (int16_t xsrc = 0; xsrc < width; xsrc += 1) {
+		for (int16_t ysrc = 0; ysrc < height; ysrc += 1) {
+			int16_t xscreen = pos_x + xsrc;
+			int16_t yscreen = pos_y + ysrc;
+			size_t offset   = yscreen * 160 * 2 + xscreen * 2;
+			if (xscreen < 0 || xscreen >= 160 || yscreen < 0 ||
+			    yscreen >= 80) {
+				continue;
+			}
+
+			uint8_t alpha  = 255;
+			uint16_t pixel = rgb565_pixel_from_buf(
+				img, format, width, xsrc, ysrc, &alpha
+			);
+
+			if (alpha == 0) {
+				continue;
+			}
+
+			epicardium_ctx_fb[offset]     = (pixel & 0xFF00) >> 8;
+			epicardium_ctx_fb[offset + 1] = pixel & 0xFF;
+		}
+	}
+
+	return 0;
+}
+
+int epic_disp_line(
+	int16_t xstart,
+	int16_t ystart,
+	int16_t xend,
+	int16_t yend,
+	uint16_t color,
+	enum disp_linestyle linestyle,
+	uint16_t pixelsize
+) {
+	int cl = check_lock();
+	if (cl < 0) {
+		return cl;
+	}
+
+	float xstartf = xstart, ystartf = ystart, xendf = xend, yendf = yend;
+
+	/*
+	 * For odd line widths, shift the line by half a pixel so it aligns
+	 * perfectly with the pixel grid.
+	 */
+	if (pixelsize % 2 == 1) {
+		xstartf += 0.5f;
+		ystartf += 0.5f;
+		yendf += 0.5f;
+		xendf += 0.5f;
+	}
+
+	uint8_t r, g, b;
+	rgb565_to_rgb888(color, &r, &g, &b);
+	ctx_rgba8_stroke(epicardium_ctx, r, g, b, 255);
+
+	ctx_line_width(epicardium_ctx, pixelsize);
+	ctx_move_to(epicardium_ctx, xstartf, ystartf);
+	ctx_line_to(epicardium_ctx, xendf, yendf);
+	ctx_stroke(epicardium_ctx);
+
+	return 0;
+}
+
+int epic_disp_rect(
+	int16_t xstart,
+	int16_t ystart,
+	int16_t xend,
+	int16_t yend,
+	uint16_t color,
+	enum disp_fillstyle fillstyle,
+	uint16_t pixelsize
+) {
+	int cl = check_lock();
+	if (cl < 0)
+		return cl;
+
+	uint8_t r, g, b;
+	rgb565_to_rgb888(color, &r, &g, &b);
+
+	ctx_rectangle(
+		epicardium_ctx,
+		xstart,
+		ystart,
+		xend - xstart + 1,
+		yend - ystart + 1
+	);
+
+	switch (fillstyle) {
+	case FILLSTYLE_EMPTY:
+		ctx_rgba8_stroke(epicardium_ctx, r, g, b, 255);
+		ctx_line_width(epicardium_ctx, pixelsize);
+		ctx_stroke(epicardium_ctx);
+		break;
+	case FILLSTYLE_FILLED:
+		ctx_rgba8(epicardium_ctx, r, g, b, 255);
+		ctx_fill(epicardium_ctx);
+		break;
+	}
+
+	return 0;
+}
+
+int epic_disp_circ(
+	int16_t x,
+	int16_t y,
+	uint16_t rad,
+	uint16_t color,
+	enum disp_fillstyle fillstyle,
+	uint16_t pixelsize
+) {
+	int cl = check_lock();
+	if (cl < 0)
+		return cl;
+
+	uint8_t r, g, b;
+	rgb565_to_rgb888(color, &r, &g, &b);
+
+	ctx_arc(epicardium_ctx, x, y, rad, 0.0f, CTX_PI * 1.95, 0);
+
+	switch (fillstyle) {
+	case FILLSTYLE_EMPTY:
+		ctx_rgba8_stroke(epicardium_ctx, r, g, b, 255);
+		ctx_line_width(epicardium_ctx, pixelsize);
+		ctx_stroke(epicardium_ctx);
+		break;
+	case FILLSTYLE_FILLED:
+		ctx_rgba8(epicardium_ctx, r, g, b, 255);
+		ctx_fill(epicardium_ctx);
+		break;
+	}
+
+	return 0;
+}
+
+int epic_disp_update()
+{
+	int cl = check_lock();
+	if (cl < 0) {
+		return cl;
+	}
+
+	lcd_write_fb(epicardium_ctx_fb);
+
+	return 0;
+}
+
+int epic_disp_framebuffer(union disp_framebuffer *fb)
+{
+	int cl = check_lock();
+	if (cl < 0) {
+		return cl;
+	}
+
+	/*
+	 * Flip the screen because that's what this API call historically
+	 * expects.
+	 */
+	lcd_set_screenflip(true);
+	lcd_write_fb(fb->raw);
+	lcd_set_screenflip(false);
+	return 0;
+}
+
+int epic_disp_backlight(uint16_t brightness)
+{
+	/* TODO: lock? */
+	if (brightness == 0) {
+		lcd_set_sleep(true);
+	} else {
+		lcd_set_sleep(false);
+	}
+	LCD_SetBacklight(brightness);
+	return 0;
+}
+
+int epic_disp_open()
+{
+	TaskHandle_t task = xTaskGetCurrentTaskHandle();
+	if (lock == task) {
+		return 0;
+	} else if (lock == NULL) {
+		lock = task;
+		return 0;
+	} else {
+		return -EBUSY;
+	}
+}
+
+int epic_disp_close()
+{
+	if (check_lock() < 0 && lock != NULL) {
+		return -EBUSY;
+	} else {
+		lock = NULL;
+		return 0;
+	}
+}
+
+void disp_update_backlight_clock(void)
+{
+	LCD_UpdateBacklightClock();
+}
+
+void disp_forcelock()
+{
+	TaskHandle_t task = xTaskGetCurrentTaskHandle();
+	lock              = task;
+}

epicardium/drivers/display/epic_ctx.h

+#pragma once
+
+#include "ctx.h"
+
+extern Ctx *epicardium_ctx;
+extern uint8_t epicardium_ctx_fb[160 * 80 * 2];

epicardium/drivers/display/init.c

+#include "drivers/display/lcd.h"
+#include "drivers/display/epic_ctx.h"
+#include "drivers/drivers.h"
+#include "display.h"
+
+#include "ctx.h"
+
+#include <stdint.h>
+#include <machine/endian.h>
+
+#if BYTE_ORDER == LITTLE_ENDIAN
+#define CARD10_CTX_FORMAT CTX_FORMAT_RGB565_BYTESWAPPED
+#else
+#define CARD10_CTX_FORMAT CTX_FORMAT_RGB565
+#endif
+
+uint8_t epicardium_ctx_fb[160 * 80 * 2] = { 0 };
+Ctx *epicardium_ctx                     = NULL;
+
+void disp_init(void)
+{
+	/*
+	 * The bootloader has already initialized the display, so we only need
+	 * to do the bare minimum here.
+	 */
+	lcd_reconfigure();
+
+	/*
+	 * Initialize the graphics context.
+	 */
+	disp_ctx_reinit();
+}
+
+void disp_ctx_reinit(void)
+{
+	if (epicardium_ctx != NULL) {
+		ctx_free(epicardium_ctx);
+	}
+
+	epicardium_ctx = ctx_new_for_framebuffer(
+		epicardium_ctx_fb, 160, 80, 160 * 2, CARD10_CTX_FORMAT
+	);
+
+	/* set some defaults */
+	ctx_rgba(epicardium_ctx, 1.0f, 1.0f, 1.0f, 1.0f);
+	ctx_rgba_stroke(epicardium_ctx, 1.0f, 1.0f, 1.0f, 1.0f);
+	ctx_font(epicardium_ctx, "ctx-mono");
+}

epicardium/drivers/display/lcd.c

+#include "os/core.h"
+
+#include "MAX77650-Arduino-Library.h"
+#include "gpio.h"
+#include "mxc_delay.h"
+#include "portexpander.h"
+#include "spi.h"
+
+#include <machine/endian.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+/* HAL Interfaces {{{ */
+static const gpio_cfg_t GPIO_PIN_DC = {
+	PORT_1, PIN_6, GPIO_FUNC_OUT, GPIO_PAD_NONE
+};
+
+static void lcd_hw_init(void)
+{
+	GPIO_Config(&GPIO_PIN_DC);
+
+	/* for the reset pin */
+	if (!portexpander_detected()) {
+		/* Open-drain */
+		MAX77650_setDRV(false);
+		/* Output */
+		MAX77650_setDIR(false);
+	}
+}
+
+static void lcd_set_dc(bool state)
+{
+	if (state) {
+		GPIO_OutSet(&GPIO_PIN_DC);
+	} else {
+		GPIO_OutClr(&GPIO_PIN_DC);
+	}
+}
+
+static void lcd_set_rst(bool state)
+{
+	if (!portexpander_detected()) {
+		MAX77650_setDO(state ? true : false);
+	} else {
+		portexpander_out_put(PIN_4, state ? 0xFF : 0);
+	}
+}
+
+/** Bit Rate. Display has 15 MHz limit */
+#define SPI_SPEED (15 * 1000 * 1000)
+
+static void lcd_spi_write(const uint8_t *data, size_t count)
+{
+	const sys_cfg_spi_t spi_master_cfg = {
+		.map    = MAP_A,
+		.ss0    = Enable,
+		.ss1    = Disable,
+		.ss2    = Disable,
+		.num_io = 2,
+	};
+	spi_req_t request = {
+		.ssel     = 0,
+		.deass    = 1,
+		.ssel_pol = SPI17Y_POL_LOW,
+		.tx_data  = data,
+		.rx_data  = NULL,
+		.width    = SPI17Y_WIDTH_1,
+		.len      = count,
+		.bits     = 8,
+		.rx_num   = 0,
+		.tx_num   = 0,
+		.callback = NULL,
+	};
+	if (SPI_Init(SPI2, 0, SPI_SPEED, spi_master_cfg) != 0) {
+		panic("Error configuring display SPI");
+	}
+	SPI_MasterTrans(SPI2, &request);
+}
+
+static void lcd_delay(size_t millis)
+{
+	// TODO: Is this what we want?
+	mxc_delay(millis * 1000);
+}
+/* HAL Interfaces }}} */
+
+enum lcd_commands {
+	/** Sleep In */
+	LCD_SLPIN = 0x10,
+	/** Sleep Out */
+	LCD_SLPOUT = 0x11,
+	/** Display Inversion On */
+	LCD_INVON = 0x21,
+	/** Display On */
+	LCD_DISPON = 0x29,
+	/** Column Address Set */
+	LCD_CASET = 0x2A,
+	/** Row Address Set */
+	LCD_RASET = 0x2B,
+	/** Memory Write */
+	LCD_RAMWR = 0x2C,
+	/** Memory Data Access Control */
+	LCD_MADCTL = 0x36,
+	/** Interface Pixel Format */
+	LCD_COLMOD = 0x3A,
+	/** Frame Rate Control (In normal mode/ Full colors) */
+	LCD_FRMCTR1 = 0xB1,
+	/** Frame Rate Control (In Idle mode/ 8-colors) */
+	LCD_FRMCTR2 = 0xB2,
+	/** Frame Rate Control (In Partial mode/ full colors) */
+	LCD_FRMCTR3 = 0xB3,
+	/** Display Inversion Control */
+	LCD_INVCTR = 0xB4,
+	/** Power Control 1 */
+	LCD_PWCTR1 = 0xC0,
+	/** Power Control 2 */
+	LCD_PWCTR2 = 0xC1,
+	/** Power Control 3 (in Normal mode/ Full colors) */
+	LCD_PWCTR3 = 0xC2,
+	/** Power Control 4 (in Idle mode/ 8-colors) */
+	LCD_PWCTR4 = 0xC3,
+	/** Power Control 5 (in Partial mode/ full-colors) */
+	LCD_PWCTR5 = 0xC4,
+	/** VCOM Control 1 */
+	LCD_VMCTR1 = 0xC5,
+	/** Gamma (+ polarity) Correction Characteristics Setting */
+	LCD_GMCTRP1 = 0xE0,
+	/** Gamma (- polarity) Correction Characteristics Setting */
+	LCD_GMCTRN1 = 0xE1,
+};
+
+enum madctl_bits {
+	MADCTL_MY  = 0x80,
+	MADCTL_MX  = 0x40,
+	MADCTL_MV  = 0x20,
+	MADCTL_ML  = 0x10,
+	MADCTL_RGB = 0x08,
+	MADCTL_MH  = 0x04,
+};
+
+static void
+lcd_send_command(enum lcd_commands cmd, const uint8_t *args, size_t count)
+{
+	lcd_set_dc(false);
+	lcd_spi_write((uint8_t *)&cmd, 1);
+	if (args != NULL && count != 0) {
+		lcd_set_dc(true);
+		lcd_spi_write(args, count);
+	}
+}
+
+static void lcd_hard_reset(void)
+{
+	lcd_delay(20);
+	lcd_set_rst(false);
+	lcd_delay(20);
+	lcd_set_rst(true);
+	lcd_delay(20);
+}
+
+void lcd_set_sleep(bool sleep)
+{
+	static int current_sleep = -1;
+
+	if (sleep == current_sleep) {
+		return;
+	}
+	current_sleep = sleep;
+
+	if (sleep) {
+		lcd_send_command(LCD_SLPIN, NULL, 0);
+	} else {
+		lcd_send_command(LCD_SLPOUT, NULL, 0);
+	}
+}
+
+/**
+ * Perform a minimal initialization under the assumption that the bootloader has
+ * already turned on the display.  This is faster and prevents visible
+ * reinitialization artifacts.
+ */
+void lcd_reconfigure(void)
+{
+	/* Invert Display (twice for unknown reasons ...). */
+	lcd_send_command(LCD_INVON, NULL, 0);
+	lcd_send_command(LCD_INVON, NULL, 0);
+
+	/* Set framerate control values for all modes to the same values. */
+	const uint8_t frmctr[] = { 0x05, 0x3A, 0x3A, 0x05, 0x3A, 0x3A };
+	lcd_send_command(LCD_FRMCTR1, frmctr, 3);
+	lcd_send_command(LCD_FRMCTR2, frmctr, 3);
+	lcd_send_command(LCD_FRMCTR3, frmctr, 6);
+
+	/* Set display inversion control (unsure what this does?). */
+	const uint8_t invctr[] = { 0x03 };
+	lcd_send_command(LCD_INVCTR, invctr, sizeof(invctr));
+
+	/* Configure GVDD voltage to 4.7V. */
+	const uint8_t pwctr1[] = { 0x62, 0x02, 0x04 };
+	lcd_send_command(LCD_PWCTR1, pwctr1, sizeof(pwctr1));
+
+	/* Configure only ignored bits? */
+	const uint8_t pwctr2[] = { 0xC0 };
+	lcd_send_command(LCD_PWCTR2, pwctr2, sizeof(pwctr2));
+
+	/*
+	 * Configure "large" amount of current in operational amplifier and
+	 * booster step-ups for all modes.
+	 */
+	const uint8_t pwctr3[] = { 0x0D, 0x00 }, pwctr4[] = { 0x8D, 0x6A },
+		      pwctr5[] = { 0x8D, 0xEE };
+	lcd_send_command(LCD_PWCTR3, pwctr3, sizeof(pwctr3));
+	lcd_send_command(LCD_PWCTR4, pwctr4, sizeof(pwctr4));
+	lcd_send_command(LCD_PWCTR5, pwctr5, sizeof(pwctr5));
+
+	/* Configure VCOMH voltage to 2.850V. */
+	const uint8_t vmctr1[] = { 0x0E };
+	lcd_send_command(LCD_VMCTR1, vmctr1, sizeof(vmctr1));
+
+	/* Write positive and negative gamma correction values. */
+	const uint8_t gmctrp1[] = {
+		0x10, 0x0E, 0x02, 0x03, 0x0E, 0x07, 0x02, 0x07,
+		0x0A, 0x12, 0x27, 0x37, 0x00, 0x0D, 0x0E, 0x10,
+	};
+	const uint8_t gmctrn1[] = {
+		0x10, 0x0E, 0x03, 0x03, 0x0F, 0x06, 0x02, 0x08,
+		0x0A, 0x13, 0x26, 0x36, 0x00, 0x0D, 0x0E, 0x10,
+	};
+	lcd_send_command(LCD_GMCTRP1, gmctrp1, sizeof(gmctrp1));
+	lcd_send_command(LCD_GMCTRN1, gmctrn1, sizeof(gmctrn1));
+
+	/* Configure 16-bit pixel format. */
+	const uint8_t colmod[] = { 0x05 };
+	lcd_send_command(LCD_COLMOD, colmod, sizeof(colmod));
+
+	/*
+	 * Configure "MADCTL", which defines the pixel and color access order.
+	 */
+	const uint8_t madctl[] = { MADCTL_MX | MADCTL_MV | MADCTL_RGB };
+	/*
+	 * Waveshare Driver:
+	 * const uint8_t madctl[] = { MADCTL_MY | MADCTL_MV | MADCTL_RGB };
+	 */
+	lcd_send_command(LCD_MADCTL, madctl, sizeof(madctl));
+
+	/* Turn the display on. */
+	lcd_send_command(LCD_DISPON, NULL, 0);
+}
+
+/**
+ * Perform a full initialization of the display.  This will ensure the display
+ * is in a deterministic state.
+ */
+void lcd_initialize(void)
+{
+	lcd_hw_init();
+
+	lcd_hard_reset();
+
+	lcd_send_command(LCD_SLPOUT, NULL, 0);
+	lcd_delay(120);
+
+	lcd_reconfigure();
+}
+
+/**
+ * Write a partial display update.
+ *
+ * The rectangle from column ``xstart`` to ``xend`` (inclusive) and row
+ * ``ystart`` to ``yend`` (inclusive) will be updated with the contents of
+ * ``fb``.
+ *
+ * ``fb`` **must** have a size of
+ * ``(xend - xstart + 1) * (yend - ystart + 1) * 2`` bytes.
+ */
+void lcd_write_fb_partial(
+	uint16_t xstart,
+	uint16_t ystart,
+	uint16_t xend,
+	uint16_t yend,
+	const uint8_t *fb
+) {
+	uint16_t param_buffer[2];
+
+	/* Column start and end are offset by 1. */
+	param_buffer[0] = __htons(xstart + 1);
+	param_buffer[1] = __htons(xend + 1);
+	lcd_send_command(
+		LCD_CASET, (uint8_t *)param_buffer, sizeof(param_buffer)
+	);
+
+	/* Row start and end are offset by a magic 26. */
+	param_buffer[0] = __htons(ystart + 26);
+	param_buffer[1] = __htons(yend + 26);
+	lcd_send_command(
+		LCD_RASET, (uint8_t *)param_buffer, sizeof(param_buffer)
+	);
+
+	/* Now write out the actual framebuffer contents. */
+	size_t fb_size = (xend - xstart + 1) * (yend - ystart + 1) * 2;
+	lcd_send_command(LCD_RAMWR, fb, fb_size);
+}
+
+/**
+ * Write out a full framebuffer update.
+ *
+ * ``fb`` **must** be 160 * 80 * 2 = **25600** bytes in size.  The pixels are
+ * ordered in rows starting at the top left of the screen.  Each pixel must have
+ * its bytes laid out as big endian (while the CPU is little endian!).
+ */
+void lcd_write_fb(const uint8_t *fb)
+{
+	lcd_write_fb_partial(0, 0, 159, 79, fb);
+}
+
+/**
+ * Flip the screen orientation upside down.
+ *
+ * Historically we had software perform a flip of the framebuffer before
+ * sending it out.  This function provides a way to make the hardware accept
+ * such a flipped framebuffer.  This exists mostly to support the
+ * :c:func:`epic_disp_framebuffer()` API call for legacy l0dables.
+ */
+void lcd_set_screenflip(bool flipped)
+{
+	const uint8_t madctl_upright[] = { MADCTL_MX | MADCTL_MV | MADCTL_RGB };
+	const uint8_t madctl_flipped[] = { MADCTL_MY | MADCTL_MV | MADCTL_RGB };
+	if (flipped) {
+		lcd_send_command(LCD_MADCTL, madctl_flipped, 1);
+	} else {
+		lcd_send_command(LCD_MADCTL, madctl_upright, 1);
+	}
+}

epicardium/drivers/display/lcd.h

+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+void lcd_set_sleep(bool sleep);
+void lcd_reconfigure(void);
+void lcd_initialize(void);
+void lcd_write_fb_partial(
+	uint16_t xstart,
+	uint16_t ystart,
+	uint16_t xend,
+	uint16_t yend,
+	const uint8_t *fb
+);
+void lcd_write_fb(const uint8_t *fb);
+void lcd_set_screenflip(bool flipped);

epicardium/drivers/drivers.h

+#ifndef DRIVERS_H
+#define DRIVERS_H
+
+#include "FreeRTOS.h"
+#include "gpio.h"
+#include "os/mutex.h"
+#include "epicardium.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* ---------- Serial ------------------------------------------------------- */
+#define SERIAL_READ_BUFFER_SIZE 128
+#define SERIAL_WRITE_STREAM_BUFFER_SIZE 512
+void serial_init();
+void vSerialTask(void *pvParameters);
+void serial_enqueue_char(char chr);
+void serial_flush(void);
+extern TaskHandle_t serial_task_id;
+/* Turn off the print queue and do prints synchroneous from now on. */
+void serial_return_to_synchronous();
+
+// For the eSetBit xTaskNotify task semaphore trigger
+enum serial_notify{
+	SERIAL_WRITE_NOTIFY = 0x01,
+	SERIAL_READ_NOTIFY  = 0x02,
+};
+
+/* ---------- PMIC --------------------------------------------------------- */
+void vPmicTask(void *pvParameters);
+
+/* ---------- Watchdog ----------------------------------------------------- */
+void watchdog_init();
+void watchdog_clearer_init();
+
+/* Critical battery voltage */
+#define BATTERY_CRITICAL   3.40f
+
+enum pmic_amux_signal {
+	PMIC_AMUX_DISABLED    = 0x0,
+	PMIC_AMUX_CHGIN_U     = 0x1,
+	PMIC_AMUX_CHGIN_I     = 0x2,
+	PMIC_AMUX_BATT_U      = 0x3,
+	PMIC_AMUX_BATT_CHG_I  = 0x4,
+	PMIC_AMUX_BATT_DIS_I  = 0x5,
+	PMIC_AMUX_BATT_NULL_I = 0x6,
+	PMIC_AMUX_THM_U       = 0x7,
+	PMIC_AMUX_TBIAS_U     = 0x8,
+	PMIC_AMUX_AGND_U      = 0x9,
+	PMIC_AMUX_SYS_U       = 0xA,
+	_PMIC_AMUX_MAX,
+};
+
+/*
+ * Read a value from the PMIC's AMUX.  The result is already converted into its
+ * proper unit.  See the MAX77650 datasheet for details.
+ */
+int pmic_read_amux(enum pmic_amux_signal sig, float *result);
+
+
+/* ---------- Display ------------------------------------------------------ */
+/* Do display and graphics initialization/configuration. */
+void disp_init(void);
+/* Reinitialize the graphics context. */
+void disp_ctx_reinit(void);
+/* Forces an unlock of the display. Only to be used in Epicardium */
+void disp_forcelock();
+void disp_update_backlight_clock(void);
+
+/* ---------- BHI160 ------------------------------------------------------- */
+#define BHI160_FIFO_SIZE             128
+#define BHI160_MUTEX_WAIT_MS          50
+void vBhi160Task(void *pvParameters);
+
+/* ---------- BME680 ------------------------------------------------------- */
+void bme680_periodic(int period);
+
+/* ---------- MAX86150 ----------------------------------------------------- */
+#define MAX86150_MUTEX_WAIT_MS		50
+void vMAX86150Task(void *pvParameters);
+void max86150_mutex_init(void);
+
+
+/* ---------- MAX30001 ----------------------------------------------------- */
+void vMAX30001Task(void *pvParameters);
+void max30001_mutex_init(void);
+
+/* ---------- GPIO --------------------------------------------------------- */
+extern gpio_cfg_t gpio_configs[];
+
+/* ---------- BSEC / BME680 ------------------------------------------------ */
+int bsec_activate(void);
+void vBSECTask(void *pvParameters);
+bool bsec_active(void);
+struct bme680_sensor_data;
+int bsec_read_bme680(struct bme680_sensor_data *data);
+
+/* ---------- Sleep -------------------------------------------------------- */
+void sleep_deepsleep(void);
+
+/* ---------- RNG ---------------------------------------------------------- */
+void rng_init(void);
+
+#endif /* DRIVERS_H */

epicardium/modules/gpio.c → epicardium/drivers/gpio.c

@@ -4,7 +4,7 @@
 #include "mxc_sys.h"
 #include "adc.h"
 #include "mxc_errors.h"
-#include "modules/log.h"
+#include "os/core.h"
 #include "modules/modules.h"
 
 /*
@@ -48,7 +48,16 @@ int epic_gpio_set_pin_mode(uint8_t pin, uint8_t mode)
 
 	gpio_cfg_t *cfg = &gpio_configs[pin];
 
-	if (mode & EPIC_GPIO_MODE_IN) {
+	if (mode & EPIC_GPIO_MODE_ADC) {
+		if (s_adc_channels[pin] == -1) {
+			LOG_WARN("gpio", "ADC not available on pin %d", pin);
+			return -EINVAL;
+		}
+		cfg->func = GPIO_FUNC_ALT1;
+		if (mode & EPIC_GPIO_MODE_OUT) {
+			return -EINVAL;
+		}
+	} else if (mode & EPIC_GPIO_MODE_IN) {
 		cfg->func = GPIO_FUNC_IN;
 		if (mode & EPIC_GPIO_MODE_OUT) {
 			return -EINVAL;
@@ -58,15 +67,6 @@ int epic_gpio_set_pin_mode(uint8_t pin, uint8_t mode)
 		if (mode & EPIC_GPIO_MODE_IN) {
 			return -EINVAL;
 		}
-	} else if (mode & EPIC_GPIO_MODE_ADC) {
-		if (s_adc_channels[pin] == -1) {
-			LOG_WARN("gpio", "ADC not available on pin %d", pin);
-			return -EINVAL;
-		}
-		cfg->func = GPIO_FUNC_ALT1;
-		if (mode & EPIC_GPIO_MODE_OUT) {
-			return -EINVAL;
-		}
 	} else {
 		return -EINVAL;
 	}
@@ -137,8 +137,7 @@ int epic_gpio_read_pin(uint8_t pin)
 	} else if (cfg->func == GPIO_FUNC_IN) {
 		return GPIO_InGet(cfg) != 0;
 	} else if (cfg->func == GPIO_FUNC_ALT1) {
-		int rc = hwlock_acquire(HWLOCK_ADC, pdMS_TO_TICKS(10));
-		if (!rc) {
+		hwlock_acquire(HWLOCK_ADC);
 		ADC_StartConvert(s_adc_channels[pin], 0, 0);
 		uint16_t value;
 		int rc = ADC_GetData(&value);
@@ -147,8 +146,6 @@ int epic_gpio_read_pin(uint8_t pin)
 			return -EIO;
 		}
 		return (int)value;
-		}
-		return rc;
 	} else {
 		return -EINVAL;
 	}

epicardium/modules/leds.c → epicardium/drivers/leds.c

 #include "leds.h"
 #include "pmic.h"
 #include "FreeRTOS.h"
+#include "timers.h"
 #include "task.h"
 #include "epicardium.h"
-#include "modules.h"
+#include "modules/modules.h"
 
 #include <stdbool.h>
 
-//TODO: create smth like vTaskDelay(pdMS_TO_TICKS(//put ms here)) for us, remove blocking delay from /lib/leds.c to avoid process blocking
+/*
+ * TODO: create smth like vTaskDelay(pdMS_TO_TICKS(//put ms here)) for us,
+ *       remove blocking delay from /lib/leds.c to avoid process blocking
+ */
 
 #define NUM_LEDS 15 /* Take from lib/card10/leds.c */
 
-static void do_update()
+static void do_update(void)
 {
-	while (hwlock_acquire(HWLOCK_LED, pdMS_TO_TICKS(1)) < 0) {
-		vTaskDelay(pdMS_TO_TICKS(1));
-	}
+	hwlock_acquire(HWLOCK_LED);
+	hwlock_acquire(HWLOCK_I2C);
 
 	leds_update_power();
 	leds_update();
 
+	hwlock_release(HWLOCK_I2C);
 	hwlock_release(HWLOCK_LED);
 }
 
@@ -96,13 +100,7 @@ void epic_leds_dim_top(uint8_t value)
 {
 	leds_set_dim_top(value);
 	if (personal_state_enabled() == 0) {
-		while (hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(1)) < 0) {
-			vTaskDelay(pdMS_TO_TICKS(1));
-		}
-
-		leds_update();
-
-		hwlock_release(HWLOCK_I2C);
+		do_update();
 	}
 }
 
@@ -110,48 +108,58 @@ void epic_leds_dim_bottom(uint8_t value)
 {
 	leds_set_dim_bottom(value);
 	if (personal_state_enabled() == 0) {
-		while (hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(1)) < 0) {
-			vTaskDelay(pdMS_TO_TICKS(1));
-		}
-
-		leds_update();
-
-		hwlock_release(HWLOCK_I2C);
+		do_update();
 	}
 }
 
 void epic_leds_set_rocket(int led, uint8_t value)
 {
-	while (hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(1)) < 0) {
-		vTaskDelay(pdMS_TO_TICKS(1));
-	}
-
-	value = value > 31 ? 31 : value;
-	pmic_set_led(led, value);
-
+	hwlock_acquire(HWLOCK_I2C);
+	pmic_set_led(led, value > 31 ? 31 : value);
 	hwlock_release(HWLOCK_I2C);
 }
 
 int epic_leds_get_rocket(int led)
 {
 	int ret = 0;
-	while (hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(1)) < 0) {
-		vTaskDelay(pdMS_TO_TICKS(1));
-	}
 
+	hwlock_acquire(HWLOCK_I2C);
 	ret = pmic_get_led(led);
 	hwlock_release(HWLOCK_I2C);
 	return ret;
 }
 
-void epic_set_flashlight(bool power)
+static StaticTimer_t flash_timer_data[3];
+static TimerHandle_t flash_timer[] = { NULL, NULL, NULL };
+
+static void rocket_timer_callback(TimerHandle_t flash_timer)
 {
-	while (hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(1)) < 0) {
-		vTaskDelay(pdMS_TO_TICKS(1));
+	uint32_t id = (uint32_t)pvTimerGetTimerID(flash_timer);
+	epic_leds_set_rocket(id, 0);
 }
 
+void epic_leds_flash_rocket(int led, uint8_t value, int millis)
+{
+	int ticks  = millis * (configTICK_RATE_HZ / 1000);
+	int32_t id = led;
+	if (flash_timer[id] == NULL) {
+		flash_timer[id] = xTimerCreateStatic(
+			"flashtimer",
+			ticks,
+			pdFALSE,
+			(void *)id,
+			rocket_timer_callback,
+			&flash_timer_data[id]
+		);
+		epic_leds_set_rocket(led, value);
+	}
+	epic_leds_set_rocket(led, value);
+	xTimerChangePeriod(flash_timer[id], ticks, 0);
+}
+void epic_set_flashlight(bool power)
+{
+	hwlock_acquire(HWLOCK_I2C);
 	leds_flashlight(power);
-
 	hwlock_release(HWLOCK_I2C);
 }
 
@@ -162,16 +170,12 @@ void epic_leds_update(void)
 
 void epic_leds_set_powersave(bool eco)
 {
-	while (hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(1)) < 0) {
-		vTaskDelay(pdMS_TO_TICKS(1));
-	}
-
+	hwlock_acquire(HWLOCK_I2C);
 	leds_powersave(eco);
-
 	hwlock_release(HWLOCK_I2C);
 }
 
-void epic_leds_set_gamma_table(uint8_t rgb_channel, uint8_t gamma_table[256])
+void epic_leds_set_gamma_table(uint8_t rgb_channel, uint8_t *gamma_table)
 {
 	leds_set_gamma_table(rgb_channel, gamma_table);
 }

epicardium/modules/light_sensor.c → epicardium/drivers/light_sensor.c

 #include "epicardium.h"
-#include "modules/log.h"
+#include "os/core.h"
+#include "os/work_queue.h"
 #include "modules/modules.h"
 
 #include "mxc_config.h"
@@ -29,9 +30,7 @@ static int light_sensor_init()
 
 uint16_t epic_light_sensor_read()
 {
-	if (hwlock_acquire(HWLOCK_ADC, pdMS_TO_TICKS(1000)) != 0) {
-		return 0;
-	}
+	hwlock_acquire(HWLOCK_ADC);
 
 	ADC_StartConvert(ADC_CH_7, 0, 0);
 	ADC_GetData(&last_value);
@@ -40,26 +39,21 @@ uint16_t epic_light_sensor_read()
 	return last_value;
 }
 
-static void readAdcCallback()
+static void workpoll(void *data)
 {
-	if (hwlock_acquire(HWLOCK_ADC, 0) != 0) {
-		/* Can't do much about this here ... Retry next time */
-		return;
+	epic_light_sensor_read();
 }
 
-	ADC_StartConvert(ADC_CH_7, 0, 0);
-	ADC_GetData(&last_value);
-
-	hwlock_release(HWLOCK_ADC);
+static void poll(TimerHandle_t xTimer)
+{
+	workqueue_schedule(workpoll, NULL);
 }
 
 int epic_light_sensor_run()
 {
 	int ret = 0;
 
-	if (hwlock_acquire(HWLOCK_ADC, pdMS_TO_TICKS(500)) != 0) {
-		return -EBUSY;
-	}
+	hwlock_acquire(HWLOCK_ADC);
 
 	light_sensor_init();
 
@@ -69,7 +63,7 @@ int epic_light_sensor_run()
 			READ_FREQ,
 			pdTRUE,
 			NULL,
-			readAdcCallback,
+			poll,
 			&poll_timer_buffer
 		);
 		// since &poll_timer_buffer is not NULL, xTimerCreateStatic should allways succeed, so

epicardium/modules/max30001.c → epicardium/drivers/max30001.c

@@ -9,17 +9,14 @@
 
 #include "FreeRTOS.h"
 #include "task.h"
-#include "semphr.h"
 #include "queue.h"
 
-#include "api/interrupt-sender.h"
 #include "epicardium.h"
-#include "modules/log.h"
+#include "os/core.h"
 #include "modules/modules.h"
 #include "modules/stream.h"
-
-/* Ticks to wait when trying to acquire lock */
-#define LOCK_WAIT pdMS_TO_TICKS(MAX30001_MUTEX_WAIT_MS)
+#include "os/mutex.h"
+#include "user_core/interrupts.h"
 
 /* Interrupt Pin */
 static const gpio_cfg_t max30001_interrupt_pin = {
@@ -36,8 +33,7 @@ static const gpio_cfg_t analog_switch = {
 static TaskHandle_t max30001_task_id = NULL;
 
 /* MAX30001 Mutex */
-static StaticSemaphore_t max30001_mutex_data;
-static SemaphoreHandle_t max30001_mutex = NULL;
+static struct mutex max30001_mutex = { 0 };
 
 /* Stream */
 static struct stream_info max30001_stream;
@@ -54,15 +50,8 @@ int epic_max30001_enable_sensor(struct max30001_sensor_config *config)
 {
 	int result = 0;
 
-	result = hwlock_acquire(HWLOCK_SPI_ECG, pdMS_TO_TICKS(100));
-	if (result < 0) {
-		return result;
-	}
-
-	if (xSemaphoreTake(max30001_mutex, LOCK_WAIT) != pdTRUE) {
-		result = -EBUSY;
-		goto out_free_spi;
-	}
+	mutex_lock(&max30001_mutex);
+	hwlock_acquire(HWLOCK_SPI_ECG);
 
 	struct stream_info *stream = &max30001_stream;
 	;
@@ -97,9 +86,8 @@ int epic_max30001_enable_sensor(struct max30001_sensor_config *config)
 	result                 = SD_MAX30001_ECG;
 
 out_free_both:
-	xSemaphoreGive(max30001_mutex);
-out_free_spi:
 	hwlock_release(HWLOCK_SPI_ECG);
+	mutex_unlock(&max30001_mutex);
 	return result;
 }
 
@@ -107,15 +95,8 @@ int epic_max30001_disable_sensor(void)
 {
 	int result = 0;
 
-	result = hwlock_acquire(HWLOCK_SPI_ECG, pdMS_TO_TICKS(100));
-	if (result < 0) {
-		return result;
-	}
-
-	if (xSemaphoreTake(max30001_mutex, LOCK_WAIT) != pdTRUE) {
-		result = -EBUSY;
-		goto out_free_spi;
-	}
+	mutex_lock(&max30001_mutex);
+	hwlock_acquire(HWLOCK_SPI_ECG);
 
 	struct stream_info *stream = &max30001_stream;
 	result                     = stream_deregister(SD_MAX30001_ECG, stream);
@@ -134,9 +115,8 @@ int epic_max30001_disable_sensor(void)
 
 	result = 0;
 out_free_both:
-	xSemaphoreGive(max30001_mutex);
-out_free_spi:
 	hwlock_release(HWLOCK_SPI_ECG);
+	mutex_unlock(&max30001_mutex);
 	return result;
 }
 
@@ -158,10 +138,15 @@ static void max30001_handle_samples(int16_t *sensor_data, int16_t n)
 
 		/* Discard overflow.  See discussion in !316. */
 		if (xQueueSend(max30001_stream.queue, &data, 0) != pdTRUE) {
+			if (!max30001_stream.was_full) {
 				LOG_WARN("max30001", "queue full");
 			}
+			max30001_stream.was_full = true;
+		} else {
+			max30001_stream.was_full = false;
 		}
-	api_interrupt_trigger(EPIC_INT_MAX30001_ECG);
+	}
+	interrupt_trigger(EPIC_INT_MAX30001_ECG);
 }
 
 /***** Functions *****/
@@ -298,23 +283,16 @@ static int max30001_fetch_fifo(void)
 {
 	int result = 0;
 
-	result = hwlock_acquire(HWLOCK_SPI_ECG, pdMS_TO_TICKS(100));
-	if (result < 0) {
-		return result;
-	}
-
-	if (xSemaphoreTake(max30001_mutex, LOCK_WAIT) != pdTRUE) {
-		result = -EBUSY;
-		goto out_free_spi;
-	}
+	mutex_lock(&max30001_mutex);
+	hwlock_acquire(HWLOCK_SPI_ECG);
 
 	uint32_t ecgFIFO, readECGSamples, ETAG[32], status;
 	int16_t ecgSample[32];
-	const int EINT_STATUS_MASK       = 1 << 23;
-	const int FIFO_OVF_MASK          = 0x7;
-	const int FIFO_VALID_SAMPLE_MASK = 0x0;
-	const int FIFO_FAST_SAMPLE_MASK  = 0x1;
-	const int ETAG_BITS_MASK         = 0x7;
+	const uint32_t EINT_STATUS_MASK       = 1 << 23;
+	const uint32_t FIFO_OVF_MASK          = 0x7;
+	const uint32_t FIFO_VALID_SAMPLE_MASK = 0x0;
+	const uint32_t FIFO_FAST_SAMPLE_MASK  = 0x1;
+	const uint32_t ETAG_BITS_MASK         = 0x7;
 
 	status = ecg_read_reg(STATUS); // Read the STATUS register
 
@@ -344,9 +322,8 @@ static int max30001_fetch_fifo(void)
 		max30001_handle_samples(ecgSample, readECGSamples);
 	}
 
-	xSemaphoreGive(max30001_mutex);
-out_free_spi:
 	hwlock_release(HWLOCK_SPI_ECG);
+	mutex_unlock(&max30001_mutex);
 	return result;
 }
 
@@ -369,24 +346,15 @@ static void max300001_interrupt_callback(void *_)
 
 void max30001_mutex_init(void)
 {
-	max30001_mutex = xSemaphoreCreateMutexStatic(&max30001_mutex_data);
+	mutex_create(&max30001_mutex);
 }
 
 void vMAX30001Task(void *pvParameters)
 {
 	max30001_task_id = xTaskGetCurrentTaskHandle();
 
-	int lockret = hwlock_acquire(HWLOCK_SPI_ECG, pdMS_TO_TICKS(100));
-	if (lockret < 0) {
-		LOG_CRIT("max30001", "Failed to acquire SPI lock!");
-		vTaskDelay(portMAX_DELAY);
-	}
-
-	/* Take Mutex during initialization, just in case */
-	if (xSemaphoreTake(max30001_mutex, 0) != pdTRUE) {
-		LOG_CRIT("max30001", "Failed to acquire MAX30001 mutex!");
-		vTaskDelay(portMAX_DELAY);
-	}
+	mutex_lock(&max30001_mutex);
+	hwlock_acquire(HWLOCK_SPI_ECG);
 
 	/* Install interrupt callback */
 	GPIO_Config(&max30001_interrupt_pin);
@@ -407,20 +375,15 @@ void vMAX30001Task(void *pvParameters)
 	GPIO_Config(&analog_switch);
 	GPIO_OutClr(&analog_switch); // Wrist
 
-	xSemaphoreGive(max30001_mutex);
 	hwlock_release(HWLOCK_SPI_ECG);
+	mutex_unlock(&max30001_mutex);
 
 	/* ----------------------------------------- */
 
 	while (1) {
 		if (max30001_sensor_active) {
 			int ret = max30001_fetch_fifo();
-			if (ret == -EBUSY) {
-				LOG_WARN(
-					"max30001", "Could not acquire mutex?"
-				);
-				continue;
-			} else if (ret < 0) {
+			if (ret < 0) {
 				LOG_ERR("max30001", "Unknown error: %d", -ret);
 			}
 		}

epicardium/drivers/max86150.c

+#include <stdbool.h>
+#include <stddef.h>
+#include <stdio.h>
+#include "max86150.h"
+#include "epicardium.h"
+#include "os/core.h"
+#include "modules/stream.h"
+#include "gpio.h"
+#include "pmic.h"
+#include "user_core/interrupts.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+#include "modules/modules.h"
+
+static const gpio_cfg_t max86150_interrupt_pin = {
+	PORT_1, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP
+};
+
+/* MAX86150 Task ID */
+static TaskHandle_t max86150_task_id = NULL;
+
+/* MAX86150 Mutex */
+static struct mutex max86150_mutex = { 0 };
+
+/* Stream */
+static struct stream_info max86150_stream;
+
+/* Active */
+static bool max86150_sensor_active = false;
+
+int epic_max86150_enable_sensor(
+	struct max86150_sensor_config *config, size_t config_size
+) {
+	int result = 0;
+
+	if (sizeof(struct max86150_sensor_config) != config_size) {
+		return -EINVAL;
+	}
+
+	mutex_lock(&max86150_mutex);
+	hwlock_acquire(HWLOCK_I2C);
+
+	struct stream_info *stream = &max86150_stream;
+	stream->item_size          = sizeof(struct max86150_sensor_data);
+	stream->queue =
+		xQueueCreate(config->sample_buffer_len, stream->item_size);
+	if (stream->queue == NULL) {
+		result = -ENOMEM;
+		goto out_free;
+	}
+
+	uint8_t ppg_sample_rate;
+
+	if (config->ppg_sample_rate == 10) {
+		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_10;
+	} else if (config->ppg_sample_rate == 20) {
+		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_20;
+	} else if (config->ppg_sample_rate == 50) {
+		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_50;
+	} else if (config->ppg_sample_rate == 84) {
+		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_84;
+	} else if (config->ppg_sample_rate == 100) {
+		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_100;
+	} else if (config->ppg_sample_rate == 200) {
+		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_200;
+	} else if (config->ppg_sample_rate == 400) {
+		ppg_sample_rate = MAX86150_PPG_SAMPLERATE_400;
+	} else {
+		result = -EINVAL;
+		goto out_free;
+	}
+
+	result = stream_register(SD_MAX86150, stream);
+	if (result < 0) {
+		vQueueDelete(stream->queue);
+		goto out_free;
+	}
+
+	bool begin_result = max86150_begin();
+	if (!begin_result) {
+		result = -ENODEV;
+		vQueueDelete(stream->queue);
+		goto out_free;
+	}
+
+	max86150_setup(ppg_sample_rate);
+	max86150_get_int1();
+	max86150_get_int2();
+
+	max86150_sensor_active = true;
+	result                 = SD_MAX86150;
+
+out_free:
+	hwlock_release(HWLOCK_I2C);
+	mutex_unlock(&max86150_mutex);
+	return result;
+}
+
+int epic_max86150_disable_sensor(void)
+{
+	int result = 0;
+
+	mutex_lock(&max86150_mutex);
+	hwlock_acquire(HWLOCK_I2C);
+
+	max86150_shut_down();
+	max86150_sensor_active = false;
+
+	struct stream_info *stream = &max86150_stream;
+	result                     = stream_deregister(SD_MAX86150, stream);
+	if (result == 0) {
+		vQueueDelete(stream->queue);
+		stream->queue = NULL;
+	}
+
+	hwlock_release(HWLOCK_I2C);
+	mutex_unlock(&max86150_mutex);
+	return result;
+}
+
+static int max86150_handle_sample(struct max86150_sensor_data *data)
+{
+	//LOG_INFO("max86150", "Sample! %ld, %ld, %ld", data->red, data->ir, data->ecg);
+
+	if (max86150_stream.queue == NULL) {
+		return -ESRCH;
+	}
+
+	/* Discard overflow.  See discussion in !316. */
+	if (xQueueSend(max86150_stream.queue, data, 0) != pdTRUE) {
+		if (!max86150_stream.was_full) {
+			LOG_WARN("max86150", "queue full");
+		}
+		max86150_stream.was_full = true;
+	} else {
+		max86150_stream.was_full = false;
+	}
+
+	return 0;
+}
+
+static int max86150_fetch_fifo(void)
+{
+	int result = 0;
+
+	mutex_lock(&max86150_mutex);
+	hwlock_acquire(HWLOCK_I2C);
+
+	struct max86150_sensor_data sample;
+
+	// There is a recommendation from Maxim not to read the entire FIFO, but rather a fixed number of samples.
+	// See https://os.mbed.com/users/laserdad/code/MAX86150_ECG_PPG//file/3c728f3d1f10/main.cpp/
+	// So we should not use max86150_check() but max86150_get_sample().
+	int n = 0;
+	while (max86150_get_sample(&sample.red, &sample.ir, &sample.ecg) > 0) {
+		n++;
+		result = max86150_handle_sample(&sample);
+		// stop in case of errors
+		if (result < 0) {
+			n = 0;
+			break;
+		}
+	}
+	hwlock_release(HWLOCK_I2C);
+	mutex_unlock(&max86150_mutex);
+
+	if (n > 0) {
+		interrupt_trigger(EPIC_INT_MAX86150);
+	}
+	//LOG_INFO("max86150", "%d", n);
+	return result;
+}
+
+/*
+ * Callback for the MAX86150 interrupt pin.  This callback is called from the
+ * SDK's GPIO interrupt driver, in interrupt context.
+ */
+static void max86150_interrupt_callback(void *_)
+{
+	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+
+	if (max86150_task_id != NULL) {
+		vTaskNotifyGiveFromISR(
+			max86150_task_id, &xHigherPriorityTaskWoken
+		);
+		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+	}
+}
+/* }}} */
+
+void max86150_mutex_init(void)
+{
+	mutex_create(&max86150_mutex);
+}
+
+void vMAX86150Task(void *pvParameters)
+{
+	max86150_task_id = xTaskGetCurrentTaskHandle();
+
+	mutex_lock(&max86150_mutex);
+	hwlock_acquire(HWLOCK_I2C);
+
+	/* Install interrupt callback */
+	GPIO_Config(&max86150_interrupt_pin);
+	GPIO_RegisterCallback(
+		&max86150_interrupt_pin, max86150_interrupt_callback, NULL
+	);
+	GPIO_IntConfig(
+		&max86150_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_FALLING
+	);
+	GPIO_IntEnable(&max86150_interrupt_pin);
+	NVIC_SetPriority(
+		(IRQn_Type)MXC_GPIO_GET_IRQ(max86150_interrupt_pin.port), 2
+	);
+	NVIC_EnableIRQ(
+		(IRQn_Type)MXC_GPIO_GET_IRQ(max86150_interrupt_pin.port)
+	);
+
+	hwlock_release(HWLOCK_I2C);
+	mutex_unlock(&max86150_mutex);
+
+	/* ----------------------------------------- */
+
+	while (1) {
+		if (max86150_sensor_active) {
+			//LOG_INFO("max86150", "Interrupt!");
+			mutex_lock(&max86150_mutex);
+			hwlock_acquire(HWLOCK_I2C);
+
+			int i1 = max86150_get_int1();
+
+			hwlock_release(HWLOCK_I2C);
+			mutex_unlock(&max86150_mutex);
+
+			//LOG_INFO("max86150", "%d", i1);
+
+			if (i1 & 16) {
+				interrupt_trigger(EPIC_INT_MAX86150_PROX);
+			}
+
+			int ret = max86150_fetch_fifo();
+			if (ret < 0) {
+				LOG_ERR("max86150", "Unknown error: %d", -ret);
+			}
+		}
+		/*
+		 * Wait for interrupt.  After two seconds, fetch FIFO anyway
+		 *
+		 * In the future, reads using epic_stream_read() might also
+		 * trigger a FIFO fetch, from outside this task.
+		 */
+		ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(2000));
+	}
+}

epicardium/drivers/meson.build

+driver_sources = files(
+  'bhi.c',
+  'bsec.c',
+  'bme680.c',
+  'buttons.c',
+  'gpio.c',
+  'leds.c',
+  'light_sensor.c',
+  'max86150.c',
+  'max30001.c',
+  'pmic.c',
+  'rtc.c',
+  'serial.c',
+  'sleep.c',
+  'rng.c',
+  'usb.c',
+  'vibra.c',
+  'watchdog.c',
+  'ws2812.c',
+
+  'display/lcd.c',
+  'display/api.c',
+  'display/init.c',
+)

epicardium/modules/pmic.c → epicardium/drivers/pmic.c

 #include "epicardium.h"
 #include "modules/modules.h"
-#include "modules/log.h"
+#include "drivers/drivers.h"
+#include "os/core.h"
+#include "os/config.h"
+#include "user_core/user_core.h"
 
 #include "card10.h"
 #include "pmic.h"
@@ -18,8 +21,6 @@
 #include <stdio.h>
 #include <string.h>
 
-#define LOCK_WAIT pdMS_TO_TICKS(1000)
-
 /* Task ID for the pmic handler */
 static TaskHandle_t pmic_task_id = NULL;
 
@@ -47,22 +48,13 @@ void pmic_interrupt_callback(void *_)
 int pmic_read_amux(enum pmic_amux_signal sig, float *result)
 {
 	int ret = 0;
-	int i2c_ret = 0;
 
 	if (sig > _PMIC_AMUX_MAX) {
 		return -EINVAL;
 	}
 
-	int adc_ret = hwlock_acquire(HWLOCK_ADC, LOCK_WAIT);
-	if (adc_ret < 0) {
-		ret = adc_ret;
-		goto done;
-	}
-	i2c_ret = hwlock_acquire(HWLOCK_I2C, LOCK_WAIT);
-	if (i2c_ret < 0) {
-		ret = i2c_ret;
-		goto done;
-	}
+	hwlock_acquire(HWLOCK_ADC);
+	hwlock_acquire(HWLOCK_I2C);
 
 	/* Select the correct channel for this measurement.  */
 	MAX77650_setMUX_SEL(sig);
@@ -75,11 +67,7 @@ int pmic_read_amux(enum pmic_amux_signal sig, float *result)
 	hwlock_release(HWLOCK_I2C);
 
 	vTaskDelay(pdMS_TO_TICKS(5));
-	i2c_ret = hwlock_acquire(HWLOCK_I2C, LOCK_WAIT);
-	if (i2c_ret < 0) {
-		ret = i2c_ret;
-		goto done;
-	}
+	hwlock_acquire(HWLOCK_I2C);
 
 	uint16_t adc_data;
 	ADC_StartConvert(ADC_CH_0, 0, 0);
@@ -120,14 +108,8 @@ int pmic_read_amux(enum pmic_amux_signal sig, float *result)
 		ret = -EINVAL;
 	}
 
-done:
-	if (i2c_ret == 0) {
 	hwlock_release(HWLOCK_I2C);
-	}
-
-	if (adc_ret == 0) {
 	hwlock_release(HWLOCK_ADC);
-	}
 
 	return ret;
 }
@@ -138,10 +120,7 @@ done:
  */
 static uint8_t pmic_poll_interrupts(void)
 {
-	while (hwlock_acquire(HWLOCK_I2C, LOCK_WAIT) < 0) {
-		LOG_WARN("pmic", "Failed to acquire I2C. Retrying ...");
-		vTaskDelay(pdMS_TO_TICKS(100));
-	}
+	hwlock_acquire(HWLOCK_I2C);
 
 	uint8_t int_flag = MAX77650_getINT_GLBL();
 	hwlock_release(HWLOCK_I2C);
@@ -162,6 +141,9 @@ __attribute__((noreturn)) static void pmic_die(float u_batt)
 	/* Grab the screen */
 	disp_forcelock();
 
+	/* Turn it on in case it was off */
+	epic_disp_backlight(100);
+
 	/* Draw an error screen */
 	epic_disp_clear(0x0000);
 
@@ -194,6 +176,30 @@ static void pmic_check_battery()
 	float u_batt;
 	int res;
 
+	/**
+	 * 0 = uncertain, ask config
+	 * 1 = disabled
+	 * 2 = enabled
+	 */
+	static int pmic_do_battery_check = 0;
+
+	if (pmic_do_battery_check == 0) {
+		if (config_get_boolean_with_default("battery_check", true)) {
+			pmic_do_battery_check = 2;
+		} else {
+			pmic_do_battery_check = 1;
+			LOG_WARN(
+				"pmic",
+				"Battery check was disabled by config!"
+			);
+		}
+	}
+
+	if (pmic_do_battery_check == 1) {
+		/* Disabled, ignore */
+		return;
+	}
+
 	res = pmic_read_amux(PMIC_AMUX_BATT_U, &u_batt);
 	if (res < 0) {
 		LOG_ERR("pmic",
@@ -334,8 +340,13 @@ void vPmicTask(void *pvParameters)
 				TickType_t duration =
 					xTaskGetTickCount() - button_start_tick;
 
-				if (duration > 1000) {
+				if (duration > pdMS_TO_TICKS(1000)) {
 					disp_forcelock();
+					disp_ctx_reinit();
+
+					/* Turn it on in case it was off */
+					epic_disp_backlight(100);
+
 					epic_disp_clear(0x0000);
 
 					char buf[20];
@@ -366,6 +377,22 @@ void vPmicTask(void *pvParameters)
 				if (duration >= pdMS_TO_TICKS(1000)) {
 					if (epic_buttons_read(
 						    BUTTON_RIGHT_TOP)) {
+						disp_forcelock();
+						disp_ctx_reinit();
+
+						/* Turn it on in case it was off */
+						epic_disp_backlight(100);
+
+						epic_disp_clear(0x0000);
+						epic_disp_print(
+							55,
+							30,
+							"Reset!",
+							0xf800,
+							0x0000
+						);
+						epic_disp_update();
+
 						serial_return_to_synchronous();
 						LOG_WARN(
 							"pmic",