static int optical_common_probe(struct iio_dev *indio_dev)
{
int err = 0;
struct optical_data *adata = iio_priv(indio_dev);
printk (KERN_ALERT "[%s]\n", __FUNCTION__);
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->info = &optical_info;
#ifdef OPTICAL_SENSOR_DEBUG
init_waitqueue_head(&proxWait);
init_waitqueue_head(&lightWait);
init_waitqueue_head(&hrWait);
#endif
err = optical_check_device_support(indio_dev,
ARRAY_SIZE(optical_sensors), optical_sensors);
if (err < 0)
{
goto st_press_common_probe_error;
}
adata->multiread_bit = adata->sensor->multi_read_bit;
indio_dev->channels = adata->sensor->ch;
indio_dev->num_channels = 2;
err = optical_init_sensor(indio_dev);
if (err < 0)
{
goto st_press_common_probe_error;
}
if (adata->get_irq_data_ready(indio_dev) > 0)
{
err = optical_allocate_ring(indio_dev);
if (err < 0)
{ printk("vunggv error optical_allocate_ring-------");
goto st_press_common_probe_error;
}
err = optical_allocate_trigger(indio_dev,&optical_trigger_ops);
if (err < 0)
{ printk("vunggv error optical_allocate_trigger------");
goto st_press_probe_trigger_error;
}
}
err = iio_device_register(indio_dev);
if (err)
{ printk("vunggv error iio_device_register------");
goto st_press_device_register_error;
}
err = iio_ring_buffer_register_ex(indio_dev->ring, 0,
indio_dev->channels,
indio_dev->num_channels);
if (err)
{ printk("vunggv error iio_ring_register------");
goto st_press_device_register_error;
}
printk (KERN_ALERT "[%s OK]\n", __FUNCTION__);
#ifdef OPTICAL_SENSOR_DEBUG
optical_set_enable(indio_dev, true);
#endif
return err;
st_press_device_register_error:
if (adata->get_irq_data_ready(indio_dev) > 0)
{
// st_sensors_deallocate_trigger(indio_dev); //cho nay VungGV
//Vung them
iio_trigger_unregister(adata->trig);
free_irq(adata->get_irq_data_ready(indio_dev), adata->trig);
iio_free_trigger(adata->trig);
}
st_press_probe_trigger_error:
if (adata->get_irq_data_ready(indio_dev) > 0)
{
optical_deallocate_ring(indio_dev);
}
st_press_common_probe_error:
return err;
}
static int __devinit ad7298_probe(struct spi_device *spi)
{
struct ad7298_platform_data *pdata = spi->dev.platform_data;
struct ad7298_state *st;
int ret, regdone = 0;
struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
if (indio_dev == NULL)
return -ENOMEM;
st = iio_priv(indio_dev);
st->reg = regulator_get(&spi->dev, "vcc");
if (!IS_ERR(st->reg)) {
ret = regulator_enable(st->reg);
if (ret)
goto error_put_reg;
}
spi_set_drvdata(spi, indio_dev);
st->spi = spi;
indio_dev->name = spi_get_device_id(spi)->name;
indio_dev->dev.parent = &spi->dev;
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->channels = ad7298_channels;
indio_dev->num_channels = ARRAY_SIZE(ad7298_channels);
indio_dev->info = &ad7298_info;
/* Setup default message */
st->scan_single_xfer[0].tx_buf = &st->tx_buf[0];
st->scan_single_xfer[0].len = 2;
st->scan_single_xfer[0].cs_change = 1;
st->scan_single_xfer[1].tx_buf = &st->tx_buf[1];
st->scan_single_xfer[1].len = 2;
st->scan_single_xfer[1].cs_change = 1;
st->scan_single_xfer[2].rx_buf = &st->rx_buf[0];
st->scan_single_xfer[2].len = 2;
spi_message_init(&st->scan_single_msg);
spi_message_add_tail(&st->scan_single_xfer[0], &st->scan_single_msg);
spi_message_add_tail(&st->scan_single_xfer[1], &st->scan_single_msg);
spi_message_add_tail(&st->scan_single_xfer[2], &st->scan_single_msg);
if (pdata && pdata->vref_mv) {
st->int_vref_mv = pdata->vref_mv;
st->ext_ref = AD7298_EXTREF;
} else {
st->int_vref_mv = AD7298_INTREF_mV;
}
ret = ad7298_register_ring_funcs_and_init(indio_dev);
if (ret)
goto error_disable_reg;
ret = iio_device_register(indio_dev);
if (ret)
goto error_disable_reg;
regdone = 1;
ret = iio_ring_buffer_register_ex(indio_dev->ring, 0,
&ad7298_channels[1], /* skip temp0 */
ARRAY_SIZE(ad7298_channels) - 1);
if (ret)
goto error_cleanup_ring;
return 0;
error_cleanup_ring:
ad7298_ring_cleanup(indio_dev);
error_disable_reg:
if (!IS_ERR(st->reg))
regulator_disable(st->reg);
error_put_reg:
if (!IS_ERR(st->reg))
regulator_put(st->reg);
if (regdone)
iio_device_unregister(indio_dev);
else
iio_free_device(indio_dev);
return ret;
}
