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; }