diff --git a/drivers/misc/sx933x/sx933x_sar.c b/drivers/misc/sx933x/sx933x_sar.c index d7af9368035e..4ccc22bec114 100644 --- a/drivers/misc/sx933x/sx933x_sar.c +++ b/drivers/misc/sx933x/sx933x_sar.c @@ -1044,6 +1044,11 @@ static int ps_notify_callback(struct notifier_block *self, static void write_flip_regs(int num_regs, struct smtc_reg_data *regs) { int i; + u32 temp = 0; + + /* Disable if we are on */ + sx933x_i2c_read_16bit(global_sx933x, SX933X_GNRLCTRL2_REG, &temp); + sx933x_i2c_write_16bit(global_sx933x, SX933X_GNRLCTRL2_REG, temp & 0xFFFFFFE0); for(i=0; i < num_regs; i++) { @@ -1053,6 +1058,14 @@ static void write_flip_regs(int num_regs, struct smtc_reg_data *regs) sx933x_i2c_write_16bit(global_sx933x, regs[i].reg, regs[i].val); } + + /* If one of the sensors is on, re-enable it */ + for (i=0; i < ARRAY_SIZE(psmtcButtons); i++) { + if (psmtcButtons[i].enabled) { + sx933x_i2c_write_16bit(global_sx933x, SX933X_GNRLCTRL2_REG, temp | 0x0000001F); + break; + } + } } static void update_flip_regs(struct sx933x_platform_data *data, unsigned long state)