Motor: ztm620: add pm_notifier

Add PM Notifier in order to control the state of motor device.
When PM_SUSPEND_PREPARE, disable the motor and in case of PM_POST_SUSPEND,
enable motor device. And do refactor the code for the readability.

Change-Id: I6e99c72ba4af08d614f8969a1bf9890614fe73b7
Signed-off-by: jeongsup.jeong <jeongsup.jeong@samsung.com>
[cw00.choi: Add commig-msg as the public style]
Signed-off-by: Chanwoo Choi <cw00.choi@samsung.com>

diff --git a/drivers/motor/ztm620_motor.c b/drivers/motor/ztm620_motor.c
index 61b39dac4a37d4453b6562451b094410fce925ea..c85f6091db92ca0f5b1db8f3aaf33052e63ad3bc 100644 (file)
--- a/drivers/motor/ztm620_motor.c
+++ b/drivers/motor/ztm620_motor.c
@@ -55,6 +55,8 @@ static struct device *sec_motor;
 #endif
 
 static struct ztm620_motor_data *g_Ztm620MotorData = NULL;
+static bool is_suspend = false;
+static unsigned char pm_noti_test = 0;
 
 static int ztm620_motor_reg_read(struct ztm620_motor_data *pMotorData, unsigned char reg)
 {
@@ -114,105 +116,109 @@ static int ztm620_motor_run(void) {
        else
                pMotorPdata = &pMotorData->msPlatData;
 
-       if (pMotorData->level) {
-               ret = ztm620_motor_reg_write(pMotorData, MOTOR_REG_SOFT_EN, SOFT_ENABLE);
-               if (ret < 0) {
-                       pr_err("[VIB] %s SOFT_EN write fail %d\n",
-                                               __func__, ret);
-                       goto out;
-               }
-               if (pMotorPdata->soft_en_delay)
-                       msleep(pMotorPdata->soft_en_delay);
+       if (pMotorData->level == 0) {
+               pr_err("[VIB] %s: level 0\n", __func__);
+               goto out;
+       }
 
-               if (pMotorData->level >= MAX_LEVEL)
-                       strength = pMotorPdata->strength_strong;
-               else
-                       strength = pMotorPdata->strength_weak;
+       ret = ztm620_motor_reg_write(pMotorData, MOTOR_REG_SOFT_EN, SOFT_ENABLE);
+       if (ret < 0) {
+               pr_err("[VIB] %s SOFT_EN write fail %d\n",
+                                       __func__, ret);
+               goto out;
+       }
+       if (pMotorPdata->soft_en_delay)
+               msleep(pMotorPdata->soft_en_delay);
 
-               ret = ztm620_motor_reg_write(pMotorData, MOTOR_REG_STRENGTH, strength);
-               if (ret < 0) {
-                       pr_err("[VIB] %s STRENGTH write %d fail %d\n",
-                                               __func__, strength, ret);
-                       goto out;
-               }
+       if (pMotorData->level >= MAX_LEVEL)
+               strength = pMotorPdata->strength_strong;
+       else
+               strength = pMotorPdata->strength_weak;
 
-               val = (pMotorPdata->meLoop << MODE_01_SHIFT_DRV_MODE );
-               ret = ztm620_motor_set_bits(pMotorData,
-                       MOTOR_REG_MODE_01, (1 << MODE_01_SHIFT_DRV_MODE), val);
-               if (ret < 0) {
-                       pr_err("[VIB] %s MODE_01 0x%02x write fail (%d)\n",
-                                               __func__, MOTOR_REG_MODE_01, ret);
-                       goto out;
-               }
+       ret = ztm620_motor_reg_write(pMotorData, MOTOR_REG_STRENGTH, strength);
+       if (ret < 0) {
+               pr_err("[VIB] %s STRENGTH write %d fail %d\n",
+                                       __func__, strength, ret);
+               goto out;
+       }
 
-               /* Driving Period = DRV_FREQ * 16 Clocks
-                  -> DRV_FREQ = Driving Period / (16 Clocks * Clock Period)
-                                Clock Frequency / (16 Clocks * Driving Frequency)
-                  DRV_FREQ_H[7:0] = DRV_FREQ[15:8]
-                  DRV_FREQ_L[7:0] = DRV_FREQ[7:0]                               */
-               if (pMotorData->level >= MAX_LEVEL)
-                       freq = MOTOR_CLK / (16 * pMotorPdata->freq_strong);
-               else
-                       freq = MOTOR_CLK / (16 * pMotorPdata->freq_weak);
+       val = (pMotorPdata->meLoop << MODE_01_SHIFT_DRV_MODE );
+       ret = ztm620_motor_set_bits(pMotorData,
+               MOTOR_REG_MODE_01, (1 << MODE_01_SHIFT_DRV_MODE), val);
+       if (ret < 0) {
+               pr_err("[VIB] %s MODE_01 0x%02x write fail (%d)\n",
+                                       __func__, MOTOR_REG_MODE_01, ret);
+               goto out;
+       }
 
-               ret = ztm620_motor_reg_write(pMotorData,
-                               MOTOR_REG_DRV_FREQ_H, freq / 256);
-               if (ret < 0) {
-                       pr_err("[VIB] %s DRV_FREQ_H 0x%02x write fail (%d)\n",
-                                               __func__, MOTOR_REG_DRV_FREQ_H, ret);
-                       goto out;
-               }
+       /* Driving Period = DRV_FREQ * 16 Clocks
+          -> DRV_FREQ = Driving Period / (16 Clocks * Clock Period)
+                        Clock Frequency / (16 Clocks * Driving Frequency)
+          DRV_FREQ_H[7:0] = DRV_FREQ[15:8]
+          DRV_FREQ_L[7:0] = DRV_FREQ[7:0]                               */
+       if (pMotorData->level >= MAX_LEVEL)
+               freq = MOTOR_CLK / (16 * pMotorPdata->freq_strong);
+       else
+               freq = MOTOR_CLK / (16 * pMotorPdata->freq_weak);
 
-               ret = ztm620_motor_reg_write(pMotorData,
-                               MOTOR_REG_DRV_FREQ_L, freq % 256);
-               if (ret < 0) {
-                       pr_err("[VIB] %s DRV_FREQ_L 0x%02x write fail (%d)\n",
-                                               __func__, MOTOR_REG_DRV_FREQ_L, ret);
-                       goto out;
-               }
+       ret = ztm620_motor_reg_write(pMotorData,
+                       MOTOR_REG_DRV_FREQ_H, freq / 256);
+       if (ret < 0) {
+               pr_err("[VIB] %s DRV_FREQ_H 0x%02x write fail (%d)\n",
+                                       __func__, MOTOR_REG_DRV_FREQ_H, ret);
+               goto out;
+       }
 
-               ret = ztm620_motor_reg_write(pMotorData,
-                               MOTOR_REG_RESO_FREQ_H, freq / 256);
-               if (ret < 0) {
-                       pr_err("[VIB] %s RESO_FREQ_H 0x%02x write fail (%d)\n",
-                                               __func__, MOTOR_REG_RESO_FREQ_H, ret);
-                       goto out;
-               }
+       ret = ztm620_motor_reg_write(pMotorData,
+                       MOTOR_REG_DRV_FREQ_L, freq % 256);
+       if (ret < 0) {
+               pr_err("[VIB] %s DRV_FREQ_L 0x%02x write fail (%d)\n",
+                                       __func__, MOTOR_REG_DRV_FREQ_L, ret);
+               goto out;
+       }
 
-               ret = ztm620_motor_reg_write(pMotorData,
-                               MOTOR_REG_RESO_FREQ_L, freq % 256);
-               if (ret < 0) {
-                       pr_err("[VIB] %s RESO_FREQ_L 0x%02x write fail (%d)\n",
-                                               __func__, MOTOR_REG_RESO_FREQ_L, ret);
-                       goto out;
+       ret = ztm620_motor_reg_write(pMotorData,
+                       MOTOR_REG_RESO_FREQ_H, freq / 256);
+       if (ret < 0) {
+               pr_err("[VIB] %s RESO_FREQ_H 0x%02x write fail (%d)\n",
+                                       __func__, MOTOR_REG_RESO_FREQ_H, ret);
+               goto out;
+       }
+
+       ret = ztm620_motor_reg_write(pMotorData,
+                       MOTOR_REG_RESO_FREQ_L, freq % 256);
+       if (ret < 0) {
+               pr_err("[VIB] %s RESO_FREQ_L 0x%02x write fail (%d)\n",
+                                       __func__, MOTOR_REG_RESO_FREQ_L, ret);
+               goto out;
+       }
+
+       val = pMotorPdata->adc_sampling_time;
+       ret = ztm620_motor_reg_write(pMotorData,
+                                       MOTOR_REG_ADC_SAMPLING_TIME, val);
+       if (ret < 0) {
+               pr_err("[VIB] %s ADC_SAMPLING_TIME write fail %d\n",
+                                       __func__, ret);
+               goto out;
+       }
+
+       if (!pMotorData->running) {
+               if (pMotorData->gpio_en > 0) {
+                       gpio_set_value(pMotorData->gpio_en, 1);
                }
 
-               val = pMotorPdata->adc_sampling_time;
                ret = ztm620_motor_reg_write(pMotorData,
-                                               MOTOR_REG_ADC_SAMPLING_TIME, val);
+                                       MOTOR_REG_MODE_00, MODE_00_I2C);
                if (ret < 0) {
-                       pr_err("[VIB] %s ADC_SAMPLING_TIME write fail %d\n",
-                                               __func__, ret);
+                       pr_err("[VIB] %s MODE_00 write fail %d\n",
+                                                       __func__, ret);
                        goto out;
                }
-
-               if (!pMotorData->running) {
-                       if (pMotorData->gpio_en > 0) {
-                               gpio_set_value(pMotorData->gpio_en, 1);
-                       }
-
-                       ret = ztm620_motor_reg_write(pMotorData,
-                                               MOTOR_REG_MODE_00, MODE_00_I2C);
-                       if (ret < 0) {
-                               pr_err("[VIB] %s MODE_00 write fail %d\n",
-                                                               __func__, ret);
-                               goto out;
-                       }
-                       pMotorData->running = true;
-               }
-               pr_info("[VIB] %s Run: level:0x%x freq:0x%x str:0x%x\n",
-                               __func__, pMotorData->level, freq, strength);
+               pMotorData->running = true;
        }
+       pr_info("[VIB] %s Run: level:0x%x freq:0x%x str:0x%x\n",
+                       __func__, pMotorData->level, freq, strength);
+
 out:
        return ret;
 }
@@ -231,20 +237,19 @@ static void vibrator_work_routine(struct work_struct *work)
        }
 
        mutex_lock(&pMotorData->lock);
+       if (is_suspend) goto out;
 
        if (pMotorData->level) {
                err = ztm620_motor_run();
                if (err < 0) {
-                       pr_err("[VIB] %s motor run fail %d\n",
-                                               __func__, err);
+                       pr_err("[VIB] %s motor run fail %d\n", __func__, err);
                        goto out;
                }
        }
        else if (pMotorData->running) {
                err = ztm620_motor_reg_write(pMotorData, MOTOR_REG_MODE_00, MODE_00_STOP);
                if (err < 0) {
-                       pr_err("[VIB] %s MODE_00 write fail %d\n",
-                                               __func__, err);
+                       pr_err("[VIB] %s MODE_00 write fail %d\n", __func__, err);
                        goto out;
                }
 
@@ -423,6 +428,91 @@ static void ztm620_motor_trigger_init(struct work_struct *work)
        mutex_unlock(&pMotorData->lock);
 }
 
+static int ztm620_motor_pm_notifier(struct notifier_block *notifier,
+                                       unsigned long pm_event, void *v)
+{
+       int err;
+       struct ztm620_motor_data *pMotorData = container_of(notifier,
+                                                      struct ztm620_motor_data,
+                                                      ztm620_motor_pm_nb);
+
+       if (pMotorData == NULL) {
+               pr_err("[VIB] %s ztm620_motor_data NULL error\n", __func__);
+               goto out;
+       }
+
+       switch (pm_event) {
+       case PM_SUSPEND_PREPARE:
+               mutex_lock(&pMotorData->lock);
+               if (pMotorData->running)
+                       pr_warn("[VIB] %s: motor is running", __func__);
+               else if (pm_noti_test) {
+                       pMotorData->level = 0;
+                       pr_info("[VIB] %s: test suspend stop", __func__);
+               }
+               else {
+                       is_suspend = true;
+                       mutex_unlock(&pMotorData->lock);
+                       break;
+               }
+
+               err = ztm620_motor_reg_write(pMotorData,
+                               MOTOR_REG_MODE_00, MODE_00_STOP);
+               if (err < 0) {
+                       pr_err("[VIB] %s MODE_00 write fail %d", __func__, err);
+                       goto out_err;
+               }
+
+               if (pMotorData->gpio_en > 0)
+                       gpio_set_value(pMotorData->gpio_en, 0);
+
+               err = ztm620_motor_reg_write(pMotorData,
+                               MOTOR_REG_SOFT_EN, SOFT_DISABLE);
+               if (err < 0) {
+                       pr_err("[VIB] %s SOFT_EN write fail %d", __func__, err);
+                       goto out_err;
+               }
+               pr_info("[VIB] %s Stop", __func__);
+
+               pMotorData->running = false;
+               pMotorData->last_motor_off = CURRENT_TIME;
+               is_suspend = true;
+               mutex_unlock(&pMotorData->lock);
+
+               break;
+       case PM_POST_SUSPEND:
+               mutex_lock(&pMotorData->lock);
+               is_suspend = false;
+
+               if (pMotorData->level)
+                       pr_warn("[VIB] %s: motor is to run", __func__);
+               else if (pm_noti_test) {
+                       pMotorData->level = MAX_LEVEL;
+                       pr_info("[VIB] %s test resume run", __func__);
+               }
+               else {
+                       mutex_unlock(&pMotorData->lock);
+                       break;
+               }
+
+               err = ztm620_motor_run();
+               if (err < 0) {
+                       pr_err("[VIB] %s motor run fail %d",
+                                       __func__, err);
+                       goto out_err;
+               }
+               mutex_unlock(&pMotorData->lock);
+
+               break;
+       }
+
+       return NOTIFY_OK;
+out_err:
+       mutex_unlock(&pMotorData->lock);
+out:
+       return NOTIFY_BAD;
+}
+
 static int Haptics_init(struct ztm620_motor_data *pMotorData)
 {
        int ret = 0;
@@ -458,6 +548,9 @@ static int Haptics_init(struct ztm620_motor_data *pMotorData)
        wake_lock_init(&pMotorData->wklock, WAKE_LOCK_SUSPEND, "vibrator");
        mutex_init(&pMotorData->lock);
 
+       pMotorData->ztm620_motor_pm_nb.notifier_call = ztm620_motor_pm_notifier;
+       register_pm_notifier(&pMotorData->ztm620_motor_pm_nb);
+
        return 0;
 err_destroy_ff:
        input_ff_destroy(input_dev);
@@ -785,8 +878,8 @@ int ztm620_motor_debug_init(void)
        debugfs_create_file("dump_regs", S_IRUSR | S_IWUSR, d, NULL, &ztm620_motor_dump_regs_fops);
        debugfs_create_u8("addr", S_IRUSR | S_IWUSR, d, &ztm620_motor_debugfs_addr);
        debugfs_create_file("data", S_IRUSR | S_IWUSR, d, NULL, &ztm620_motor_debugfs_data_fops);
-       if (g_Ztm620MotorData->gpio_en > 0)
-               debugfs_create_file("enable", S_IRUSR | S_IWUSR, d, NULL, &ztm620_motor_debugfs_enable);
+       debugfs_create_file("enable", S_IRUSR | S_IWUSR, d, NULL, &ztm620_motor_debugfs_enable);
+       debugfs_create_u8("pm_noti_test", S_IRUSR | S_IWUSR, d, &pm_noti_test);
 
        return 0;
 }

diff --git a/include/linux/ztm620_motor.h b/include/linux/ztm620_motor.h
index 2ff8695faf389149515765dda2fe687acb825683..9efa64d1c31d02bc6daaa9667b771c83092d4ffc 100644 (file)
--- a/include/linux/ztm620_motor.h
+++ b/include/linux/ztm620_motor.h
@@ -145,6 +145,8 @@ struct ztm620_motor_data {
        bool running;
        struct regulator *regulator;
        int gpio_en;
+
+       struct notifier_block ztm620_motor_pm_nb;
 };
 
 int ztm620_motor_reset_handler(void);