int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr) { int err; struct st_sensor_odr_avl odr_out = {0, 0}; struct st_sensor_data *sdata = iio_priv(indio_dev); err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out); if (err < 0) goto st_sensors_match_odr_error; if ((sdata->sensor_settings->odr.addr == sdata->sensor_settings->pw.addr) && (sdata->sensor_settings->odr.mask == sdata->sensor_settings->pw.mask)) { if (sdata->enabled == true) { err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->odr.addr, sdata->sensor_settings->odr.mask, odr_out.value); } else { err = 0; } } else { err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->odr.addr, sdata->sensor_settings->odr.mask, odr_out.value); } if (err >= 0) sdata->odr = odr_out.hz; st_sensors_match_odr_error: return err; }
int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) { int err; u8 drdy_mask; struct st_sensor_data *sdata = iio_priv(indio_dev); if (!sdata->sensor->drdy_irq.addr) return 0; /* Enable/Disable the interrupt generator 1. */ if (sdata->sensor->drdy_irq.ig1.en_addr > 0) { err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor->drdy_irq.ig1.en_addr, sdata->sensor->drdy_irq.ig1.en_mask, (int)enable); if (err < 0) goto st_accel_set_dataready_irq_error; } if (sdata->drdy_int_pin == 1) drdy_mask = sdata->sensor->drdy_irq.mask_int1; else drdy_mask = sdata->sensor->drdy_irq.mask_int2; /* Enable/Disable the interrupt generator for data ready. */ err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable); st_accel_set_dataready_irq_error: return err; }
int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) { int err; u8 drdy_mask; struct st_sensor_data *sdata = iio_priv(indio_dev); if (!sdata->sensor_settings->drdy_irq.addr) return 0; /* Enable/Disable the interrupt generator 1. */ if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) { err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->drdy_irq.ig1.en_addr, sdata->sensor_settings->drdy_irq.ig1.en_mask, (int)enable); if (err < 0) goto st_accel_set_dataready_irq_error; } if (sdata->drdy_int_pin == 1) drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1; else drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2; /* Flag to the poll function that the hardware trigger is in use */ sdata->hw_irq_trigger = enable; /* Enable/Disable the interrupt generator for data ready. */ err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->drdy_irq.addr, drdy_mask, (int)enable); st_accel_set_dataready_irq_error: return err; }
static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs) { int err, i = 0; struct st_sensor_data *sdata = iio_priv(indio_dev); if (sdata->sensor_settings->fs.addr == 0) return 0; err = st_sensors_match_fs(sdata->sensor_settings, fs, &i); if (err < 0) goto st_accel_set_fullscale_error; err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->fs.addr, sdata->sensor_settings->fs.mask, sdata->sensor_settings->fs.fs_avl[i].value); if (err < 0) goto st_accel_set_fullscale_error; sdata->current_fullscale = (struct st_sensor_fullscale_avl *) &sdata->sensor_settings->fs.fs_avl[i]; return err; st_accel_set_fullscale_error: dev_err(&indio_dev->dev, "failed to set new fullscale.\n"); return err; }
int st_sensors_init_sensor(struct iio_dev *indio_dev) { int err; struct st_sensor_data *sdata = iio_priv(indio_dev); mutex_init(&sdata->tb.buf_lock); err = st_sensors_set_enable(indio_dev, false); if (err < 0) goto init_error; err = st_sensors_set_fullscale(indio_dev, sdata->current_fullscale->num); if (err < 0) goto init_error; err = st_sensors_set_odr(indio_dev, sdata->odr); if (err < 0) goto init_error; /* set BDU */ err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true); if (err < 0) goto init_error; err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); init_error: return err; }
int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable) { struct st_sensor_data *sdata = iio_priv(indio_dev); return st_sensors_write_data_with_mask(indio_dev, sdata->sensor->enable_axis.addr, sdata->sensor->enable_axis.mask, axis_enable); }
int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable) { u8 tmp_value; int err = -EINVAL; bool found = false; struct st_sensor_odr_avl odr_out = {0, 0}; struct st_sensor_data *sdata = iio_priv(indio_dev); if (enable) { tmp_value = sdata->sensor_settings->pw.value_on; if ((sdata->sensor_settings->odr.addr == sdata->sensor_settings->pw.addr) && (sdata->sensor_settings->odr.mask == sdata->sensor_settings->pw.mask)) { err = st_sensors_match_odr(sdata->sensor_settings, sdata->odr, &odr_out); if (err < 0) goto set_enable_error; tmp_value = odr_out.value; found = true; } err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->pw.addr, sdata->sensor_settings->pw.mask, tmp_value); if (err < 0) goto set_enable_error; sdata->enabled = true; if (found) sdata->odr = odr_out.hz; } else { err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->pw.addr, sdata->sensor_settings->pw.mask, sdata->sensor_settings->pw.value_off); if (err < 0) goto set_enable_error; sdata->enabled = false; } set_enable_error: return err; }
int st_sensors_init_sensor(struct iio_dev *indio_dev, struct st_sensors_platform_data *pdata) { struct st_sensor_data *sdata = iio_priv(indio_dev); struct st_sensors_platform_data *of_pdata; int err = 0; /* If OF/DT pdata exists, it will take precedence of anything else */ of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata); if (of_pdata) pdata = of_pdata; if (pdata) { err = st_sensors_set_drdy_int_pin(indio_dev, pdata); if (err < 0) return err; } err = st_sensors_set_enable(indio_dev, false); if (err < 0) return err; if (sdata->current_fullscale) { err = st_sensors_set_fullscale(indio_dev, sdata->current_fullscale->num); if (err < 0) return err; } else dev_info(&indio_dev->dev, "Full-scale not possible\n"); err = st_sensors_set_odr(indio_dev, sdata->odr); if (err < 0) return err; /* set BDU */ err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->bdu.addr, sdata->sensor_settings->bdu.mask, true); if (err < 0) return err; err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); return err; }
int st_sensors_allocate_trigger(struct iio_dev *indio_dev, const struct iio_trigger_ops *trigger_ops) { int err, irq; struct st_sensor_data *sdata = iio_priv(indio_dev); unsigned long irq_trig; sdata->trig = iio_trigger_alloc("%s-trigger", indio_dev->name); if (sdata->trig == NULL) { dev_err(&indio_dev->dev, "failed to allocate iio trigger.\n"); return -ENOMEM; } irq = sdata->get_irq_data_ready(indio_dev); irq_trig = irqd_get_trigger_type(irq_get_irq_data(irq)); /* * If the IRQ is triggered on falling edge, we need to mark the * interrupt as active low, if the hardware supports this. */ if (irq_trig == IRQF_TRIGGER_FALLING) { if (!sdata->sensor_settings->drdy_irq.addr_ihl) { dev_err(&indio_dev->dev, "falling edge specified for IRQ but hardware " "only support rising edge, will request " "rising edge\n"); irq_trig = IRQF_TRIGGER_RISING; } else { /* Set up INT active low i.e. falling edge */ err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->drdy_irq.addr_ihl, sdata->sensor_settings->drdy_irq.mask_ihl, 1); if (err < 0) goto iio_trigger_free; dev_info(&indio_dev->dev, "interrupts on the falling edge\n"); } } else if (irq_trig == IRQF_TRIGGER_RISING) { dev_info(&indio_dev->dev, "interrupts on the rising edge\n"); } else { dev_err(&indio_dev->dev, "unsupported IRQ trigger specified (%lx), only " "rising and falling edges supported, enforce " "rising edge\n", irq_trig); irq_trig = IRQF_TRIGGER_RISING; } /* * If the interrupt pin is Open Drain, by definition this * means that the interrupt line may be shared with other * peripherals. But to do this we also need to have a status * register and mask to figure out if this sensor was firing * the IRQ or not, so we can tell the interrupt handle that * it was "our" interrupt. */ if (sdata->int_pin_open_drain && sdata->sensor_settings->drdy_irq.addr_stat_drdy) irq_trig |= IRQF_SHARED; err = request_threaded_irq(irq, iio_trigger_generic_data_rdy_poll, NULL, irq_trig, sdata->trig->name, sdata->trig); if (err) { dev_err(&indio_dev->dev, "failed to request trigger IRQ.\n"); goto iio_trigger_free; } iio_trigger_set_drvdata(sdata->trig, indio_dev); sdata->trig->ops = trigger_ops; sdata->trig->dev.parent = sdata->dev; err = iio_trigger_register(sdata->trig); if (err < 0) { dev_err(&indio_dev->dev, "failed to register iio trigger.\n"); goto iio_trigger_register_error; } indio_dev->trig = iio_trigger_get(sdata->trig); return 0; iio_trigger_register_error: free_irq(sdata->get_irq_data_ready(indio_dev), sdata->trig); iio_trigger_free: iio_trigger_free(sdata->trig); return err; }
int st_sensors_init_sensor(struct iio_dev *indio_dev, struct st_sensors_platform_data *pdata) { struct st_sensor_data *sdata = iio_priv(indio_dev); struct st_sensors_platform_data *of_pdata; int err = 0; /* If OF/DT pdata exists, it will take precedence of anything else */ of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata); if (of_pdata) pdata = of_pdata; if (pdata) { err = st_sensors_set_drdy_int_pin(indio_dev, pdata); if (err < 0) return err; } err = st_sensors_set_enable(indio_dev, false); if (err < 0) return err; /* Disable DRDY, this might be still be enabled after reboot. */ err = st_sensors_set_dataready_irq(indio_dev, false); if (err < 0) return err; if (sdata->current_fullscale) { err = st_sensors_set_fullscale(indio_dev, sdata->current_fullscale->num); if (err < 0) return err; } else dev_info(&indio_dev->dev, "Full-scale not possible\n"); err = st_sensors_set_odr(indio_dev, sdata->odr); if (err < 0) return err; /* set BDU */ if (sdata->sensor_settings->bdu.addr) { err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->bdu.addr, sdata->sensor_settings->bdu.mask, true); if (err < 0) return err; } if (sdata->int_pin_open_drain) { dev_info(&indio_dev->dev, "set interrupt line to open drain mode\n"); err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->drdy_irq.addr_od, sdata->sensor_settings->drdy_irq.mask_od, 1); if (err < 0) return err; } err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); return err; }
int st_sensors_allocate_trigger(struct iio_dev *indio_dev, const struct iio_trigger_ops *trigger_ops) { int err, irq; struct st_sensor_data *sdata = iio_priv(indio_dev); unsigned long irq_trig; sdata->trig = iio_trigger_alloc("%s-trigger", indio_dev->name); if (sdata->trig == NULL) { dev_err(&indio_dev->dev, "failed to allocate iio trigger.\n"); return -ENOMEM; } irq = sdata->get_irq_data_ready(indio_dev); irq_trig = irqd_get_trigger_type(irq_get_irq_data(irq)); /* * If the IRQ is triggered on falling edge, we need to mark the * interrupt as active low, if the hardware supports this. */ if (irq_trig == IRQF_TRIGGER_FALLING) { if (!sdata->sensor_settings->drdy_irq.addr_ihl) { dev_err(&indio_dev->dev, "falling edge specified for IRQ but hardware " "only support rising edge, will request " "rising edge\n"); irq_trig = IRQF_TRIGGER_RISING; } else { /* Set up INT active low i.e. falling edge */ err = st_sensors_write_data_with_mask(indio_dev, sdata->sensor_settings->drdy_irq.addr_ihl, sdata->sensor_settings->drdy_irq.mask_ihl, 1); if (err < 0) goto iio_trigger_free; dev_info(&indio_dev->dev, "interrupts on the falling edge\n"); } } else if (irq_trig == IRQF_TRIGGER_RISING) { dev_info(&indio_dev->dev, "interrupts on the rising edge\n"); } else { dev_err(&indio_dev->dev, "unsupported IRQ trigger specified (%lx), only " "rising and falling edges supported, enforce " "rising edge\n", irq_trig); irq_trig = IRQF_TRIGGER_RISING; } err = request_threaded_irq(irq, iio_trigger_generic_data_rdy_poll, NULL, irq_trig, sdata->trig->name, sdata->trig); if (err) { dev_err(&indio_dev->dev, "failed to request trigger IRQ.\n"); goto iio_trigger_free; } iio_trigger_set_drvdata(sdata->trig, indio_dev); sdata->trig->ops = trigger_ops; sdata->trig->dev.parent = sdata->dev; err = iio_trigger_register(sdata->trig); if (err < 0) { dev_err(&indio_dev->dev, "failed to register iio trigger.\n"); goto iio_trigger_register_error; } indio_dev->trig = iio_trigger_get(sdata->trig); return 0; iio_trigger_register_error: free_irq(sdata->get_irq_data_ready(indio_dev), sdata->trig); iio_trigger_free: iio_trigger_free(sdata->trig); return err; }