/**************************************************************************** * drivers/sensors/lis2dh.c * * Licensed to the Apache Software Foundation (ASF) under one or more * contributor license agreements. See the NOTICE file distributed with * this work for additional information regarding copyright ownership. The * ASF licenses this file to you under the Apache License, Version 2.0 (the * "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the * License for the specific language governing permissions and limitations * under the License. * ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #ifdef CONFIG_LIS2DH_DEBUG # define lis2dh_dbg(x, ...) _info(x, ##__VA_ARGS__) #else # define lis2dh_dbg(x, ...) sninfo(x, ##__VA_ARGS__) #endif #ifndef CONFIG_LIS2DH_I2C_FREQUENCY # define CONFIG_LIS2DH_I2C_FREQUENCY 400000 #endif #ifdef CONFIG_LIS2DH_DRIVER_SELFTEST # define LSB_AT_10BIT_RESOLUTION 4 # define LSB_AT_12BIT_RESOLUTION 1 # define SELFTEST_BUF_SIZE 5 # define SELFTEST_MAX_READ_ATTEMPTS 200 # define SELFTEST_ABS_DIFF_MIN_10BIT 17 # define SELFTEST_ABS_DIFF_MAX_10_BIT 360 # define SELFTEST_ABS_DIFF_MIN_12BIT (LSB_AT_10BIT_RESOLUTION * SELFTEST_ABS_DIFF_MIN_10BIT) # define SELFTEST_ABS_DIFF_MAX_12BIT (LSB_AT_10BIT_RESOLUTION * SELFTEST_ABS_DIFF_MAX_10_BIT) # define SELFTEST_0 0 # define SELFTEST_1 1 #endif /* Miscellaneous macros */ #define LIS2DH_I2C_RETRIES 10 #define LIS2DH_COUNT_INTS /**************************************************************************** * Private Data Types ****************************************************************************/ enum interrupts { LIS2DH_INT1 = 1, LIS2DH_INT2 = 2 }; struct lis2dh_dev_s { FAR struct i2c_master_s *i2c; /* I2C interface */ uint8_t addr; /* I2C address */ FAR struct lis2dh_config_s *config; /* Platform specific configuration */ struct lis2dh_setup *setup; /* User defined device operation mode setup */ struct lis2dh_vector_s vector_data; /* Latest read data read from lis2dh */ int scale; /* Full scale in milliG */ mutex_t devlock; /* Manages exclusive access to this structure */ bool fifo_used; /* LIS2DH configured to use FIFO */ bool fifo_stopped; /* FIFO got full and has stopped. */ #ifdef LIS2DH_COUNT_INTS volatile int16_t int_pending; /* Interrupt received but data not read, yet */ #else volatile bool int_pending; /* Interrupt received but data not read, yet */ #endif struct pollfd *fds[CONFIG_LIS2DH_NPOLLWAITERS]; }; /**************************************************************************** * Private Function prototypes ****************************************************************************/ static int lis2dh_open(FAR struct file *filep); static int lis2dh_close(FAR struct file *filep); static ssize_t lis2dh_read(FAR struct file *, FAR char *, size_t); static ssize_t lis2dh_write(FAR struct file *filep, FAR const char *buffer, size_t buflen); static int lis2dh_ioctl(FAR struct file *filep, int cmd, unsigned long arg); static int lis2dh_access(FAR struct lis2dh_dev_s *dev, uint8_t subaddr, FAR uint8_t *buf, int length); static int lis2dh_get_reading(FAR struct lis2dh_dev_s *dev, FAR struct lis2dh_vector_s *res, bool force_read); static int lis2dh_powerdown(FAR struct lis2dh_dev_s *dev); static int lis2dh_reboot(FAR struct lis2dh_dev_s *dev); static int lis2dh_poll(FAR struct file *filep, FAR struct pollfd *fds, bool setup); static int lis2dh_int_handler(int irq, FAR void *context, FAR void *arg); static int lis2dh_setup(FAR struct lis2dh_dev_s *dev, FAR struct lis2dh_setup *new_setup); static inline int16_t lis2dh_raw_to_mg(uint8_t raw_hibyte, uint8_t raw_lobyte, int scale); static int lis2dh_read_temp(FAR struct lis2dh_dev_s *dev, FAR int16_t *temper); static int lis2dh_clear_interrupts(FAR struct lis2dh_dev_s *priv, uint8_t interrupts); static unsigned int lis2dh_get_fifo_readings(FAR struct lis2dh_dev_s *priv, FAR struct lis2dh_result *res, unsigned int readcount, FAR int *perr); #ifdef CONFIG_LIS2DH_DRIVER_SELFTEST static int lis2dh_handle_selftest(FAR struct lis2dh_dev_s *priv); static int16_t lis2dh_raw_convert_to_12bit(uint8_t raw_hibyte, uint8_t raw_lobyte); static FAR const struct lis2dh_vector_s * lis2dh_get_raw_readings(FAR struct lis2dh_dev_s *dev, FAR int *err); #endif /**************************************************************************** * Private Data ****************************************************************************/ static const struct file_operations g_lis2dhops = { lis2dh_open, /* open */ lis2dh_close, /* close */ lis2dh_read, /* read */ lis2dh_write, /* write */ NULL, /* seek */ lis2dh_ioctl, /* ioctl */ NULL, /* mmap */ NULL, /* truncate */ lis2dh_poll /* poll */ }; /**************************************************************************** * Private Functions ****************************************************************************/ static int lis2dh_who_am_i(FAR struct lis2dh_dev_s *dev, uint8_t *id) { int ret; ret = lis2dh_access(dev, ST_LIS2DH_WHOAMI_REG, id, 1); if (ret < 0) { lis2dh_dbg("Cannot read who am i value\n"); return -EIO; } return ret; } /**************************************************************************** * Name: lis2dh_open * * Description: * This function is called whenever the LIS2DH device is opened. * ****************************************************************************/ static int lis2dh_open(FAR struct file *filep) { FAR struct inode *inode = filep->f_inode; FAR struct lis2dh_dev_s *priv = inode->i_private; uint8_t regval; int ret = OK; /* Probe device */ if (lis2dh_access(priv, ST_LIS2DH_WHOAMI_REG, ®val, 1) > 0) { /* Check chip identification, in the future several more compatible * parts may be added here. */ if (regval == ST_LIS2DH_WHOAMI_VALUE) { priv->config->irq_enable(priv->config, true); /* Normal exit point */ ret = lis2dh_clear_interrupts(priv, LIS2DH_INT1 | LIS2DH_INT2); return ret; } /* Otherwise, we mark an invalid device found at given address */ ret = -ENODEV; } else { /* No response at given address is marked as */ ret = -EFAULT; } /* Error exit */ return ret; } /**************************************************************************** * Name: lis2dh_close * * Description: * This routine is called when the LIS2DH device is closed. * ****************************************************************************/ static int lis2dh_close(FAR struct file *filep) { FAR struct inode *inode = filep->f_inode; FAR struct lis2dh_dev_s *priv = inode->i_private; priv->config->irq_enable(priv->config, false); return lis2dh_powerdown(priv); } /**************************************************************************** * Name: lis2dh_fifo_start * * Description: * This function restarts FIFO reading. * ****************************************************************************/ static int lis2dh_fifo_start(FAR struct lis2dh_dev_s *priv) { uint8_t buf; int ret = OK; buf = 0x00 | priv->setup->trigger_selection | priv->setup->fifo_trigger_threshold; if (lis2dh_access(priv, ST_LIS2DH_FIFO_CTRL_REG, &buf, -1) != 1) { lis2dh_dbg("lis2dh: Failed to write FIFO control register\n"); ret = -EIO; } else { buf = priv->setup->fifo_mode | priv->setup->trigger_selection | priv->setup->fifo_trigger_threshold; if (lis2dh_access(priv, ST_LIS2DH_FIFO_CTRL_REG, &buf, -1) != 1) { lis2dh_dbg("lis2dh: Failed to write FIFO control register\n"); ret = -EIO; } else { priv->fifo_stopped = false; lis2dh_dbg("lis2dh: FIFO restarted\n"); } } return ret; } /**************************************************************************** * Name: lis2dh_read * * Description: * This routine is called when the LIS2DH device is read. * ****************************************************************************/ static ssize_t lis2dh_read(FAR struct file *filep, FAR char *buffer, size_t buflen) { FAR struct inode *inode = filep->f_inode; FAR struct lis2dh_dev_s *priv = inode->i_private; FAR struct lis2dh_result *ptr; int readcount = (buflen - sizeof(struct lis2dh_res_header)) / sizeof(struct lis2dh_vector_s); uint8_t buf; uint8_t int1_src = 0; uint8_t int2_src = 0; irqstate_t flags; int ret; if (buflen < sizeof(struct lis2dh_result) || (buflen - sizeof(struct lis2dh_res_header)) % sizeof(struct lis2dh_vector_s) != 0) { lis2dh_dbg("lis2dh: Illegal amount of bytes to read: %d\n", buflen); return -EINVAL; } ret = nxmutex_lock(&dev->devlock); if (ret < 0) { return ret; } /* Do not allow read() if no SNIOC_WRITESETUP first. */ if (!priv->setup) { lis2dh_dbg("lis2dh: Read from unconfigured device\n"); return -EINVAL; } flags = enter_critical_section(); #ifdef LIS2DH_COUNT_INTS if (priv->int_pending > 0) { priv->int_pending--; } DEBUGASSERT(priv->int_pending >= 0 && priv->int_pending < 10); #else priv->int_pending = false; #endif leave_critical_section(flags); /* Set pointer to first measurement data */ ptr = (FAR struct lis2dh_result *)buffer; ptr->header.meas_count = 0; if (!priv->fifo_used) { /* FIFO not used, read only one sample. */ if (readcount > 0) { ret = lis2dh_get_reading(priv, &ptr->measurements[0], true); if (ret < 0) { lis2dh_dbg("lis2dh: Failed to read xyz\n"); } else { ptr->header.meas_count = 1; } } } else /* FIFO modes */ { uint8_t fifo_mode = priv->setup->fifo_mode & ST_LIS2DH_FIFOCR_MODE_MASK; bool fifo_empty = false; uint8_t fifo_num_samples; ptr->header.meas_count = 0; do { /* Check if FIFO needs to be restarted after being read empty. * We need to read SRC_REG before reading measurement, as reading * sample from FIFO clears OVRN_FIFO flag. */ if (lis2dh_access(priv, ST_LIS2DH_FIFO_SRC_REG, &buf, 1) != 1) { lis2dh_dbg("lis2dh: Failed to read FIFO source register\n"); return -EIO; } if (fifo_mode != LIS2DH_STREAM_MODE) { /* FIFO is full and has stopped. */ priv->fifo_stopped |= !!(buf & ST_LIS2DH_FIFOSR_OVRN_FIFO); } if (buf & ST_LIS2DH_FIFOSR_OVRN_FIFO) { lis2dh_dbg("lis2dh: FIFO overrun\n"); } if (buf & ST_LIS2DH_FIFOSR_EMPTY) { lis2dh_dbg("lis2dh: FIFO empty\n"); fifo_empty = true; if (fifo_mode != LIS2DH_STREAM_MODE) { priv->fifo_stopped = true; } /* FIFO is empty, skip reading. */ break; } /* How many samples available in FIFO? */ fifo_num_samples = (buf & ST_LIS2DH_FIFOSR_NUM_SAMP_MASK) + 1; if (fifo_num_samples > (readcount - ptr->header.meas_count)) { fifo_num_samples = (readcount - ptr->header.meas_count); } ptr->header.meas_count += lis2dh_get_fifo_readings(priv, ptr, fifo_num_samples, &ret); } while (!fifo_empty && ptr->header.meas_count < readcount); if (!fifo_empty && fifo_mode != LIS2DH_TRIGGER_MODE) { /* FIFO was not read empty, more data available. */ flags = enter_critical_section(); #ifdef LIS2DH_COUNT_INTS priv->int_pending++; #else priv->int_pending = true; #endif poll_notify(priv->fds, CONFIG_LIS2DH_NPOLLWAITERS, POLLIN); leave_critical_section(flags); } else if (fifo_mode != LIS2DH_STREAM_MODE && priv->fifo_stopped) { /* FIFO is empty and has stopped by overrun event. Reset FIFO for * further reading. */ ret = lis2dh_fifo_start(priv); } } /* Make sure interrupt will get cleared (by reading this register) in case * of latched configuration. */ buf = 0; if (lis2dh_access(priv, ST_LIS2DH_INT1_SRC_REG, &buf, 1) != 1) { lis2dh_dbg("lis2dh: Failed to read INT1_SRC_REG\n"); ret = -EIO; } if (buf & ST_LIS2DH_INT_SR_ACTIVE) { /* Interrupt has happened */ int1_src = buf; ptr->header.int1_occurred = true; } else { ptr->header.int1_occurred = false; } /* Make sure interrupt will get cleared (by reading this register) in case * of latched configuration. */ buf = 0; if (lis2dh_access(priv, ST_LIS2DH_INT2_SRC_REG, &buf, 1) != 1) { lis2dh_dbg("lis2dh: Failed to read INT2_SRC_REG\n"); ret = -EIO; } if (buf & ST_LIS2DH_INT_SR_ACTIVE) { /* Interrupt has happened */ int2_src = buf; ptr->header.int2_occurred = true; } else { ptr->header.int2_occurred = false; } ptr->header.int1_source = int1_src; ptr->header.int2_source = int2_src; nxmutex_unlock(&dev->devlock); /* 'ret' was just for debugging, we do return partial reads here. */ return sizeof(ptr->header) + ptr->header.meas_count * sizeof(struct lis2dh_vector_s); } /**************************************************************************** * Name: lis2dh_write * Description: * This routine is called when the LIS2DH device is written to. ****************************************************************************/ static ssize_t lis2dh_write(FAR struct file *filep, FAR const char *buffer, size_t buflen) { DEBUGASSERT(filep != NULL && buffer != NULL && buflen > 0); return -ENOSYS; } /**************************************************************************** * Name: lis2dh_ioctl * * Description: * This routine is called when ioctl function call * for the LIS2DH device is done. * ****************************************************************************/ static int lis2dh_ioctl(FAR struct file *filep, int cmd, unsigned long arg) { FAR struct inode *inode; FAR struct lis2dh_dev_s *priv; int ret; uint8_t buf; DEBUGASSERT(filep); inode = filep->f_inode; DEBUGASSERT(inode && inode->i_private); priv = (FAR struct lis2dh_dev_s *)inode->i_private; ret = nxmutex_lock(&dev->devlock); if (ret < 0) { return ret; } switch (cmd) { case SNIOC_WRITESETUP: { /* Write to the configuration registers. */ ret = lis2dh_setup(priv, (struct lis2dh_setup *)arg); lis2dh_dbg("lis2dh: conf: %p ret: %d\n", (struct lis2dh_setup *)arg, ret); /* Make sure interrupt will get cleared in * case of latched configuration. */ lis2dh_clear_interrupts(priv, LIS2DH_INT1 | LIS2DH_INT2); } break; case SNIOC_WRITE_INT1THRESHOLD: { buf = (uint8_t)arg; if (lis2dh_access(priv, ST_LIS2DH_INT1_THS_REG, &buf, -1) != 1) { lis2dh_dbg("lis2dh: Failed to write INT1_THS_REG\n"); ret = -EIO; } lis2dh_clear_interrupts(priv, LIS2DH_INT1); } break; case SNIOC_WRITE_INT2THRESHOLD: { buf = (uint8_t)arg; if (lis2dh_access(priv, ST_LIS2DH_INT2_THS_REG, &buf, -1) != 1) { lis2dh_dbg("lis2dh: Failed to write INT2_THS_REG\n"); ret = -EIO; } lis2dh_clear_interrupts(priv, LIS2DH_INT2); } break; case SNIOC_RESET_HPFILTER: { /* Read reference register to reset/recalib DC offset for HP filter */ if (lis2dh_access(priv, ST_LIS2DH_REFERENCE_REG, &buf, 1) != 1) { lis2dh_dbg("lis2dh: Failed to write reference register\n"); ret = -EIO; } lis2dh_clear_interrupts(priv, LIS2DH_INT2); } break; case SNIOC_START_SELFTEST: #ifdef CONFIG_LIS2DH_DRIVER_SELFTEST { priv->config->irq_enable(priv->config, false); lis2dh_clear_interrupts(priv, LIS2DH_INT1 | LIS2DH_INT2); ret = lis2dh_handle_selftest(priv); priv->config->irq_enable(priv->config, true); } #else { ret = -EINVAL; } #endif break; case SNIOC_READ_TEMP: { ret = lis2dh_read_temp(priv, (int16_t *)arg); } break; case SNIOC_WHO_AM_I: { ret = lis2dh_who_am_i(priv, (uint8_t *)arg); } break; default: { lis2dh_dbg("lis2dh: Unrecognized cmd: %d\n", cmd); ret = -ENOTTY; } break; } nxmutex_unlock(&dev->devlock); return ret; } /**************************************************************************** * Name: lis2dh_poll * * Description: * This routine is called during LIS2DH device poll * ****************************************************************************/ static int lis2dh_poll(FAR struct file *filep, FAR struct pollfd *fds, bool setup) { FAR struct inode *inode; FAR struct lis2dh_dev_s *priv; int ret; int i; DEBUGASSERT(filep && fds); inode = filep->f_inode; DEBUGASSERT(inode && inode->i_private); priv = (FAR struct lis2dh_dev_s *)inode->i_private; ret = nxmutex_lock(&dev->devlock); if (ret < 0) { return ret; } if (setup) { /* Ignore waits that do not include POLLIN */ if ((fds->events & POLLIN) == 0) { ret = -EDEADLK; goto out; } /* This is a request to set up the poll. Find an available * slot for the poll structure reference */ for (i = 0; i < CONFIG_LIS2DH_NPOLLWAITERS; i++) { /* Find an available slot */ if (!priv->fds[i]) { /* Bind the poll structure and this slot */ priv->fds[i] = fds; fds->priv = &priv->fds[i]; break; } } if (i >= CONFIG_LIS2DH_NPOLLWAITERS) { fds->priv = NULL; ret = -EBUSY; goto out; } if (priv->int_pending) { poll_notify(priv->fds, CONFIG_LIS2DH_NPOLLWAITERS, POLLIN); } } else if (fds->priv) { /* This is a request to tear down the poll. */ struct pollfd **slot = (struct pollfd **)fds->priv; DEBUGASSERT(slot != NULL); /* Remove all memory of the poll setup */ *slot = NULL; fds->priv = NULL; } out: nxmutex_unlock(&dev->devlock); return ret; } /**************************************************************************** * Name: lis2dh_callback * * Description: * lis2dh interrupt handler * ****************************************************************************/ static int lis2dh_int_handler(int irq, FAR void *context, FAR void *arg) { FAR struct lis2dh_dev_s *priv = (FAR struct lis2dh_dev_s *)arg; irqstate_t flags; DEBUGASSERT(priv != NULL); flags = enter_critical_section(); #ifdef LIS2DH_COUNT_INTS priv->int_pending++; #else priv->int_pending = true; #endif poll_notify(priv->fds, CONFIG_LIS2DH_NPOLLWAITERS, POLLIN); leave_critical_section(flags); return OK; } #ifdef CONFIG_LIS2DH_DRIVER_SELFTEST /**************************************************************************** * Name: lis2dh_clear_registers * * Description: * Clear lis2dh registers * * Input Parameters: * priv - pointer to LIS2DH Private Structure * * Returned Value: * Returns OK in case of success, otherwise ERROR * ****************************************************************************/ static int lis2dh_clear_registers(FAR struct lis2dh_dev_s *priv) { uint8_t buf = 0; uint8_t i; DEBUGASSERT(priv); for (i = ST_LIS2DH_TEMP_CFG_REG; i <= ST_LIS2DH_ACT_DUR_REG; i++) { /* Skip read only registers */ if ((i <= 0x1e) || (i >= 0x27 && i <= 0x2d) || (i == 0x2f) || (i == 0x31)) { continue; } if (lis2dh_access(priv, i, &buf, -1) != 1) { lis2dh_dbg("lis2dh: Failed to clear register 0x%02x\n", i); return ERROR; } } return OK; } /**************************************************************************** * Name: lis2dh_write_register * * Description: * Clear lis2dh registers * * Input Parameters: * priv - pointer to LIS2DH Private Structure * reg - target register * value - value to write * * Returned Value: * Returns OK in case of success, otherwise ERROR * ****************************************************************************/ static int lis2dh_write_register(FAR struct lis2dh_dev_s *priv, uint8_t reg, uint8_t value) { DEBUGASSERT(priv); if (lis2dh_access(priv, reg, &value, -1) != 1) { lis2dh_dbg("lis2dh: Failed to write %d to register 0x%02x\n", value, reg); return ERROR; } return OK; } /**************************************************************************** * Name: lis2dh_read_register * * Description: * read lis2dh register * * Input Parameters: * priv - pointer to LIS2DH Private Structure * reg - register to read * * Returned Value: * Returns positive register value in case of success, * otherwise ERROR ( < 0) ****************************************************************************/ static int lis2dh_read_register(FAR struct lis2dh_dev_s *priv, uint8_t reg) { uint8_t buf; DEBUGASSERT(priv); if (lis2dh_access(priv, reg, &buf, sizeof(buf)) == sizeof(buf)) { return buf; } return ERROR; } /**************************************************************************** * Name: lis2dh_handle_selftest * * Description: * Handle selftest. Note, that after running selftest lis2dh is left in * shutdown mode without valid setup. Therefore SNIOC_WRITESETUP must be * sent again to proceed with normal operations. * ****************************************************************************/ static int lis2dh_handle_selftest(FAR struct lis2dh_dev_s *priv) { const struct lis2dh_vector_s *results; uint8_t i; uint8_t j; uint8_t buf; int16_t avg_x_no_st = 0; int16_t avg_y_no_st = 0; int16_t avg_z_no_st = 0; int16_t avg_x_with_st = 0; int16_t avg_y_with_st = 0; int16_t avg_z_with_st = 0; int16_t abs_st_x_value; int16_t abs_st_y_value; int16_t abs_st_z_value; int ret = OK; int err = OK; DEBUGASSERT(priv); lis2dh_powerdown(priv); if (lis2dh_clear_registers(priv) != OK) { ret = -EIO; goto out; } /* Set the control register (23h) to ±2g FS, normal mode with BDU (Block * Data Update) and HR (High Resolution) bits enabled. */ if (lis2dh_write_register(priv, ST_LIS2DH_CTRL_REG4, 0x88) != OK) { lis2dh_dbg("lis2dh: Failed to write CTRL4 REG for selftest\n"); ret = -EIO; goto out; } /* Set the control register (20h) to 50Hz ODR (Output Data Rate) with * X/Y/Z axis enabled. */ if (lis2dh_write_register(priv, ST_LIS2DH_CTRL_REG1, 0x47) != OK) { lis2dh_dbg("lis2dh: Failed to write CTRL1 REG for selftest\n"); ret = -EIO; goto out; } /* Dummy reads so that values have stabilized */ for (i = 0; i < 20; i++) { if (lis2dh_get_raw_readings(priv, &err) == NULL) { ret = -EIO; goto out; } } for (i = 0; i < SELFTEST_BUF_SIZE; i++) { results = lis2dh_get_raw_readings(priv, &err); if (results == NULL) { ret = -EIO; goto out; } avg_x_no_st += results->x; avg_y_no_st += results->y; avg_z_no_st += results->z; } avg_x_no_st = avg_x_no_st / SELFTEST_BUF_SIZE; avg_y_no_st = avg_y_no_st / SELFTEST_BUF_SIZE; avg_z_no_st = avg_z_no_st / SELFTEST_BUF_SIZE; for (i = SELFTEST_0; i <= SELFTEST_1; i++) { avg_x_with_st = 0; avg_y_with_st = 0; avg_z_with_st = 0; /* Enable self-test 0 or 1 at +/-2g FS with BDU and HR bits enabled. */ buf = (i == SELFTEST_0) ? 0x8a : 0x8c; if (lis2dh_write_register(priv, ST_LIS2DH_CTRL_REG4, buf) != OK) { lis2dh_dbg("lis2dh: Failed to write CTRL4 REG for selftest\n"); ret = -EIO; goto out; } /* Dummy reads so that values have stabilized */ for (i = 0; i < 10; i++) { if (lis2dh_get_raw_readings(priv, &err) == NULL) { ret = -EIO; goto out; } } for (j = 0; j < SELFTEST_BUF_SIZE; j++) { results = lis2dh_get_raw_readings(priv, &err); if (results == NULL) { ret = -EIO; goto out; } avg_x_with_st += results->x; avg_y_with_st += results->y; avg_z_with_st += results->z; } avg_x_with_st = avg_x_with_st / SELFTEST_BUF_SIZE; avg_y_with_st = avg_y_with_st / SELFTEST_BUF_SIZE; avg_z_with_st = avg_z_with_st / SELFTEST_BUF_SIZE; abs_st_x_value = abs(avg_x_with_st - avg_x_no_st); abs_st_y_value = abs(avg_y_with_st - avg_y_no_st); abs_st_z_value = abs(avg_z_with_st - avg_z_no_st); syslog(LOG_NOTICE, "ST %d, ABSX: %d, ABSY: %d, ABSZ: %d\n", i, abs_st_x_value, abs_st_y_value, abs_st_z_value); if (abs_st_x_value < SELFTEST_ABS_DIFF_MIN_12BIT || abs_st_x_value > SELFTEST_ABS_DIFF_MAX_12BIT || abs_st_y_value < SELFTEST_ABS_DIFF_MIN_12BIT || abs_st_y_value > SELFTEST_ABS_DIFF_MAX_12BIT || abs_st_z_value < SELFTEST_ABS_DIFF_MIN_12BIT || abs_st_z_value > SELFTEST_ABS_DIFF_MAX_12BIT) { syslog(LOG_NOTICE, "Selftest %d fail! Limits (%d <= value <= %d). " "Results: x: %d, y: %d, z: %d ", i, SELFTEST_ABS_DIFF_MIN_12BIT, SELFTEST_ABS_DIFF_MAX_12BIT, abs_st_x_value, abs_st_y_value, abs_st_z_value); ret = -ERANGE; goto out; } } /* Verify INT1 and INT2 lines */ if (lis2dh_clear_registers(priv) != OK) { ret = -EIO; goto out; } /* Both INT lines should be low */ if (priv->config->read_int1_pin != NULL) { if (priv->config->read_int1_pin() != 0) { syslog(LOG_NOTICE, "INT1 line is HIGH - expected LOW\n"); ret = -ENXIO; goto out; } } if (priv->config->read_int2_pin) { if (priv->config->read_int2_pin() != 0) { syslog(LOG_NOTICE, "INT2 line is HIGH - expected LOW\n"); ret = -ENODEV; goto out; } } /* 400Hz ODR all axes enabled * FIFO overrun & DATA READY on INT1 * FIFO enabled and INT1 & INT2 latched * FIFO mode, INT1 , THS 0 * OR combination, all events enabled */ if ((lis2dh_write_register(priv, ST_LIS2DH_CTRL_REG1, 0x77) != OK) || (lis2dh_write_register(priv, ST_LIS2DH_CTRL_REG3, 0x12) != OK) || (lis2dh_write_register(priv, ST_LIS2DH_CTRL_REG5, 0x4a) != OK) || (lis2dh_write_register(priv, ST_LIS2DH_FIFO_CTRL_REG, 0x40) != OK) || (lis2dh_write_register(priv, ST_LIS2DH_INT1_CFG_REG, 0x3f) != OK)) { syslog(LOG_NOTICE, "Writing registers for INT line check failed\n"); ret = -EIO; goto out; } /* Clear INT1 & INT2 */ if ((lis2dh_read_register(priv, ST_LIS2DH_INT1_SRC_REG) == ERROR) || (lis2dh_read_register(priv, ST_LIS2DH_INT2_SRC_REG) == ERROR)) { syslog(LOG_NOTICE, "Failed to clear INT1 / INT2 registers\n"); ret = -EIO; goto out; } nxsig_usleep(20000); /* Now INT1 should have been latched high and INT2 should be still low */ if (priv->config->read_int1_pin) { if (priv->config->read_int1_pin() != 1) { syslog(LOG_NOTICE, "INT1 line is LOW - expected HIGH\n"); ret = -ENXIO; goto out; } } if (priv->config->read_int2_pin != NULL) { if (priv->config->read_int2_pin() != 0) { syslog(LOG_NOTICE, "INT2 line is HIGH - expected LOW\n"); ret = -ENODEV; goto out; } /* Enable interrupt 1 on INT2 pin */ if (lis2dh_write_register(priv, ST_LIS2DH_CTRL_REG6, 0x40) != OK) { syslog(LOG_NOTICE, "Failed to enable interrupt 1 on INT2 pin"); ret = -EIO; goto out; } nxsig_usleep(20000); if (priv->config->read_int2_pin() != 1) { syslog(LOG_NOTICE, "INT2 line is LOW - expected HIGH\n"); ret = -ENODEV; goto out; } } out: lis2dh_clear_registers(priv); lis2dh_powerdown(priv); return ret; } /**************************************************************************** * Name: lis2dh_raw_to_mg * * Description: * Convert raw acceleration value to mg * * Input Parameters: * raw_hibyte - Hi byte of raw data * raw_lobyte - Lo byte of raw data * * Returned Value: * Returns acceleration value in mg ****************************************************************************/ static int16_t lis2dh_raw_convert_to_12bit(uint8_t raw_hibyte, uint8_t raw_lobyte) { int16_t value; value = (raw_hibyte << 8) | raw_lobyte; value = value >> 4; value &= 0xfff; if (value & 0x800) { value = ~value; value &= 0xfff; value += 1; value = -value; } return value; } /**************************************************************************** * Name: lis2dh_data_available * * Description: * Check if new data is available to read * * Input Parameters: * dev - pointer to LIS2DH Private Structure * * Returned Value: * Return true if new data is available. Otherwise returns false * ****************************************************************************/ static bool lis2dh_data_available(FAR struct lis2dh_dev_s *dev) { uint8_t retval; DEBUGASSERT(dev); if (lis2dh_access(dev, ST_LIS2DH_STATUS_REG, &retval, sizeof(retval)) == sizeof(retval)) { return ((retval & ST_LIS2DH_SR_ZYXDA) != 0); } return false; } /**************************************************************************** * Name: lis2dh_get_raw_readings * * Description: * Read X, Y, Z - acceleration values from chip * * Input Parameters: * dev - pointer to LIS2DH Private Structure * * Returned Value: * Returns acceleration vectors (High resolution = 12bit values) on * success, NULL otherwise. * ****************************************************************************/ static FAR const struct lis2dh_vector_s * lis2dh_get_raw_readings(FAR struct lis2dh_dev_s *dev, int *err) { uint8_t retval[6]; uint8_t retries_left = SELFTEST_MAX_READ_ATTEMPTS; DEBUGASSERT(dev); *err = 0; while (--retries_left > 0) { nxsig_usleep(20000); if (lis2dh_data_available(dev)) { if (lis2dh_access(dev, ST_LIS2DH_OUT_X_L_REG, retval, sizeof(retval)) == sizeof(retval)) { dev->vector_data.x = lis2dh_raw_convert_to_12bit(retval[1], retval[0]); dev->vector_data.y = lis2dh_raw_convert_to_12bit(retval[3], retval[2]); dev->vector_data.z = lis2dh_raw_convert_to_12bit(retval[5], retval[4]); return &dev->vector_data; } return NULL; } } return NULL; } #endif /* CONFIG_LIS2DH_DRIVER_SELFTEST */ /**************************************************************************** * Name: lis2dh_clear_interrupts * * Description: * Clear interrupts from LIS2DH chip * ****************************************************************************/ static int lis2dh_clear_interrupts(FAR struct lis2dh_dev_s *priv, uint8_t interrupts) { uint8_t buf; int ret = OK; if (interrupts & LIS2DH_INT1) { /* Make sure interrupt will get cleared (by reading this register) in * case of latched configuration. */ if (lis2dh_access(priv, ST_LIS2DH_INT1_SRC_REG, &buf, 1) != 1) { lis2dh_dbg("lis2dh: Failed to read INT1_SRC_REG\n"); ret = -EIO; } } if (interrupts & LIS2DH_INT2) { /* Make sure interrupt will get cleared (by reading this register) in * case of latched configuration. */ if (lis2dh_access(priv, ST_LIS2DH_INT2_SRC_REG, &buf, 1) != 1) { lis2dh_dbg("lis2dh: Failed to read INT2_SRC_REG\n"); ret = -EIO; } } return ret; } /**************************************************************************** * Name: lis2dh_get_reading * * Description: * Read X, Y, Z - acceleration value from chip * * Input Parameters: * dev - pointer to LIS2DH Private Structure * force_read - Read even if new data is not available (old data) * * Returned Value: * Returns OK if success, negative error code otherwise * ****************************************************************************/ static int lis2dh_get_reading(FAR struct lis2dh_dev_s *dev, FAR struct lis2dh_vector_s *res, bool force_read) { int scale = dev->scale; uint8_t retval[7]; int16_t x; int16_t y; int16_t z; if (lis2dh_access(dev, ST_LIS2DH_STATUS_REG, retval, 7) == 7) { /* If result is not yet ready, return NULL */ if (!force_read && !(retval[0] & ST_LIS2DH_SR_ZYXDA)) { lis2dh_dbg("lis2dh: Results were not ready\n"); return -EAGAIN; } /* Add something to entropy pool. */ add_sensor_randomness((((uint32_t)retval[6] << 25) | ((uint32_t)retval[6] >> 7)) ^ ((uint32_t)retval[5] << 20) ^ ((uint32_t)retval[4] << 15) ^ ((uint32_t)retval[3] << 10) ^ ((uint32_t)retval[2] << 5) ^ ((uint32_t)retval[1] << 0)); x = lis2dh_raw_to_mg(retval[2], retval[1], scale); y = lis2dh_raw_to_mg(retval[4], retval[3], scale); z = lis2dh_raw_to_mg(retval[6], retval[5], scale); if (dev->setup->xy_axis_fixup) { res->x = y; res->y = -x; } else { res->x = x; res->y = y; } res->z = z; return OK; } return -EIO; } /**************************************************************************** * Name: lis2dh_get_fifo_readings * * Description: * Bulk read from FIFO * ****************************************************************************/ static unsigned int lis2dh_get_fifo_readings(FAR struct lis2dh_dev_s *priv, FAR struct lis2dh_result *res, unsigned int readcount, FAR int *perr) { int scale = priv->scale; union { uint8_t raw[6]; struct lis2dh_vector_s sample; } *buf = (void *)&res->measurements[res->header.meas_count]; bool xy_axis_fixup = priv->setup->xy_axis_fixup; size_t buflen = readcount * 6; int16_t x; int16_t y; int16_t z; unsigned int i; if (readcount == 0) { return 0; } if (lis2dh_access(priv, ST_LIS2DH_OUT_X_L_REG, (void *)buf, buflen) != buflen) { lis2dh_dbg("lis2dh: Failed to read FIFO (%d bytes, %d samples)\n", buflen, readcount); *perr = -EIO; return 0; } /* Add something to entropy pool. */ up_rngaddentropy(RND_SRC_SENSOR, (void *)buf, buflen / 4); /* Convert raw values to mG */ for (i = 0; i < readcount; i++) { x = lis2dh_raw_to_mg(buf[i].raw[1], buf[i].raw[0], scale); y = lis2dh_raw_to_mg(buf[i].raw[3], buf[i].raw[2], scale); z = lis2dh_raw_to_mg(buf[i].raw[5], buf[i].raw[4], scale); if (xy_axis_fixup) { buf[i].sample.x = y; buf[i].sample.y = -x; } else { buf[i].sample.x = x; buf[i].sample.y = y; } buf[i].sample.z = z; } return readcount; } /**************************************************************************** * Name: lis2dh_raw_to_mg * * Description: * Convert raw acceleration value to mg * * Input Parameters: * raw_hibyte - Hi byte of raw data * raw_lobyte - Lo byte of raw data * scale - full scale in milliG * * Returned Value: * Returns acceleration value in mg * ****************************************************************************/ static inline int16_t lis2dh_raw_to_mg(uint8_t raw_hibyte, uint8_t raw_lobyte, int scale) { int16_t value; /* Value is signed integer, range INT16_MIN..INT16_MAX. */ value = (raw_hibyte << 8) | raw_lobyte; /* Scale to mg, INT16_MIN..INT16_MAX => -scale..scale */ return (int32_t)value * scale / INT16_MAX; } /**************************************************************************** * Name: lis2dh_read_temp * * Description: * * Input Parameters: * * Returned Value: * ****************************************************************************/ static int lis2dh_read_temp(FAR struct lis2dh_dev_s *dev, FAR int16_t *temper) { int ret; uint8_t buf[2] = { 0 }; ret = lis2dh_access(dev, ST_LIS2DH_OUT_TEMP_L_REG, buf, 2); if (ret < 0) { lis2dh_dbg("Cannot read temperature\n"); return -EIO; } *temper = buf[0] | ((int16_t)buf[1] << 8); return OK; } /**************************************************************************** * LIS2DH Access with range check * * Description: * Read or write data via I2C * * Input Parameters: * dev LIS2DH Private Structure * subaddr LIS2DH Sub Address * buf Pointer to buffer, either for read or write access * length When >0 it denotes read access, when <0 it denotes write access * of -length * * Returned Value: * Returns actual length of data on success or negated errno. * ****************************************************************************/ static int lis2dh_access(FAR struct lis2dh_dev_s *dev, uint8_t subaddr, FAR uint8_t *buf, int length) { uint16_t flags = 0; int retval; int retries; DEBUGASSERT(dev != NULL && buf != NULL && length != 0); if (length > 0) { flags = I2C_M_READ; } else { flags = I2C_M_NOSTART; length = -length; } /* Check valid address ranges and set auto address increment flag */ if (subaddr == ST_LIS2DH_STATUS_AUX_REG) { if (length > 1) { length = 1; } } else if (subaddr >= ST_LIS2DH_OUT_TEMP_L_REG && subaddr < 0x10) { if (length > (0x10 - subaddr)) { length = 0x10 - subaddr; } } else if (subaddr >= ST_LIS2DH_TEMP_CFG_REG && subaddr <= ST_LIS2DH_ACT_DUR_REG) { if (subaddr == ST_LIS2DH_OUT_X_L_REG) { /* FIFO bulk read, length maximum 6*32 = 192 bytes. */ if (length > 6 * 32) { length = 6 * 32; } } else { if (length > (ST_LIS2DH_ACT_DUR_REG + 1 - subaddr)) { length = ST_LIS2DH_ACT_DUR_REG + 1 - subaddr; } } } else { return -EFAULT; } if (length > 1) { subaddr |= 0x80; } for (retries = 0; retries < LIS2DH_I2C_RETRIES; retries++) { /* Create message and send */ struct i2c_msg_s msgv[2] = { { .frequency = CONFIG_LIS2DH_I2C_FREQUENCY, .addr = dev->addr, .flags = 0, .buffer = &subaddr, .length = 1 }, { .frequency = CONFIG_LIS2DH_I2C_FREQUENCY, .addr = dev->addr, .flags = flags, .buffer = buf, .length = length } }; retval = I2C_TRANSFER(dev->i2c, msgv, 2); if (retval >= 0) { return length; } else { /* Some error. Try to reset I2C bus and keep trying. */ #ifdef CONFIG_I2C_RESET int ret = I2C_RESET(dev->i2c); if (ret < 0) { lis2dh_dbg("I2C_RESET failed: %d\n", ret); return ret; } #endif continue; } } lis2dh_dbg("failed, error: %d\n", retval); return retval; } /**************************************************************************** * Name: lis2dh_reboot * * Description: * * Input Parameters: * * Returned Value: * ****************************************************************************/ static int lis2dh_reboot(FAR struct lis2dh_dev_s *dev) { struct timespec start; struct timespec curr; int32_t diff_msec; uint8_t value; /* Prefer monotonic for timeout calculation when enabled. */ clock_systime_timespec(&start); /* Reboot to reset chip. */ value = ST_LIS2DH_CR5_BOOT; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG5, &value, -1) != 1) { return -EIO; } /* Reboot is completed when reboot bit is cleared. */ do { value = 0; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG5, &value, 1) != 1) { return -EIO; } if (!(value & ST_LIS2DH_CR5_BOOT)) { break; } clock_systime_timespec(&curr); diff_msec = (curr.tv_sec - start.tv_sec) * 1000; diff_msec += (curr.tv_nsec - start.tv_nsec) / (1000 * 1000); if (diff_msec > 100) { return -ETIMEDOUT; } nxsig_usleep(1); } while (true); /* Reboot completed, chip is now in power-down state. */ return OK; } /**************************************************************************** * Name: lis2dh_powerdown * * Description: * * Input Parameters: * * Returned Value: * ****************************************************************************/ static int lis2dh_powerdown(FAR struct lis2dh_dev_s * dev) { uint8_t buf = 0; int ret = OK; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG1, &buf, -1) != 1) { lis2dh_dbg("Failed to clear CTRL_REG1\n"); ret = -EIO; } return ret; } /**************************************************************************** * LIS2DH Setup * * Description: * Apply new register setup * * Input Parameters: * dev - pointer to LIS2DH Private Structure * new_setup - pointer to new setup data to be configured * * Returned Value: * Returns OK on success, ERROR otherwise. * ****************************************************************************/ static int lis2dh_setup(FAR struct lis2dh_dev_s * dev, FAR struct lis2dh_setup *new_setup) { uint8_t value; dev->setup = new_setup; /* Clear old configuration. On first boot after power-loss, reboot bit does * not get cleared, and lis2dh_reboot() times out. Anyway, chip accepts * new configuration and functions correctly. */ lis2dh_reboot(dev); /* TEMP_CFG_REG */ value = dev->setup->temp_enable ? (0x3 << 6): 0; if (lis2dh_access(dev, ST_LIS2DH_TEMP_CFG_REG, &value, -1) != 1) { goto error; } /* CTRL_REG2 */ value = dev->setup->hpmode | dev->setup->hpcf | dev->setup->fds | dev->setup->hpclick | dev->setup->hpis2 | dev->setup->hpis1; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG2, &value, -1) != 1) { goto error; } /* CTRL_REG3 */ value = dev->setup->int1_click_enable | dev->setup->int1_aoi_enable | dev->setup->int2_aoi_enable | dev->setup->int1_drdy_enable | dev->setup->int2_drdy_enable | dev->setup->int_wtm_enable | dev->setup->int_overrun_enable; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG3, &value, -1) != 1) { goto error; } /* CTRL_REG4 */ value = dev->setup->bdu | dev->setup->endian | dev->setup->fullscale | dev->setup->high_resolution_enable | dev->setup->selftest | dev->setup->spi_mode; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG4, &value, -1) != 1) { goto error; } /* CTRL_REG5 */ value = dev->setup->reboot | dev->setup->fifo_enable | dev->setup->int1_latch | dev->setup->int1_4d_enable | dev->setup->int2_latch | dev->setup->int2_4d_enable; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG5, &value, -1) != 1) { goto error; } /* CTRL_REG6 */ value = dev->setup->int2_click_enable | dev->setup->int_enable | dev->setup->boot_int1_enable | dev->setup->high_low_active; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG6, &value, -1) != 1) { goto error; } /* REFERENCE */ value = dev->setup->reference; if (lis2dh_access(dev, ST_LIS2DH_REFERENCE_REG, &value, -1) != 1) { goto error; } /* FIFO_CTRL_REG */ value = dev->setup->fifo_mode | dev->setup->trigger_selection | dev->setup->fifo_trigger_threshold; if (lis2dh_access(dev, ST_LIS2DH_FIFO_CTRL_REG, &value, -1) != 1) { goto error; } /* INT1_CFG */ value = dev->setup->int1_interrupt_mode | dev->setup->int1_enable_6d | dev->setup->int1_int_z_high_enable | dev->setup->int1_int_z_low_enable | dev->setup->int1_int_y_high_enable | dev->setup->int1_int_y_low_enable | dev->setup->int1_int_x_high_enable | dev->setup->int1_int_x_low_enable; if (lis2dh_access(dev, ST_LIS2DH_INT1_CFG_REG, &value, -1) != 1) { goto error; } /* INT1_THS */ value = dev->setup->int1_int_threshold; if (lis2dh_access(dev, ST_LIS2DH_INT1_THS_REG, &value, -1) != 1) { goto error; } /* INT1_DURATION */ value = dev->setup->int1_int_duration; if (lis2dh_access(dev, ST_LIS2DH_INT1_DUR_REG, &value, -1) != 1) { goto error; } /* INT2_CFG */ value = dev->setup->int2_interrupt_mode | dev->setup->int2_enable_6d | dev->setup->int2_int_z_high_enable | dev->setup->int2_int_z_low_enable | dev->setup->int2_int_y_high_enable | dev->setup->int2_int_y_low_enable | dev->setup->int2_int_x_high_enable | dev->setup->int2_int_x_low_enable; if (lis2dh_access(dev, ST_LIS2DH_INT2_CFG_REG, &value, -1) != 1) { goto error; } /* INT2_THS */ value = dev->setup->int2_int_threshold; if (lis2dh_access(dev, ST_LIS2DH_INT2_THS_REG, &value, -1) != 1) { goto error; } /* INT2_DURATION */ value = dev->setup->int2_int_duration; if (lis2dh_access(dev, ST_LIS2DH_INT2_DUR_REG, &value, -1) != 1) { goto error; } /* CLICK_CFG */ value = dev->setup->z_double_click_enable | dev->setup->z_single_click_enable | dev->setup->y_double_click_enable | dev->setup->y_single_click_enable | dev->setup->x_double_click_enable | dev->setup->x_single_click_enable; if (lis2dh_access(dev, ST_LIS2DH_CLICK_CFG_REG, &value, -1) != 1) { goto error; } /* CLICK_THS */ value = dev->setup->click_threshold; if (lis2dh_access(dev, ST_LIS2DH_CLICK_THS_REG, &value, -1) != 1) { goto error; } /* TIME_LIMIT */ value = dev->setup->click_time_limit; if (lis2dh_access(dev, ST_LIS2DH_TIME_LIMIT_REG, &value, -1) != 1) { goto error; } /* TIME_LATENCY */ value = dev->setup->click_time_latency; if (lis2dh_access(dev, ST_LIS2DH_TIME_LATENCY_REG, &value, -1) != 1) { goto error; } /* TIME_WINDOW */ value = dev->setup->click_time_window; if (lis2dh_access(dev, ST_LIS2DH_TIME_WINDOW_REG, &value, -1) != 1) { goto error; } /* CTRL_REG1 */ value = dev->setup->data_rate | dev->setup->low_power_mode_enable | dev->setup->zen | dev->setup->yen | dev->setup->xen; if (lis2dh_access(dev, ST_LIS2DH_CTRL_REG1, &value, -1) != 1) { goto error; } switch (dev->setup->fullscale & 0x30) { default: case ST_LIS2DH_CR4_FULL_SCALE_2G: dev->scale = 2000; break; case ST_LIS2DH_CR4_FULL_SCALE_4G: dev->scale = 4000; break; case ST_LIS2DH_CR4_FULL_SCALE_8G: dev->scale = 8000; break; case ST_LIS2DH_CR4_FULL_SCALE_16G: dev->scale = 16000; break; } if (dev->setup->fifo_enable) { dev->fifo_used = true; if (lis2dh_fifo_start(dev) < 0) { goto error; } } else { dev->fifo_used = false; } return OK; error: /* Setup failed - power down */ lis2dh_powerdown(dev); return -EIO; } /**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: lis2dh_register * * Description: * Register the LIS2DH character device as 'devpath' * * Input Parameters: * devpath - The full path to the driver to register. E.g., "/dev/acc0" * i2c - An instance of the I2C interface to use to communicate with * LIS2DH * addr - The I2C address of the LIS2DH. The base I2C address of the * LIS2DH is 0x18. Bit 0 can be controlled via SA0 pad - when * connected to voltage supply the address is 0x19. * config - Pointer to LIS2DH configuration * * Returned Value: * Zero (OK) on success; a negated errno value on failure. * ****************************************************************************/ int lis2dh_register(FAR const char *devpath, FAR struct i2c_master_s *i2c, uint8_t addr, FAR struct lis2dh_config_s *config) { FAR struct lis2dh_dev_s *priv; int ret; DEBUGASSERT(devpath != NULL && i2c != NULL && config != NULL); priv = (FAR struct lis2dh_dev_s *)kmm_zalloc(sizeof(struct lis2dh_dev_s)); if (!priv) { lis2dh_dbg("lis2dh: Failed to allocate instance\n"); return -ENOMEM; } nxmutex_init(&priv->devlock); priv->fifo_used = false; #ifdef LIS2DH_COUNT_INTS priv->int_pending = 0; #else priv->int_pending = false; #endif priv->i2c = i2c; priv->addr = addr; priv->config = config; ret = register_driver(devpath, &g_lis2dhops, 0666, priv); if (ret < 0) { lis2dh_dbg("lis2dh: Failed to register driver: %d\n", ret); goto errout_with_priv; } if (priv->config->irq_clear) { priv->config->irq_clear(config); } priv->config->irq_attach(config, lis2dh_int_handler, priv); priv->config->irq_enable(config, false); return OK; errout_with_priv: nxmutex_destroy(&priv->devlock); kmm_free(priv); return ret; }