static int yas_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
{
struct yas_state *st;
struct iio_dev *indio_dev;
struct yas53x_platform_data *pdata;
int ret;
D("%s", __func__);
this_client = i2c;
indio_dev = iio_allocate_device(sizeof(*st));
if (!indio_dev) {
ret = -ENOMEM;
goto error_ret;
}
i2c_set_clientdata(i2c, indio_dev);
indio_dev->name = YAS_MAG_NAME;
indio_dev->dev.parent = &i2c->dev;
indio_dev->info = &yas_info;
indio_dev->channels = yas_channels;
indio_dev->num_channels = ARRAY_SIZE(yas_channels);
indio_dev->modes = INDIO_DIRECT_MODE;
printk("YAMAHA id->name[%s]\n", id->name);
st = iio_priv(indio_dev);
st->client = i2c;
st->sampling_frequency = 20;
st->mag.callback.device_open = yas_device_open;
st->mag.callback.device_close = yas_device_close;
st->mag.callback.device_read = yas_device_read;
st->mag.callback.device_write = yas_device_write;
st->mag.callback.usleep = yas_usleep;
st->mag.callback.current_time = yas_current_time;
INIT_DELAYED_WORK(&st->work, yas_work_func);
mutex_init(&st->lock);
#ifdef CONFIG_HAS_EARLYSUSPEND
st->sus.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
st->sus.suspend = yas_early_suspend;
st->sus.resume = yas_late_resume;
register_early_suspend(&st->sus);
#endif
ret = yas_probe_buffer(indio_dev);
if (ret)
goto error_free_dev;
ret = yas_probe_trigger(indio_dev);
if (ret)
goto error_remove_buffer;
ret = iio_device_register(indio_dev);
if (ret)
goto error_remove_trigger;
ret = yas_mag_driver_init(&st->mag);
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = st->mag.init();
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = st->mag.set_enable(1);
if (ret < 0) {
ret = -EFAULT;
goto error_driver_term;
}
pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
if (pdata == NULL) {
D("%s: memory allocation for pdata failed.", __func__);
} else {
yas53x_parse_dt(&i2c->dev , pdata);
}
mutex_lock(&st->lock);
if (pdata->chip_layout<8 && pdata->chip_layout>=0) {
ret = st->mag.set_position(pdata->chip_layout);
D("%s: set position to %d\n", __func__, pdata->chip_layout);
} else {
ret = st->mag.set_position(5);
D("%s: set default position: 5\n", __func__);
}
mutex_unlock(&st->lock);
return 0;
error_driver_term:
st->mag.term();
error_unregister_iio:
iio_device_unregister(indio_dev);
error_remove_trigger:
yas_remove_trigger(indio_dev);
error_remove_buffer:
yas_remove_buffer(indio_dev);
error_free_dev:
#ifdef CONFIG_HAS_EARLYSUSPEND
unregister_early_suspend(&st->sus);
#endif
iio_free_device(indio_dev);
error_ret:
i2c_set_clientdata(i2c, NULL);
this_client = NULL;
return ret;
}
static int yas_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
{
struct yas_state *st;
struct iio_dev *indio_dev;
int position, i;
int ret;
pr_info("[SENSOR] %s is called!!\n", __func__);
this_client = i2c;
indio_dev = iio_allocate_device(sizeof(*st));
if (!indio_dev) {
ret = -ENOMEM;
goto error_ret;
}
i2c_set_clientdata(i2c, indio_dev);
indio_dev->name = id->name;
indio_dev->dev.parent = &i2c->dev;
indio_dev->info = &yas_info;
indio_dev->channels = yas_channels;
indio_dev->num_channels = ARRAY_SIZE(yas_channels);
indio_dev->modes = INDIO_DIRECT_MODE;
st = iio_priv(indio_dev);
st->client = i2c;
st->sampling_frequency = 20;
st->mag.callback.device_open = yas_device_open;
st->mag.callback.device_close = yas_device_close;
st->mag.callback.device_read = yas_device_read;
st->mag.callback.device_write = yas_device_write;
st->mag.callback.usleep = yas_usleep;
st->mag.callback.current_time = yas_current_time;
INIT_DELAYED_WORK(&st->work, yas_work_func);
mutex_init(&st->lock);
#ifdef CONFIG_HAS_EARLYSUSPEND
st->sus.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
st->sus.suspend = yas_early_suspend;
st->sus.resume = yas_late_resume;
register_early_suspend(&st->sus);
#endif
#ifdef CONFIG_SENSORS
ret = sensors_register(st->yas_device, st, sensor_attrs, "magnetic_sensor");
if (ret) {
pr_err("%s: cound not register gyro sensor device(%d).\n",
__func__, ret);
goto err_yas_sensor_register_failed;
}
#endif
for (i = 0; i < 3; i++)
st->compass_data[i] = 0;
ret = yas_probe_buffer(indio_dev);
if (ret)
goto error_free_dev;
ret = yas_probe_trigger(indio_dev);
if (ret)
goto error_remove_buffer;
ret = iio_device_register(indio_dev);
if (ret)
goto error_remove_trigger;
ret = yas_mag_driver_init(&st->mag);
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = st->mag.init();
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = yas_parse_dt(&i2c->dev, st);
if(!ret){
position = st->position;
ret = st->mag.set_position(position);
pr_info("[SENSOR] set_position (%d)\n", position);
}
spin_lock_init(&st->spin_lock);
pr_info("[SENSOR] %s is finished!!\n", __func__);
return 0;
error_unregister_iio:
iio_device_unregister(indio_dev);
error_remove_trigger:
yas_remove_trigger(indio_dev);
error_remove_buffer:
yas_remove_buffer(indio_dev);
error_free_dev:
#ifdef CONFIG_SENSORS
sensors_unregister(st->yas_device, sensor_attrs);
err_yas_sensor_register_failed:
#endif
#ifdef CONFIG_HAS_EARLYSUSPEND
unregister_early_suspend(&st->sus);
#endif
iio_free_device(indio_dev);
error_ret:
i2c_set_clientdata(i2c, NULL);
this_client = NULL;
return ret;
}
