path: root/driver/accelgyro_bmi260.c

/* Copyright 2020 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

/**
 * BMI260 accelerometer and gyro module for Chrome EC
 * 3D digital accelerometer & 3D digital gyroscope
 */

#include "accelgyro.h"
#include "console.h"
#include "driver/accelgyro_bmi_common.h"
#include "driver/accelgyro_bmi260.h"
#include "endian.h"
#include "hwtimer.h"
#include "i2c.h"
#include "math_util.h"
#include "spi.h"
#include "task.h"
#include "third_party/bmi260/accelgyro_bmi260_config_tbin.h"
#include "timer.h"
#include "util.h"
#include "watchdog.h"

#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)

#ifdef CONFIG_ACCEL_FIFO
static volatile uint32_t last_interrupt_timestamp;
#endif

/*
 * The gyro start-up time is 45ms in normal mode
 *                            2ms in fast start-up mode
 */
static int wakeup_time[] = {
	[MOTIONSENSE_TYPE_ACCEL] = 2,
	[MOTIONSENSE_TYPE_GYRO] = 45,
	[MOTIONSENSE_TYPE_MAG] = 1
};

static int enable_sensor(const struct motion_sensor_t *s, int enable)
{
	int ret;

	ret = bmi_enable_reg8(s, BMI260_PWR_CTRL,
			      BMI260_PWR_EN(s->type),
			      enable);
	if (ret)
		return ret;

	if (s->type == MOTIONSENSE_TYPE_GYRO) {
		/* switch to performance mode */
		ret = bmi_enable_reg8(s, BMI_CONF_REG(s->type),
				      BMI260_FILTER_PERF |
				      BMI260_GYR_NOISE_PERF,
				      enable);
	} else {
		ret = bmi_enable_reg8(s, BMI_CONF_REG(s->type),
				      BMI260_FILTER_PERF,
				      enable);
	}
	return ret;

}

static int set_data_rate(const struct motion_sensor_t *s,
			 int rate,
			 int rnd)
{
	int ret, normalized_rate;
	uint8_t reg_val;
	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);

	if (rate == 0) {
		/* FIFO stop collecting events */
#ifdef CONFIG_ACCEL_FIFO
		bmi_enable_fifo(s, 0);
#endif
		/* disable sensor */
		ret = enable_sensor(s, 0);
		msleep(3);
		data->odr = 0;
		return ret;
	} else if (data->odr == 0) {
		/* enable sensor */
		ret = enable_sensor(s, 1);
		if (ret)
			return ret;
		/* Wait for accel/gyro to wake up */
		msleep(wakeup_time[s->type]);
	}

	ret = bmi_get_normalized_rate(s, rate, rnd,
				      &normalized_rate, &reg_val);
	if (ret)
		return ret;

	/*
	 * Lock accel resource to prevent another task from attempting
	 * to write accel parameters until we are done.
	 */
	mutex_lock(s->mutex);

	ret = bmi_set_reg8(s, BMI_CONF_REG(s->type),
			   reg_val, BMI_ODR_MASK);
	if (ret != EC_SUCCESS)
		goto accel_cleanup;

	/* Wait for the change to become effective */
	if (data->odr != 0)
		msleep(1000000 / MIN(data->odr, normalized_rate));
	/* Now that we have set the odr, update the driver's value. */
	data->odr = normalized_rate;

	/*
	 * FIFO start collecting events.
	 * They will be discarded if AP does not want them.
	 */
#ifdef CONFIG_ACCEL_FIFO
	bmi_enable_fifo(s, 1);
#endif
accel_cleanup:
	mutex_unlock(s->mutex);
	return ret;
}

static int set_offset(const struct motion_sensor_t *s,
			const int16_t *offset,
			int16_t    temp)
{
	int ret, val98, val_nv_conf;
	intv3_t v = { offset[X], offset[Y], offset[Z] };

	rotate_inv(v, *s->rot_standard_ref, v);

	ret = bmi_read8(s->port, s->addr,
			BMI260_OFFSET_EN_GYR98, &val98);
	if (ret)
		return ret;
	ret = bmi_read8(s->port, s->addr,
			BMI260_NV_CONF, &val_nv_conf);
	if (ret)
		return ret;

	switch (s->type) {
	case MOTIONSENSE_TYPE_ACCEL:
		bmi_set_accel_offset(s, v);
		ret = bmi_write8(s->port, s->addr,
				 BMI260_NV_CONF,
				 val_nv_conf | BMI260_ACC_OFFSET_EN);
		break;
	case MOTIONSENSE_TYPE_GYRO:
		bmi_set_gyro_offset(s, v, &val98);
		ret = bmi_write8(s->port, s->addr,
				 BMI260_OFFSET_EN_GYR98,
				 val98 | BMI260_OFFSET_GYRO_EN);
		break;
	default:
		ret = EC_RES_INVALID_PARAM;
	}
	return ret;
}

static int wait_and_read_data(const struct motion_sensor_t *s,
			      intv3_t v, int try_cnt, int msec)
{
	uint8_t data[6];
	int ret, status = 0;

	/* Check if data is ready */
	while (try_cnt && !(status & BMI260_DRDY_ACC)) {
		msleep(msec);
		ret = bmi_read8(s->port, s->addr,
				BMI260_STATUS, &status);
		if (ret)
			return ret;
		try_cnt -= 1;
	}
	if (!(status & BMI260_DRDY_ACC))
		return EC_ERROR_TIMEOUT;
	/* Read 6 bytes starting at xyz_reg */
	ret = bmi_read_n(s->port, s->addr,
			 bmi_get_xyz_reg(s), data, 6);
	bmi_normalize(s, v, data);
	return ret;
}

static int calibrate_offset(const struct motion_sensor_t *s,
			    int range, intv3_t target, int16_t *offset)
{
	int ret = EC_ERROR_UNKNOWN;
	int i, n_sample = 32;
	int data_diff[3] = {0};

	/* Manually offset compensation */
	for (i = 0; i < n_sample; ++i) {
		intv3_t v;
		/* Wait data for at most 3 * 10 msec */
		ret = wait_and_read_data(s, v, 3, 10);
		if (ret)
			return ret;
		data_diff[X] += v[X] - target[X];
		data_diff[Y] += v[Y] - target[Y];
		data_diff[Z] += v[Z] - target[Z];
	}

	/* The data LSB: 1000 * range / 32768 (mdps | mg)*/
	for (i = X; i <= Z; ++i)
		offset[i] -= ((int64_t)(data_diff[i] / n_sample) *
			     1000 * range) >> 15;
	return ret;
}

static int perform_calib(const struct motion_sensor_t *s)
{
	int ret, rate;
	int16_t temp;
	int16_t offset[3];
	intv3_t target = {0, 0, 0};
	/* Get sensor range for calibration*/
	int range = bmi_get_range(s);

	rate = bmi_get_data_rate(s);
	ret = set_data_rate(s, 100000, 0);
	if (ret)
		return ret;

	ret = bmi_get_offset(s, offset, &temp);
	if (ret)
		goto end_perform_calib;

	switch (s->type) {
	case MOTIONSENSE_TYPE_ACCEL:
		target[Z] = BMI260_ACC_DATA_PLUS_1G(range);
		break;
	case MOTIONSENSE_TYPE_GYRO:
		break;
	default:
		/* Not supported on Magnetometer */
		ret = EC_RES_INVALID_PARAM;
		goto end_perform_calib;
	}

	/* Get the calibrated offset */
	ret = calibrate_offset(s, range, target, offset);
	if (ret)
		goto end_perform_calib;

	ret = set_offset(s, offset, temp);
	if (ret)
		goto end_perform_calib;

end_perform_calib:
	if (ret == EC_ERROR_TIMEOUT)
		CPRINTS("%s timeout", __func__);
	set_data_rate(s, rate, 0);
	return ret;
}
#ifdef CONFIG_ACCEL_INTERRUPTS

/**
 * bmi260_interrupt - called when the sensor activates the interrupt line.
 *
 * This is a "top half" interrupt handler, it just asks motion sense ask
 * to schedule the "bottom half", ->irq_handler().
 */
void bmi260_interrupt(enum gpio_signal signal)
{
#ifdef CONFIG_ACCEL_FIFO
	last_interrupt_timestamp = __hw_clock_source_read();
#endif

	task_set_event(TASK_ID_MOTIONSENSE,
		       CONFIG_ACCELGYRO_BMI260_INT_EVENT, 0);
}

static int config_interrupt(const struct motion_sensor_t *s)
{
	int ret;

	if (s->type != MOTIONSENSE_TYPE_ACCEL)
		return EC_SUCCESS;

	mutex_lock(s->mutex);
	bmi_write8(s->port, s->addr,
		   BMI260_CMD_REG, BMI260_CMD_FIFO_FLUSH);

	/* configure int1 as an interrupt */
	ret = bmi_write8(s->port, s->addr,
			 BMI260_INT1_IO_CTRL,
			 BMI260_INT1_OUTPUT_EN);
#ifdef CONFIG_ACCELGYRO_BMI260_INT2_OUTPUT
	/* TODO(chingkang): Test it if we want int2 as an interrupt */
	/* configure int2 as an interrupt */
	ret = bmi_write8(s->port, s->addr,
			 BMI260_INT2_IO_CTRL,
			 BMI260_INT2_OUTPUT_EN);
#else
	/* configure int2 as an external input. */
	ret = bmi_write8(s->port, s->addr,
			 BMI260_INT2_IO_CTRL,
			 BMI260_INT2_INPUT_EN);
#endif

#ifdef CONFIG_ACCEL_FIFO
		/* map fifo water mark to int 1 */
		ret = bmi_write8(s->port, s->addr,
				 BMI260_INT_MAP_DATA,
				 BMI260_INT_MAP_DATA_REG(1, FWM) |
				 BMI260_INT_MAP_DATA_REG(1, FFULL));

		/*
		 * Configure fifo watermark to int whenever there's any data in
		 * there
		 */
		ret = bmi_write8(s->port, s->addr,
				 BMI260_FIFO_WTM_0, 1);
		ret = bmi_write8(s->port, s->addr,
				 BMI260_FIFO_WTM_1, 0);
#ifdef CONFIG_ACCELGYRO_BMI260_INT2_OUTPUT
		ret = bmi_write8(s->port, s->addr,
				 BMI260_FIFO_CONFIG_1,
				 BMI260_FIFO_HEADER_EN);
#else
		ret = bmi_write8(s->port, s->addr,
				 BMI260_FIFO_CONFIG_1,
				 (BMI260_FIFO_TAG_INT_LEVEL <<
				 BMI260_FIFO_TAG_INT2_EN_OFFSET) |
				 BMI260_FIFO_HEADER_EN);
#endif
		/* disable FIFO sensortime frame */
		ret = bmi_write8(s->port, s->addr,
				 BMI260_FIFO_CONFIG_0, 0);
#endif
	mutex_unlock(s->mutex);
	return ret;
}

/**
 * irq_handler - bottom half of the interrupt stack.
 * Ran from the motion_sense task, finds the events that raised the interrupt.
 *
 * For now, we just print out. We should set a bitmask motion sense code will
 * act upon.
 */
static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
{
	/* use uint16_t interrupt can cause error. */
	uint32_t interrupt = 0;
	int rv;

	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
			(!(*event & CONFIG_ACCELGYRO_BMI260_INT_EVENT)))
		return EC_ERROR_NOT_HANDLED;

	do {
		rv = bmi_read16(s->port, s->addr,
				BMI260_INT_STATUS_0, &interrupt);
		/*
		 * Bail out of this loop there was an error reading the register
		 */
		if (rv)
			return rv;
#ifdef CONFIG_ACCEL_FIFO
		if (interrupt & (BMI260_FWM_INT | BMI260_FFULL_INT)) {
			bmi_load_fifo(s, last_interrupt_timestamp);
		}
#endif
	} while (interrupt != 0);

	return EC_SUCCESS;
}
#endif  /* CONFIG_ACCEL_INTERRUPTS */

/*
 * TODO(b/160330682): Eliminate or reduce size of BMI260 initialization file.
 * Remove this option once the BMI260 initialization file is moved to the
 * kernel rootfs.
 */
#ifdef CONFIG_ACCELGYRO_BMI160_COMPRESSED_CONFIG

#define INCBIN_STYLE INCBIN_STYLE_SNAKE
#define INCBIN_PREFIX
#include "third_party/incbin/incbin.h"
INCBIN(bmi260_config,
	"third_party/bmi260/accelgyro_bmi260_config_compressed.bin");

#define COMPRESS_KEY		0xE9EA
#define BMI_BUFFER_SIZE		256
static uint8_t bmi_buffer[BMI_BUFFER_SIZE];
static int bmi_buffer_bytes;
static int bmi_config_offset;

static int write_bmi_data(const struct motion_sensor_t *s)
{
	uint8_t addr[2];
	int ret;

	if (bmi_buffer_bytes == 0)
		return EC_SUCCESS;

	addr[0] = (bmi_config_offset / 2) & 0xF;
	addr[1] = (bmi_config_offset / 2) >> 4;
	ret = bmi_write_n(s->port, s->addr,
			  BMI260_INIT_ADDR_0, addr, 2);
	if (ret)
		return ret;

	ret = bmi_write_n(s->port, s->addr,
			  BMI260_INIT_DATA, bmi_buffer,
			  bmi_buffer_bytes);
	if (ret)
		return ret;

	bmi_config_offset += bmi_buffer_bytes;
	bmi_buffer_bytes = 0;

	return EC_SUCCESS;
}

/*
 * Stores 4 bytes of BMI configuration data into a static buffer. If the buffer
 * is filled, write the buffer contents over I2C.
 */
static int enqueue_bmi_data(const struct motion_sensor_t *s, uint32_t *data)
{
	int ret;

	memcpy(&bmi_buffer[bmi_buffer_bytes], data, 4);
	bmi_buffer_bytes += 4;

	if (bmi_buffer_bytes >= BMI_BUFFER_SIZE) {
		ret = write_bmi_data(s);
		if (ret)
			return ret;
	}

	return EC_SUCCESS;
}

/*
 * Load the BMI configuration data from a compressed buffer.
 *
 * Compression scheme:
 *	Repeated 32-bit words are replaced by a 16-bit key, 16-bit count, and
 *	the 32-bit data word. All values stored big-endian.
 *
 *	For example, if the uncompressed file had the following data words:
 *		0x89ABCDEF 0x89ABCDEF 0x89ABCDEF
 *
 *	This is represented compressed as (key 0xE9EA):
 *		0xE9EA0003 0x89ABCDEF
 *
 *	Key value (0xE9EA) chosen as it wasn't found in the BMI configuration
 *	data.
 */
int bmi_compressed_config_load(const struct motion_sensor_t *s)
{
	uint32_t *bmi_compressed_config = (uint32_t *)bmi260_config_data;
	int length = bmi260_config_size;
	int i;
	int output_offset;
	uint16_t *buf16;
	uint16_t key;
	uint16_t repeat_count;
	uint32_t *data32;
	int ret;

	length /= sizeof(uint32_t);

	bmi_buffer_bytes = 0;
	bmi_config_offset = 0;

	output_offset = 0;

	for (i = 0; i < length; i++) {
		buf16 = (uint16_t *)&bmi_compressed_config[i];
		key = be16toh(*buf16);

		if (key == COMPRESS_KEY) {
			repeat_count = be16toh(*++buf16);

			if (repeat_count == 0) {
				CPRINTF("BMI260 config: invalid repeat count "
					"found at word offset %d\n",
					i);
				return EC_ERROR_UNKNOWN;
			}

			/*
			 * Advance to the next word in the buffer, which
			 * contains the actual data to write.
			 */
			if (++i >= length) {
				CPRINTF("BMI260 config: "
					"Unexpected end of file\n");
				return EC_ERROR_UNKNOWN;
			}

			data32 = &bmi_compressed_config[i];

			while (repeat_count-- > 0) {
				ret = enqueue_bmi_data(s, data32);
				if (ret)
					return ret;

				output_offset += 4;
			}
		} else {
			data32 = &bmi_compressed_config[i];
			ret = enqueue_bmi_data(s, data32);
			if (ret)
				return ret;

			output_offset += 4;
		}
	}

	ret = write_bmi_data(s);

	return ret;
}

static int bmi_config_load(const struct motion_sensor_t *s)
{
	return EC_ERROR_UNKNOWN;
}
#else /* !CONFIG_ACCELGYRO_BMI160_COMPRESSED_CONFIG */
static int bmi_compressed_config_load(const struct motion_sensor_t *s)
{
	return EC_ERROR_UNKNOWN;
}

static int bmi_config_load(const struct motion_sensor_t *s)
{
	int ret;
	uint16_t i;

	/*
	 * Due to i2c transaction timeout limit,
	 * burst_write_len should not be above 2048 to prevent timeout.
	 */
	const int burst_write_len = 2048;
	/* We have to write the config even bytes of data every time */
	BUILD_ASSERT((burst_write_len & 1) == 0);

	for (i = 0; i < g_bmi260_config_tbin_len; i += burst_write_len) {
		uint8_t addr[2];
		const int len = MIN(burst_write_len,
				    g_bmi260_config_tbin_len - i);

		addr[0] = (i / 2) & 0xF;
		addr[1] = (i / 2) >> 4;
		ret = bmi_write_n(s->port, s->addr,
				  BMI260_INIT_ADDR_0, addr, 2);
		if (ret)
			break;
		ret = bmi_write_n(s->port, s->addr,
				  BMI260_INIT_DATA, &g_bmi260_config_tbin[i],
				  len);
		if (ret)
			return ret;
;
	}

	return EC_SUCCESS;
}

#endif /* CONFIG_ACCELGYRO_BMI160_COMPRESSED_CONFIG */

static int init_config(const struct motion_sensor_t *s)
{
	int init_status, ret;
	uint16_t i;

	/* disable advance power save but remain fifo self wakeup*/
	bmi_write8(s->port, s->addr, BMI260_PWR_CONF, 2);
	msleep(1);
	/* prepare for config load */
	bmi_write8(s->port, s->addr, BMI260_INIT_CTRL, 0);

	/* load config file to INIT_DATA */
	if (IS_ENABLED(CONFIG_ACCELGYRO_BMI160_COMPRESSED_CONFIG))
		ret = bmi_compressed_config_load(s);
	else
		ret = bmi_config_load(s);

	/* finish config load */
	bmi_write8(s->port, s->addr, BMI260_INIT_CTRL, 1);
	/* return error if load config failed */
	if (ret)
		return ret;
	/* wait INTERNAL_STATUS.message to be 0x1 which take at most 150ms */
	for (i = 0; i < 15; ++i) {
		msleep(10);
		ret = bmi_read8(s->port, s->addr,
			BMI260_INTERNAL_STATUS, &init_status);
		if (ret)
			break;
		init_status &= BMI260_MESSAGE_MASK;
		if (init_status == BMI260_INIT_OK)
			break;
	}
	if (ret || init_status != BMI260_INIT_OK)
		return EC_ERROR_INVALID_CONFIG;
	return EC_SUCCESS;
}

static int init(const struct motion_sensor_t *s)
{
	int ret = 0, tmp, i;
	struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s);

	ret = bmi_read8(s->port, s->addr,
			BMI260_CHIP_ID, &tmp);
	if (ret)
		return EC_ERROR_UNKNOWN;

	if (tmp != BMI260_CHIP_ID_MAJOR)
		return EC_ERROR_ACCESS_DENIED;

	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
		struct bmi_drv_data_t *data = BMI_GET_DATA(s);

		/* Reset the chip to be in a good state */
		bmi_write8(s->port, s->addr,
				 BMI260_CMD_REG, BMI260_CMD_SOFT_RESET);
		msleep(2);
		if (init_config(s))
			return EC_ERROR_INVALID_CONFIG;

		data->flags &= ~(BMI_FLAG_SEC_I2C_ENABLED |
				(BMI_FIFO_ALL_MASK <<
				 BMI_FIFO_FLAG_OFFSET));
	}

	for (i = X; i <= Z; i++)
		saved_data->scale[i] = MOTION_SENSE_DEFAULT_SCALE;
	/*
	 * The sensor is in Suspend mode at init,
	 * so set data rate to 0.
	 */
	saved_data->odr = 0;
	bmi_set_range(s, s->default_range, 0);

	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
#ifdef CONFIG_ACCEL_INTERRUPTS
		ret = config_interrupt(s);
#endif
	}

	return sensor_init_done(s);
}

const struct accelgyro_drv bmi260_drv = {
	.init = init,
	.read = bmi_read,
	.set_range = bmi_set_range,
	.get_range = bmi_get_range,
	.get_resolution = bmi_get_resolution,
	.set_data_rate = set_data_rate,
	.get_data_rate = bmi_get_data_rate,
	.set_offset = set_offset,
	.get_scale = bmi_get_scale,
	.set_scale = bmi_set_scale,
	.get_offset = bmi_get_offset,
	.perform_calib = perform_calib,
#ifdef CONFIG_ACCEL_INTERRUPTS
	.irq_handler = irq_handler,
#endif
#ifdef CONFIG_GESTURE_HOST_DETECTION
	.list_activities = bmi_list_activities,
#endif
};

#ifdef CONFIG_CMD_I2C_STRESS_TEST_ACCEL
struct i2c_stress_test_dev bmi260_i2c_stress_test_dev = {
	.reg_info = {
		.read_reg = BMI260_CHIP_ID,
		.read_val = BMI260_CHIP_ID_MAJOR,
		.write_reg = BMI260_PMU_TRIGGER,
	},
	.i2c_read = &bmi_read8,
	.i2c_write = &bmi_write8,
};
#endif /* CONFIG_CMD_I2C_STRESS_TEST_ACCEL */