#include <linux/l3g4200d.h>
-#define DEBUG 1
-
/** Register map */
#define L3G4200D_WHO_AM_I 0x0f
#define L3G4200D_CTRL_REG1 0x20
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REF_DATA_CAP 0x25
+#define L3G4200D_OUT_TEMP 0x26
#define L3G4200D_STATUS_REG 0x27
#define L3G4200D_OUT_X_L 0x28
#define L3G4200D_OUT_Z_L 0x2c
#define L3G4200D_OUT_Z_H 0x2d
+#define L3G4200D_FIFO_CTRL 0x2e
+#define L3G4200D_FIFO_SRC 0x2e
+
#define L3G4200D_INTERRUPT_CFG 0x30
#define L3G4200D_INTERRUPT_SRC 0x31
#define L3G4200D_INTERRUPT_THRESH_X_H 0x32
#define I2C_RETRY_DELAY 5
#define I2C_RETRIES 5
#define AUTO_INCREMENT 0x80
-
-#define ODRHALF 0x40 /* 0.5Hz output data rate */
-#define ODR1 0x60 /* 1Hz output data rate */
-#define ODR2 0x80 /* 2Hz output data rate */
-#define ODR5 0xA0 /* 5Hz output data rate */
-#define ODR10 0xC0 /* 10Hz output data rate */
-#define ODR50 0x00 /* 50Hz output data rate */
-#define ODR100 0x08 /* 100Hz output data rate */
-#define ODR400 0x10 /* 400Hz output data rate */
-#define ODR1000 0x18 /* 1000Hz output data rate */
#define L3G4200D_PU_DELAY 300
+
+
struct l3g4200d_data {
struct i2c_client *client;
struct l3g4200d_platform_data *pdata;
u8 shift_adj;
u8 resume_state[5];
- u8 multiplier;
};
-#ifdef DEBUG
-struct l3g4200d_reg {
- const char *name;
- uint8_t reg;
-} l3g4200d_regs[] = {
- { "WHO_AM_I", L3G4200D_WHO_AM_I },
- { "CNTRL_1", L3G4200D_CTRL_REG1 },
- { "CNTRL_2", L3G4200D_CTRL_REG2 },
- { "CNTRL_3", L3G4200D_CTRL_REG3 },
- { "CNTRL_4", L3G4200D_CTRL_REG4 },
- { "CNTRL_5", L3G4200D_CTRL_REG5 },
- { "REF_DATA_CAP", L3G4200D_REF_DATA_CAP },
- { "STATUS_REG", L3G4200D_STATUS_REG },
- { "INT_CFG", L3G4200D_INTERRUPT_CFG },
- { "INT_SRC", L3G4200D_INTERRUPT_SRC },
- { "INT_TH_X_H", L3G4200D_INTERRUPT_THRESH_X_H },
- { "INT_TH_X_L", L3G4200D_INTERRUPT_THRESH_X_L },
- { "INT_TH_Y_H", L3G4200D_INTERRUPT_THRESH_Y_H },
- { "INT_TH_Y_L", L3G4200D_INTERRUPT_THRESH_Y_L },
- { "INT_TH_Z_H", L3G4200D_INTERRUPT_THRESH_Z_H },
- { "INT_TH_Z_L", L3G4200D_INTERRUPT_THRESH_Z_L },
- { "INT_DUR", L3G4200D_INTERRUPT_DURATION },
- { "OUT_X_H", L3G4200D_OUT_X_H },
- { "OUT_X_L", L3G4200D_OUT_X_L },
- { "OUT_Y_H", L3G4200D_OUT_Y_H },
- { "OUT_Y_L", L3G4200D_OUT_Y_L },
- { "OUT_Z_H", L3G4200D_OUT_Z_H },
- { "OUT_Z_L", L3G4200D_OUT_Z_L },
+
+struct gyro_val {
+ s16 x;
+ s16 y;
+ s16 z;
};
-#endif
+
static uint32_t l3g4200d_debug;
module_param_named(gyro_debug, l3g4200d_debug, uint, 0664);
static int l3g4200d_hw_init(struct l3g4200d_data *gyro)
{
int err = -1;
- u8 buf[6];
+ u8 buf[8];
buf[0] = (AUTO_INCREMENT | L3G4200D_CTRL_REG1);
- buf[1] = gyro->resume_state[0];
- buf[2] = gyro->resume_state[1];
- buf[3] = gyro->resume_state[2];
- buf[4] = gyro->resume_state[3];
- buf[5] = gyro->resume_state[4];
- err = l3g4200d_i2c_write(gyro, buf, 5);
+ buf[1] = gyro->pdata->ctrl_reg1;
+ buf[2] = gyro->pdata->ctrl_reg2;
+ buf[3] = gyro->pdata->ctrl_reg3;
+ buf[4] = gyro->pdata->ctrl_reg4;
+ buf[5] = gyro->pdata->ctrl_reg5;
+ buf[6] = gyro->pdata->reference;
+ err = l3g4200d_i2c_write(gyro, buf, 6);
if (err < 0)
return err;
- gyro->hw_initialized = 1;
+ buf[0] = (L3G4200D_FIFO_CTRL);
+ buf[1] = gyro->pdata->fifo_ctrl_reg;
+ err = l3g4200d_i2c_write(gyro, buf, 1);
+ if (err < 0)
+ return err;
+
+ buf[0] = (L3G4200D_INTERRUPT_CFG);
+ buf[1] = gyro->pdata->int1_cfg;
+ err = l3g4200d_i2c_write(gyro, buf, 1);
+ if (err < 0)
+ return err;
+
+ buf[0] = (AUTO_INCREMENT | L3G4200D_INTERRUPT_THRESH_X_H);
+ buf[1] = gyro->pdata->int1_tsh_xh;
+ buf[2] = gyro->pdata->int1_tsh_xl;
+ buf[3] = gyro->pdata->int1_tsh_yh;
+ buf[4] = gyro->pdata->int1_tsh_yl;
+ buf[5] = gyro->pdata->int1_tsh_zh;
+ buf[6] = gyro->pdata->int1_tsh_zl;
+ buf[7] = gyro->pdata->int1_duration;
+ err = l3g4200d_i2c_write(gyro, buf, 7);
+ if (err < 0)
+ return err;
+
+ gyro->hw_initialized = true;
return 0;
}
static void l3g4200d_device_power_off(struct l3g4200d_data *gyro)
{
int err;
- u8 buf[2] = {L3G4200D_CTRL_REG1,0 };
+ u8 buf[2] = {L3G4200D_CTRL_REG1, 0};
err = l3g4200d_i2c_read(gyro, buf, 1);
- if (err < 0){
+ if (err < 0) {
dev_err(&gyro->client->dev, "read register control_1 failed\n");
- return ;
- }
- buf[1] = buf[0] & ~PM_MASK;
- buf[0] = L3G4200D_CTRL_REG1;
+ return;
+ }
+ buf[1] = buf[0] & ~PM_MASK;
+ buf[0] = L3G4200D_CTRL_REG1;
err = l3g4200d_i2c_write(gyro, buf, 1);
if (err < 0)
if (gyro->regulator) {
regulator_disable(gyro->regulator);
- gyro->hw_initialized = 0;
+ gyro->hw_initialized = false;
}
}
err = l3g4200d_i2c_read(gyro, buf, 1);
if (err < 0)
dev_err(&gyro->client->dev, "read register control_1 failed\n");
- buf[1] = buf[0] | PM_MASK;
- buf[0] = L3G4200D_CTRL_REG1;
+ buf[1] = buf[0] | PM_MASK;
+ buf[0] = L3G4200D_CTRL_REG1;
err = l3g4200d_i2c_write(gyro, buf, 1);
if (err < 0)
return 0;
}
-static int l3g4200d_get_gyro_data(struct l3g4200d_data *gyro, int *xyz)
+static int l3g4200d_get_gyro_data(struct l3g4200d_data *gyro,
+ struct gyro_val *data)
{
int err = -1;
/* Data bytes from hardware xL, xH, yL, yH, zL, zH */
u8 gyro_data[6];
- /* x,y,z hardware data */
- int hw_d[3] = { 0 };
gyro_data[0] = (AUTO_INCREMENT | L3G4200D_OUT_X_L);
err = l3g4200d_i2c_read(gyro, gyro_data, 6);
if (err < 0)
return err;
- hw_d[0] = (int) (((gyro_data[1]) << 8) | gyro_data[0]);
- hw_d[1] = (int) (((gyro_data[3]) << 8) | gyro_data[2]);
- hw_d[2] = (int) (((gyro_data[5]) << 8) | gyro_data[4]);
+ data->x = (gyro_data[1] << 8) | gyro_data[0];
+ data->y = (gyro_data[3] << 8) | gyro_data[2];
+ data->z = (gyro_data[5] << 8) | gyro_data[4];
- hw_d[0] = (hw_d[0] & 0x8000) ? (hw_d[0] | 0xFFFF0000) : (hw_d[0]);
- hw_d[1] = (hw_d[1] & 0x8000) ? (hw_d[1] | 0xFFFF0000) : (hw_d[1]);
- hw_d[2] = (hw_d[2] & 0x8000) ? (hw_d[2] | 0xFFFF0000) : (hw_d[2]);
-
- xyz[0] = ((gyro->pdata->negate_x) ? (-hw_d[gyro->pdata->axis_map_x])
- : (hw_d[gyro->pdata->axis_map_x])) * gyro->multiplier;
- xyz[1] = ((gyro->pdata->negate_y) ? (-hw_d[gyro->pdata->axis_map_y])
- : (hw_d[gyro->pdata->axis_map_y])) * gyro->multiplier;
- xyz[2] = ((gyro->pdata->negate_z) ? (-hw_d[gyro->pdata->axis_map_z])
- : (hw_d[gyro->pdata->axis_map_z])) * gyro->multiplier;
-
- return err;
+ return 0;
}
-static void l3g4200d_report_values(struct l3g4200d_data *gyro, int *xyz)
+static void l3g4200d_report_values(struct l3g4200d_data *gyro,
+ struct gyro_val *data)
{
- input_report_rel(gyro->input_dev, REL_RX, xyz[0]);
- input_report_rel(gyro->input_dev, REL_RY, xyz[1]);
- input_report_rel(gyro->input_dev, REL_RZ, xyz[2]);
+ input_report_rel(gyro->input_dev, REL_RX, data->x);
+ input_report_rel(gyro->input_dev, REL_RY, data->y);
+ input_report_rel(gyro->input_dev, REL_RZ, data->z);
if (l3g4200d_debug)
pr_info("%s: Reporting x: %d, y: %d, z: %d\n",
- __func__, xyz[0], xyz[1], xyz[2]);
+ __func__, data->x, data->y, data->z);
input_sync(gyro->input_dev);
}
break;
case L3G4200D_IOCTL_SET_DELAY:
- if (copy_from_user(&interval, argp, sizeof(interval))) {
- gyro->pdata->poll_interval = 0;
+ if (copy_from_user(&interval, argp, sizeof(interval)))
return -EFAULT;
- }
gyro->pdata->poll_interval =
max(interval, gyro->pdata->min_interval);
break;
struct l3g4200d_data *gyro = container_of((struct delayed_work *)work,
struct l3g4200d_data,
input_work);
- int xyz[3] = { 0 };
+ struct gyro_val data;
int err;
- err = l3g4200d_get_gyro_data(gyro, xyz);
+ err = l3g4200d_get_gyro_data(gyro, &data);
if (err < 0)
dev_err(&gyro->client->dev, "get_acceleration_data failed\n");
else
- l3g4200d_report_values(gyro, xyz);
+ l3g4200d_report_values(gyro, &data);
schedule_delayed_work(&gyro->input_work,
msecs_to_jiffies(gyro->pdata->poll_interval));
}
-#ifdef DEBUG
-static ssize_t l3g4200d_registers_show(struct device *dev,
- struct device_attribute *attr, char *buf)
-{
- struct i2c_client *client = container_of(dev, struct i2c_client,
- dev);
- struct l3g4200d_data *gyro = i2c_get_clientdata(client);
- u8 l3g4200d_buf[2];
- unsigned i, n, reg_count;
-
- reg_count = sizeof(l3g4200d_regs) / sizeof(l3g4200d_regs[0]);
- for (i = 0, n = 0; i < reg_count; i++) {
- l3g4200d_buf[0] = (AUTO_INCREMENT | l3g4200d_regs[i].reg);
- l3g4200d_i2c_read(gyro, l3g4200d_buf, 1);
- n += scnprintf(buf + n, PAGE_SIZE - n,
- "%-20s = 0x%02X\n",
- l3g4200d_regs[i].name, l3g4200d_buf[0]);
- }
- return n;
-}
-
-static ssize_t l3g4200d_registers_store(struct device *dev,
- struct device_attribute *attr,
- const char *buf, size_t count)
-{
- struct i2c_client *client = container_of(dev, struct i2c_client,
- dev);
- struct l3g4200d_data *gyro = i2c_get_clientdata(client);
- unsigned i, reg_count, value;
- int error;
- u8 l3g4200d_buf[2];
- char name[30];
-
- if (count >= 30) {
- pr_err("%s:input too long\n", __func__);
- return -1;
- }
- if (sscanf(buf, "%s %x", name, &value) != 2) {
- pr_err("%s:unable to parse input\n", __func__);
- return -1;
- }
-
- reg_count = sizeof(l3g4200d_regs) / sizeof(l3g4200d_regs[0]);
- for (i = 0; i < reg_count; i++) {
- if (!strcmp(name, l3g4200d_regs[i].name)) {
- l3g4200d_buf[0] = (AUTO_INCREMENT | l3g4200d_regs[i].reg);
- l3g4200d_buf[1] = value;
- error = l3g4200d_i2c_write(gyro, l3g4200d_buf, 2);
- if (error) {
- pr_err("%s:Failed to write register %s\n",
- __func__, name);
- return -1;
- }
- return count;
- }
- }
- if (!strcmp("Go", name)) {
- l3g4200d_enable(gyro);
- return 0;
- }
- if (!strcmp("Stop", name)) {
- l3g4200d_disable(gyro);
- return 0;
- }
- pr_err("%s:no such register %s\n", __func__, name);
- return -1;
-}
-static DEVICE_ATTR(registers, 0644, l3g4200d_registers_show,
- l3g4200d_registers_store);
-#endif
#ifdef L3G4200D_OPEN_ENABLE
int l3g4200d_input_open(struct input_dev *input)
{
gyro->pdata->poll_interval = max(gyro->pdata->poll_interval,
gyro->pdata->min_interval);
- if (gyro->pdata->axis_map_x > 2 ||
- gyro->pdata->axis_map_y > 2 || gyro->pdata->axis_map_z > 2) {
- dev_err(&gyro->client->dev,
- "invalid axis_map value x:%u y:%u z%u\n",
- gyro->pdata->axis_map_x, gyro->pdata->axis_map_y,
- gyro->pdata->axis_map_z);
- return -EINVAL;
- }
-
- /* Only allow 0 and 1 for negation boolean flag */
- if (gyro->pdata->negate_x > 1 || gyro->pdata->negate_y > 1 ||
- gyro->pdata->negate_z > 1) {
- dev_err(&gyro->client->dev,
- "invalid negate value x:%u y:%u z:%u\n",
- gyro->pdata->negate_x, gyro->pdata->negate_y,
- gyro->pdata->negate_z);
- return -EINVAL;
- }
-
/* Enforce minimum polling interval */
if (gyro->pdata->poll_interval < gyro->pdata->min_interval) {
dev_err(&gyro->client->dev, "minimum poll interval violated\n");
{
struct l3g4200d_data *gyro;
int err = -1;
- u8 full_scale;
pr_err("%s:Enter\n", __func__);
if (client->dev.platform_data == NULL) {
i2c_set_clientdata(client, gyro);
- memset(gyro->resume_state, 0, ARRAY_SIZE(gyro->resume_state));
-
- gyro->resume_state[0] = gyro->pdata->ctrl_reg_1;
- gyro->resume_state[1] = gyro->pdata->ctrl_reg_2;
- gyro->resume_state[2] = gyro->pdata->ctrl_reg_3;
- gyro->resume_state[3] = gyro->pdata->ctrl_reg_4;
- gyro->resume_state[4] = gyro->pdata->ctrl_reg_5;
-
/* As default, do not report information */
atomic_set(&gyro->enabled, 0);
dev_err(&client->dev, "l3g4200d_device register failed\n");
goto err4;
}
-#ifdef DEBUG
- err = device_create_file(&client->dev, &dev_attr_registers);
- if (err < 0)
- pr_err("%s:File device creation failed: %d\n", __func__, err);
-#endif
- full_scale = gyro->pdata->ctrl_reg_4 & 0x30;
- if ( full_scale == 0x00)
- gyro->multiplier = 1;
- if ( full_scale == 0x10)
- gyro->multiplier = 2;
- if ((full_scale == 0x20) || (full_scale == 0x30))
- gyro->multiplier = 8;
- pr_info("%s: multiplier %d\n", __func__, gyro->multiplier);
-
- pr_err("%s:Gyro probed\n", __func__);
+
+ pr_info("%s:Gyro probed\n", __func__);
return 0;
err4:
{
struct l3g4200d_data *gyro = i2c_get_clientdata(client);
-#ifdef DEBUG
- device_remove_file(&client->dev, &dev_attr_registers);
-#endif
misc_deregister(&l3g4200d_misc_device);
l3g4200d_input_cleanup(gyro);
l3g4200d_disable(gyro);
projects / firefly-linux-kernel-4.4.55.git / commitdiff