drivers/misc/m4sensorhub_pedometer.c

/*
 *  Copyright (C) 2012-2014 Motorola, Inc.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *  Adds ability to program periodic interrupts from user space that
 *  can wake the phone out of low power modes.
 *
 */

#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/platform_device.h>
#include <linux/fs.h>
#include <linux/m4sensorhub.h>
#include <linux/slab.h>
#include <linux/iio/iio.h>
#include <linux/iio/types.h>
#include <linux/iio/sysfs.h>
#include <linux/iio/events.h>
#include <linux/iio/buffer.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/m4sensorhub/m4sensorhub_pedometer.h>

#define m4ped_err(format, args...)  KDEBUG(M4SH_ERROR, format, ## args)

#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;
	struct mutex                mutex; /* controls driver entry points */

	struct m4sensorhub_pedometer_iio_data   iiodat;
	struct m4sensorhub_pedometer_iio_data   base_dat;
	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;
};

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)) {
		m4ped_err("%s: invalid inputs passed in\n", __func__);
		return -EINVAL;
	}

	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_PEDOMETER_ACTIVITY);
	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_PEDOMETER_ACTIVITY,
		(char *)&(dd->iiodat.ped_activity));
	if (err < 0) {
		m4ped_err("%s: Failed to read ped_activity 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, "ped_activity");
		err = -EBADE;
		goto m4ped_read_fail;
	}

	size = m4sensorhub_reg_getsize(dd->m4,
		M4SH_REG_PEDOMETER_TOTALDISTANCE);
	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_PEDOMETER_TOTALDISTANCE,
		(char *)&(dat.total_distance));
	if (err < 0) {
		m4ped_err("%s: Failed to read total_distance 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, "total_distance");
		err = -EBADE;
		goto m4ped_read_fail;
	}

	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_PEDOMETER_TOTALSTEPS);
	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_PEDOMETER_TOTALSTEPS,
		(char *)&(dat.total_steps));
	if (err < 0) {
		m4ped_err("%s: Failed to read total_steps 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, "total_steps");
		err = -EBADE;
		goto m4ped_read_fail;
	}

	size = m4sensorhub_reg_getsize(dd->m4,
		M4SH_REG_PEDOMETER_CURRENTSPEED);
	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_PEDOMETER_CURRENTSPEED,
		(char *)&(dd->iiodat.current_speed));
	if (err < 0) {
		m4ped_err("%s: Failed to read current_speed 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, "current_speed");
		err = -EBADE;
		goto m4ped_read_fail;
	}

	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_METS_HEALTHYMINUTES);
	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_METS_HEALTHYMINUTES,
		(char *)&(dat.healthy_minutes));
	if (err < 0) {
		m4ped_err("%s: Failed to read healthy_minutes 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, "healthy_minutes");
		err = -EBADE;
		goto m4ped_read_fail;
	}

	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_METS_CALORIES);
	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_METS_CALORIES,
		(char *)&(dat.calories));
	if (err < 0) {
		m4ped_err("%s: Failed to read calories 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");
		err = -EBADE;
		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();

	/* 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));

m4ped_read_fail:
	if (err < 0)
		m4ped_err("%s: Failed with error code %d.\n", __func__, err);

	return err;
}

static void m4ped_work_func(struct work_struct *work)
{
	int err = 0;
	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)
		queue_delayed_work(system_freezable_wq, &(dd->m4ped_work),
			msecs_to_jiffies(dd->samplerate));
	mutex_unlock(&(dd->mutex));
	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;
	struct m4ped_driver_data *dd = iio_priv(iio);

	if ((rate >= 0) && (rate <= dd->fastest_rate))
		rate = dd->fastest_rate;

	if (rate == dd->samplerate)
		goto m4ped_set_samplerate_fail;

	cancel_delayed_work(&(dd->m4ped_work));
	dd->samplerate = rate;
	if (dd->samplerate > 0)
		queue_delayed_work(system_freezable_wq, &(dd->m4ped_work),
			msecs_to_jiffies(dd->samplerate));
m4ped_set_samplerate_fail:
	return err;
}

static ssize_t m4ped_setrate_show(struct device *dev,
		struct device_attribute *attr, char *buf)
{
	struct platform_device *pdev = to_platform_device(dev);
	struct iio_dev *iio = platform_get_drvdata(pdev);
	struct m4ped_driver_data *dd = iio_priv(iio);
	ssize_t size = 0;

	mutex_lock(&(dd->mutex));
	size = snprintf(buf, PAGE_SIZE, "Current rate: %hd\n", dd->samplerate);
	mutex_unlock(&(dd->mutex));
	return size;
}
static ssize_t m4ped_setrate_store(struct device *dev,
		struct device_attribute *attr, const char *buf, size_t size)
{
	int err = 0;
	struct platform_device *pdev = to_platform_device(dev);
	struct iio_dev *iio = platform_get_drvdata(pdev);
	struct m4ped_driver_data *dd = iio_priv(iio);
	int value = 0;

	mutex_lock(&(dd->mutex));

	err = kstrtoint(buf, 10, &value);
	if (err < 0) {
		m4ped_err("%s: Failed to convert value.\n", __func__);
		goto m4ped_enable_store_exit;
	}

	if ((value < -1) || (value > 32767)) {
		m4ped_err("%s: Invalid samplerate %d passed.\n",
			  __func__, value);
		err = -EINVAL;
		goto m4ped_enable_store_exit;
	}

	err = m4ped_set_samplerate(iio, value);
	if (err < 0) {
		m4ped_err("%s: Failed to set sample rate.\n", __func__);
		goto m4ped_enable_store_exit;
	}
	if (value >= 0) {
		/* When an app registers, there is no data reported
		unless the user starts walking. But the application
		would like to have atleast one set of data sent
		immediately following the register */
		err = m4ped_read_report_data(iio, dd);
		if (err < 0) {
			m4ped_err("%s:Failed to report pedo data\n", __func__);
			goto m4ped_enable_store_exit;
		}
	}

m4ped_enable_store_exit:
	if (err < 0) {
		m4ped_err("%s: Failed with error code %d.\n", __func__, err);
		size = err;
	}

	mutex_unlock(&(dd->mutex));

	return size;
}
static IIO_DEVICE_ATTR(setrate, S_IRUSR | S_IWUSR,
		m4ped_setrate_show, m4ped_setrate_store, 0);

static ssize_t m4ped_iiodata_show(struct device *dev,
		struct device_attribute *attr, char *buf)
{
	struct platform_device *pdev = to_platform_device(dev);
	struct iio_dev *iio = platform_get_drvdata(pdev);
	struct m4ped_driver_data *dd = iio_priv(iio);
	ssize_t size = 0;

	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%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_normr: ", dd->iiodat.calories_normr);
	mutex_unlock(&(dd->mutex));
	return size;
}
static IIO_DEVICE_ATTR(iiodata, S_IRUGO, m4ped_iiodata_show, NULL, 0);

static ssize_t m4ped_userdata_show(struct device *dev,
		struct device_attribute *attr, char *buf)
{
	int err = 0;
	struct platform_device *pdev = to_platform_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[M4PED_USERDATA_SIZE] = {0x00};

	mutex_lock(&(dd->mutex));

	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__);
		goto m4ped_userdata_show_fail;
	} else if (err < ARRAY_SIZE(data)) {
		m4ped_err("%s: Read %d bytes instead of %d.\n",
			  __func__, err, size);
		err = -EBADE;
		goto m4ped_userdata_show_fail;
	}

	size = snprintf(buf, PAGE_SIZE,
		"%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));

	if (err < 0)
		size = snprintf(buf, PAGE_SIZE, "Read failed (check dmesg)\n");

	return size;
}
/*
 * Expected input is Gender, Age, Height, Weight
 * 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)
{
	int err = 0;
	struct platform_device *pdev = to_platform_device(dev);
	struct iio_dev *iio = platform_get_drvdata(pdev);
	struct m4ped_driver_data *dd = iio_priv(iio);
	unsigned int value = 0;
	unsigned char convbuf[5] = {0x00, 0x00, 0x00, 0x00, 0x00};
	unsigned char outbuf[M4PED_USERDATA_SIZE] = {0x00};
	int i = 0;

	mutex_lock(&(dd->mutex));

	/* Size includes a terminating '\n' but not the terminating null */
	if (size != 20) {
		m4ped_err("%s: Invalid data format.  %s\n",
			  __func__, "Use \"0xHH,0xHH,0xHH,0xHH\\n\" instead.");
		err = -EINVAL;
		goto m4ped_userdata_store_fail;
	}

	for (i = 0; i < 4; i++) {
		memcpy(convbuf, &(buf[i*5]), 4);
		err = kstrtouint(convbuf, 16, &value);
		if (err < 0) {
			m4ped_err("%s: Argument %d conversion failed.\n",
				  __func__, i);
			goto m4ped_userdata_store_fail;
		} else if (value > 255) {
			m4ped_err("%s: Value 0x%08X is too large.\n",
				  __func__, value);
			err = -EOVERFLOW;
			goto m4ped_userdata_store_fail;
		}
		outbuf[i] = (unsigned char) value;
	}

	for (i = 0; i < M4PED_USERDATA_SIZE; i++)
		dd->userdata[i] = outbuf[i];

	dd->userdata[0] = outbuf[1]; /* Age */
	dd->userdata[1] = outbuf[0]; /* Gender */

	err = m4ped_write_userdata(dd);
	if (err < 0) {
		m4ped_err("%s: Failed to write user data (%d).\n",
			__func__, err);
		goto m4ped_userdata_store_fail;
	}

	err = size;

m4ped_userdata_store_fail:
	mutex_unlock(&(dd->mutex));

	return (ssize_t) err;
}
static IIO_DEVICE_ATTR(userdata, S_IRUSR | S_IWUSR,
		m4ped_userdata_show, m4ped_userdata_store, 0);

static ssize_t m4ped_feature_enable_show(struct device *dev,
		struct device_attribute *attr, char *buf)
{
	struct platform_device *pdev = to_platform_device(dev);
	struct iio_dev *iio = platform_get_drvdata(pdev);
	struct m4ped_driver_data *dd = iio_priv(iio);
	ssize_t size = 0;

	mutex_lock(&(dd->mutex));
	if (dd->status & (1 << M4PED_FEATURE_ENABLED_BIT))
		size = snprintf(buf, PAGE_SIZE, "Enabled\n");
	else
		size = snprintf(buf, PAGE_SIZE, "Disabled\n");
	mutex_unlock(&(dd->mutex));
	return size;
}
static ssize_t m4ped_feature_enable_store(struct device *dev,
		struct device_attribute *attr, const char *buf, size_t size)
{
	int err = 0;
	struct platform_device *pdev = to_platform_device(dev);
	struct iio_dev *iio = platform_get_drvdata(pdev);
	struct m4ped_driver_data *dd = iio_priv(iio);
	int value = 0;
	uint8_t enable = 0x00;

	mutex_lock(&(dd->mutex));

	err = kstrtoint(buf, 10, &value);
	if (err < 0) {
		m4ped_err("%s: Failed to convert value.\n", __func__);
		goto m4ped_feature_enable_store_fail;
	}

	if (value == 0) {
		if (dd->status & (1 << M4PED_FEATURE_ENABLED_BIT)) {
			enable = 0x00;
			err = m4sensorhub_reg_write(dd->m4,
				M4SH_REG_PEDOMETER_ENABLE, &enable,
				m4sh_no_mask);
			if (err < 0) {
				m4ped_err("%s: Failed to write ped disable.\n",
					__func__);
				goto m4ped_feature_enable_store_fail;
			}

			err = m4sensorhub_reg_write(dd->m4,
				M4SH_REG_METS_ENABLE, &enable,
				m4sh_no_mask);
			if (err < 0) {
				m4ped_err("%s: Failed to write mets disable.\n",
					__func__);
				goto m4ped_feature_enable_store_fail;
			}

			dd->status = dd->status &
				~(1 << M4PED_FEATURE_ENABLED_BIT);
		}
	} else {
		if (!(dd->status & (1 << M4PED_FEATURE_ENABLED_BIT))) {
			enable = 0x01;
			err = m4sensorhub_reg_write(dd->m4,
				M4SH_REG_PEDOMETER_ENABLE, &enable,
				m4sh_no_mask);
			if (err < 0) {
				m4ped_err("%s: Failed to write ped enable.\n",
					__func__);
				goto m4ped_feature_enable_store_fail;
			}

			err = m4sensorhub_reg_write(dd->m4,
				M4SH_REG_METS_ENABLE, &enable,
				m4sh_no_mask);
			if (err < 0) {
				m4ped_err("%s: Failed to write mets enable.\n",
					__func__);
				goto m4ped_feature_enable_store_fail;
			}

			dd->status = dd->status |
				1 << M4PED_FEATURE_ENABLED_BIT;
		}
	}

m4ped_feature_enable_store_fail:
	mutex_unlock(&(dd->mutex));

	return size;
}
static IIO_DEVICE_ATTR(feature_enable, S_IRUSR | S_IWUSR,
		m4ped_feature_enable_show, m4ped_feature_enable_store, 0);

static struct attribute *m4ped_iio_attributes[] = {
	&iio_dev_attr_setrate.dev_attr.attr,
	&iio_dev_attr_iiodata.dev_attr.attr,
	&iio_dev_attr_userdata.dev_attr.attr,
	&iio_dev_attr_feature_enable.dev_attr.attr,
	NULL,
};

static const struct attribute_group m4ped_iio_attr_group = {
	.attrs = m4ped_iio_attributes,
};

static const struct iio_info m4ped_iio_info = {
	.driver_module = THIS_MODULE,
	.attrs = &m4ped_iio_attr_group,
};

static const struct iio_chan_spec m4ped_iio_channels[] = {
	{
		.type = IIO_PEDOMETER,
		.scan_index = 0,
		.scan_type = {
			.sign = 'u',
			.realbits = M4PED_DATA_STRUCT_SIZE_BITS,
			.storagebits = M4PED_DATA_STRUCT_SIZE_BITS,
			.shift = 0,
		},
	},
};

static void m4ped_remove_iiodev(struct iio_dev *iio)
{
	struct m4ped_driver_data *dd = iio_priv(iio);

	/* Remember, only call when dd->mutex is locked */
	iio_kfifo_free(iio->buffer);
	iio_buffer_unregister(iio);
	iio_device_unregister(iio);
	mutex_destroy(&(dd->mutex));
	iio_device_free(iio); /* dd is freed here */
	return;
}

static int m4ped_create_iiodev(struct iio_dev *iio)
{
	int err = 0;
	struct m4ped_driver_data *dd = iio_priv(iio);

	iio->name = M4PED_DRIVER_NAME;
	iio->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_HARDWARE;
	iio->num_channels = 1;
	iio->info = &m4ped_iio_info;
	iio->channels = m4ped_iio_channels;

	iio->buffer = iio_kfifo_allocate(iio);
	if (iio->buffer == NULL) {
		m4ped_err("%s: Failed to allocate IIO buffer.\n", __func__);
		err = -ENOMEM;
		goto m4ped_create_iiodev_kfifo_fail;
	}

	iio->buffer->scan_timestamp = true;
	iio->buffer->access->set_bytes_per_datum(iio->buffer,
		sizeof(dd->iiodat));
	err = iio_buffer_register(iio, iio->channels, iio->num_channels);
	if (err < 0) {
		m4ped_err("%s: Failed to register IIO buffer.\n", __func__);
		goto m4ped_create_iiodev_buffer_fail;
	}

	err = iio_device_register(iio);
	if (err < 0) {
		m4ped_err("%s: Failed to register IIO device.\n", __func__);
		goto m4ped_create_iiodev_iioreg_fail;
	}

	goto m4ped_create_iiodev_exit;

m4ped_create_iiodev_iioreg_fail:
	iio_buffer_unregister(iio);
m4ped_create_iiodev_buffer_fail:
	iio_kfifo_free(iio->buffer);
m4ped_create_iiodev_kfifo_fail:
	iio_device_free(iio); /* dd is freed here */
m4ped_create_iiodev_exit:
	return err;
}

static void m4ped_panic_restore(struct m4sensorhub_data *m4sensorhub,
				void *data)
{
	struct m4ped_driver_data *dd = (struct m4ped_driver_data *)data;
	int err = 0;
	uint8_t disable = 0x00;

	if (dd == NULL) {
		m4ped_err("%s: Driver data is null, unable to restore\n",
			  __func__);
		return;
	}

	mutex_lock(&(dd->mutex));

	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,
			&disable, m4sh_no_mask);
		if (err < 0) {
			m4ped_err("%s: Failed to write ped disable (%d).\n",
				__func__, err);
			goto m4ped_panic_restore_fail;
		}

		err = m4sensorhub_reg_write(dd->m4, M4SH_REG_METS_ENABLE,
			&disable, m4sh_no_mask);
		if (err < 0) {
			m4ped_err("%s: Failed to write mets disable (%d).\n",
				__func__, err);
			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),
			msecs_to_jiffies(dd->samplerate));

m4ped_panic_restore_fail:
	mutex_unlock(&(dd->mutex));
}

static int m4ped_driver_init(struct init_calldata *p_arg)
{
	struct iio_dev *iio = p_arg->p_data;
	struct m4ped_driver_data *dd = iio_priv(iio);
	int err = 0;

	mutex_lock(&(dd->mutex));

	dd->m4 = p_arg->p_m4sensorhub_data;
	if (dd->m4 == NULL) {
		m4ped_err("%s: M4 sensor data is NULL.\n", __func__);
		err = -ENODATA;
		goto m4ped_driver_init_fail;
	}

	INIT_DELAYED_WORK(&(dd->m4ped_work), m4ped_work_func);

	err = m4sensorhub_panic_register(dd->m4, PANICHDL_PEDOMETER_RESTORE,
					 m4ped_panic_restore, dd);
	if (err < 0)
		m4ped_err("Pedometer panic callbk register failed\n");

	goto m4ped_driver_init_exit;

m4ped_driver_init_fail:
	m4ped_err("%s: Init failed with error code %d.\n", __func__, err);
m4ped_driver_init_exit:
	mutex_unlock(&(dd->mutex));
	return err;
}

static int m4ped_probe(struct platform_device *pdev)
{
	struct m4ped_driver_data *dd = NULL;
	struct iio_dev *iio = NULL;
	int err = 0;

	iio = iio_device_alloc(sizeof(dd));
	if (iio == NULL) {
		m4ped_err("%s: Failed to allocate IIO data.\n", __func__);
		err = -ENOMEM;
		goto m4ped_probe_fail_noiio;
	}

	dd = iio_priv(iio);
	dd->pdev = pdev;
	mutex_init(&(dd->mutex));
	platform_set_drvdata(pdev, iio);
	dd->samplerate = -1; /* We always start disabled */
	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__);
		goto m4ped_probe_fail_noiio;
	}

	err = m4sensorhub_register_initcall(m4ped_driver_init, iio);
	if (err < 0) {
		m4ped_err("%s: Failed to register initcall.\n", __func__);
		goto m4ped_probe_fail;
	}

	return 0;

m4ped_probe_fail:
	m4ped_remove_iiodev(iio); /* iio and dd are freed here */
m4ped_probe_fail_noiio:
	m4ped_err("%s: Probe failed with error code %d.\n", __func__, err);
	return err;
}

static int __exit m4ped_remove(struct platform_device *pdev)
{
	struct iio_dev *iio = platform_get_drvdata(pdev);
	struct m4ped_driver_data *dd = NULL;

	if (iio == NULL)
		goto m4ped_remove_exit;

	dd = iio_priv(iio);
	if (dd == NULL)
		goto m4ped_remove_exit;

	mutex_lock(&(dd->mutex));
	m4sensorhub_unregister_initcall(m4ped_driver_init);
	m4ped_remove_iiodev(iio);  /* dd is freed here */

m4ped_remove_exit:
	return 0;
}

static struct of_device_id m4pedometer_match_tbl[] = {
	{ .compatible = "mot,m4pedometer" },
	{},
};

static struct platform_driver m4ped_driver = {
	.probe		= m4ped_probe,
	.remove		= __exit_p(m4ped_remove),
	.shutdown	= NULL,
	.suspend	= NULL,
	.resume		= NULL,
	.driver		= {
		.name	= M4PED_DRIVER_NAME,
		.owner	= THIS_MODULE,
		.of_match_table = of_match_ptr(m4pedometer_match_tbl),
	},
};

static int __init m4ped_init(void)
{
	return platform_driver_register(&m4ped_driver);
}

static void __exit m4ped_exit(void)
{
	platform_driver_unregister(&m4ped_driver);
}

module_init(m4ped_init);
module_exit(m4ped_exit);

MODULE_ALIAS("platform:m4ped");
MODULE_DESCRIPTION("M4 Sensor Hub Pedometer client driver");
MODULE_AUTHOR("Motorola");
MODULE_LICENSE("GPL");