static int l3g4200d_start_work(struct l3g4200d_data *l3g4200d)
{
int ret;
ret = l3g4200d_hw_init();
if (ret)
return ret;
schedule_delayed_work(&l3g4200d->dwork, US_TO_JIFFIES(l3g4200d->delay));
return 0;
}
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;
}
