diff options
| author | fsd017 <fsd017@motorola.com> | 2014-09-16 15:52:07 -0500 |
|---|---|---|
| committer | Sajid Dalvi <fsd017@motorola.com> | 2014-09-18 16:57:47 +0000 |
| commit | a9de9fcd582ef2264b844da6a71194df73ca41c3 (patch) | |
| tree | cf48357aeee1c39b7e10efe470c4b1fe28a29178 /drivers/misc/m4sensorhub_fusion.c | |
| parent | c7df60a7a961c50d7ee7d4dc9c8deef4f32c3b58 (diff) | |
| download | olio-linux-3.10-a9de9fcd582ef2264b844da6a71194df73ca41c3.tar.xz olio-linux-3.10-a9de9fcd582ef2264b844da6a71194df73ca41c3.zip | |
IKXCLOCK-3536 : Implement Gravity Sensor on M4
Change-Id: I016607a49207319d92ed1050a802b00b729c11bb
Diffstat (limited to 'drivers/misc/m4sensorhub_fusion.c')
| -rw-r--r-- | drivers/misc/m4sensorhub_fusion.c | 42 |
1 files changed, 42 insertions, 0 deletions
diff --git a/drivers/misc/m4sensorhub_fusion.c b/drivers/misc/m4sensorhub_fusion.c index e1330f56263..bef75791514 100644 --- a/drivers/misc/m4sensorhub_fusion.c +++ b/drivers/misc/m4sensorhub_fusion.c @@ -126,6 +126,48 @@ static void m4fus_work_func(struct work_struct *work) dd->iiodat[1].type = FUSION_TYPE_ORIENTATION; dd->iiodat[1].timestamp = iio_get_time_ns(); + size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_FUSION_GRAVITYX); + err = m4sensorhub_reg_read(dd->m4, M4SH_REG_FUSION_GRAVITYX, + (char *)&(dd->iiodat[2].values[0])); + if (err < 0) { + m4fus_err("%s: Failed to read gravityX data.\n", __func__); + goto m4fus_isr_fail; + } else if (err != size) { + m4fus_err("%s: Read %d bytes instead of %d for %s.\n", + __func__, err, size, "gravityX"); + err = -EBADE; + goto m4fus_isr_fail; + } + + size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_FUSION_GRAVITYY); + err = m4sensorhub_reg_read(dd->m4, M4SH_REG_FUSION_GRAVITYY, + (char *)&(dd->iiodat[2].values[1])); + if (err < 0) { + m4fus_err("%s: Failed to read gravityY data.\n", __func__); + goto m4fus_isr_fail; + } else if (err != size) { + m4fus_err("%s: Read %d bytes instead of %d for %s.\n", + __func__, err, size, "gravityY"); + err = -EBADE; + goto m4fus_isr_fail; + } + + size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_FUSION_GRAVITYZ); + err = m4sensorhub_reg_read(dd->m4, M4SH_REG_FUSION_GRAVITYZ, + (char *)&(dd->iiodat[2].values[2])); + if (err < 0) { + m4fus_err("%s: Failed to read gravityZ data.\n", __func__); + goto m4fus_isr_fail; + } else if (err != size) { + m4fus_err("%s: Read %d bytes instead of %d for %s.\n", + __func__, err, size, "gravityZ"); + err = -EBADE; + goto m4fus_isr_fail; + } + + dd->iiodat[2].type = FUSION_TYPE_GRAVITY; + dd->iiodat[2].timestamp = iio_get_time_ns(); + /* * For some reason, IIO knows we are sending an array, * so all FUSION_TYPE_* indicies will be sent |