bhi.c

#include <stdio.h>
#include <string.h>

#include "gpio.h"
#include "bhy_uc_driver.h"
#include "bhy.h"
#include "pmic.h"

#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 "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[];

/* Interrupt Pin */
static const gpio_cfg_t bhi160_interrupt_pin = {
	PORT_0, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP
};

/* 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 };

/*
 * From the official docs:
 *
 *    The sic matrix should be calculated for customer platform by logging
 *    uncalibrated magnetometer data.  The sic matrix here is only an example
 *    array (identity matrix). Customer should generate their own matrix.  This
 *    affects magnetometer fusion performance.
 *
 * 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 };
/* 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;

/* Streams */
static struct stream_info bhi160_streams[10];

/* Active */
static bool bhi160_sensor_active[10] = { 0 };

/* -- Utilities -------------------------------------------------------- {{{ */
/*
 * Retrieve the data size for a sensor.  This value is needed for the creation
 * of the sensor's sample queue.
 */
static size_t bhi160_lookup_data_size(enum bhi160_sensor_type type)
{
	switch (type) {
	case BHI160_ACCELEROMETER:
	case BHI160_MAGNETOMETER:
	case BHI160_ORIENTATION:
	case BHI160_GYROSCOPE:
		return sizeof(struct bhi160_data_vector);
	default:
		return 0;
	}
}

/*
 * Map a sensor type to the virtual sensor ID used by BHy1.
 */
static bhy_virtual_sensor_t bhi160_lookup_vs_id(enum bhi160_sensor_type type)
{
	switch (type) {
	case BHI160_ACCELEROMETER:
		return VS_ID_ACCELEROMETER;
	case BHI160_ORIENTATION:
		return VS_ID_ORIENTATION;
	case BHI160_GYROSCOPE:
		return VS_ID_GYROSCOPE;
	default:
		return -1;
	}
}

/*
 * Map a sensor type to its stream descriptor.
 */
static int bhi160_lookup_sd(enum bhi160_sensor_type type)
{
	switch (type) {
	case BHI160_ACCELEROMETER:
		return SD_BHI160_ACCELEROMETER;
	case BHI160_ORIENTATION:
		return SD_BHI160_ORIENTATION;
	case BHI160_GYROSCOPE:
		return SD_BHI160_GYROSCOPE;
	default:
		return -1;
	}
}
/* }}} */

/* -- API -------------------------------------------------------------- {{{ */
int epic_bhi160_enable_sensor(
	enum bhi160_sensor_type sensor_type,
	struct bhi160_sensor_config *config
) {
	int result = 0;

	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
	if (vs_id < 0) {
		return -ENODEV;
	}

	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;
	}

	struct stream_info *stream = &bhi160_streams[sensor_type];
	stream->item_size          = bhi160_lookup_data_size(sensor_type);
	/* TODO: Sanity check length */
	stream->queue =
		xQueueCreate(config->sample_buffer_len, stream->item_size);
	if (stream->queue == NULL) {
		result = -ENOMEM;
		goto out_free_both;
	}

	result = stream_register(bhi160_lookup_sd(sensor_type), stream);
	if (result < 0) {
		goto out_free_both;
	}

	result = bhy_enable_virtual_sensor(
		vs_id,
		VS_WAKEUP,
		config->sample_rate,
		0,
		VS_FLUSH_NONE,
		0,
		config->dynamic_range /* dynamic range is sensor dependent */
	);
	if (result != BHY_SUCCESS) {
		goto out_free_both;
	}

	bhi160_sensor_active[sensor_type] = true;
	result                            = bhi160_lookup_sd(sensor_type);

out_free_both:
	xSemaphoreGive(bhi160_mutex);
out_free_i2c:
	hwlock_release(HWLOCK_I2C);
	return result;
}

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) {
		return -ENODEV;
	}

	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;
	}

	struct stream_info *stream = &bhi160_streams[sensor_type];
	result = stream_deregister(bhi160_lookup_sd(sensor_type), stream);
	if (result < 0) {
		goto out_free_both;
	}

	vQueueDelete(stream->queue);
	stream->queue = NULL;
	result        = bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
	if (result < 0) {
		goto out_free_both;
	}

	bhi160_sensor_active[sensor_type] = false;

	result = 0;
out_free_both:
	xSemaphoreGive(bhi160_mutex);
out_free_i2c:
	hwlock_release(HWLOCK_I2C);
	return result;
}

void epic_bhi160_disable_all_sensors()
{
	for (int i = 0; i < sizeof(bhi160_sensor_active); i++) {
		if (bhi160_sensor_active[i]) {
			epic_bhi160_disable_sensor(i);
		}
	}
}

/* }}} */

/* -- Driver ----------------------------------------------------------- {{{ */
/*
 * Handle a single packet from the FIFO.  For most sensors this means pushing
 * the sample into its sample queue.
 */
static void
bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
{
	uint8_t sensor_id = sensor_data->data_vector.sensor_id;
	struct bhi160_data_vector data_vector;
	/*
	 * Timestamp of the next samples, counting at 32 kHz.
	 * Currently unused.
	 */
	static uint32_t timestamp           = 0;
	enum bhi160_sensor_type sensor_type = 0;
	int epic_int                        = 0;
	bool wakeup                         = false;

	switch (sensor_id) {
	case VS_ID_TIMESTAMP_MSW_WAKEUP:
		wakeup = true;
		/* fall through */
	case VS_ID_TIMESTAMP_MSW:
		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
		timestamp = sensor_data->data_scalar_u16.data << 16;
		break;
	case VS_ID_TIMESTAMP_LSW_WAKEUP:
		wakeup = true;
		/* fall through */
	case VS_ID_TIMESTAMP_LSW:
		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
		timestamp = (timestamp & 0xFFFF0000) |
			    sensor_data->data_scalar_u16.data;
		break;
	case VS_ID_ACCELEROMETER_WAKEUP:
	case VS_ID_ORIENTATION_WAKEUP:
	case VS_ID_GYROSCOPE_WAKEUP:
		wakeup = true;
		/* fall through */
	case VS_ID_ACCELEROMETER:
	case VS_ID_ORIENTATION:
	case VS_ID_GYROSCOPE:
		switch (sensor_id) {
		case VS_ID_ACCELEROMETER_WAKEUP:
		case VS_ID_ACCELEROMETER:
			sensor_type = BHI160_ACCELEROMETER;
			epic_int    = EPIC_INT_BHI160_ACCELEROMETER;
			break;
		case VS_ID_ORIENTATION_WAKEUP:
		case VS_ID_ORIENTATION:
			sensor_type = BHI160_ORIENTATION;
			epic_int    = EPIC_INT_BHI160_ORIENTATION;
			break;
		case VS_ID_GYROSCOPE_WAKEUP:
		case VS_ID_GYROSCOPE:
			sensor_type = BHI160_GYROSCOPE;
			epic_int    = EPIC_INT_BHI160_GYROSCOPE;
			break;
		}

		MXC_ASSERT(data_type == BHY_DATA_TYPE_VECTOR);
		if (bhi160_streams[sensor_type].queue == NULL) {
			break;
		}
		data_vector.data_type = BHI160_DATA_TYPE_VECTOR;
		data_vector.x         = sensor_data->data_vector.x;
		data_vector.y         = sensor_data->data_vector.y;
		data_vector.z         = sensor_data->data_vector.z;
		data_vector.status    = sensor_data->data_vector.status;
		xQueueSend(
			bhi160_streams[sensor_type].queue,
			&data_vector,
			BHI160_MUTEX_WAIT_MS
		);
		if (wakeup) {
			api_interrupt_trigger(epic_int);
		}
		break;
	default:
		break;
	}
}

/*
 * Fetch all data available from BHI160's FIFO buffer and handle all packets
 * contained in it.
 */
static int bhi160_fetch_fifo(void)
{
	/*
	 * Warning:  The code from the BHy1 docs has some issues.  This
	 * implementation looks similar, but has a few important differences.
	 * You'll probably be best of leaving it as it is ...
	 */

	int 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;
	}

	while (bytes_left_in_fifo) {
		/* Fill local FIFO buffer with as many bytes as possible */
		uint16_t bytes_read;
		bhy_read_fifo(
			&bhi160_fifo[start_index],
			BHI160_FIFO_SIZE - start_index,
			&bytes_read,
			&bytes_left_in_fifo
		);

		/* Add the bytes left from the last transfer on top */
		bytes_read += start_index;

		/* Handle all full packets received in this transfer */
		uint8_t *fifo_ptr   = bhi160_fifo;
		uint16_t bytes_left = bytes_read;
		while (bytes_left > 0) {
			bhy_data_generic_t sensor_data;
			bhy_data_type_t data_type;
			result = bhy_parse_next_fifo_packet(
				&fifo_ptr,
				&bytes_left,
				&sensor_data,
				&data_type
			);

			if (result == BHY_SUCCESS) {
				bhi160_handle_packet(data_type, &sensor_data);
			} else {
				break;
			}
		}

		/* Shift the remaining bytes to the beginning */
		for (int i = 0; i < bytes_left; i++) {
			bhi160_fifo[i] =
				bhi160_fifo[bytes_read - bytes_left + i];
		}
		start_index = bytes_left;
	}

	xSemaphoreGive(bhi160_mutex);
out_free_i2c:
	hwlock_release(HWLOCK_I2C);
	return result;
}

/*
 * Callback for the BHI160 interrupt pin.  This callback is called from the
 * SDK's GPIO interrupt driver, in interrupt context.
 */
static void bhi160_interrupt_callback(void *_)
{
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;

	if (bhi160_task_id != NULL) {
		vTaskNotifyGiveFromISR(
			bhi160_task_id, &xHigherPriorityTaskWoken
		);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
}
/* }}} */

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);
	}

	memset(bhi160_streams, 0x00, sizeof(bhi160_streams));

	/* Install interrupt callback */
	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_IntEnable(&bhi160_interrupt_pin);
	NVIC_SetPriority(
		(IRQn_Type)MXC_GPIO_GET_IRQ(bhi160_interrupt_pin.port), 2
	);
	NVIC_EnableIRQ((IRQn_Type)MXC_GPIO_GET_IRQ(bhi160_interrupt_pin.port));

	/* Upload firmware */
	ret = bhy_driver_init(bhy1_fw);
	if (ret) {
		LOG_CRIT("bhi160", "BHy1 init failed!");
		vTaskDelay(portMAX_DELAY);
	}

	/* Wait for first interrupt */
	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);
	}

	/* Remap axes to match card10 layout */
	/* Due to a known issue (#133) the first call to
	 * bhy_mapping_matrix_set might fail. */
	bhy_mapping_matrix_set(
		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
	);
	bhy_mapping_matrix_set(
		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
	);
	bhy_mapping_matrix_set(
		PHYSICAL_SENSOR_INDEX_MAG, bmm150_mapping_matrix
	);
	bhy_mapping_matrix_set(
		PHYSICAL_SENSOR_INDEX_GYRO, bhi160_mapping_matrix
	);

	/* Set "SIC" matrix.  TODO: Find out what this is about */
	bhy_set_sic_matrix(bhi160_sic_array);

	xSemaphoreGive(bhi160_mutex);
	hwlock_release(HWLOCK_I2C);

	/* ----------------------------------------- */

	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);
		}

		/*
		 * Wait for interrupt.  After two seconds, fetch FIFO anyway in
		 * case there are any diagnostics or errors.
		 *
		 * In the future, reads using epic_stream_read() might also
		 * trigger a FIFO fetch, from outside this task.
		 */
		ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(2000));
	}
}