static int lsm330dlc_gyro_report_values\ (struct lsm330dlc_gyro_data *gyro_data) { int res; struct gyro_t data; res = lsm330dlc_gyro_read_values(gyro_data->client, &data, gyro_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; 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); lsm330dlc_gyro_restart_fifo(gyro_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); #endif return res; }
static void lsm330dlc_gyro_work_func(struct work_struct *work) { 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(data->client, STATUS_REG); if (status & 0x08) break; } while (retry--); 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); 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 { 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); } 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(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_adjusted[0], gyro_adjusted[1], gyro_adjusted[2]); #endif }
static int lsm330dlc_gyro_report_values\ (struct lsm330dlc_gyro_data *data) { int res, i, j; s16 raw[3] = {0,}, gyro_adjusted[3] = {0,}; res = lsm330dlc_gyro_read_values(data->client, &data->xyz_data, data->entries); if (res < 0) return res; 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(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(data); #ifdef LOGGING_GYRO pr_info("%s, x = %d, y = %d, z = %d\n" , __func__, gyro_adjusted[0], gyro_adjusted[1], gyro_adjusted[2]); #endif return res; }
static void lsm330dlc_gyro_work_func(struct work_struct *work) { int res, retry = 3; struct lsm330dlc_gyro_data *gyro_data\ = container_of(work, struct lsm330dlc_gyro_data, work); s32 status = 0; do { status = i2c_smbus_read_byte_data(gyro_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 (res < 0) pr_err("%s, reading data fail(res = %d)\n", __func__, res); 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; }else { pr_warn("%s, use last data(%d, %d, %d), status=%d, selftest=%d\n", __func__, gyro_data->xyz_data.x, gyro_data->xyz_data.y, gyro_data->xyz_data.z, status, gyro_data->self_test); } 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); #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); #endif }
static int lsm330dlc_gyro_fifo_self_test(struct lsm330dlc_gyro_data *data,\ u8 *cal_pass, s16 *zero_rate_data) { struct gyro_t raw_data; int err; int i, j; s16 raw[3] = {0,}, zero_rate_delta[3] = {0,}; int sum_raw[3] = {0,}; bool zero_rate_read_2nd = false; u8 reg[5]; u8 fifo_pass = 2; u8 status_reg; struct file *cal_filp = NULL; mm_segment_t old_fs; /* fifo mode, enable interrupt, 500dps */ reg[0] = 0x6F; reg[1] = 0x00; reg[2] = 0x04; reg[3] = 0x90; reg[4] = 0x40; for (i = 0; i < 10; i++) { err = i2c_smbus_write_i2c_block_data(data->client, CTRL_REG1 | AC, sizeof(reg), reg); if (err >= 0) break; } if (err < 0) { pr_err("%s: CTRL_REGs i2c writing failed\n", __func__); goto exit; } /* Power up, wait for 800ms for stable output */ msleep(800); read_zero_rate_again: for (i = 0; i < 10; i++) { err = i2c_smbus_write_byte_data(data->client, FIFO_CTRL_REG, BYPASS_MODE); if (err >= 0) break; } if (err < 0) { pr_err("%s : failed to set bypass_mode\n", __func__); goto exit; } for (i = 0; i < 10; i++) { err = i2c_smbus_write_byte_data(data->client, FIFO_CTRL_REG, FIFO_MODE | FIFO_TEST_WTM); if (err >= 0) break; } if (err < 0) { pr_err("%s: failed to set fifo_mode\n", __func__); goto exit; } /* if interrupt mode */ if (!data->enable && data->interruptible) { enable_irq(data->client->irq); err = wait_for_completion_timeout(&data->data_ready, 5*HZ); msleep(200); if (err <= 0) { disable_irq(data->client->irq); if (!err) pr_err("%s: wait timed out\n", __func__); goto exit; } /* if polling mode */ } else msleep(200); /* check out watermark status */ status_reg = i2c_smbus_read_byte_data(data->client, FIFO_SRC_REG); if (!(status_reg & 0x80)) { pr_err("%s: Watermark level is not enough\n", __func__); goto exit; } /* read fifo entries */ err = lsm330dlc_gyro_read_values(data->client, &raw_data, FIFO_TEST_WTM + 2); if (err < 0) { pr_err("%s: lsm330dlc_gyro_read_values() failed\n", __func__); goto exit; } /* print out fifo data */ printk(KERN_INFO "[gyro_self_test] fifo data\n"); for (i = 0; i < sizeof(raw_data) * (FIFO_TEST_WTM + 1); i += sizeof(raw_data)) { raw[0] = (data->fifo_data[i+1] << 8) | data->fifo_data[i]; raw[1] = (data->fifo_data[i+3] << 8) | data->fifo_data[i+2]; raw[2] = (data->fifo_data[i+5] << 8) | data->fifo_data[i+4]; pr_info("%2dth: %8d %8d %8d\n", i/6, raw[0], raw[1], raw[2]); /* for calibration of gyro sensor data */ sum_raw[0] += raw[0]; sum_raw[1] += raw[1]; sum_raw[2] += raw[2]; for (j = 0; j < 3; j++) { if (raw[j] < MIN_ZERO_RATE || raw[j] > MAX_ZERO_RATE) { pr_err("%s: %dth data(%d) is out of zero-rate", __func__, i/6, raw[j]); pr_err("%s: fifo test failed\n", __func__); fifo_pass = 0; *cal_pass = 0; goto exit; } } } /* zero_rate_data */ zero_rate_data[0] = raw[0] * 175 / 10000; zero_rate_data[1] = raw[1] * 175 / 10000; zero_rate_data[2] = raw[2] * 175 / 10000; if (zero_rate_read_2nd == true) { /* check zero_rate second time */ zero_rate_delta[0] -= zero_rate_data[0]; zero_rate_delta[1] -= zero_rate_data[1]; zero_rate_delta[2] -= zero_rate_data[2]; pr_info("[gyro_self_test] zero rate second: %8d %8d %8d\n", zero_rate_data[0], zero_rate_data[1], zero_rate_data[2]); pr_info("[gyro_self_test] zero rate delta: %8d %8d %8d\n", zero_rate_delta[0], zero_rate_delta[1], zero_rate_delta[2]); if ((-5 < zero_rate_delta[0] && zero_rate_delta[0] < 5) && (-5 < zero_rate_delta[1] && zero_rate_delta[1] < 5) && (-5 < zero_rate_delta[2] && zero_rate_delta[2] < 5)) { /* calibration of gyro sensor data */ data->cal_data.x = sum_raw[0]/(FIFO_TEST_WTM + 1); data->cal_data.y = sum_raw[1]/(FIFO_TEST_WTM + 1); data->cal_data.z = sum_raw[2]/(FIFO_TEST_WTM + 1); pr_info("%s: cal data (%d,%d,%d)\n", __func__, data->cal_data.x, data->cal_data.y, data->cal_data.z); /* save cal data */ old_fs = get_fs(); set_fs(KERNEL_DS); cal_filp = filp_open(CALIBRATION_FILE_PATH, O_CREAT | O_TRUNC | O_WRONLY, 0666); if (IS_ERR(cal_filp)) { pr_err("%s: Can't open calibration file\n", __func__); set_fs(old_fs); err = PTR_ERR(cal_filp); return err; } err = cal_filp->f_op->write(cal_filp, (char *)&data->cal_data, 3 * sizeof(s16), &cal_filp->f_pos); if (err != 3 * sizeof(s16)) { pr_err("%s: Can't write the cal data to file\n", __func__); err = -EIO; } filp_close(cal_filp, current->files); set_fs(old_fs); *cal_pass = 1; } /* else calibration is failed */ } else { /* check zero_rate first time, go to check again */ zero_rate_read_2nd = true; sum_raw[0] = 0; sum_raw[1] = 0; sum_raw[2] = 0; zero_rate_delta[0] = zero_rate_data[0]; zero_rate_delta[1] = zero_rate_data[1]; zero_rate_delta[2] = zero_rate_data[2]; pr_info("[gyro_self_test] zero rate first: %8d %8d %8d\n", zero_rate_data[0], zero_rate_data[1], zero_rate_data[2]); goto read_zero_rate_again; } fifo_pass = 1; exit: /* 1: success, 0: fail, 2: retry */ return fifo_pass; }