// SPDX-License-Identifier: GPL-2.0+ /* * STMicroelectronics st_lsm6dsr embedded function sensor driver * * Copyright 2019 STMicroelectronics Inc. * * Lorenzo Bianconi */ #include #include #include #include "st_lsm6dsr.h" #define ST_LSM6DSR_REG_PAGE_SEL_ADDR 0x02 #define ST_LSM6DSR_REG_PAGE_SEL_RST_MASK BIT(0) #define ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR 0x04 #define ST_LSM6DSR_REG_PAGE_ADDRESS 0x08 #define ST_LSM6DSR_REG_PAGE_VALUE 0x09 #define ST_LSM6DSR_FSM_BASE_ADDRESS 0x2da #define ST_LSM6DSR_REG_PEDO_EN_MASK BIT(3) #define ST_LSM6DSR_REG_TILT_EN_MASK BIT(4) #define ST_LSM6DSR_REG_SIGN_MOTION_EN_MASK BIT(5) #define ST_LSM6DSR_REG_INT_DTAP_MASK BIT(3) #define ST_LSM6DSR_REG_INT_STAP_MASK BIT(6) #define ST_LSM6DSR_REG_EMB_FUNC_EN_B_ADDR 0x05 #define ST_LSM6DSR_REG_FSM_EN_MASK BIT(0) #define ST_LSM6DSR_REG_INT_STEP_DET_MASK BIT(3) #define ST_LSM6DSR_REG_INT_TILT_MASK BIT(4) #define ST_LSM6DSR_REG_INT_SIGMOT_MASK BIT(5) #define ST_LSM6DSR_PAGE_RW_ADDR 0x17 #define ST_LSM6DSR_REG_WR_MASK GENMASK(6, 5) #define ST_LSM6DSR_REG_EMB_FUNC_LIR_MASK BIT(7) #define ST_LSM6DSR_REG_EMB_FUNC_FIFO_CFG_ADDR 0x44 #define ST_LSM6DSR_REG_PEDO_FIFO_EN_MASK BIT(6) #define ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR 0x46 #define ST_LSM6DSR_REG_FSM_OUTS6_ADDR 0x51 #define ST_LSM6DSR_REG_ORIENTATION_0_MASK BIT(5) #define ST_LSM6DSR_REG_ORIENTATION_90_MASK BIT(7) #define ST_LSM6DSR_REG_ORIENTATION_180_MASK BIT(4) #define ST_LSM6DSR_REG_ORIENTATION_270_MASK BIT(6) /* Finite State Machine ODR configuration */ #define ST_LSM6DSR_REG_EMB_FUNC_ODR_CFG_B_ADDR 0x5f #define ST_LSM6DSR_REG_FSM_ODR_MASK GENMASK(5, 3) #define ST_LSM6DSR_FSM_ODR_12_5 0 #define ST_LSM6DSR_FSM_ODR_26 1 #define ST_LSM6DSR_FSM_ODR_52 2 #define ST_LSM6DSR_FSM_ODR_104 3 #define ST_LSM6DSR_FSM_ODR_208 4 #define ST_LSM6DSR_FSM_ODR_416 5 #define ST_LSM6DSR_REG_STEP_COUNTER_L_ADDR 0x62 #define ST_LSM6DSR_REG_EMB_FUNC_SRC_ADDR 0x64 #define ST_LSM6DSR_REG_PEDO_RST_STEP_MASK BIT(7) #define ST_LSM6DSR_FSM_MAX_SIZE 255 /** * @struct st_lsm6dsr_fsm_sensor * @brief Single FSM description entry * * Implements #595543 Feature * * The following FSM state machine LSM6DSR features listed in EX_FUN_FSM_SENSOR: * * SENSOR_TYPE_GLANCE_GESTURE * SENSOR_TYPE_MOTION_DETECT * SENSOR_TYPE_STATIONARY_DETECT * SENSOR_TYPE_WAKE_GESTURE * SENSOR_TYPE_PICK_UP_GESTURE * SENSOR_TYPE_WRIST_TILT_GESTURE * * will be managed as event sensors * * data: FSM binary data block. * id: Sensor Identifier. * FSM binary data block len. */ struct st_lsm6dsr_fsm_sensor { u8 data[ST_LSM6DSR_FSM_MAX_SIZE]; enum st_lsm6dsr_sensor_id id; u16 len; }; static const struct st_lsm6dsr_fsm_sensor st_lsm6dsr_fsm_sensor_list[] = { /* glance */ { .id = ST_LSM6DSR_ID_GLANCE, .data = { 0xb2, 0x10, 0x24, 0x20, 0x17, 0x17, 0x66, 0x32, 0x66, 0x3c, 0x20, 0x20, 0x02, 0x02, 0x08, 0x08, 0x00, 0x04, 0x0c, 0x00, 0xc7, 0x66, 0x33, 0x73, 0x77, 0x64, 0x88, 0x75, 0x99, 0x66, 0x33, 0x53, 0x44, 0xf5, 0x22, 0x00, }, .len = 36, }, /* motion */ { .id = ST_LSM6DSR_ID_MOTION, .data = { 0x51, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3c, 0x02, 0x00, 0x00, 0x7d, 0x00, 0xc7, 0x05, 0x99, 0x33, 0x53, 0x44, 0xf5, 0x22, 0x00, }, .len = 22, }, /* no motion */ { .id = ST_LSM6DSR_ID_NO_MOTION, .data = { 0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x66, 0x3c, 0x02, 0x00, 0x00, 0x7d, 0xff, 0x53, 0x99, 0x50, }, .len = 16, }, /* wakeup */ { .id = ST_LSM6DSR_ID_WAKEUP, .data = { 0xe2, 0x00, 0x1e, 0x20, 0x13, 0x15, 0x66, 0x3e, 0x66, 0xbe, 0xcd, 0x3c, 0xc0, 0xc0, 0x02, 0x02, 0x0b, 0x10, 0x05, 0x66, 0xcc, 0x35, 0x38, 0x35, 0x77, 0xdd, 0x03, 0x54, 0x22, 0x00, }, .len = 30, }, /* pickup */ { .id = ST_LSM6DSR_ID_PICKUP, .data = { 0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x33, 0x3c, 0x02, 0x00, 0x00, 0x05, 0x05, 0x99, 0x30, 0x00, }, .len = 16, }, /* orientation */ { .id = ST_LSM6DSR_ID_ORIENTATION, .data = { 0x91, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3a, 0x66, 0x32, 0xf0, 0x00, 0x00, 0x0d, 0x00, 0xc7, 0x05, 0x73, 0x99, 0x08, 0xf5, 0x22, }, .len = 22, }, /* wrist tilt */ { .id = ST_LSM6DSR_ID_WRIST_TILT, .data = { 0x52, 0x00, 0x14, 0x00, 0x00, 0x00, 0xae, 0xb7, 0x80, 0x00, 0x00, 0x06, 0x0f, 0x05, 0x73, 0x33, 0x07, 0x54, 0x44, 0x22, }, .len = 20, }, }; struct st_lsm6dsr_fsm_fs { u32 gain; __le16 val; }; static const struct st_lsm6dsr_fsm_fs st_lsm6dsr_fsm_fs_table[] = { { ST_LSM6DSR_ACC_FS_2G_GAIN, 0x03ff }, { ST_LSM6DSR_ACC_FS_4G_GAIN, 0x07fe }, { ST_LSM6DSR_ACC_FS_8G_GAIN, 0x0bfe }, { ST_LSM6DSR_ACC_FS_16G_GAIN, 0x0ffe }, }; static inline int st_lsm6dsr_fsm_set_access(struct st_lsm6dsr_hw *hw, bool enable) { u8 val = enable ? 2 : 0; return __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_PAGE_RW_ADDR, ST_LSM6DSR_REG_WR_MASK, val); } static int st_lsm6dsr_fsm_write(struct st_lsm6dsr_hw *hw, u16 base_addr, int len, const u8 *data) { u8 msb, lsb; int i, err; msb = (((base_addr >> 8) & 0xf) << 4) | 1; lsb = base_addr & 0xff; err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_PAGE_ADDRESS, sizeof(lsb), &lsb); if (err < 0) return err; err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_PAGE_SEL_ADDR, sizeof(msb), &msb); if (err < 0) return err; for (i = 0; i < len; i++) { err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_PAGE_VALUE, sizeof(u8), &data[i]); if (err < 0) return err; if (++lsb == 0) { msb += (1 << 4); err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_PAGE_SEL_ADDR, sizeof(msb), &msb); if (err < 0) return err; } } return err; } static int st_lsm6dsr_ef_pg1_sensor_set_enable(struct st_lsm6dsr_sensor *sensor, u8 mask, u8 irq_mask, bool enable) { struct st_lsm6dsr_hw *hw = sensor->hw; int err; err = st_lsm6dsr_sensor_set_enable(sensor, enable); if (err < 0) return err; mutex_lock(&hw->page_lock); err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, true); if (err < 0) goto unlock; err = __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR, mask, enable); if (err < 0) goto reset_page; err = __st_lsm6dsr_write_with_mask(hw, hw->embfunc_irq_reg, irq_mask, enable); reset_page: st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false); unlock: mutex_unlock(&hw->page_lock); return err; } /** * FSM Function sensor [FSM_FUN] * * @param sensor: ST IMU sensor instance * @param enable: Enable/Disable sensor * @return < 0 if error, 0 otherwise */ static int st_lsm6dsr_fsm_set_enable(struct st_lsm6dsr_sensor *sensor, bool enable) { struct st_lsm6dsr_hw *hw = sensor->hw; u16 enable_mask = hw->fsm_enable_mask; int err, i; for (i = 0; i < ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list); i++) if (st_lsm6dsr_fsm_sensor_list[i].id == sensor->id) break; if (i == ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list)) return -EINVAL; err = st_lsm6dsr_sensor_set_enable(sensor, enable); if (err < 0) return err; mutex_lock(&hw->page_lock); err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, true); if (err < 0) goto unlock; if (enable) enable_mask |= BIT(i); else enable_mask &= ~BIT(i); err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR, sizeof(enable_mask), (u8 *)&enable_mask); if (err < 0) goto reset_page; hw->fsm_enable_mask = enable_mask; reset_page: st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false); unlock: mutex_unlock(&hw->page_lock); return err; } /** * Enable Embedded Function sensor [EMB_FUN] * * @param sensor: ST IMU sensor instance * @param enable: Enable/Disable sensor * @return < 0 if error, 0 otherwise */ int st_lsm6dsr_embfunc_sensor_set_enable(struct st_lsm6dsr_sensor *sensor, bool enable) { int err; switch (sensor->id) { case ST_LSM6DSR_ID_STEP_DETECTOR: err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor, ST_LSM6DSR_REG_PEDO_EN_MASK, ST_LSM6DSR_REG_INT_STEP_DET_MASK, enable); break; case ST_LSM6DSR_ID_SIGN_MOTION: err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor, ST_LSM6DSR_REG_SIGN_MOTION_EN_MASK, ST_LSM6DSR_REG_INT_SIGMOT_MASK, enable); break; case ST_LSM6DSR_ID_TILT: err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor, ST_LSM6DSR_REG_TILT_EN_MASK, ST_LSM6DSR_REG_TILT_EN_MASK, enable); break; case ST_LSM6DSR_ID_NO_MOTION: case ST_LSM6DSR_ID_MOTION: case ST_LSM6DSR_ID_WAKEUP: case ST_LSM6DSR_ID_PICKUP: case ST_LSM6DSR_ID_ORIENTATION: case ST_LSM6DSR_ID_WRIST_TILT: case ST_LSM6DSR_ID_GLANCE: err = st_lsm6dsr_fsm_set_enable(sensor, enable); break; default: err = -EINVAL; break; } return err; } /** * Enable Step Counter Sensor [EMB_FUN] * * @param sensor: ST IMU sensor instance * @param enable: Enable/Disable sensor * @return < 0 if error, 0 otherwise */ int st_lsm6dsr_step_counter_set_enable(struct st_lsm6dsr_sensor *sensor, bool enable) { struct st_lsm6dsr_hw *hw = sensor->hw; int err; err = st_lsm6dsr_sensor_set_enable(sensor, enable); if (err < 0) return err; mutex_lock(&hw->page_lock); err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, true); if (err < 0) goto unlock; err = __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR, ST_LSM6DSR_REG_PEDO_EN_MASK, enable); if (err < 0) goto reset_page; err = __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_EMB_FUNC_FIFO_CFG_ADDR, ST_LSM6DSR_REG_PEDO_FIFO_EN_MASK, enable); reset_page: st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false); unlock: mutex_unlock(&hw->page_lock); return err; } /** * Reset Step Counter value [EMB_FUN] * * @param iio_dev: IIO device * @return < 0 if error, 0 otherwise */ int st_lsm6dsr_reset_step_counter(struct iio_dev *iio_dev) { struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev); struct st_lsm6dsr_hw *hw = sensor->hw; u16 prev_val, val = 0; __le16 data; int err; mutex_lock(&iio_dev->mlock); if (iio_buffer_enabled(iio_dev)) { err = -EBUSY; goto unlock_iio_dev; } err = st_lsm6dsr_step_counter_set_enable(sensor, true); if (err < 0) goto unlock_iio_dev; mutex_lock(&hw->page_lock); err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, true); if (err < 0) goto unlock_page; do { prev_val = val; err = __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_EMB_FUNC_SRC_ADDR, ST_LSM6DSR_REG_PEDO_RST_STEP_MASK, 1); if (err < 0) goto reset_page; msleep(100); err = hw->tf->read(hw->dev, ST_LSM6DSR_REG_STEP_COUNTER_L_ADDR, sizeof(data), (u8 *)&data); if (err < 0) goto reset_page; val = le16_to_cpu(data); } while (val && val >= prev_val); reset_page: st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false); unlock_page: mutex_unlock(&hw->page_lock); err = st_lsm6dsr_step_counter_set_enable(sensor, false); unlock_iio_dev: mutex_unlock(&iio_dev->mlock); return err; } /** * Read Orientation data sensor [EMB_FUN] * * @param hw: ST IMU MEMS hw instance. * @param out: Out data buffer. * @return < 0 if error, 0 otherwise */ int st_lsm6dsr_fsm_get_orientation(struct st_lsm6dsr_hw *hw, u8 *out) { int err; u8 data; mutex_lock(&hw->page_lock); err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, true); if (err < 0) goto unlock; err = hw->tf->read(hw->dev, ST_LSM6DSR_REG_FSM_OUTS6_ADDR, sizeof(data), &data); if (err < 0) goto reset_page; switch (data) { case ST_LSM6DSR_REG_ORIENTATION_0_MASK: *out = 0; break; case ST_LSM6DSR_REG_ORIENTATION_90_MASK: *out = 1; break; case ST_LSM6DSR_REG_ORIENTATION_180_MASK: *out = 2; break; case ST_LSM6DSR_REG_ORIENTATION_270_MASK: *out = 3; break; default: err = -EINVAL; break; } reset_page: st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false); unlock: mutex_unlock(&hw->page_lock); return err; } /** * Initialize Finite State Machine HW block [FSM_FUN] * * @param hw: ST IMU MEMS hw instance * @return < 0 if error, 0 otherwise */ int st_lsm6dsr_fsm_init(struct st_lsm6dsr_hw *hw) { u8 nfsm[] = { ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list), ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list) }; __le16 irq_mask, fsm_addr = ST_LSM6DSR_FSM_BASE_ADDRESS; u8 val[2] = {}; int i, err; mutex_lock(&hw->page_lock); err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, true); if (err < 0) goto unlock; /* enable gesture rec */ err = __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_EMB_FUNC_EN_B_ADDR, ST_LSM6DSR_REG_FSM_EN_MASK, 1); if (err < 0) goto reset_page; /* gest rec ODR 52Hz */ err = __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_EMB_FUNC_ODR_CFG_B_ADDR, ST_LSM6DSR_REG_FSM_ODR_MASK, ST_LSM6DSR_FSM_ODR_52); if (err < 0) goto reset_page; /* disable all fsm sensors */ err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR, sizeof(val), val); if (err < 0) goto reset_page; /* enable fsm interrupt */ irq_mask = (1 << ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list)) - 1; err = hw->tf->write(hw->dev, hw->embfunc_irq_reg + 1, sizeof(irq_mask), (u8 *)&irq_mask); if (err < 0) goto reset_page; /* enable latched interrupts */ err = __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_PAGE_RW_ADDR, ST_LSM6DSR_REG_EMB_FUNC_LIR_MASK, 1); if (err < 0) goto reset_page; /* enable access */ err = st_lsm6dsr_fsm_set_access(hw, true); if (err < 0) goto reset_page; /* # of configured fsm */ err = st_lsm6dsr_fsm_write(hw, 0x17c, sizeof(nfsm), nfsm); if (err < 0) goto reset_access; err = st_lsm6dsr_fsm_write(hw, 0x17e, sizeof(fsm_addr), (u8 *) &fsm_addr); if (err < 0) goto reset_access; /* configure fsm */ for (i = 0; i < ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list); i++) { err = st_lsm6dsr_fsm_write(hw, fsm_addr, st_lsm6dsr_fsm_sensor_list[i].len, st_lsm6dsr_fsm_sensor_list[i].data); if (err < 0) goto reset_access; fsm_addr += st_lsm6dsr_fsm_sensor_list[i].len; } reset_access: st_lsm6dsr_fsm_set_access(hw, false); __st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_PAGE_SEL_ADDR, ST_LSM6DSR_REG_PAGE_SEL_RST_MASK, 1); reset_page: st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false); unlock: mutex_unlock(&hw->page_lock); return err; }