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;
}
