static int __devinit l3gd20_probe(struct i2c_client *client,
const struct i2c_device_id *devid)
{
struct l3g4200d_data *data;
int err = 0;
int tempvalue = 0;
if (client->dev.platform_data == NULL) {
dev_err(&client->dev, "platform data is NULL. exiting.\n");
err = -ENODEV;
goto exit;
}
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
err = -ENODEV;
goto exit;
}
if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) {
err = -ENODEV;
goto exit;
}
/*
* OK. For now, we presume we have a valid client. We now create the
* client structure, even though we cannot fill it completely yet.
*/
data = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL);
if (data == NULL) {
dev_err(&client->dev,
"failed to allocate memory for module data\n");
err = -ENOMEM;
goto exit;
}
mutex_init(&data->ioctl_lock);
i2c_set_clientdata(client, data);
data->client = client;
gyro = data;
data->pdata = kmemdup(client->dev.platform_data,
sizeof(*data->pdata), GFP_KERNEL);
if (!data->pdata)
goto exit_kfree;
err = l3g4200d_validate_pdata(data);
if (err < 0) {
dev_err(&client->dev, "failed to validate platform data\n");
goto exit_kfree_pdata;
}
err = i2c_smbus_read_byte(client);
if (err < 0) {
#if DEBUG
pr_err("i2c_smbus_read_byte error!!\n");
pr_info("%s(), client's content: addr: 0x%x, adapter: %p \n"
"name: %s, driver:%p, client->adapter.name is %s\n", __func__, client->addr,
client->adapter, client->name, client->driver,
client->adapter->name);
#endif
goto exit_kfree_pdata;
} else {
#if DEBUG
pr_info("L3GD20 DSevice detected!\n");
#endif
}
/* read chip id */
tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I);
printk("%s(), tempvalue is 0x%x\n", __func__, tempvalue);
if ((tempvalue & 0x00FF) == GYROSCPOE_CHIP_ID) {
#if DEBUG
pr_info("I2C driver registered!\n");
#endif
} else {
pr_err("%s(), Read Chip ID Error!\n", __func__);
data->client = NULL;
err = -ENODEV;
goto exit_kfree_pdata;
}
data->misc_device.minor = MISC_DYNAMIC_MINOR;
data->misc_device.name = GYROSCPOE_CHIP_NAME;
data->misc_device.fops = &l3g4200d_fops;
err = misc_register(&data->misc_device);
if (err < 0) {
pr_err("%s()->%d:can not create misc device!\n",
__func__, __LINE__);
goto exit_kfree_pdata;
}
/* add by jerrymo, create input device. */
err = l3g4200d_create_input(data);
if (err)
goto exit_misc_deregister;
INIT_DELAYED_WORK(&data->dwork, l3g4200d_work_func);
data->delay = L3G4200D_DEFAULT_DELAY;
l3g4200d_gyro_update_odr(data->delay);
#ifdef CONFIG_HAS_EARLYSUSPEND
data->esuspend = gyro_esuspend;
register_early_suspend(&data->esuspend);
#endif
#ifdef CONFIG_SENSORS_L3GD20_SELFTEST
do {
int test_result;
pr_info("%s(), Initial self test\n", __func__);
err = l3g4200d_selftest(&test_result);
if (err < 0)
pr_err("%s: fail\n", __func__);
else
pr_info("%s: self test\n", __func__);
if (test_result == 1)
pr_info("%s: PASS\n", __func__);
else
pr_info("%s: FAIL\n", __func__);
} while (0);
#endif
#if DEBUG
pr_info("L3GD20 device created successfully\n");
#endif
return 0;
exit_misc_deregister:
misc_deregister(&data->misc_device);
exit_kfree_pdata:
kfree(data->pdata);
exit_kfree:
kfree(data);
exit:
gyro = NULL;
#if DEBUG
pr_err("%s: Driver Initialization failed\n", __FILE__);
#endif
return err;
}
static int l3g4200d_probe(struct i2c_client *client,
const struct i2c_device_id *devid)
{
struct l3g4200d_data *data;
struct gyro_platform_data *platform_data = NULL;
int ret = -1;
int tempvalue;
GYRO_DBG("l3g4200d_probe start!\n");
if (client->dev.platform_data == NULL) {
dev_err(&client->dev, "platform data is NULL. exiting.\n");
ret = -ENODEV;
goto exit;
}
platform_data = client->dev.platform_data;
if(platform_data->gyro_power)
{
ret = platform_data->gyro_power(IC_PM_ON);
if( ret < 0)
{
dev_err(&client->dev, "gyro power on error!\n");
goto exit;
}
}
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
ret = -ENODEV;
goto exit_pm_off;
}
if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK)){
ret = -ENODEV;
goto exit_pm_off;
}
/*
* OK. For now, we presume we have a valid client. We now create the
* client structure, even though we cannot fill it completely yet.
*/
data = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL);
if (NULL == data) {
dev_err(&client->dev,
"failed to allocate memory for module data\n");
ret = -ENOMEM;
goto exit_pm_off;
}
mutex_init(&data->mlock);
INIT_WORK(&data->work, gy_work_func);
i2c_set_clientdata(client, data);
data->client = client;
data->pdata = platform_data;
ret = l3g4200d_validate_pdata(data);
if (ret < 0) {
dev_err(&client->dev, "failed to validate platform data\n");
goto exit_kfree;
}
ret = i2c_smbus_read_byte(client);
if ( ret < 0) {
GYRO_DBG("i2c_smbus_read_byte error!!\n");
goto err_detect_failed;
} else {
GYRO_DBG("L3G4200D Device detected!\n");
}
/* read chip id */
tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I);
if ((tempvalue & 0x00FF) == 0x00D3) {
GYRO_DBG("I2C driver registered!\n");
} else {
data->client = NULL;
ret = -ENODEV;
goto err_detect_failed;
}
if (sensor_dev == NULL)
{
data->input_dev = input_allocate_device();
if (data->input_dev == NULL) {
ret = -ENOMEM;
printk(KERN_ERR "gs_probe: Failed to allocate input device\n");
goto err_input_dev_alloc_failed;
}
data->input_dev->name = "gy_sensors";
sensor_dev = data->input_dev;
}else{
data->input_dev = sensor_dev;
}
data->input_dev->id.vendor = VENDOR;
#if 0
set_bit(EV_REL,data->input_dev->evbit);
set_bit(REL_RX, data->input_dev->absbit);
set_bit(REL_RY, data->input_dev->absbit);
set_bit(REL_RZ, data->input_dev->absbit);
#endif
set_bit(EV_ABS,data->input_dev->evbit);
/* modify the func of init */
input_set_abs_params(data->input_dev, ABS_RX, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_RY, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_RZ, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_X, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_Y, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_Z, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_THROTTLE, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_RUDDER, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_WHEEL, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_GAS, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_HAT0X, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_HAT0Y, MIN_VALUE, MAX_VALUE, 0, 0);
input_set_abs_params(data->input_dev, ABS_BRAKE, MIN_VALUE, MAX_VALUE, 0, 0);
set_bit(EV_SYN,data->input_dev->evbit);
data->input_dev->id.bustype = BUS_I2C;
input_set_drvdata(data->input_dev, data);
ret = input_register_device(data->input_dev);
if (ret) {
printk(KERN_ERR "gy_probe: Unable to register %s input device\n", data->input_dev->name);
/* create l3g-dev device class */
goto err_input_register_device_failed;
}
ret = misc_register(&gysensor_device);
if (ret) {
printk(KERN_ERR "gy_probe: gysensor_device register failed\n");
goto err_misc_device_register_failed;
}
hrtimer_init(&data->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
data->timer.function = gy_timer_func;
atomic_set(&a_flag, 0);
data->flags = -1;
#ifdef CONFIG_HAS_EARLYSUSPEND
data->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
data->early_suspend.suspend = gy_early_suspend;
data->early_suspend.resume = gy_late_resume;
register_early_suspend(&data->early_suspend);
#endif
GYRO_DBG("L3G4200D device created successfully\n");
gy_wq = create_singlethread_workqueue("gy_wq");
if (!gy_wq)
return -ENOMEM;
gyro = data;
// hrtimer_start(&this_gs_data->timer, ktime_set(0, 500000000), HRTIMER_MODE_REL);
#ifdef CONFIG_HUAWEI_HW_DEV_DCT
/* detect current device successful, set the flag as present */
set_hw_dev_flag(DEV_I2C_GYROSCOPE);
#endif
printk(KERN_DEBUG "l3g4200d_probe successful");
return 0;
err_misc_device_register_failed:
misc_deregister(&gysensor_device);
err_input_register_device_failed:
input_free_device(gyro->input_dev);
err_input_dev_alloc_failed:
err_detect_failed:
exit_kfree:
kfree(gyro);
exit_pm_off:
if(platform_data->gyro_power)
{
platform_data->gyro_power(IC_PM_OFF);
}
exit:
return ret;
}
static int l3g4200d_probe(struct i2c_client *client,
const struct i2c_device_id *devid)
{
int err;
struct l3g4200d_data *gyro;
dev_info(&client->dev, "probe start.\n");
if (client->dev.platform_data == NULL) {
dev_err(&client->dev, "platform data is NULL. exiting.\n");
err = -ENODEV;
goto err_out;
}
if (!i2c_check_functionality(client->adapter,
I2C_FUNC_I2C | I2C_FUNC_SMBUS_I2C_BLOCK)) {
dev_err(&client->dev, "client not i2c capable.\n");
err = -ENODEV;
goto err_out;
}
gyro = kzalloc(sizeof(*gyro), GFP_KERNEL);
if (gyro == NULL) {
dev_err(&client->dev, "failed to allocate memory.\n");
err = -ENOMEM;
goto err_out;
}
gyro->pdata = kmalloc(sizeof(*gyro->pdata), GFP_KERNEL);
if (gyro->pdata == NULL) {
dev_err(&client->dev, "failed to allocate memory for pdata.");
err = -ENOMEM;
goto err_free_gyro;
}
memcpy(gyro->pdata, client->dev.platform_data, sizeof(*gyro->pdata));
err = l3g4200d_validate_pdata(gyro);
if (err < 0) {
dev_err(&client->dev, "failed to validate platform data.\n");
goto err_free_pdata;
}
gyro->client = client;
i2c_set_clientdata(client, gyro);
err = l3g4200d_input_init(gyro);
if (err < 0)
goto err_free_pdata;
err = l3g4200d_hw_init(gyro);
if (err < 0) {
dev_err(&client->dev, "failed to init l3g4200d hardware.\n");
goto err_clean_input;
}
err = create_sysfs_interfaces(&client->dev);
if (err < 0)
goto err_clean_input;
mutex_init(&gyro->lock);
INIT_DELAYED_WORK(&gyro->work, l3g4200d_poll_work);
gyro->es.level = EARLY_SUSPEND_LEVEL_DISABLE_FB + 10;
gyro->es.suspend = l3g4200d_early_suspend;
gyro->es.resume = l3g4200d_late_resume;
register_early_suspend(&gyro->es);
dev_info(&client->dev, "probed.\n");
return 0;
err_clean_input:
input_unregister_device(gyro->input_dev);
err_free_pdata:
kfree(gyro->pdata);
err_free_gyro:
kfree(gyro);
err_out:
dev_err(&client->dev, "Driver Initialization failed, %d\n", err);
return err;
}
