#include <linux/err.h>
#include <linux/errno.h>
#include <linux/delay.h>
#include <linux/fs.h>
#include <linux/input.h>
#include <linux/uaccess.h>
#include <linux/workqueue.h>
#include <linux/irq.h>
#include <linux/gpio.h>
#include <linux/interrupt.h>
#include <linux/slab.h>
#include <linux/module.h>

#include "ais328dq.h"

#define SENSITIVITY_2G		1	/**	mg/LSB	*/
#define SENSITIVITY_4G		2	/**	mg/LSB	*/
#define SENSITIVITY_8G		4	/**	mg/LSB	*/

#define AXISDATA_REG		0x28
#define WHOAMI_AIS328DQ_ACC	0x32	/*	Expctd content for WAI	*/

/*	CONTROL REGISTERS ADDRESSES	*/
#define WHO_AM_I		0x0F	/*	WhoAmI register		*/
#define CTRL_REG1		0x20	/*				*/
#define CTRL_REG2		0x21	/*				*/
#define CTRL_REG3		0x22	/*				*/
#define CTRL_REG4		0x23	/*				*/
#define	CTRL_REG5		0x24	/*				*/

#define	INT_CFG1		0x30	/*	interrupt 1 config	*/
#define	INT_SRC1		0x31	/*	interrupt 1 source	*/
#define	INT_THS1		0x32	/*	interrupt 1 threshold	*/
#define	INT_DUR1		0x33	/*	interrupt 1 duration	*/

#define	INT_CFG2		0x34	/*	interrupt 2 config	*/
#define	INT_SRC2		0x35	/*	interrupt 2 source	*/
#define	INT_THS2		0x36	/*	interrupt 2 threshold	*/
#define	INT_DUR2		0x37	/*	interrupt 2 duration	*/
/*	end CONTROL REGISTRES ADDRESSES	*/

#define AIS328DQ_ACC_ENABLE_ALL_AXES	0x07
#define AIS328DQ_SELFTEST_EN		0x02
#define AIS328DQ_SELFTEST_DIS		0x00
#define AIS328DQ_SELFTEST_POS		0x00
#define AIS328DQ_SELFTEST_NEG		0x08

#define AIS328DQ_ACC_BDU_EN		0x80

/* Accelerometer output data rate  */
#define AIS328DQ_ACC_ODRHALF		0x40	/* 0.5Hz output data rate */
#define AIS328DQ_ACC_ODR1		0x60	/* 1Hz output data rate */
#define AIS328DQ_ACC_ODR2		0x80	/* 2Hz output data rate */
#define AIS328DQ_ACC_ODR5		0xA0	/* 5Hz output data rate */
#define AIS328DQ_ACC_ODR10		0xC0	/* 10Hz output data rate */
#define AIS328DQ_ACC_ODR50		0x00	/* 50Hz output data rate */
#define AIS328DQ_ACC_ODR100		0x08	/* 100Hz output data rate */
#define AIS328DQ_ACC_ODR400		0x10	/* 400Hz output data rate */
#define AIS328DQ_ACC_ODR1000		0x18	/* 1000Hz output data rate */

/* RESUME STATE INDICES */
#define	RES_CTRL_REG1		0
#define	RES_CTRL_REG2		1
#define	RES_CTRL_REG3		2
#define	RES_CTRL_REG4		3
#define	RES_CTRL_REG5		4
#define	RES_REFERENCE		5

#define	RES_INT_CFG1		6
#define	RES_INT_THS1		7
#define	RES_INT_DUR1		8
#define	RES_INT_CFG2		9
#define	RES_INT_THS2		10
#define	RES_INT_DUR2		11

/* end RESUME STATE INDICES */

#define INPUT_EVENT_TYPE		EV_MSC
#define INPUT_EVENT_X			MSC_SERIAL
#define INPUT_EVENT_Y			MSC_PULSELED
#define INPUT_EVENT_Z			MSC_GESTURE
#define INPUT_EVENT_TIME_MSB		MSC_SCAN
#define INPUT_EVENT_TIME_LSB		MSC_MAX

#define MS_TO_NS(x)	((x)*1000000L)

static struct {
unsigned int cutoff_ms;
unsigned int mask;
} ais328dq_acc_odr_table[] = {
{1, AIS328DQ_ACC_PM_NORMAL | AIS328DQ_ACC_ODR1000},
{3, AIS328DQ_ACC_PM_NORMAL | AIS328DQ_ACC_ODR400},
{10, AIS328DQ_ACC_PM_NORMAL | AIS328DQ_ACC_ODR100},
{20, AIS328DQ_ACC_PM_NORMAL | AIS328DQ_ACC_ODR50},
/* low power settings, max low pass filter cut-off freq */
{100, AIS328DQ_ACC_ODR10 | AIS328DQ_ACC_ODR1000},
{200, AIS328DQ_ACC_ODR5 | AIS328DQ_ACC_ODR1000},
{5000, AIS328DQ_ACC_ODR2 | AIS328DQ_ACC_ODR1000 },
{1000, AIS328DQ_ACC_ODR1 | AIS328DQ_ACC_ODR1000 },
{2000, AIS328DQ_ACC_ODRHALF | AIS328DQ_ACC_ODR1000 },
};

static struct ais328dq_acc_platform_data def_pdata = {
.poll_interval = 100,
.min_interval = 1,
.g_range = 2,

.axis_map_x = 0,
.axis_map_y = 1,
.axis_map_z = 2,

.gpio_int1 = -1,
.gpio_int2 = -1,
};

static inline s64 ais328dq_get_time_ns(void)
{
return ktime_to_ns(ktime_get_boottime());
}

static int ais328dq_acc_hw_init(struct ais328dq_acc_data *acc)
{
int err = -1;
u8 buf[4];

printk(KERN_INFO "%s: hw init start
", AIS328DQ_ACC_DEV_NAME);

err = acc->tf->read(acc->dev, WHO_AM_I, 1, buf);
if (err < 0){
dev_warn(acc->dev, "Error reading WHO_AM_I: is device "
"available/working?
");
goto err_firstread;
} else
acc->hw_working = 1;
if (buf[0] != WHOAMI_AIS328DQ_ACC) {
dev_err(acc->dev,
"device unknown. Expected: 0x%x,"
" Replies: 0x%x
", WHOAMI_AIS328DQ_ACC, buf[0]);
err = -1; /* choose the right coded error */
goto err_unknown_device;
}

buf[0] = acc->resume_state[RES_CTRL_REG1];
err = acc->tf->write(acc->dev, CTRL_REG1, 1, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_THS1];
buf[1] = acc->resume_state[RES_INT_DUR1];
err = acc->tf->write(acc->dev, INT_THS1, 2, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_CFG1];
err = acc->tf->write(acc->dev, INT_CFG1, 1, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_THS2];
buf[1] = acc->resume_state[RES_INT_DUR2];
err = acc->tf->write(acc->dev, INT_THS2, 2, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_CFG2];
err = acc->tf->write(acc->dev, INT_CFG2, 1, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_CTRL_REG2];
buf[1] = acc->resume_state[RES_CTRL_REG3];
buf[2] = acc->resume_state[RES_CTRL_REG4];
buf[3] = acc->resume_state[RES_CTRL_REG5];
err = acc->tf->write(acc->dev, CTRL_REG2, 4, buf);
if (err < 0)
goto err_resume_state;

acc->hw_initialized = 1;
printk(KERN_INFO "%s: hw init done
", AIS328DQ_ACC_DEV_NAME);
return 0;

err_firstread:
acc->hw_working = 0;
err_unknown_device:
err_resume_state:
acc->hw_initialized = 0;
dev_err(acc->dev, "hw init error 0x%x,0x%x: %d
", buf[0],
buf[1], err);
return err;
}

static void ais328dq_acc_device_power_off(struct ais328dq_acc_data *acc)
{
int err;
u8 buf[1] = {AIS328DQ_ACC_PM_OFF};

err = acc->tf->write(acc->dev, CTRL_REG1, 1, buf);
if (err < 0)
dev_err(acc->dev, "soft power off failed: %d
", err);

if (acc->pdata->power_off) {
if(acc->pdata->gpio_int1 >= 0)
disable_irq_nosync(acc->irq1);
if(acc->pdata->gpio_int2 >= 0)
disable_irq_nosync(acc->irq2);
acc->pdata->power_off();
acc->hw_initialized = 0;
}
if (acc->hw_initialized) {
if(acc->pdata->gpio_int1 >= 0)
disable_irq_nosync(acc->irq1);
if(acc->pdata->gpio_int2 >= 0)
disable_irq_nosync(acc->irq2);
acc->hw_initialized = 0;
}

}

static int ais328dq_acc_device_power_on(struct ais328dq_acc_data *acc)
{
int err = -1;

if (acc->pdata->power_on) {
err = acc->pdata->power_on();
if (err < 0) {
dev_err(acc->dev,
"power_on failed: %d
", err);
return err;
}
if(acc->pdata->gpio_int1 >= 0)
enable_irq(acc->irq1);
if(acc->pdata->gpio_int2 >= 0)
enable_irq(acc->irq2);
}

if (!acc->hw_initialized) {
err = ais328dq_acc_hw_init(acc);
if (acc->hw_working == 1 && err < 0) {
ais328dq_acc_device_power_off(acc);
return err;
}
}

if (acc->hw_initialized) {
if(acc->pdata->gpio_int1 >= 0)
enable_irq(acc->irq1);
if(acc->pdata->gpio_int2 >= 0)
enable_irq(acc->irq2);
}
return 0;
}

static irqreturn_t ais328dq_acc_isr1(int irq, void *dev)
{
struct ais328dq_acc_data *acc = dev;

disable_irq_nosync(irq);
queue_work(acc->irq1_work_queue, &acc->irq1_work);
printk(KERN_INFO "%s: isr1 queued
", AIS328DQ_ACC_DEV_NAME);

return IRQ_HANDLED;
}

static irqreturn_t ais328dq_acc_isr2(int irq, void *dev)
{
struct ais328dq_acc_data *acc = dev;

disable_irq_nosync(irq);
queue_work(acc->irq2_work_queue, &acc->irq2_work);
printk(KERN_INFO "%s: isr2 queued
", AIS328DQ_ACC_DEV_NAME);

return IRQ_HANDLED;
}

static void ais328dq_acc_irq1_work_func(struct work_struct *work)
{

struct ais328dq_acc_data *acc =
container_of(work, struct ais328dq_acc_data, irq1_work);
/* TODO  add interrupt service procedure.
ie:ais328dq_acc_get_int1_source(acc); */
;
/*  */
enable_irq(acc->irq1);
}

static void ais328dq_acc_irq2_work_func(struct work_struct *work)
{

struct ais328dq_acc_data *acc =
container_of(work, struct ais328dq_acc_data, irq2_work);
/* TODO  add interrupt service procedure.
ie:ais328dq_acc_get_tap_source(acc); */
;
/*  */

enable_irq(acc->irq2);
}

int ais328dq_acc_update_g_range(struct ais328dq_acc_data *acc, u8 range)
{
int err = -1;

u8 sensitivity;
u8 buf[1];
u8 updated_val;
u8 init_val;
u8 new_val, new_g_range;
u8 mask = AIS328DQ_ACC_FS_MASK;

switch (range) {
case 2:
new_g_range = AIS328DQ_ACC_G_2G;
sensitivity = SENSITIVITY_2G;
break;
case 4:
new_g_range = AIS328DQ_ACC_G_4G;
sensitivity = SENSITIVITY_4G;
break;
case 8:
new_g_range = AIS328DQ_ACC_G_8G;
sensitivity = SENSITIVITY_8G;
break;
default:
dev_err(acc->dev, "invalid g range requested: %u
", range);
return -EINVAL;
}

/* Set configuration register 4, which contains g range setting
*  NOTE: this is a straight overwrite because this driver does
*  not use any of the other configuration bits in this
*  register.  Should this become untrue, we will have to read
*  out the value and only change the relevant bits --XX----
*  (marked by X) */
err = acc->tf->read(acc->dev, CTRL_REG4, 1, buf);
if (err < 0)
goto error;

init_val = buf[0];
new_val = new_g_range;
updated_val = ((mask & new_val) | ((~mask) & init_val));

if (atomic_read(&acc->enabled)) {
buf[0] = updated_val;
err = acc->tf->write(acc->dev, CTRL_REG4, 1, buf);
if (err < 0)
goto error;
}

acc->resume_state[RES_CTRL_REG4] = updated_val;
acc->sensitivity = sensitivity;

return 0;

error:
dev_err(acc->dev, "update g range failed 0x%x: %d
",
buf[0], err);

return err;
}

int ais328dq_acc_update_odr(struct ais328dq_acc_data *acc,
int poll_interval_ms)
{
int err = -1;
int i;
u8 config[1];

/* Following, looks for the longest possible odr interval scrolling the
* odr_table vector from the end (shortest interval) backward (longest
* interval), to support the poll_interval requested by the system.
* It must be the longest interval lower then the poll interval.*/
for (i = ARRAY_SIZE(ais328dq_acc_odr_table) - 1; i >= 0; i--) {
if (ais328dq_acc_odr_table[i].cutoff_ms <= poll_interval_ms)
break;
}

config[0] = ais328dq_acc_odr_table[i].mask;
config[0] |= AIS328DQ_ACC_ENABLE_ALL_AXES;

/* If device is currently enabled, we need to write new
*  configuration out to it */
if (atomic_read(&acc->enabled)) {
err = acc->tf->write(acc->dev, CTRL_REG1, 1, config);
if (err < 0)
goto error;
}
acc->resume_state[RES_CTRL_REG1] = config[0];
acc->delta_ts = MS_TO_NS(poll_interval_ms);

return err;

error:
dev_err(acc->dev, "update odr failed 0x%x: %d
",
config[0], err);

return err;
}

static int ais328dq_acc_register_update(struct ais328dq_acc_data *acc,
u8 *buf, u8 reg_address, u8 mask, u8 new_bit_values)
{
int err = -1;
u8 init_val;
u8 updated_val;
err = acc->tf->read(acc->dev, reg_address, 1, buf);
if (!(err < 0)) {
init_val = buf[1];
updated_val = ((mask & new_bit_values) | (~mask & init_val));
err = acc->tf->write(acc->dev, reg_address, 1, &updated_val);
}

return err;
}

static int ais328dq_acc_selftest(struct ais328dq_acc_data *acc, u8 enable)
{
int err = -1;
u8 buf[2]={0x00,0x00};
char reg_address, mask, bit_values;

reg_address = CTRL_REG4;
mask = 0x0A;
if (enable > 0)
bit_values = AIS328DQ_SELFTEST_EN |
AIS328DQ_SELFTEST_POS;
else
bit_values = AIS328DQ_SELFTEST_DIS |
AIS328DQ_SELFTEST_POS;
if (atomic_read(&acc->enabled)) {
mutex_lock(&acc->lock);
err = ais328dq_acc_register_update(acc, buf, reg_address,
mask, bit_values);
acc->selftest_enabled = enable;
mutex_unlock(&acc->lock);
if (err < 0)
return err;
acc->resume_state[RES_CTRL_REG4] = ((mask & bit_values) |
( ~mask & acc->resume_state[RES_CTRL_REG4]));
}
return err;
}

static int ais328dq_acc_get_acceleration_data(struct ais328dq_acc_data *acc,
int *xyz)
{
int err = -1;
/* Data bytes from hardware xL, xH, yL, yH, zL, zH */
u8 acc_data[6];
/* x,y,z hardware data */
s16 hw_d[3] = { 0 };

err = acc->tf->read(acc->dev, AXISDATA_REG, 6, acc_data);
if (err < 0)
return err;

hw_d[0] = (((s16) ((acc_data[1] << 8) | acc_data[0])) >> 4);
hw_d[1] = (((s16) ((acc_data[3] << 8) | acc_data[2])) >> 4);
hw_d[2] = (((s16) ((acc_data[5] << 8) | acc_data[4])) >> 4);

hw_d[0] = hw_d[0] * acc->sensitivity;
hw_d[1] = hw_d[1] * acc->sensitivity;
hw_d[2] = hw_d[2] * acc->sensitivity;

xyz[0] = ((acc->pdata->negate_x) ? (-hw_d[acc->pdata->axis_map_x])
: (hw_d[acc->pdata->axis_map_x]));
xyz[1] = ((acc->pdata->negate_y) ? (-hw_d[acc->pdata->axis_map_y])
: (hw_d[acc->pdata->axis_map_y]));
xyz[2] = ((acc->pdata->negate_z) ? (-hw_d[acc->pdata->axis_map_z])
: (hw_d[acc->pdata->axis_map_z]));

return err;
}

static void ais328dq_acc_report_values(struct ais328dq_acc_data *acc,
int *xyz)
{
input_event(acc->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_X,
xyz[0]);
input_event(acc->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_Y,
xyz[1]);
input_event(acc->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_Z,
xyz[2]);
input_event(acc->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_TIME_MSB,
acc->timestamp >> 32);
input_event(acc->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_TIME_LSB,
acc->timestamp & 0xffffffff);
input_sync(acc->input_dev);
}

int ais328dq_acc_enable(struct ais328dq_acc_data *acc)
{
if (!atomic_cmpxchg(&acc->enabled, 0, 1)) {
int err;

mutex_lock(&acc->lock);
err = ais328dq_acc_device_power_on(acc);
if (err < 0) {
atomic_set(&acc->enabled, 0);
mutex_unlock(&acc->lock);
return err;
}

hrtimer_start(&acc->hr_timer, ktime_set(0, acc->delta_ts),
HRTIMER_MODE_REL);

mutex_unlock(&acc->lock);
}

return 0;
}
EXPORT_SYMBOL(ais328dq_acc_enable);

int ais328dq_acc_disable(struct ais328dq_acc_data *acc)
{
if (atomic_cmpxchg(&acc->enabled, 1, 0)) {
mutex_lock(&acc->lock);
ais328dq_acc_device_power_off(acc);
mutex_unlock(&acc->lock);

cancel_work_sync(&acc->poll_work);
hrtimer_cancel(&acc->hr_timer);
}

return 0;
}
EXPORT_SYMBOL(ais328dq_acc_disable);

static ssize_t read_single_reg(struct device *dev, char *buf, u8 reg)
{
u8 data;
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
int rc = 0;

mutex_lock(&acc->lock);
rc = acc->tf->read(acc->dev, reg, 1, &data);
mutex_unlock(&acc->lock);

/*TODO: error need to be managed */
return sprintf(buf, "0x%02x
", data);

}

static int write_reg(struct device *dev, const char *buf, u8 reg)
{
int rc = 0;
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
u8 x[1];
unsigned long val;

if (kstrtoul(buf, 16, &val))
return -EINVAL;

mutex_lock(&acc->lock);
x[0] = val;
rc = acc->tf->write(acc->dev, reg, 1, x);
mutex_unlock(&acc->lock);

/*TODO: error need to be managed */
return rc;
}

static ssize_t attr_get_polling_rate(struct device *dev,
struct device_attribute *attr,
char *buf)
{
int val;
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
mutex_lock(&acc->lock);
val = acc->pdata->poll_interval;
mutex_unlock(&acc->lock);
return sprintf(buf, "%d
", val);
}

static ssize_t attr_set_polling_rate(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
unsigned long interval_ms;

if (kstrtoul(buf, 10, &interval_ms))
return -EINVAL;
if (!interval_ms)
return -EINVAL;
mutex_lock(&acc->lock);
acc->pdata->poll_interval = interval_ms;
ais328dq_acc_update_odr(acc, interval_ms);
mutex_unlock(&acc->lock);

return size;
}

static ssize_t attr_get_range(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
char range = 2;

mutex_lock(&acc->lock);
range = acc->pdata->g_range ;
mutex_unlock(&acc->lock);

return sprintf(buf, "%d
", range);
}

static ssize_t attr_set_range(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
int err;
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
unsigned long val;

if (kstrtoul(buf, 10, &val))
return -EINVAL;

mutex_lock(&acc->lock);
err = ais328dq_acc_update_g_range(acc, val);
if (err == 0)
acc->pdata->g_range = val;
mutex_unlock(&acc->lock);

return !err ? size : err;
}

static ssize_t attr_get_enable(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
int val = atomic_read(&acc->enabled);
return sprintf(buf, "%d
", val);
}

static ssize_t attr_set_enable(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
unsigned long val;

if (kstrtoul(buf, 10, &val))
return -EINVAL;

if (val)
ais328dq_acc_enable(acc);
else
ais328dq_acc_disable(acc);

return size;
}

static ssize_t attr_get_selftest(struct device *dev,
struct device_attribute *attr, char *buf)
{
int val;
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
mutex_lock(&acc->lock);
val = acc->selftest_enabled;
mutex_unlock(&acc->lock);
return sprintf(buf, "%d
", val);
}

static ssize_t attr_set_selftest(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
unsigned long val;

if (kstrtoul(buf, 10, &val))
return -EINVAL;

ais328dq_acc_selftest(acc, val);

return size;
}

static ssize_t attr_set_intconfig1(struct device *dev,
struct device_attribute *attr,	const char *buf, size_t size)
{
return write_reg(dev, buf, INT_CFG1);
}

static ssize_t attr_get_intconfig1(struct device *dev,
struct device_attribute *attr,	char *buf)
{
return read_single_reg(dev, buf, INT_CFG1);
}

static ssize_t attr_set_duration1(struct device *dev,
struct device_attribute *attr,	const char *buf, size_t size)
{
return write_reg(dev, buf, INT_DUR1);
}

static ssize_t attr_get_duration1(struct device *dev,
struct device_attribute *attr,	char *buf)
{
return read_single_reg(dev, buf, INT_DUR1);
}

static ssize_t attr_set_thresh1(struct device *dev,
struct device_attribute *attr,	const char *buf, size_t size)
{
return write_reg(dev, buf, INT_THS1);
}

static ssize_t attr_get_thresh1(struct device *dev,
struct device_attribute *attr,	char *buf)
{
return read_single_reg(dev, buf, INT_THS1);
}

static ssize_t attr_get_source1(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_SRC1);
}

static ssize_t attr_set_intconfig2(struct device *dev,
struct device_attribute *attr,	const char *buf, size_t size)
{
return write_reg(dev, buf, INT_CFG2);
}

static ssize_t attr_get_intconfig2(struct device *dev,
struct device_attribute *attr,	char *buf)
{
return read_single_reg(dev, buf, INT_CFG2);
}

static ssize_t attr_set_duration2(struct device *dev,
struct device_attribute *attr,	const char *buf, size_t size)
{
return write_reg(dev, buf, INT_DUR2);
}

static ssize_t attr_get_duration2(struct device *dev,
struct device_attribute *attr,	char *buf)
{
return read_single_reg(dev, buf, INT_DUR2);
}

static ssize_t attr_set_thresh2(struct device *dev,
struct device_attribute *attr,	const char *buf, size_t size)
{
return write_reg(dev, buf, INT_THS2);
}

static ssize_t attr_get_thresh2(struct device *dev,
struct device_attribute *attr,	char *buf)
{
return read_single_reg(dev, buf, INT_THS2);
}
static ssize_t attr_get_source2(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_SRC2);
}

#ifdef AIS328DQ_DEBUG
/* PAY ATTENTION: These AIS328DQ_DEBUG funtions don't manage resume_state */
static ssize_t attr_reg_set(struct device *dev, struct device_attribute *attr,
const char *buf, size_t size)
{
int rc;
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
u8 x[2];
unsigned long val;

if (kstrtoul(buf, 16, &val))
return -EINVAL;
mutex_lock(&acc->lock);
x[0] = val;
rc = acc->tf->write(acc->dev, acc->reg_addr, 1, x);
mutex_unlock(&acc->lock);

/*TODO: error need to be managed */
return size;
}

static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr,
char *buf)
{
ssize_t ret;
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
int rc;
u8 data;

mutex_lock(&acc->lock);
err = acc->tf->read(acc->dev, acc->reg_addr, 1, &data);
mutex_unlock(&acc->lock);

/*TODO: error need to be managed */
ret = sprintf(buf, "0x%02x
", data);
return ret;
}

static ssize_t attr_addr_set(struct device *dev, struct device_attribute *attr,
const char *buf, size_t size)
{
struct ais328dq_acc_data *acc = dev_get_drvdata(dev);
unsigned long val;
if (kstrtoul(buf, 16, &val))
return -EINVAL;
mutex_lock(&acc->lock);
acc->reg_addr = val;
mutex_unlock(&acc->lock);
return size;
}
#endif

static struct device_attribute attributes[] = {

__ATTR(pollrate_ms, 0664, attr_get_polling_rate, attr_set_polling_rate),
__ATTR(range, 0664, attr_get_range, attr_set_range),
__ATTR(enable_device, 0664, attr_get_enable, attr_set_enable),
__ATTR(enable_selftest, 0664, attr_get_selftest, attr_set_selftest),
__ATTR(int1_config, 0664, attr_get_intconfig1, attr_set_intconfig1),
__ATTR(int1_duration, 0664, attr_get_duration1, attr_set_duration1),
__ATTR(int1_threshold, 0664, attr_get_thresh1, attr_set_thresh1),
__ATTR(int1_source, 0444, attr_get_source1, NULL),
__ATTR(int2_config, 0664, attr_get_intconfig2, attr_set_intconfig2),
__ATTR(int2_duration, 0664, attr_get_duration2, attr_set_duration2),
__ATTR(int2_threshold, 0664, attr_get_thresh2, attr_set_thresh2),
__ATTR(int2_source, 0444, attr_get_source2, NULL),
#ifdef AIS328DQ_DEBUG
__ATTR(reg_value, 0600, attr_reg_get, attr_reg_set),
__ATTR(reg_addr, 0200, NULL, attr_addr_set),
#endif
};

static int create_sysfs_interfaces(struct device *dev)
{
int i;
for (i = 0; i < ARRAY_SIZE(attributes); i++)
if (device_create_file(dev, attributes + i))
goto error;
return 0;

error:
for ( ; i >= 0; i--)
device_remove_file(dev, attributes + i);
dev_err(dev, "%s:Unable to create interface
", __func__);
return -1;
}

static int remove_sysfs_interfaces(struct device *dev)
{
int i;
for (i = 0; i < ARRAY_SIZE(attributes); i++)
device_remove_file(dev, attributes + i);
return 0;
}

static enum hrtimer_restart ais328dq_timer_poll(struct hrtimer *timer)
{
struct ais328dq_acc_data *acc;

acc = container_of((struct hrtimer *)timer, struct ais328dq_acc_data,
hr_timer);

acc->timestamp = ais328dq_get_time_ns();

queue_work(acc->work_queue, &acc->poll_work);

return HRTIMER_NORESTART;
}

static void ais328dq_poll_work_func(struct work_struct *work)
{
struct ais328dq_acc_data *acc;
int xyz[3] = { 0 };
int err;
s64 ts, delta;

acc = container_of((struct work_struct *)work,
struct ais328dq_acc_data, poll_work);

ts = ais328dq_get_time_ns();
delta = acc->delta_ts - (ts - acc->timestamp);
hrtimer_start(&acc->hr_timer, ktime_set(0, delta),
HRTIMER_MODE_REL);

mutex_lock(&acc->lock);
err = ais328dq_acc_get_acceleration_data(acc, xyz);
if (err < 0)
dev_err(acc->dev, "get_acceleration_data failed
");
else
ais328dq_acc_report_values(acc, xyz);

mutex_unlock(&acc->lock);
}

static int ais328dq_acc_validate_pdata(struct ais328dq_acc_data *acc)
{
acc->pdata->poll_interval = max(acc->pdata->poll_interval,
acc->pdata->min_interval);

if (acc->pdata->axis_map_x > 2 ||
acc->pdata->axis_map_y > 2 || acc->pdata->axis_map_z > 2) {
dev_err(acc->dev,
"invalid axis_map value x:%u y:%u z%u
",
acc->pdata->axis_map_x, acc->pdata->axis_map_y,
acc->pdata->axis_map_z);
return -EINVAL;
}

/* Only allow 0 and 1 for negation boolean flag */
if (acc->pdata->negate_x > 1 || acc->pdata->negate_y > 1 ||
acc->pdata->negate_z > 1) {
dev_err(acc->dev,
"invalid negate value x:%u y:%u z:%u
",
acc->pdata->negate_x, acc->pdata->negate_y,
acc->pdata->negate_z);
return -EINVAL;
}

/* Enforce minimum polling interval */
if (acc->pdata->poll_interval < acc->pdata->min_interval) {
dev_err(acc->dev, "minimum poll interval violated
");
return -EINVAL;
}

return 0;
}

static int ais328dq_acc_input_init(struct ais328dq_acc_data *acc)
{
int err;

acc->work_queue = create_workqueue("ais328dq_wq");
if (!acc->work_queue)
return -ENOMEM;

hrtimer_init(&acc->hr_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
acc->hr_timer.function = &ais328dq_timer_poll;

INIT_WORK(&acc->poll_work, ais328dq_poll_work_func);

acc->input_dev = input_allocate_device();
if (!acc->input_dev) {
err = -ENOMEM;
dev_err(acc->dev, "input device allocation failed
");
goto err0;
}

acc->input_dev->name = AIS328DQ_ACC_DEV_NAME;
acc->input_dev->id.bustype = acc->bus_type;
acc->input_dev->dev.parent = acc->dev;

input_set_drvdata(acc->input_dev, acc);

set_bit(INPUT_EVENT_TYPE, acc->input_dev->evbit);
set_bit(INPUT_EVENT_X, acc->input_dev->mscbit);
set_bit(INPUT_EVENT_Y, acc->input_dev->mscbit);
set_bit(INPUT_EVENT_Z, acc->input_dev->mscbit);
set_bit(INPUT_EVENT_TIME_MSB, acc->input_dev->mscbit);
set_bit(INPUT_EVENT_TIME_LSB, acc->input_dev->mscbit);

err = input_register_device(acc->input_dev);
if (err) {
dev_err(acc->dev,
"unable to register input device %s
",
acc->input_dev->name);
goto err1;
}

return 0;

err1:
input_free_device(acc->input_dev);
err0:
return err;
}

static void ais328dq_acc_input_cleanup(struct ais328dq_acc_data *acc)
{
if (acc->work_queue) {
flush_workqueue(acc->work_queue);
destroy_workqueue(acc->work_queue);
acc->work_queue = NULL;
}
input_unregister_device(acc->input_dev);
input_free_device(acc->input_dev);
}

int ais328dq_acc_probe(struct ais328dq_acc_data *acc)
{
int err = -1;

if (acc->dev->platform_data == NULL) {
dev_info(acc->dev,
"platform data is NULL. using default one.
");
acc->dev->platform_data = &def_pdata;
}

mutex_init(&acc->lock);
mutex_lock(&acc->lock);

acc->pdata = kmalloc(sizeof(*acc->pdata), GFP_KERNEL);
if (acc->pdata == NULL) {
err = -ENOMEM;
dev_err(acc->dev,
"failed to allocate memory for pdata: %d
",
err);
goto err_mutexunlock;
}

memcpy(acc->pdata, acc->dev->platform_data, sizeof(*acc->pdata));

err = ais328dq_acc_validate_pdata(acc);
if (err < 0) {
dev_err(acc->dev, "failed to validate platform data
");
goto exit_kfree_pdata;
}

if (acc->pdata->init) {
err = acc->pdata->init();
if (err < 0) {
dev_err(acc->dev, "init failed: %d
", err);
goto err_pdata_init;
}
}

if(acc->pdata->gpio_int1 >= 0){
acc->irq1 = gpio_to_irq(acc->pdata->gpio_int1);
printk(KERN_INFO "%s: %s has set irq1 to irq: %d "
"mapped on gpio:%d
",
AIS328DQ_ACC_DEV_NAME, __func__, acc->irq1,
acc->pdata->gpio_int1);
}

if(acc->pdata->gpio_int2 >= 0){
acc->irq2 = gpio_to_irq(acc->pdata->gpio_int2);
printk(KERN_INFO "%s: %s has set irq2 to irq: %d "
"mapped on gpio:%d
",
AIS328DQ_ACC_DEV_NAME, __func__, acc->irq2,
acc->pdata->gpio_int2);
}

memset(acc->resume_state, 0, ARRAY_SIZE(acc->resume_state));

acc->resume_state[RES_CTRL_REG1] = AIS328DQ_ACC_ENABLE_ALL_AXES;
acc->resume_state[RES_CTRL_REG4] = AIS328DQ_ACC_BDU_EN;

err = ais328dq_acc_device_power_on(acc);
if (err < 0) {
dev_err(acc->dev, "power on failed: %d
", err);
goto err_pdata_init;
}

atomic_set(&acc->enabled, 1);

err = ais328dq_acc_update_g_range(acc, acc->pdata->g_range);
if (err < 0) {
dev_err(acc->dev, "update_g_range failed
");
goto err_power_off;
}

err = ais328dq_acc_update_odr(acc, acc->pdata->poll_interval);
if (err < 0) {
dev_err(acc->dev, "update_odr failed
");
goto err_power_off;
}

err = ais328dq_acc_input_init(acc);
if (err < 0) {
dev_err(acc->dev, "input init failed
");
goto err_power_off;
}

err = create_sysfs_interfaces(acc->dev);
if (err < 0) {
dev_err(acc->dev,
"device AIS328DQ_ACC_DEV_NAME "
"sysfs register failed
");
goto err_input_cleanup;
}

ais328dq_acc_device_power_off(acc);

/* As default, do not report information */
atomic_set(&acc->enabled, 0);

if(acc->pdata->gpio_int1 >= 0){
INIT_WORK(&acc->irq1_work, ais328dq_acc_irq1_work_func);
acc->irq1_work_queue =
create_singlethread_workqueue("ais328dq_acc_wq1");
if (!acc->irq1_work_queue) {
err = -ENOMEM;
dev_err(acc->dev,
"cannot create work queue1: %d
", err);
goto err_remove_sysfs_int;
}
err = request_irq(acc->irq1, ais328dq_acc_isr1,
IRQF_TRIGGER_RISING, "ais328dq_acc_irq1", acc);
if (err < 0) {
dev_err(acc->dev, "request irq1 failed: %d
", err);
goto err_destoyworkqueue1;
}
disable_irq_nosync(acc->irq1);
}

if(acc->pdata->gpio_int2 >= 0){
INIT_WORK(&acc->irq2_work, ais328dq_acc_irq2_work_func);
acc->irq2_work_queue =
create_singlethread_workqueue("ais328dq_acc_wq2");
if (!acc->irq2_work_queue) {
err = -ENOMEM;
dev_err(acc->dev,
"cannot create work queue2: %d
", err);
goto err_free_irq1;
}
err = request_irq(acc->irq2, ais328dq_acc_isr2,
IRQF_TRIGGER_RISING, "ais328dq_acc_irq2", acc);
if (err < 0) {
dev_err(acc->dev, "request irq2 failed: %d
", err);
goto err_destoyworkqueue2;
}
disable_irq_nosync(acc->irq2);
}

mutex_unlock(&acc->lock);

return 0;

err_destoyworkqueue2:
if(acc->pdata->gpio_int2 >= 0)
destroy_workqueue(acc->irq2_work_queue);
err_free_irq1:
free_irq(acc->irq1, acc);
err_destoyworkqueue1:
if(acc->pdata->gpio_int1 >= 0)
destroy_workqueue(acc->irq1_work_queue);
err_remove_sysfs_int:
remove_sysfs_interfaces(acc->dev);
err_input_cleanup:
ais328dq_acc_input_cleanup(acc);
err_power_off:
ais328dq_acc_device_power_off(acc);
err_pdata_init:
if (acc->pdata->exit)
acc->pdata->exit();
exit_kfree_pdata:
kfree(acc->pdata);
err_mutexunlock:
mutex_unlock(&acc->lock);

return err;
}
EXPORT_SYMBOL(ais328dq_acc_probe);

int ais328dq_acc_remove(struct ais328dq_acc_data *acc)
{
if (acc->pdata->gpio_int1 >= 0){
free_irq(acc->irq1, acc);
gpio_free(acc->pdata->gpio_int1);
destroy_workqueue(acc->irq1_work_queue);
}

if (acc->pdata->gpio_int2 >= 0){
free_irq(acc->irq2, acc);
gpio_free(acc->pdata->gpio_int2);
destroy_workqueue(acc->irq2_work_queue);
}

ais328dq_acc_input_cleanup(acc);
ais328dq_acc_device_power_off(acc);
remove_sysfs_interfaces(acc->dev);

if (acc->pdata->exit)
acc->pdata->exit();
kfree(acc->pdata);

return 0;
}
EXPORT_SYMBOL(ais328dq_acc_remove);

MODULE_DESCRIPTION("ais328dq accelerometer sysfs driver");
MODULE_AUTHOR("Matteo Dameno, STMicroelectronics");
MODULE_AUTHOR("Lorenzo Bianconi, STMicroelectronics");
MODULE_LICENSE("GPL v2");


#include <linux/module.h>
#include <linux/err.h>
#include <linux/errno.h>
#include <linux/delay.h>
#include <linux/fs.h>
#include <linux/i2c.h>
#include <linux/input.h>
#include <linux/uaccess.h>
#include <linux/workqueue.h>
#include <linux/irq.h>
#include <linux/gpio.h>
#include <linux/interrupt.h>
#include <linux/slab.h>
#include <linux/version.h>
#include <linux/hrtimer.h>
#include <linux/ktime.h>

#ifdef CONFIG_OF
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/of_gpio.h>
#endif

#include "h3lis100dl.h"

#define MS_TO_NS(msec)          ((msec) * 1000 * 1000)

//#define	DEBUG	1

#define SENSITIVITY_100G	780	/**	mg/LSB	*/

#define AXISDATA_REG		0x28
#define WHOAMI_H3LIS100DL	0x32	/*	Expctd content for WAI	*/

/*	CONTROL REGISTERS	*/
#define WHO_AM_I		0x0F	/*	WhoAmI register		*/
#define CTRL_REG1		0x20	/*				*/
#define CTRL_REG2		0x21	/*				*/
#define CTRL_REG3		0x22	/*				*/
#define CTRL_REG4		0x23	/*				*/
#define	CTRL_REG5		0x24	/*				*/

#define	INT_CFG1		0x30	/*	interrupt 1 config	*/
#define	INT_SRC1		0x31	/*	interrupt 1 source	*/
#define	INT_THS1		0x32	/*	interrupt 1 threshold	*/
#define	INT_DUR1		0x33	/*	interrupt 1 duration	*/

#define	INT_CFG2		0x34	/*	interrupt 2 config	*/
#define	INT_SRC2		0x35	/*	interrupt 2 source	*/
#define	INT_THS2		0x36	/*	interrupt 2 threshold	*/
#define	INT_DUR2		0x37	/*	interrupt 2 duration	*/
/*	end CONTROL REGISTRES	*/

#define H3LIS100DL_ENABLE_ALL_AXES	0x07
#define H3LIS100DL_SELFTEST_EN	0x02
#define H3LIS100DL_SELFTEST_DIS	0x00
#define H3LIS100DL_SELFTEST_POS	0x00
#define H3LIS100DL_SELFTEST_NEG	0x08

/* Accelerometer output data rate  */
#define H3LIS100DL_ODRHALF	0x40	/* 0.5Hz output data rate */
#define H3LIS100DL_ODR1		0x60	/* 1Hz output data rate */
#define H3LIS100DL_ODR2		0x80	/* 2Hz output data rate */
#define H3LIS100DL_ODR5		0xA0	/* 5Hz output data rate */
#define H3LIS100DL_ODR10	0xC0	/* 10Hz output data rate */
#define H3LIS100DL_ODR50	0x00	/* 50Hz output data rate */
#define H3LIS100DL_ODR100	0x08	/* 100Hz output data rate */
#define H3LIS100DL_ODR400	0x10	/* 400Hz output data rate */
#define H3LIS100DL_ODR1000	0x18	/* 1000Hz output data rate */

/* RESUME STATE INDICES */
#define	RES_CTRL_REG1		0
#define	RES_CTRL_REG2		1
#define	RES_CTRL_REG3		2
#define	RES_CTRL_REG4		3
#define	RES_CTRL_REG5		4
#define	RES_REFERENCE		5

#define	RES_INT_CFG1		6
#define	RES_INT_THS1		7
#define	RES_INT_DUR1		8
#define	RES_INT_CFG2		9
#define	RES_INT_THS2		10
#define	RES_INT_DUR2		11

/* end RESUME STATE INDICES */

static struct {
unsigned int cutoff_ms;
unsigned int mask;
} h3lis100dl_odr_table[] = {
{1, H3LIS100DL_PM_NORMAL | H3LIS100DL_ODR1000},
{3, H3LIS100DL_PM_NORMAL | H3LIS100DL_ODR400},
{10, H3LIS100DL_PM_NORMAL | H3LIS100DL_ODR100},
{20, H3LIS100DL_PM_NORMAL | H3LIS100DL_ODR50},
/* low power settings, max low pass filter cut-off freq */
{100, H3LIS100DL_ODR10 | H3LIS100DL_ODR1000},
{200, H3LIS100DL_ODR5 | H3LIS100DL_ODR1000},
{5000, H3LIS100DL_ODR2 | H3LIS100DL_ODR1000 },
{1000, H3LIS100DL_ODR1 | H3LIS100DL_ODR1000 },
{2000, H3LIS100DL_ODRHALF | H3LIS100DL_ODR1000 },
};

static inline int64_t h3lis100dl_get_time_ns(void)
{
return ktime_to_ns(ktime_get_boottime());
}

static struct workqueue_struct *h3lis100dl_workqueue;

static int h3lis100dl_hw_init(struct h3lis100dl_data *acc)
{
int err = -1;
u8 buf[6];

#ifdef DEBUG
printk(KERN_INFO "%s: hw init start
", H3LIS100DL_DEV_NAME);
#endif

err = acc->tf->read(acc, WHO_AM_I, 1, buf);
if (err < 0){
dev_warn(acc->dev, "Error reading WHO_AM_I: is device "
"available/working?
");
goto err_firstread;
} else {
acc->hw_working = 1;
}
if (buf[0] != WHOAMI_H3LIS100DL) {
dev_err(acc->dev,
"device unknown. Expected: 0x%x,"
" Replies: 0x%x
", WHOAMI_H3LIS100DL, buf[0]);
err = -1; /* choose the right coded error */
goto err_unknown_device;
}

buf[0] = acc->resume_state[RES_CTRL_REG1];
err = acc->tf->write(acc, CTRL_REG1, 1, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_THS1];
buf[1] = acc->resume_state[RES_INT_DUR1];
err = acc->tf->write(acc, (AUTO_INCREMENT | INT_THS1), 2, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_CFG1];
err = acc->tf->write(acc, INT_CFG1, 1, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_THS2];
buf[1] = acc->resume_state[RES_INT_DUR2];
err = acc->tf->write(acc, (AUTO_INCREMENT | INT_THS2), 2, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_INT_CFG2];
err = acc->tf->write(acc, INT_CFG2, 1, buf);
if (err < 0)
goto err_resume_state;

buf[0] = acc->resume_state[RES_CTRL_REG2];
buf[1] = acc->resume_state[RES_CTRL_REG3];
buf[2] = acc->resume_state[RES_CTRL_REG4];
buf[3] = acc->resume_state[RES_CTRL_REG5];
err = acc->tf->write(acc, (AUTO_INCREMENT | CTRL_REG2), 4, buf);
if (err < 0)
goto err_resume_state;

acc->hw_initialized = 1;

#ifdef DEBUG
printk(KERN_INFO "%s: hw init done
", H3LIS100DL_DEV_NAME);
#endif
return 0;

err_firstread:
acc->hw_working = 0;
err_unknown_device:
err_resume_state:
acc->hw_initialized = 0;
dev_err(acc->dev, "hw init error 0x%x,0x%x: %d
", buf[0],
buf[1], err);
return err;
}

static void h3lis100dl_device_power_off(struct h3lis100dl_data *acc)
{
int err;
u8 buf = H3LIS100DL_PM_OFF;

err = acc->tf->write(acc, CTRL_REG1, 1, &buf);
if (err < 0)
dev_err(acc->dev, "soft power off failed: %d
", err);

if (acc->pdata->power_off) {
acc->pdata->power_off();
acc->hw_initialized = 0;
}
if (acc->hw_initialized)
acc->hw_initialized = 0;
}

static int h3lis100dl_device_power_on(struct h3lis100dl_data *acc)
{
int err = -1;

if (acc->pdata->power_on) {
err = acc->pdata->power_on();
if (err < 0) {
dev_err(acc->dev, "power_on failed: %d
", err);
return err;
}
}

if (!acc->hw_initialized) {
err = h3lis100dl_hw_init(acc);
if (acc->hw_working == 1 && err < 0) {
h3lis100dl_device_power_off(acc);
return err;
}
}

return 0;
}

/*
* hrtimer callback - it is called every polling_rate msec.
*/
enum hrtimer_restart h3lis100dl_timer_func_queue_work(struct hrtimer *timer)
{
struct h3lis100dl_data *acc;

acc = container_of((struct hrtimer *)timer,
struct h3lis100dl_data, hr_timer);

acc->timestamp = h3lis100dl_get_time_ns();
queue_work(h3lis100dl_workqueue, &acc->input_work);

return HRTIMER_NORESTART;
}

int h3lis100dl_update_g_range(struct h3lis100dl_data *acc, u8 new_g_range)
{
int err = -1;

u16 sensitivity;
u8 buf;
u8 updated_val;
u8 init_val;
u8 new_val;
u8 mask = H3LIS100DL_FS_MASK;

sensitivity = SENSITIVITY_100G;

if (atomic_read(&acc->enabled)) {
/* Set configuration register 4, which contains g range setting
*  NOTE: this is a straight overwrite because this driver does
*  not use any of the other configuration bits in this
*  register.  Should this become untrue, we will have to read
*  out the value and only change the relevant bits --XX----
*  (marked by X) */
err = acc->tf->read(acc, CTRL_REG4, 1, &buf);
if (err < 0)
goto error;

init_val = buf;
acc->resume_state[RES_CTRL_REG4] = init_val;
new_val = new_g_range;
updated_val = ((mask & new_val) | ((~mask) & init_val));
buf = updated_val;
err = acc->tf->write(acc, CTRL_REG4, 1, &buf);
if (err < 0)
goto error;
acc->resume_state[RES_CTRL_REG4] = updated_val;
acc->sensitivity = sensitivity;
}

return err;
error:
dev_err(acc->dev, "update g range failed 0x%x,0x%x: %d
",
CTRL_REG4, buf, err);

return err;
}

int h3lis100dl_update_odr(struct h3lis100dl_data *acc,
int poll_interval_ms)
{
int err = -1;
int i;
u8 config;

/* Following, looks for the longest possible odr interval scrolling the
* odr_table vector from the end (shortest interval) backward (longest
* interval), to support the poll_interval requested by the system.
* It must be the longest interval lower then the poll interval.*/
for (i = ARRAY_SIZE(h3lis100dl_odr_table) - 1; i >= 0; i--) {
if (h3lis100dl_odr_table[i].cutoff_ms <= poll_interval_ms)
break;
}
config = h3lis100dl_odr_table[i].mask;

config |= H3LIS100DL_ENABLE_ALL_AXES;

/* If device is currently enabled, we need to write new
*  configuration out to it */
if (atomic_read(&acc->enabled)) {
flush_workqueue(h3lis100dl_workqueue);

err = acc->tf->write(acc, CTRL_REG1, 1, &config);
if (err < 0)
goto error;
}
acc->resume_state[RES_CTRL_REG1] = config;
acc->ktime = ktime_set(0, MS_TO_NS(poll_interval_ms));

return err;

error:
dev_err(acc->dev, "update odr failed 0x%x,0x%x: %d
",
CTRL_REG1, config, err);

return err;
}

static int h3lis100dl_register_write(struct h3lis100dl_data *acc, u8 *buf,
u8 reg_address, u8 new_value)
{
int err = -1;

/* Sets configuration register at reg_address
*  NOTE: this is a straight overwrite  */
buf[0] = new_value;
err = acc->tf->write(acc, reg_address, 1, buf);
if (err < 0)
return err;

return err;
}

static int h3lis100dl_register_read(struct h3lis100dl_data *acc, u8 *buf,
u8 reg_address)
{

int err = -1;

err = acc->tf->read(acc, reg_address, 1, buf);

return err;
}

static int h3lis100dl_register_update(struct h3lis100dl_data *acc,
u8 *buf, u8 reg_address, u8 mask, u8 new_bit_values)
{
int err = -1;
u8 init_val;
u8 updated_val;
err = h3lis100dl_register_read(acc, buf, reg_address);
if (!(err < 0)) {
init_val = buf[1];
updated_val = ((mask & new_bit_values) | ((~mask) & init_val));
err = h3lis100dl_register_write(acc, buf, reg_address,
updated_val);
}
return err;
}

static int h3lis100dl_selftest(struct h3lis100dl_data *acc, u8 enable)
{
int err = -1;
u8 buf[2] = { 0x00, 0x00 };
char reg_address, mask, bit_values;

reg_address = CTRL_REG4;
mask = 0x0A;
if (enable > 0)
bit_values = H3LIS100DL_SELFTEST_EN |
H3LIS100DL_SELFTEST_POS;
else
bit_values = H3LIS100DL_SELFTEST_DIS |
H3LIS100DL_SELFTEST_POS;
if (atomic_read(&acc->enabled)) {
mutex_lock(&acc->lock);
err = h3lis100dl_register_update(acc, buf, reg_address,
mask, bit_values);
acc->selftest_enabled = enable;
mutex_unlock(&acc->lock);
if (err < 0)
return err;
acc->resume_state[RES_CTRL_REG4] = ((mask & bit_values) |
(~mask & acc->resume_state[RES_CTRL_REG4]));
}
return err;
}

static int h3lis100dl_get_acceleration_data(struct h3lis100dl_data *acc,
int *xyz)
{
int err = -1;
/* Data bytes from hardware xL, xH, yL, yH, zL, zH */
u8 acc_data[6];
/* x,y,z hardware data */
s16 hw_d[3] = { 0 };

err = acc->tf->read(acc, (AUTO_INCREMENT | AXISDATA_REG), 6, acc_data);
if (err < 0)
return err;

hw_d[0] = (s8)acc_data[1];
hw_d[1] = (s8)acc_data[3];
hw_d[2] = (s8)acc_data[5];

xyz[0] = hw_d[0] * acc->sensitivity;
xyz[1] = hw_d[1] * acc->sensitivity;
xyz[2] = hw_d[2] * acc->sensitivity;

#ifdef DEBUG
printk(KERN_INFO "%s read x=%d, y=%d, z=%d
",
H3LIS100DL_DEV_NAME, xyz[0], xyz[1], xyz[2]);
#endif
return err;
}

static void h3lis100dl_report_values(struct h3lis100dl_data *acc,
int *xyz)
{
struct input_dev  *input = acc->input_dev;

input_event(input, INPUT_EVENT_TYPE, INPUT_EVENT_X, xyz[0]);
input_event(input, INPUT_EVENT_TYPE, INPUT_EVENT_Y, xyz[1]);
input_event(input, INPUT_EVENT_TYPE, INPUT_EVENT_Z, xyz[2]);
input_event(input, INPUT_EVENT_TYPE, INPUT_EVENT_TIME_MSB,
acc->timestamp >> 32);
input_event(input, INPUT_EVENT_TYPE, INPUT_EVENT_TIME_LSB,
acc->timestamp & 0xffffffff);
input_sync(input);
}

static int h3lis100dl_enable(struct h3lis100dl_data *acc)
{
int err;

if (!atomic_cmpxchg(&acc->enabled, 0, 1)) {
err = h3lis100dl_device_power_on(acc);
if (err < 0) {
atomic_set(&acc->enabled, 0);
return err;
}

hrtimer_start(&acc->hr_timer, acc->ktime, HRTIMER_MODE_REL);
}

return 0;
}

static int h3lis100dl_disable(struct h3lis100dl_data *acc)
{
if (atomic_cmpxchg(&acc->enabled, 1, 0)) {
cancel_work_sync(&acc->input_work);
hrtimer_cancel(&acc->hr_timer);
h3lis100dl_device_power_off(acc);
}

return 0;
}

static ssize_t read_single_reg(struct device *dev, char *buf, u8 reg)
{
ssize_t ret;
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
int rc = 0;
u8 data;

rc = acc->tf->read(acc, reg, 1, &data);
/*TODO: error need to be managed */
ret = sprintf(buf, "0x%02x
", data);

return ret;
}

static int write_reg(struct device *dev, const char *buf, u8 reg)
{
int rc = 0;
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
unsigned long val;
u8 x[2];

if (kstrtoul(buf, 16, &val))
return -EINVAL;

x[0] = val;
rc = acc->tf->write(acc, reg, 1, x);
/*TODO: error need to be managed */
return rc;
}

static ssize_t attr_get_polling_rate(struct device *dev,
struct device_attribute *attr,
char *buf)
{
int val;
struct h3lis100dl_data *acc = dev_get_drvdata(dev);

mutex_lock(&acc->lock);
val = acc->pdata->poll_interval;
mutex_unlock(&acc->lock);
return sprintf(buf, "%d
", val);
}

static ssize_t attr_set_polling_rate(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
unsigned long interval_ms;

if (kstrtoul(buf, 10, &interval_ms))
return -EINVAL;
if (!interval_ms)
return -EINVAL;
mutex_lock(&acc->lock);
acc->pdata->poll_interval = interval_ms;
h3lis100dl_update_odr(acc, interval_ms);
mutex_unlock(&acc->lock);
return size;
}

static ssize_t attr_get_enable(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
int val = atomic_read(&acc->enabled);
return sprintf(buf, "%d
", val);
}

static ssize_t attr_set_enable(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
unsigned long val;

if (kstrtoul(buf, 10, &val))
return -EINVAL;

if (val)
h3lis100dl_enable(acc);
else
h3lis100dl_disable(acc);

return size;
}

static ssize_t attr_get_selftest(struct device *dev,
struct device_attribute *attr, char *buf)
{
int val;
struct h3lis100dl_data *acc = dev_get_drvdata(dev);

mutex_lock(&acc->lock);
val = acc->selftest_enabled;
mutex_unlock(&acc->lock);
return sprintf(buf, "%d
", val);
}

static ssize_t attr_set_selftest(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
unsigned long val;

if (kstrtoul(buf, 10, &val))
return -EINVAL;

h3lis100dl_selftest(acc, val);

return size;
}

static ssize_t attr_set_intconfig1(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
return write_reg(dev, buf, INT_CFG1);
}

static ssize_t attr_get_intconfig1(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_CFG1);
}

static ssize_t attr_set_duration1(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
return write_reg(dev, buf, INT_DUR1);
}

static ssize_t attr_get_duration1(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_DUR1);
}

static ssize_t attr_set_thresh1(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
return write_reg(dev, buf, INT_THS1);
}

static ssize_t attr_get_thresh1(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_THS1);
}

static ssize_t attr_get_source1(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_SRC1);
}

static ssize_t attr_set_intconfig2(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
return write_reg(dev, buf, INT_CFG2);
}

static ssize_t attr_get_intconfig2(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_CFG2);
}

static ssize_t attr_set_duration2(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
return write_reg(dev, buf, INT_DUR2);
}

static ssize_t attr_get_duration2(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_DUR2);
}

static ssize_t attr_set_thresh2(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
return write_reg(dev, buf, INT_THS2);
}

static ssize_t attr_get_thresh2(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_THS2);
}
static ssize_t attr_get_source2(struct device *dev,
struct device_attribute *attr, char *buf)
{
return read_single_reg(dev, buf, INT_SRC2);
}

#ifdef DEBUG
/* PAY ATTENTION: These DEBUG funtions don't manage resume_state */
static ssize_t attr_reg_set(struct device *dev, struct device_attribute *attr,
const char *buf, size_t size)
{
int rc;
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
u8 reg;
unsigned long val;

if (kstrtoul(buf, 16, &val))
return -EINVAL;

mutex_lock(&acc->lock);
reg = acc->reg_addr;
mutex_unlock(&acc->lock);

rc = acc->tf->write(acc, reg, 1, &val);
/*TODO: error need to be managed */
return size;
}

static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr,
char *buf)
{
ssize_t ret;
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
int rc;
u8 reg, val;

mutex_lock(&acc->lock);
reg = acc->reg_addr;
mutex_unlock(&acc->lock);
rc = acc->tf->read(acc, reg, 1, val);
/*TODO: error need to be managed */
ret = sprintf(buf, "0x%02x
", val);

return ret;
}

static ssize_t attr_addr_set(struct device *dev, struct device_attribute *attr,
const char *buf, size_t size)
{
struct h3lis100dl_data *acc = dev_get_drvdata(dev);
unsigned long val;

if (kstrtoul(buf, 16, &val))
return -EINVAL;

mutex_lock(&acc->lock);
acc->reg_addr = val;
mutex_unlock(&acc->lock);
return size;
}
#endif

static struct device_attribute attributes[] = {
__ATTR(pollrate_ms, 0664, attr_get_polling_rate, attr_set_polling_rate),
__ATTR(enable_device, 0664, attr_get_enable, attr_set_enable),
__ATTR(enable_selftest, 0664, attr_get_selftest, attr_set_selftest),
__ATTR(int1_config, 0664, attr_get_intconfig1, attr_set_intconfig1),
__ATTR(int1_duration, 0664, attr_get_duration1, attr_set_duration1),
__ATTR(int1_threshold, 0664, attr_get_thresh1, attr_set_thresh1),
__ATTR(int1_source, 0444, attr_get_source1, NULL),
__ATTR(int2_config, 0664, attr_get_intconfig2, attr_set_intconfig2),
__ATTR(int2_duration, 0664, attr_get_duration2, attr_set_duration2),
__ATTR(int2_threshold, 0664, attr_get_thresh2, attr_set_thresh2),
__ATTR(int2_source, 0444, attr_get_source2, NULL),
#ifdef DEBUG
__ATTR(reg_value, 0600, attr_reg_get, attr_reg_set),
__ATTR(reg_addr, 0200, NULL, attr_addr_set),
#endif
};

static int create_sysfs_interfaces(struct device *dev)
{
int i;
for (i = 0; i < ARRAY_SIZE(attributes); i++)
if (device_create_file(dev, attributes + i))
goto error;
return 0;

error:
for ( ; i >= 0; i--)
device_remove_file(dev, attributes + i);
dev_err(dev, "%s:Unable to create interface
", __func__);
return -1;
}

static int remove_sysfs_interfaces(struct device *dev)
{
int i;
for (i = 0; i < ARRAY_SIZE(attributes); i++)
device_remove_file(dev, attributes + i);
return 0;
}

/*
* Workqueue callback - it is called as soon as a job in the queue
* is scheduled.
*/
static void h3lis100dl_input_work_func(struct work_struct *work)
{
struct h3lis100dl_data *acc;
int xyz[3] = { 0 };
int err;

acc = container_of((struct work_struct *)work,
struct h3lis100dl_data, input_work);

hrtimer_start(&acc->hr_timer, acc->ktime, HRTIMER_MODE_REL);

err = h3lis100dl_get_acceleration_data(acc, xyz);
if (err < 0)
dev_err(acc->dev, "get_acceleration_data failed
");
else
h3lis100dl_report_values(acc, xyz);
}

static int h3lis100dl_validate_pdata(struct h3lis100dl_data *acc)
{
acc->pdata->poll_interval = max(acc->pdata->poll_interval,
acc->pdata->min_interval);

/* Enforce minimum polling interval */
if (acc->pdata->poll_interval < acc->pdata->min_interval) {
dev_err(acc->dev, "minimum poll interval violated
");
return -EINVAL;
}

return 0;
}

static int h3lis100dl_input_init(struct h3lis100dl_data *acc)
{
int err;

if (!h3lis100dl_workqueue)
h3lis100dl_workqueue = create_workqueue(acc->name);

if (!h3lis100dl_workqueue)
return -EINVAL;

INIT_WORK(&acc->input_work, h3lis100dl_input_work_func);
acc->input_dev = input_allocate_device();
if (!acc->input_dev) {
err = -ENOMEM;
dev_err(acc->dev, "input device allocation failed
");
goto err0;
}

acc->input_dev->name = H3LIS100DL_DEV_NAME;
acc->input_dev->id.bustype = acc->bustype;
acc->input_dev->dev.parent = acc->dev;

input_set_drvdata(acc->input_dev, acc);

set_bit(INPUT_EVENT_TYPE, acc->input_dev->evbit);
__set_bit(INPUT_EVENT_X, acc->input_dev->mscbit);
__set_bit(INPUT_EVENT_Y, acc->input_dev->mscbit);
__set_bit(INPUT_EVENT_Z, acc->input_dev->mscbit);
__set_bit(INPUT_EVENT_TIME_MSB, acc->input_dev->mscbit);
__set_bit(INPUT_EVENT_TIME_LSB, acc->input_dev->mscbit);

err = input_register_device(acc->input_dev);
if (err) {
dev_err(acc->dev,
"unable to register input device %s
",
acc->input_dev->name);
goto err1;
}

return 0;

err1:
input_free_device(acc->input_dev);
err0:
return err;
}

static void h3lis100dl_input_cleanup(struct h3lis100dl_data *acc)
{
input_unregister_device(acc->input_dev);
input_free_device(acc->input_dev);
}

#ifdef CONFIG_OF
static u32 h3lis100dl_parse_dt(struct h3lis100dl_data *cdata, struct device* dev)
{
u32 val;
struct device_node *np;

np = dev->of_node;
if (!np)
return -EINVAL;

if (!of_property_read_u32(np, "poll-interval", &val))
cdata->pdata->poll_interval = val;
else
cdata->pdata->poll_interval = 10;

cdata->pdata->init = NULL;
cdata->pdata->exit = NULL;
cdata->pdata->power_on = NULL;
cdata->pdata->power_off = NULL;

return 0;
}
#endif

int h3lis100dl_common_probe(struct h3lis100dl_data *acc)
{
int err = -1;

pr_info("%s: probe start.
", H3LIS100DL_DEV_NAME);

#ifndef CONFIG_OF
if (acc->dev->platform_data == NULL) {
dev_err(acc->dev, "platform data is NULL. exiting.
");
err = -ENODEV;
goto exit_check_functionality_failed;

}
#else
acc->dev->platform_data = NULL;
#endif

mutex_init(&acc->lock);
mutex_init(&acc->tb.buf_lock);
mutex_lock(&acc->lock);

acc->pdata = kzalloc(sizeof(struct h3lis100dl_platform_data), GFP_KERNEL);
if (acc->pdata == NULL) {
err = -ENOMEM;
dev_err(acc->dev,
"failed to allocate memory for pdata: %d
", err);
goto err_mutexunlock;
}

#ifdef CONFIG_OF
h3lis100dl_parse_dt(acc, acc->dev);
#else
memcpy(acc->pdata, acc->dev->platform_data, sizeof(*acc->pdata));
#endif

err = h3lis100dl_validate_pdata(acc);
if (err < 0) {
dev_err(acc->dev, "failed to validate platform data
");
goto exit_kfree_pdata;
}

acc->pdata->g_range = H3LIS100DL_G_100G;

if (acc->pdata->init) {
err = acc->pdata->init();
if (err < 0) {
dev_err(acc->dev, "init failed: %d
", err);
goto err_pdata_init;
}
}

memset(acc->resume_state, 0, ARRAY_SIZE(acc->resume_state));

acc->resume_state[RES_CTRL_REG1] = H3LIS100DL_ENABLE_ALL_AXES;

err = h3lis100dl_input_init(acc);
if (err < 0) {
dev_err(acc->dev, "input init failed
");
goto err_power_off;
}

err = h3lis100dl_device_power_on(acc);
if (err < 0) {
dev_err(acc->dev, "power on failed: %d
", err);
goto err_pdata_init;
}

atomic_set(&acc->enabled, 1);

err = h3lis100dl_update_g_range(acc, acc->pdata->g_range);
if (err < 0) {
dev_err(acc->dev, "update_g_range failed
");
goto err_power_off;
}

err = h3lis100dl_update_odr(acc, acc->pdata->poll_interval);
if (err < 0) {
dev_err(acc->dev, "update_odr failed
");
goto err_power_off;
}

err = create_sysfs_interfaces(acc->dev);
if (err < 0) {
dev_err(acc->dev,
"device H3LIS100DL_DEV_NAME "
"sysfs register failed
");
goto err_input_cleanup;
}

h3lis100dl_device_power_off(acc);

/* As default, do not report information */
atomic_set(&acc->enabled, 0);

hrtimer_init(&acc->hr_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
acc->hr_timer.function = &h3lis100dl_timer_func_queue_work;

mutex_unlock(&acc->lock);

dev_info(acc->dev, "%s: probed
", H3LIS100DL_DEV_NAME);

return 0;

err_input_cleanup:
h3lis100dl_input_cleanup(acc);
err_power_off:
h3lis100dl_device_power_off(acc);
err_pdata_init:
if (acc->pdata->exit)
acc->pdata->exit();
exit_kfree_pdata:
kfree(acc->pdata);
err_mutexunlock:
mutex_unlock(&acc->lock);
#ifndef CONFIG_OF
exit_check_functionality_failed:
#endif
printk(KERN_ERR "%s: Driver Init failed
", H3LIS100DL_DEV_NAME);

return err;
}
EXPORT_SYMBOL(h3lis100dl_common_probe);

int h3lis100dl_common_remove(struct h3lis100dl_data *acc)
{
h3lis100dl_input_cleanup(acc);
h3lis100dl_device_power_off(acc);
remove_sysfs_interfaces(acc->dev);

if (acc->pdata->exit)
acc->pdata->exit();
kfree(acc->pdata);

if(h3lis100dl_workqueue) {
flush_workqueue(h3lis100dl_workqueue);
destroy_workqueue(h3lis100dl_workqueue);
h3lis100dl_workqueue = NULL;
}

return 0;
}
EXPORT_SYMBOL(h3lis100dl_common_remove);

#ifdef CONFIG_PM_SLEEP
int h3lis100dl_common_resume(struct h3lis100dl_data *acc)
{
if (acc->on_before_suspend)
return h3lis100dl_enable(acc);
return 0;
}
EXPORT_SYMBOL(h3lis100dl_common_resume);

int h3lis100dl_common_suspend(struct h3lis100dl_data *acc)
{
acc->on_before_suspend = atomic_read(&acc->enabled);
return h3lis100dl_disable(acc);
}
EXPORT_SYMBOL(h3lis100dl_common_suspend);
#endif /* CONFIG_PM_SLEEP */

MODULE_DESCRIPTION("h3lis100dl accelerometer driver");
MODULE_AUTHOR("Armando Visconti, STMicroelectronics");
MODULE_LICENSE("GPL v2");


#include	<linux/err.h>
#include	<linux/errno.h>
#include	<linux/delay.h>
#include	<linux/fs.h>
#include	<linux/input.h>
#include	<linux/uaccess.h>
#include	<linux/workqueue.h>
#include	<linux/irq.h>
#include	<linux/gpio.h>
#include	<linux/interrupt.h>
#include	<linux/slab.h>
#include	<linux/kernel.h>
#include	<linux/device.h>
#include	<linux/module.h>
#include	<linux/moduleparam.h>
#include	<linux/version.h>

#include	"lis3dhh.h"

/* Acc. full scale fixed */
#define LIS3DHH_ACC_FULL_SCALE  "2.5"

/* Acc. sensitivity in ug/LSB */
#define LIS3DHH_SENSITIVITY	76

/* Acc. number of samples to discard for stable data */
#define LIS3DHH_DISC_SAMPLE 10

/* Acc. operating mode */
#define LIS3DHH_ENABLE	0x80
#define LIS3DHH_DISABLE	0x00

/* Acc. ID */
#define LIS3DHH_WHOAMI	0x11

/* Device registers */
#define WHO_AM_I		0x0F
#define CTRL1			0x20
#define STATUS			0x27
#define AXISDATA_REG	0x28

/* Register bitmasks */
#define CTRL1_EN_MODE   	0x80
#define CTRL1_EN_MASK   	0x80

#define CTRL1_BDU_ENABLE	0x08
#define CTRL1_BDU_MASK		0x08

#define CTRL1_IF_ADD_INC	0x40
#define CTRL1_IF_ADD_INC_MASK	0x40

static inline int64_t lis3dhh_acc_get_time_ns(void)
{
return ktime_to_ns(ktime_get_boottime());
}

static int lis3dhh_acc_hw_init(struct lis3dhh_acc_status *stat)
{
int err;

stat->resume_state |= CTRL1_EN_MODE;

err = stat->tf->write(stat, CTRL1, 1, &stat->resume_state);
if (err < 0)
return err;

stat->hw_initialized = 1;

return 0;
}

static void lis3dhh_acc_device_power_off(struct lis3dhh_acc_status *stat)
{
int err;
u8 buf = (CTRL1_BDU_ENABLE | CTRL1_IF_ADD_INC);

err = stat->tf->write(stat, CTRL1, 1, &buf);
if (err < 0)
dev_err(stat->dev, "soft power off failed: %d
", err);

if (stat->pdata->power_off) {
stat->pdata->power_off();
stat->hw_initialized = 0;
}

if (stat->hw_initialized) {
stat->hw_initialized = 0;
}
}

static int lis3dhh_acc_device_power_on(struct lis3dhh_acc_status *stat)
{
int err;

if (stat->pdata->power_on) {
err = stat->pdata->power_on();
if (err < 0) {
dev_err(stat->dev,
"power_on failed: %d
", err);
return err;
}
}

if (!stat->hw_initialized) {
err = lis3dhh_acc_hw_init(stat);
if (stat->hw_working == 1 && err < 0) {
lis3dhh_acc_device_power_off(stat);
return err;
}
}

return 0;
}

static void lis3dhh_acc_report_values(struct lis3dhh_acc_status *stat,
int *xyz, int64_t timestamp)
{
input_event(stat->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_X, xyz[0]);
input_event(stat->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_Y, xyz[1]);
input_event(stat->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_Z, xyz[2]);
input_event(stat->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_TIME_MSB,
timestamp >> 32);
input_event(stat->input_dev, INPUT_EVENT_TYPE, INPUT_EVENT_TIME_LSB,
timestamp & 0xffffffff);
input_sync(stat->input_dev);
}

static void lis3dhh_acc_report_triple(struct lis3dhh_acc_status *stat)
{
int err;
u8 acc_data[6];
int xyz[3] = { 0 };

err = stat->tf->read(stat, AXISDATA_REG, 6, acc_data);
if (err < 0)
return;

xyz[0] = (s32)((s16)(acc_data[0] | (acc_data[1] << 8)));
xyz[1] = (s32)((s16)(acc_data[2] | (acc_data[3] << 8)));
xyz[2] = (s32)((s16)(acc_data[4] | (acc_data[5] << 8)));
xyz[0] *= LIS3DHH_SENSITIVITY;
xyz[1] *= LIS3DHH_SENSITIVITY;
xyz[2] *= LIS3DHH_SENSITIVITY;

if (stat->sample_to_discard)
stat->sample_to_discard--;
else
lis3dhh_acc_report_values(stat, xyz, stat->timestamp);
}

static int lis3dhh_acc_enable(struct lis3dhh_acc_status *stat)
{
int err;

if (!atomic_cmpxchg(&stat->enabled, 0, 1)) {
err = lis3dhh_acc_device_power_on(stat);
if (err < 0) {
atomic_set(&stat->enabled, 0);
return err;
}

/* Dischard first # samples. */
stat->sample_to_discard = LIS3DHH_DISC_SAMPLE;
stat->polling_ktime = ktime_set(0,
(LIS3DHH_MIN_POLL_PERIOD_NS % 1000000));
hrtimer_start(&stat->hr_timer_poll,
stat->polling_ktime, HRTIMER_MODE_REL);
}

return 0;
}

static int lis3dhh_acc_disable(struct lis3dhh_acc_status *stat)
{
if (atomic_cmpxchg(&stat->enabled, 1, 0)) {
cancel_work_sync(&stat->input_poll_work);
lis3dhh_acc_device_power_off(stat);
}

return 0;
}

/*
* Device doesn't support change in ODR HW so we left it run at 1.1 kHz
* and change only the timer period
*/
static ssize_t attr_set_polling_rate(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
return size;
}

static ssize_t attr_get_polling_rate(struct device *dev,
struct device_attribute *attr,
char *buf)
{
return sprintf(buf, "%d.%d
", LIS3DHH_MIN_POLL_PERIOD_NS / 1000000,
(LIS3DHH_MIN_POLL_PERIOD_NS % 1000000) / 1000);
}

static ssize_t attr_set_range(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct lis3dhh_acc_status *stat = dev_get_drvdata(dev);

dev_info(stat->dev, "range set fixed to %s
", LIS3DHH_ACC_FULL_SCALE);

return size;
}

static ssize_t attr_get_range(struct device *dev,
struct device_attribute *attr, char *buf)
{
return sprintf(buf, "%s
", LIS3DHH_ACC_FULL_SCALE);
}

static ssize_t attr_get_enable(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct lis3dhh_acc_status *stat = dev_get_drvdata(dev);
int val = atomic_read(&stat->enabled);

return sprintf(buf, "%d
", val);
}

static ssize_t attr_set_enable(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct lis3dhh_acc_status *stat = dev_get_drvdata(dev);
unsigned long val;

if (kstrtoul(buf, 10, &val))
return -EINVAL;

if (val)
lis3dhh_acc_enable(stat);
else
lis3dhh_acc_disable(stat);

return size;
}

static struct device_attribute attributes[] = {
__ATTR(pollrate_ms, 0664, attr_get_polling_rate, attr_set_polling_rate),
__ATTR(enable_device, 0664, attr_get_enable, attr_set_enable),
__ATTR(range, 0664, attr_get_range, attr_set_range),
};

static int create_sysfs_interfaces(struct device *dev)
{
int i;

for (i = 0; i < ARRAY_SIZE(attributes); i++)
if (device_create_file(dev, attributes + i))
goto error;

return 0;

error:
for ( ; i >= 0; i--)
device_remove_file(dev, attributes + i);
dev_err(dev, "%s:Unable to create interface
", __func__);

return -1;
}

static int remove_sysfs_interfaces(struct device *dev)
{
int i;

for (i = 0; i < ARRAY_SIZE(attributes); i++)
device_remove_file(dev, attributes + i);

return 0;
}

static void lis3dhh_acc_input_poll_work_func(struct work_struct *work)
{
struct lis3dhh_acc_status *stat;
ktime_t tmpkt, ktdelta;

stat = container_of((struct work_struct *) work,
struct lis3dhh_acc_status, input_poll_work);

if (atomic_read(&stat->enabled)) {
/* Adjust new timeout. */
ktdelta = ktime_set(0, lis3dhh_acc_get_time_ns() - stat->timestamp);
/* Avoid negative value in case of High ODR. */
if (ktime_after(stat->polling_ktime, ktdelta))
tmpkt = ktime_sub(stat->polling_ktime, ktdelta);
else
tmpkt = stat->polling_ktime;

hrtimer_start(&stat->hr_timer_poll, tmpkt, HRTIMER_MODE_REL);
}

lis3dhh_acc_report_triple(stat);
}

enum hrtimer_restart lis3dhh_acc_hr_timer_poll_function(struct hrtimer *timer)
{
struct lis3dhh_acc_status *stat;

stat = container_of((struct hrtimer *)timer,
struct lis3dhh_acc_status, hr_timer_poll);

stat->timestamp = lis3dhh_acc_get_time_ns();

queue_work(stat->hr_timer_poll_work_queue, &stat->input_poll_work);

return HRTIMER_NORESTART;
}

int lis3dhh_acc_input_open(struct input_dev *input)
{
struct lis3dhh_acc_status *stat = input_get_drvdata(input);

return lis3dhh_acc_enable(stat);
}

void lis3dhh_acc_input_close(struct input_dev *dev)
{
struct lis3dhh_acc_status *stat = input_get_drvdata(dev);

lis3dhh_acc_disable(stat);
}

static int lis3dhh_acc_input_init(struct lis3dhh_acc_status *stat)
{
int err;

INIT_WORK(&stat->input_poll_work, lis3dhh_acc_input_poll_work_func);
stat->input_dev = input_allocate_device();
if (!stat->input_dev) {
err = -ENOMEM;
dev_err(stat->dev, "input device allocation failed
");
return err;
}

stat->input_dev->open = lis3dhh_acc_input_open;
stat->input_dev->close = lis3dhh_acc_input_close;
stat->input_dev->name = stat->name;
stat->input_dev->id.bustype = stat->bustype;
stat->input_dev->dev.parent = stat->dev;
input_set_drvdata(stat->input_dev, stat);

/* Set Misc event type */
__set_bit(INPUT_EVENT_TYPE, stat->input_dev->evbit);
__set_bit(INPUT_EVENT_X, stat->input_dev->mscbit);
__set_bit(INPUT_EVENT_Y, stat->input_dev->mscbit);
__set_bit(INPUT_EVENT_Z, stat->input_dev->mscbit);
__set_bit(INPUT_EVENT_TIME_MSB, stat->input_dev->mscbit);
__set_bit(INPUT_EVENT_TIME_LSB, stat->input_dev->mscbit);

err = input_register_device(stat->input_dev);
if (err) {
dev_err(stat->dev, "unable to register input device %s
",
stat->input_dev->name);
input_free_device(stat->input_dev);
return err;
}

return 0;
}

static void lis3dhh_acc_input_cleanup(struct lis3dhh_acc_status *stat)
{
input_unregister_device(stat->input_dev);
input_free_device(stat->input_dev);
}

static int lis3dhh_discover(struct lis3dhh_acc_status *stat)
{
int err;
u8 wai = 0x00;

err = stat->tf->read(stat, WHO_AM_I, 1, &wai);
if (err < 0) {
dev_warn(stat->dev, "failed to read Who-Am-I register");
return err;
}

if (wai != LIS3DHH_WHOAMI) {
dev_err(stat->dev, "device unknown. Expected: 0x%02x,"
" Replies: 0x%02x
", LIS3DHH_WHOAMI, wai);
return -ENODEV;
}

stat->hw_working = 1;

return 0;
}

int lis3dhh_common_probe(struct lis3dhh_acc_status *stat)
{
int err;

mutex_init(&stat->tb.buf_lock);

err = lis3dhh_discover(stat);
if (err < 0)
return err;

stat->pdata = kzalloc(sizeof(*stat->pdata), GFP_KERNEL);
if (stat->pdata == NULL) {
dev_err(stat->dev, "failed to allocate memory for pdata: %d
", err);
return -ENOMEM;
}

/* Attach if exists custom platform data/functions */
if (stat->dev->platform_data != NULL)
memcpy(stat->pdata, stat->dev->platform_data, sizeof(*stat->pdata));

if (stat->pdata->init) {
err = stat->pdata->init();
if (err < 0) {
dev_err(stat->dev, "init failed: %d
", err);
goto err_pdata_init;
}
}

/* Set registers initial value */
stat->resume_state = (CTRL1_BDU_ENABLE | CTRL1_IF_ADD_INC);

err = lis3dhh_acc_device_power_on(stat);
if (err < 0) {
dev_err(stat->dev, "power on failed: %d
", err);
goto err_pdata_init;
}

atomic_set(&stat->enabled, 1);

/* Init polling workqueue */
stat->hr_timer_poll_work_queue =
create_workqueue("lis3dhh_acc_hr_timer_poll_wq");
if (!stat->hr_timer_poll_work_queue) {
dev_err(stat->dev, "create workqueue failed
");
err = -ENOMEM;
goto err_wk_null;
}

hrtimer_init(&stat->hr_timer_poll, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
stat->hr_timer_poll.function = &lis3dhh_acc_hr_timer_poll_function;

err = lis3dhh_acc_input_init(stat);
if (err < 0) {
dev_err(stat->dev, "input init failed
");
goto err_remove_hr_work_queue;
}

err = create_sysfs_interfaces(stat->dev);
if (err < 0) {
dev_err(stat->dev, "device %s sysfs register failed
",
LIS3DHH_ACC_DEV_NAME);
goto err_input_cleanup;
}

lis3dhh_acc_device_power_off(stat);

/* As default, do not report information */
atomic_set(&stat->enabled, 0);

dev_info(stat->dev, "%s: probed
", LIS3DHH_ACC_DEV_NAME);

return 0;

err_input_cleanup:
lis3dhh_acc_input_cleanup(stat);

err_remove_hr_work_queue:
if (stat->hr_timer_poll_work_queue) {
flush_workqueue(stat->hr_timer_poll_work_queue);
destroy_workqueue(stat->hr_timer_poll_work_queue);
stat->hr_timer_poll_work_queue = NULL;
}

err_wk_null:
lis3dhh_acc_device_power_off(stat);

err_pdata_init:
if (stat->pdata->exit)
stat->pdata->exit();
kfree(stat->pdata);
pr_err("%s: Driver Init failed
", LIS3DHH_ACC_DEV_NAME);

return err;
}
EXPORT_SYMBOL(lis3dhh_common_probe);

int lis3dhh_common_remove(struct lis3dhh_acc_status *stat)
{
dev_info(stat->dev, "driver removing
");

lis3dhh_acc_disable(stat);
lis3dhh_acc_input_cleanup(stat);
remove_sysfs_interfaces(stat->dev);

if (stat->hr_timer_poll_work_queue) {
flush_workqueue(stat->hr_timer_poll_work_queue);
destroy_workqueue(stat->hr_timer_poll_work_queue);
stat->hr_timer_poll_work_queue = NULL;
}

if (stat->pdata->exit)
stat->pdata->exit();

if (stat->pdata) {
kfree(stat->pdata);
stat->pdata = NULL;
}

return 0;
}
EXPORT_SYMBOL(lis3dhh_common_remove);

#ifdef CONFIG_PM_SLEEP
int lis3dhh_common_resume(struct lis3dhh_acc_status *stat)
{
if (stat->on_before_suspend)
return lis3dhh_acc_enable(stat);

return 0;
}
EXPORT_SYMBOL(lis3dhh_common_resume);

int lis3dhh_common_suspend(struct lis3dhh_acc_status *stat)
{
stat->on_before_suspend = atomic_read(&stat->enabled);

return lis3dhh_acc_disable(stat);
}
EXPORT_SYMBOL(lis3dhh_common_suspend);
#endif /* CONFIG_PM_SLEEP */

MODULE_DESCRIPTION("lis3dhh acc driver");
MODULE_AUTHOR("Mario Tesi, STMicroelectronics");
MODULE_LICENSE("GPL v2");


#include <linux/bitfield.h>
#include <linux/module.h>
#include <linux/regmap.h>

#include "adxl313.h"

static const struct regmap_range adxl312_readable_reg_range[] = {
regmap_reg_range(ADXL313_REG_DEVID0, ADXL313_REG_DEVID0),
regmap_reg_range(ADXL313_REG_OFS_AXIS(0), ADXL313_REG_OFS_AXIS(2)),
regmap_reg_range(ADXL313_REG_THRESH_ACT, ADXL313_REG_ACT_INACT_CTL),
regmap_reg_range(ADXL313_REG_BW_RATE, ADXL313_REG_FIFO_STATUS),
};

static const struct regmap_range adxl313_readable_reg_range[] = {
regmap_reg_range(ADXL313_REG_DEVID0, ADXL313_REG_XID),
regmap_reg_range(ADXL313_REG_SOFT_RESET, ADXL313_REG_SOFT_RESET),
regmap_reg_range(ADXL313_REG_OFS_AXIS(0), ADXL313_REG_OFS_AXIS(2)),
regmap_reg_range(ADXL313_REG_THRESH_ACT, ADXL313_REG_ACT_INACT_CTL),
regmap_reg_range(ADXL313_REG_BW_RATE, ADXL313_REG_FIFO_STATUS),
};

const struct regmap_access_table adxl312_readable_regs_table = {
.yes_ranges = adxl312_readable_reg_range,
.n_yes_ranges = ARRAY_SIZE(adxl312_readable_reg_range),
};
EXPORT_SYMBOL_NS_GPL(adxl312_readable_regs_table, IIO_ADXL313);

const struct regmap_access_table adxl313_readable_regs_table = {
.yes_ranges = adxl313_readable_reg_range,
.n_yes_ranges = ARRAY_SIZE(adxl313_readable_reg_range),
};
EXPORT_SYMBOL_NS_GPL(adxl313_readable_regs_table, IIO_ADXL313);

const struct regmap_access_table adxl314_readable_regs_table = {
.yes_ranges = adxl312_readable_reg_range,
.n_yes_ranges = ARRAY_SIZE(adxl312_readable_reg_range),
};
EXPORT_SYMBOL_NS_GPL(adxl314_readable_regs_table, IIO_ADXL313);

static int adxl312_check_id(struct device *dev,
struct adxl313_data *data)
{
unsigned int regval;
int ret;

ret = regmap_read(data->regmap, ADXL313_REG_DEVID0, &regval);
if (ret)
return ret;

if (regval != ADXL313_DEVID0_ADXL312_314)
dev_warn(dev, "Invalid manufacturer ID: %#02x
", regval);

return 0;
}

static int adxl313_check_id(struct device *dev,
struct adxl313_data *data)
{
unsigned int regval;
int ret;

ret = regmap_read(data->regmap, ADXL313_REG_DEVID0, &regval);
if (ret)
return ret;

if (regval != ADXL313_DEVID0)
dev_warn(dev, "Invalid manufacturer ID: 0x%02x
", regval);

/* Check DEVID1 and PARTID */
if (regval == ADXL313_DEVID0) {
ret = regmap_read(data->regmap, ADXL313_REG_DEVID1, &regval);
if (ret)
return ret;

if (regval != ADXL313_DEVID1)
dev_warn(dev, "Invalid mems ID: 0x%02x
", regval);

ret = regmap_read(data->regmap, ADXL313_REG_PARTID, &regval);
if (ret)
return ret;

if (regval != ADXL313_PARTID)
dev_warn(dev, "Invalid device ID: 0x%02x
", regval);
}

return 0;
}

const struct adxl313_chip_info adxl31x_chip_info[] = {
[ADXL312] = {
.name = "adxl312",
.type = ADXL312,
.scale_factor = 28425072,
.variable_range = true,
.soft_reset = false,
.check_id = &adxl312_check_id,
},
[ADXL313] = {
.name = "adxl313",
.type = ADXL313,
.scale_factor = 9576806,
.variable_range = true,
.soft_reset = true,
.check_id = &adxl313_check_id,
},
[ADXL314] = {
.name = "adxl314",
.type = ADXL314,
.scale_factor = 478858719,
.variable_range = false,
.soft_reset = false,
.check_id = &adxl312_check_id,
},
};
EXPORT_SYMBOL_NS_GPL(adxl31x_chip_info, IIO_ADXL313);

static const struct regmap_range adxl312_writable_reg_range[] = {
regmap_reg_range(ADXL313_REG_OFS_AXIS(0), ADXL313_REG_OFS_AXIS(2)),
regmap_reg_range(ADXL313_REG_THRESH_ACT, ADXL313_REG_ACT_INACT_CTL),
regmap_reg_range(ADXL313_REG_BW_RATE, ADXL313_REG_INT_MAP),
regmap_reg_range(ADXL313_REG_DATA_FORMAT, ADXL313_REG_DATA_FORMAT),
regmap_reg_range(ADXL313_REG_FIFO_CTL, ADXL313_REG_FIFO_CTL),
};

static const struct regmap_range adxl313_writable_reg_range[] = {
regmap_reg_range(ADXL313_REG_SOFT_RESET, ADXL313_REG_SOFT_RESET),
regmap_reg_range(ADXL313_REG_OFS_AXIS(0), ADXL313_REG_OFS_AXIS(2)),
regmap_reg_range(ADXL313_REG_THRESH_ACT, ADXL313_REG_ACT_INACT_CTL),
regmap_reg_range(ADXL313_REG_BW_RATE, ADXL313_REG_INT_MAP),
regmap_reg_range(ADXL313_REG_DATA_FORMAT, ADXL313_REG_DATA_FORMAT),
regmap_reg_range(ADXL313_REG_FIFO_CTL, ADXL313_REG_FIFO_CTL),
};

const struct regmap_access_table adxl312_writable_regs_table = {
.yes_ranges = adxl312_writable_reg_range,
.n_yes_ranges = ARRAY_SIZE(adxl312_writable_reg_range),
};
EXPORT_SYMBOL_NS_GPL(adxl312_writable_regs_table, IIO_ADXL313);

const struct regmap_access_table adxl313_writable_regs_table = {
.yes_ranges = adxl313_writable_reg_range,
.n_yes_ranges = ARRAY_SIZE(adxl313_writable_reg_range),
};
EXPORT_SYMBOL_NS_GPL(adxl313_writable_regs_table, IIO_ADXL313);

const struct regmap_access_table adxl314_writable_regs_table = {
.yes_ranges = adxl312_writable_reg_range,
.n_yes_ranges = ARRAY_SIZE(adxl312_writable_reg_range),
};
EXPORT_SYMBOL_NS_GPL(adxl314_writable_regs_table, IIO_ADXL313);

static const int adxl313_odr_freqs[][2] = {
[0] = { 6, 250000 },
[1] = { 12, 500000 },
[2] = { 25, 0 },
[3] = { 50, 0 },
[4] = { 100, 0 },
[5] = { 200, 0 },
[6] = { 400, 0 },
[7] = { 800, 0 },
[8] = { 1600, 0 },
[9] = { 3200, 0 },
};

#define ADXL313_ACCEL_CHANNEL(index, axis) {				\n.type = IIO_ACCEL,						\n.address = index,						\n.modified = 1,							\n.channel2 = IIO_MOD_##axis,					\n.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\nBIT(IIO_CHAN_INFO_CALIBBIAS),		\n.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |		\nBIT(IIO_CHAN_INFO_SAMP_FREQ),	\n.info_mask_shared_by_type_available =				\nBIT(IIO_CHAN_INFO_SAMP_FREQ),				\n.scan_type = {							\n.realbits = 13,						\n},								\n}

static const struct iio_chan_spec adxl313_channels[] = {
ADXL313_ACCEL_CHANNEL(0, X),
ADXL313_ACCEL_CHANNEL(1, Y),
ADXL313_ACCEL_CHANNEL(2, Z),
};

static int adxl313_set_odr(struct adxl313_data *data,
unsigned int freq1, unsigned int freq2)
{
unsigned int i;

for (i = 0; i < ARRAY_SIZE(adxl313_odr_freqs); i++) {
if (adxl313_odr_freqs[i][0] == freq1 &&
adxl313_odr_freqs[i][1] == freq2)
break;
}

if (i == ARRAY_SIZE(adxl313_odr_freqs))
return -EINVAL;

return regmap_update_bits(data->regmap, ADXL313_REG_BW_RATE,
ADXL313_RATE_MSK,
FIELD_PREP(ADXL313_RATE_MSK, ADXL313_RATE_BASE + i));
}

static int adxl313_read_axis(struct adxl313_data *data,
struct iio_chan_spec const *chan)
{
int ret;

mutex_lock(&data->lock);

ret = regmap_bulk_read(data->regmap,
ADXL313_REG_DATA_AXIS(chan->address),
&data->transf_buf, sizeof(data->transf_buf));
if (ret)
goto unlock_ret;

ret = le16_to_cpu(data->transf_buf);

unlock_ret:
mutex_unlock(&data->lock);
return ret;
}

static int adxl313_read_freq_avail(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
const int **vals, int *type, int *length,
long mask)
{
switch (mask) {
case IIO_CHAN_INFO_SAMP_FREQ:
*vals = (const int *)adxl313_odr_freqs;
*length = ARRAY_SIZE(adxl313_odr_freqs) * 2;
*type = IIO_VAL_INT_PLUS_MICRO;
return IIO_AVAIL_LIST;
default:
return -EINVAL;
}
}

static int adxl313_read_raw(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int *val, int *val2, long mask)
{
struct adxl313_data *data = iio_priv(indio_dev);
unsigned int regval;
int ret;

switch (mask) {
case IIO_CHAN_INFO_RAW:
ret = adxl313_read_axis(data, chan);
if (ret < 0)
return ret;

*val = sign_extend32(ret, chan->scan_type.realbits - 1);
return IIO_VAL_INT;
case IIO_CHAN_INFO_SCALE:
*val = 0;

*val2 = data->chip_info->scale_factor;

return IIO_VAL_INT_PLUS_NANO;
case IIO_CHAN_INFO_CALIBBIAS:
ret = regmap_read(data->regmap,
ADXL313_REG_OFS_AXIS(chan->address), &regval);
if (ret)
return ret;

/*
* 8-bit resolution at minimum range, that is 4x accel data scale
* factor at full resolution
*/
*val = sign_extend32(regval, 7) * 4;
return IIO_VAL_INT;
case IIO_CHAN_INFO_SAMP_FREQ:
ret = regmap_read(data->regmap, ADXL313_REG_BW_RATE, &regval);
if (ret)
return ret;

ret = FIELD_GET(ADXL313_RATE_MSK, regval) - ADXL313_RATE_BASE;
*val = adxl313_odr_freqs[ret][0];
*val2 = adxl313_odr_freqs[ret][1];
return IIO_VAL_INT_PLUS_MICRO;
default:
return -EINVAL;
}
}

static int adxl313_write_raw(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int val, int val2, long mask)
{
struct adxl313_data *data = iio_priv(indio_dev);

switch (mask) {
case IIO_CHAN_INFO_CALIBBIAS:
/*
* 8-bit resolution at minimum range, that is 4x accel data scale
* factor at full resolution
*/
if (clamp_val(val, -128 * 4, 127 * 4) != val)
return -EINVAL;

return regmap_write(data->regmap,
ADXL313_REG_OFS_AXIS(chan->address),
val / 4);
case IIO_CHAN_INFO_SAMP_FREQ:
return adxl313_set_odr(data, val, val2);
default:
return -EINVAL;
}
}

static const struct iio_info adxl313_info = {
.read_raw	= adxl313_read_raw,
.write_raw	= adxl313_write_raw,
.read_avail	= adxl313_read_freq_avail,
};

static int adxl313_setup(struct device *dev, struct adxl313_data *data,
int (*setup)(struct device *, struct regmap *))
{
int ret;

/*
* If sw reset available, ensures the device is in a consistent
* state after start up
*/
if (data->chip_info->soft_reset) {
ret = regmap_write(data->regmap, ADXL313_REG_SOFT_RESET,
ADXL313_SOFT_RESET);
if (ret)
return ret;
}

if (setup) {
ret = setup(dev, data->regmap);
if (ret)
return ret;
}

ret = data->chip_info->check_id(dev, data);
if (ret)
return ret;

/* Sets the range to maximum, full resolution, if applicable */
if (data->chip_info->variable_range) {
ret = regmap_update_bits(data->regmap, ADXL313_REG_DATA_FORMAT,
ADXL313_RANGE_MSK,
FIELD_PREP(ADXL313_RANGE_MSK, ADXL313_RANGE_MAX));
if (ret)
return ret;

/* Enables full resolution */
ret = regmap_update_bits(data->regmap, ADXL313_REG_DATA_FORMAT,
ADXL313_FULL_RES, ADXL313_FULL_RES);
if (ret)
return ret;
}

/* Enables measurement mode */
return regmap_update_bits(data->regmap, ADXL313_REG_POWER_CTL,
ADXL313_POWER_CTL_MSK,
ADXL313_MEASUREMENT_MODE);
}

/**
* adxl313_core_probe() - probe and setup for adxl313 accelerometer
* @dev:	Driver model representation of the device
* @regmap:	Register map of the device
* @chip_info:	Structure containing device specific data
* @setup:	Setup routine to be executed right before the standard device
*		setup, can also be set to NULL if not required
*
* Return: 0 on success, negative errno on error cases
*/
int adxl313_core_probe(struct device *dev,
struct regmap *regmap,
const struct adxl313_chip_info *chip_info,
int (*setup)(struct device *, struct regmap *))
{
struct adxl313_data *data;
struct iio_dev *indio_dev;
int ret;

indio_dev = devm_iio_device_alloc(dev, sizeof(*data));
if (!indio_dev)
return -ENOMEM;

data = iio_priv(indio_dev);
data->regmap = regmap;
data->chip_info = chip_info;

mutex_init(&data->lock);

indio_dev->name = chip_info->name;
indio_dev->info = &adxl313_info;
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->channels = adxl313_channels;
indio_dev->num_channels = ARRAY_SIZE(adxl313_channels);

ret = adxl313_setup(dev, data, setup);
if (ret) {
dev_err(dev, "ADXL313 setup failed
");
return ret;
}

return devm_iio_device_register(dev, indio_dev);
}
EXPORT_SYMBOL_NS_GPL(adxl313_core_probe, IIO_ADXL313);

MODULE_AUTHOR("Lucas Stankus <lucas.p.stankus@gmail.com>");
MODULE_DESCRIPTION("ADXL313 3-Axis Digital Accelerometer core driver");
MODULE_LICENSE("GPL v2");