NED
/
tegrakernel
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
tegrakernel/kernel/nvidia/drivers/iio/gyro/nvs_a3g4250d.c

866 lines
20 KiB
C
Raw Blame History

/* Copyright (c) 2015-2017, NVIDIA CORPORATION. All rights reserved.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
/* The NVS = NVidia Sensor framework */
/* See nvs_iio.c and nvs.h for documentation */
#include <linux/i2c.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/regulator/consumer.h>
#include <linux/workqueue.h>
#include <linux/interrupt.h>
#include <linux/of.h>
#include <linux/nvs.h>
#define STM_NAME "a3g4250d"
#define STM_VENDOR "STMicroelectronics"
#define STM_VERSION (1)
#define STM_KBUF_SIZE (128)
#define STM_DELAY_US_MAX (255000)
#define STM_HW_DELAY_POR_MS (50)
#define STM_HW_DELAY_US (100)
#define STM_POLL_DELAY_MS_DFLT (200)
#define STM_ERR_CNT_MAX (20)
/* HW registers */
#define STM_REG_WHO_AM_I (0x0F)
#define STM_REG_WHO_AM_I_ID (0xD3)
#define STM_REG_CTRL1 (0x20)
#define STM_REG_CTRL1_XEN (0)
#define STM_REG_CTRL1_YEN (1)
#define STM_REG_CTRL1_ZEN (2)
#define STM_REG_CTRL1_PD (3)
#define STM_REG_CTRL1_BW (4)
#define STM_REG_CTRL1_BW_MASK (0x30)
#define STM_REG_CTRL1_DR (6)
#define STM_REG_CTRL2 (0x21)
#define STM_REG_CTRL2_HPCF (0)
#define STM_REG_CTRL2_HPM (4)
#define STM_REG_CTRL3 (0x22)
#define STM_REG_CTRL3_I2_EMPTY (0)
#define STM_REG_CTRL3_I2_ORUN (1)
#define STM_REG_CTRL3_I2_WTM (2)
#define STM_REG_CTRL3_I2_DRDY (3)
#define STM_REG_CTRL3_PP_OD (4)
#define STM_REG_CTRL3_H_LACTIVE (5)
#define STM_REG_CTRL3_I1_BOOT (6)
#define STM_REG_CTRL3_I1_INT1 (7)
#define STM_REG_CTRL4 (0x23)
#define STM_REG_CTRL4_SIM (0)
#define STM_REG_CTRL4_ST (1)
#define STM_REG_CTRL4_BLE (6)
#define STM_REG_CTRL5 (0x24)
#define STM_REG_CTRL5_OUT_SEL (0)
#define STM_REG_CTRL5_INT1_SEL (2)
#define STM_REG_CTRL5_HPEN (4)
#define STM_REG_CTRL5_FIFO_EN (6)
#define STM_REG_CTRL5_BOOT (7)
#define STM_REG_REFERENCE (0x25)
#define STM_REG_OUT_TEMP (0x26)
#define STM_REG_STATUS (0x27)
#define STM_REG_STATUS_XDA (0)
#define STM_REG_STATUS_YDA (1)
#define STM_REG_STATUS_ZDA (2)
#define STM_REG_STATUS_ZYXDA (3)
#define STM_REG_STATUS_DA_MASK (0x0F)
#define STM_REG_STATUS_XOR (4)
#define STM_REG_STATUS_YOR (5)
#define STM_REG_STATUS_ZOR (6)
#define STM_REG_STATUS_ZYXOR (7)
#define STM_REG_OUT_X_L (0x28)
#define STM_REG_OUT_X_H (0x29)
#define STM_REG_OUT_Y_L (0x2A)
#define STM_REG_OUT_Y_H (0x2B)
#define STM_REG_OUT_Z_L (0x2C)
#define STM_REG_OUT_Z_H (0x2D)
#define STM_REG_FIFO_CTRL (0x2E)
#define STM_REG_FIFO_CTRL_WTM (0)
#define STM_REG_FIFO_CTRL_WTM_MASK (0x1F)
#define STM_REG_FIFO_CTRL_FM (5)
#define STM_REG_FIFO_SRC (0x2F)
#define STM_REG_FIFO_SRC_FSS (0)
#define STM_REG_FIFO_SRC_EMPTY (5)
#define STM_REG_FIFO_SRC_OVRN (6)
#define STM_REG_FIFO_SRC_WTM (7)
#define STM_REG_INT1_CFG (0x30)
#define STM_REG_INT1_SRC (0x31)
#define STM_REG_INT1_TSH_XH (0x32)
#define STM_REG_INT1_TSH_XL (0x33)
#define STM_REG_INT1_TSH_YH (0x34)
#define STM_REG_INT1_TSH_YL (0x35)
#define STM_REG_INT1_TSH_ZH (0x36)
#define STM_REG_INT1_TSH_ZL (0x37)
#define STM_REG_INT1_DURATION (0x38)
#define STM_REG_INT1_DURATION_WAIT (7)
#define STM_REG_INT_CFG_XLIE (0)
#define STM_REG_INT_CFG_XHIE (1)
#define STM_REG_INT_CFG_YLIE (2)
#define STM_REG_INT_CFG_YHIE (3)
#define STM_REG_INT_CFG_ZLIE (4)
#define STM_REG_INT_CFG_ZHIE (5)
#define STM_REG_INT_CFG_LIR (6)
#define STM_REG_INT_CFG_AOI (7)
#define STM_REG_INT_SRC_XL (0)
#define STM_REG_INT_SRC_XH (1)
#define STM_REG_INT_SRC_YL (2)
#define STM_REG_INT_SRC_YH (3)
#define STM_REG_INT_SRC_ZL (4)
#define STM_REG_INT_SRC_ZH (5)
#define STM_REG_INT_SRC_IA (6)
#define STM_I2C_AUTO_INC_AD (0x80)
#define AXIS_X (0)
#define AXIS_Y (1)
#define AXIS_Z (2)
#define AXIS_N (3)
#define STM_ODR_OVERRIDE_CELLS (3)
/* regulator names in order of powering on */
static char *stm_vregs[] = {
"vdd",
"vdd_IO",
};
static struct sensor_cfg stm_cfg_dflt = {
.name = "gyroscope",
.kbuf_sz = STM_KBUF_SIZE,
.ch_n = AXIS_N,
.ch_sz = -2,
.part = STM_NAME,
.vendor = STM_VENDOR,
.version = STM_VERSION,
/* milliamp is dynamic based on delay */
.milliamp = {
.ival = 0,
.fval = 400000000,
},
/* the spec has 245 deg/s => 4.276056667 rad/s */
.max_range = {
.ival = 4,
.fval = 276056667,
},
.resolution = {
.ival = 0,
.fval = 130499,
},
.delay_us_min = 1250,
.delay_us_max = 10000,
/* default matrix to get the attribute */
.matrix[0] = 1,
.matrix[4] = 1,
.matrix[8] = 1,
.float_significance = NVS_FLOAT_NANO,
.scale = {
.ival = 0,
.fval = 130499,
},
};
static unsigned short stm_i2c_addrs[] = {
0x68,
0x69,
};
struct stm_state {
struct i2c_client *i2c;
struct nvs_fn_if *nvs;
void *nvs_st;
struct sensor_cfg cfg;
struct regulator_bulk_data vreg[ARRAY_SIZE(stm_vregs)];
struct work_struct dw;
u32* odr_override_tbl; /* Sampling freq override table */
unsigned int sts; /* status flags */
unsigned int errs; /* error count */
unsigned int enabled; /* enable status */
unsigned int delay_us; /* requested sampling delay (us) */
u16 i2c_addr; /* I2C address */
bool irq_dis; /* interrupt host disable flag */
u8 ru_ctrl1; /* register user CTRL_REG1 */
u8 ru_ctrl2; /* register user CTRL_REG2 */
u8 ru_ctrl3; /* register user CTRL_REG3 */
u8 ru_ctrl4; /* register user CTRL_REG3 */
u8 ru_ctrl5; /* register user CTRL_REG3 */
u8 rc_ctrl1; /* register cache CTRL_REG1 */
u8 rc_fifo_ctrl; /* register cache FIFO_CTRL */
u8 buf[9]; /* status + data + FIFO */
s64 ts_irq; /* interrupt timestamp */
s64 ts_last; /* interrupt last pushed with data */
};
struct stm_odr {
unsigned int us;
u8 hw;
};
static struct stm_odr stm_odr_tbl[] = {
{ 10000, 0x08, },
{ 5000, 0x48, },
{ 2500, 0x88, },
{ 1250, 0xC8, },
};
static void stm_err(struct stm_state *st)
{
st->errs++;
if (!st->errs)
st->errs--;
}
static int stm_i2c_rd(struct stm_state *st, u8 reg, u16 len, u8 *val)
{
struct i2c_msg msg[2];
if (len > 1)
reg |= STM_I2C_AUTO_INC_AD;
msg[0].addr = st->i2c_addr;
msg[0].flags = 0;
msg[0].len = 1;
msg[0].buf = &reg;
msg[1].addr = st->i2c_addr;
msg[1].flags = I2C_M_RD;
msg[1].len = len;
msg[1].buf = val;
if (i2c_transfer(st->i2c->adapter, msg, 2) != 2) {
stm_err(st);
return -EIO;
}
return 0;
}
static int stm_i2c_wr(struct stm_state *st, u8 reg, u8 val)
{
struct i2c_msg msg;
u8 buf[2];
if (st->i2c_addr) {
buf[0] = reg;
buf[1] = val;
msg.addr = st->i2c_addr;
msg.flags = 0;
msg.len = 2;
msg.buf = buf;
if (i2c_transfer(st->i2c->adapter, &msg, 1) != 1) {
stm_err(st);
return -EIO;
}
}
return 0;
}
static int stm_fifo_ctrl_wr(struct stm_state *st, u8 fifo_ctrl)
{
int ret = 0;
if (st->rc_fifo_ctrl != fifo_ctrl) {
ret = stm_i2c_wr(st, STM_REG_FIFO_CTRL, fifo_ctrl);
if (ret) {
dev_err(&st->i2c->dev, "%s:%x->%x ERR=%d\n",
__func__, st->rc_fifo_ctrl, fifo_ctrl, ret);
} else {
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&st->i2c->dev, "%s:%x->%x\n",
__func__, st->rc_fifo_ctrl, fifo_ctrl);
st->rc_fifo_ctrl = fifo_ctrl;
}
}
return ret;
}
static int stm_cmd(struct stm_state *st, int enable)
{
u8 ctrl1 = enable;
int i;
int ret;
int ret_t = 0;
if (enable) {
for (i = 0; i < ARRAY_SIZE(stm_odr_tbl) - 1; i++) {
if (st->delay_us >= stm_odr_tbl[i].us)
break;
}
ctrl1 |= stm_odr_tbl[i].hw;
ctrl1 |= st->ru_ctrl1;
ret_t |= stm_i2c_wr(st, STM_REG_CTRL2, st->ru_ctrl2);
ret_t |= stm_i2c_wr(st, STM_REG_CTRL3, st->ru_ctrl3);
ret_t |= stm_i2c_wr(st, STM_REG_CTRL4, st->ru_ctrl4);
ret_t |= stm_i2c_wr(st, STM_REG_CTRL5, st->ru_ctrl5);
st->ts_last = nvs_timestamp();
}
ret = stm_i2c_wr(st, STM_REG_CTRL1, ctrl1);
if (ret) {
ret_t |= ret;
dev_err(&st->i2c->dev, "%s:%x->%x ERR=%d\n",
__func__, st->rc_ctrl1, ctrl1, ret);
} else {
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&st->i2c->dev, "%s:%x->%x\n",
__func__, st->rc_ctrl1, ctrl1);
st->rc_ctrl1 = ctrl1;
}
return ret_t;
}
static int stm_pm(struct stm_state *st, bool enable)
{
int ret = 0;
if (enable) {
ret = nvs_vregs_enable(&st->i2c->dev, st->vreg,
ARRAY_SIZE(stm_vregs));
if (ret > 0)
mdelay(STM_HW_DELAY_POR_MS);
} else {
if (st->i2c->irq) {
ret = nvs_vregs_sts(st->vreg, ARRAY_SIZE(stm_vregs));
if ((ret < 0) || (ret == ARRAY_SIZE(stm_vregs))) {
ret = stm_i2c_wr(st, STM_REG_CTRL1, 0);
} else if (ret > 0) {
ret = nvs_vregs_enable(&st->i2c->dev, st->vreg,
ARRAY_SIZE(stm_vregs));
mdelay(STM_HW_DELAY_POR_MS);
ret = stm_i2c_wr(st, STM_REG_CTRL1, 0);
}
}
ret |= nvs_vregs_disable(&st->i2c->dev, st->vreg,
ARRAY_SIZE(stm_vregs));
}
if (ret > 0)
ret = 0;
if (ret) {
dev_err(&st->i2c->dev, "%s pwr=%x ERR=%d\n",
__func__, enable, ret);
} else {
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&st->i2c->dev, "%s pwr=%x\n",
__func__, enable);
}
return ret;
}
static void stm_pm_exit(struct stm_state *st)
{
stm_pm(st, false);
nvs_vregs_exit(&st->i2c->dev, st->vreg, ARRAY_SIZE(stm_vregs));
}
static int stm_pm_init(struct stm_state *st)
{
int ret;
st->delay_us = STM_DELAY_US_MAX;
nvs_vregs_init(&st->i2c->dev,
st->vreg, ARRAY_SIZE(stm_vregs), stm_vregs);
ret = stm_pm(st, true);
return ret;
}
static int stm_rd(struct stm_state *st, s64 ts)
{
int ret;
ret = stm_i2c_rd(st, STM_REG_STATUS, sizeof(st->buf), st->buf);
if (ret)
return ret;
if (st->buf[0] & STM_REG_STATUS_DA_MASK) {
#if 0 /* TODO: add FIFO and FIFO timestamp support */
ts = stm_odr_tbl[st->rc_ctrl1 >> STM_REG_CTRL1_DR].us * 1000;
st->ts_last += ts;
st->nvs->handler(st->nvs_st, &st->buf[1], st->ts_last);
#endif
st->nvs->handler(st->nvs_st, &st->buf[1], ts);
} else {
ret = -EAGAIN;
}
return ret;
}
static void stm_read(struct stm_state *st, s64 ts)
{
st->nvs->nvs_mutex_lock(st->nvs_st);
if (st->enabled)
stm_rd(st, ts);
st->nvs->nvs_mutex_unlock(st->nvs_st);
}
static void stm_work(struct work_struct *ws)
{
struct stm_state *st = container_of((struct work_struct *)ws,
struct stm_state, dw);
s64 ts1=0, ts2=0;
unsigned long delay_value = 0;
u64 ts_diff = 0;
usleep_range(st->delay_us, st->delay_us);
while (st->enabled) {
ts1 = nvs_timestamp();
stm_read(st, ts1);
ts2 = nvs_timestamp();
ts_diff = (ts2 - ts1)/1000;
if (st->delay_us > (unsigned int)ts_diff) {
delay_value = st->delay_us - (unsigned int)ts_diff;
usleep_range(delay_value, delay_value);
}
}
}
static irqreturn_t stm_irq_thread(int irq, void *dev_id)
{
struct stm_state *st = (struct stm_state *)dev_id;
s64 ts = nvs_timestamp();
stm_read(st, ts);
return IRQ_HANDLED;
}
static irqreturn_t stm_irq_handler(int irq, void *dev_id)
{
struct stm_state *st = (struct stm_state *)dev_id;
st->ts_irq = nvs_timestamp();
if (st->sts & NVS_STS_SPEW_IRQ)
dev_info(&st->i2c->dev, "%s %lld\n", __func__, st->ts_irq);
return IRQ_WAKE_THREAD;
}
static void stm_disable_irq(struct stm_state *st)
{
if (!st->irq_dis) {
disable_irq_nosync(st->i2c->irq);
st->irq_dis = true;
}
}
static void stm_enable_irq(struct stm_state *st)
{
if (st->irq_dis) {
enable_irq(st->i2c->irq);
st->irq_dis = false;
}
}
static int stm_dis(struct stm_state *st)
{
if (st->i2c->irq)
stm_disable_irq(st);
st->enabled = 0;
return 0;
}
static int stm_disable(struct stm_state *st)
{
int ret;
ret = stm_dis(st);
if (!ret)
stm_pm(st, false);
return ret;
}
static int stm_enable(void *client, int snsr_id, int enable)
{
struct stm_state *st = (struct stm_state *)client;
int ret;
if (enable < 0)
/* just return enable status */
return st->enabled;
if (enable) {
ret = stm_pm(st, true);
if (!ret) {
ret = stm_cmd(st, enable);
if (ret) {
stm_disable(st);
} else {
st->enabled = enable;
if (st->i2c->irq)
stm_enable_irq(st);
else
schedule_work(&st->dw);
}
}
} else {
ret = stm_disable(st);
}
return ret;
}
static int stm_batch(void *client, int snsr_id, int flags,
unsigned int period_us, unsigned int timeout_us)
{
struct stm_state *st = (struct stm_state *)client;
int ret = 0;
if (period_us < st->cfg.delay_us_min)
period_us = st->cfg.delay_us_min;
if (period_us != st->delay_us) {
st->delay_us = period_us;
if (st->enabled)
ret = stm_cmd(st, st->enabled);
}
return ret;
}
static int stm_flush(void *client, int snsr_id)
{
struct stm_state *st = (struct stm_state *)client;
u8 val1;
u8 val2;
val1 = st->rc_fifo_ctrl;
val2 = val1;
val1 &= STM_REG_FIFO_CTRL_WTM_MASK;
/* put FIFO in bypass mode and restore */
stm_fifo_ctrl_wr(st, val1);
stm_fifo_ctrl_wr(st, val2);
/* then push a ts=0 to signal flush complete */
st->nvs->handler(st->nvs_st, NULL, 0);
/* assume no errors */
return 0;
}
static int stm_reset(void *client, int snsr_id)
{
struct stm_state *st = (struct stm_state *)client;
unsigned int enabled = st->enabled;
int ret;
stm_dis(st);
stm_pm(st, true);
ret = stm_i2c_wr(st, STM_REG_CTRL5, 1 << STM_REG_CTRL5_BOOT);
stm_enable(st, snsr_id, enabled);
return ret;
}
static int stm_regs(void *client, int snsr_id, char *buf)
{
struct stm_state *st = (struct stm_state *)client;
ssize_t t;
u8 val[2];
u8 i;
int ret;
t = sprintf(buf, "registers:\n");
ret = stm_i2c_rd(st, STM_REG_WHO_AM_I, 1, val);
if (ret)
t += sprintf(buf + t, "0x%hhx=ERR\n", STM_REG_WHO_AM_I);
else
t += sprintf(buf + t, "0x%hhx=0x%hhx\n",
STM_REG_WHO_AM_I, val[0]);
for (i = STM_REG_CTRL1; i <= STM_REG_STATUS; i++) {
ret = stm_i2c_rd(st, i, 1, val);
if (ret)
t += sprintf(buf + t, "0x%hhx=ERR\n", i);
else
t += sprintf(buf + t, "0x%hhx=0x%hhx\n", i, val[0]);
}
for (i = STM_REG_OUT_X_L; i < STM_REG_OUT_Z_H; i += 2) {
ret = stm_i2c_rd(st, i, 2, val);
if (ret)
t += sprintf(buf + t, "0x%hhx:0x%hhx=ERR\n",
i, i + 1);
else
t += sprintf(buf + t, "0x%hhx:0x%hhx=0x%hx\n",
i, i + 1, *((u16 *)val));
}
for (i = STM_REG_FIFO_CTRL; i <= STM_REG_INT1_DURATION; i++) {
ret = stm_i2c_rd(st, i, 1, val);
if (ret)
t += sprintf(buf + t, "0x%hhx=ERR\n", i);
else
t += sprintf(buf + t, "0x%hhx=0x%hhx\n", i, val[0]);
}
return t;
}
static struct nvs_fn_dev stm_fn_dev = {
.enable = stm_enable,
.batch = stm_batch,
.flush = stm_flush,
.reset = stm_reset,
.regs = stm_regs,
};
#ifdef CONFIG_PM_SLEEP
static int stm_suspend(struct device *dev)
{
struct i2c_client *client = to_i2c_client(dev);
struct stm_state *st = i2c_get_clientdata(client);
int ret = 0;
st->sts |= NVS_STS_SUSPEND;
if (st->nvs && st->nvs_st)
ret = st->nvs->suspend(st->nvs_st);
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&client->dev, "%s\n", __func__);
return ret;
}
static int stm_resume(struct device *dev)
{
struct i2c_client *client = to_i2c_client(dev);
struct stm_state *st = i2c_get_clientdata(client);
int ret = 0;
if (st->nvs && st->nvs_st)
ret = st->nvs->resume(st->nvs_st);
st->sts &= ~NVS_STS_SUSPEND;
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&client->dev, "%s\n", __func__);
return ret;
}
#endif /* CONFIG_PM_SLEEP */
static SIMPLE_DEV_PM_OPS(stm_pm_ops, stm_suspend, stm_resume);
static void stm_shutdown(struct i2c_client *client)
{
struct stm_state *st = i2c_get_clientdata(client);
st->sts |= NVS_STS_SHUTDOWN;
if (st->nvs && st->nvs_st)
st->nvs->shutdown(st->nvs_st);
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&client->dev, "%s\n", __func__);
}
static int stm_remove(struct i2c_client *client)
{
struct stm_state *st = i2c_get_clientdata(client);
if (st != NULL) {
stm_shutdown(client);
if (st->nvs && st->nvs_st)
st->nvs->remove(st->nvs_st);
stm_pm_exit(st);
}
dev_info(&client->dev, "%s\n", __func__);
return 0;
}
static int stm_id_dev(struct stm_state *st, const char *name)
{
u8 val;
int ret = 0;
ret = stm_i2c_rd(st, STM_REG_WHO_AM_I, 1, &val);
if (!ret) {
if (val == STM_REG_WHO_AM_I_ID)
dev_info(&st->i2c->dev, "%s %s found\n",
__func__, name);
else
dev_info(&st->i2c->dev, "%s %hhx response @ I2C=%x\n",
__func__, val, st->i2c->addr);
}
return ret;
}
static int stm_id_i2c(struct stm_state *st,
const struct i2c_device_id *id)
{
int i;
int ret;
for (i = 0; i < ARRAY_SIZE(stm_i2c_addrs); i++) {
if (st->i2c->addr == stm_i2c_addrs[i])
break;
}
if (i < ARRAY_SIZE(stm_i2c_addrs)) {
st->i2c_addr = st->i2c->addr;
ret = stm_id_dev(st, id->name);
} else {
for (i = 0; i < ARRAY_SIZE(stm_i2c_addrs); i++) {
st->i2c_addr = stm_i2c_addrs[i];
ret = stm_id_dev(st, id->name);
if (!ret)
break;
}
}
if (ret)
st->i2c_addr = 0;
return ret;
}
static int stm_of_dt(struct stm_state *st, struct device_node *dn)
{
int count;
int i;
int j;
memcpy(&st->cfg, &stm_cfg_dflt, sizeof(st->cfg));
if (dn) {
/* device specific parameters */
of_property_read_u8(dn, "CTRL_REG1", &st->ru_ctrl1);
st->ru_ctrl1 &= STM_REG_CTRL1_BW_MASK;
of_property_read_u8(dn, "CTRL_REG2", &st->ru_ctrl2);
of_property_read_u8(dn, "CTRL_REG3", &st->ru_ctrl3);
of_property_read_u8(dn, "CTRL_REG4", &st->ru_ctrl4);
of_property_read_u8(dn, "CTRL_REG5", &st->ru_ctrl5);
count = of_property_count_elems_of_size(dn, "stm_odr_override",
sizeof(u32));
if (count > 0) {
if ((count % STM_ODR_OVERRIDE_CELLS) != 0) {
dev_err(&st->i2c->dev,
"%s: Invalid sampling override table length\n",
__func__);
return -EINVAL;
}
st->odr_override_tbl = devm_kzalloc(&st->i2c->dev,
sizeof(u32) * count, GFP_KERNEL);
if (IS_ERR_OR_NULL(st->odr_override_tbl))
return -ENOMEM;
if (of_property_read_u32_array(dn, "stm_odr_override",
st->odr_override_tbl, count)) {
dev_err(&st->i2c->dev,
" %s Fetching odr override table failed\n",
__func__);
return -EINVAL;
}
for (i = 0; i < count; i += STM_ODR_OVERRIDE_CELLS) {
for (j = 0; j < ARRAY_SIZE(stm_odr_tbl); j++) {
if (st->odr_override_tbl[i] == stm_odr_tbl[j].us) {
stm_odr_tbl[j].us = st->odr_override_tbl[i+1];
stm_odr_tbl[j].hw = (u8)st->odr_override_tbl[i+2];
break;
}
}
}
}
} else {
dev_info(&st->i2c->dev, "%s dev.of_node=NULL\n", __func__);
}
return 0;
}
static int stm_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct stm_state *st;
unsigned long irqflags;
int ret;
st = devm_kzalloc(&client->dev, sizeof(*st), GFP_KERNEL);
if (st == NULL) {
dev_err(&client->dev, "%s devm_kzalloc ERR\n", __func__);
return -ENOMEM;
}
i2c_set_clientdata(client, st);
st->i2c = client;
ret = stm_of_dt(st, client->dev.of_node);
if (ret) {
dev_err(&client->dev, "%s _of_dt ERR\n", __func__);
goto stm_probe_exit;
}
stm_pm_init(st);
ret = stm_id_i2c(st, id);
if (ret) {
dev_err(&client->dev, "%s _id_i2c ERR\n", __func__);
ret = -ENODEV;
goto stm_probe_exit;
}
stm_pm(st, false);
ret = nvs_of_dt(client->dev.of_node, &st->cfg, NULL);
if (ret < 0)
dev_info(&client->dev, "%s nvs_of_dt ERR\n", __func__);
stm_fn_dev.errs = &st->errs;
stm_fn_dev.sts = &st->sts;
st->nvs = nvs_iio();
if (st->nvs == NULL) {
ret = -ENODEV;
goto stm_probe_exit;
}
ret = st->nvs->probe(&st->nvs_st, st, &client->dev,
&stm_fn_dev, &st->cfg);
if (ret) {
dev_err(&client->dev, "%s nvs_probe ERR\n", __func__);
ret = -ENODEV;
goto stm_probe_exit;
}
if (client->irq) {
if (st->ru_ctrl3 & (1 << STM_REG_CTRL3_H_LACTIVE))
irqflags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT;
else
irqflags = IRQF_TRIGGER_RISING | IRQF_ONESHOT;
ret = request_threaded_irq(client->irq,
stm_irq_handler, stm_irq_thread,
irqflags, STM_NAME, st);
if (ret) {
dev_err(&client->dev, "%s req_threaded_irq ERR %d\n",
__func__, ret);
ret = -ENOMEM;
goto stm_probe_exit;
}
} else {
INIT_WORK(&st->dw, stm_work);
}
dev_info(&client->dev, "%s done\n", __func__);
return 0;
stm_probe_exit:
stm_remove(client);
return ret;
}
static const struct i2c_device_id stm_i2c_device_id[] = {
{ STM_NAME, 0 },
{}
};
MODULE_DEVICE_TABLE(i2c, stm_i2c_device_id);
static const struct of_device_id stm_of_match[] = {
{ .compatible = "stm,a3g4250d", },
{}
};
MODULE_DEVICE_TABLE(of, stm_of_match);
static struct i2c_driver stm_driver = {
.class = I2C_CLASS_HWMON,
.probe = stm_probe,
.remove = stm_remove,
.shutdown = stm_shutdown,
.driver = {
.name = STM_NAME,
.owner = THIS_MODULE,
.of_match_table = of_match_ptr(stm_of_match),
.pm = &stm_pm_ops,
},
.id_table = stm_i2c_device_id,
};
module_i2c_driver(stm_driver);
MODULE_LICENSE("GPL");
MODULE_DESCRIPTION("A3G4250D driver");
MODULE_AUTHOR("NVIDIA Corporation");
Reference in New Issue View Git Blame Copy Permalink