Motor: ztm620: add pm_notifier

TODO

Change-Id: I149410be0d38924867feea98dd6aa9da48d163b0
Signed-off-by: jeongsup.jeong <jeongsup.jeong@samsung.com>
[cw00.choi: Cherry-pick commit 36764495ab76 ("Motor: ztm620: add pm_notifier")
from slp-info.sec.samsung.net and add commit-msg]
Signed-off-by: Chanwoo Choi <cw00.choi@samsung.com>

2 files changed, 185 insertions, 90 deletions

diff --git a/drivers/motor/ztm620_motor.c b/drivers/motor/ztm620_motor.c
index 61b39dac4a37..c85f6091db92 100644
--- 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 2ff8695faf38..9efa64d1c31d 100644
--- 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);