/* Device Initialization */
static int device_init(void)
{
int res = -1;
#if 0
unsigned char buf[5];
buf[0] = 0x27;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = 0x00;
res = l3g4200d_i2c_write(CTRL_REG1, &buf[0], 5);
#endif
res = l3g4200d_set_mode(PM_NORMAL);
if(res < 0)
{
printk(KERN_ERR "open device i2c write mode error\n");
goto out;
}
res = l3g4200d_set_range(L3G4200D_FS_2000DPS);
if(res < 0)
{
printk(KERN_ERR "open device i2c write range error\n");
goto out;
}
res = l3g4200d_set_bandwidth(ODR100_BW25);
if(res < 0)
{
printk(KERN_ERR "open device i2c write bandwidth error\n");
goto out;
}
out:
return res;
}
/*
* configure device registers
* add by jerrymo
*/
static int l3g4200d_hw_init(void)
{
int ret;
ret = l3g4200d_set_mode(PM_NORMAL);
if (ret)
goto err_exit;
ret = l3g4200d_set_range(L3G4200D_FS_DPS);
if (ret) {
pr_info("%s: set range fail\n", __func__);
goto err_exit;
}
ret = l3g4200d_set_bandwidth(L3G4200D_ODR_BW);
if (ret) {
pr_info("%s: set bandwidth fail\n", __func__);
goto err_exit;
}
return 0;
err_exit:
pr_err("%s()->%d: error happen(ret = %d)!\n",
__func__, __LINE__, ret);
return ret;
}
static int l3g4200d_suspend(struct i2c_client *client, pm_message_t state)
{
int ret;
struct l3g4200d_data *lis = i2c_get_clientdata(client);
mutex_lock(&lis->mlock);
ret = l3g4200d_set_mode(PM_OFF);
if(ret < 0)
{
printk(KERN_ERR "open device i2c set mode PM_OFF err!\n");
}
hrtimer_cancel(&lis->timer);
ret = cancel_work_sync(&lis->work);
mutex_unlock(&lis->mlock);
return 0;
}
static int l3g4200d_resume(struct i2c_client *client)
{
int ret;
struct l3g4200d_data *lis = i2c_get_clientdata(client);
ret = l3g4200d_set_mode(PM_NORMAL);
if(ret < 0)
{
printk(KERN_ERR "open device i2c set mode PM_NORMAL err!\n");
}
mutex_lock(&lis->mlock);
if(gyro->flags > 0)
hrtimer_start(&lis->timer, ktime_set(0, NORMAL_TM), HRTIMER_MODE_REL);
else
hrtimer_start(&lis->timer, ktime_set(SUSPEND_VAL, 0), HRTIMER_MODE_REL);
mutex_unlock(&lis->mlock);
return 0;
}
/* release command for l3g4200d device file */
static int l3g4200d_close(struct inode *inode, struct file *file)
{
int ret = -1;
if (gyro == NULL || gyro->client == NULL) {
GYRO_DBG( "I2C driver not install\n");
return -1;
}
hrtimer_cancel(&gyro->timer);
ret = l3g4200d_set_mode(PM_OFF);
if(ret < 0)
{
printk(KERN_ERR "close device i2c set mode PM_OFF err!\n");
}
gyro->flags = -1;
GYRO_DBG("L3G4200D has been closed\n");
return 0;
}
static int l3g4200d_resume(struct device *dev)
{
int ret;
pr_debug("%s\n", __func__);
if (!gyro || !gyro->client) {
pr_err("%s(), Sorry, Gyro driver L3GD20 has not been initialized properly!\n", __func__);
return -1;
}
if (gyro->mode == PM_NORMAL) {
ret = l3g4200d_set_mode(PM_NORMAL);
if (ret < 0)
pr_err("%s():set pm normal mode fail(ret = %d)!\n", __func__, ret);
}
return 0;
}
/* 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;
}
static void l3g4200d_shutdown(struct i2c_client *client)
{
l3g4200d_set_mode(PM_OFF);
}
/* 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;
}
static int l3g4200d_power_down(void)
{
return l3g4200d_set_mode(PM_OFF);
}