static int yas_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
{
struct yas_state *st;
struct iio_dev *indio_dev;
int ret, i;
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->acc.callback.device_open = yas_device_open;
st->acc.callback.device_close = yas_device_close;
st->acc.callback.device_read = yas_device_read;
st->acc.callback.device_write = yas_device_write;
st->acc.callback.usleep = yas_usleep;
st->acc.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
for (i = 0; i < 3; i++) {
st->compass_data[i] = 0;
st->calib_bias[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_acc_driver_init(&st->acc);
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = st->acc.init();
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = st->acc.set_enable(1);
if (ret < 0) {
ret = -EFAULT;
goto error_driver_term;
}
return 0;
error_driver_term:
st->acc.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;
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 ret, i;
s8 *bias;
struct yas_acc_platform_data *pdata;
I("%s\n", __func__);
this_client = i2c;
indio_dev = iio_device_alloc(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->acc.callback.device_open = yas_device_open;
st->acc.callback.device_close = yas_device_close;
st->acc.callback.device_read = yas_device_read;
st->acc.callback.device_write = yas_device_write;
st->acc.callback.usleep = yas_usleep;
st->acc.callback.current_time = yas_current_time;
st->indio_dev = indio_dev;
INIT_DELAYED_WORK(&st->work, yas_work_func);
INIT_WORK(&st->resume_work, yas_resume_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_acc_driver_init(&st->acc);
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = st->acc.init();
if (ret < 0) {
ret = -EFAULT;
goto error_unregister_iio;
}
ret = st->acc.set_enable(1);
if (ret < 0) {
ret = -EFAULT;
goto error_driver_term;
}
pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
if (pdata == NULL)
E("%s: memory allocation for pdata failed.", __func__);
else
yas_parse_dt(&i2c->dev, pdata);
for (i = 0; i < 3; i++) {
st->accel_data[i] = 0;
bias = (s8 *)&pdata->gs_kvalue;
st->calib_bias[i] = -(bias[2-i] *
YAS_GRAVITY_EARTH / 256);
I("%s: calib_bias[%d] = %d\n", __func__, i, st->calib_bias[i]);
}
mutex_lock(&st->lock);
if ((pdata->placement < 8) && (pdata->placement >= 0)) {
ret = st->acc.set_position(pdata->placement);
I("%s: set position = %d\n", __func__, pdata->placement);
} else {
ret = st->acc.set_position(5);
D("%s: set default position = 5\n", __func__);
}
mutex_unlock(&st->lock);
#ifdef CONFIG_CIR_ALWAYS_READY
module.IRQ = pdata->intr;
I("%s: IRQ = %d\n", __func__, module.IRQ);
ret = request_irq(module.IRQ, kxtj2_irq_handler, IRQF_TRIGGER_RISING,
"kxtj2", &module);
enable_irq_wake(module.IRQ);
if (ret)
E("%s: Could not request irq = %d\n", __func__, module.IRQ);
module.kxtj2_wq = create_singlethread_workqueue("kxtj2_wq");
if (!module.kxtj2_wq) {
E("%s: Can't create workqueue\n", __func__);
ret = -ENOMEM;
goto error_create_singlethread_workqueue;
}
#endif
init_irq_work(&st->iio_irq_work, iio_trigger_work);
g_st = st;
ret = create_sysfs_interfaces(st);
if (ret) {
E("%s: create_sysfs_interfaces fail, ret = %d\n",
__func__, ret);
goto err_create_fixed_sysfs;
}
I("%s: Successfully probe\n", __func__);
return 0;
err_create_fixed_sysfs:
#ifdef CONFIG_CIR_ALWAYS_READY
if (module.kxtj2_wq)
destroy_workqueue(module.kxtj2_wq);
error_create_singlethread_workqueue:
#endif
kfree(pdata);
error_driver_term:
st->acc.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_device_free(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 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
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;
}
pr_info("[SENSOR] %s is finished!!\n", __func__);
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_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;
}
static int yas_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
{
struct yas_state *st;
struct iio_dev *indio_dev;
int ret, i;
int position;
pr_err("%s : PROBE START\n", __func__);
this_client = i2c;
indio_dev = iio_device_alloc(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;
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
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);
ret = sensors_register(factory_dev, indio_dev, mag_sensor_attrs,
MODULE_NAME_MAG);
if (ret < 0) {
pr_err("%s: cound not register mag sensor device(%d).\n",
__func__, ret);
goto err_mag_sensor_register_failed;
}
pr_err("%s : PROBE END\n", __func__);
return 0;
err_mag_sensor_register_failed:
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_device_free(indio_dev);
error_ret:
i2c_set_clientdata(i2c, NULL);
this_client = NULL;
return ret;
}
static int yas_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct yas_state *st;
struct iio_dev *indio_dev;
int ret, i;
u8 temp;
pr_info("[SENSOR]: %s - Probe Start!\n", __func__);
this_client = client;
/* Check if the device is there or not. */
ret = yas_device_read(YAS_TYPE_MAG, REG_DIDR, &temp, 1);
if (ret < 0) {
ret = -ENODEV;
goto error_check_dev;
}
indio_dev = iio_device_alloc(sizeof(*st));
if (!indio_dev) {
ret = -ENOMEM;
goto error_ret;
}
st = iio_priv(indio_dev);
i2c_set_clientdata(client, indio_dev);
st->client = client;
/* IIO device init */
indio_dev->name = "magnetic_sensor";
indio_dev->channels = yas_channels;
indio_dev->num_channels = ARRAY_SIZE(yas_channels);
indio_dev->dev.parent = &client->dev;
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->info = &yas_info;
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
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(st, &client->dev);
if (ret < 0) {
pr_err("%s of_node error\n", __func__);
ret = -ENODEV;
goto error_of_node;
}
ret= st->mag.set_position(st->chip_pos);
if (ret < 0) {
ret = -EFAULT;
goto error_of_node;
}
spin_lock_init(&st->spin_lock);
ret = sensors_register(st->factory_dev, st, mag_sensor_attrs,
MODULE_NAME);
if (ret < 0) {
pr_err("[SENSOR] %s: cound not register mag device(%d).\n",
__func__, ret);
goto err_mag_sensor_register_failed;
}
pr_info("[SENSOR]: %s - Probe done!\n", __func__);
return 0;
err_mag_sensor_register_failed:
error_of_node:
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
i2c_set_clientdata(client, NULL);
error_ret:
error_check_dev:
this_client = NULL;
pr_err("[SENSOR]: %s - Probe fail!\n", __func__);
return ret;
}