iio: add driver for Freescale MMA9553

Add support for Freescale MMA9553L Intelligent Pedometer Platform.

The following functionalities are supported:
 - step counter (counts the number of steps using a HW register)
 - step detector (generates an iio event at every step the user takes)
 - activity recognition (rest, walking, jogging, running)
 - speed
 - calories
 - distance

To get accurate pedometer results, the user's height, weight and gender
need to be configured.

The specifications can be downloaded from:
http://www.freescale.com/files/sensors/doc/ref_manual/MMA955xLSWRM.pdf
http://www.freescale.com/files/sensors/doc/ref_manual/MMA9553LSWRM.pdf

Signed-off-by: Irina Tirdea <irina.tirdea@intel.com>
Signed-off-by: Jonathan Cameron <jic23@kernel.org>

5 files changed, 1544 insertions, 1 deletions

diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index c53047d28ad3..7c9a9a94a8ce 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -126,4 +126,14 @@ config MMA9551
 	  To compile this driver as a module, choose M here: the module
 	  will be called mma9551.
 
+config MMA9553
+	tristate "Freescale MMA9553L Intelligent Pedometer Platform Driver"
+	depends on I2C
+	select MMA9551_CORE
+	help
+	  Say yes here to build support for the Freescale MMA9553L
+	  Intelligent Pedometer Platform Driver.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mma9553.
 endmenu
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index 101a86d7b0a9..99d89e46cad1 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_MMA8452)	+= mma8452.o
 
 obj-$(CONFIG_MMA9551_CORE)	+= mma9551_core.o
 obj-$(CONFIG_MMA9551)		+= mma9551.o
+obj-$(CONFIG_MMA9553)		+= mma9553.o
 
 obj-$(CONFIG_IIO_SSP_SENSORS_COMMONS) += ssp_accel_sensor.o
 
diff --git a/drivers/iio/accel/mma9551_core.c b/drivers/iio/accel/mma9551_core.c
index 7f1a73e6aa36..7f55a6d7cd03 100644
--- a/drivers/iio/accel/mma9551_core.c
+++ b/drivers/iio/accel/mma9551_core.c
@@ -53,6 +53,11 @@
 #define MMA9551_AFE_Y_ACCEL_REG		0x02
 #define MMA9551_AFE_Z_ACCEL_REG		0x04
 
+/* Reset/Suspend/Clear application */
+#define MMA9551_RSC_RESET		0x00
+#define MMA9551_RSC_OFFSET(mask)	(3 - (ffs(mask) - 1) / 8)
+#define MMA9551_RSC_VAL(mask)		(mask >> (((ffs(mask) - 1) / 8) * 8))
+
 /*
  * A response is composed of:
  * - control registers: MB0-3
@@ -275,6 +280,64 @@ int mma9551_read_status_byte(struct i2c_client *client, u8 app_id,
 EXPORT_SYMBOL(mma9551_read_status_byte);
 
 /**
+ * mma9551_read_config_word() - read 1 config word
+ * @client:	I2C client
+ * @app_id:	Application ID
+ * @reg:	Application register
+ * @val:	Pointer to store value read
+ *
+ * Read one configuration word from the device using MMA955xL command format.
+ * Commands to the MMA955xL platform consist of a write followed by one or
+ * more reads.
+ *
+ * Locking note: This function must be called with the device lock held.
+ * Locking is not handled inside the function. Callers should ensure they
+ * serialize access to the HW.
+ *
+ * Returns: 0 on success, negative value on failure.
+ */
+int mma9551_read_config_word(struct i2c_client *client, u8 app_id,
+			    u16 reg, u16 *val)
+{
+	int ret;
+	__be16 v;
+
+	ret = mma9551_transfer(client, app_id, MMA9551_CMD_READ_CONFIG,
+			       reg, NULL, 0, (u8 *)&v, 2);
+	*val = be16_to_cpu(v);
+
+	return ret;
+}
+EXPORT_SYMBOL(mma9551_read_config_word);
+
+/**
+ * mma9551_write_config_word() - write 1 config word
+ * @client:	I2C client
+ * @app_id:	Application ID
+ * @reg:	Application register
+ * @val:	Value to write
+ *
+ * Write one configuration word from the device using MMA955xL command format.
+ * Commands to the MMA955xL platform consist of a write followed by one or
+ * more reads.
+ *
+ * Locking note: This function must be called with the device lock held.
+ * Locking is not handled inside the function. Callers should ensure they
+ * serialize access to the HW.
+ *
+ * Returns: 0 on success, negative value on failure.
+ */
+int mma9551_write_config_word(struct i2c_client *client, u8 app_id,
+			     u16 reg, u16 val)
+{
+	__be16 v = cpu_to_be16(val);
+
+	return mma9551_transfer(client, app_id, MMA9551_CMD_WRITE_CONFIG, reg,
+				(u8 *) &v, 2, NULL, 0);
+}
+EXPORT_SYMBOL(mma9551_write_config_word);
+
+/**
  * mma9551_read_status_word() - read 1 status word
  * @client:	I2C client
  * @app_id:	Application ID
@@ -306,6 +369,107 @@ int mma9551_read_status_word(struct i2c_client *client, u8 app_id,
 EXPORT_SYMBOL(mma9551_read_status_word);
 
 /**
+ * mma9551_read_config_words() - read multiple config words
+ * @client:	I2C client
+ * @app_id:	Application ID
+ * @reg:	Application register
+ * @len:	Length of array to read in bytes
+ * @val:	Array of words to read
+ *
+ * Read multiple configuration registers (word-sized registers).
+ *
+ * Locking note: This function must be called with the device lock held.
+ * Locking is not handled inside the function. Callers should ensure they
+ * serialize access to the HW.
+ *
+ * Returns: 0 on success, negative value on failure.
+ */
+int mma9551_read_config_words(struct i2c_client *client, u8 app_id,
+			     u16 reg, u8 len, u16 *buf)
+{
+	int ret, i;
+	int len_words = len / sizeof(u16);
+	__be16 be_buf[MMA9551_MAX_MAILBOX_DATA_REGS];
+
+	ret = mma9551_transfer(client, app_id, MMA9551_CMD_READ_CONFIG,
+			       reg, NULL, 0, (u8 *) be_buf, len);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < len_words; i++)
+		buf[i] = be16_to_cpu(be_buf[i]);
+
+	return 0;
+}
+EXPORT_SYMBOL(mma9551_read_config_words);
+
+/**
+ * mma9551_read_status_words() - read multiple status words
+ * @client:	I2C client
+ * @app_id:	Application ID
+ * @reg:	Application register
+ * @len:	Length of array to read in bytes
+ * @val:	Array of words to read
+ *
+ * Read multiple status registers (word-sized registers).
+ *
+ * Locking note: This function must be called with the device lock held.
+ * Locking is not handled inside the function. Callers should ensure they
+ * serialize access to the HW.
+ *
+ * Returns: 0 on success, negative value on failure.
+ */
+int mma9551_read_status_words(struct i2c_client *client, u8 app_id,
+			      u16 reg, u8 len, u16 *buf)
+{
+	int ret, i;
+	int len_words = len / sizeof(u16);
+	__be16 be_buf[MMA9551_MAX_MAILBOX_DATA_REGS];
+
+	ret = mma9551_transfer(client, app_id, MMA9551_CMD_READ_STATUS,
+			       reg, NULL, 0, (u8 *) be_buf, len);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < len_words; i++)
+		buf[i] = be16_to_cpu(be_buf[i]);
+
+	return 0;
+}
+EXPORT_SYMBOL(mma9551_read_status_words);
+
+/**
+ * mma9551_write_config_words() - write multiple config words
+ * @client:	I2C client
+ * @app_id:	Application ID
+ * @reg:	Application register
+ * @len:	Length of array to write in bytes
+ * @val:	Array of words to write
+ *
+ * Write multiple configuration registers (word-sized registers).
+ *
+ * Locking note: This function must be called with the device lock held.
+ * Locking is not handled inside the function. Callers should ensure they
+ * serialize access to the HW.
+ *
+ * Returns: 0 on success, negative value on failure.
+ */
+int mma9551_write_config_words(struct i2c_client *client, u8 app_id,
+			       u16 reg, u8 len, u16 *buf)
+{
+	int i;
+	int len_words = len / sizeof(u16);
+	__be16 be_buf[MMA9551_MAX_MAILBOX_DATA_REGS];
+
+	for (i = 0; i < len_words; i++)
+		be_buf[i] = cpu_to_be16(buf[i]);
+
+	return mma9551_transfer(client, app_id, MMA9551_CMD_WRITE_CONFIG,
+				reg, (u8 *) be_buf, len, NULL, 0);
+}
+EXPORT_SYMBOL(mma9551_write_config_words);
+
+/**
  * mma9551_update_config_bits() - update bits in register
  * @client:	I2C client
  * @app_id:	Application ID
@@ -609,6 +773,25 @@ int mma9551_read_accel_scale(int *val, int *val2)
 }
 EXPORT_SYMBOL(mma9551_read_accel_scale);
 
+/**
+ * mma9551_app_reset() - reset application
+ * @client:	I2C client
+ * @app_mask:	Application to reset
+ *
+ * Reset the given application (using the Reset/Suspend/Clear
+ * Control Application)
+ *
+ * Returns: 0 on success, negative value on failure.
+ */
+int mma9551_app_reset(struct i2c_client *client, u32 app_mask)
+{
+	return mma9551_write_config_byte(client, MMA9551_APPID_RCS,
+					 MMA9551_RSC_RESET +
+					 MMA9551_RSC_OFFSET(app_mask),
+					 MMA9551_RSC_VAL(app_mask));
+}
+EXPORT_SYMBOL(mma9551_app_reset);
+
 MODULE_AUTHOR("Irina Tirdea <irina.tirdea@intel.com>");
 MODULE_AUTHOR("Vlad Dogaru <vlad.dogaru@intel.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/mma9551_core.h b/drivers/iio/accel/mma9551_core.h
index e6efd027f49a..edaa56b1078e 100644
--- a/drivers/iio/accel/mma9551_core.h
+++ b/drivers/iio/accel/mma9551_core.h
@@ -21,9 +21,13 @@
 #define MMA9551_APPID_AFE		0x06
 #define MMA9551_APPID_TILT		0x0B
 #define MMA9551_APPID_SLEEP_WAKE	0x12
-#define MMA9551_APPID_RESET		0x17
+#define MMA9551_APPID_PEDOMETER	        0x15
+#define MMA9551_APPID_RCS		0x17
 #define MMA9551_APPID_NONE		0xff
 
+/* Reset/Suspend/Clear application app masks */
+#define MMA9551_RSC_PED			BIT(21)
+
 #define MMA9551_AUTO_SUSPEND_DELAY_MS	2000
 
 enum mma9551_gpio_pin {
@@ -48,8 +52,18 @@ int mma9551_write_config_byte(struct i2c_client *client, u8 app_id,
 			      u16 reg, u8 val);
 int mma9551_read_status_byte(struct i2c_client *client, u8 app_id,
 			     u16 reg, u8 *val);
+int mma9551_read_config_word(struct i2c_client *client, u8 app_id,
+			    u16 reg, u16 *val);
+int mma9551_write_config_word(struct i2c_client *client, u8 app_id,
+			     u16 reg, u16 val);
 int mma9551_read_status_word(struct i2c_client *client, u8 app_id,
 			     u16 reg, u16 *val);
+int mma9551_read_config_words(struct i2c_client *client, u8 app_id,
+			     u16 reg, u8 len, u16 *buf);
+int mma9551_read_status_words(struct i2c_client *client, u8 app_id,
+			      u16 reg, u8 len, u16 *buf);
+int mma9551_write_config_words(struct i2c_client *client, u8 app_id,
+			       u16 reg, u8 len, u16 *buf);
 int mma9551_update_config_bits(struct i2c_client *client, u8 app_id,
 			       u16 reg, u8 mask, u8 val);
 int mma9551_gpio_config(struct i2c_client *client, enum mma9551_gpio_pin pin,
@@ -62,5 +76,6 @@ int mma9551_read_accel_chan(struct i2c_client *client,
 			    const struct iio_chan_spec *chan,
 			    int *val, int *val2);
 int mma9551_read_accel_scale(int *val, int *val2);
+int mma9551_app_reset(struct i2c_client *client, u32 app_mask);
 
 #endif /* _MMA9551_CORE_H_ */
diff --git a/drivers/iio/accel/mma9553.c b/drivers/iio/accel/mma9553.c
new file mode 100644
index 000000000000..d23ebf192f63
--- /dev/null
+++ b/drivers/iio/accel/mma9553.c
@@ -0,0 +1,1334 @@
+/*
+ * Freescale MMA9553L Intelligent Pedometer driver
+ * Copyright (c) 2014, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/gpio/consumer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+#include <linux/pm_runtime.h>
+#include "mma9551_core.h"
+
+#define MMA9553_DRV_NAME			"mma9553"
+#define MMA9553_IRQ_NAME			"mma9553_event"
+#define MMA9553_GPIO_NAME			"mma9553_int"
+
+/* Pedometer configuration registers (R/W) */
+#define MMA9553_REG_CONF_SLEEPMIN		0x00
+#define MMA9553_REG_CONF_SLEEPMAX		0x02
+#define MMA9553_REG_CONF_SLEEPTHD		0x04
+#define MMA9553_MASK_CONF_WORD			GENMASK(15, 0)
+
+#define MMA9553_REG_CONF_CONF_STEPLEN		0x06
+#define MMA9553_MASK_CONF_CONFIG		BIT(15)
+#define MMA9553_MASK_CONF_ACT_DBCNTM		BIT(14)
+#define MMA9553_MASK_CONF_SLP_DBCNTM		BIT(13)
+#define MMA9553_MASK_CONF_STEPLEN		GENMASK(7, 0)
+
+#define MMA9553_REG_CONF_HEIGHT_WEIGHT		0x08
+#define MMA9553_MASK_CONF_HEIGHT		GENMASK(15, 8)
+#define MMA9553_MASK_CONF_WEIGHT		GENMASK(7, 0)
+
+#define MMA9553_REG_CONF_FILTER			0x0A
+#define MMA9553_MASK_CONF_FILTSTEP		GENMASK(15, 8)
+#define MMA9553_MASK_CONF_MALE			BIT(7)
+#define MMA9553_MASK_CONF_FILTTIME		GENMASK(6, 0)
+
+#define MMA9553_REG_CONF_SPEED_STEP		0x0C
+#define MMA9553_MASK_CONF_SPDPRD		GENMASK(15, 8)
+#define MMA9553_MASK_CONF_STEPCOALESCE		GENMASK(7, 0)
+
+#define MMA9553_REG_CONF_ACTTHD			0x0E
+
+/* Pedometer status registers (R-only) */
+#define MMA9553_REG_STATUS			0x00
+#define MMA9553_MASK_STATUS_MRGFL		BIT(15)
+#define MMA9553_MASK_STATUS_SUSPCHG		BIT(14)
+#define MMA9553_MASK_STATUS_STEPCHG		BIT(13)
+#define MMA9553_MASK_STATUS_ACTCHG		BIT(12)
+#define MMA9553_MASK_STATUS_SUSP		BIT(11)
+#define MMA9553_MASK_STATUS_ACTIVITY		(BIT(10) | BIT(9) | BIT(8))
+#define MMA9553_MASK_STATUS_VERSION		0x00FF
+
+#define MMA9553_REG_STEPCNT			0x02
+#define MMA9553_REG_DISTANCE			0x04
+#define MMA9553_REG_SPEED			0x06
+#define MMA9553_REG_CALORIES			0x08
+#define MMA9553_REG_SLEEPCNT			0x0A
+
+/* Pedometer events are always mapped to this pin. */
+#define MMA9553_DEFAULT_GPIO_PIN	mma9551_gpio6
+#define MMA9553_DEFAULT_GPIO_POLARITY	0
+
+/* Bitnum used for gpio configuration = bit number in high status byte */
+#define STATUS_TO_BITNUM(bit)		(ffs(bit) - 9)
+
+#define MMA9553_DEFAULT_SAMPLE_RATE	30	/* Hz */
+
+/*
+ * The internal activity level must be stable for ACTTHD samples before
+ * ACTIVITY is updated.The ACTIVITY variable contains the current activity
+ * level and is updated every time a step is detected or once a second
+ * if there are no steps.
+ */
+#define MMA9553_ACTIVITY_THD_TO_SEC(thd) ((thd) / MMA9553_DEFAULT_SAMPLE_RATE)
+#define MMA9553_ACTIVITY_SEC_TO_THD(sec) ((sec) * MMA9553_DEFAULT_SAMPLE_RATE)
+
+/*
+ * Autonomously suspend pedometer if acceleration vector magnitude
+ * is near 1g (4096 at 0.244 mg/LSB resolution) for 30 seconds.
+ */
+#define MMA9553_DEFAULT_SLEEPMIN	3688	/* 0,9 g */
+#define MMA9553_DEFAULT_SLEEPMAX	4508	/* 1,1 g */
+#define MMA9553_DEFAULT_SLEEPTHD	(MMA9553_DEFAULT_SAMPLE_RATE * 30)
+
+#define MMA9553_CONFIG_RETRIES		2
+
+/* Status register - activity field  */
+enum activity_level {
+	ACTIVITY_UNKNOWN,
+	ACTIVITY_REST,
+	ACTIVITY_WALKING,
+	ACTIVITY_JOGGING,
+	ACTIVITY_RUNNING,
+};
+
+static struct mma9553_event_info {
+	enum iio_chan_type type;
+	enum iio_modifier mod;
+	enum iio_event_direction dir;
+} mma9553_events_info[] = {
+	{
+		.type = IIO_STEPS,
+		.mod = IIO_NO_MOD,
+		.dir = IIO_EV_DIR_NONE,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_STILL,
+		.dir = IIO_EV_DIR_RISING,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_STILL,
+		.dir = IIO_EV_DIR_FALLING,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_WALKING,
+		.dir = IIO_EV_DIR_RISING,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_WALKING,
+		.dir = IIO_EV_DIR_FALLING,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_JOGGING,
+		.dir = IIO_EV_DIR_RISING,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_JOGGING,
+		.dir = IIO_EV_DIR_FALLING,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_RUNNING,
+		.dir = IIO_EV_DIR_RISING,
+	},
+	{
+		.type = IIO_ACTIVITY,
+		.mod = IIO_MOD_RUNNING,
+		.dir = IIO_EV_DIR_FALLING,
+	},
+};
+
+#define MMA9553_EVENTS_INFO_SIZE ARRAY_SIZE(mma9553_events_info)
+
+struct mma9553_event {
+	struct mma9553_event_info *info;
+	bool enabled;
+};
+
+struct mma9553_conf_regs {
+	u16 sleepmin;
+	u16 sleepmax;
+	u16 sleepthd;
+	u16 config;
+	u16 height_weight;
+	u16 filter;
+	u16 speed_step;
+	u16 actthd;
+} __packed;
+
+struct mma9553_data {
+	struct i2c_client *client;
+	struct mutex mutex;
+	struct mma9553_conf_regs conf;
+	struct mma9553_event events[MMA9553_EVENTS_INFO_SIZE];
+	int num_events;
+	u8 gpio_bitnum;
+	/*
+	 * This is used for all features that depend on step count:
+	 * step count, distance, speed, calories.
+	 */
+	bool stepcnt_enabled;
+	u16 stepcnt;
+	u8 activity;
+	s64 timestamp;
+};
+
+static u8 mma9553_get_bits(u16 val, u16 mask)
+{
+	return (val & mask) >> (ffs(mask) - 1);
+}
+
+static u16 mma9553_set_bits(u16 current_val, u16 val, u16 mask)
+{
+	return (current_val & ~mask) | (val << (ffs(mask) - 1));
+}
+
+static enum iio_modifier mma9553_activity_to_mod(enum activity_level activity)
+{
+	switch (activity) {
+	case ACTIVITY_RUNNING:
+		return IIO_MOD_RUNNING;
+	case ACTIVITY_JOGGING:
+		return IIO_MOD_JOGGING;
+	case ACTIVITY_WALKING:
+		return IIO_MOD_WALKING;
+	case ACTIVITY_REST:
+		return IIO_MOD_STILL;
+	case ACTIVITY_UNKNOWN:
+	default:
+		return IIO_NO_MOD;
+	}
+}
+
+static void mma9553_init_events(struct mma9553_data *data)
+{
+	int i;
+
+	data->num_events = MMA9553_EVENTS_INFO_SIZE;
+	for (i = 0; i < data->num_events; i++) {
+		data->events[i].info = &mma9553_events_info[i];
+		data->events[i].enabled = false;
+	}
+}
+
+static struct mma9553_event *mma9553_get_event(struct mma9553_data *data,
+					       enum iio_chan_type type,
+					       enum iio_modifier mod,
+					       enum iio_event_direction dir)
+{
+	int i;
+
+	for (i = 0; i < data->num_events; i++)
+		if (data->events[i].info->type == type &&
+		    data->events[i].info->mod == mod &&
+		    data->events[i].info->dir == dir)
+			return &data->events[i];
+
+	return NULL;
+}
+
+static bool mma9553_is_any_event_enabled(struct mma9553_data *data,
+					 bool check_type,
+					 enum iio_chan_type type)
+{
+	int i;
+
+	for (i = 0; i < data->num_events; i++)
+		if ((check_type && data->events[i].info->type == type &&
+		     data->events[i].enabled) ||
+		     (!check_type && data->events[i].enabled))
+			return true;
+
+	return false;
+}
+
+static int mma9553_set_config(struct mma9553_data *data, u16 reg,
+			      u16 *p_reg_val, u16 val, u16 mask)
+{
+	int ret, retries;
+	u16 reg_val, config;
+
+	reg_val = *p_reg_val;
+	if (val == mma9553_get_bits(reg_val, mask))
+		return 0;
+
+	reg_val = mma9553_set_bits(reg_val, val, mask);
+	ret = mma9551_write_config_word(data->client, MMA9551_APPID_PEDOMETER,
+					reg, reg_val);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"error writing config register 0x%x\n", reg);
+		return ret;
+	}
+
+	*p_reg_val = reg_val;
+
+	/* Reinitializes the pedometer with current configuration values */
+	config = mma9553_set_bits(data->conf.config, 1,
+				  MMA9553_MASK_CONF_CONFIG);
+
+	ret = mma9551_write_config_word(data->client, MMA9551_APPID_PEDOMETER,
+					MMA9553_REG_CONF_CONF_STEPLEN, config);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"error writing config register 0x%x\n",
+			MMA9553_REG_CONF_CONF_STEPLEN);
+		return ret;
+	}
+
+	retries = MMA9553_CONFIG_RETRIES;
+	do {
+		mma9551_sleep(MMA9553_DEFAULT_SAMPLE_RATE);
+		ret = mma9551_read_config_word(data->client,
+					       MMA9551_APPID_PEDOMETER,
+					       MMA9553_REG_CONF_CONF_STEPLEN,
+					       &config);
+		if (ret < 0)
+			return ret;
+	} while (mma9553_get_bits(config, MMA9553_MASK_CONF_CONFIG) &&
+		 --retries > 0);
+
+	return 0;
+}
+
+static int mma9553_read_activity_stepcnt(struct mma9553_data *data,
+					 u8 *activity, u16 *stepcnt)
+{
+	u32 status_stepcnt;
+	u16 status;
+	int ret;
+
+	ret = mma9551_read_status_words(data->client, MMA9551_APPID_PEDOMETER,
+					MMA9553_REG_STATUS, sizeof(u32),
+					(u16 *) &status_stepcnt);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"error reading status and stepcnt\n");
+		return ret;
+	}
+
+	status = status_stepcnt & MMA9553_MASK_CONF_WORD;
+	*activity = mma9553_get_bits(status, MMA9553_MASK_STATUS_ACTIVITY);
+	*stepcnt = status_stepcnt >> 16;
+
+	return 0;
+}
+
+static int mma9553_conf_gpio(struct mma9553_data *data)
+{
+	u8 bitnum = 0, appid = MMA9551_APPID_PEDOMETER;
+	int ret;
+	struct mma9553_event *ev_step_detect;
+	bool activity_enabled;
+
+	activity_enabled =
+	    mma9553_is_any_event_enabled(data, true, IIO_ACTIVITY);
+	ev_step_detect =
+	    mma9553_get_event(data, IIO_STEPS, IIO_NO_MOD, IIO_EV_DIR_NONE);
+
+	/*
+	 * If both step detector and activity are enabled, use the MRGFL bit.
+	 * This bit is the logical OR of the SUSPCHG, STEPCHG, and ACTCHG flags.
+	 */
+	if (activity_enabled && ev_step_detect->enabled)
+		bitnum = STATUS_TO_BITNUM(MMA9553_MASK_STATUS_MRGFL);
+	else if (ev_step_detect->enabled)
+		bitnum = STATUS_TO_BITNUM(MMA9553_MASK_STATUS_STEPCHG);
+	else if (activity_enabled)
+		bitnum = STATUS_TO_BITNUM(MMA9553_MASK_STATUS_ACTCHG);
+	else			/* Reset */
+		appid = MMA9551_APPID_NONE;
+
+	if (data->gpio_bitnum == bitnum)
+		return 0;
+
+	/* Save initial values for activity and stepcnt */
+	if (activity_enabled || ev_step_detect->enabled)
+		mma9553_read_activity_stepcnt(data, &data->activity,
+					      &data->stepcnt);
+
+	ret = mma9551_gpio_config(data->client,
+				  MMA9553_DEFAULT_GPIO_PIN,
+				  appid, bitnum, MMA9553_DEFAULT_GPIO_POLARITY);
+	if (ret < 0)
+		return ret;
+	data->gpio_bitnum = bitnum;
+
+	return 0;
+}
+
+static int mma9553_init(struct mma9553_data *data)
+{
+	int ret;
+
+	ret = mma9551_read_version(data->client);
+	if (ret)
+		return ret;
+
+	/*
+	 * Read all the pedometer configuration registers. This is used as
+	 * a device identification command to differentiate the MMA9553L
+	 * from the MMA9550L.
+	 */
+	ret =
+	    mma9551_read_config_words(data->client, MMA9551_APPID_PEDOMETER,
+				      MMA9553_REG_CONF_SLEEPMIN,
+				      sizeof(data->conf), (u16 *) &data->conf);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"device is not MMA9553L: failed to read cfg regs\n");
+		return ret;
+	}
+
+
+	/* Reset gpio */
+	data->gpio_bitnum = -1;
+	ret = mma9553_conf_gpio(data);
+	if (ret < 0)
+		return ret;
+
+	ret = mma9551_app_reset(data->client, MMA9551_RSC_PED);
+	if (ret < 0)
+		return ret;
+
+	/* Init config registers */
+	data->conf.sleepmin = MMA9553_DEFAULT_SLEEPMIN;
+	data->conf.sleepmax = MMA9553_DEFAULT_SLEEPMAX;
+	data->conf.sleepthd = MMA9553_DEFAULT_SLEEPTHD;
+	data->conf.config =
+	    mma9553_set_bits(data->conf.config, 1, MMA9553_MASK_CONF_CONFIG);
+	/*
+	 * Clear the activity debounce counter when the activity level changes,
+	 * so that the confidence level applies for any activity level.
+	 */
+	data->conf.config = mma9553_set_bits(data->conf.config, 1,
+					     MMA9553_MASK_CONF_ACT_DBCNTM);
+	ret =
+	    mma9551_write_config_words(data->client, MMA9551_APPID_PEDOMETER,
+				       MMA9553_REG_CONF_SLEEPMIN,
+				       sizeof(data->conf), (u16 *) &data->conf);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"failed to write configuration registers\n");
+		return ret;
+	}
+
+	return mma9551_set_device_state(data->client, true);
+}
+
+static int mma9553_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct mma9553_data *data = iio_priv(indio_dev);
+	int ret;
+	u16 tmp;
+	u8 activity;
+	bool powered_on;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		switch (chan->type) {
+		case IIO_STEPS:
+			/*
+			 * The HW only counts steps and other dependent
+			 * parameters (speed, distance, calories, activity)
+			 * if power is on (from enabling an event or the
+			 * step counter */
+			powered_on =
+			    mma9553_is_any_event_enabled(data, false, 0) ||
+			    data->stepcnt_enabled;
+			if (!powered_on) {
+				dev_err(&data->client->dev,
+					"No channels enabled\n");
+				return -EINVAL;
+			}
+			mutex_lock(&data->mutex);
+			ret = mma9551_read_status_word(data->client,
+						       MMA9551_APPID_PEDOMETER,
+						       MMA9553_REG_STEPCNT,
+						       &tmp);
+			mutex_unlock(&data->mutex);
+			if (ret < 0)
+				return ret;
+			*val = tmp;
+			return IIO_VAL_INT;
+		case IIO_DISTANCE:
+			powered_on =
+			    mma9553_is_any_event_enabled(data, false, 0) ||
+			    data->stepcnt_enabled;
+			if (!powered_on) {
+				dev_err(&data->client->dev,
+					"No channels enabled\n");
+				return -EINVAL;
+			}
+			mutex_lock(&data->mutex);
+			ret = mma9551_read_status_word(data->client,
+						       MMA9551_APPID_PEDOMETER,
+						       MMA9553_REG_DISTANCE,
+						       &tmp);
+			mutex_unlock(&data->mutex);
+			if (ret < 0)
+				return ret;
+			*val = tmp;
+			return IIO_VAL_INT;
+		case IIO_ACTIVITY:
+			powered_on =
+			    mma9553_is_any_event_enabled(data, false, 0) ||
+			    data->stepcnt_enabled;
+			if (!powered_on) {
+				dev_err(&data->client->dev,
+					"No channels enabled\n");
+				return -EINVAL;
+			}
+			mutex_lock(&data->mutex);
+			ret = mma9551_read_status_word(data->client,
+						       MMA9551_APPID_PEDOMETER,
+						       MMA9553_REG_STATUS,
+						       &tmp);
+			mutex_unlock(&data->mutex);
+			if (ret < 0)
+				return ret;
+
+			activity =
+			    mma9553_get_bits(tmp, MMA9553_MASK_STATUS_ACTIVITY);
+
+			/*
+			 * The device does not support confidence value levels,
+			 * so we will always have 100% for current activity and
+			 * 0% for the others.
+			 */
+			if (chan->channel2 == mma9553_activity_to_mod(activity))
+				*val = 100;
+			else
+				*val = 0;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_RAW:
+		switch (chan->type) {
+		case IIO_VELOCITY:	/* m/h */
+			if (chan->channel2 != IIO_MOD_ROOT_SUM_SQUARED_X_Y_Z)
+				return -EINVAL;
+			powered_on =
+			    mma9553_is_any_event_enabled(data, false, 0) ||
+			    data->stepcnt_enabled;
+			if (!powered_on) {
+				dev_err(&data->client->dev,
+					"No channels enabled\n");
+				return -EINVAL;
+			}
+			mutex_lock(&data->mutex);
+			ret = mma9551_read_status_word(data->client,
+						       MMA9551_APPID_PEDOMETER,
+						       MMA9553_REG_SPEED, &tmp);
+			mutex_unlock(&data->mutex);
+			if (ret < 0)
+				return ret;
+			*val = tmp;
+			return IIO_VAL_INT;
+		case IIO_ENERGY:	/* Cal or kcal */
+			powered_on =
+			    mma9553_is_any_event_enabled(data, false, 0) ||
+			    data->stepcnt_enabled;
+			if (!powered_on) {
+				dev_err(&data->client->dev,
+					"No channels enabled\n");
+				return -EINVAL;
+			}
+			mutex_lock(&data->mutex);
+			ret = mma9551_read_status_word(data->client,
+						       MMA9551_APPID_PEDOMETER,
+						       MMA9553_REG_CALORIES,
+						       &tmp);
+			mutex_unlock(&data->mutex);
+			if (ret < 0)
+				return ret;
+			*val = tmp;
+			return IIO_VAL_INT;
+		case IIO_ACCEL:
+			mutex_lock(&data->mutex);
+			ret = mma9551_read_accel_chan(data->client,
+						      chan, val, val2);
+			mutex_unlock(&data->mutex);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_VELOCITY:	/* m/h to m/s */
+			if (chan->channel2 != IIO_MOD_ROOT_SUM_SQUARED_X_Y_Z)
+				return -EINVAL;
+			*val = 0;
+			*val2 = 277;	/* 0.000277 */
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_ENERGY:	/* Cal or kcal to J */
+			*val = 4184;
+			return IIO_VAL_INT;
+		case IIO_ACCEL:
+			return mma9551_read_accel_scale(val, val2);
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_ENABLE:
+		*val = data->stepcnt_enabled;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBHEIGHT:
+		tmp = mma9553_get_bits(data->conf.height_weight,
+					MMA9553_MASK_CONF_HEIGHT);
+		*val = tmp / 100;	/* cm to m */
+		*val2 = (tmp % 100) * 10000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_CALIBWEIGHT:
+		*val = mma9553_get_bits(data->conf.height_weight,
+					MMA9553_MASK_CONF_WEIGHT);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_DEBOUNCE_COUNT:
+		switch (chan->type) {
+		case IIO_STEPS:
+			*val = mma9553_get_bits(data->conf.filter,
+						MMA9553_MASK_CONF_FILTSTEP);
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_DEBOUNCE_TIME:
+		switch (chan->type) {
+		case IIO_STEPS:
+			*val = mma9553_get_bits(data->conf.filter,
+						MMA9553_MASK_CONF_FILTTIME);
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_INT_TIME:
+		switch (chan->type) {
+		case IIO_VELOCITY:
+			if (chan->channel2 != IIO_MOD_ROOT_SUM_SQUARED_X_Y_Z)
+				return -EINVAL;
+			*val = mma9553_get_bits(data->conf.speed_step,
+						MMA9553_MASK_CONF_SPDPRD);
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mma9553_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	struct mma9553_data *data = iio_priv(indio_dev);
+	int ret, tmp;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_ENABLE:
+		if (data->stepcnt_enabled == !!val)
+			return 0;
+		mutex_lock(&data->mutex);
+		ret = mma9551_set_power_state(data->client, val);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+		data->stepcnt_enabled = val;
+		mutex_unlock(&data->mutex);
+		return 0;
+	case IIO_CHAN_INFO_CALIBHEIGHT:
+		/* m to cm */
+		tmp = val * 100 + val2 / 10000;
+		if (tmp < 0 || tmp > 255)
+			return -EINVAL;
+		mutex_lock(&data->mutex);
+		ret = mma9553_set_config(data,
+					 MMA9553_REG_CONF_HEIGHT_WEIGHT,
+					 &data->conf.height_weight,
+					 tmp, MMA9553_MASK_CONF_HEIGHT);
+		mutex_unlock(&data->mutex);
+		return ret;
+	case IIO_CHAN_INFO_CALIBWEIGHT:
+		if (val < 0 || val > 255)
+			return -EINVAL;
+		mutex_lock(&data->mutex);
+		ret = mma9553_set_config(data,
+					 MMA9553_REG_CONF_HEIGHT_WEIGHT,
+					 &data->conf.height_weight,
+					 val, MMA9553_MASK_CONF_WEIGHT);
+		mutex_unlock(&data->mutex);
+		return ret;
+	case IIO_CHAN_INFO_DEBOUNCE_COUNT:
+		switch (chan->type) {
+		case IIO_STEPS:
+			/*
+			 * Set to 0 to disable step filtering. If the value
+			 * specified is greater than 6, then 6 will be used.
+			 */
+			if (val < 0)
+				return -EINVAL;
+			if (val > 6)
+				val = 6;
+			mutex_lock(&data->mutex);
+			ret = mma9553_set_config(data, MMA9553_REG_CONF_FILTER,
+						 &data->conf.filter, val,
+						 MMA9553_MASK_CONF_FILTSTEP);
+			mutex_unlock(&data->mutex);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_DEBOUNCE_TIME:
+		switch (chan->type) {
+		case IIO_STEPS:
+			if (val < 0 || val > 127)
+				return -EINVAL;
+			mutex_lock(&data->mutex);
+			ret = mma9553_set_config(data, MMA9553_REG_CONF_FILTER,
+						 &data->conf.filter, val,
+						 MMA9553_MASK_CONF_FILTTIME);
+			mutex_unlock(&data->mutex);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_INT_TIME:
+		switch (chan->type) {
+		case IIO_VELOCITY:
+			if (chan->channel2 != IIO_MOD_ROOT_SUM_SQUARED_X_Y_Z)
+				return -EINVAL;
+			/*
+			 * If set to a value greater than 5, then 5 will be
+			 * used. Warning: Do not set SPDPRD to 0 or 1 as
+			 * this may cause undesirable behavior.
+			 */
+			if (val < 2)
+				return -EINVAL;
+			if (val > 5)
+				val = 5;
+			mutex_lock(&data->mutex);
+			ret = mma9553_set_config(data,
+						 MMA9553_REG_CONF_SPEED_STEP,
+						 &data->conf.speed_step, val,
+						 MMA9553_MASK_CONF_SPDPRD);
+			mutex_unlock(&data->mutex);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mma9553_read_event_config(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     enum iio_event_type type,
+				     enum iio_event_direction dir)
+{
+
+	struct mma9553_data *data = iio_priv(indio_dev);
+	struct mma9553_event *event;
+
+	event = mma9553_get_event(data, chan->type, chan->channel2, dir);
+	if (!event)
+		return -EINVAL;
+
+	return event->enabled;
+}
+
+static int mma9553_write_event_config(struct iio_dev *indio_dev,
+				      const struct iio_chan_spec *chan,
+				      enum iio_event_type type,
+				      enum iio_event_direction dir, int state)
+{
+	struct mma9553_data *data = iio_priv(indio_dev);
+	struct mma9553_event *event;
+	int ret;
+
+	event = mma9553_get_event(data, chan->type, chan->channel2, dir);
+	if (!event)
+		return -EINVAL;
+
+	if (event->enabled == state)
+		return 0;
+
+	mutex_lock(&data->mutex);
+
+	ret = mma9551_set_power_state(data->client, state);
+	if (ret < 0)
+		goto err_out;
+	event->enabled = state;
+
+	ret = mma9553_conf_gpio(data);
+	if (ret < 0)
+		goto err_conf_gpio;
+
+	mutex_unlock(&data-