static int selftest_read(struct l3g4200d_t *data) { int total[3]; int i, ret; pr_info("%s\n", __func__); total[0] = 0; total[1] = 0; total[2] = 0; for (i = 0; i < SELFTEST_SAMPLES; i++) { ret = selftest_wait_ZYXDA(); if (ret) { pr_err("%s: selftest_check_ZYXDA fail\n", __func__); return ret; } ret = l3g4200d_read_gyro_values(data); if (ret < 0) { pr_err("%s: l3gd20_read_gyro_values fail\n", __func__); return ret; } pr_info("%s: data: x = %d, y = %d, z = %d\n", __func__, data->x, data->y, data->z); total[0] += data->x; total[1] += data->y; total[2] += data->z; pr_info("%s: total: x = %d, y = %d, z = %d\n", __func__, total[0], total[1], total[2]); } data->x = total[0] / SELFTEST_SAMPLES; data->y = total[1] / SELFTEST_SAMPLES; data->z = total[2] / SELFTEST_SAMPLES; pr_info("%s: average: x = %d, y = %d, z = %d\n", __func__, data->x, data->y, data->z); return 0; }
/* * add by jerrymo * for delay work function, update data */ static void l3g4200d_work_func(struct work_struct *work) { int ret; struct l3g4200d_t data; ret = l3g4200d_read_gyro_values(&data); if (ret < 0) { pr_err("%s()->%d:read gyro values fail!\n", __func__, __LINE__); return; } input_report_abs(gyro->input_dev, ABS_X, data.x); input_report_abs(gyro->input_dev, ABS_Y, data.y); input_report_abs(gyro->input_dev, ABS_Z, data.z); input_sync(gyro->input_dev); #if 0 pr_info("%s():x = %d, y = %d, z = %d.\n", __func__, data.x, data.y, data.z); #endif if (gyro->enabled) schedule_delayed_work(&gyro->dwork, US_TO_JIFFIES(gyro->delay)); }
/* read command for l3g4200d device file */ static ssize_t l3g4200d_read(struct file *file, char __user *buf, size_t count, loff_t *offset) { struct l3g4200d_t data; if (gyro->client == NULL) return -1; l3g4200d_read_gyro_values(&data); GYRO_DBG("X axis: %d\n", data.x); GYRO_DBG("Y axis: %d\n", data.y); GYRO_DBG("Z axis: %d\n", data.z); return 0; }
/* read command for l3g4200d device file */ static ssize_t l3g4200d_read(struct file *file, char __user *buf, size_t count, loff_t *offset) { #if DEBUG struct l3g4200d_t data; #endif if (!gyro || !gyro->client) { pr_err("%s(), Sorry, Gyro driver L3GD20 has not been initialized properly!\n", __func__); return -1; } #if DEBUG l3g4200d_read_gyro_values(&data); pr_info("X axis: %d\n", data.x); pr_info("Y axis: %d\n", data.y); pr_info("Z axis: %d\n", data.z); #endif return 0; }
static void gy_work_func(struct work_struct *work) { int sesc = 0; struct l3g4200d_data *gy = container_of(work, struct l3g4200d_data, work); memset(&sensor_data, 0, sizeof(sensor_data)); if (gy->client == NULL) { printk(KERN_ERR "gy client is null"); return; } sesc = 0; l3g4200d_read_gyro_values(&sensor_data); GYRO_DBG( "X axis: %d Y axis: %d Z axis: %d\n", sensor_data.x,sensor_data.y,sensor_data.z); if (atomic_read(&a_flag)) { GYRO_DBG("report fusion event to input dev!"); input_report_abs(gy->input_dev, ABS_RX, fusiondata[0]); input_report_abs(gy->input_dev, ABS_RY, fusiondata[1]); input_report_abs(gy->input_dev, ABS_RZ, fusiondata[2]); input_report_abs(gy->input_dev, ABS_THROTTLE, fusiondata[3]); input_report_abs(gy->input_dev, ABS_RUDDER, fusiondata[4]); input_report_abs(gy->input_dev, ABS_WHEEL, fusiondata[5]); input_report_abs(gy->input_dev, ABS_GAS, fusiondata[6]); input_report_abs(gy->input_dev, ABS_BRAKE, fusiondata[7]); input_report_abs(gy->input_dev, ABS_HAT0X, fusiondata[8]); input_report_abs(gy->input_dev, ABS_HAT0Y, fusiondata[9]); atomic_set(&a_flag, 0); } input_report_abs(gy->input_dev, ABS_X, sensor_data.x);//cross x,y adapter hal sensors_akm8973.c input_report_abs(gy->input_dev, ABS_Y, sensor_data.y); input_report_abs(gy->input_dev, ABS_Z, sensor_data.z); input_sync(gy->input_dev); /*initalize timer os gyro*/ if(gyro->flags > 0) { hrtimer_start(&gy->timer, ktime_set(sesc, NORMAL_TM), HRTIMER_MODE_REL); } else { hrtimer_start(&gy->timer, ktime_set(SUSPEND_VAL, 0), HRTIMER_MODE_REL); } }
/* modify iotcl interface */ static long l3g4200d_ioctl(struct file *file, unsigned int cmd, unsigned long arg) { int err = 0; unsigned char data[6]; short flag = 1; int val = 0; void __user *argp = (void __user *)arg; /* check l3g4200d_client */ if (gyro->client == NULL) { GYRO_DBG( "I2C driver not install\n"); return -EFAULT; } /* cmd mapping */ switch (cmd) { case L3G4200D_SET_RANGE: if (copy_from_user(data, argp, 1) != 0) { GYRO_DBG( "copy_from_user error\n"); return -EFAULT; } err = l3g4200d_set_range(*data); return err; case L3G4200D_SET_MODE: if (copy_from_user(data, argp, 1) != 0) { GYRO_DBG( "copy_to_user error\n"); return -EFAULT; } /*enable or disable timer*/ if( PM_NORMAL == *data ) { gyro->flags = GYRO_ENABLE; hrtimer_start(&gyro->timer, ktime_set(0, NORMAL_TM), HRTIMER_MODE_REL); } else { err = 0; gyro->flags = GYRO_DISABLE; } GYRO_DBG("L3G4200D_SET_MODE %d\n", *data); err = l3g4200d_set_mode(*data); return err; case L3G4200D_SET_BANDWIDTH: if (copy_from_user(data, argp, 1) != 0) { GYRO_DBG("copy_from_user error\n"); return -EFAULT; } err = l3g4200d_set_bandwidth(*data); return err; case L3G4200D_READ_GYRO_VALUES: err = l3g4200d_read_gyro_values( (struct l3g4200d_t *)data); if (copy_to_user((struct l3g4200d_t *)arg, (struct l3g4200d_t *)data, 6) != 0) { GYRO_DBG("copy_to error\n"); return -EFAULT; } return err; case L3G4200D_SET_FUSION_DATA: if(copy_from_user((void*)fusiondata, argp, sizeof(fusiondata)) != 0){ return -EFAULT; } atomic_set(&a_flag, 1); GYRO_DBG("L3G4200D_SET_FUSION_DATA copy_from error\n"); return err; case L3G4200D_GET_GYRO_DATA: if(copy_to_user(argp, &userdata[0], sizeof(userdata))!= 0) { return -EFAULT; } GYRO_DBG("L3G4200D_GET_GYRO_DATA copy_to error\n"); return 0; case ECS_IOCTL_APP_GET_GYRO_DATA: hrtimer_cancel(&gyro->timer); memset(&sensor_data, 0, sizeof(sensor_data)); err = l3g4200d_read_gyro_values(&sensor_data); if(err < 0) { GYRO_DBG("L3G4200D ECS_IOCTL_APP_GET_GYRO_DATA TEST error!\n"); return err; } GYRO_DBG(" before X axis: %d Y axis: %d Z axis: %d\n", sensor_data.x,sensor_data.y,sensor_data.z); if(copy_to_user(argp, &sensor_data, sizeof(sensor_data))!= 0) { GYRO_DBG("ECS_IOCTL_APP_GET_GYRO_DATA copy to user error\n"); return -EFAULT; } return 0; case ECS_IOCTL_APP_GET_GYRO_CAL: GYRO_DBG("L3G4200D mmi get ret!\n"); /* self-test flowchart update*/ hrtimer_cancel(&gyro->timer); memset(&sensor_data, 0, sizeof(sensor_data)); err = l3g4200d_read_gyro_values(&sensor_data); printk(KERN_ERR " before X axis: %d Y axis: %d Z axis: %d\n", sensor_data.x,sensor_data.y,sensor_data.z); l3g4200d_set_selftest(L3G4200D_SELFTEST_EN); msleep(800); err = l3g4200d_read_gyro_values(&sensor_data); printk(KERN_ERR "after X axis: %d Y axis: %d Z axis: %d\n", sensor_data.x,sensor_data.y,sensor_data.z); if(err < 0) { GYRO_DBG("L3G4200D SELF-TEST read error!\n"); return err; } /*put data to user*/ if (copy_to_user(argp, &sensor_data, sizeof(sensor_data))) { return -EFAULT; } l3g4200d_disable_selftest(); return 0; case ECS_IOCTL_APP_GET_CAL: GYRO_DBG("L3G4200D mmi get ret!\n"); /* self-test flowchart update*/ hrtimer_cancel(&gyro->timer); memset(&sensor_data, 0, sizeof(sensor_data)); err = l3g4200d_read_gyro_values(&sensor_data); printk(KERN_ERR " before X axis: %d Y axis: %d Z axis: %d\n", sensor_data.x,sensor_data.y,sensor_data.z); l3g4200d_set_selftest(L3G4200D_SELFTEST_EN); msleep(800); err = l3g4200d_read_gyro_values(&sensor_data); printk(KERN_ERR "after X axis: %d Y axis: %d Z axis: %d\n", sensor_data.x,sensor_data.y,sensor_data.z); if(err < 0) { GYRO_DBG("L3G4200D SELF-TEST read error!\n"); return err; } val = abs(sensor_data.x); if( (val > MAX_VAL) || (val < MIN_VAL) ) { flag = -1; GYRO_DBG("L3G4200D SELF-TEST x %d error!\n", val); } val = abs(sensor_data.y); if( (val > MAX_VAL) || (val < MIN_VAL) ) { flag = -1; GYRO_DBG("L3G4200D SELF-TEST y %d error!\n", val); } val = abs(sensor_data.z); if( (val > MAX_VAL) || (val < MIN_VAL) ) { flag = -1; GYRO_DBG("L3G4200D SELF-TEST z %d error!\n", val); } if (copy_to_user(argp, &flag, sizeof(flag))) { return -EFAULT; } l3g4200d_disable_selftest(); return 0; default: return 0; } return err; }
/* ioctl command for l3g4200d device file */ static long l3g4200d_ioctl_int(struct file *file, unsigned int cmd, unsigned long arg) { int err = 0; unsigned char data[6]; int enable; int64_t delay; int test_result; int status; char temp[1] = {0}; /* check l3g4200d_client */ if (!gyro || gyro->client == NULL) { #if DEBUG pr_err("%s(), Sorry, Gyro driver L3GD20 has not been initialized properly!\n", __func__); #endif return -EFAULT; } /* cmd mapping */ switch (cmd) { case L3GD20_GET_TEMP: if (l3gd20_read_temp(temp) < 0) { pr_err("%s(), L3GD20_GET_TEMP ioctl error!\n", __func__); return -EFAULT; } if (copy_to_user((char __user *)arg, temp, 1) != 0) { #if DEBUG pr_err("L3GD20_GET_TEMP copy_to error\n"); #endif return -EFAULT; } return 0; case L3G4200D_SELFTEST: err = l3g4200d_selftest(&test_result); pr_info("%s: self test\n", __func__); if (copy_to_user((void __user *)arg, &test_result, sizeof(int)) != 0) { pr_err("%s: copy_to_user error\n", __func__); return -EFAULT; } return err; case L3G4200D_SET_RANGE: if (copy_from_user(data, (unsigned char *)arg, 1) != 0) { #if DEBUG pr_err("copy_from_user error\n"); #endif return -EFAULT; } err = l3g4200d_set_range(*data); return err; case L3G4200D_SET_MODE: if (copy_from_user(data, (unsigned char *)arg, 1) != 0) { #if DEBUG pr_err("copy_to_user error\n"); #endif return -EFAULT; } err = l3g4200d_set_mode(*data); if (!err) gyro->mode = data[0]; return err; case L3G4200D_SET_BANDWIDTH: if (copy_from_user(data, (unsigned char *)arg, 1) != 0) { #if DEBUG pr_err("copy_from_user error\n"); #endif return -EFAULT; } err = l3g4200d_set_bandwidth(*data); return err; case L3G4200D_READ_GYRO_VALUES: err = l3g4200d_read_gyro_values( (struct l3g4200d_t *)data); if (copy_to_user((struct l3g4200d_t *)arg, (struct l3g4200d_t *)data, 6) != 0) { #if DEBUG pr_err("copy_to error\n"); #endif return -EFAULT; } return err; /*add by jerrymo*/ case L3G4200D_SET_ENABLE: if (copy_from_user(&enable, (void __user *)arg, sizeof(int))) { pr_err("%s()->%d:copy from user fail!\n", __func__, __LINE__); return -EINVAL; } pr_info("%s():set enable %d.\n", __func__, enable); err = l3g4200d_set_enable(gyro, enable); return err; break; case L3G4200D_GET_ENABLE: if (copy_to_user((void __user *)arg, &gyro->enabled, sizeof(int))) { pr_err("%s()->%d:copy to user fail!\n", __func__, __LINE__); return -EINVAL; } pr_info("%s():get enable %d.\n", __func__, gyro->enabled); break; case L3G4200D_SET_DELAY: if (copy_from_user(&delay, (void __user *)arg, sizeof(int64_t))) { pr_err("%s()->%d:copy from user fail!\n", __func__, __LINE__); return -EINVAL; } /* delay in us */ gyro->delay = delay; l3g4200d_gyro_update_odr(gyro->delay); pr_debug("%s():set delay %lld.\n", __func__, delay); break; case L3G4200D_GET_DELAY: if (copy_to_user((void __user *)arg, &gyro->delay, sizeof(int64_t))) { pr_err("%s()->%d:copy to user fail!\n", __func__, __LINE__); return -EINVAL; } pr_debug("%s():get delay %lld.\n", __func__, gyro->delay); break; case L3G4200D_GET_SUSPEND_STATUS: status = atomic_read(&suspend_flag); if (copy_to_user((void __user *)arg, &status, sizeof(status))) { pr_err("%s()->%d:copy to user fail!\n", __func__, __LINE__); return -EFAULT; } pr_debug("%s():get delay %d.\n", __func__, atomic_read(&suspend_flag)); break; /*end add*/ default: return 0; } return 0; }