diff options
Diffstat (limited to 'drivers/sensor/lsm330dlc_gyro.c')
| -rw-r--r-- | drivers/sensor/lsm330dlc_gyro.c | 210 |
1 files changed, 167 insertions, 43 deletions
diff --git a/drivers/sensor/lsm330dlc_gyro.c b/drivers/sensor/lsm330dlc_gyro.c index 631730a..3b96f84 100644 --- a/drivers/sensor/lsm330dlc_gyro.c +++ b/drivers/sensor/lsm330dlc_gyro.c @@ -35,7 +35,11 @@ #define VENDOR "STM" #define CHIP_ID "LSM330" +#ifdef CONFIG_SLP +#define CALIBRATION_FILE_PATH "/csa/sensor/gyro_cal_data" +#else #define CALIBRATION_FILE_PATH "/efs/gyro_cal_data" +#endif /* lsm330dlc_gyro chip id */ #define DEVICE_ID 0xD4 @@ -137,6 +141,17 @@ struct gyro_t { s16 z; }; +static const int position_map[][3][3] = { + {{-1, 0, 0}, { 0, -1, 0}, { 0, 0, 1} }, /* 0 top/upper-left */ + {{ 0, -1, 0}, { 1, 0, 0}, { 0, 0, 1} }, /* 1 top/upper-right */ + {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1} }, /* 2 top/lower-right */ + {{ 0, 1, 0}, {-1, 0, 0}, { 0, 0, 1} }, /* 3 top/lower-left */ + {{ 1, 0, 0}, { 0, -1, 0}, { 0, 0, -1} }, /* 4 bottom/upper-left */ + {{ 0, 1, 0}, { 1, 0, 0}, { 0, 0, -1} }, /* 5 bottom/upper-right */ + {{-1, 0, 0}, { 0, 1, 0}, { 0, 0, -1} }, /* 6 bottom/lower-right */ + {{ 0, -1, 0}, {-1, 0, 0}, { 0, 0, -1} }, /* 7 bottom/lower-left*/ +}; + struct lsm330dlc_gyro_data { struct i2c_client *client; struct input_dev *input_dev; @@ -146,11 +161,11 @@ struct lsm330dlc_gyro_data { struct hrtimer timer; atomic_t opened; bool enable; - bool drop_next_event; bool self_test; /* is self_test or not? */ bool interruptible; /* interrupt or polling? */ int entries; /* number of fifo entries */ int dps; /* scale selection */ + int sensitivity_value; /* fifo data entries */ u8 fifo_data[sizeof(struct gyro_t) * 32]; u8 ctrl_regs[5]; /* saving register settings */ @@ -164,6 +179,8 @@ struct lsm330dlc_gyro_data { int count; #endif struct gyro_t cal_data; + int position; + bool axis_adjust; }; static int lsm330dlc_gyro_open_calibration(struct lsm330dlc_gyro_data *data) @@ -262,30 +279,46 @@ static int lsm330dlc_gyro_read_values(struct i2c_client *client, } static int lsm330dlc_gyro_report_values\ - (struct lsm330dlc_gyro_data *gyro_data) + (struct lsm330dlc_gyro_data *data) { - int res; - struct gyro_t data; + int res, i, j; + s16 raw[3] = {0,}, gyro_adjusted[3] = {0,}; - res = lsm330dlc_gyro_read_values(gyro_data->client, &data, - gyro_data->entries); + res = lsm330dlc_gyro_read_values(data->client, + &data->xyz_data, data->entries); if (res < 0) return res; - data.x -= gyro_data->cal_data.x; - data.y -= gyro_data->cal_data.y; - data.z -= gyro_data->cal_data.z; + data->xyz_data.x -= data->cal_data.x; + data->xyz_data.y -= data->cal_data.y; + data->xyz_data.z -= data->cal_data.z; + + if (data->axis_adjust) { + raw[0] = data->xyz_data.x; + raw[1] = data->xyz_data.y; + raw[2] = data->xyz_data.z; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) + gyro_adjusted[i] + += position_map[data->position][i][j] * raw[j]; + } + } else { + gyro_adjusted[0] = data->xyz_data.x; + gyro_adjusted[1] = data->xyz_data.y; + gyro_adjusted[2] = data->xyz_data.z; + } - input_report_rel(gyro_data->input_dev, REL_RX, data.x); - input_report_rel(gyro_data->input_dev, REL_RY, data.y); - input_report_rel(gyro_data->input_dev, REL_RZ, data.z); - input_sync(gyro_data->input_dev); + input_report_rel(data->input_dev, REL_RX, gyro_adjusted[0]); + input_report_rel(data->input_dev, REL_RY, gyro_adjusted[1]); + input_report_rel(data->input_dev, REL_RZ, gyro_adjusted[2]); + input_sync(data->input_dev); - lsm330dlc_gyro_restart_fifo(gyro_data); + lsm330dlc_gyro_restart_fifo(data); #ifdef LOGGING_GYRO - printk(KERN_INFO "%s, x = %d, y = %d, z = %d, count = %d\n" - , __func__, data.x, data.y, data.z, gyro_data->count); + pr_info("%s, x = %d, y = %d, z = %d\n" + , __func__, gyro_adjusted[0], gyro_adjusted[1], + gyro_adjusted[2]); #endif return res; @@ -302,40 +335,67 @@ static enum hrtimer_restart lsm330dlc_gyro_timer_func(struct hrtimer *timer) static void lsm330dlc_gyro_work_func(struct work_struct *work) { - int res, retry = 3; - struct lsm330dlc_gyro_data *gyro_data\ + int res, retry = 3, i, j; + struct lsm330dlc_gyro_data *data\ = container_of(work, struct lsm330dlc_gyro_data, work); s32 status = 0; + s16 raw[3] = {0,}, gyro_adjusted[3] = {0,}; do { - status = i2c_smbus_read_byte_data(gyro_data->client, + status = i2c_smbus_read_byte_data(data->client, STATUS_REG); if (status & 0x08) break; } while (retry--); - if (status & 0x08 && gyro_data->self_test == false) { - res = lsm330dlc_gyro_read_values(gyro_data->client, - &gyro_data->xyz_data, 0); + if (status & 0x08 && data->self_test == false) { + res = lsm330dlc_gyro_read_values(data->client, + &data->xyz_data, 0); if (res < 0) pr_err("%s, reading data fail(res = %d)\n", __func__, res); - } else - pr_warn("%s, use last data.\n", __func__); + if (data->dps == DPS500) { + data->xyz_data.x -= data->cal_data.x; + data->xyz_data.y -= data->cal_data.y; + data->xyz_data.z -= data->cal_data.z; + } else { + data->xyz_data.x -= (data->cal_data.x * + data->sensitivity_value / 2); + data->xyz_data.y -= (data->cal_data.y * + data->sensitivity_value / 2); + data->xyz_data.z -= (data->cal_data.z * + data->sensitivity_value / 2); + } + } else { + pr_warn("%s, use last data(%d, %d, %d), status=%d, selftest=%d\n", + __func__, data->xyz_data.x, data->xyz_data.y, + data->xyz_data.z, status, data->self_test); + } - gyro_data->xyz_data.x -= gyro_data->cal_data.x; - gyro_data->xyz_data.y -= gyro_data->cal_data.y; - gyro_data->xyz_data.z -= gyro_data->cal_data.z; + if (data->axis_adjust) { + raw[0] = data->xyz_data.x; + raw[1] = data->xyz_data.y; + raw[2] = data->xyz_data.z; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) + gyro_adjusted[i] + += position_map[data->position][i][j] * raw[j]; + } + } else { + gyro_adjusted[0] = data->xyz_data.x; + gyro_adjusted[1] = data->xyz_data.y; + gyro_adjusted[2] = data->xyz_data.z; + } - input_report_rel(gyro_data->input_dev, REL_RX, gyro_data->xyz_data.x); - input_report_rel(gyro_data->input_dev, REL_RY, gyro_data->xyz_data.y); - input_report_rel(gyro_data->input_dev, REL_RZ, gyro_data->xyz_data.z); - input_sync(gyro_data->input_dev); + input_report_rel(data->input_dev, REL_RX, gyro_adjusted[0]); + input_report_rel(data->input_dev, REL_RY, gyro_adjusted[1]); + input_report_rel(data->input_dev, REL_RZ, gyro_adjusted[2]); + input_sync(data->input_dev); #ifdef LOGGING_GYRO pr_info("%s, x = %d, y = %d, z = %d\n" - , __func__, gyro_data->xyz_data.x, gyro_data->xyz_data.y, - gyro_data->xyz_data.z); + , __func__, gyro_adjusted[0], gyro_adjusted[1], + gyro_adjusted[2]); #endif } @@ -458,14 +518,19 @@ static ssize_t lsm330dlc_gyro_selftest_dps_store(struct device *dev, ctrl = (data->ctrl_regs[3] & ~FS_MASK); - if (new_dps == DPS250) + if (new_dps == DPS250) { ctrl |= FS_250DPS; - else if (new_dps == DPS500) + data->sensitivity_value = 1; + } else if (new_dps == DPS500) { ctrl |= FS_500DPS; - else if (new_dps == DPS2000) + data->sensitivity_value = 2; + } else if (new_dps == DPS2000) { ctrl |= FS_2000DPS; - else + data->sensitivity_value = 8; + } else { ctrl |= FS_DEFULAT_DPS; + data->sensitivity_value = 2; + } /* apply new dps */ mutex_lock(&data->lock); @@ -480,7 +545,8 @@ static ssize_t lsm330dlc_gyro_selftest_dps_store(struct device *dev, mutex_unlock(&data->lock); data->dps = new_dps; - pr_err("%s: %d dps stored\n", __func__, data->dps); + pr_info("%s: %d dps stored, sensitivity = %d\n", + __func__, data->dps, data->sensitivity_value); return count; } @@ -1007,7 +1073,7 @@ static int lsm330dlc_gyro_bypass_self_test\ msleep(800); /* Read 5 samples output before self-test on */ - for (i = 0; i < 5; i++) { + for (i = 0; i < sample_num; i++) { /* check ZYXDA ready bit */ for (j = 0; j < 10; j++) { temp = i2c_smbus_read_byte_data(gyro_data->client, @@ -1269,6 +1335,32 @@ static ssize_t lsm330dlc_gyro_name_show(struct device *dev, return sprintf(buf, "%s\n", CHIP_ID); } +static ssize_t +lsm330dlc_gyro_position_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct lsm330dlc_gyro_data *data = dev_get_drvdata(dev); + + return sprintf(buf, "%d\n", data->position); +} + +static ssize_t +lsm330dlc_gyro_position_store(struct device *dev, + struct device_attribute *attr, + const char *buf, + size_t count) +{ + struct lsm330dlc_gyro_data *data = dev_get_drvdata(dev); + int err = 0; + + err = kstrtoint(buf, 10, &data->position); + if (err < 0) + pr_err("%s, kstrtoint failed.", __func__); + + return count; +} + static DEVICE_ATTR(power_off, 0664, lsm330dlc_gyro_power_off, NULL); static DEVICE_ATTR(power_on, 0664, @@ -1283,7 +1375,8 @@ static DEVICE_ATTR(vendor, 0664, lsm330dlc_gyro_vendor_show, NULL); static DEVICE_ATTR(name, 0664, lsm330dlc_gyro_name_show, NULL); - +static DEVICE_ATTR(position, 0664, + lsm330dlc_gyro_position_show, lsm330dlc_gyro_position_store); #ifdef DEBUG_REGISTER static DEVICE_ATTR(reg_data, 0664, register_data_show, NULL); @@ -1299,6 +1392,7 @@ static int lsm330dlc_gyro_probe(struct i2c_client *client, int err = 0; struct lsm330dlc_gyro_data *data; struct input_dev *input_dev; + struct gyro_platform_data *pdata; pr_info("%s, started", __func__); data = kzalloc(sizeof(*data), GFP_KERNEL); @@ -1310,10 +1404,17 @@ static int lsm330dlc_gyro_probe(struct i2c_client *client, } data->client = client; - data->drop_next_event = 0; + data->sensitivity_value = 2; /* read chip id */ ret = i2c_smbus_read_byte_data(client, WHO_AM_I); + +#if defined(CONFIG_MACH_GRANDE) || defined(CONFIG_MACH_IRON) + if (system_rev == 13) { + if (ret == 0xD5) /*KR330D chip ID only use CHN rev.13 */ + ret = 0xD4; + } +#endif if (ret != DEVICE_ID) { if (ret < 0) { pr_err("%s: i2c for reading chip id failed\n", @@ -1460,11 +1561,31 @@ static int lsm330dlc_gyro_probe(struct i2c_client *client, goto err_device_create_file9; } + /* set mounting position of the sensor */ + pdata = client->dev.platform_data; + if (!pdata) { + /*Set by default position 2, it doesn't adjust raw value*/ + data->position = 2; + data->axis_adjust = false; + pr_info("%s, using defualt position = %d\n", __func__, + data->position); + } else { + data->position = pdata->gyro_get_position(); + data->axis_adjust = pdata->axis_adjust; + pr_info("%s, position = %d\n", __func__, data->position); + } + err = device_create_file(data->dev, &dev_attr_position); + if (err < 0) { + pr_err("%s: Failed to create device file(%s)\n", + __func__, dev_attr_position.attr.name); + goto err_device_create_file10; + } + #ifdef DEBUG_REGISTER if (device_create_file(data->dev, &dev_attr_reg_data) < 0) { pr_err("%s: Failed to create device file(%s)!\n", __func__, dev_attr_reg_data.attr.name); - goto err_device_create_file10; + goto err_device_create_file11; } #endif @@ -1474,9 +1595,11 @@ static int lsm330dlc_gyro_probe(struct i2c_client *client, return 0; #ifdef DEBUG_REGISTER +err_device_create_file11: + device_remove_file(data->dev, &dev_attr_position); +#endif err_device_create_file10: device_remove_file(data->dev, &dev_attr_name); -#endif err_device_create_file9: device_remove_file(data->dev, &dev_attr_vendor); err_device_create_file8: @@ -1517,6 +1640,7 @@ static int lsm330dlc_gyro_remove(struct i2c_client *client) int err = 0; struct lsm330dlc_gyro_data *data = i2c_get_clientdata(client); + device_remove_file(data->dev, &dev_attr_position); device_remove_file(data->dev, &dev_attr_vendor); device_remove_file(data->dev, &dev_attr_name); device_remove_file(data->dev, &dev_attr_selftest_dps); |