示例#1
0
static int __devinit l3gd20_probe(struct i2c_client *client,
			       const struct i2c_device_id *devid)
{
	struct l3g4200d_data *data;
	int err = 0;
	int tempvalue = 0;

	if (client->dev.platform_data == NULL) {
		dev_err(&client->dev, "platform data is NULL. exiting.\n");
		err = -ENODEV;
		goto exit;
	}

	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
		err = -ENODEV;
		goto exit;
	}

	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) {
		err = -ENODEV;
		goto exit;
	}

	/*
	 * OK. For now, we presume we have a valid client. We now create the
	 * client structure, even though we cannot fill it completely yet.
	 */
	data = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL);
	if (data == NULL) {
		dev_err(&client->dev,
			"failed to allocate memory for module data\n");
		err = -ENOMEM;
		goto exit;
	}

	mutex_init(&data->ioctl_lock);
	i2c_set_clientdata(client, data);
	data->client = client;
	gyro = data;

	data->pdata = kmemdup(client->dev.platform_data,
							sizeof(*data->pdata), GFP_KERNEL);
	if (!data->pdata)
		goto exit_kfree;

	err = l3g4200d_validate_pdata(data);
	if (err < 0) {
		dev_err(&client->dev, "failed to validate platform data\n");
		goto exit_kfree_pdata;
	}

	err = i2c_smbus_read_byte(client);
	
	if (err < 0) {
#if DEBUG
		pr_err("i2c_smbus_read_byte error!!\n");
		pr_info("%s(), client's content: addr: 0x%x, adapter: %p \n"
			"name: %s, driver:%p, client->adapter.name is %s\n", __func__, client->addr,
			client->adapter, client->name, client->driver,
			client->adapter->name);
#endif
		goto exit_kfree_pdata;
	} else {
#if DEBUG
		pr_info("L3GD20 DSevice detected!\n");
#endif
	}

	/* read chip id */
	tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I);
	printk("%s(), tempvalue is 0x%x\n", __func__, tempvalue);
	if ((tempvalue & 0x00FF) == GYROSCPOE_CHIP_ID) {
#if DEBUG
		pr_info("I2C driver registered!\n");
#endif
	} else {
		pr_err("%s(), Read Chip ID Error!\n", __func__);
		data->client = NULL;
		err = -ENODEV;
		goto exit_kfree_pdata;
	}

	data->misc_device.minor = MISC_DYNAMIC_MINOR;
	data->misc_device.name = GYROSCPOE_CHIP_NAME;
	data->misc_device.fops = &l3g4200d_fops;
	err = misc_register(&data->misc_device);
	if (err < 0) {
		pr_err("%s()->%d:can not create misc device!\n",
			__func__, __LINE__);
		goto exit_kfree_pdata;
	}

	/* add by jerrymo, create input device. */
	err = l3g4200d_create_input(data);
	if (err)
		goto exit_misc_deregister;

	INIT_DELAYED_WORK(&data->dwork, l3g4200d_work_func);
	data->delay = L3G4200D_DEFAULT_DELAY;
	l3g4200d_gyro_update_odr(data->delay);

#ifdef CONFIG_HAS_EARLYSUSPEND
	data->esuspend = gyro_esuspend;
	register_early_suspend(&data->esuspend);
#endif

#ifdef CONFIG_SENSORS_L3GD20_SELFTEST
	do {
		int test_result;

		pr_info("%s(), Initial self test\n", __func__);
		err = l3g4200d_selftest(&test_result);
		if (err < 0)
			pr_err("%s: fail\n", __func__);
		else
			pr_info("%s: self test\n", __func__);

		if (test_result == 1)
			pr_info("%s: PASS\n", __func__);
		else
			pr_info("%s: FAIL\n", __func__);
	} while (0);
#endif

#if DEBUG
	pr_info("L3GD20 device created successfully\n");
#endif

	return 0;

exit_misc_deregister:
	misc_deregister(&data->misc_device);
exit_kfree_pdata:
	kfree(data->pdata);
exit_kfree:
	kfree(data);
exit:
	gyro = NULL;
#if DEBUG
	pr_err("%s: Driver Initialization failed\n", __FILE__);
#endif
	return err;
}
static int l3g4200d_probe(struct i2c_client *client,
			       const struct i2c_device_id *devid)
{
	struct l3g4200d_data *data;
	struct gyro_platform_data *platform_data = NULL;
	int ret = -1;
	int tempvalue;
	GYRO_DBG("l3g4200d_probe start!\n");
	
	if (client->dev.platform_data == NULL) {
		dev_err(&client->dev, "platform data is NULL. exiting.\n");
		ret = -ENODEV;
		goto exit;
	}
    platform_data = client->dev.platform_data;
    if(platform_data->gyro_power)
    {
		ret = platform_data->gyro_power(IC_PM_ON);
		if( ret < 0)
	    {
	    	dev_err(&client->dev, "gyro power on error!\n");
	    	goto exit;
	    }
    }  
	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
		ret = -ENODEV;
		goto exit_pm_off;
	}

	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK)){
		ret = -ENODEV;
		goto exit_pm_off;
	}
	
	/*
	 * OK. For now, we presume we have a valid client. We now create the
	 * client structure, even though we cannot fill it completely yet.
	 */
	data = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL);

	if (NULL == data) {
		dev_err(&client->dev,
			"failed to allocate memory for module data\n");
		ret = -ENOMEM;
		goto exit_pm_off;
	}
	mutex_init(&data->mlock);

	INIT_WORK(&data->work, gy_work_func);
	i2c_set_clientdata(client, data);
	data->client = client;
	data->pdata = platform_data;
	
	ret = l3g4200d_validate_pdata(data);
	if (ret < 0) {
		dev_err(&client->dev, "failed to validate platform data\n");
		goto exit_kfree;
	}

    ret = i2c_smbus_read_byte(client);
	if ( ret < 0) {
		GYRO_DBG("i2c_smbus_read_byte error!!\n");
		goto err_detect_failed;
	} else {
		GYRO_DBG("L3G4200D Device detected!\n");
	}

	/* read chip id */
	tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I);
	if ((tempvalue & 0x00FF) == 0x00D3) {
		GYRO_DBG("I2C driver registered!\n");
	} else {
		data->client = NULL;
		ret = -ENODEV;
		goto err_detect_failed;
	}
	if (sensor_dev == NULL)
	{
		data->input_dev = input_allocate_device();
		if (data->input_dev == NULL) {
			ret = -ENOMEM;
			printk(KERN_ERR "gs_probe: Failed to allocate input device\n");
			goto err_input_dev_alloc_failed;
		}
       
		data->input_dev->name = "gy_sensors";
		sensor_dev = data->input_dev;

	}else{
		data->input_dev = sensor_dev;
	}
	data->input_dev->id.vendor = VENDOR;
	#if 0
	set_bit(EV_REL,data->input_dev->evbit);
	set_bit(REL_RX, data->input_dev->absbit);
	set_bit(REL_RY, data->input_dev->absbit);
	set_bit(REL_RZ, data->input_dev->absbit);
	#endif
	set_bit(EV_ABS,data->input_dev->evbit);
	/* modify the func of init */
	input_set_abs_params(data->input_dev, ABS_RX, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_RY, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_RZ, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_X, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_Y, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_Z, MIN_VALUE, MAX_VALUE, 0, 0);
	
	input_set_abs_params(data->input_dev, ABS_THROTTLE, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_RUDDER, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_WHEEL, MIN_VALUE, MAX_VALUE, 0, 0);
	
	input_set_abs_params(data->input_dev, ABS_GAS, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_HAT0X, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_HAT0Y, MIN_VALUE, MAX_VALUE, 0, 0);
	input_set_abs_params(data->input_dev, ABS_BRAKE, MIN_VALUE, MAX_VALUE, 0, 0);
	set_bit(EV_SYN,data->input_dev->evbit);
	data->input_dev->id.bustype = BUS_I2C;
	input_set_drvdata(data->input_dev, data);
	ret = input_register_device(data->input_dev);
	if (ret) {
		printk(KERN_ERR "gy_probe: Unable to register %s input device\n", data->input_dev->name);
	/* create l3g-dev device class */
		goto err_input_register_device_failed;
	}
	ret = misc_register(&gysensor_device);

	if (ret) {
		printk(KERN_ERR "gy_probe: gysensor_device register failed\n");
		goto err_misc_device_register_failed;
	}

	hrtimer_init(&data->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
	data->timer.function = gy_timer_func;
	atomic_set(&a_flag, 0);
	data->flags = -1;
#ifdef CONFIG_HAS_EARLYSUSPEND
	data->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
	data->early_suspend.suspend = gy_early_suspend;
	data->early_suspend.resume = gy_late_resume;
	register_early_suspend(&data->early_suspend);
#endif
	GYRO_DBG("L3G4200D device created successfully\n");
	gy_wq = create_singlethread_workqueue("gy_wq");
	if (!gy_wq)
		return -ENOMEM;

	gyro = data;
//	hrtimer_start(&this_gs_data->timer, ktime_set(0, 500000000), HRTIMER_MODE_REL);

    #ifdef CONFIG_HUAWEI_HW_DEV_DCT
    /* detect current device successful, set the flag as present */
    set_hw_dev_flag(DEV_I2C_GYROSCOPE);
    #endif
	printk(KERN_DEBUG "l3g4200d_probe   successful");

	return 0;
err_misc_device_register_failed:
	misc_deregister(&gysensor_device);
err_input_register_device_failed:
	input_free_device(gyro->input_dev);
err_input_dev_alloc_failed:
err_detect_failed:
exit_kfree:
	kfree(gyro);
exit_pm_off:
	if(platform_data->gyro_power)
	{
		platform_data->gyro_power(IC_PM_OFF);
	}
exit:
	return ret;
}
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;
}