static int gs_probe(
struct i2c_client *client, const struct i2c_device_id *id)
{
int ret;
struct gs_data *gs;
struct gs_platform_data *pdata;
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
printk(KERN_ERR "gs_probe: need I2C_FUNC_I2C\n");
ret = -ENODEV;
goto err_check_functionality_failed;
}
pdata = client->dev.platform_data;
if (pdata){
if(pdata->adapt_fn != NULL){
ret = pdata->adapt_fn();
if(ret > 0){
client->addr = pdata->slave_addr;//actual address
printk(KERN_INFO "%s:change i2c addr to actrual address = %d\n", __FUNCTION__, pdata->slave_addr);
if(client->addr == 0){
printk(KERN_ERR "%s: bad i2c address = %d\n", __FUNCTION__, client->addr);
ret = -EFAULT;
goto err_check_functionality_failed;
}
}
}
if(pdata->init_flag != NULL){
if(*(pdata->init_flag)){
printk(KERN_ERR "gs_st probe failed, because the othe gsensor has been probed.\n");
ret = -ENODEV;
goto err_check_functionality_failed;
}
}
}
#ifndef GS_POLLING
ret = gs_config_int_pin();
if(ret <0)
{
goto err_check_functionality_failed;
}
#endif
gs = kzalloc(sizeof(*gs), GFP_KERNEL);
if (gs == NULL) {
ret = -ENOMEM;
goto err_alloc_data_failed;
}
mutex_init(&gs->mlock);
INIT_WORK(&gs->work, gs_work_func);
gs->client = client;
i2c_set_clientdata(client, gs);
#if 0
ret = i2c_smbus_write_byte_data(gs->client, 0x20, 0x47); /* device command = ctrl_reg1 */
if (ret < 0) {
printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
/* fail? */
goto err_detect_failed;
}
#endif
ret = reg_write(gs, GS_ST_REG_CTRL2, 0x00); /* device command = ctrl_reg2 */
if (ret < 0) {
printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
/* fail? */
goto err_detect_failed;
}
#if 0
ret = i2c_smbus_write_byte_data(gs->client, 0x22, 0x04); /* device command = ctrl_reg3 */
if (ret < 0) {
printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
/* fail? */
goto err_detect_failed;
}
#endif
if (sensor_dev == NULL)
{
gs->input_dev = input_allocate_device();
if (gs->input_dev == NULL) {
ret = -ENOMEM;
printk(KERN_ERR "gs_probe: Failed to allocate input device\n");
goto err_input_dev_alloc_failed;
}
gs->input_dev->name = "sensors";
sensor_dev = gs->input_dev;
}else{
gs->input_dev = sensor_dev;
}
gs->input_dev->id.vendor = GS_ST35DE;//for akm8973 compass detect.
set_bit(EV_ABS,gs->input_dev->evbit);
set_bit(ABS_X, gs->input_dev->absbit);
set_bit(ABS_Y, gs->input_dev->absbit);
set_bit(ABS_Z, gs->input_dev->absbit);
set_bit(EV_SYN,gs->input_dev->evbit);
gs->input_dev->id.bustype = BUS_I2C;
//gs->input_dev->open = gs_st_input_open;
//gs->input_dev->close = gs_st_input_close;
input_set_drvdata(gs->input_dev, gs);
ret = input_register_device(gs->input_dev);
if (ret) {
printk(KERN_ERR "gs_probe: Unable to register %s input device\n", gs->input_dev->name);
goto err_input_register_device_failed;
}
ret = misc_register(&gsensor_device);
if (ret) {
printk(KERN_ERR "gs_probe: gsensor_device register failed\n");
goto err_misc_device_register_failed;
}
#ifndef GS_POLLING
if (client->irq) {
ret = request_irq(client->irq, gs_irq_handler, 0, client->name, gs);
if (ret == 0)
gs->use_irq = 1;
else
dev_err(&client->dev, "request_irq failed\n");
}
#endif
if (!gs->use_irq) {
hrtimer_init(&gs->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
gs->timer.function = gs_timer_func;
}
#ifdef CONFIG_HAS_EARLYSUSPEND
gs->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
gs->early_suspend.suspend = gs_early_suspend;
gs->early_suspend.resume = gs_late_resume;
register_early_suspend(&gs->early_suspend);
#endif
gs_wq = create_singlethread_workqueue("gs_wq");
if (!gs_wq)
return -ENOMEM;
this_gs_data =gs;
printk(KERN_INFO "gs_probe: Start LIS35DE in %s mode\n", gs->use_irq ? "interrupt" : "polling");
if(pdata && pdata->init_flag)
*(pdata->init_flag) = 1;
return 0;
err_misc_device_register_failed:
misc_deregister(&gsensor_device);
err_input_register_device_failed:
input_free_device(gs->input_dev);
err_input_dev_alloc_failed:
err_detect_failed:
kfree(gs);
err_alloc_data_failed:
#ifndef GS_POLLING
gs_free_int();
#endif
err_check_functionality_failed:
return ret;
}
static int gs_probe(
struct i2c_client *client, const struct i2c_device_id *id)
{
int ret = -1;
struct gs_data *gs;
printk("my gs_probe_lis3xh\n");
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
printk(KERN_ERR "gs_probe: need I2C_FUNC_I2C\n");
ret = -ENODEV;
goto err_check_functionality_failed;
}
#ifndef GS_POLLING
ret = gs_config_int_pin();
if(ret <0)
{
goto err_check_functionality_failed;
}
#endif
gs = kzalloc(sizeof(*gs), GFP_KERNEL);
if (gs == NULL) {
ret = -ENOMEM;
goto err_alloc_data_failed;
}
mutex_init(&gs->mlock);
INIT_WORK(&gs->work, gs_work_func);
gs->client = client;
i2c_set_clientdata(client, gs);
printk("mode is %x\n", reg_read(gs, 0x20));
gs->sub_type = reg_read(gs, 0x0f);
printk("sub_type = %d\n", gs->sub_type);
printk("gs is %s\n", (gs->sub_type==0x33)? "ST LIS3DH":"ST LIS331DLH");
ret = reg_write(gs, GS_ST_REG_CTRL2, 0x00); /* device command = ctrl_reg2 */
if (ret < 0) {
printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
/* fail? */
goto err_detect_failed;
}
if (sensor_dev == NULL)
{
gs->input_dev = input_allocate_device();
if (gs->input_dev == NULL) {
ret = -ENOMEM;
printk(KERN_ERR "gs_probe: Failed to allocate input device\n");
goto err_input_dev_alloc_failed;
}
gs->input_dev->name = "sensors";
sensor_dev = gs->input_dev;
}else{
gs->input_dev = sensor_dev;
}
gs->input_dev->id.vendor = GS_STLIS3XH;
set_bit(EV_ABS,gs->input_dev->evbit);
set_bit(ABS_X, gs->input_dev->absbit);
set_bit(ABS_Y, gs->input_dev->absbit);
set_bit(ABS_Z, gs->input_dev->absbit);
set_bit(EV_SYN,gs->input_dev->evbit);
gs->input_dev->id.bustype = BUS_I2C;
input_set_drvdata(gs->input_dev, gs);
ret = input_register_device(gs->input_dev);
if (ret) {
printk(KERN_ERR "gs_probe: Unable to register %s input device\n", gs->input_dev->name);
goto err_input_register_device_failed;
}
ret = misc_register(&gsensor_device);
if (ret) {
printk(KERN_ERR "gs_probe: gsensor_device register failed\n");
goto err_misc_device_register_failed;
}
#ifndef GS_POLLING
if (client->irq) {
ret = request_irq(client->irq, gs_irq_handler, 0, client->name, gs);
if (ret == 0)
gs->use_irq = 1;
else
dev_err(&client->dev, "request_irq failed\n");
}
#endif
if (!gs->use_irq) {
hrtimer_init(&gs->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
gs->timer.function = gs_timer_func;
}
#ifdef CONFIG_HAS_EARLYSUSPEND
gs->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
gs->early_suspend.suspend = gs_early_suspend;
gs->early_suspend.resume = gs_late_resume;
register_early_suspend(&gs->early_suspend);
#endif
gs_wq = create_singlethread_workqueue("gs_wq");
if (!gs_wq)
{
ret = -ENOMEM;
printk("gs_probe: create_singlethread_workqueue error!");
goto err_request_irq;
}
this_gs_data =gs;
#ifdef CONFIG_HUAWEI_HW_DEV_DCT
/* detect current device successful, set the flag as present */
set_hw_dev_flag(DEV_I2C_G_SENSOR);
#endif
printk(KERN_INFO "gs_probe: Start GS LIS3XH in %s mode\n", gs->use_irq ? "interrupt" : "polling");
return 0;
err_request_irq:
#ifndef GS_POLLING
if (client->irq && gs->use_irq)
{
free_irq(client->irq, gs);
}
#endif
err_misc_device_register_failed:
misc_deregister(&gsensor_device);
err_input_register_device_failed:
input_free_device(gs->input_dev);
err_input_dev_alloc_failed:
err_detect_failed:
kfree(gs);
err_alloc_data_failed:
#ifndef GS_POLLING
gs_free_int();
#endif
err_check_functionality_failed:
return ret;
}
