diff options
| -rw-r--r-- | drivers/misc/m4sensorhub_als.c | 70 | ||||
| -rw-r--r-- | drivers/misc/m4sensorhub_fusion.c | 71 | ||||
| -rw-r--r-- | drivers/misc/m4sensorhub_heartrate.c | 74 | ||||
| -rw-r--r-- | drivers/misc/m4sensorhub_mpu9150.c | 313 | ||||
| -rw-r--r-- | drivers/misc/m4sensorhub_pedometer.c | 121 |
5 files changed, 196 insertions, 453 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; } diff --git a/drivers/misc/m4sensorhub_fusion.c b/drivers/misc/m4sensorhub_fusion.c index c8d579fe4fe..f1ce277f2fe 100644 --- a/drivers/misc/m4sensorhub_fusion.c +++ b/drivers/misc/m4sensorhub_fusion.c @@ -45,17 +45,20 @@ struct m4fus_driver_data { struct mutex mutex; /* controls driver entry points */ struct m4sensorhub_fusion_iio_data iiodat[M4FUS_NUM_FUSION_BUFFERS]; + struct delayed_work m4fus_work; int16_t samplerate; int16_t latest_samplerate; + int16_t fastest_rate; uint16_t status; }; - -static void m4fus_isr(enum m4sensorhub_irqs int_event, void *handle) +static void m4fus_work_func(struct work_struct *work) { int err = 0; - struct iio_dev *iio = handle; - struct m4fus_driver_data *dd = iio_priv(iio); + struct m4fus_driver_data *dd = container_of(work, + struct m4fus_driver_data, + m4fus_work.work); + struct iio_dev *iio = platform_get_drvdata(dd->pdev); int size = 0; mutex_lock(&(dd->mutex)); @@ -129,6 +132,9 @@ static void m4fus_isr(enum m4sensorhub_irqs int_event, void *handle) * in this one call (see the M4 passive driver). */ iio_push_to_buffers(iio, (unsigned char *)&(dd->iiodat[0])); + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4fus_work), + msecs_to_jiffies(dd->samplerate)); m4fus_isr_fail: if (err < 0) @@ -145,9 +151,13 @@ static int m4fus_set_samplerate(struct iio_dev *iio, int16_t rate) struct m4fus_driver_data *dd = iio_priv(iio); int size = 0; + if ((rate >= 0) && (rate <= dd->fastest_rate)) + rate = dd->fastest_rate; + dd->latest_samplerate = rate; + if (rate == dd->samplerate) - goto m4fus_set_samplerate_irq_check; + goto m4fus_set_samplerate_fail; size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_FUSION_SAMPLERATE); err = m4sensorhub_reg_write(dd->m4, M4SH_REG_FUSION_SAMPLERATE, @@ -162,33 +172,10 @@ static int m4fus_set_samplerate(struct iio_dev *iio, int16_t rate) goto m4fus_set_samplerate_fail; } dd->samplerate = rate; - -m4fus_set_samplerate_irq_check: - if (rate >= 0) { - /* Enable the IRQ if necessary */ - if (!(dd->status & (1 << M4FUS_IRQ_ENABLED_BIT))) { - err = m4sensorhub_irq_enable(dd->m4, - M4SH_IRQ_FUSION_DATA_READY); - if (err < 0) { - m4fus_err("%s: Failed to enable irq.\n", - __func__); - goto m4fus_set_samplerate_fail; - } - dd->status = dd->status | (1 << M4FUS_IRQ_ENABLED_BIT); - } - } else { - /* Disable the IRQ if necessary */ - if (dd->status & (1 << M4FUS_IRQ_ENABLED_BIT)) { - err = m4sensorhub_irq_disable(dd->m4, - M4SH_IRQ_FUSION_DATA_READY); - if (err < 0) { - m4fus_err("%s: Failed to disable irq.\n", - __func__); - goto m4fus_set_samplerate_fail; - } - dd->status = dd->status & ~(1 << M4FUS_IRQ_ENABLED_BIT); - } - } + cancel_delayed_work(&(dd->m4fus_work)); + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4fus_work), + msecs_to_jiffies(rate)); m4fus_set_samplerate_fail: return err; @@ -383,6 +370,10 @@ static void m4fus_panic_restore(struct m4sensorhub_data *m4sensorhub, __func__, err, size); } + cancel_delayed_work(&(dd->m4fus_work)); + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4fus_work), + msecs_to_jiffies(dd->samplerate)); mutex_unlock(&(dd->mutex)); } @@ -401,12 +392,7 @@ static int m4fus_driver_init(struct init_calldata *p_arg) goto m4fus_driver_init_fail; } - err = m4sensorhub_irq_register(dd->m4, - M4SH_IRQ_FUSION_DATA_READY, m4fus_isr, iio, 0); - if (err < 0) { - m4fus_err("%s: Failed to register M4 IRQ.\n", __func__); - goto m4fus_driver_init_fail; - } + INIT_DELAYED_WORK(&(dd->m4fus_work), m4fus_work_func); err = m4sensorhub_panic_register(dd->m4, PANICHDL_FUSION_RESTORE, m4fus_panic_restore, dd); @@ -441,6 +427,7 @@ static int m4fus_probe(struct platform_device *pdev) platform_set_drvdata(pdev, iio); dd->samplerate = -1; /* We always start disabled */ dd->latest_samplerate = dd->samplerate; + dd->fastest_rate = 40; err = m4fus_create_iiodev(iio); /* iio and dd are freed on fail */ if (err < 0) { @@ -476,11 +463,7 @@ static int __exit m4fus_remove(struct platform_device *pdev) goto m4fus_remove_exit; mutex_lock(&(dd->mutex)); - if (dd->status & (1 << M4FUS_IRQ_ENABLED_BIT)) { - m4sensorhub_irq_disable(dd->m4, M4SH_IRQ_FUSION_DATA_READY); - dd->status = dd->status & ~(1 << M4FUS_IRQ_ENABLED_BIT); - } - m4sensorhub_irq_unregister(dd->m4, M4SH_IRQ_FUSION_DATA_READY); + cancel_delayed_work(&(dd->m4fus_work)); m4sensorhub_unregister_initcall(m4fus_driver_init); m4fus_remove_iiodev(iio); /* dd is freed here */ @@ -492,10 +475,8 @@ static int m4fus_suspend(struct platform_device *pdev, pm_message_t state) { struct iio_dev *iio = platform_get_drvdata(pdev); struct m4fus_driver_data *dd = iio_priv(iio); - mutex_lock(&(dd->mutex)); if (m4fus_set_samplerate(iio, dd->latest_samplerate) < 0) m4fus_err("%s: setrate retry failed\n", __func__); - mutex_unlock(&(dd->mutex)); return 0; } diff --git a/drivers/misc/m4sensorhub_heartrate.c b/drivers/misc/m4sensorhub_heartrate.c index 99b4a3d81f8..01fc5f210b7 100644 --- a/drivers/misc/m4sensorhub_heartrate.c +++ b/drivers/misc/m4sensorhub_heartrate.c @@ -46,19 +46,22 @@ struct m4hrt_driver_data { struct mutex mutex; /* controls driver entry points */ struct m4sensorhub_heartrate_iio_data iiodat; + struct delayed_work m4hrt_work; int32_t samplerate; int32_t latest_samplerate; + int32_t fastest_rate; uint16_t status; uint8_t dbg_addr; }; - -static void m4hrt_isr(enum m4sensorhub_irqs int_event, void *handle) +static void m4hrt_work_func(struct work_struct *work) { int err = 0; - struct iio_dev *iio = handle; - struct m4hrt_driver_data *dd = iio_priv(iio); + struct m4hrt_driver_data *dd = container_of(work, + struct m4hrt_driver_data, + m4hrt_work.work); + struct iio_dev *iio = platform_get_drvdata(dd->pdev); int size = 0; mutex_lock(&(dd->mutex)); @@ -91,6 +94,9 @@ static void m4hrt_isr(enum m4sensorhub_irqs int_event, void *handle) dd->iiodat.timestamp = iio_get_time_ns(); iio_push_to_buffers(iio, (unsigned char *)&(dd->iiodat)); + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4hrt_work), + msecs_to_jiffies(dd->samplerate)); m4hrt_isr_fail: if (err < 0) @@ -107,10 +113,13 @@ static int m4hrt_set_samplerate(struct iio_dev *iio, int32_t rate) struct m4hrt_driver_data *dd = iio_priv(iio); int size = 0; + if ((rate >= 0) && (rate <= dd->fastest_rate)) + rate = dd->fastest_rate; + dd->latest_samplerate = rate; if (rate == dd->samplerate) - goto m4hrt_set_samplerate_irq_check; + goto m4hrt_set_samplerate_fail; size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_HEARTRATE_APSAMPLERATE); @@ -125,35 +134,11 @@ static int m4hrt_set_samplerate(struct iio_dev *iio, int32_t rate) err = -EBADE; goto m4hrt_set_samplerate_fail; } + cancel_delayed_work(&(dd->m4hrt_work)); dd->samplerate = rate; - -m4hrt_set_samplerate_irq_check: - if (rate >= 0) { - /* Enable the IRQ if necessary */ - if (!(dd->status & (1 << M4HRT_IRQ_ENABLED_BIT))) { - err = m4sensorhub_irq_enable(dd->m4, - M4SH_IRQ_HEARTRATE_DATA_READY); - if (err < 0) { - m4hrt_err("%s: Failed to enable irq.\n", - __func__); - goto m4hrt_set_samplerate_fail; - } - dd->status = dd->status | (1 << M4HRT_IRQ_ENABLED_BIT); - } - } else { - /* Disable the IRQ if necessary */ - if (dd->status & (1 << M4HRT_IRQ_ENABLED_BIT)) { - err = m4sensorhub_irq_disable(dd->m4, - M4SH_IRQ_HEARTRATE_DATA_READY); - if (err < 0) { - m4hrt_err("%s: Failed to disable irq.\n", - __func__); - goto m4hrt_set_samplerate_fail; - } - dd->status = dd->status & ~(1 << M4HRT_IRQ_ENABLED_BIT); - } - } - + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4hrt_work), + msecs_to_jiffies(dd->samplerate)); m4hrt_set_samplerate_fail: return err; } @@ -504,7 +489,10 @@ static void m4hrt_panic_restore(struct m4sensorhub_data *m4sensorhub, m4hrt_err("%s: Wrote %d bytes instead of %d.\n", __func__, err, size); } - + cancel_delayed_work(&(dd->m4hrt_work)); + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4hrt_work), + msecs_to_jiffies(dd->samplerate)); mutex_unlock(&(dd->mutex)); } @@ -523,12 +511,7 @@ static int m4hrt_driver_init(struct init_calldata *p_arg) goto m4hrt_driver_init_fail; } - err = m4sensorhub_irq_register(dd->m4, - M4SH_IRQ_HEARTRATE_DATA_READY, m4hrt_isr, iio, 0); - if (err < 0) { - m4hrt_err("%s: Failed to register M4 IRQ.\n", __func__); - goto m4hrt_driver_init_fail; - } + INIT_DELAYED_WORK(&(dd->m4hrt_work), m4hrt_work_func); err = m4sensorhub_panic_register(dd->m4, PANICHDL_HEARTRATE_RESTORE, m4hrt_panic_restore, dd); @@ -563,6 +546,7 @@ static int m4hrt_probe(struct platform_device *pdev) platform_set_drvdata(pdev, iio); dd->samplerate = -1; /* We always start disabled */ dd->latest_samplerate = dd->samplerate; + dd->fastest_rate = 80; /* in milli secs */ err = m4hrt_create_iiodev(iio); /* iio and dd are freed on fail */ if (err < 0) { @@ -598,13 +582,7 @@ static int __exit m4hrt_remove(struct platform_device *pdev) goto m4hrt_remove_exit; mutex_lock(&(dd->mutex)); - if (dd->status & (1 << M4HRT_IRQ_ENABLED_BIT)) { - m4sensorhub_irq_disable(dd->m4, - M4SH_IRQ_HEARTRATE_DATA_READY); - dd->status = dd->status & ~(1 << M4HRT_IRQ_ENABLED_BIT); - } - m4sensorhub_irq_unregister(dd->m4, - M4SH_IRQ_HEARTRATE_DATA_READY); + cancel_delayed_work(&(dd->m4hrt_work)); m4sensorhub_unregister_initcall(m4hrt_driver_init); m4hrt_remove_iiodev(iio); /* dd is freed here */ @@ -616,10 +594,8 @@ static int m4hrt_suspend(struct platform_device *pdev, pm_message_t state) { struct iio_dev *iio = platform_get_drvdata(pdev); struct m4hrt_driver_data *dd = iio_priv(iio); - mutex_lock(&(dd->mutex)); if (m4hrt_set_samplerate(iio, dd->latest_samplerate) < 0) m4hrt_err("%s: setrate retry failed\n", __func__); - mutex_unlock(&(dd->mutex)); return 0; } diff --git a/drivers/misc/m4sensorhub_mpu9150.c b/drivers/misc/m4sensorhub_mpu9150.c index 0d852d4bdd4..52de21885d4 100644 --- a/drivers/misc/m4sensorhub_mpu9150.c +++ b/drivers/misc/m4sensorhub_mpu9150.c @@ -77,12 +77,11 @@ struct mpu9150_client { struct mpu9150_gyro_data gyro_data; struct mpu9150_compass_data compass_data; struct mutex mutex; /* prevent concurrent thread access */ + struct delayed_work mpu9150_work[NUM_TYPES]; + signed short fastest_rate[NUM_TYPES]; }; struct mpu9150_client *misc_mpu9150_data; -static int mpu9150_irq_enable_disable(struct mpu9150_client *mpu9150_client_data, - enum mpu9150_sensor type, int flag); - static int mpu9150_client_open(struct inode *inode, struct file *file) { int err = 0; @@ -144,22 +143,30 @@ static void m4_report_mpu9150_inputevent( static void m4_set_mpu9150_delay(struct mpu9150_client *mpu9150_client_data, int delay, enum mpu9150_sensor type) { - mpu9150_client_data->latest_samplerate[type] = delay; + struct mpu9150_client *dd = mpu9150_client_data; + + if ((delay >= 0) && (delay <= dd->fastest_rate[type])) + delay = dd->fastest_rate[type]; - if (delay != mpu9150_client_data->samplerate[type]) { + dd->latest_samplerate[type] = delay; + if (delay != dd->samplerate[type]) { switch (type) { case TYPE_GYRO: - m4sensorhub_reg_write(mpu9150_client_data->m4sensorhub, - M4SH_REG_GYRO_SAMPLERATE, (char *)&delay, m4sh_no_mask); + m4sensorhub_reg_write(dd->m4sensorhub, + M4SH_REG_GYRO_SAMPLERATE, + (char *)&delay, m4sh_no_mask); break; case TYPE_ACCEL: - m4sensorhub_reg_write(mpu9150_client_data->m4sensorhub, - M4SH_REG_ACCEL_SAMPLERATE, (char *)&delay, m4sh_no_mask); + m4sensorhub_reg_write(dd->m4sensorhub, + M4SH_REG_ACCEL_SAMPLERATE, + (char *)&delay, m4sh_no_mask); break; case TYPE_COMPASS: - m4sensorhub_reg_write(mpu9150_client_data->m4sensorhub, - M4SH_REG_COMPASS_SAMPLERATE, (char *)&delay, m4sh_no_mask); + m4sensorhub_reg_write(dd->m4sensorhub, + M4SH_REG_COMPASS_SAMPLERATE, + (char *)&delay, m4sh_no_mask); break; + default: return; break; @@ -168,8 +175,12 @@ static void m4_set_mpu9150_delay(struct mpu9150_client *mpu9150_client_data, " %d to %d\n", __func__, type, mpu9150_client_data->samplerate[type], delay); + cancel_delayed_work(&(dd->mpu9150_work[type])); + dd->samplerate[type] = delay; + if (dd->samplerate[type] > 0) + schedule_delayed_work(&(dd->mpu9150_work[type]), + msecs_to_jiffies(delay)); - mpu9150_client_data->samplerate[type] = delay; } } @@ -224,48 +235,64 @@ static void m4_read_mpu9150_data(struct mpu9150_client *mpu9150_client_data, default: break; } - } -static void m4_handle_mpu9150_gyro_irq(enum m4sensorhub_irqs int_event, - void *mpu9150_data) +static void m4gyro_work_func(struct work_struct *work) { - struct mpu9150_client *mpu9150_client_data = mpu9150_data; - - mutex_lock(&(mpu9150_client_data->mutex)); - - m4_read_mpu9150_data(mpu9150_client_data, TYPE_GYRO); - m4_report_mpu9150_inputevent(mpu9150_client_data, TYPE_GYRO); + struct mpu9150_client *dd = + container_of(work, + struct mpu9150_client, + mpu9150_work[TYPE_GYRO].work); + signed short rate; - mutex_unlock(&(mpu9150_client_data->mutex)); + mutex_lock(&(dd->mutex)); + m4_read_mpu9150_data(dd, TYPE_GYRO); + m4_report_mpu9150_inputevent(dd, TYPE_GYRO); + rate = dd->samplerate[TYPE_GYRO]; + if (rate > 0) + schedule_delayed_work(&(dd->mpu9150_work[TYPE_GYRO]), + msecs_to_jiffies(rate)); + mutex_unlock(&(dd->mutex)); } -static void m4_handle_mpu9150_accel_irq(enum m4sensorhub_irqs int_event, - void *mpu9150_data) -{ - struct mpu9150_client *mpu9150_client_data = mpu9150_data; - - mutex_lock(&(mpu9150_client_data->mutex)); - m4_read_mpu9150_data(mpu9150_client_data, TYPE_ACCEL); - m4_report_mpu9150_inputevent(mpu9150_client_data, TYPE_ACCEL); +static void m4accel_work_func(struct work_struct *work) +{ + struct mpu9150_client *dd = + container_of(work, + struct mpu9150_client, + mpu9150_work[TYPE_ACCEL].work); + signed short rate; - mutex_unlock(&(mpu9150_client_data->mutex)); + mutex_lock(&(dd->mutex)); + m4_read_mpu9150_data(dd, TYPE_ACCEL); + m4_report_mpu9150_inputevent(dd, TYPE_ACCEL); + rate = dd->samplerate[TYPE_ACCEL]; + if (rate > 0) + schedule_delayed_work(&(dd->mpu9150_work[TYPE_ACCEL]), + msecs_to_jiffies(rate)); + mutex_unlock(&(dd->mutex)); } -static void m4_handle_mpu9150_compass_irq(enum m4sensorhub_irqs int_event, - void *mpu9150_data) +static void m4compass_work_func(struct work_struct *work) { - struct mpu9150_client *mpu9150_client_data = mpu9150_data; + struct mpu9150_client *dd = + container_of(work, + struct mpu9150_client, + mpu9150_work[TYPE_COMPASS].work); + signed short rate; - mutex_lock(&(mpu9150_client_data->mutex)); - - m4_read_mpu9150_data(mpu9150_client_data, TYPE_COMPASS); - m4_report_mpu9150_inputevent(mpu9150_client_data, TYPE_COMPASS); - - mutex_unlock(&(mpu9150_client_data->mutex)); + mutex_lock(&(dd->mutex)); + m4_read_mpu9150_data(dd, TYPE_COMPASS); + m4_report_mpu9150_inputevent(dd, TYPE_COMPASS); + rate = dd->samplerate[TYPE_COMPASS]; + if (rate > 0) + schedule_delayed_work(&(dd->mpu9150_work[TYPE_COMPASS]), + msecs_to_jiffies(rate)); + mutex_unlock(&(dd->mutex)); } + static ssize_t m4_mpu9150_write_accel_setdelay(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) @@ -287,21 +314,7 @@ static ssize_t m4_mpu9150_write_accel_setdelay(struct device *dev, } mutex_lock(&(misc_mpu9150_data->mutex)); - m4_set_mpu9150_delay(misc_mpu9150_data, scanresult, TYPE_ACCEL); - - if (scanresult == -1) { - mpu9150_irq_enable_disable( - misc_mpu9150_data, TYPE_ACCEL, - SENSOR_IRQ_DISABLE - ); - } else { - mpu9150_irq_enable_disable( - misc_mpu9150_data, TYPE_ACCEL, - SENSOR_IRQ_ENABLE - ); - } - mutex_unlock(&(misc_mpu9150_data->mutex)); return count; @@ -332,17 +345,6 @@ static ssize_t m4_mpu9150_write_gyro_setdelay(struct device *dev, mutex_lock(&(misc_mpu9150_data->mutex)); m4_set_mpu9150_delay(misc_mpu9150_data, scanresult, TYPE_GYRO); - if (scanresult == -1) { - mpu9150_irq_enable_disable( - misc_mpu9150_data, TYPE_GYRO, - SENSOR_IRQ_DISABLE - ); - } else { - mpu9150_irq_enable_disable( - misc_mpu9150_data, TYPE_GYRO, - SENSOR_IRQ_ENABLE - ); - } mutex_unlock(&(misc_mpu9150_data->mutex)); @@ -374,17 +376,6 @@ static ssize_t m4_mpu9150_write_compass_setdelay(struct device *dev, mutex_lock(&(misc_mpu9150_data->mutex)); m4_set_mpu9150_delay(misc_mpu9150_data, scanresult, TYPE_COMPASS); - if (scanresult == -1) { - mpu9150_irq_enable_disable( - misc_mpu9150_data, TYPE_COMPASS, - SENSOR_IRQ_DISABLE - ); - } else { - mpu9150_irq_enable_disable( - misc_mpu9150_data, TYPE_COMPASS, - SENSOR_IRQ_ENABLE - ); - } mutex_unlock(&(misc_mpu9150_data->mutex)); @@ -556,130 +547,12 @@ static struct miscdevice mpu9150_client_miscdrv = { .fops = &mpu9150_client_fops, }; -static int mpu9150_irq_init(struct mpu9150_client *mpu9150_client_data) -{ - int ret = -1; - - ret = m4sensorhub_irq_register(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_GYRO_DATA_READY, - m4_handle_mpu9150_gyro_irq, - mpu9150_client_data, 0); - if (ret < 0) { - KDEBUG(M4SH_ERROR, "Error registering int %d (%d)\n", - M4SH_IRQ_GYRO_DATA_READY, ret); - return ret; - } - ret = m4sensorhub_irq_register(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_ACCEL_DATA_READY, - m4_handle_mpu9150_accel_irq, - mpu9150_client_data, 0); - if (ret < 0) { - KDEBUG(M4SH_ERROR, "Error registering int %d (%d)\n", - M4SH_IRQ_ACCEL_DATA_READY, ret); - goto unregister_gyro_irq; - } - ret = m4sensorhub_irq_register(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_COMPASS_DATA_READY, - m4_handle_mpu9150_compass_irq, - mpu9150_client_data, 0); - if (ret < 0) { - KDEBUG(M4SH_ERROR, "Error registering int %d (%d)\n", - M4SH_IRQ_COMPASS_DATA_READY, ret); - goto unregister_accel_irq; - } - return ret; - -unregister_accel_irq: - m4sensorhub_irq_unregister(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_ACCEL_DATA_READY); -unregister_gyro_irq: - m4sensorhub_irq_unregister(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_GYRO_DATA_READY); - return ret; -} - -static void mpu9150_irq_deinit(struct mpu9150_client *mpu9150_client_data) -{ - m4sensorhub_irq_unregister(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_COMPASS_DATA_READY); - m4sensorhub_irq_unregister(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_ACCEL_DATA_READY); - m4sensorhub_irq_unregister(mpu9150_client_data->m4sensorhub, - M4SH_IRQ_GYRO_DATA_READY); -} - -static int mpu9150_irq_enable_disable(struct mpu9150_client *mpu9150_client_data, - enum mpu9150_sensor type, int flag) -{ - int ret = 0; - int irq_status = 0; - - switch (type) { - case TYPE_GYRO: - irq_status = m4sensorhub_irq_enable_get( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_GYRO_DATA_READY); - if (flag && (!irq_status)) { - ret = m4sensorhub_irq_enable( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_GYRO_DATA_READY); - if (ret < 0) { - KDEBUG(M4SH_ERROR, "Error enabling int %d (%d)\n", - M4SH_IRQ_GYRO_DATA_READY, ret); - return ret; - } - } else if ((!flag) && irq_status) - m4sensorhub_irq_disable( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_GYRO_DATA_READY); - break; - case TYPE_ACCEL: - irq_status = m4sensorhub_irq_enable_get( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_ACCEL_DATA_READY); - if (flag && (!irq_status)) { - ret = m4sensorhub_irq_enable( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_ACCEL_DATA_READY); - if (ret < 0) { - KDEBUG(M4SH_ERROR, "Error enabling int %d (%d)\n", - M4SH_IRQ_ACCEL_DATA_READY, ret); - return ret; - } - } else if ((!flag) && irq_status) - m4sensorhub_irq_disable( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_ACCEL_DATA_READY); - break; - case TYPE_COMPASS: - irq_status = m4sensorhub_irq_enable_get( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_COMPASS_DATA_READY); - if (flag && (!irq_status)) { - ret = m4sensorhub_irq_enable( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_COMPASS_DATA_READY); - if (ret < 0) { - KDEBUG(M4SH_ERROR, "Error enabling int %d (%d)\n", - M4SH_IRQ_COMPASS_DATA_READY, ret); - return ret; - } - } else if ((!flag) && irq_status) - m4sensorhub_irq_disable( - mpu9150_client_data->m4sensorhub, - M4SH_IRQ_COMPASS_DATA_READY); - break; - default: - ret = -EINVAL; - break; - } - return ret; -} - static void mpu9150_panic_restore(struct m4sensorhub_data *m4sensorhub, void *data) { struct mpu9150_client *dd = (struct mpu9150_client *)data; + int type; + signed short rate; if (dd == NULL) { KDEBUG(M4SH_INFO, "%s: Driver data is null,unable to restore\n", @@ -688,36 +561,37 @@ static void mpu9150_panic_restore(struct m4sensorhub_data *m4sensorhub, } mutex_lock(&(dd->mutex)); - KDEBUG(M4SH_INFO, "Executing mpu9150 panic restore\n"); - m4_set_mpu9150_delay(dd, dd->samplerate[TYPE_ACCEL], TYPE_ACCEL); - m4_set_mpu9150_delay(dd, dd->samplerate[TYPE_GYRO], TYPE_GYRO); - m4_set_mpu9150_delay(dd, dd->samplerate[TYPE_COMPASS], TYPE_COMPASS); - + for (type = 0; type < NUM_TYPES; type++) { + rate = dd->samplerate[type]; + m4_set_mpu9150_delay(dd, rate, type); + cancel_delayed_work(&(dd->mpu9150_work[type])); + if (rate > 0) + schedule_delayed_work(&(dd->mpu9150_work[type]), + msecs_to_jiffies(rate)); + } mutex_unlock(&(dd->mutex)); } static int mpu9150_driver_init(struct init_calldata *p_arg) { int ret; + struct mpu9150_client *dd = p_arg->p_data; - mutex_lock(&(misc_mpu9150_data->mutex)); - - ret = mpu9150_irq_init(misc_mpu9150_data); - if (ret < 0) { - KDEBUG(M4SH_ERROR, "mpu9150 irq init failed\n"); - goto driver_init_exit; - } - - ret = m4sensorhub_panic_register(misc_mpu9150_data->m4sensorhub, + mutex_lock(&(dd->mutex)); + INIT_DELAYED_WORK(&(dd->mpu9150_work[TYPE_ACCEL]), m4accel_work_func); + INIT_DELAYED_WORK(&(dd->mpu9150_work[TYPE_COMPASS]), + m4compass_work_func); + INIT_DELAYED_WORK(&(dd->mpu9150_work[TYPE_GYRO]), + m4gyro_work_func); + ret = m4sensorhub_panic_register(dd->m4sensorhub, PANICHDL_MPU9150_RESTORE, mpu9150_panic_restore, - misc_mpu9150_data); + dd); if (ret < 0) KDEBUG(M4SH_ERROR, "HR panic callback register failed\n"); -driver_init_exit: - mutex_unlock(&(misc_mpu9150_data->mutex)); + mutex_unlock(&(dd->mutex)); return ret; } @@ -746,6 +620,9 @@ static int mpu9150_client_probe(struct platform_device *pdev) mpu9150_client_data->samplerate[TYPE_GYRO]; mpu9150_client_data->latest_samplerate[TYPE_COMPASS] = mpu9150_client_data->samplerate[TYPE_COMPASS]; + mpu9150_client_data->fastest_rate[TYPE_ACCEL] = 40; + mpu9150_client_data->fastest_rate[TYPE_GYRO] = 40; + mpu9150_client_data->fastest_rate[TYPE_COMPASS] = 40; mpu9150_client_data->input_dev = input_allocate_device(); if (!mpu9150_client_data->input_dev) { @@ -841,7 +718,9 @@ static int __exit mpu9150_client_remove(struct platform_device *pdev) #ifdef MPU9150_DEBUG sysfs_remove_group(&pdev->dev.kobj, &mpu9150_group); #endif - mpu9150_irq_deinit(mpu9150_client_data); + cancel_delayed_work(&(mpu9150_client_data->mpu9150_work[TYPE_COMPASS])); + cancel_delayed_work(&(mpu9150_client_data->mpu9150_work[TYPE_ACCEL])); + cancel_delayed_work(&(mpu9150_client_data->mpu9150_work[TYPE_GYRO])); m4sensorhub_unregister_initcall(mpu9150_driver_init); misc_mpu9150_data = NULL; misc_deregister(&mpu9150_client_miscdrv); @@ -865,18 +744,16 @@ static int mpu9150_client_suspend(struct platform_device *pdev, return 0; } - mutex_lock(&(dd->mutex)); m4_set_mpu9150_delay(dd, dd->latest_samplerate[TYPE_ACCEL], TYPE_ACCEL); m4_set_mpu9150_delay(dd, dd->latest_samplerate[TYPE_GYRO], TYPE_GYRO); m4_set_mpu9150_delay(dd, dd->latest_samplerate[TYPE_COMPASS], TYPE_COMPASS); - - mutex_unlock(&(dd->mutex)); - return 0; } + + static struct of_device_id m4mpu9150_match_tbl[] = { { .compatible = "mot,m4mpu9150" }, {}, diff --git a/drivers/misc/m4sensorhub_pedometer.c b/drivers/misc/m4sensorhub_pedometer.c index ff3a0c1e913..56b2d912f7a 100644 --- a/drivers/misc/m4sensorhub_pedometer.c +++ b/drivers/misc/m4sensorhub_pedometer.c @@ -47,9 +47,9 @@ struct m4ped_driver_data { struct m4sensorhub_pedometer_iio_data iiodat; struct m4sensorhub_pedometer_iio_data base_dat; + struct delayed_work m4ped_work; int16_t samplerate; - int16_t latest_samplerate; - + int16_t fastest_rate; uint16_t status; }; @@ -162,15 +162,21 @@ m4ped_read_fail: return err; } -static void m4ped_isr(enum m4sensorhub_irqs int_event, void *handle) +static void m4ped_work_func(struct work_struct *work) { int err = 0; - struct iio_dev *iio = handle; - struct m4ped_driver_data *dd = iio_priv(iio); + struct m4ped_driver_data *dd = container_of(work, + struct m4ped_driver_data, + m4ped_work.work); + struct iio_dev *iio = platform_get_drvdata(dd->pdev); + mutex_lock(&(dd->mutex)); err = m4ped_read_report_data(iio, dd); if (err < 0) m4ped_err("%s: Failed with error code %d.\n", __func__, err); + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4ped_work), + msecs_to_jiffies(dd->samplerate)); mutex_unlock(&(dd->mutex)); return; } @@ -180,60 +186,17 @@ static int m4ped_set_samplerate(struct iio_dev *iio, int16_t rate) int err = 0; struct m4ped_driver_data *dd = iio_priv(iio); - /* - * Currently, there is no concept of setting a sample rate for this - * sensor, so this function only enables/disables interrupt reporting. - */ - dd->latest_samplerate = rate; + if ((rate >= 0) && (rate <= dd->fastest_rate)) + rate = dd->fastest_rate; + if (rate == dd->samplerate) goto m4ped_set_samplerate_fail; - if (rate >= 0) { - /* Enable the IRQ if necessary */ - if (!(dd->status & (1 << M4PED_IRQ_ENABLED_BIT))) { - err = m4sensorhub_irq_enable(dd->m4, - M4SH_IRQ_PEDOMETER_DATA_READY); - if (err < 0) { - m4ped_err("%s: Failed to enable ped irq.\n", - __func__); - goto m4ped_set_samplerate_fail; - } - - err = m4sensorhub_irq_enable(dd->m4, - M4SH_IRQ_ACTIVITY_CHANGE); - if (err < 0) { - m4ped_err("%s: Failed to enable act irq.\n", - __func__); - goto m4ped_set_samplerate_fail; - } - - dd->status = dd->status | (1 << M4PED_IRQ_ENABLED_BIT); - dd->samplerate = rate; - } - } else { - /* Disable the IRQ if necessary */ - if (dd->status & (1 << M4PED_IRQ_ENABLED_BIT)) { - err = m4sensorhub_irq_disable(dd->m4, - M4SH_IRQ_PEDOMETER_DATA_READY); - if (err < 0) { - m4ped_err("%s: Failed to disable ped irq.\n", - __func__); - goto m4ped_set_samplerate_fail; - } - - err = m4sensorhub_irq_disable(dd->m4, - M4SH_IRQ_ACTIVITY_CHANGE); - if (err < 0) { - m4ped_err("%s: Failed to disable act irq.\n", - __func__); - goto m4ped_set_samplerate_fail; - } - - dd->status = dd->status & ~(1 << M4PED_IRQ_ENABLED_BIT); - dd->samplerate = rate; - } - } - + cancel_delayed_work(&(dd->m4ped_work)); + dd->samplerate = rate; + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4ped_work), + msecs_to_jiffies(rate)); m4ped_set_samplerate_fail: return err; } @@ -668,6 +631,10 @@ static void m4ped_panic_restore(struct m4sensorhub_data *m4sensorhub, goto m4ped_panic_restore_fail; } } + cancel_delayed_work(&(dd->m4ped_work)); + if (dd->samplerate > 0) + schedule_delayed_work(&(dd->m4ped_work), + msecs_to_jiffies(dd->samplerate)); m4ped_panic_restore_fail: mutex_unlock(&(dd->mutex)); @@ -688,19 +655,7 @@ static int m4ped_driver_init(struct init_calldata *p_arg) goto m4ped_driver_init_fail; } - err = m4sensorhub_irq_register(dd->m4, - M4SH_IRQ_PEDOMETER_DATA_READY, m4ped_isr, iio, 0); - if (err < 0) { - m4ped_err("%s: Failed to register M4 PED IRQ.\n", __func__); - goto m4ped_driver_init_fail; - } - - err = m4sensorhub_irq_register(dd->m4, - M4SH_IRQ_ACTIVITY_CHANGE, m4ped_isr, iio, 0); - if (err < 0) { - m4ped_err("%s: Failed to register M4 ACT IRQ.\n", __func__); - goto m4ped_driver_init_irq_act_fail; - } + INIT_DELAYED_WORK(&(dd->m4ped_work), m4ped_work_func); err = m4sensorhub_panic_register(dd->m4, PANICHDL_PEDOMETER_RESTORE, m4ped_panic_restore, dd); @@ -709,8 +664,6 @@ static int m4ped_driver_init(struct init_calldata *p_arg) goto m4ped_driver_init_exit; -m4ped_driver_init_irq_act_fail: - m4sensorhub_irq_unregister(dd->m4, M4SH_IRQ_PEDOMETER_DATA_READY); m4ped_driver_init_fail: m4ped_err("%s: Init failed with error code %d.\n", __func__, err); m4ped_driver_init_exit: @@ -736,7 +689,7 @@ static int m4ped_probe(struct platform_device *pdev) mutex_init(&(dd->mutex)); platform_set_drvdata(pdev, iio); dd->samplerate = -1; /* We always start disabled */ - dd->latest_samplerate = dd->samplerate; + dd->fastest_rate = 1000; /* in milli secs */ dd->status = dd->status | (1 << M4PED_FEATURE_ENABLED_BIT); err = m4ped_create_iiodev(iio); /* iio and dd are freed on fail */ @@ -773,17 +726,6 @@ static int __exit m4ped_remove(struct platform_device *pdev) goto m4ped_remove_exit; mutex_lock(&(dd->mutex)); - if (dd->status & (1 << M4PED_IRQ_ENABLED_BIT)) { - m4sensorhub_irq_disable(dd->m4, - M4SH_IRQ_PEDOMETER_DATA_READY); - m4sensorhub_irq_disable(dd->m4, - M4SH_IRQ_ACTIVITY_CHANGE); - dd->status = dd->status & ~(1 << M4PED_IRQ_ENABLED_BIT); - } - m4sensorhub_irq_unregister(dd->m4, - M4SH_IRQ_PEDOMETER_DATA_READY); - m4sensorhub_irq_unregister(dd->m4, - M4SH_IRQ_ACTIVITY_CHANGE); m4sensorhub_unregister_initcall(m4ped_driver_init); m4ped_remove_iiodev(iio); /* dd is freed here */ @@ -791,17 +733,6 @@ m4ped_remove_exit: return 0; } -static int m4ped_suspend(struct platform_device *pdev, pm_message_t state) -{ - struct iio_dev *iio = platform_get_drvdata(pdev); - struct m4ped_driver_data *dd = iio_priv(iio); - mutex_lock(&(dd->mutex)); - if (m4ped_set_samplerate(iio, dd->latest_samplerate) < 0) - m4ped_err("%s: setrate retry failed\n", __func__); - mutex_unlock(&(dd->mutex)); - return 0; -} - static struct of_device_id m4pedometer_match_tbl[] = { { .compatible = "mot,m4pedometer" }, {}, @@ -811,7 +742,7 @@ static struct platform_driver m4ped_driver = { .probe = m4ped_probe, .remove = __exit_p(m4ped_remove), .shutdown = NULL, - .suspend = m4ped_suspend, + .suspend = NULL, .resume = NULL, .driver = { .name = M4PED_DRIVER_NAME, |