diff options
| -rw-r--r-- | drivers/mfd/m4sensorhub-reg.h | 6 | ||||
| -rw-r--r-- | drivers/misc/m4sensorhub_fusion.c | 42 | ||||
| -rw-r--r-- | include/linux/iio/m4sensorhub/m4sensorhub_fusion.h | 3 | ||||
| -rw-r--r-- | include/linux/m4sensorhub/m4sensorhub_reg_enum.h | 6 |
4 files changed, 50 insertions, 7 deletions
diff --git a/drivers/mfd/m4sensorhub-reg.h b/drivers/mfd/m4sensorhub-reg.h index 96719a08927..021bb85233a 100644 --- a/drivers/mfd/m4sensorhub-reg.h +++ b/drivers/mfd/m4sensorhub-reg.h @@ -87,9 +87,9 @@ static const struct { [M4SH_REG_FUSION_LOCALX] = {M4SH_TYPE_FUSION, 0x10, 4}, [M4SH_REG_FUSION_LOCALY] = {M4SH_TYPE_FUSION, 0x14, 4}, [M4SH_REG_FUSION_LOCALZ] = {M4SH_TYPE_FUSION, 0x18, 4}, - [M4SH_REG_FUSION_WORLDX] = {M4SH_TYPE_FUSION, 0x1c, 4}, - [M4SH_REG_FUSION_WORLDY] = {M4SH_TYPE_FUSION, 0x20, 4}, - [M4SH_REG_FUSION_WORLDZ] = {M4SH_TYPE_FUSION, 0x24, 4}, + [M4SH_REG_FUSION_GRAVITYX] = {M4SH_TYPE_FUSION, 0x1c, 4}, + [M4SH_REG_FUSION_GRAVITYY] = {M4SH_TYPE_FUSION, 0x20, 4}, + [M4SH_REG_FUSION_GRAVITYZ] = {M4SH_TYPE_FUSION, 0x24, 4}, [M4SH_REG_FUSION_ROTATIONVECTOR] = {M4SH_TYPE_FUSION, 0x28, 16}, [M4SH_REG_FUSION_HEADING] = {M4SH_TYPE_FUSION, 0x38, 2}, [M4SH_REG_FUSION_HEADING_ACCURACY] = {M4SH_TYPE_FUSION, 0x3a, 1}, 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 diff --git a/include/linux/iio/m4sensorhub/m4sensorhub_fusion.h b/include/linux/iio/m4sensorhub/m4sensorhub_fusion.h index 57680018826..02647c5f390 100644 --- a/include/linux/iio/m4sensorhub/m4sensorhub_fusion.h +++ b/include/linux/iio/m4sensorhub/m4sensorhub_fusion.h @@ -26,6 +26,8 @@ enum m4sensorhub_fusion_iio_type { FUSION_TYPE_ROTATION = 0, FUSION_TYPE_ORIENTATION = 1, + FUSION_TYPE_GRAVITY = 2, + M4FUS_NUM_FUSION_BUFFERS }; struct m4sensorhub_fusion_iio_data { @@ -38,6 +40,5 @@ struct m4sensorhub_fusion_iio_data { #define M4FUS_DATA_STRUCT_SIZE_BITS \ (sizeof(struct m4sensorhub_fusion_iio_data) * 8) -#define M4FUS_NUM_FUSION_BUFFERS 2 #endif /* _M4SENSORHUB_FUSION_IIO_H */ diff --git a/include/linux/m4sensorhub/m4sensorhub_reg_enum.h b/include/linux/m4sensorhub/m4sensorhub_reg_enum.h index 040df03a2ed..ba32b25cdfc 100644 --- a/include/linux/m4sensorhub/m4sensorhub_reg_enum.h +++ b/include/linux/m4sensorhub/m4sensorhub_reg_enum.h @@ -81,9 +81,9 @@ enum m4sensorhub_reg { M4SH_REG_FUSION_LOCALX, M4SH_REG_FUSION_LOCALY, M4SH_REG_FUSION_LOCALZ, - M4SH_REG_FUSION_WORLDX, - M4SH_REG_FUSION_WORLDY, - M4SH_REG_FUSION_WORLDZ, + M4SH_REG_FUSION_GRAVITYX, + M4SH_REG_FUSION_GRAVITYY, + M4SH_REG_FUSION_GRAVITYZ, M4SH_REG_FUSION_ROTATIONVECTOR, M4SH_REG_FUSION_HEADING, M4SH_REG_FUSION_HEADING_ACCURACY, |