IKXCLOCK-3536 : Implement Gravity Sensor on M4

Change-Id: I016607a49207319d92ed1050a802b00b729c11bb

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,