Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 2e085115 authored by Bingzhe Cai's avatar Bingzhe Cai
Browse files

input: sensor: enable MPU6050 motion detection 



When MPU6050 is run in interrupt mode, its build in accelerometer will
stay powered on and the device is woken up when significant motion is
detected. This is part of sensor low power implementation.

Change-Id: I979d586413fd364d5fc787888fbd2369dcab83c9
Signed-off-by: default avatarBingzhe Cai <bingzhec@codeaurora.org>
parent e33b9fba

drivers/input/misc/mpu6050.c

+97 −41
Original line number Diff line number Diff line
@@ -261,7 +261,8 @@ static const u8 mpu_accel_fs_shift[NUM_ACCL_FSR] = { 
/* Function declarations */

static void mpu6050_pinctrl_state(struct mpu6050_sensor *sensor,

			bool active);

static int mpu6050_set_drdy_int(struct mpu6050_sensor *sensor, bool on);

static int mpu6050_set_interrupt(struct mpu6050_sensor *sensor,

		const u8 mask, bool on);





static int mpu6050_power_ctl(struct mpu6050_sensor *sensor, bool on)
@@ -847,7 +848,8 @@ static int mpu6050_gyro_enable(struct mpu6050_sensor *sensor, bool on) 
		}



		if (!sensor->cfg.int_enabled) {

			ret = mpu6050_set_drdy_int(sensor, true);

			ret = mpu6050_set_interrupt(sensor,

				BIT_DATA_RDY_EN, true);

			if (ret < 0) {

				dev_err(&sensor->client->dev,

					"Fail to enable interrupt mode for gyro, ret=%d\n",
@@ -861,7 +863,8 @@ static int mpu6050_gyro_enable(struct mpu6050_sensor *sensor, bool on) 
		sensor->cfg.enable = 1;

	} else {

		if (sensor->cfg.int_enabled && !sensor->cfg.accel_enable) {

			ret = mpu6050_set_drdy_int(sensor, false);

			ret = mpu6050_set_interrupt(sensor,

				BIT_DATA_RDY_EN, false);

			if (ret < 0) {

				dev_err(&sensor->client->dev,

					"Fail to disable interrupt mode for gyro, ret=%d\n",
@@ -1089,10 +1092,10 @@ exit: 
}



/*

  * Configure the sensor interrupt register that allow sensor to

  * issue interrupt for each new sensor event.

  * Set interrupt enabling bits to enable/disable specific type of interrupt.

  */

static int mpu6050_set_drdy_int(struct mpu6050_sensor *sensor, bool on)

static int mpu6050_set_interrupt(struct mpu6050_sensor *sensor,

		const u8 mask, bool on)

{

	int ret;

	u8 data;
@@ -1100,7 +1103,6 @@ static int mpu6050_set_drdy_int(struct mpu6050_sensor *sensor, bool on) 
	if (sensor->cfg.is_asleep)

		return -EINVAL;



	if (on) {

	ret = i2c_smbus_read_byte_data(sensor->client,

				sensor->reg.int_enable);

	if (ret < 0) {
@@ -1109,26 +1111,14 @@ static int mpu6050_set_drdy_int(struct mpu6050_sensor *sensor, bool on) 
		return ret;

	}



	if (on) {

		data = (u8)ret;

		data |= BIT_DATA_RDY_EN;

		ret = i2c_smbus_write_byte_data(sensor->client,

				sensor->reg.int_enable, data);

		if (ret < 0) {

			dev_err(&sensor->client->dev,

				"Fail to set interrupt. ret=%d\n", ret);

			return ret;

		}

		data |= mask;

	} else {

		ret = i2c_smbus_read_byte_data(sensor->client,

					sensor->reg.int_enable);

		if (ret < 0) {

			dev_err(&sensor->client->dev,

				"Fail read interrupt mode. ret=%d\n", ret);

			return ret;

		data = (u8)ret;

		data &= ~mask;

	}



		data = (u8)ret;

		data &= ~BIT_DATA_RDY_EN;

	ret = i2c_smbus_write_byte_data(sensor->client,

			sensor->reg.int_enable, data);

	if (ret < 0) {
@@ -1136,8 +1126,42 @@ static int mpu6050_set_drdy_int(struct mpu6050_sensor *sensor, bool on) 
			"Fail to set interrupt. ret=%d\n", ret);

		return ret;

	}

	return 0;

}



/*

  * Enable/disable motion detection interrupt.

  */

static int mpu6050_set_motion_det(struct mpu6050_sensor *sensor, bool on)

{

	int ret;



	if (on) {

		ret = i2c_smbus_write_byte_data(sensor->client,

				sensor->reg.mot_thr, DEFAULT_MOT_THR);

		if (ret < 0)

			goto err_exit;



		ret = i2c_smbus_write_byte_data(sensor->client,

				sensor->reg.mot_dur, DEFAULT_MOT_DET_DUR);

		if (ret < 0)

			goto err_exit;



	}



	ret = mpu6050_set_interrupt(sensor, BIT_MOT_EN, on);

	if (ret < 0)

		goto err_exit;



	sensor->cfg.mot_det_on = on;

	/* Use default motion detection delay 4ms */



	return 0;



err_exit:

	dev_err(&sensor->client->dev,

			"Fail to set motion detection. ret=%d\n", ret);

	return ret;

}



/* Update sensor sample rate divider upon accel and gyro polling rate. */
@@ -1364,7 +1388,8 @@ static int mpu6050_accel_enable(struct mpu6050_sensor *sensor, bool on) 
		}



		if (!sensor->cfg.int_enabled) {

			ret = mpu6050_set_drdy_int(sensor, true);

			ret = mpu6050_set_interrupt(sensor,

				BIT_DATA_RDY_EN, true);

			if (ret < 0) {

				dev_err(&sensor->client->dev,

					"Fail to enable interrupt mode for accel, ret=%d\n",
@@ -1378,7 +1403,8 @@ static int mpu6050_accel_enable(struct mpu6050_sensor *sensor, bool on) 
		sensor->cfg.enable = 1;

	} else {

		if (sensor->cfg.int_enabled && !sensor->cfg.gyro_enable) {

			ret = mpu6050_set_drdy_int(sensor, false);

			ret = mpu6050_set_interrupt(sensor,

				BIT_DATA_RDY_EN, false);

			if (ret < 0) {

				dev_err(&sensor->client->dev,

					"Fail to disable interrupt mode for accel, ret=%d\n",
@@ -1701,6 +1727,8 @@ static void setup_mpu6050_reg(struct mpu_reg_map *reg) 
	reg->fifo_en		= REG_FIFO_EN;

	reg->gyro_config	= REG_GYRO_CONFIG;

	reg->accel_config	= REG_ACCEL_CONFIG;

	reg->mot_thr		= REG_ACCEL_MOT_THR;

	reg->mot_dur		= REG_ACCEL_MOT_DUR;

	reg->fifo_count_h	= REG_FIFO_COUNT_H;

	reg->fifo_r_w		= REG_FIFO_R_W;

	reg->raw_gyro		= REG_RAW_GYRO;
@@ -2302,9 +2330,27 @@ static int mpu6050_suspend(struct device *dev) 
	int ret = 0;



	mutex_lock(&sensor->op_lock);

	if (!sensor->use_poll)

	if (sensor->cfg.accel_enable && !sensor->use_poll) {

		/* keep accel on and config motion detection wakeup */

		ret = mpu6050_set_interrupt(sensor,

				BIT_DATA_RDY_EN, false);

		if (ret == 0)

			ret = mpu6050_set_motion_det(sensor, true);

		if (ret == 0) {

			irq_set_irq_wake(client->irq, 1);



			dev_dbg(&client->dev,

				"Enable motion detection success\n");

			goto exit;

		}

		/*  if motion detection config does not success,

		  *  not exit suspend and sensor will be power off.

		  */

	}



	if (!sensor->use_poll) {

		disable_irq(client->irq);

	else {

	} else {

		if (sensor->cfg.gyro_enable)

			cancel_delayed_work_sync(&sensor->gyro_poll_work);
@@ -2339,6 +2385,16 @@ static int mpu6050_resume(struct device *dev) 
	struct mpu6050_sensor *sensor = i2c_get_clientdata(client);

	int ret = 0;



	if (sensor->cfg.mot_det_on) {

		/* keep accel on and config motion detection wakeup */

		irq_set_irq_wake(client->irq, 0);

		mpu6050_set_motion_det(sensor, false);

		mpu6050_set_interrupt(sensor,

				BIT_DATA_RDY_EN, true);

		dev_dbg(&client->dev, "Disable motion detection success\n");

		goto exit;

	}



	/* Keep sensor power on to prevent bad power state */

	ret = mpu6050_power_ctl(sensor, true);

	if (ret < 0) {

drivers/input/misc/mpu6050.h

+14 −0
Original line number Diff line number Diff line
@@ -28,6 +28,10 @@ 
#define REG_ACCEL_MOT_DUR	0x20

#define ACCL_CONFIG_FSR_SHIFT	3



#define REG_ACCELMOT_THR	0x1F



#define REG_ACCEL_MOT_DUR	0x20



#define REG_FIFO_EN		0x23

#define BIT_ACCEL_OUT		0x08

#define BITS_GYRO_OUT		0x70
@@ -68,6 +72,9 @@ 
#define BIT_ACCEL_FIFO		0x08

#define BIT_GYRO_FIFO		0x70



#define REG_DETECT_CTRL		0x69

#define MOT_DET_DELAY_SHIFT	4



#define REG_PWR_MGMT_1		0x6B

#define BIT_H_RESET		0x80

#define BIT_SLEEP		0x40
@@ -110,6 +117,9 @@ 


/* initial configure */

#define INIT_FIFO_RATE		50

#define DEFAULT_MOT_THR		1

#define DEFAULT_MOT_DET_DUR	1

#define DEFAULT_MOT_DET_DELAY	0



/* chip reset wait */

#define MPU6050_RESET_RETRY_CNT	10
@@ -185,6 +195,7 @@ enum inv_devices { 
 *  @fifo_en:	Determines which data will appear in FIFO.

 *  @gyro_config:	gyro config register.

 *  @accel_config:	accel config register

 *  @mot_thr:	Motion detection threshold.

 *  @fifo_count_h:	Upper byte of FIFO count.

 *  @fifo_r_w:	FIFO register.

 *  @raw_gyro:	Address of first gyro register.
@@ -204,6 +215,8 @@ struct mpu_reg_map { 
	u8 gyro_config;

	u8 accel_config;

	u8 fifo_count_h;

	u8 mot_thr;

	u8 mot_dur;

	u8 fifo_r_w;

	u8 raw_gyro;

	u8 raw_accel;
@@ -247,6 +260,7 @@ struct mpu_chip_config { 
	u32 tap_on:1;

	u32 flick_int_on:1;

	u32 int_enabled:1;

	u32 mot_det_on:1;

	u8 int_pin_cfg;

	u16 lpa_freq;

	u16 rate_div;