static void bma255_work_func(struct work_struct *work)
{
struct bma255_v acc;
struct bma255_p *data = container_of((struct delayed_work *)work,
struct bma255_p, work);
unsigned long delay = msecs_to_jiffies(atomic_read(&data->delay));
bma255_read_accel_xyz(data, &acc);
data->accdata.x = acc.x - data->caldata.x;
data->accdata.y = acc.y - data->caldata.y;
data->accdata.z = acc.z - data->caldata.z;
input_report_rel(data->input, REL_X, data->accdata.x);
input_report_rel(data->input, REL_Y, data->accdata.y);
input_report_rel(data->input, REL_Z, data->accdata.z);
input_sync(data->input);
if ((atomic_read(&data->delay) * data->time_count)
>= (ACCEL_LOG_TIME * MSEC_PER_SEC)) {
pr_info("[SENSOR]: %s - x = %d, y = %d, z = %d (ra:%d)\n",
__func__, data->accdata.x, data->accdata.y,
data->accdata.z, data->recog_flag);
data->time_count = 0;
} else
data->time_count++;
schedule_delayed_work(&data->work, delay);
}
static ssize_t bma255_raw_data_read(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct bma255_v acc;
struct bma255_p *data = dev_get_drvdata(dev);
//struct bma255_p *data = bma_acc_get_data();
if (atomic_read(&data->enable) == OFF) {
bma255_set_mode(data->client, BMA255_MODE_NORMAL);
msleep(20);
bma255_read_accel_xyz(data, &acc);
bma255_set_mode(data->client, BMA255_MODE_SUSPEND);
acc.x = acc.x - data->caldata.x;
acc.y = acc.y - data->caldata.y;
acc.z = acc.z - data->caldata.z;
} else {
acc = data->accdata;
}
return snprintf(buf, PAGE_SIZE, "%d,%d,%d\n",
acc.x, acc.y, acc.z);
}
static int bma255_do_calibrate(struct bma255_p *data, int enable)
{
int sum[3] = { 0, };
int ret = 0, cnt;
struct file *cal_filp = NULL;
struct bma255_v acc;
mm_segment_t old_fs;
if (enable) {
data->caldata.x = 0;
data->caldata.y = 0;
data->caldata.z = 0;
if (atomic_read(&data->enable) == ON)
cancel_delayed_work_sync(&data->work);
else
bma255_set_mode(data, BMA255_MODE_NORMAL);
msleep(300);
for (cnt = 0; cnt < CALIBRATION_DATA_AMOUNT; cnt++) {
bma255_read_accel_xyz(data, &acc);
sum[0] += acc.x;
sum[1] += acc.y;
sum[2] += acc.z;
mdelay(10);
}
if (atomic_read(&data->enable) == ON)
schedule_delayed_work(&data->work,
msecs_to_jiffies(atomic_read(&data->delay)));
else
bma255_set_mode(data, BMA255_MODE_SUSPEND);
data->caldata.x = (sum[0] / CALIBRATION_DATA_AMOUNT);
data->caldata.y = (sum[1] / CALIBRATION_DATA_AMOUNT);
data->caldata.z = (sum[2] / CALIBRATION_DATA_AMOUNT);
if (data->caldata.z > 0)
data->caldata.z -= MAX_ACCEL_1G;
else if (data->caldata.z < 0)
data->caldata.z += MAX_ACCEL_1G;
} else {
data->caldata.x = 0;
data->caldata.y = 0;
data->caldata.z = 0;
}
pr_info("[SENSOR]: %s - do accel calibrate %d, %d, %d\n", __func__,
data->caldata.x, data->caldata.y, data->caldata.z);
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("[SENSOR]: %s - Can't open calibration file\n",
__func__);
set_fs(old_fs);
ret = PTR_ERR(cal_filp);
return ret;
}
ret = cal_filp->f_op->write(cal_filp, (char *)&data->caldata,
3 * sizeof(int), &cal_filp->f_pos);
if (ret != 3 * sizeof(int)) {
pr_err("[SENSOR]: %s - Can't write the caldata to file\n",
__func__);
ret = -EIO;
}
filp_close(cal_filp, current->files);
set_fs(old_fs);
return ret;
}
