From 5bfcbbd79993dfb0ee7e7f3a916fa4ef6ec9042b Mon Sep 17 00:00:00 2001 From: Zorro Liu Date: Wed, 31 Aug 2016 15:33:19 +0800 Subject: [PATCH] driver, inv_mpu: store register suspend and recover register resume, if hardware poweroff when suspend Change-Id: I758be101acfd7a3a756c75bd4f13542daef850d1 Signed-off-by: Zorro Liu --- .../staging/iio/imu/inv_mpu/inv_mpu_core.c | 51 ++++++++++++++++ drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c | 60 +++++++++++++++++-- drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h | 4 +- drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c | 60 +++++++++++++++++-- 4 files changed, 162 insertions(+), 13 deletions(-) diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c index 58dcb08236ec..471317f7b59c 100644 --- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c +++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c @@ -77,6 +77,57 @@ static const struct inv_hw_s hw_info[INV_NUM_PARTS] = { {128, "MPU6515"}, }; +/* define INV_REG_NUM_MAX the max register unmbers of all sensor type */ +#define INV_REG_NUM_MAX 128 +static u8 inv_reg_data[128] = {0}; +/** + * inv_reg_store() - Register store for hw poweroff. + */ +int inv_reg_store(struct inv_mpu_iio_s *st) +{ + int ii; + char data; + + if (!st->chip_config.enable) + st->set_power_state(st, true); + for (ii = 0; ii < st->hw->num_reg; ii++) { + /* don't read fifo r/w register */ + if (ii == st->reg.fifo_r_w) + data = 0; + else + inv_plat_read(st, ii, 1, &data); + inv_reg_data[ii] = data; + } + if (!st->chip_config.enable) + st->set_power_state(st, false); + + return 0; +} + +/** + * inv_reg_recover() - Register recover for hw poweroff. + */ +int inv_reg_recover(struct inv_mpu_iio_s *st) +{ + int ii; + char data; + + if (!st->chip_config.enable) + st->set_power_state(st, true); + for (ii = 0; ii < st->hw->num_reg; ii++) { + data = inv_reg_data[ii]; + /* don't write fifo r/w register */ + if (ii == st->reg.fifo_r_w) + continue; + else + inv_plat_single_write(st, ii, data); + } + if (!st->chip_config.enable) + st->set_power_state(st, false); + + return 0; +} + static void inv_setup_reg(struct inv_reg_map_s *reg) { reg->sample_rate_div = REG_SAMPLE_RATE_DIV; diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c index 95232da1b803..38bd151a3e6f 100644 --- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c +++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c @@ -342,10 +342,13 @@ static int of_inv_parse_platform_data(struct i2c_client *client, int orig_x, orig_y, orig_z; int i; struct device_node *np = client->dev.of_node; + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_mpu_iio_s *st = iio_priv(indio_dev); unsigned long irq_flags; int irq_pin; int gpio_pin; int debug; + int hw_pwoff; gpio_pin = of_get_named_gpio_flags(np, "irq-gpio", 0, (enum of_gpio_flags *)&irq_flags); gpio_request(gpio_pin, "mpu6500"); @@ -417,6 +420,12 @@ static int of_inv_parse_platform_data(struct i2c_client *client, mpu_data.orientation[i] = -1; } + ret = of_property_read_u32(np, "support-hw-poweroff", &hw_pwoff); + if (ret != 0) { + st->support_hw_poweroff = 0; + } + st->support_hw_poweroff = hw_pwoff; + ret = of_property_read_u32(np, "mpu-debug", &debug); if (ret != 0) { dev_err(&client->dev, "get mpu-debug error\n"); @@ -465,6 +474,7 @@ static int inv_mpu_probe(struct i2c_client *client, st->client = client; st->sl_handle = client->adapter; st->i2c_addr = client->addr; + i2c_set_clientdata(client, indio_dev); if (client->dev.of_node) { result = of_inv_parse_platform_data(client, &st->plat_data); if (result) @@ -502,7 +512,6 @@ static int inv_mpu_probe(struct i2c_client *client, } /* Make state variables available to all _show and _store functions. */ - i2c_set_clientdata(client, indio_dev); indio_dev->dev.parent = &client->dev; if (!strcmp(id->name, "mpu6xxx")) indio_dev->name = st->name; @@ -615,21 +624,60 @@ static int inv_mpu_remove(struct i2c_client *client) #ifdef CONFIG_PM static int inv_mpu_resume(struct device *dev) { - struct inv_mpu_iio_s *st = - iio_priv(i2c_get_clientdata(to_i2c_client(dev))); + struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev)); + struct inv_mpu_iio_s *st = iio_priv(indio_dev); + int result; + pr_debug("%s inv_mpu_resume\n", st->hw->name); - return st->set_power_state(st, true); + + if (st->support_hw_poweroff) { + mutex_lock(&indio_dev->mlock); + /* reset to make sure previous state are not there */ + result = inv_plat_single_write(st, st->reg.pwr_mgmt_1, BIT_H_RESET); + if (result) { + pr_err("%s, reset failed\n", __func__); + goto rw_err; + } + msleep(POWER_UP_TIME); + /* toggle power state */ + result = st->set_power_state(st, false); + if (result) { + pr_err("%s, set_power_state false failed\n", __func__); + goto rw_err; + } + result = st->set_power_state(st, true); + if (result) { + pr_err("%s, set_power_state true failed\n", __func__); + goto rw_err; + } + result = inv_plat_single_write(st, st->reg.user_ctrl, st->i2c_dis); + if (result) { + pr_err("%s, set user_ctrl failed\n", __func__); + goto rw_err; + } + inv_reg_recover(st); + mutex_unlock(&indio_dev->mlock); + } else { + result = st->set_power_state(st, true); + } + return result; + +rw_err: + mutex_unlock(&indio_dev->mlock); + return result; } static int inv_mpu_suspend(struct device *dev) { struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev)); struct inv_mpu_iio_s *st = iio_priv(indio_dev); - int result; + int result = 0; pr_debug("%s inv_mpu_suspend\n", st->hw->name); + mutex_lock(&indio_dev->mlock); - result = 0; + if (st->support_hw_poweroff) + inv_reg_store(st); if ((!st->chip_config.dmp_on) || (!st->chip_config.enable) || (!st->chip_config.dmp_event_int_on)) diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h b/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h index 0722c6397ab0..aa3335320bcc 100644 --- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h +++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h @@ -365,6 +365,7 @@ struct inv_mpu_iio_s { signed short hid_temperature; u64 hid_timestamp; int use_hid; + int support_hw_poweroff; int accel_bias[3]; int gyro_bias[3]; short raw_gyro[3]; @@ -859,7 +860,8 @@ ssize_t inv_dmp_firmware_read(struct file *filp, struct kobject *kobj, struct bin_attribute *bin_attr, char *buf, loff_t off, size_t count); - +int inv_reg_store(struct inv_mpu_iio_s *st); +int inv_reg_recover(struct inv_mpu_iio_s *st); int inv_mpu_configure_ring(struct iio_dev *indio_dev); int inv_mpu_probe_trigger(struct iio_dev *indio_dev); void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev); diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c index e95802f0be40..21c80cdeb3f6 100644 --- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c +++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c @@ -190,11 +190,14 @@ static int of_inv_parse_platform_data(struct spi_device *spi, u32 orientation[9]; int orig_x, orig_y, orig_z; int i; + struct iio_dev *indio_dev = spi_get_drvdata(spi); + struct inv_mpu_iio_s *st = iio_priv(indio_dev); struct device_node *np = spi->dev.of_node; unsigned long irq_flags; int irq_pin; int gpio_pin; int debug; + int hw_pwoff; gpio_pin = of_get_named_gpio_flags(np, "irq-gpio", 0, (enum of_gpio_flags *)&irq_flags); gpio_request(gpio_pin, "mpu6500"); @@ -264,6 +267,12 @@ static int of_inv_parse_platform_data(struct spi_device *spi, mpu_data.orientation[i] = -1; } + ret = of_property_read_u32(np, "support-hw-poweroff", &hw_pwoff); + if (ret != 0) { + st->support_hw_poweroff = 0; + } + st->support_hw_poweroff = hw_pwoff; + ret = of_property_read_u32(np, "mpu-debug", &debug); if (ret != 0) { dev_err(&spi->dev, "get mpu-debug error\n"); @@ -312,6 +321,7 @@ static int inv_mpu_probe(struct spi_device *spi) goto out_no_free; } st = iio_priv(indio_dev); + spi_set_drvdata(spi, indio_dev); if (spi->dev.of_node) { result = of_inv_parse_platform_data(spi, &st->plat_data); if (result) @@ -323,7 +333,6 @@ static int inv_mpu_probe(struct spi_device *spi) *(struct mpu_platform_data *)dev_get_platdata(&spi->dev); /* Make state variables available to all _show and _store functions. */ - spi_set_drvdata(spi, indio_dev); indio_dev->dev.parent = &spi->dev; st->dev = &spi->dev; st->irq = spi->irq; @@ -461,21 +470,60 @@ static int inv_mpu_remove(struct spi_device *spi) #ifdef CONFIG_PM static int inv_mpu_resume(struct device *dev) { - struct inv_mpu_iio_s *st = - iio_priv(spi_get_drvdata(to_spi_device(dev))); + struct iio_dev *indio_dev = spi_get_drvdata(to_spi_device(dev)); + struct inv_mpu_iio_s *st = iio_priv(indio_dev); + int result; + pr_debug("%s inv_mpu_resume\n", st->hw->name); - return st->set_power_state(st, true); + + if (st->support_hw_poweroff) { + mutex_lock(&indio_dev->mlock); + /* reset to make sure previous state are not there */ + result = inv_plat_single_write(st, st->reg.pwr_mgmt_1, BIT_H_RESET); + if (result) { + pr_err("%s, reset failed\n", __func__); + goto rw_err; + } + msleep(POWER_UP_TIME); + /* toggle power state */ + result = st->set_power_state(st, false); + if (result) { + pr_err("%s, set_power_state false failed\n", __func__); + goto rw_err; + } + result = st->set_power_state(st, true); + if (result) { + pr_err("%s, set_power_state true failed\n", __func__); + goto rw_err; + } + result = inv_plat_single_write(st, st->reg.user_ctrl, st->i2c_dis); + if (result) { + pr_err("%s, set user_ctrl failed\n", __func__); + goto rw_err; + } + inv_reg_recover(st); + mutex_unlock(&indio_dev->mlock); + } else { + result = st->set_power_state(st, true); + } + return result; + +rw_err: + mutex_unlock(&indio_dev->mlock); + return result; } static int inv_mpu_suspend(struct device *dev) { struct iio_dev *indio_dev = spi_get_drvdata(to_spi_device(dev)); struct inv_mpu_iio_s *st = iio_priv(indio_dev); - int result; + int result = 0; pr_debug("%s inv_mpu_suspend\n", st->hw->name); + mutex_lock(&indio_dev->mlock); - result = 0; + if (st->support_hw_poweroff) + inv_reg_store(st); if ((!st->chip_config.dmp_on) || (!st->chip_config.enable) || (!st->chip_config.dmp_event_int_on)) -- 2.34.1