1 files changed, 146 insertions, 48 deletions

diff --git a/drivers/misc/m4sensorhub_pedometer.c b/drivers/misc/m4sensorhub_pedometer.c
index cf326454bea..0be14698e1d 100644
--- a/drivers/misc/m4sensorhub_pedometer.c
+++ b/drivers/misc/m4sensorhub_pedometer.c
@@ -40,6 +40,8 @@
 #define M4PED_IRQ_ENABLED_BIT       0
 #define M4PED_FEATURE_ENABLED_BIT   1
 
+#define M4PED_USERDATA_SIZE         5
+
 struct m4ped_driver_data {
 	struct platform_device      *pdev;
 	struct m4sensorhub_data     *m4;
@@ -47,9 +49,12 @@ struct m4ped_driver_data {
 
 	struct m4sensorhub_pedometer_iio_data   iiodat;
 	struct m4sensorhub_pedometer_iio_data   base_dat;
-	struct delayed_work	m4ped_work;
+	struct m4sensorhub_pedometer_iio_data   last_dat;
+	struct delayed_work                     m4ped_work;
+
 	int16_t         samplerate;
 	int16_t         fastest_rate;
+	uint8_t         userdata[M4PED_USERDATA_SIZE];
 	uint16_t        status;
 };
 
@@ -57,6 +62,7 @@ static int m4ped_read_report_data(struct iio_dev *iio,
 				  struct m4ped_driver_data *dd)
 {
 	int err = 0, size = 0;
+	struct m4sensorhub_pedometer_iio_data dat;
 
 	/*input validations */
 	if ((iio == NULL) || (dd == NULL)) {
@@ -80,7 +86,7 @@ static int m4ped_read_report_data(struct iio_dev *iio,
 	size = m4sensorhub_reg_getsize(dd->m4,
 		M4SH_REG_PEDOMETER_TOTALDISTANCE);
 	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_PEDOMETER_TOTALDISTANCE,
-		(char *)&(dd->iiodat.total_distance));
+		(char *)&(dat.total_distance));
 	if (err < 0) {
 		m4ped_err("%s: Failed to read total_distance data.\n",
 			  __func__);
@@ -94,7 +100,7 @@ static int m4ped_read_report_data(struct iio_dev *iio,
 
 	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_PEDOMETER_TOTALSTEPS);
 	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_PEDOMETER_TOTALSTEPS,
-		(char *)&(dd->iiodat.total_steps));
+		(char *)&(dat.total_steps));
 	if (err < 0) {
 		m4ped_err("%s: Failed to read total_steps data.\n", __func__);
 		goto m4ped_read_fail;
@@ -121,7 +127,7 @@ static int m4ped_read_report_data(struct iio_dev *iio,
 
 	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_METS_HEALTHYMINUTES);
 	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_METS_HEALTHYMINUTES,
-		(char *)&(dd->iiodat.healthy_minutes));
+		(char *)&(dat.healthy_minutes));
 	if (err < 0) {
 		m4ped_err("%s: Failed to read healthy_minutes data.\n",
 			  __func__);
@@ -135,7 +141,7 @@ static int m4ped_read_report_data(struct iio_dev *iio,
 
 	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_METS_CALORIES);
 	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_METS_CALORIES,
-		(char *)&(dd->iiodat.calories));
+		(char *)&(dat.calories));
 	if (err < 0) {
 		m4ped_err("%s: Failed to read calories data.\n", __func__);
 		goto m4ped_read_fail;
@@ -146,12 +152,63 @@ static int m4ped_read_report_data(struct iio_dev *iio,
 		goto m4ped_read_fail;
 	}
 
+	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_METS_CALORIES_NO_RMR);
+	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_METS_CALORIES_NO_RMR,
+		(char *)&(dat.calories_normr));
+	if (err < 0) {
+		m4ped_err("%s: Failed to read calories_normr data.\n",
+			__func__);
+		goto m4ped_read_fail;
+	} else if (err != size) {
+		m4ped_err("%s: Read %d bytes instead of %d for %s.\n",
+			  __func__, err, size, "calories_normr");
+		err = -EBADE;
+		goto m4ped_read_fail;
+	}
+
 	dd->iiodat.timestamp = iio_get_time_ns();
 
-	dd->iiodat.total_distance += dd->base_dat.total_distance;
-	dd->iiodat.total_steps += dd->base_dat.total_steps;
-	dd->iiodat.healthy_minutes += dd->base_dat.healthy_minutes;
-	dd->iiodat.calories += dd->base_dat.calories;
+	/* Save data if these values decrease (they monotonically increase) */
+	if ((dat.total_distance < dd->last_dat.total_distance) ||
+		(dat.total_steps < dd->last_dat.total_steps) ||
+		(dat.healthy_minutes < dd->last_dat.healthy_minutes) ||
+		(dat.calories < dd->last_dat.calories) ||
+		(dat.calories_normr < dd->last_dat.calories_normr)) {
+		m4ped_err("%s: Error: Current = %u %u %u %u %u "
+			  "Last = %u %u %u %u %u, Base = %u %u %u %u %u\n",
+			   __func__, dat.total_distance,
+			  dat.total_steps, dat.healthy_minutes,
+			  dat.calories, dat.calories_normr,
+			  dd->last_dat.total_distance,
+			  dd->last_dat.total_steps,
+			  dd->last_dat.healthy_minutes, dd->last_dat.calories,
+			  dd->last_dat.calories_normr,
+			  dd->base_dat.total_distance,
+			  dd->base_dat.total_steps,
+			  dd->base_dat.healthy_minutes, dd->base_dat.calories,
+			  dd->base_dat.calories_normr);
+		m4ped_err("%s: iio = %u %u %u %u %u\n", __func__,
+			  dd->iiodat.total_distance,
+			  dd->iiodat.total_steps,
+			  dd->iiodat.healthy_minutes, dd->iiodat.calories,
+			  dd->iiodat.calories_normr);
+		goto m4ped_read_fail;
+	}
+
+	dd->last_dat.total_distance = dat.total_distance;
+	dd->last_dat.total_steps = dat.total_steps;
+	dd->last_dat.healthy_minutes = dat.healthy_minutes;
+	dd->last_dat.calories = dat.calories;
+	dd->last_dat.calories_normr = dat.calories_normr;
+
+	dd->iiodat.total_distance = dat.total_distance +
+		dd->base_dat.total_distance;
+	dd->iiodat.total_steps = dat.total_steps + dd->base_dat.total_steps;
+	dd->iiodat.healthy_minutes = dat.healthy_minutes +
+		dd->base_dat.healthy_minutes;
+	dd->iiodat.calories = dat.calories + dd->base_dat.calories;
+	dd->iiodat.calories_normr = dat.calories_normr +
+		dd->base_dat.calories_normr;
 
 	iio_push_to_buffers(iio, (unsigned char *)&(dd->iiodat));
 
@@ -181,6 +238,42 @@ static void m4ped_work_func(struct work_struct *work)
 	return;
 }
 
+static int m4ped_write_userdata(struct m4ped_driver_data *dd)
+{
+	int err;
+
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_USERSETTINGS_USERAGE,
+		&(dd->userdata[0]), m4sh_no_mask);
+	if (err < 0) {
+		m4ped_err("%s: Failed to write age.\n", __func__);
+		goto m4ped_write_userdata_fail;
+	}
+
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_USERSETTINGS_USERGENDER,
+		&(dd->userdata[1]), m4sh_no_mask);
+	if (err < 0) {
+		m4ped_err("%s: Failed to write gender.\n", __func__);
+		goto m4ped_write_userdata_fail;
+	}
+
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_USERSETTINGS_USERHEIGHT,
+		&(dd->userdata[2]), m4sh_no_mask);
+	if (err < 0) {
+		m4ped_err("%s: Failed to write height.\n", __func__);
+		goto m4ped_write_userdata_fail;
+	}
+
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_USERSETTINGS_USERWEIGHT,
+		&(dd->userdata[3]), m4sh_no_mask);
+	if (err < 0) {
+		m4ped_err("%s: Failed to write weight.\n", __func__);
+		goto m4ped_write_userdata_fail;
+	}
+
+m4ped_write_userdata_fail:
+	return err;
+}
+
 static int m4ped_set_samplerate(struct iio_dev *iio, int16_t rate)
 {
 	int err = 0;
@@ -278,13 +371,14 @@ static ssize_t m4ped_iiodata_show(struct device *dev,
 
 	mutex_lock(&(dd->mutex));
 	size = snprintf(buf, PAGE_SIZE,
-		"%s%hhu\n%s%u\n%s%u\n%s%hu\n%s%u\n%s%u\n",
+		"%s%hhu\n%s%u\n%s%u\n%s%hu\n%s%u\n%s%u\n%s%u\n",
 		"ped_activity: ", dd->iiodat.ped_activity,
 		"total_distance: ", dd->iiodat.total_distance,
 		"total_steps: ", dd->iiodat.total_steps,
 		"current_speed: ", dd->iiodat.current_speed,
 		"healthy_minutes: ", dd->iiodat.healthy_minutes,
-		"calories: ", dd->iiodat.calories);
+		"calories: ", dd->iiodat.calories,
+		"calories_normr: ", dd->iiodat.calories_normr);
 	mutex_unlock(&(dd->mutex));
 	return size;
 }
@@ -298,11 +392,11 @@ static ssize_t m4ped_userdata_show(struct device *dev,
 	struct iio_dev *iio = platform_get_drvdata(pdev);
 	struct m4ped_driver_data *dd = iio_priv(iio);
 	ssize_t size = 0;
-	uint8_t data[5] = {0x00, 0x00, 0x00, 0x00, 0x00};
+	uint8_t data[M4PED_USERDATA_SIZE] = {0x00};
 
 	mutex_lock(&(dd->mutex));
 
-	err = m4sensorhub_reg_read_n(dd->m4, M4SH_REG_USERSETTINGS_SCREENSTATUS,
+	err = m4sensorhub_reg_read_n(dd->m4, M4SH_REG_USERSETTINGS_USERAGE,
 		(char *)&(data[0]), ARRAY_SIZE(data));
 	if (err < 0) {
 		m4ped_err("%s: Failed to read user data.\n", __func__);
@@ -315,11 +409,11 @@ static ssize_t m4ped_userdata_show(struct device *dev,
 	}
 
 	size = snprintf(buf, PAGE_SIZE,
-		"%s%s\n%s%hhu\n%s%hhu\n%s%hhu\n",
-		"Gender (M/F): ", data[2] ? "M" : "F",
-		"Age    (yrs): ", data[1],
-		"Height  (cm): ", data[3],
-		"Weight  (kg): ", data[4]);
+		"%s%s\n%s%hhu\n%s%hhu\n%s%hu\n",
+		"Gender (M/F): ", data[1] ? "M" : "F",
+		"Age    (yrs): ", data[0],
+		"Height  (cm): ", data[2],
+		"Weight  (kg): ", data[3] | (data[4] << 8));
 
 m4ped_userdata_show_fail:
 	mutex_unlock(&(dd->mutex));
@@ -334,6 +428,7 @@ m4ped_userdata_show_fail:
  * Example:
  *    Female, 22, 168cm, 49kg
  *    0x00,0x16,0xA7,0x31\n
+ *    echo 0x00,0x16,0xA7,0x31 > userdata
  */
 static ssize_t m4ped_userdata_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
@@ -344,7 +439,7 @@ static ssize_t m4ped_userdata_store(struct device *dev,
 	struct m4ped_driver_data *dd = iio_priv(iio);
 	unsigned int value = 0;
 	unsigned char convbuf[5] = {0x00, 0x00, 0x00, 0x00, 0x00};
-	unsigned char outbuf[4] = {0x00, 0x00, 0x00, 0x00};
+	unsigned char outbuf[M4PED_USERDATA_SIZE] = {0x00};
 	int i = 0;
 
 	mutex_lock(&(dd->mutex));
@@ -373,31 +468,16 @@ static ssize_t m4ped_userdata_store(struct device *dev,
 		outbuf[i] = (unsigned char) value;
 	}
 
-	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_USERSETTINGS_USERAGE,
-		&(outbuf[1]), m4sh_no_mask);
-	if (err < 0) {
-		m4ped_err("%s: Failed to write user data.\n", __func__);
-		goto m4ped_userdata_store_fail;
-	}
+	for (i = 0; i < M4PED_USERDATA_SIZE; i++)
+		dd->userdata[i] = outbuf[i];
 
-	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_USERSETTINGS_USERGENDER,
-		&(outbuf[0]), m4sh_no_mask);
-	if (err < 0) {
-		m4ped_err("%s: Failed to write user data.\n", __func__);
-		goto m4ped_userdata_store_fail;
-	}
-
-	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_USERSETTINGS_USERHEIGHT,
-		&(outbuf[2]), m4sh_no_mask);
-	if (err < 0) {
-		m4ped_err("%s: Failed to write user data.\n", __func__);
-		goto m4ped_userdata_store_fail;
-	}
+	dd->userdata[0] = outbuf[1]; /* Age */
+	dd->userdata[1] = outbuf[0]; /* Gender */
 
-	err = m4sensorhub_reg_write_n(dd->m4, M4SH_REG_USERSETTINGS_USERWEIGHT,
-		&(outbuf[3]), m4sh_no_mask, 1);
+	err = m4ped_write_userdata(dd);
 	if (err < 0) {
-		m4ped_err("%s: Failed to write user data.\n", __func__);
+		m4ped_err("%s: Failed to write user data (%d).\n",
+			__func__, err);
 		goto m4ped_userdata_store_fail;
 	}
 
@@ -606,13 +686,12 @@ static void m4ped_panic_restore(struct m4sensorhub_data *m4sensorhub,
 
 	mutex_lock(&(dd->mutex));
 
-	dd->base_dat.total_distance = dd->iiodat.total_distance;
-	dd->base_dat.total_steps = dd->iiodat.total_steps;
-	dd->base_dat.healthy_minutes = dd->iiodat.healthy_minutes;
-	dd->base_dat.calories = dd->iiodat.calories;
-	m4ped_err("%s: Pedometer bases after panic = %d %d %d %d", __func__,
-		  dd->base_dat.total_distance, dd->base_dat.total_steps,
-		  dd->base_dat.healthy_minutes, dd->base_dat.calories);
+	err = m4ped_write_userdata(dd);
+	if (err < 0) {
+		m4ped_err("%s: Failed to write user data (%d).\n",
+			__func__, err);
+		goto m4ped_panic_restore_fail;
+	}
 
 	if (!(dd->status & (1 << M4PED_FEATURE_ENABLED_BIT))) {
 		err = m4sensorhub_reg_write(dd->m4, M4SH_REG_PEDOMETER_ENABLE,
@@ -631,6 +710,19 @@ static void m4ped_panic_restore(struct m4sensorhub_data *m4sensorhub,
 			goto m4ped_panic_restore_fail;
 		}
 	}
+	/* Update base and reset last */
+	dd->base_dat.total_distance = dd->iiodat.total_distance;
+	dd->base_dat.total_steps = dd->iiodat.total_steps;
+	dd->base_dat.healthy_minutes = dd->iiodat.healthy_minutes;
+	dd->base_dat.calories = dd->iiodat.calories;
+	dd->base_dat.calories_normr = dd->iiodat.calories_normr;
+
+	dd->last_dat.total_distance = 0;
+	dd->last_dat.total_steps = 0;
+	dd->last_dat.healthy_minutes = 0;
+	dd->last_dat.calories = 0;
+	dd->last_dat.calories_normr = 0;
+
 	cancel_delayed_work(&(dd->m4ped_work));
 	if (dd->samplerate > 0)
 		queue_delayed_work(system_freezable_wq, &(dd->m4ped_work),
@@ -692,6 +784,12 @@ static int m4ped_probe(struct platform_device *pdev)
 	dd->fastest_rate = 1000; /* in milli secs */
 	dd->status = dd->status | (1 << M4PED_FEATURE_ENABLED_BIT);
 
+	dd->userdata[0] = 0x23; /* Age (35) */
+	dd->userdata[1] = 0x01; /* Gender (Male) */
+	dd->userdata[2] = 0xB2; /* Height (178cm) */
+	dd->userdata[3] = 0x5B; /* Weight (91kg) */
+	dd->userdata[4] = 0x00; /* Weight */
+
 	err = m4ped_create_iiodev(iio); /* iio and dd are freed on fail */
 	if (err < 0) {
 		m4ped_err("%s: Failed to create IIO device.\n", __func__);