1 files changed, 24 insertions, 46 deletions

diff --git a/drivers/misc/m4sensorhub_als.c b/drivers/misc/m4sensorhub_als.c
index b5f90fd72a3..84cb2ee8cfa 100644
--- a/drivers/misc/m4sensorhub_als.c
+++ b/drivers/misc/m4sensorhub_als.c
@@ -40,18 +40,21 @@ struct m4als_driver_data {
 	struct m4sensorhub_data     *m4;
 	struct mutex                mutex; /* controls driver entry points */
 	struct input_dev            *indev;
+	struct delayed_work         m4als_work;
 
 	uint16_t        luminosity;
 	int16_t         samplerate;
 	int16_t         latest_samplerate;
+	int16_t         fastest_rate;
 	uint16_t        status;
 };
 
-
-static void m4als_isr(enum m4sensorhub_irqs int_event, void *handle)
+static void m4als_work_func(struct work_struct *work)
 {
 	int err = 0;
-	struct m4als_driver_data *dd = handle;
+	struct m4als_driver_data *dd = container_of(work,
+						    struct m4als_driver_data,
+						    m4als_work.work);
 	int size = 0;
 	uint16_t luminosity = 0;
 
@@ -80,6 +83,9 @@ static void m4als_isr(enum m4sensorhub_irqs int_event, void *handle)
 
 	input_event(dd->indev, EV_MSC, MSC_RAW, dd->luminosity);
 	input_sync(dd->indev);
+	if (dd->samplerate > 0)
+		schedule_delayed_work(&(dd->m4als_work),
+				      msecs_to_jiffies(dd->samplerate));
 
 m4als_isr_fail:
 	if (err < 0)
@@ -95,12 +101,15 @@ static int m4als_set_samplerate(struct m4als_driver_data *dd, int16_t rate)
 	int err = 0;
 	int size = 0;
 
+	if ((rate >= 0) && (rate <= dd->fastest_rate))
+		rate = dd->fastest_rate;
+
 	/* This variabled is to be always updated ireespective of the
 	   transaction status */
 	dd->latest_samplerate = rate;
 
 	if (rate == dd->samplerate)
-		goto m4als_change_interrupt_bit;
+		goto m4als_set_samplerate_fail;
 
 	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_LIGHTSENSOR_SAMPLERATE);
 	if (size < 0) {
@@ -121,32 +130,11 @@ static int m4als_set_samplerate(struct m4als_driver_data *dd, int16_t rate)
 		err = -EBADE;
 		goto m4als_set_samplerate_fail;
 	}
+	cancel_delayed_work(&(dd->m4als_work));
 	dd->samplerate = rate;
-
-m4als_change_interrupt_bit:
-	if (rate == -1) {
-		if (dd->status & (1 << M4ALS_IRQ_ENABLED_BIT)) {
-			err = m4sensorhub_irq_disable(dd->m4,
-				M4SH_IRQ_LIGHTSENSOR_DATA_READY);
-			if (err < 0) {
-				m4als_err("%s: Failed to disable interrupt.\n",
-					  __func__);
-				goto m4als_set_samplerate_fail;
-			}
-			dd->status = dd->status & ~(1 << M4ALS_IRQ_ENABLED_BIT);
-		}
-	} else {
-		 if (!(dd->status & (1 << M4ALS_IRQ_ENABLED_BIT))) {
-			err = m4sensorhub_irq_enable(dd->m4,
-				M4SH_IRQ_LIGHTSENSOR_DATA_READY);
-			if (err < 0) {
-				m4als_err("%s: Failed to enable interrupt.\n",
-					  __func__);
-				goto m4als_set_samplerate_fail;
-			}
-			dd->status = dd->status | (1 << M4ALS_IRQ_ENABLED_BIT);
-		}
-	}
+	if (dd->samplerate > 0)
+		schedule_delayed_work(&(dd->m4als_work),
+				      msecs_to_jiffies(rate));
 
 m4als_set_samplerate_fail:
 	return err;
@@ -296,7 +284,10 @@ static void m4als_panic_restore(struct m4sensorhub_data *m4sensorhub,
 		m4als_err("%s: Wrote %d bytes instead of %d.\n",
 			  __func__, err, size);
 	}
-
+	cancel_delayed_work(&(dd->m4als_work));
+	if (dd->samplerate > 0)
+		schedule_delayed_work(&(dd->m4als_work),
+				      msecs_to_jiffies(dd->samplerate));
 	mutex_unlock(&(dd->mutex));
 }
 
@@ -319,12 +310,7 @@ static int m4als_driver_init(struct init_calldata *p_arg)
 		goto m4als_driver_init_sysfs_fail;
 	}
 
-	err = m4sensorhub_irq_register(dd->m4, M4SH_IRQ_LIGHTSENSOR_DATA_READY,
-		m4als_isr, dd, 0);
-	if (err < 0) {
-		m4als_err("%s: Failed to register M4 IRQ.\n", __func__);
-		goto m4als_driver_init_irq_fail;
-	}
+	INIT_DELAYED_WORK(&(dd->m4als_work), m4als_work_func);
 
 	err = m4sensorhub_panic_register(dd->m4, PANICHDL_ALS_RESTORE,
 					 m4als_panic_restore, dd);
@@ -332,8 +318,6 @@ static int m4als_driver_init(struct init_calldata *p_arg)
 		KDEBUG(M4SH_ERROR, "Als panic callback register failed\n");
 	goto m4als_driver_init_exit;
 
-m4als_driver_init_irq_fail:
-	m4als_remove_sysfs(dd);
 m4als_driver_init_sysfs_fail:
 	input_unregister_device(dd->indev);
 m4als_driver_init_fail:
@@ -360,6 +344,7 @@ static int m4als_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, dd);
 	dd->samplerate = -1; /* We always start disabled */
 	dd->latest_samplerate = dd->samplerate;
+	dd->fastest_rate = 80; /* in milli secs */
 
 	dd->m4 = m4sensorhub_client_get_drvdata();
 	if (dd->m4 == NULL) {
@@ -389,13 +374,8 @@ static int __exit m4als_remove(struct platform_device *pdev)
 	struct m4als_driver_data *dd = platform_get_drvdata(pdev);
 
 	mutex_lock(&(dd->mutex));
+	cancel_delayed_work(&(dd->m4als_work));
 	m4als_remove_sysfs(dd);
-	if (dd->status & (1 << M4ALS_IRQ_ENABLED_BIT)) {
-		m4sensorhub_irq_disable(dd->m4,
-					M4SH_IRQ_LIGHTSENSOR_DATA_READY);
-		dd->status = dd->status & ~(1 << M4ALS_IRQ_ENABLED_BIT);
-	}
-	m4sensorhub_irq_unregister(dd->m4, M4SH_IRQ_LIGHTSENSOR_DATA_READY);
 	m4sensorhub_unregister_initcall(m4als_driver_init);
 	if (dd->indev != NULL)
 		input_unregister_device(dd->indev);
@@ -408,10 +388,8 @@ static int __exit m4als_remove(struct platform_device *pdev)
 static int m4als_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct m4als_driver_data *dd = platform_get_drvdata(pdev);
-	mutex_lock(&(dd->mutex));
 	if (m4als_set_samplerate(dd, dd->latest_samplerate) < 0)
 		m4als_err("%s: setrate retry failed\n", __func__);
-	mutex_unlock(&(dd->mutex));
 	return 0;
 }