Exemplo n.º 1
0
static int ak09911c_probe(struct i2c_client *client,
	const struct i2c_device_id *id)
{
	int ret = -ENODEV;
	struct ak09911c_p *data = NULL;
	struct ak09911c_platform_data *pdata;

	pr_info("%s Probe Start!\n", __func__);
	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
		pr_err("%s i2c_check_functionality error\n", __func__);
		goto exit;
	}

	ak09911c_regulator_onoff(&client->dev, 1);

	data = kzalloc(sizeof(struct ak09911c_p), GFP_KERNEL);
	if (data == NULL) {
		pr_err("%s kzalloc error\n", __func__);
		ret = -ENOMEM;
		goto exit_kzalloc;
	}
	if (client->dev.of_node) {
		pdata = devm_kzalloc(&client->dev,
			sizeof(struct ak09911c_platform_data), GFP_KERNEL);
		if (!pdata) {
			dev_err(&client->dev, "Failed to allocate memory\n");
			return -ENOMEM;
		}
		ret = ak09911c_parse_dt(&client->dev, pdata);
		if (ret < 0) {
			pr_err("%s of_node error\n", __func__);
			ret = -ENODEV;
			goto exit_of_node;
		}

	} else
		pdata = client->dev.platform_data;

	if (!pdata) {
		ret = -EINVAL;
		goto exit_pdata;
	}

	data->pdata = pdata;
	ret = ak09911c_setup_pin(pdata);
	if (ret) {
		pr_err("%s could not setup pin\n", __func__);
		goto exit_setup_pin;
	}

	/* ak09911c chip reset */
	ak09911c_reset(data);

	i2c_set_clientdata(client, data);
	data->client = client;

	ret = ak09911c_check_device(data);
	if (ret < 0)
		goto exit_set_mode_check_device;

	/* input device init */
	ret = ak09911c_input_init(data);
	if (ret < 0)
		goto exit_input_init;

	ret = sensors_register(data->factory_device, data, sensor_attrs,
		MODULE_NAME);
	if (ret) {
		pr_err("%s: failed to sensors_register (%d)\n", __func__, ret);
		goto exit_register_failed;
	}

	/* workqueue init */
	INIT_DELAYED_WORK(&data->work, ak09911c_work_func);
	mutex_init(&data->lock);

	atomic_set(&data->delay, AK09911C_DEFAULT_DELAY);
	atomic_set(&data->enable, 0);

	ak09911c_read_fuserom(data);

	pr_info("%s Probe done!(chip pos : %d)\n", __func__, pdata->chip_pos);

	return 0;

exit_register_failed:
	sysfs_remove_group(&data->input->dev.kobj, &ak09911c_attribute_group);
	sensors_remove_symlink(&data->input->dev.kobj, data->input->name);
	input_unregister_device(data->input);
exit_input_init:
exit_set_mode_check_device:
	gpio_free(data->pdata->m_rst_n);
exit_setup_pin:
exit_pdata:
exit_of_node:
	kfree(data);
exit_kzalloc:
	ak09911c_regulator_onoff(&client->dev, 0);
exit:
	pr_err("%s Probe fail!\n", __func__);
	return ret;
}
static int ak09911c_probe(struct i2c_client *client,
		const struct i2c_device_id *id)
{
	int ret = -ENODEV;
	struct ak09911c_p *data = NULL;

	pr_info("[SENSOR]: %s - Probe Start!\n", __func__);

#if defined(CONFIG_MACH_FRESCONEOLTE_CTC)
	ak09911c_power_enable(1);
	mdelay(10);
#elif defined(CONFIG_SEC_BERLUTI_PROJECT)
	ak09911c_power_enable(&client->dev, 1);
#endif
	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
		pr_err("[SENSOR]: %s - i2c_check_functionality error\n",
			__func__);
		goto exit;
	}

	data = kzalloc(sizeof(struct ak09911c_p), GFP_KERNEL);
	if (data == NULL) {
		pr_err("[SENSOR]: %s - kzalloc error\n", __func__);
		ret = -ENOMEM;
		goto exit_kzalloc;
	}

	ret = ak09911c_parse_dt(data, &client->dev);
	if (ret < 0) {
		pr_err("[SENSOR]: %s - of_node error\n", __func__);
		ret = -ENODEV;
		goto exit_of_node;
	}

	ret = ak09911c_setup_pin(data);
	if (ret) {
		pr_err("[SENSOR]: %s - could not setup pin\n", __func__);
		goto exit_setup_pin;
	}

	i2c_set_clientdata(client, data);
	data->client = client;

	ret = ak09911c_check_device(data);
	if (ret < 0) {
		pr_err("[SENSOR]: %s - ak09911c_check_device fail"\
			"(err=%d)\n", __func__, ret);
		goto exit_set_mode_check_device;
	}

	/* input device init */
	ret = ak09911c_input_init(data);
	if (ret < 0)
		goto exit_input_init;

	sensors_register(data->factory_device, data, sensor_attrs, MODULE_NAME);

	/* workqueue init */
	INIT_DELAYED_WORK(&data->work, ak09911c_work_func);
	mutex_init(&data->lock);

	atomic_set(&data->delay, AK09911C_DEFAULT_DELAY);
	atomic_set(&data->enable, 0);

	ak09911c_read_fuserom(data);

	pr_info("[SENSOR]: %s - Probe done!(chip pos : %d)\n",
		__func__, data->chip_pos);

	return 0;

exit_input_init:
exit_set_mode_check_device:
	gpio_free(data->m_rst_n);
exit_setup_pin:
exit_of_node:
	kfree(data);
exit_kzalloc:
exit:
	pr_err("[SENSOR]: %s - Probe fail!\n", __func__);
	return ret;
}