You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@mynewt.apache.org by je...@apache.org on 2022/02/14 10:15:21 UTC
[mynewt-core] 01/02: sensors: Add driver for LSM6DSL
This is an automated email from the ASF dual-hosted git repository.
jerzy pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/mynewt-core.git
commit 1f76a4e1b873bb9d0ec9594dc176072a726db347
Author: Jerzy Kasenberg <je...@codecoup.pl>
AuthorDate: Tue Dec 14 15:49:38 2021 +0100
sensors: Add driver for LSM6DSL
Accelerometer/Gyroscope driver
---
.../sensors/lsm6dsl/include/lsm6dsl/lsm6dsl.h | 784 ++++++
hw/drivers/sensors/lsm6dsl/pkg.yml | 41 +
hw/drivers/sensors/lsm6dsl/src/lsm6dsl.c | 2602 ++++++++++++++++++++
hw/drivers/sensors/lsm6dsl/src/lsm6dsl_priv.h | 775 ++++++
hw/drivers/sensors/lsm6dsl/src/lsm6dsl_shell.c | 324 +++
hw/drivers/sensors/lsm6dsl/syscfg.yml | 83 +
6 files changed, 4609 insertions(+)
diff --git a/hw/drivers/sensors/lsm6dsl/include/lsm6dsl/lsm6dsl.h b/hw/drivers/sensors/lsm6dsl/include/lsm6dsl/lsm6dsl.h
new file mode 100644
index 0000000..e0c341f
--- /dev/null
+++ b/hw/drivers/sensors/lsm6dsl/include/lsm6dsl/lsm6dsl.h
@@ -0,0 +1,784 @@
+/*
+ * 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.
+ */
+
+#ifndef __LSM6DSL_H__
+#define __LSM6DSL_H__
+
+#include <os/mynewt.h>
+#include <sensor/sensor.h>
+#include "../src/lsm6dsl_priv.h"
+
+#if MYNEWT_VAL(BUS_DRIVER_PRESENT)
+#include "bus/drivers/i2c_common.h"
+#include "bus/drivers/spi_common.h"
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Threshold for D4D/D6D function */
+enum lsm6dsl_ths_6d {
+ LSM6DSL_6D_THS_80_DEG = 0,
+ LSM6DSL_6D_THS_70_DEG = 1,
+ LSM6DSL_6D_THS_60_DEG = 2,
+ LSM6DSL_6D_THS_50_DEG = 3
+};
+
+/* Sensors read mode */
+enum lsm6dsl_read_mode {
+ LSM6DSL_READ_POLL = 0,
+ LSM6DSL_READ_STREAM = 1,
+};
+
+/* Threshold for Free Fall detection */
+typedef enum {
+ LSM6DSL_FF_THS_156_MG_VAL,
+ LSM6DSL_FF_THS_219_MG_VAL,
+ LSM6DSL_FF_THS_250_MG_VAL,
+ LSM6DSL_FF_THS_312_MG_VAL,
+ LSM6DSL_FF_THS_344_MG_VAL,
+ LSM6DSL_FF_THS_406_MG_VAL,
+ LSM6DSL_FF_THS_469_MG_VAL,
+ LSM6DSL_FF_THS_500_MG_VAL,
+} free_fall_threshold_t;
+
+struct lsm6dsl_int {
+ /* Sleep waiting for an interrupt to occur */
+ struct os_sem wait;
+
+ /* Is the interrupt currently active */
+ bool active;
+
+ /* Is there a waiter currently sleeping */
+ bool asleep;
+
+ /* Configured interrupts */
+ struct sensor_int ints[1];
+};
+
+struct lsm6dsl_orientation_settings {
+ uint8_t en_4d: 1;
+
+ /* 6D/4D angle threshold */
+ enum lsm6dsl_ths_6d ths_6d;
+};
+
+typedef enum {
+ LSM6DSL_INACTIVITY_DISABLED,
+ LSM6DSL_INACTIVITY_WITH_GYRO_UNCHANGED,
+ LSM6DSL_INACTIVITY_WITH_GYRO_SLEEP,
+ LSM6DSL_INACTIVITY_WITH_GYRO_POWER_DOWN,
+} lsm6dsl_inactivity_t;
+
+struct lsm6dsl_wk_settings {
+ /* Wakeup threshold in FS_XL/(2^6) */
+ uint8_t wake_up_ths;
+ /* Wake duration in ODR_time */
+ uint8_t wake_up_dur;
+ /* Inactivity time before going to sleep in 512 ODR_Time (0 = 16 ODR time) */
+ uint8_t sleep_duration;
+ /* hpf or slope, 1 = hpf, 0 = slope */
+ uint8_t hpf_slope;
+ /* Inactivity mode, when enabled, sleep interrupt will fire when changing to inactive mode */
+ lsm6dsl_inactivity_t inactivity;
+};
+
+struct lsm6dsl_ff_settings {
+ /* Free-fall configuration parameters 1LSB = 1 ORD time */
+ uint8_t free_fall_dur;
+ /* Free-fall threshold */
+ free_fall_threshold_t free_fall_ths;
+};
+
+struct lsm6dsl_tap_settings {
+ /* Axis enabled bitmask */
+ uint8_t en_x: 1;
+ uint8_t en_y: 1;
+ uint8_t en_z: 1;
+ uint8_t en_dtap: 1;
+
+ /* Threshold for tap recognition */
+ int8_t tap_ths;
+
+ /* Time duration of maximum time gap for double tap recognition */
+ uint8_t dur;
+
+ /* Expected quiet time after a tap detection */
+ uint8_t quiet;
+
+ /* Maximum duration of overthreshold event */
+ uint8_t shock;
+};
+
+struct lsm6dsl_notif_cfg {
+ /* Interrupt event registered */
+ sensor_event_type_t event;
+
+ /* Interrupt pin number (0/1) */
+ uint8_t int_num: 1;
+
+ /* Interrupt bit mask */
+ uint8_t int_mask;
+
+ /* Int enable bit */
+ uint8_t int_en;
+};
+
+/* Read mode configuration */
+struct lsm6dsl_read_mode_cfg {
+ enum lsm6dsl_read_mode mode;
+ uint8_t int_num: 1;
+ uint8_t int_cfg;
+ uint8_t int_reg;
+};
+
+typedef enum {
+ /* Supported FIFO mode by driver */
+ LSM6DSL_FIFO_MODE_BYPASS_VAL = 0x00,
+ LSM6DSL_FIFO_MODE_CONTINUOUS_VAL = 0x06
+} lsm6dsl_fifo_mode_t;
+
+struct lsm6dsl_fifo_cfg {
+ lsm6dsl_fifo_mode_t mode;
+ uint16_t wtm;
+};
+
+struct int_src_regs {
+ uint8_t wake_up_src;
+ uint8_t tap_src;
+ uint8_t d6d_src;
+ uint8_t status_reg;
+};
+
+struct lsm6dsl_cfg {
+ uint8_t acc_fs;
+ uint8_t acc_rate;
+ uint8_t gyro_fs;
+ uint8_t gyro_rate;
+
+ struct lsm6dsl_tap_settings tap;
+ struct lsm6dsl_orientation_settings orientation;
+ struct lsm6dsl_wk_settings wk;
+ struct lsm6dsl_ff_settings ff;
+
+ /* Event notification config */
+ struct lsm6dsl_notif_cfg *notify_cfg;
+ uint8_t notify_cfg_count;
+
+ /* Read mode config */
+ struct lsm6dsl_read_mode_cfg read;
+
+ /* FIFO configuration */
+ struct lsm6dsl_fifo_cfg fifo;
+
+ /* Pin interrupt config */
+ uint8_t int1_pin_cfg;
+ uint8_t int2_pin_cfg;
+ uint8_t map_int2_to_int1 : 1;
+ uint8_t latched_int : 1;
+
+ /* The sensors mask */
+ sensor_type_t lc_s_mask;
+};
+
+/* Private per driver data */
+struct lsm6dsl_pdd {
+ /* Notification event context */
+ struct sensor_notify_ev_ctx notify_ctx;
+
+ /* Interrupt state */
+ struct lsm6dsl_int *interrupt;
+
+ /* Interrupt enabled flag */
+ uint16_t int_enable;
+};
+
+struct lsm6dsl_create_dev_cfg {
+#if MYNEWT_VAL(BUS_DRIVER_PRESENT)
+ bool node_is_spi;
+ union {
+#if MYNEWT_VAL(LSM6DSL_SPI_SUPPORT)
+ struct bus_spi_node_cfg spi_cfg;
+#endif
+#if MYNEWT_VAL(LSM6DSL_I2C_SUPPORT)
+ struct bus_i2c_node_cfg i2c_cfg;
+#endif
+ };
+#else
+ struct sensor_itf itf;
+#endif
+ int16_t int_pin;
+ bool int_active_level;
+};
+
+struct lsm6dsl {
+#if MYNEWT_VAL(BUS_DRIVER_PRESENT)
+ union {
+ struct bus_i2c_node i2c_node;
+ struct bus_spi_node spi_node;
+ };
+ bool node_is_spi;
+#else
+ struct os_dev dev;
+#endif
+ struct sensor sensor;
+ struct lsm6dsl_cfg_regs1 cfg_regs1;
+ struct lsm6dsl_cfg_regs2 cfg_regs2;
+ struct lsm6dsl_cfg cfg;
+ struct lsm6dsl_int intr;
+ struct lsm6dsl_pdd pdd;
+ float acc_mult;
+ float gyro_mult;
+};
+
+/* Angular rate sensor self-test mode selection */
+#define LSM6DSL_NORMAL_MODE_G_ST_VAL 0x00
+#define LSM6DSL_POSITIVE_SIGN_G_ST_VAL 0x01
+#define LSM6DSL_NEGATIVE_SIGN_G_ST_VAL 0x03
+
+/* Linear acceleration sensor self-test mode selection */
+#define LSM6DSL_NORMAL_MODE_XL_ST_VAL 0x00
+#define LSM6DSL_POSITIVE_SIGN_XL_ST_VAL 0x01
+#define LSM6DSL_NEGATIVE_SIGN_XL_ST_VAL 0x02
+
+/* Accelerometer bandwidth configurations */
+#define LSM6DSL_BW_LP_XL_ODR_2_VAL 0x00
+#define LSM6DSL_BW_LP_XL_ODR_4_VAL 0x00
+#define LSM6DSL_BW_LP_XL_ODR_10_VAL 0x01
+#define LSM6DSL_BW_LP_XL_ODR_20_VAL 0x02
+#define LSM6DSL_BW_LP_XL_ODR_45_VAL 0x03
+#define LSM6DSL_BW_LP_XL_ODR_100_VAL 0x04
+#define LSM6DSL_BW_LP_XL_ODR_200_VAL 0x05
+#define LSM6DSL_BW_LP_XL_ODR_400_VAL 0x06
+#define LSM6DSL_BW_LP_XL_ODR_800_VAL 0x07
+
+#define LSM6DSL_BW_HP_XL_SLOPE_VAL 0x00
+#define LSM6DSL_BW_HP_XL_ODR_10_VAL 0x01
+#define LSM6DSL_BW_HP_XL_ODR_20_VAL 0x02
+#define LSM6DSL_BW_HP_XL_ODR_45_VAL 0x03
+#define LSM6DSL_BW_HP_XL_ODR_100_VAL 0x04
+#define LSM6DSL_BW_HP_XL_ODR_200_VAL 0x05
+#define LSM6DSL_BW_HP_XL_ODR_400_VAL 0x06
+#define LSM6DSL_BW_HP_XL_ODR_800_VAL 0x07
+
+/* TAP priority decoding */
+#define LSM6DSL_TAP_PRIO_XYZ_VAL 0x00
+#define LSM6DSL_TAP_PRIO_YXZ_VAL 0x01
+#define LSM6DSL_TAP_PRIO_XZY_VAL 0x02
+#define LSM6DSL_TAP_PRIO_ZYX_VAL 0x03
+
+/* Accelerometer data rate */
+typedef enum {
+ LSM6DSL_ACCEL_OFF = 0x00,
+ LSM6DSL_ACCEL_12_5HZ = 0x01,
+ LSM6DSL_ACCEL_26HZ = 0x02,
+ LSM6DSL_ACCEL_52HZ = 0x03,
+ LSM6DSL_ACCEL_104HZ = 0x04,
+ LSM6DSL_ACCEL_208HZ = 0x05,
+ LSM6DSL_ACCEL_416HZ = 0x06,
+ LSM6DSL_ACCEL_833HZ = 0x07,
+ LSM6DSL_ACCEL_1666HZ = 0x08,
+ LSM6DSL_ACCEL_3333HZ = 0x09,
+ LSM6DSL_ACCEL_6666HZ = 0x0a,
+} acc_data_rate_t;
+
+/* Gyroscope data rate */
+typedef enum {
+ LSM6DSL_GYRO_OFF = 0x00,
+ LSM6DSL_GYRO_12_5HZ = 0x01,
+ LSM6DSL_GYRO_26HZ = 0x02,
+ LSM6DSL_GYRO_52HZ = 0x03,
+ LSM6DSL_GYRO_104HZ = 0x04,
+ LSM6DSL_GYRO_208HZ = 0x05,
+ LSM6DSL_GYRO_416HZ = 0x06,
+ LSM6DSL_GYRO_833HZ = 0x07,
+ LSM6DSL_GYRO_1666HZ = 0x08,
+ LSM6DSL_GYRO_3333HZ = 0x09,
+ LSM6DSL_GYRO_6666HZ = 0x0a,
+} gyro_data_rate_t;
+
+/* Accelerometer full scale range in G */
+typedef enum {
+ LSM6DSL_ACCEL_FS_2G = 0x00,
+ LSM6DSL_ACCEL_FS_4G = 0x02,
+ LSM6DSL_ACCEL_FS_8G = 0x03,
+ LSM6DSL_ACCEL_FS_16G = 0x01,
+} accel_full_scale_t;
+
+typedef enum {
+ /* 2^-10 g/LSB */
+ LSM6DSL_USER_WEIGHT_LO,
+ /* 2^-6 g/LSB */
+ LSM6DSL_USER_WEIGHT_HI,
+} user_off_weight_t;
+
+typedef enum {
+ DATA_NOT_IN_FIFO = 0,
+ NO_DECIMATION = 1,
+ DECIMATION_FACTOR_2 = 2,
+ DECIMATION_FACTOR_3 = 3,
+ DECIMATION_FACTOR_4 = 4,
+ DECIMATION_FACTOR_8 = 5,
+ DECIMATION_FACTOR_16 = 6,
+ DECIMATION_FACTOR_32 = 7,
+} fifo_decimation_t;
+
+#define LSM6DSL_ACCEL_FS_MIN_VAL 2
+#define LSM6DSL_ACCEL_FS_MAX_VAL 16
+
+/* Gyroscope full scale range in DPS */
+typedef enum {
+ LSM6DSL_GYRO_FS_125DPS = 0x01,
+ LSM6DSL_GYRO_FS_250DPS = 0x00,
+ LSM6DSL_GYRO_FS_500DPS = 0x02,
+ LSM6DSL_GYRO_FS_1000DPS = 0x04,
+ LSM6DSL_GYRO_FS_2000DPS = 0x08,
+} gyro_full_scale_t;
+
+#define LSM6DSL_GYRO_FS_MIN_VAL 125
+#define LSM6DSL_GYRO_FS_MAX_VAL 2000
+
+/**
+ * Expects to be called back through os_dev_create().
+ *
+ * @param ptr to the device object associated with this accelerometer
+ * @param argument passed to OS device init
+ *
+ * @return 0 on success, non-zero on failure.
+ */
+int lsm6dsl_init(struct os_dev *dev, void *arg);
+
+/**
+ * Configure the sensor
+ *
+ * @param ptr to sensor driver
+ * @param ptr to sensor driver config
+ */
+int lsm6dsl_config(struct lsm6dsl *lsm6dsl, struct lsm6dsl_cfg *cfg);
+
+/**
+ * Init shell
+ *
+ * @return 0 on success, non-zero on failure.
+ */
+int lsm6dsl_shell_init(void);
+
+/**
+ * Run Self test on sensor
+ *
+ * @param lsm6dsl The device
+ * @param pointer to return test result in
+ * 0 on pass
+ * 1 on XL failure
+ * 2 on Gyro failure
+ *
+ * @return 0 on sucess, non-zero on failure
+ */
+int lsm6dsl_run_self_test(struct lsm6dsl *lsm6dsl, int *result);
+
+/**
+ * Gets a new data sample from the acc/gyro/temp sensor.
+ *
+ * @param lsm6dsl The device
+ * @param Sensor type
+ * @param axis data
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_ag_data(struct lsm6dsl *lsm6dsl, sensor_type_t type, void *data);
+
+/**
+ * Create lsm6dsl device
+ *
+ * @param lsm6dsl device object to initialize
+ * @param name name of the device
+ * @param cfg device configuration
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_create_dev(struct lsm6dsl *lsm6dsl, const char *name,
+ const struct lsm6dsl_create_dev_cfg *cfg);
+
+/**
+ * Write number of data bytes to LSM6DSL sensor over interface
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param buffer data buffer
+ * @param len number of bytes to write
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_write(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ const uint8_t *buffer, uint8_t len);
+
+/**
+ * Write single byte to registers
+ *
+ * For configuration registers (that don't change by themselves) new value is only written
+ * when change from locally stored cached value.
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param val new register value
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_write_reg(struct lsm6dsl *lsm6dsl, uint8_t reg, uint8_t val);
+
+/**
+ * Read number of data bytes from LSM6DSL sensor over interface
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param buffer data buffer
+ * @param len number of bytes to read
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_read(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ uint8_t *buffer, uint8_t len);
+
+/**
+ * Reads single byte from register
+ *
+ * Node: if register is cached no read is performed.
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param val buffer for return value
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_read_reg(struct lsm6dsl *lsm6dsl, uint8_t reg, uint8_t *val);
+
+/**
+ * Sets gyro full scale selection
+ *
+ * @param lsm6dsl The device
+ * @param fs full scale setting
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_gyro_full_scale(struct lsm6dsl *lsm6dsl, uint8_t fs);
+
+/**
+ * Sets accelerometer full scale selection
+ *
+ * @param lsm6dsl The device
+ * @param The fs setting
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_acc_full_scale(struct lsm6dsl *lsm6dsl, uint8_t fs);
+
+/**
+ * Sets accelerometer rate
+ *
+ * @param lsm6dsl The device
+ * @param rate data rate for accelerometer
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_acc_rate(struct lsm6dsl *lsm6dsl, acc_data_rate_t rate);
+
+/**
+ * Sets gyro rate
+ *
+ * @param lsm6dsl The device
+ * @param rate data rage for gyro
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_gyro_rate(struct lsm6dsl *lsm6dsl, gyro_data_rate_t rate);
+
+/**
+ * Set FIFO mode
+ *
+ * @param lsm6dsl The device
+ * @param mode
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_fifo_mode(struct lsm6dsl *lsm6dsl, lsm6dsl_fifo_mode_t mode);
+
+/**
+ * Set FIFO watermark
+ *
+ * @param lsm6dsl The device
+ * @param wtm water mark for FIFO
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_fifo_watermark(struct lsm6dsl *lsm6dsl, uint16_t wtm);
+
+/**
+ * Get Number of Samples in FIFO
+ *
+ * @param lsm6dsl The device
+ * @param Pointer to return number of samples in FIFO, 0 empty, 2048 for full
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_fifo_samples(struct lsm6dsl *lsm6dsl, uint16_t *samples);
+
+/**
+ * Get FIFO pattern
+ *
+ * @param lsm6dsl The device
+ * @param Pointer to return number of samples in FIFO, 0 empty, 2048 for full
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_fifo_pattern(struct lsm6dsl *lsm6dsl, uint16_t *pattern);
+
+/**
+ * Sets accelerometer sensor user offsets
+ *
+ * Offset weight is 2^(-10) g/LSB or 2^(-6) g/LSB
+ *
+ * @param lsm6dsl The device
+ * @param offset_x X offset
+ * @param offset_y Y offset
+ * @param offset_z Z offset
+ * @param weight LSB wight of offset values
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+int lsm6dsl_set_offsets(struct lsm6dsl *lsm6dsl, int8_t offset_x, int8_t offset_y,
+ int8_t offset_z, user_off_weight_t weight);
+
+/**
+ * Gets accelerometer sensor user offsets
+ *
+ * @param lsm6dsl The device
+ * @param offset_x pointer to location to store X offset
+ * @param offset_y pointer to location to store Y offset
+ * @param offset_z pointer to location to store Z offset
+ * @param weight pointer to location to store weight
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+int lsm6dsl_get_offsets(struct lsm6dsl *lsm6dsl, int8_t *offset_x,
+ int8_t *offset_y, int8_t *offset_z, user_off_weight_t *weight);
+
+/**
+ * Sets push-pull/open-drain on INT1 and INT2 pins
+ *
+ * @param lsm6dsl The device
+ * @param open_drain true INT pin is open drain, false push-pull
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_int_pp_od(struct lsm6dsl *lsm6dsl, bool open_drain);
+
+/**
+ * Gets push-pull/open-drain on INT1 and INT2 pins
+ *
+ * @param lsm6dsl The device
+ * @param open_drain buffer for result
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_int_pp_od(struct lsm6dsl *lsm6dsl, bool *open_drain);
+
+/**
+ * Sets whether latched interrupts are enabled
+ *
+ * @param lsm6dsl The device
+ * @param value to set (0 = not latched, 1 = latched)
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_latched_int(struct lsm6dsl *lsm6dsl, bool en);
+
+/**
+ * Gets whether latched interrupts are enabled
+ *
+ * @param lsm6dsl The device
+ * @param ptr to store value (0 = not latched, 1 = latched)
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_latched_int(struct lsm6dsl *lsm6dsl, uint8_t *en);
+
+/**
+ * Redirects int2 to int1
+ *
+ * @param lsm6dsl The device
+ * @param value to set (0 = int2 is separate, 1 = int2 activates int1 pin)
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_map_int2_to_int1(struct lsm6dsl *lsm6dsl, bool en);
+
+/**
+ * Gets whether latched interrupts are enabled
+ *
+ * @param lsm6dsl The device
+ * @param ptr to store value (0 = not latched, 1 = latched)
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_map_int2_to_int1(struct lsm6dsl *lsm6dsl, uint8_t *en);
+
+/**
+ * Sets whether interrupts are active high or low
+ *
+ * @param lsm6dsl The device
+ * @param level value to set (1 = active high, 0 = active low)
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_int_level(struct lsm6dsl *lsm6dsl, uint8_t level);
+
+/**
+ * Gets whether interrupts are active high or low
+ *
+ * @param lsm6dsl The device
+ * @param ptr to store value (1 = active high, 0 = active low)
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_int_level(struct lsm6dsl *lsm6dsl, uint8_t *level);
+
+/**
+ * Clear interrupt pin configuration for interrupt
+ *
+ * @param lsm6dsl The device
+ * @param interrupt pin
+ * @param interrupt mask
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_clear_int_pin_cfg(struct lsm6dsl *lsm6dsl, uint8_t int_pin,
+ uint8_t int_mask);
+
+/**
+ * Clear all interrupts by reading all four interrupt registers status
+ *
+ * @param itf The sensor interface
+ * @param src Ptr to return 4 interrupt sources in
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_clear_int(struct lsm6dsl *lsm6dsl, struct int_src_regs *int_src);
+
+/**
+ * Set interrupt pin configuration for interrupt
+ *
+ * @param lsm6dsl The device
+ * @param interrupt pin
+ * @param interrupt mask
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_int_pin_cfg(struct lsm6dsl *lsm6dsl, uint8_t int_pin,
+ uint8_t int_mask);
+
+/**
+ * Set orientation configuration
+ *
+ * @param lsm6dsl The device
+ * @param cfg orientation settings to set
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_orientation(struct lsm6dsl *lsm6dsl,
+ const struct lsm6dsl_orientation_settings *cfg);
+
+/**
+ * Get orientation configuration
+ *
+ * @param lsm6dsl The device
+ * @param cfg orientation settings to fill
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_orientation_cfg(struct lsm6dsl *lsm6dsl,
+ struct lsm6dsl_orientation_settings *cfg);
+
+/**
+ * Set tap detection configuration
+ *
+ * @param lsm6dsl The device
+ * @param cfg new tap settings
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_tap_cfg(struct lsm6dsl *lsm6dsl,
+ const struct lsm6dsl_tap_settings *cfg);
+
+/**
+ * Get tap detection config
+ *
+ * @param lsm6dsl The device
+ * @param ptr to the tap settings
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_tap_cfg(struct lsm6dsl *lsm6dsl,
+ struct lsm6dsl_tap_settings *cfg);
+
+/**
+ * Set free-fall detection configuration
+ *
+ * @param lsm6dsl The device
+ * @param ff free fall settings
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_free_fall(struct lsm6dsl *lsm6dsl, struct lsm6dsl_ff_settings *ff);
+
+/**
+ * Get free-fall detection config
+ *
+ * @param lsm6dsl The device
+ * @param ff ptr to free-fall configuration
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_free_fall(struct lsm6dsl *lsm6dsl, struct lsm6dsl_ff_settings *ff);
+
+/**
+ * Set Wake Up Duration
+ *
+ * @param lsm6dsl The device
+ * @param wk configuration to set
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_set_wake_up(struct lsm6dsl *lsm6dsl, struct lsm6dsl_wk_settings *wk);
+
+/**
+ * Get Wake Up Duration
+ *
+ * @param lsm6dsl The device
+ * @param ptr to wake up configuration
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int lsm6dsl_get_wake_up(struct lsm6dsl *lsm6dsl, struct lsm6dsl_wk_settings *wk);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __LSM6DSL_H__ */
diff --git a/hw/drivers/sensors/lsm6dsl/pkg.yml b/hw/drivers/sensors/lsm6dsl/pkg.yml
new file mode 100644
index 0000000..5cd1e6f
--- /dev/null
+++ b/hw/drivers/sensors/lsm6dsl/pkg.yml
@@ -0,0 +1,41 @@
+#
+# 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.
+#
+
+pkg.name: hw/drivers/sensors/lsm6dsl
+pkg.description: Driver for the LSM6DSL 3D accelerometer and 3D gyroscope
+pkg.keywords:
+ - lsm6dsl
+ - i2c
+ - spi
+ - sensor
+
+pkg.deps:
+ - "@apache-mynewt-core/kernel/os"
+ - "@apache-mynewt-core/hw/hal"
+ - "@apache-mynewt-core/hw/sensor"
+ - "@apache-mynewt-core/sys/log/modlog"
+pkg.deps.!BUS_DRIVER_PRESENT:
+ - "@apache-mynewt-core/hw/util/i2cn"
+
+pkg.req_apis:
+ - stats
+
+pkg.init.LSM6DSL_CLI:
+ lsm6dsl_pkg_init: 'MYNEWT_VAL(LSM6DSL_SYSINIT_STAGE)'
+
diff --git a/hw/drivers/sensors/lsm6dsl/src/lsm6dsl.c b/hw/drivers/sensors/lsm6dsl/src/lsm6dsl.c
new file mode 100644
index 0000000..1de931c
--- /dev/null
+++ b/hw/drivers/sensors/lsm6dsl/src/lsm6dsl.c
@@ -0,0 +1,2602 @@
+/*
+ * 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.
+ */
+
+/**
+ * Driver for 6 axis IMU LSM6DSL
+ * For more details please refers to www.st.com AN5040
+ */
+#include <assert.h>
+#include <string.h>
+
+#include <os/mynewt.h>
+#include <hal/hal_gpio.h>
+#include <modlog/modlog.h>
+#include <stats/stats.h>
+#if MYNEWT_VAL(BUS_DRIVER_PRESENT)
+#include <bus/drivers/i2c_common.h>
+#include <bus/drivers/spi_common.h>
+#else /* BUS_DRIVER_PRESENT */
+#include <hal/hal_spi.h>
+#include <hal/hal_i2c.h>
+#include <i2cn/i2cn.h>
+#endif /* BUS_DRIVER_PRESENT */
+#include <sensor/sensor.h>
+#include <sensor/accel.h>
+#include <sensor/gyro.h>
+#include <sensor/temperature.h>
+#include <lsm6dsl/lsm6dsl.h>
+#include "lsm6dsl_priv.h"
+
+/* Default event notification */
+static const struct lsm6dsl_notif_cfg default_notif_cfg[] = {
+ {
+ .event = SENSOR_EVENT_TYPE_SINGLE_TAP,
+ .int_num = 0,
+ .int_mask = LSM6DSL_SINGLE_TAP_MASK,
+ .int_en = LSM6DSL_INT1_SINGLE_TAP_MASK
+ },
+ {
+ .event = SENSOR_EVENT_TYPE_DOUBLE_TAP,
+ .int_num = 0,
+ .int_mask = LSM6DSL_DOUBLE_TAP_MASK,
+ .int_en = LSM6DSL_INT1_DOUBLE_TAP_MASK
+ },
+ {
+ .event = SENSOR_EVENT_TYPE_FREE_FALL,
+ .int_num = 0,
+ .int_mask = LSM6DSL_FF_IA_MASK,
+ .int_en = LSM6DSL_INT1_FF_MASK
+ },
+ {
+ .event = SENSOR_EVENT_TYPE_WAKEUP,
+ .int_num = 0,
+ .int_mask = LSM6DSL_WU_IA_MASK,
+ .int_en = LSM6DSL_INT1_WU_MASK
+ },
+ {
+ .event = SENSOR_EVENT_TYPE_SLEEP,
+ .int_num = 0,
+ .int_mask = LSM6DSL_SLEEP_STATE_IA_MASK,
+ .int_en = LSM6DSL_INT1_INACT_STATE_MASK
+ },
+ {
+ .event = SENSOR_EVENT_TYPE_ORIENT_CHANGE,
+ .int_num = 0,
+ .int_mask = LSM6DSL_D6D_IA_MASK,
+ .int_en = LSM6DSL_INT1_6D_MASK
+ }
+};
+
+/* Define the stats section and records */
+STATS_SECT_START(lsm6dsl_stat_section)
+ STATS_SECT_ENTRY(write_errors)
+ STATS_SECT_ENTRY(read_errors)
+#if MYNEWT_VAL(LSM6DSL_NOTIF_STATS)
+ STATS_SECT_ENTRY(single_tap_notify)
+ STATS_SECT_ENTRY(double_tap_notify)
+ STATS_SECT_ENTRY(free_fall_notify)
+ STATS_SECT_ENTRY(sleep_notify)
+ STATS_SECT_ENTRY(orientation_notify)
+ STATS_SECT_ENTRY(wakeup_notify)
+#endif /* LSM6DSL_NOTIF_STATS */
+STATS_SECT_END
+
+/* Define stat names for querying */
+STATS_NAME_START(lsm6dsl_stat_section)
+ STATS_NAME(lsm6dsl_stat_section, write_errors)
+ STATS_NAME(lsm6dsl_stat_section, read_errors)
+#if MYNEWT_VAL(LSM6DSL_NOTIF_STATS)
+ STATS_NAME(lsm6dsl_stat_section, single_tap_notify)
+ STATS_NAME(lsm6dsl_stat_section, double_tap_notify)
+ STATS_NAME(lsm6dsl_stat_section, free_fall_notify)
+ STATS_NAME(lsm6dsl_stat_section, sleep_notify)
+ STATS_NAME(lsm6dsl_stat_section, orientation_notify)
+ STATS_NAME(lsm6dsl_stat_section, wakeup_notify)
+#endif /* LSM6DSL_NOTIFY_STATS */
+STATS_NAME_END(lsm6dsl_stat_section)
+
+/* Global variable used to hold stats data */
+STATS_SECT_DECL(lsm6dsl_stat_section) g_lsm6dsl_stats;
+
+static int lsm6dsl_sensor_read(struct sensor *sensor, sensor_type_t type,
+ sensor_data_func_t data_func, void *data_arg,
+ uint32_t timeout);
+static int lsm6dsl_sensor_get_config(struct sensor *sensor, sensor_type_t type,
+ struct sensor_cfg *cfg);
+static int lsm6dsl_sensor_set_notification(struct sensor *sensor,
+ sensor_event_type_t event);
+static int lsm6dsl_sensor_unset_notification(struct sensor *sensor,
+ sensor_event_type_t event);
+static int lsm6dsl_sensor_handle_interrupt(struct sensor *sensor);
+static int lsm6dsl_sensor_set_config(struct sensor *sensor, void *cfg);
+static int lsm6dsl_sensor_reset(struct sensor *sensor);
+
+static const struct sensor_driver g_lsm6dsl_sensor_driver = {
+ .sd_read = lsm6dsl_sensor_read,
+ .sd_get_config = lsm6dsl_sensor_get_config,
+ .sd_set_config = lsm6dsl_sensor_set_config,
+ .sd_set_notification = lsm6dsl_sensor_set_notification,
+ .sd_unset_notification = lsm6dsl_sensor_unset_notification,
+ .sd_handle_interrupt = lsm6dsl_sensor_handle_interrupt,
+ .sd_reset = lsm6dsl_sensor_reset,
+};
+
+#if !MYNEWT_VAL(BUS_DRIVER_PRESENT)
+/**
+ * Read number of data byte from LSM6DSL sensor over I2C
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param buffer data buffer
+ * @param len number of bytes to read
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_i2c_read(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ uint8_t *buffer, uint8_t len)
+{
+ int rc;
+ struct hal_i2c_master_data data_struct = {
+ .address = lsm6dsl->sensor.s_itf.si_addr,
+ .len = 1,
+ .buffer = ®
+ };
+
+ /* First byte is register address */
+ rc = i2cn_master_write(lsm6dsl->sensor.s_itf.si_num,
+ &data_struct, MYNEWT_VAL(LSM6DSL_I2C_TIMEOUT_TICKS),
+ 1,
+ MYNEWT_VAL(LSM6DSL_I2C_RETRIES));
+ if (rc) {
+ LSM6DSL_LOG_ERROR("I2C access failed at address 0x%02X\n",
+ data_struct.address);
+ STATS_INC(g_lsm6dsl_stats, read_errors);
+ goto end;
+ }
+
+ data_struct.buffer = buffer;
+ data_struct.len = len;
+
+ /* Read data from register(s) */
+ rc = i2cn_master_read(lsm6dsl->sensor.s_itf.si_num, &data_struct,
+ MYNEWT_VAL(LSM6DSL_I2C_TIMEOUT_TICKS), len,
+ MYNEWT_VAL(LSM6DSL_I2C_RETRIES));
+ if (rc) {
+ LSM6DSL_LOG_ERROR("Failed to read from 0x%02X:0x%02X\n",
+ data_struct.address, reg);
+ STATS_INC(g_lsm6dsl_stats, read_errors);
+ }
+
+end:
+
+ return rc;
+}
+
+/**
+ * Read number of data bytes from LSM6DSL sensor over SPI
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param buffer buffer for register data
+ * @param len number of bytes to read
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_spi_read(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ uint8_t *buffer, uint8_t len)
+{
+ int i;
+ uint16_t retval;
+ int rc = 0;
+
+ /* Select the device */
+ hal_gpio_write(lsm6dsl->sensor.s_itf.si_cs_pin, 0);
+
+ /* Send the address */
+ retval = hal_spi_tx_val(lsm6dsl->sensor.s_itf.si_num, LSM6DSL_SPI_READ_CMD_BIT(reg));
+ if (retval == 0xFFFF) {
+ rc = SYS_EINVAL;
+ LSM6DSL_LOG_ERROR("SPI_%u register write failed addr:0x%02X\n",
+ lsm6dsl->sensor.s_itf.si_num, reg);
+ STATS_INC(g_lsm6dsl_stats, read_errors);
+ goto end;
+ }
+
+ for (i = 0; i < len; i++) {
+ /* Read data */
+ retval = hal_spi_tx_val(lsm6dsl->sensor.s_itf.si_num, 0xFF);
+ if (retval == 0xFFFF) {
+ rc = SYS_EINVAL;
+ LSM6DSL_LOG_ERROR("SPI_%u read failed addr:0x%02X\n",
+ lsm6dsl->sensor.s_itf.si_num, reg);
+ STATS_INC(g_lsm6dsl_stats, read_errors);
+ goto end;
+ }
+ buffer[i] = retval;
+ }
+
+end:
+ /* De-select the device */
+ hal_gpio_write(lsm6dsl->sensor.s_itf.si_cs_pin, 1);
+
+ return rc;
+}
+
+/**
+ * Write number of bytes to LSM6DSL sensor over I2C
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param buffer data buffer
+ * @param len number of bytes to write
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_i2c_write(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ const uint8_t *buffer, uint8_t len)
+{
+ int rc;
+ uint8_t payload[20] = { reg };
+ struct hal_i2c_master_data data_struct = {
+ .address = lsm6dsl->sensor.s_itf.si_addr,
+ .len = len + 1,
+ .buffer = (uint8_t *)payload
+ };
+
+ /* Max tx payload can be sizeof(payload) less register address */
+ if (len >= sizeof(payload)) {
+ return OS_EINVAL;
+ }
+
+ memcpy(&payload[1], buffer, len);
+
+ /* Register write */
+ rc = i2cn_master_write(lsm6dsl->sensor.s_itf.si_num, &data_struct,
+ MYNEWT_VAL(LSM6DSL_I2C_TIMEOUT_TICKS), 1,
+ MYNEWT_VAL(LSM6DSL_I2C_RETRIES));
+ if (rc) {
+ LSM6DSL_LOG_ERROR("I2C access failed at address 0x%02X\n",
+ data_struct.address);
+ STATS_INC(g_lsm6dsl_stats, write_errors);
+ }
+
+ return rc;
+}
+
+/**
+ * Write number of bytes to LSM6DSL sensor over SPI
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param buffer data buffer
+ * @param len number of bytes to write
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_spi_write(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ const uint8_t *buffer, uint8_t len)
+{
+ int i;
+ int rc = 0;
+
+ /* Select the device */
+ hal_gpio_write(lsm6dsl->sensor.s_itf.si_cs_pin, 0);
+
+ /* Send the address */
+ rc = hal_spi_tx_val(lsm6dsl->sensor.s_itf.si_num, reg);
+ if (rc == 0xFFFF) {
+ rc = SYS_EINVAL;
+ LSM6DSL_LOG_ERROR("SPI_%u register write failed addr:0x%02X\n",
+ lsm6dsl->sensor.s_itf.si_num, reg);
+ STATS_INC(g_lsm6dsl_stats, write_errors);
+ goto end;
+ }
+
+ for (i = 0; i < len; i++) {
+ /* Read register data */
+ rc = hal_spi_tx_val(lsm6dsl->sensor.s_itf.si_num, buffer[i]);
+ if (rc == 0xFFFF) {
+ rc = SYS_EINVAL;
+ LSM6DSL_LOG_ERROR("SPI_%u write failed addr:0x%02X\n",
+ lsm6dsl->sensor.s_itf.si_num, reg);
+ STATS_INC(g_lsm6dsl_stats, write_errors);
+ goto end;
+ }
+ }
+
+end:
+ /* De-select the device */
+ hal_gpio_write(lsm6dsl->sensor.s_itf.si_cs_pin, 1);
+
+ return rc;
+}
+#endif /* BUS_DRIVER_PRESENT */
+
+int
+lsm6dsl_write(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ const uint8_t *buffer, uint8_t len)
+{
+ int rc;
+
+#if MYNEWT_VAL(BUS_DRIVER_PRESENT)
+ uint8_t write_data[len + 1];
+
+ write_data[0] = reg;
+ memcpy(write_data + 1, buffer, len);
+
+ rc = bus_node_simple_write((struct os_dev *)lsm6dsl, &write_data, len + 1);
+#else /* BUS_DRIVER_PRESENT */
+ rc = sensor_itf_lock(&lsm6dsl->sensor.s_itf, MYNEWT_VAL(LSM6DSL_ITF_LOCK_TMO));
+ if (rc) {
+ goto end;
+ }
+
+ if (lsm6dsl->sensor.s_itf.si_type == SENSOR_ITF_I2C) {
+ rc = lsm6dsl_i2c_write(lsm6dsl, reg, buffer, len);
+ } else {
+ rc = lsm6dsl_spi_write(lsm6dsl, reg, buffer, len);
+ }
+
+ sensor_itf_unlock(&lsm6dsl->sensor.s_itf);
+end:
+#endif /* BUS_DRIVER_PRESENT */
+
+ return rc;
+}
+
+int
+lsm6dsl_write_reg(struct lsm6dsl *lsm6dsl, uint8_t reg, uint8_t val)
+{
+ uint8_t *shadow_reg = NULL;
+ bool write_through = true;
+ int rc = 0;
+
+ if (reg >= LSM6DSL_FUNC_CFG_ACCESS_REG && reg <= LSM6DSL_D6D_SRC_REG) {
+ shadow_reg = &lsm6dsl->cfg_regs1.regs[reg - LSM6DSL_FUNC_CFG_ACCESS_REG];
+ } else if (reg >= LSM6DSL_TAP_CFG_REG && reg <= LSM6DSL_MD2_CFG_REG) {
+ shadow_reg = &lsm6dsl->cfg_regs2.regs[reg - LSM6DSL_TAP_CFG_REG];
+ }
+
+ if (shadow_reg) {
+ write_through = *shadow_reg != val;
+ *shadow_reg = val;
+ }
+ if (write_through) {
+ rc = lsm6dsl_write(lsm6dsl, reg, &val, 1);
+ }
+
+ return rc;
+}
+
+int
+lsm6dsl_read(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ uint8_t *buffer, uint8_t len)
+{
+ int rc;
+
+#if MYNEWT_VAL(BUS_DRIVER_PRESENT)
+
+ if (lsm6dsl->node_is_spi) {
+ LSM6DSL_SPI_READ_CMD_BIT(reg);
+ }
+
+ rc = bus_node_simple_write_read_transact((struct os_dev *)lsm6dsl, ®, 1,
+ buffer, len);
+#else /* BUS_DRIVER_PRESENT */
+ rc = sensor_itf_lock(&lsm6dsl->sensor.s_itf, MYNEWT_VAL(LSM6DSL_ITF_LOCK_TMO));
+ if (rc) {
+ goto end;
+ }
+
+ if (lsm6dsl->sensor.s_itf.si_type == SENSOR_ITF_I2C) {
+ rc = lsm6dsl_i2c_read(lsm6dsl, reg, buffer, len);
+ } else {
+ rc = lsm6dsl_spi_read(lsm6dsl, reg, buffer, len);
+ }
+
+ sensor_itf_unlock(&lsm6dsl->sensor.s_itf);
+end:
+#endif /* BUS_DRIVER_PRESENT */
+
+ return rc;
+}
+
+/**
+ * Return cached register variable address for give register address
+ *
+ * @param lsm6dsl the device
+ * @param reg register number
+ *
+ * @return address of local copy of register or NULL if register is not cached.
+ */
+static uint8_t *
+lsm6dsl_shadow_reg(struct lsm6dsl *lsm6dsl, uint8_t reg)
+{
+ uint8_t *shadow_reg;
+
+ if (reg >= LSM6DSL_FUNC_CFG_ACCESS_REG && reg <= LSM6DSL_D6D_SRC_REG) {
+ shadow_reg = &lsm6dsl->cfg_regs1.regs[reg - LSM6DSL_FUNC_CFG_ACCESS_REG];
+ } else if (reg >= LSM6DSL_TAP_CFG_REG && reg <= LSM6DSL_MD2_CFG_REG) {
+ shadow_reg = &lsm6dsl->cfg_regs2.regs[reg - LSM6DSL_TAP_CFG_REG];
+ } else {
+ shadow_reg = NULL;
+ }
+
+ return shadow_reg;
+}
+
+int
+lsm6dsl_read_reg(struct lsm6dsl *lsm6dsl, uint8_t reg, uint8_t *val)
+{
+ int rc;
+ uint8_t *shadow_reg = lsm6dsl_shadow_reg(lsm6dsl, reg);
+
+ if (shadow_reg) {
+ *val = *shadow_reg;
+ rc = 0;
+ } else {
+ rc = lsm6dsl_read(lsm6dsl, reg, val, 1);
+ }
+
+ return rc;
+}
+
+/**
+ * Modify register bit field
+ *
+ * @param lsm6dsl The device
+ * @param reg register address
+ * @param mask register mask
+ * @param data new bit filed value
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_write_reg_bit_field(struct lsm6dsl *lsm6dsl, uint8_t reg,
+ uint8_t mask, uint8_t data)
+{
+ int rc;
+
+ uint8_t new_data;
+ uint8_t old_data;
+
+ rc = lsm6dsl_read_reg(lsm6dsl, reg, &old_data);
+ if (rc) {
+ goto end;
+ }
+
+ new_data = ((old_data & (~mask)) | LSM6DSL_SHIFT_DATA_MASK(data, mask));
+
+ /* Try to limit bus access if possible */
+ if (new_data != old_data) {
+ rc = lsm6dsl_write_reg(lsm6dsl, reg, new_data);
+ }
+end:
+ return rc;
+}
+
+/**
+ * Reset lsm6dsl
+ *
+ * @param lsm6dsl The device
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_reset(struct lsm6dsl *lsm6dsl)
+{
+ int rc;
+
+ rc = lsm6dsl_write_reg(lsm6dsl, LSM6DSL_CTRL3_C_REG,
+ LSM6DSL_SW_RESET_MASK | LSM6DSL_IF_INC_MASK | LSM6DSL_BDU_MASK);
+
+ if (rc == 0) {
+ os_time_delay((OS_TICKS_PER_SEC * 10 / 1000) + 1);
+
+ rc = lsm6dsl_write_reg(lsm6dsl, LSM6DSL_CTRL3_C_REG,
+ LSM6DSL_IF_INC_MASK | LSM6DSL_BDU_MASK);
+ }
+
+ return rc;
+}
+
+/**
+ * Get chip ID
+ *
+ * @param lsm6dsl The device
+ * @param ptr to chip id to be filled up
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_get_chip_id(struct lsm6dsl *lsm6dsl, uint8_t *chip_id)
+{
+ return lsm6dsl_read(lsm6dsl, LSM6DSL_WHO_AM_I_REG, chip_id, 1);
+}
+
+int
+lsm6dsl_set_gyro_full_scale(struct lsm6dsl *lsm6dsl, uint8_t fs)
+{
+ switch (fs) {
+ case LSM6DSL_GYRO_FS_125DPS:
+ lsm6dsl->gyro_mult = 125.0f / 32768;
+ break;
+ case LSM6DSL_GYRO_FS_250DPS:
+ lsm6dsl->gyro_mult = 250.0f / 32768;
+ break;
+ case LSM6DSL_GYRO_FS_500DPS:
+ lsm6dsl->gyro_mult = 500.0f / 32768;
+ break;
+ case LSM6DSL_GYRO_FS_1000DPS:
+ lsm6dsl->gyro_mult = 1000.0f / 32768;
+ break;
+ case LSM6DSL_GYRO_FS_2000DPS:
+ lsm6dsl->gyro_mult = 2000.0f / 32768;
+ break;
+ }
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL2_G_REG,
+ LSM6DSL_FS_G_MASK, fs);
+}
+
+int
+lsm6dsl_set_acc_full_scale(struct lsm6dsl *lsm6dsl, uint8_t fs)
+{
+ switch (fs) {
+ case LSM6DSL_ACCEL_FS_2G:
+ lsm6dsl->acc_mult = 2 * STANDARD_ACCEL_GRAVITY / 32768;
+ break;
+ case LSM6DSL_ACCEL_FS_4G:
+ lsm6dsl->acc_mult = 4 * STANDARD_ACCEL_GRAVITY / 32768;
+ break;
+ case LSM6DSL_ACCEL_FS_8G:
+ lsm6dsl->acc_mult = 8 * STANDARD_ACCEL_GRAVITY / 32768;
+ break;
+ case LSM6DSL_ACCEL_FS_16G:
+ lsm6dsl->acc_mult = 16 * STANDARD_ACCEL_GRAVITY / 32768;
+ break;
+ }
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL1_XL_REG,
+ LSM6DSL_FS_XL_MASK, fs);
+}
+
+int
+lsm6dsl_set_acc_rate(struct lsm6dsl *lsm6dsl, acc_data_rate_t rate)
+{
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL1_XL_REG,
+ LSM6DSL_ODR_XL_MASK, rate);
+}
+
+int
+lsm6dsl_set_gyro_rate(struct lsm6dsl *lsm6dsl, gyro_data_rate_t rate)
+{
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL2_G_REG,
+ LSM6DSL_ODR_G_MASK, rate);
+}
+
+int
+lsm6dsl_set_fifo_mode(struct lsm6dsl *lsm6dsl, lsm6dsl_fifo_mode_t mode)
+{
+ int rc;
+ uint8_t fifo_odr = max(lsm6dsl->cfg.acc_rate, lsm6dsl->cfg.gyro_rate);
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_FIFO_CTRL5_REG,
+ LSM6DSL_ODR_FIFO_MASK, fifo_odr);
+ if (rc == 0) {
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_FIFO_CTRL5_REG,
+ LSM6DSL_FIFO_MODE_MASK, mode);
+ }
+ return rc;
+}
+
+int
+lsm6dsl_set_fifo_watermark(struct lsm6dsl *lsm6dsl, uint16_t wtm)
+{
+ int rc = 0;
+ uint16_t old_wtm;
+
+ if (wtm > LSM6DSL_MAX_FIFO_DEPTH) {
+ return SYS_EINVAL;
+ }
+
+ old_wtm = (lsm6dsl->cfg_regs1.fifo_ctrl1 | (lsm6dsl->cfg_regs1.fifo_ctrl2 << 8u)) & (LSM6DSL_MAX_FIFO_DEPTH - 1);
+ if (old_wtm != wtm) {
+ lsm6dsl->cfg_regs1.fifo_ctrl1 = (uint8_t)wtm;
+ lsm6dsl->cfg_regs1.fifo_ctrl2 &= ~LSM6DSL_FTH_8_10_MASK;
+ lsm6dsl->cfg_regs1.fifo_ctrl2 |= wtm >> 8;
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_FIFO_CTRL1_REG,
+ &lsm6dsl->cfg_regs1.fifo_ctrl1, 2);
+ }
+
+ return rc;
+}
+
+int
+lsm6dsl_get_fifo_samples(struct lsm6dsl *lsm6dsl, uint16_t *samples)
+{
+ uint8_t fifo_status[2];
+ int rc;
+
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_FIFO_STATUS1_REG,
+ (uint8_t *)&fifo_status, sizeof(fifo_status));
+ if (rc) {
+ return rc;
+ }
+
+ if (fifo_status[1] & LSM6DSL_OVER_RUN_MASK) {
+ *samples = 2048;
+ } else {
+ *samples = fifo_status[0] | ((LSM6DSL_DIFF_FIFO_MASK & fifo_status[1]) << 8u);
+ }
+
+ return 0;
+}
+
+int
+lsm6dsl_get_fifo_pattern(struct lsm6dsl *lsm6dsl, uint16_t *pattern)
+{
+ uint8_t fifo_status[2];
+ int rc;
+
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_FIFO_STATUS3_REG,
+ fifo_status, sizeof(fifo_status));
+ if (rc) {
+ return rc;
+ }
+
+ *pattern = (fifo_status[1] << 8u) | fifo_status[0];
+
+ return 0;
+}
+
+/**
+ * Set block data update
+ *
+ * @param lsm6dsl The device
+ * @param enable bdu
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_set_bdu(struct lsm6dsl *lsm6dsl, bool en)
+{
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL3_C_REG,
+ LSM6DSL_BDU_MASK, en);
+}
+
+int
+lsm6dsl_set_offsets(struct lsm6dsl *lsm6dsl, int8_t offset_x, int8_t offset_y,
+ int8_t offset_z, user_off_weight_t weight)
+{
+ int rc;
+ int8_t offset[] = { offset_x, offset_y, offset_z };
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL6_C_REG, LSM6DSL_USR_OFF_W_MASK, weight);
+ if (rc == 0) {
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_X_OFS_USR_REG,
+ (uint8_t *)offset, sizeof(offset));
+ }
+
+ return rc;
+}
+
+int
+lsm6dsl_get_offsets(struct lsm6dsl *lsm6dsl, int8_t *offset_x,
+ int8_t *offset_y, int8_t *offset_z, user_off_weight_t *weight)
+{
+ uint8_t ctrl6_c;
+ int8_t offset[3];
+ int rc;
+
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_X_OFS_USR_REG,
+ (uint8_t *)offset, sizeof(offset));
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_read_reg(lsm6dsl, LSM6DSL_CTRL6_C_REG, &ctrl6_c);
+ if (rc) {
+ goto end;
+ }
+
+ *weight = (ctrl6_c & LSM6DSL_USR_OFF_W_MASK) ? LSM6DSL_USER_WEIGHT_HI : LSM6DSL_USER_WEIGHT_LO;
+ *offset_x = offset[0];
+ *offset_y = offset[1];
+ *offset_z = offset[2];
+end:
+ return 0;
+}
+
+int
+lsm6dsl_set_int_pp_od(struct lsm6dsl *lsm6dsl, bool open_drain)
+{
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL3_C_REG,
+ LSM6DSL_PP_OD_MASK, open_drain);
+}
+
+int
+lsm6dsl_get_int_pp_od(struct lsm6dsl *lsm6dsl, bool *open_drain)
+{
+ *open_drain = (lsm6dsl->cfg_regs1.ctrl3_c & LSM6DSL_PP_OD_MASK) != 0;
+
+ return 0;
+}
+
+int
+lsm6dsl_set_latched_int(struct lsm6dsl *lsm6dsl, bool en)
+{
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_TAP_CFG_REG,
+ LSM6DSL_LIR_MASK, en);
+}
+
+int
+lsm6dsl_get_latched_int(struct lsm6dsl *lsm6dsl, uint8_t *en)
+{
+ *en = (lsm6dsl->cfg_regs2.tap_cfg & LSM6DSL_LIR_MASK) != 0;
+
+ return 0;
+}
+
+int
+lsm6dsl_set_map_int2_to_int1(struct lsm6dsl *lsm6dsl, bool en)
+{
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL4_C_REG,
+ LSM6DSL_INT2_ON_INT1_MASK, en);
+}
+
+int
+lsm6dsl_get_map_int2_to_int1(struct lsm6dsl *lsm6dsl, uint8_t *en)
+{
+ *en = (lsm6dsl->cfg_regs1.ctrl4_c & LSM6DSL_INT2_ON_INT1_MASK) != 0;
+
+ return 0;
+}
+
+int
+lsm6dsl_set_int_level(struct lsm6dsl *lsm6dsl, uint8_t level)
+{
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_CTRL3_C_REG,
+ LSM6DSL_H_LACTIVE_MASK, level ? 0 : 1);
+}
+
+int
+lsm6dsl_get_int_level(struct lsm6dsl *lsm6dsl, uint8_t *level)
+{
+ *level = (lsm6dsl->cfg_regs2.tap_cfg & LSM6DSL_H_LACTIVE_MASK) == 0;
+
+ return 0;
+}
+
+int
+lsm6dsl_clear_int_pin_cfg(struct lsm6dsl *lsm6dsl, uint8_t int_pin,
+ uint8_t int_mask)
+{
+ uint8_t reg;
+
+ switch (int_pin) {
+ case 0:
+ reg = LSM6DSL_MD1_CFG_REG;
+ break;
+ case 1:
+ reg = LSM6DSL_MD2_CFG_REG;
+ break;
+ default:
+ LSM6DSL_LOG_ERROR("Invalid int pin %d\n", int_pin);
+
+ return SYS_EINVAL;
+ }
+
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, reg, int_mask, LSM6DSL_DIS_BIT);
+}
+
+int
+lsm6dsl_clear_int(struct lsm6dsl *lsm6dsl, struct int_src_regs *int_src)
+{
+ int rc;
+ /*
+ * Interrupt status could have been read in single 4 byte I2C transaction, but
+ * if wake_up_src is read first D6D_IA bit from D6D_SRC is cleared
+ * and information about orientation change is lost.
+ */
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_D6D_SRC_REG, (uint8_t *)&int_src->d6d_src, 2);
+ if (rc == 0) {
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_WAKE_UP_SRC_REG, (uint8_t *)&int_src->wake_up_src, 2);
+ }
+ return rc;
+}
+
+int
+lsm6dsl_set_int_pin_cfg(struct lsm6dsl *lsm6dsl, uint8_t int_pin,
+ uint8_t int_mask)
+{
+ uint8_t reg;
+
+ switch (int_pin) {
+ case 0:
+ reg = LSM6DSL_MD1_CFG_REG;
+ break;
+ case 1:
+ reg = LSM6DSL_MD2_CFG_REG;
+ break;
+ default:
+ LSM6DSL_LOG_ERROR("Invalid int pin %d\n", int_pin);
+
+ return SYS_EINVAL;
+ }
+
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, reg, int_mask, LSM6DSL_EN_BIT);
+}
+
+int
+lsm6dsl_set_orientation(struct lsm6dsl *lsm6dsl,
+ const struct lsm6dsl_orientation_settings *cfg)
+{
+ uint8_t val = cfg->en_4d ? 0x4 : 0;
+ val |= (uint8_t)cfg->ths_6d;
+
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_TAP_THS_6D_REG,
+ LSM6DSL_D4D_EN_MASK | LSM6DSL_SIXD_THS_MASK, val);
+}
+
+int
+lsm6dsl_get_orientation_cfg(struct lsm6dsl *lsm6dsl,
+ struct lsm6dsl_orientation_settings *cfg)
+{
+ cfg->en_4d = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_cfg, LSM6DSL_D4D_EN_MASK);
+ cfg->ths_6d = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_cfg, LSM6DSL_SIXD_THS_MASK);
+
+ return 0;
+}
+
+int
+lsm6dsl_set_tap_cfg(struct lsm6dsl *lsm6dsl,
+ const struct lsm6dsl_tap_settings *cfg)
+{
+ int rc;
+ uint8_t val;
+
+ val = LSM6DSL_SHIFT_DATA_MASK(cfg->tap_ths, LSM6DSL_TAP_THS_MASK);
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_TAP_THS_6D_REG,
+ LSM6DSL_TAP_THS_MASK, val);
+ if (rc) {
+ goto end;
+ }
+
+ val = LSM6DSL_SHIFT_DATA_MASK(cfg->dur, LSM6DSL_DUR_MASK) |
+ LSM6DSL_SHIFT_DATA_MASK(cfg->quiet, LSM6DSL_QUIET_MASK) |
+ LSM6DSL_SHIFT_DATA_MASK(cfg->shock, LSM6DSL_SHOCK_MASK);
+
+ rc = lsm6dsl_write_reg(lsm6dsl, LSM6DSL_INT_DUR2_REG, val);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_WAKE_UP_THS_REG,
+ LSM6DSL_SINGLE_DOUBLE_TAP_MASK,
+ cfg->en_dtap);
+ if (rc) {
+ goto end;
+ }
+
+ val = cfg->en_x ? (LSM6DSL_TAP_X_EN_MASK >> 1) : 0;
+ val |= cfg->en_y ? (LSM6DSL_TAP_Y_EN_MASK >> 1) : 0;
+ val |= cfg->en_z ? (LSM6DSL_TAP_Z_EN_MASK >> 1) : 0;
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_TAP_CFG_REG,
+ LSM6DSL_TAP_XYZ_EN_MASK, val);
+end:
+ return rc;
+}
+
+int
+lsm6dsl_get_tap_cfg(struct lsm6dsl *lsm6dsl,
+ struct lsm6dsl_tap_settings *cfg)
+{
+ cfg->en_x = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_cfg, LSM6DSL_TAP_X_EN_MASK);
+ cfg->en_y = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_cfg, LSM6DSL_TAP_Y_EN_MASK);
+ cfg->en_z = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_cfg, LSM6DSL_TAP_Z_EN_MASK);
+
+ cfg->tap_ths = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_ths_6d, LSM6DSL_TAP_THS_MASK);
+
+ cfg->dur = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.int_dur2, LSM6DSL_DUR_MASK);
+ cfg->quiet = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.int_dur2, LSM6DSL_QUIET_MASK);
+ cfg->shock = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.int_dur2, LSM6DSL_SHOCK_MASK);
+
+ cfg->en_dtap = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.wake_up_ths, LSM6DSL_SINGLE_DOUBLE_TAP_MASK);
+
+ return 0;
+}
+
+int
+lsm6dsl_set_free_fall(struct lsm6dsl *lsm6dsl, struct lsm6dsl_ff_settings *ff)
+{
+ int rc;
+ uint8_t val;
+
+ val = LSM6DSL_SHIFT_DATA_MASK(ff->free_fall_dur, LSM6DSL_FF_DUR_MASK);
+ val |= LSM6DSL_SHIFT_DATA_MASK(ff->free_fall_ths, LSM6DSL_FF_THS_MASK);
+
+ rc = lsm6dsl_write_reg(lsm6dsl, LSM6DSL_FREE_FALL_REG, val);
+ if (rc == 0) {
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_WAKE_UP_DUR_REG,
+ LSM6DSL_FF_DUR5_MASK,
+ ff->free_fall_dur >> 5u);
+ }
+ return rc;
+}
+
+int
+lsm6dsl_get_free_fall(struct lsm6dsl *lsm6dsl, struct lsm6dsl_ff_settings *ff)
+{
+ ff->free_fall_dur = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.free_fall, LSM6DSL_FF_DUR_MASK) |
+ (LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.wake_up_dur, LSM6DSL_FF_DUR5_MASK) << 5);
+ ff->free_fall_ths = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.free_fall, LSM6DSL_FF_THS_MASK);
+
+ return 0;
+}
+
+int
+lsm6dsl_set_wake_up(struct lsm6dsl *lsm6dsl, struct lsm6dsl_wk_settings *wk)
+{
+ int rc;
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_WAKE_UP_THS_REG,
+ LSM6DSL_WK_THS_MASK, wk->wake_up_ths);
+ if (rc) {
+ return rc;
+ }
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_WAKE_UP_DUR_REG,
+ LSM6DSL_WAKE_DUR_MASK, wk->wake_up_dur);
+ if (rc) {
+ return rc;
+ }
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_WAKE_UP_DUR_REG,
+ LSM6DSL_SLEEP_DUR_MASK, wk->sleep_duration);
+ if (rc) {
+ return rc;
+ }
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_TAP_CFG_REG,
+ LSM6DSL_INACT_EN_MASK, wk->inactivity);
+ if (rc) {
+ return rc;
+ }
+
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_TAP_CFG_REG,
+ LSM6DSL_SLOPE_FDS_MASK, wk->hpf_slope);
+}
+
+int
+lsm6dsl_get_wake_up(struct lsm6dsl *lsm6dsl, struct lsm6dsl_wk_settings *wk)
+{
+ wk->wake_up_ths = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.wake_up_ths, LSM6DSL_WK_THS_MASK);
+ wk->wake_up_dur = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.wake_up_dur, LSM6DSL_WAKE_DUR_MASK);
+ wk->sleep_duration = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.wake_up_dur, LSM6DSL_SLEEP_DUR_MASK);
+ wk->hpf_slope = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_cfg, LSM6DSL_SLOPE_FDS_MASK);
+ wk->inactivity = LSM6DSL_DESHIFT_DATA_MASK(lsm6dsl->cfg_regs2.tap_cfg, LSM6DSL_INACT_EN_MASK);
+
+ return 0;
+}
+
+static void
+init_interrupt(struct lsm6dsl_int *interrupt, int pin, int active_level)
+{
+ os_error_t error;
+
+ /* Init semaphore for task to wait on when irq asleep */
+ error = os_sem_init(&interrupt->wait, 0);
+ assert(error == OS_OK);
+
+ interrupt->active = false;
+ interrupt->asleep = false;
+ interrupt->ints[0].host_pin = pin;
+ interrupt->ints[0].active = active_level;
+ interrupt->ints[0].device_pin = 0;
+}
+
+static void
+undo_interrupt(struct lsm6dsl_int *interrupt)
+{
+ os_sr_t sr;
+
+ OS_ENTER_CRITICAL(sr);
+ interrupt->active = false;
+ interrupt->asleep = false;
+ OS_EXIT_CRITICAL(sr);
+}
+
+/**
+ * Wait on interrupt->wait lock
+ *
+ * This call suspend task until wake_interrupt is called
+ *
+ * @param the interrupt structure
+ * @param int_num registered
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+wait_interrupt(struct lsm6dsl_int *interrupt, uint8_t int_num)
+{
+ os_sr_t sr;
+ bool wait;
+ os_error_t error;
+
+ OS_ENTER_CRITICAL(sr);
+
+ /* Check if we did not miss interrupt */
+ if (hal_gpio_read(interrupt->ints[int_num].host_pin) ==
+ interrupt->ints[int_num].active) {
+ OS_EXIT_CRITICAL(sr);
+ return OS_OK;
+ }
+
+ if (interrupt->active) {
+ interrupt->active = false;
+ wait = false;
+ } else {
+ interrupt->asleep = true;
+ wait = true;
+ }
+ OS_EXIT_CRITICAL(sr);
+
+ if (wait) {
+ error = os_sem_pend(&interrupt->wait, LSM6DSL_MAX_INT_WAIT);
+ if (error == OS_TIMEOUT) {
+ return error;
+ }
+ assert(error == OS_OK);
+ }
+
+ return OS_OK;
+}
+
+/**
+ * Wake tasks waiting on interrupt->wait lock
+ *
+ * This call resume task waiting on wake_interrupt lock
+ *
+ * @param the interrupt structure
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static void
+wake_interrupt(struct lsm6dsl_int *interrupt)
+{
+ os_sr_t sr;
+ os_error_t error;
+ bool wake;
+
+ OS_ENTER_CRITICAL(sr);
+ if (interrupt->asleep) {
+ interrupt->asleep = false;
+ wake = true;
+ } else {
+ interrupt->active = true;
+ wake = false;
+ }
+ OS_EXIT_CRITICAL(sr);
+
+ if (wake) {
+ /* Release semaphore to wait_interrupt routine */
+ error = os_sem_release(&interrupt->wait);
+ assert(error == OS_OK);
+ }
+}
+
+/**
+ * Wake tasks waiting on interrupt->wait lock
+ *
+ * This call resume task waiting on wake_interrupt lock
+ *
+ * @param the interrupt structure
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static void
+lsm6dsl_int_irq_handler(void *arg)
+{
+ struct lsm6dsl *lsm6dsl = arg;
+
+ if (lsm6dsl->pdd.interrupt) {
+ wake_interrupt(lsm6dsl->pdd.interrupt);
+ }
+
+ sensor_mgr_put_interrupt_evt(&lsm6dsl->sensor);
+}
+
+/**
+ * Register irq pin handler
+ *
+ * @param the lsm6dsl device
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+init_intpin(struct lsm6dsl *lsm6dsl)
+{
+ hal_gpio_irq_trig_t trig;
+ int rc;
+
+ if (lsm6dsl->intr.ints[0].host_pin < 0) {
+ LSM6DSL_LOG_ERROR("Interrupt pin not configured\n");
+
+ return SYS_EINVAL;
+ }
+
+ if (lsm6dsl->intr.ints[0].active) {
+ trig = HAL_GPIO_TRIG_RISING;
+ } else {
+ trig = HAL_GPIO_TRIG_FALLING;
+ }
+
+ rc = hal_gpio_irq_init(lsm6dsl->intr.ints[0].host_pin, lsm6dsl_int_irq_handler, lsm6dsl,
+ trig, HAL_GPIO_PULL_NONE);
+ if (rc) {
+ LSM6DSL_LOG_ERROR("Failed to initialise interrupt pin %d\n", lsm6dsl->intr.ints[0].host_pin);
+ }
+
+ lsm6dsl_set_int_level(lsm6dsl, lsm6dsl->intr.ints[0].active);
+
+ return rc;
+}
+
+/**
+ * Disable sensor interrupt
+ *
+ * @param sensor device
+ * @param int_mask interrupt bit
+ * @param int_num interrupt pin
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_disable_interrupt(struct sensor *sensor, uint8_t int_mask,
+ uint8_t int_num)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_pdd *pdd;
+
+ if (int_mask == 0) {
+ return SYS_EINVAL;
+ }
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ pdd = &lsm6dsl->pdd;
+
+ pdd->int_enable &= ~(int_mask << (int_num * 8));
+
+ /* disable int pin */
+ if (!pdd->int_enable) {
+ hal_gpio_irq_disable(lsm6dsl->intr.ints[0].host_pin);
+ }
+
+ /* update interrupt setup in device */
+ return lsm6dsl_clear_int_pin_cfg(lsm6dsl, int_num, int_mask);
+}
+
+/**
+ * Enable sensor interrupt
+ *
+ * @param sensor device
+ * @param int_to_enable interrupt bit
+ * @param int_num interrupt pin
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_enable_interrupt(struct sensor *sensor, uint8_t int_mask, uint8_t int_num)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_pdd *pdd;
+ struct int_src_regs int_src;
+ int rc;
+
+ if (!int_mask) {
+ return SYS_EINVAL;
+ }
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ pdd = &lsm6dsl->pdd;
+
+ rc = lsm6dsl_clear_int(lsm6dsl, &int_src);
+ if (rc) {
+ return rc;
+ }
+
+ /* if no interrupts are currently in use disable int pin */
+ if (!pdd->int_enable) {
+ hal_gpio_irq_enable(lsm6dsl->intr.ints[0].host_pin);
+ }
+
+ /* Save bitmask of enabled events interrupt */
+ pdd->int_enable |= (int_mask << (int_num * 8));
+
+ /* enable interrupt in device */
+ rc = lsm6dsl_set_int_pin_cfg(lsm6dsl, int_num, int_mask);
+ if (rc) {
+ lsm6dsl_disable_interrupt(sensor, int_mask, int_num);
+ }
+
+ return rc;
+}
+
+/**
+ * Disable sensor fifo interrupt
+ *
+ * @param sensor device
+ * @param lsm6dsl_cfg configuration
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+disable_fifo_interrupt(struct sensor *sensor, sensor_type_t type,
+ struct lsm6dsl_cfg *cfg)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_pdd *pdd;
+ uint8_t reg;
+ uint8_t int_pin;
+ int rc;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ pdd = &lsm6dsl->pdd;
+ int_pin = cfg->read.int_num;
+
+ /* Clear it in interrupt bitmask */
+ pdd->int_enable &= ~(LSM6DSL_INT_FIFO_TH_MASK << (int_pin * 8));
+
+ /* The last one closes the door */
+ if (!pdd->int_enable) {
+ hal_gpio_irq_disable(lsm6dsl->intr.ints[0].host_pin);
+ }
+
+ switch (int_pin) {
+ case 0:
+ reg = LSM6DSL_INT1_CTRL;
+ break;
+ case 1:
+ reg = LSM6DSL_INT2_CTRL;
+ break;
+ default:
+ LSM6DSL_LOG_ERROR("Invalid int pin %d\n", int_pin);
+ rc = SYS_EINVAL;
+ goto end;
+ }
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, reg, LSM6DSL_INT_FIFO_TH_MASK,
+ LSM6DSL_DIS_BIT);
+ if (rc) {
+ goto end;
+ }
+
+ if (type & SENSOR_TYPE_GYROSCOPE) {
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_FIFO_CTRL3_REG,
+ LSM6DSL_DEC_FIFO_GYRO_MASK, 0);
+ if (rc) {
+ goto end;
+ }
+ }
+
+ if (type & SENSOR_TYPE_ACCELEROMETER) {
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_FIFO_CTRL3_REG,
+ LSM6DSL_DEC_FIFO_XL_MASK, 0);
+ }
+
+end:
+ return rc;
+}
+
+static int
+lsm6dsl_enable_fifo_interrupt(struct sensor *sensor, sensor_type_t type,
+ struct lsm6dsl_cfg *cfg)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_pdd *pdd;
+ uint8_t reg;
+ uint8_t int_pin;
+ int rc;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ pdd = &lsm6dsl->pdd;
+ int_pin = cfg->read.int_num;
+
+ /* if no interrupts are currently in use enable int pin */
+ if (!pdd->int_enable) {
+ hal_gpio_irq_enable(lsm6dsl->intr.ints[0].host_pin);
+ }
+
+ switch (int_pin) {
+ case 0:
+ reg = LSM6DSL_INT1_CTRL;
+ break;
+ case 1:
+ reg = LSM6DSL_INT2_CTRL;
+ break;
+ default:
+ rc = SYS_EINVAL;
+ LSM6DSL_LOG_ERROR("Invalid int pin %d\n", int_pin);
+ goto err;
+ }
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, reg, LSM6DSL_INT_FIFO_TH_MASK,
+ LSM6DSL_EN_BIT);
+ if (rc) {
+ goto err;
+ }
+
+ /* Update enabled interrupt bitmask */
+ pdd->int_enable |= (LSM6DSL_INT_FIFO_TH_MASK << (int_pin * 8));
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_FIFO_CTRL3_REG,
+ LSM6DSL_DEC_FIFO_GYRO_MASK,
+ type & SENSOR_TYPE_GYROSCOPE ? 1 : 0);
+ if (rc) {
+ goto err;
+ }
+
+ rc = lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_FIFO_CTRL3_REG,
+ LSM6DSL_DEC_FIFO_XL_MASK,
+ type & SENSOR_TYPE_ACCELEROMETER ? 1 : 0);
+ if (rc) {
+ goto err;
+ }
+
+ return rc;
+
+err:
+ disable_fifo_interrupt(sensor, type, cfg);
+
+ return rc;
+}
+
+/**
+ * Enable embedded function interrupt
+ *
+ * @param sensor device
+ * @param enable/disable embedded function interrupt
+ *
+ * @return 0 on success, non-zero on error
+ */
+static int
+enable_embedded_interrupt(struct sensor *sensor, bool en)
+{
+ struct lsm6dsl *lsm6dsl;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+
+ return lsm6dsl_write_reg_bit_field(lsm6dsl, LSM6DSL_TAP_CFG_REG,
+ LSM6DSL_INTERRUPTS_ENABLE_MASK, en);
+}
+
+/**
+ * Get temperature data
+ *
+ * If both the accelerometer and the gyroscope sensors are in Power-Down mode,
+ * the temperature sensor is off.
+ * The maximum output data rate of the temperature sensor is 52 Hz and its
+ * value depends on how the accelerometer and gyroscope sensors are configured:
+ *
+ * - If the gyroscope is in Power-Down mode:
+ * - If the accelerometer is configured in Ultra-Low-Power or Low-Power mode
+ * and its ODR is lower than 52 Hz, the temperature data rate is equal to
+ * the configured accelerometer ODR;
+ * - The temperature data rate is equal to 52 Hz for all other accelerometer
+ * configurations.
+ * - If the gyroscope is not in Power-Down mode, the temperature data rate is
+ * equal to 52 Hz, regardless of the accelerometer and gyroscope
+ * configuration.
+ *
+ * @param lsm6dsl The device
+ * @param pointer to the temperature data structure
+ *
+ * @return 0 on success, non-zero on error
+ */
+static int
+lsm6dsl_get_temp_data(struct lsm6dsl *lsm6dsl,
+ struct sensor_temp_data *std)
+{
+ int rc;
+ uint8_t temp[2];
+
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_OUT_TEMP_L_REG,
+ temp, sizeof(temp));
+ if (rc) {
+ return rc;
+ }
+
+ std->std_temp = (int16_t)((temp[1] << 8) | temp[0]) / 256.0f + 25.0f;
+ std->std_temp_is_valid = 1;
+
+ return 0;
+}
+
+/**
+ * Gets a raw sensor data from the acc/gyro/temp sensor.
+ *
+ * @param lsm6dsl The device
+ * @param Sensor type
+ * @param axis data
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static inline int
+lsm6dsl_get_ag_raw_data(struct lsm6dsl *lsm6dsl,
+ sensor_type_t type, int16_t *data)
+{
+ int rc;
+ uint8_t reg;
+ uint8_t payload[6];
+
+ /* Set data out base register for sensor */
+ reg = LSM6DSL_GET_OUT_REG(type);
+ rc = lsm6dsl_read(lsm6dsl, reg, payload, 6);
+ if (rc) {
+ return rc;
+ }
+
+ /*
+ * Both acc and gyro data are represented as 16-bit word in
+ * two’s complement
+ */
+ data[0] = (int16_t)(payload[0] | (payload[1] << 8));
+ data[1] = (int16_t)(payload[2] | (payload[3] << 8));
+ data[2] = (int16_t)(payload[4] | (payload[5] << 8));
+
+ return 0;
+}
+
+int
+lsm6dsl_run_self_test(struct lsm6dsl *lsm6dsl, int *result)
+{
+ int rc;
+ int min, max;
+ int16_t data[3];
+ int diff[3] = { 0 };
+ uint8_t i;
+ uint8_t prev_config[10];
+ uint8_t st_xl_config[] = {
+ 0x38, 0x00, 0x44, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00
+ };
+ uint8_t st_g_config[] = {
+ 0x00, 0x5c, 0x44, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00
+ };
+ uint8_t val;
+
+ *result = 0;
+
+ /* Configure xl as for AN5040 */
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_CTRL1_XL_REG,
+ st_xl_config, sizeof(st_xl_config));
+ if (rc) {
+ goto end;
+ }
+
+ /* Wait 100ms for stable output data */
+ os_time_delay(OS_TICKS_PER_SEC / 10);
+
+ /* Read and discard first data sample */
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_ACCELEROMETER, data);
+ if (rc) {
+ goto end;
+ }
+
+ /* Take 5 samples */
+ for (i = 0; i < 5; i++) {
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_ACCELEROMETER, data);
+ if (rc) {
+ goto end;
+ }
+ diff[0] += data[0];
+ diff[1] += data[1];
+ diff[2] += data[2];
+
+ /* Wait at least 1/52 s ~ 20 ms */
+ os_time_delay(OS_TICKS_PER_SEC / 52);
+ }
+
+ /* Enable positive sign self-test mode */
+ val = LSM6DSL_XL_SELF_TEST_POS_SIGN;
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_CTRL5_C_REG, &val, 1);
+ if (rc) {
+ goto end;
+ }
+
+ /* Wait 100ms for stable output data */
+ os_time_delay(OS_TICKS_PER_SEC / 10);
+
+ /* Read and discard first data sample */
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_ACCELEROMETER, data);
+ if (rc) {
+ goto end;
+ }
+
+ /* Take 5 samples */
+ for (i = 0; i < 5; i++) {
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_ACCELEROMETER, data);
+ if (rc) {
+ goto end;
+ }
+
+ diff[0] -= data[0];
+ diff[1] -= data[1];
+ diff[2] -= data[2];
+
+ /* Wait at least 1/52 s ~ 20 ms */
+ os_time_delay(OS_TICKS_PER_SEC / 52);
+ }
+
+ /* Restore register configuration */
+ val = 0;
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_CTRL1_XL_REG, &val, 1);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_CTRL5_C_REG, &val, 1);
+ if (rc) {
+ goto end;
+ }
+
+ /* Compare values to thresholds */
+ min = LSM6DSL_XL_ST_MIN * 5 * 2;
+ max = LSM6DSL_XL_ST_MAX * 5 * 2;
+ for (i = 0; i < 3; i++) {
+ /* ABS */
+ if (diff[i] < 0)
+ diff[i] = -diff[i];
+
+ if ((diff[i] < min) || (diff[i] > max)) {
+ *result |= 1;
+ }
+ }
+
+ /* Configure gyro as for AN5040 */
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_CTRL1_XL_REG,
+ st_g_config, sizeof(st_g_config));
+ if (rc) {
+ goto end;
+ }
+
+ /* Wait 150ms for stable output data */
+ os_time_delay(OS_TICKS_PER_SEC * 150 / 1000);
+
+ /* Read and discard first gyro data sample */
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_GYROSCOPE, data);
+ if (rc) {
+ goto end;
+ }
+
+ memset(diff, 0, sizeof(diff));
+
+ /* Take 5 samples */
+ for (i = 0; i < 5; i++) {
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_GYROSCOPE, data);
+ if (rc) {
+ return rc;
+ }
+
+ diff[0] += data[0];
+ diff[1] += data[1];
+ diff[2] += data[2];
+
+ /* Wait at least 1/208 s ~ 5 ms */
+ os_time_delay(OS_TICKS_PER_SEC / 208);
+ }
+
+ /* Enable positive sign self-test mode */
+ val = 0x04;
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_CTRL5_C_REG, &val, 1);
+ if (rc) {
+ goto end;
+ }
+
+ /* Wait 50ms for stable output data */
+ os_time_delay(OS_TICKS_PER_SEC * 50 / 1000);
+
+ /* Read and discard first data sample */
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_GYROSCOPE, data);
+ if (rc) {
+ goto end;
+ }
+
+ /* Take 5 samples */
+ for (i = 0; i < 5; i++) {
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, SENSOR_TYPE_GYROSCOPE, data);
+ if (rc) {
+ return rc;
+ }
+ diff[0] -= data[0];
+ diff[1] -= data[1];
+ diff[2] -= data[2];
+
+ /* Wait at least 1/208 s ~ 20 ms */
+ os_time_delay(OS_TICKS_PER_SEC / 208);
+ }
+
+ /* Restore register configuration */
+ rc = lsm6dsl_write(lsm6dsl, LSM6DSL_CTRL1_XL_REG,
+ lsm6dsl_shadow_reg(lsm6dsl, LSM6DSL_CTRL1_XL_REG),
+ sizeof(prev_config));
+ if (rc) {
+ goto end;
+ }
+
+ /* Compare values to thresholds */
+ min = LSM6DSL_G_ST_MIN * 5 * 2;
+ max = LSM6DSL_G_ST_MAX * 5 * 2;
+ for (i = 0; i < 3; i++) {
+ /* ABS */
+ if (diff[i] < 0)
+ diff[i] = -diff[i];
+ if ((diff[i] < min) || (diff[i] > max)) {
+ *result |= 2;
+ }
+ }
+end:
+ return rc;
+}
+
+int
+lsm6dsl_get_ag_data(struct lsm6dsl *lsm6dsl, sensor_type_t type, void *data)
+{
+ int rc;
+ int16_t x_y_z[3];
+ struct sensor_accel_data *sad = data;
+ float mult;
+
+ rc = lsm6dsl_get_ag_raw_data(lsm6dsl, type, (void *)x_y_z);
+ if (rc) {
+ return rc;
+ }
+
+ switch (type) {
+ case SENSOR_TYPE_GYROSCOPE:
+ mult = lsm6dsl->gyro_mult;
+ break;
+ case SENSOR_TYPE_ACCELEROMETER:
+ mult = lsm6dsl->acc_mult;
+ break;
+ default:
+ LSM6DSL_LOG_ERROR("Invalid sensor type: %d\n", (int)type);
+ rc = SYS_EINVAL;
+ goto end;
+ }
+
+ sad->sad_x = (float)x_y_z[0] * mult;
+ sad->sad_y = (float)x_y_z[1] * mult;
+ sad->sad_z = (float)x_y_z[2] * mult;
+
+ sad->sad_x_is_valid = 1;
+ sad->sad_y_is_valid = 1;
+ sad->sad_z_is_valid = 1;
+end:
+ return rc;
+}
+
+static int
+lsm6dsl_drop_fifo_samples(struct lsm6dsl *lsm6dsl, uint16_t samples_to_drop)
+{
+ int rc = 0;
+ uint8_t sample_buffer[2];
+
+ while (rc == 0 && samples_to_drop != 0) {
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_FIFO_DATA_OUT_L_REG, sample_buffer, 2);
+ --samples_to_drop;
+ }
+
+ return rc;
+}
+
+static int
+lsm6dsl_read_3_samples_from_fifo(struct lsm6dsl *lsm6dsl, uint8_t buf[6])
+{
+ int rc;
+
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_FIFO_DATA_OUT_L_REG, buf, 2);
+ if (rc) {
+ goto end;
+ }
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_FIFO_DATA_OUT_L_REG, buf + 2, 2);
+ if (rc) {
+ goto end;
+ }
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_FIFO_DATA_OUT_L_REG, buf + 4, 2);
+end:
+ return rc;
+}
+
+/**
+ * Gets a data sample of acc/gyro/temp sensor from FIFO.
+ *
+ * @param lsm6dsl The device
+ * @param axis data
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lsm6dsl_read_fifo(struct lsm6dsl *lsm6dsl,
+ sensor_type_t sensor_type,
+ struct sensor_accel_data *sample_data, sensor_type_t *sample_type,
+ uint16_t *pattern)
+{
+ int rc;
+ uint8_t sample_buf[6];
+ int16_t x, y, z;
+ float mult;
+
+ /*
+ * Only gyroscope and accelerometer supported
+ * If pattern == 3 - next sample is from accelerometer
+ * If pattern == 0 - next sample is from gyro is it is enabled,
+ * otherwise from accelerometer
+ */
+ assert(*pattern == 0 || *pattern == 3);
+
+ if (sensor_type & SENSOR_TYPE_GYROSCOPE || *pattern == 0) {
+ *sample_type = SENSOR_TYPE_GYROSCOPE;
+ mult = lsm6dsl->gyro_mult;
+ if (sensor_type & SENSOR_TYPE_ACCELEROMETER) {
+ /* Next sample is form accelerometer */
+ *pattern = 3;
+ }
+ } else {
+ mult = lsm6dsl->acc_mult;
+ *sample_type = SENSOR_TYPE_ACCELEROMETER;
+ *pattern = 0;
+ }
+
+ rc = lsm6dsl_read_3_samples_from_fifo(lsm6dsl, sample_buf);
+ if (rc) {
+ return rc;
+ }
+
+ /*
+ * Both acc and gyro data are represented as 16-bit word in
+ * two’s complement
+ */
+ x = (int16_t)(sample_buf[0] | (sample_buf[1] << 8));
+ y = (int16_t)(sample_buf[2] | (sample_buf[3] << 8));
+ z = (int16_t)(sample_buf[4] | (sample_buf[5] << 8));
+
+
+ sample_data->sad_x = (float)x * mult;
+ sample_data->sad_y = (float)y * mult;
+ sample_data->sad_z = (float)z * mult;
+
+ sample_data->sad_x_is_valid = 1;
+ sample_data->sad_y_is_valid = 1;
+ sample_data->sad_z_is_valid = 1;
+
+ return 0;
+}
+
+/**
+ * Expects to be called back through os_dev_create().
+ *
+ * @param The device object associated with this sensor
+ * @param Argument passed to OS device init, unused
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+int
+lsm6dsl_init(struct os_dev *dev, void *arg)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_create_dev_cfg *cfg = arg;
+ struct sensor *sensor;
+ int rc;
+
+ if (!arg || !dev) {
+ rc = SYS_ENODEV;
+ goto end;
+ }
+
+ lsm6dsl = (struct lsm6dsl *)dev;
+ lsm6dsl->cfg.lc_s_mask = SENSOR_TYPE_ALL;
+ sensor = &lsm6dsl->sensor;
+
+ /* Initialise the stats entry */
+ rc = stats_init(
+ STATS_HDR(g_lsm6dsl_stats),
+ STATS_SIZE_INIT_PARMS(g_lsm6dsl_stats, STATS_SIZE_32),
+ STATS_NAME_INIT_PARMS(lsm6dsl_stat_section));
+ SYSINIT_PANIC_ASSERT(rc == 0);
+
+ /* Register the entry with the stats registry */
+ rc = stats_register(dev->od_name, STATS_HDR(g_lsm6dsl_stats));
+ SYSINIT_PANIC_ASSERT(rc == 0);
+
+ rc = sensor_init(sensor, dev);
+ if (rc) {
+ goto end;
+ }
+
+ /* Add the IMU driver plus temperature sensor */
+ rc = sensor_set_driver(sensor,
+ SENSOR_TYPE_ACCELEROMETER |
+ SENSOR_TYPE_GYROSCOPE |
+ SENSOR_TYPE_TEMPERATURE,
+ (struct sensor_driver *)&g_lsm6dsl_sensor_driver);
+ if (rc) {
+ goto end;
+ }
+
+#if !MYNEWT_VAL(BUS_DRIVER_PRESENT)
+ /* Set the interface */
+ rc = sensor_set_interface(sensor, &cfg->itf);
+ if (rc) {
+ goto end;
+ }
+#endif
+
+ rc = sensor_mgr_register(sensor);
+ if (rc) {
+ goto end;
+ }
+
+ init_interrupt(&lsm6dsl->intr, cfg->int_pin, cfg->int_active_level);
+
+ lsm6dsl->pdd.notify_ctx.snec_sensor = sensor;
+ lsm6dsl->pdd.interrupt = NULL;
+
+ rc = init_intpin(lsm6dsl);
+
+end:
+ return rc;
+}
+
+/**
+ * Read data samples from FIFO.
+ *
+ * The sensor_type can be a bitmask like this
+ * (SENSOR_TYPE_ACCELEROMETER | SENSOR_TYPE_GYROSCOPE)
+ *
+ * @param sensor The sensor object
+ * @param type sensor type bitmask
+ * @param data_func callback register data function
+ * @param data_arg function arguments
+ * @param timeout
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_stream_read(struct sensor *sensor,
+ sensor_type_t sensor_type,
+ sensor_data_func_t read_func,
+ void *data_arg,
+ uint32_t time_ms)
+{
+ struct lsm6dsl_pdd *pdd;
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_cfg *cfg;
+ os_time_t time_ticks;
+ os_time_t stop_ticks = 0;
+ uint16_t fifo_samples;
+ int rc;
+ struct sensor_accel_data sad;
+ sensor_type_t r_type;
+ /* combined FIFO_STATUS3 and FIFO_STATUS4 */
+ uint16_t pattern;
+
+ /* Temperature reading not supported in FIFO */
+ if (!(sensor_type & SENSOR_TYPE_ACCELEROMETER) &&
+ !(sensor_type & SENSOR_TYPE_GYROSCOPE)) {
+ return SYS_EINVAL;
+ }
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ pdd = &lsm6dsl->pdd;
+ cfg = &lsm6dsl->cfg;
+
+ if (cfg->read.mode != LSM6DSL_READ_STREAM) {
+ return SYS_EINVAL;
+ }
+
+ undo_interrupt(&lsm6dsl->intr);
+
+ if (pdd->interrupt) {
+ return SYS_EBUSY;
+ }
+
+ /* Enable interrupt */
+ pdd->interrupt = &lsm6dsl->intr;
+
+ /* Set FIFO to configuration value */
+ rc = lsm6dsl_set_fifo_mode(lsm6dsl, cfg->fifo.mode);
+ if (rc) {
+ goto err;
+ }
+
+ rc = lsm6dsl_enable_fifo_interrupt(sensor, sensor_type, cfg);
+ if (rc) {
+ goto err;
+ }
+
+ if (time_ms > 0) {
+ rc = os_time_ms_to_ticks(time_ms, &time_ticks);
+ if (rc) {
+ goto err;
+ }
+ stop_ticks = os_time_get() + time_ticks;
+ }
+
+ for (;;) {
+ /* Force at least one read for cases when fifo is disabled */
+ rc = wait_interrupt(&lsm6dsl->intr, cfg->read.int_num);
+ if (rc) {
+ goto err;
+ }
+
+ rc = lsm6dsl_get_fifo_samples(lsm6dsl, &fifo_samples);
+ if (rc) {
+ goto err;
+ }
+
+ if (fifo_samples) {
+ rc = lsm6dsl_get_fifo_pattern(lsm6dsl, &pattern);
+ if (rc) {
+ goto err;
+ }
+
+ rc = lsm6dsl_drop_fifo_samples(lsm6dsl, (6 - pattern) % 3);
+ if (rc) {
+ goto err;
+ }
+ }
+
+ while (fifo_samples >= 3) {
+ /* Read all fifo samples */
+ rc = lsm6dsl_read_fifo(lsm6dsl, sensor_type, &sad, &r_type, &pattern);
+ if (rc) {
+ goto err;
+ }
+
+ if ((sensor_type & r_type) == r_type) {
+ rc = read_func(sensor, data_arg, &sad, r_type);
+ if (rc) {
+ goto err;
+ }
+
+ }
+ fifo_samples -= 3;
+ }
+
+ if (time_ms > 0 && OS_TIME_TICK_GT(os_time_get(), stop_ticks)) {
+ break;
+ }
+ }
+
+err:
+ /* Disable FIFO */
+ rc |= lsm6dsl_set_fifo_mode(lsm6dsl, LSM6DSL_FIFO_MODE_BYPASS_VAL);
+
+ /* Disable interrupt */
+ pdd->interrupt = NULL;
+
+ rc |= disable_fifo_interrupt(sensor, sensor_type, cfg);
+
+ return rc;
+}
+
+/**
+ * Single sensor read
+ *
+ * @param sensor The sensor object
+ * @param type sensor type (acc, gyro etc)
+ * @param data_func callback register data function
+ * @param data_arg function arguments
+ * @param timeout
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_poll_read(struct sensor *sensor, sensor_type_t type,
+ sensor_data_func_t data_func, void *data_arg,
+ uint32_t timeout)
+{
+ int rc = 0;
+ struct lsm6dsl *lsm6dsl;
+ struct sensor_accel_data sad;
+ struct sensor_temp_data std;
+
+ /* Check if requested sensor type is supported */
+ if (!(type & SENSOR_TYPE_ACCELEROMETER) &&
+ !(type & SENSOR_TYPE_GYROSCOPE) &&
+ !(type & SENSOR_TYPE_TEMPERATURE)) {
+ rc = SYS_EINVAL;
+ goto end;
+ }
+
+ /* Retrieve configuration from device for sensitivity */
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+
+ if (type & (SENSOR_TYPE_ACCELEROMETER | SENSOR_TYPE_GYROSCOPE)) {
+
+ /* Acc and Gyro data can share the same data structure */
+ rc = lsm6dsl_get_ag_data(lsm6dsl, type, &sad);
+ if (rc) {
+ goto end;
+ }
+
+ rc = data_func(sensor, data_arg, &sad, type);
+ if (rc) {
+ goto end;
+ }
+ }
+
+ if (type & SENSOR_TYPE_TEMPERATURE) {
+
+ rc = lsm6dsl_get_temp_data(lsm6dsl, &std);
+ if (rc) {
+ goto end;
+ }
+
+ /* Call data function */
+ rc = data_func(sensor, data_arg, &std, SENSOR_TYPE_TEMPERATURE);
+ }
+
+end:
+ return rc;
+}
+
+/**
+ * Read sensor data
+ *
+ * @param sensor The sensor object
+ * @param type sensor type (acc, gyro etc)
+ * @param data_func callback register data function
+ * @param data_arg function arguments
+ * @param timeout
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_sensor_read(struct sensor *sensor, sensor_type_t type,
+ sensor_data_func_t data_func, void *data_arg,
+ uint32_t timeout)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_cfg *cfg;
+
+ /* Check if requested sensor type is supported */
+ if (!(type & SENSOR_TYPE_ACCELEROMETER) &&
+ !(type & SENSOR_TYPE_GYROSCOPE) &&
+ !(type & SENSOR_TYPE_TEMPERATURE)) {
+ return SYS_EINVAL;
+ }
+
+ /* Retrieve configuration from device for sensitivity */
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ cfg = &lsm6dsl->cfg;
+
+ if (cfg->read.mode == LSM6DSL_READ_POLL) {
+ return lsm6dsl_poll_read(sensor, type, data_func, data_arg, timeout);
+ }
+
+ return lsm6dsl_stream_read(sensor, type, data_func, data_arg, timeout);
+}
+
+static struct lsm6dsl_notif_cfg *
+lsm6dsl_find_notify_cfg(struct lsm6dsl *lsm6dsl, sensor_event_type_t event)
+{
+ int i;
+ struct lsm6dsl_notif_cfg *notify_cfg = NULL;
+ struct lsm6dsl_cfg *cfg = &lsm6dsl->cfg;
+
+ for (i = 0; i < cfg->notify_cfg_count; i++) {
+ if (event == cfg->notify_cfg[i].event) {
+ notify_cfg = &cfg->notify_cfg[i];
+ break;
+ }
+ }
+
+ return notify_cfg;
+}
+
+static int
+lsm6dsl_notify(struct lsm6dsl *lsm6dsl, uint8_t src,
+ sensor_event_type_t event_type)
+{
+ struct lsm6dsl_notif_cfg *notify_cfg;
+
+ notify_cfg = lsm6dsl_find_notify_cfg(lsm6dsl, event_type);
+ if (!notify_cfg) {
+ return SYS_EINVAL;
+ }
+
+ if (src & notify_cfg->int_mask) {
+ sensor_mgr_put_notify_evt(&lsm6dsl->pdd.notify_ctx, event_type);
+ return 0;
+ }
+
+ return -1;
+}
+
+static void
+lsm6dsl_inc_notif_stats(sensor_event_type_t event)
+{
+#if MYNEWT_VAL(LSM6DSL_NOTIF_STATS)
+ switch (event) {
+ case SENSOR_EVENT_TYPE_SINGLE_TAP:
+ STATS_INC(g_lsm6dsl_stats, single_tap_notify);
+ break;
+ case SENSOR_EVENT_TYPE_DOUBLE_TAP:
+ STATS_INC(g_lsm6dsl_stats, double_tap_notify);
+ break;
+ case SENSOR_EVENT_TYPE_ORIENT_CHANGE:
+ STATS_INC(g_lsm6dsl_stats, orientation_notify);
+ break;
+ case SENSOR_EVENT_TYPE_SLEEP:
+ STATS_INC(g_lsm6dsl_stats, sleep_notify);
+ break;
+ case SENSOR_EVENT_TYPE_WAKEUP:
+ STATS_INC(g_lsm6dsl_stats, wakeup_notify);
+ break;
+ case SENSOR_EVENT_TYPE_FREE_FALL:
+ STATS_INC(g_lsm6dsl_stats, free_fall_notify);
+ break;
+ default:
+ break;
+ }
+#endif /* LSM6DSL_NOTIF_STATS */
+}
+
+/**
+ * Manage events from sensor
+ *
+ * @param sensor The sensor object
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_sensor_handle_interrupt(struct sensor *sensor)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct int_src_regs int_src;
+ int rc;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+
+ rc = lsm6dsl_clear_int(lsm6dsl, &int_src);
+ if (rc) {
+ LSM6DSL_LOG_ERROR("Could not read int src err=0x%02x\n", rc);
+ return rc;
+ }
+
+ rc = lsm6dsl_notify(lsm6dsl, int_src.tap_src, SENSOR_EVENT_TYPE_SINGLE_TAP);
+ if (!rc) {
+ lsm6dsl_inc_notif_stats(SENSOR_EVENT_TYPE_SINGLE_TAP);
+ }
+
+ rc = lsm6dsl_notify(lsm6dsl, int_src.tap_src, SENSOR_EVENT_TYPE_DOUBLE_TAP);
+ if (!rc) {
+ lsm6dsl_inc_notif_stats(SENSOR_EVENT_TYPE_DOUBLE_TAP);
+ }
+
+ rc = lsm6dsl_notify(lsm6dsl, int_src.wake_up_src, SENSOR_EVENT_TYPE_FREE_FALL);
+ if (!rc) {
+ lsm6dsl_inc_notif_stats(SENSOR_EVENT_TYPE_FREE_FALL);
+ }
+
+ rc = lsm6dsl_notify(lsm6dsl, int_src.wake_up_src, SENSOR_EVENT_TYPE_WAKEUP);
+ if (!rc) {
+ lsm6dsl_inc_notif_stats(SENSOR_EVENT_TYPE_WAKEUP);
+ }
+
+ rc = lsm6dsl_notify(lsm6dsl, int_src.wake_up_src, SENSOR_EVENT_TYPE_SLEEP);
+ if (!rc) {
+ lsm6dsl_inc_notif_stats(SENSOR_EVENT_TYPE_SLEEP);
+ }
+
+ rc = lsm6dsl_notify(lsm6dsl, int_src.d6d_src, SENSOR_EVENT_TYPE_ORIENT_CHANGE);
+ if (!rc) {
+ lsm6dsl_inc_notif_stats(SENSOR_EVENT_TYPE_ORIENT_CHANGE);
+ }
+
+ return rc;
+}
+
+/**
+ * Find registered events in available event list
+ *
+ * @param event event type to find
+ * @param int_cfg interrupt bit configuration
+ * @param int_reg interrupt register
+ * @param int_num interrupt number
+ * @param cfg lsm6dsl_cfg configuration structure
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_find_int_by_event(sensor_event_type_t event, uint8_t *int_en,
+ uint8_t *int_num, struct lsm6dsl_cfg *cfg)
+{
+ int i;
+
+ *int_num = 0;
+ *int_en = 0;
+
+ if (!cfg) {
+ return SYS_EINVAL;
+ }
+
+ /* Search in enabled event list */
+ for (i = 0; i < cfg->notify_cfg_count; i++) {
+ if (event == cfg->notify_cfg[i].event) {
+ *int_en = cfg->notify_cfg[i].int_en;
+ *int_num = cfg->notify_cfg[i].int_num;
+ break;
+ }
+ }
+
+ if (i == cfg->notify_cfg_count) {
+ return SYS_EINVAL;
+ }
+
+ return 0;
+}
+
+/**
+ * Reset sensor
+ *
+ * @param sensor sensor object
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_sensor_reset(struct sensor *sensor)
+{
+ struct lsm6dsl *lsm6dsl;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+
+ return lsm6dsl_reset(lsm6dsl);
+}
+
+/**
+ * Enable notification event
+ *
+ * @param sensor sensor object
+ * @param event event mask
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_sensor_set_notification(struct sensor *sensor,
+ sensor_event_type_t event)
+{
+ struct lsm6dsl *lsm6dsl;
+ struct lsm6dsl_pdd *pdd;
+ uint8_t int_num;
+ uint8_t int_mask;
+ int rc;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ pdd = &lsm6dsl->pdd;
+
+ rc = lsm6dsl_find_int_by_event(event, &int_mask, &int_num, &lsm6dsl->cfg);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_enable_interrupt(sensor, int_mask, int_num);
+ if (rc) {
+ goto end;
+ }
+
+ pdd->notify_ctx.snec_evtype |= event;
+
+ if (pdd->notify_ctx.snec_evtype) {
+ rc = enable_embedded_interrupt(sensor, 1);
+ }
+
+end:
+ return rc;
+}
+
+/**
+ * Disable notification event
+ *
+ * @param sensor sensor object
+ * @param event event mask
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_sensor_unset_notification(struct sensor *sensor, sensor_event_type_t event)
+{
+ struct lsm6dsl *lsm6dsl;
+ uint8_t int_num;
+ uint8_t int_mask;
+ int rc;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+ lsm6dsl->pdd.notify_ctx.snec_evtype &= ~event;
+
+ rc = lsm6dsl_find_int_by_event(event, &int_mask, &int_num, &lsm6dsl->cfg);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_disable_interrupt(sensor, int_mask, int_num);
+ if (rc) {
+ goto end;
+ }
+
+ if (lsm6dsl->pdd.notify_ctx.snec_evtype) {
+ rc = enable_embedded_interrupt(sensor, 0);
+ }
+
+end:
+ return rc;
+}
+
+/**
+ * Sensor driver get_config implementation
+ *
+ * @param sensor sensor object
+ * @param type type of sensor
+ * @param cfg sensor_cfg object
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_sensor_get_config(struct sensor *sensor, sensor_type_t type,
+ struct sensor_cfg *cfg)
+{
+ if ((type != SENSOR_TYPE_ACCELEROMETER) &&
+ (type != SENSOR_TYPE_GYROSCOPE) &&
+ (type != SENSOR_TYPE_TEMPERATURE)) {
+ return SYS_EINVAL;
+ }
+
+ if (type != SENSOR_TYPE_TEMPERATURE) {
+ cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT_TRIPLET;
+ } else {
+ cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT;
+ }
+
+ return 0;
+}
+
+/**
+ * Read notification event enabled
+ *
+ * @param sensor object
+ * @param cfg lsm6dsl_cfg object
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+static int
+lsm6dsl_sensor_set_config(struct sensor *sensor, void *cfg)
+{
+ struct lsm6dsl *lsm6dsl;
+
+ lsm6dsl = (struct lsm6dsl *)SENSOR_GET_DEVICE(sensor);
+
+ return lsm6dsl_config(lsm6dsl, (struct lsm6dsl_cfg*)cfg);
+}
+
+/**
+ * Configure the sensor
+ *
+ * @param lsm6dsl The device
+ * @param cfg device config
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+int
+lsm6dsl_config(struct lsm6dsl *lsm6dsl, struct lsm6dsl_cfg *cfg)
+{
+ int rc;
+ uint8_t chip_id;
+ struct sensor *sensor;
+
+ sensor = &(lsm6dsl->sensor);
+
+ rc = lsm6dsl_get_chip_id(lsm6dsl, &chip_id);
+ if (rc) {
+ goto end;
+ }
+
+ if (chip_id != LSM6DSL_WHO_AM_I) {
+ rc = SYS_EINVAL;
+ goto end;
+ }
+
+ rc = lsm6dsl_reset(lsm6dsl);
+ if (rc) {
+ goto end;
+ }
+
+ /* Cache all registers */
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_FUNC_CFG_ACCESS_REG,
+ &lsm6dsl->cfg_regs1.func_cfg_access, sizeof(lsm6dsl->cfg_regs1));
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_read(lsm6dsl, LSM6DSL_TAP_CFG_REG,
+ &lsm6dsl->cfg_regs2.tap_cfg, sizeof(lsm6dsl->cfg_regs2));
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_bdu(lsm6dsl, LSM6DSL_EN_BIT);
+ if (rc) {
+ goto end;
+ }
+
+ lsm6dsl->cfg = *cfg;
+
+ assert(cfg->gyro_fs >= LSM6DSL_GYRO_FS_250DPS && cfg->gyro_fs < LSM6DSL_GYRO_FS_2000DPS);
+ assert(cfg->acc_fs >= LSM6DSL_ACCEL_FS_2G && cfg->acc_fs <= LSM6DSL_ACCEL_FS_8G);
+ rc = lsm6dsl_set_gyro_full_scale(lsm6dsl, cfg->gyro_fs);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_acc_full_scale(lsm6dsl, cfg->acc_fs);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_gyro_rate(lsm6dsl, cfg->gyro_rate);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_acc_rate(lsm6dsl, cfg->acc_rate);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_offsets(lsm6dsl, 0, 0, 0, LSM6DSL_USER_WEIGHT_LO);
+ if (rc) {
+ goto end;
+ }
+
+ /*
+ * Disable FIFO by default
+ * Save FIFO default configuration value to be used later on
+ */
+ rc = lsm6dsl_set_fifo_mode(lsm6dsl, LSM6DSL_FIFO_MODE_BYPASS_VAL);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_fifo_watermark(lsm6dsl, cfg->fifo.wtm);
+ if (rc) {
+ goto end;
+ }
+
+ /* Add embedded gesture configuration */
+ rc = lsm6dsl_set_wake_up(lsm6dsl, &cfg->wk);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_free_fall(lsm6dsl, &cfg->ff);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_tap_cfg(lsm6dsl, &cfg->tap);
+ if (rc) {
+ goto end;
+ }
+
+ rc = lsm6dsl_set_orientation(lsm6dsl, &cfg->orientation);
+ if (rc) {
+ goto end;
+ }
+
+ lsm6dsl_set_latched_int(lsm6dsl, cfg->latched_int);
+ lsm6dsl_set_map_int2_to_int1(lsm6dsl, cfg->map_int2_to_int1);
+
+ if (!cfg->notify_cfg) {
+ lsm6dsl->cfg.notify_cfg = (struct lsm6dsl_notif_cfg *)default_notif_cfg;
+ lsm6dsl->cfg.notify_cfg_count = ARRAY_SIZE(default_notif_cfg);
+ } else {
+ lsm6dsl->cfg.notify_cfg = cfg->notify_cfg;
+ lsm6dsl->cfg.notify_cfg_count = cfg->notify_cfg_count;
+ }
+
+ rc = sensor_set_type_mask(sensor, cfg->lc_s_mask);
+
+end:
+ return rc;
+}
+
+#if MYNEWT_VAL(BUS_DRIVER_PRESENT)
+static void
+init_node_cb(struct bus_node *bnode, void *arg)
+{
+ lsm6dsl_init((struct os_dev *)bnode, arg);
+}
+
+static int
+lsm6dsl_create_i2c_sensor_dev(struct lsm6dsl *lsm6dsl, const char *name,
+ const struct lsm6dsl_create_dev_cfg *cfg)
+{
+ const struct bus_i2c_node_cfg *i2c_cfg = &cfg->i2c_cfg;
+ struct bus_node_callbacks cbs = {
+ .init = init_node_cb,
+ };
+ int rc;
+
+ lsm6dsl->node_is_spi = false;
+
+ lsm6dsl->sensor.s_itf.si_dev = &lsm6dsl->i2c_node.bnode.odev;
+ bus_node_set_callbacks((struct os_dev *)lsm6dsl, &cbs);
+
+ rc = bus_i2c_node_create(name, &lsm6dsl->i2c_node, i2c_cfg, (void *)cfg);
+
+ return rc;
+}
+
+static int
+lsm6dsl_create_spi_sensor_dev(struct lsm6dsl *lsm6dsl, const char *name,
+ const struct lsm6dsl_create_dev_cfg *cfg)
+{
+ const struct bus_spi_node_cfg *spi_cfg = &cfg->spi_cfg;
+ struct bus_node_callbacks cbs = {
+ .init = init_node_cb,
+ };
+ int rc;
+
+ lsm6dsl->node_is_spi = true;
+
+ lsm6dsl->sensor.s_itf.si_dev = &lsm6dsl->spi_node.bnode.odev;
+ bus_node_set_callbacks((struct os_dev *)lsm6dsl, &cbs);
+
+ rc = bus_spi_node_create(name, &lsm6dsl->spi_node, spi_cfg, (void *)cfg);
+
+ return rc;
+}
+
+int
+lsm6dsl_create_dev(struct lsm6dsl *lsm6dsl, const char *name,
+ const struct lsm6dsl_create_dev_cfg *cfg)
+{
+ if (MYNEWT_VAL(LSM6DSL_SPI_SUPPORT) && cfg->node_is_spi) {
+ return lsm6dsl_create_spi_sensor_dev(lsm6dsl, name, cfg);
+ } else if (MYNEWT_VAL(LSM6DSL_I2C_SUPPORT) && !cfg->node_is_spi) {
+ return lsm6dsl_create_i2c_sensor_dev(lsm6dsl, name, cfg);
+ }
+ return 0;
+}
+
+#else
+int
+lsm6dsl_create_dev(struct lsm6dsl *lsm6dsl, const char *name,
+ const struct lsm6dsl_create_dev_cfg *cfg)
+{
+ return os_dev_create((struct os_dev *)lsm6dsl, name,
+ OS_DEV_INIT_PRIMARY, 0, lsm6dsl_init, (void *)cfg);
+}
+
+#endif /* BUS_DRIVER_PRESENT */
diff --git a/hw/drivers/sensors/lsm6dsl/src/lsm6dsl_priv.h b/hw/drivers/sensors/lsm6dsl/src/lsm6dsl_priv.h
new file mode 100644
index 0000000..5a070cc
--- /dev/null
+++ b/hw/drivers/sensors/lsm6dsl/src/lsm6dsl_priv.h
@@ -0,0 +1,775 @@
+/*
+ * 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
+ * resarding 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.
+ */
+
+#ifndef __LSM6DSL_PRIV_H__
+#define __LSM6DSL_PRIV_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Common defines for Acc and Gyro sensors */
+#define LSM6DSL_EN_BIT 0x01
+#define LSM6DSL_DIS_BIT 0x00
+
+/*
+ * Access to embedded sensor hub register bank
+ *
+ * FUNC_CFG_ACCESS - Enable access to the embedded functions registers
+ * SHUB_REG_ACCESS - Enable access to the sensor hub registers
+ */
+#define LSM6DSL_FUNC_CFG_ACCESS_REG 0x01
+
+#define LSM6DSL_FUNC_CFG_ACCESS_MASK 0x80
+#define LSM6DSL_SHUB_REG_ACCESS_MASK 0x40
+
+/*
+ * FIFO control register 1
+ *
+ * FTH_[7:0] - FIFO threshold level bits
+ */
+#define LSM6DSL_FIFO_CTRL1_REG 0x06
+#define LSM6DSL_FTH_0_7_MASK 0xFF
+
+/*
+ * FIFO control register 2
+ *
+ * FTH_[10:8] - FIFO threshold level bits
+ * FIFO_TEMP_EN - Enable the temperature data storage in FIFO.
+ * TIMER_PEDO_FIFO_DRDY - FIFO write mode.
+ * TIMER_PEDO_FIFO_EN - Enable pedometer step counter and timestamp as 4th FIFO data set.
+ */
+#define LSM6DSL_FIFO_CTRL2_REG 0x07
+#define LSM6DSL_FTH_8_10_MASK 0x07
+#define LSM6DSL_FIFO_TEMP_EN_MASK 0x08
+#define LSM6DSL_TIMER_PEDO_FIFO_DRDY_MASK 0x40
+#define LSM6DSL_TIMER_PEDO_FIFO_EN_MASK 0x80
+
+#define LSM6DSL_FIFO_FTH_MASK 0x07ff
+
+/*
+ * FIFO control register 3
+ *
+ * DEC_FIFO_XL[2:0] - Accelerometer FIFO (second data set) decimation setting.
+ * DEC_FIFO_GYRO[2:0] - Gyro FIFO (first data set) decimation setting.
+ */
+#define LSM6DSL_FIFO_CTRL3_REG 0x08
+#define LSM6DSL_DEC_FIFO_XL_MASK 0x07
+#define LSM6DSL_DEC_FIFO_GYRO_MASK 0x38
+
+/*
+ * FIFO control register 4
+ *
+ * DEC_DS3_FIFO[2:0] - Third FIFO data set decimation setting
+ * DEC_DS3_FIFO[2:0] - Fourth FIFO data set decimation setting
+ * ONLY_HIGH_DATA - 8-bit data storage in FIFO
+ * STOP_ON_FTH - Enable FIFO threshold level use
+ */
+#define LSM6DSL_FIFO_CTRL4_REG 0x09
+#define LSM6DSL_DEC_DES3_FIFO_MASK 0x07
+#define LSM6DSL_DEC_DES4_FIFO_MASK 0x38
+#define LSM6DSL_ONLY_HIGH_DATA_MASK 0x40
+#define LSM6DSL_STOP_ON_FTH_MASK 0x80
+
+/*
+ * FIFO control register 5
+ *
+ * FIFO_MODE_[2:0] - FIFO mode selection bits
+ * ODR_FIFO_[3:0] - FIFO ODR selection
+ */
+#define LSM6DSL_FIFO_CTRL5_REG 0x0A
+#define LSM6DSL_FIFO_MODE_MASK 0x07
+#define LSM6DSL_ODR_FIFO_MASK 0x78
+
+/*
+ * INT1 pin control register
+ *
+ * Each bit in this register enables a signal to be carried out on INT1
+ * The output of the pin will be the OR combination of the signals selected here
+ * and in MD1_CFG
+ *
+ * INT1_DRDY_XL - Enables accelerometer data-ready interrupt on INT1 pin
+ * INT1_DRDY_G - Enables gyroscope data-ready interrupt on INT1 pin
+ * INT1_BOOT - Enables boot status on INT1 pin
+ * INT1_FIFO_TH - Enables FIFO threshold interrupt on INT1 pin
+ * INT1_FIFO_OVR - Enables FIFO overrun interrupt on INT1 pin
+ * INT1_FIFO_FULL - Enables FIFO full flag interrupt on INT1 pin
+ * INT1_CNT_BDR - Enables COUNTER_BDA_IA on INT1 pin
+ * DEN_DRDY_flag - Send DEN_DRDY to INT1 pin
+ */
+#define LSM6DSL_INT1_CTRL 0x0d
+
+/*
+ * INT2 pin control register
+ *
+ * Each bit in this register enables a signal to be carried out on INT2
+ * The output of the pin will be the OR combination of the signals selected here
+ * and in MD2_CFG
+ *
+ * INT2_DRDY_XL - Enables accelerometer data-ready interrupt on INT2 pin
+ * INT2_DRDY_G - Enables gyroscope data-ready interrupt on INT2 pin
+ * INT2_DRDY_TEMP - Enables temperature sensor data-ready interrupt on INT2 pin
+ * INT2_FIFO_TH - Enables FIFO threshold interrupt on INT2 pin
+ * INT2_FIFO_OVR - Enables FIFO overrun interrupt on INT2 pin
+ * INT2_FIFO_FULL - Enables FIFO full flag interrupt on INT2 pin
+ * INT2_CNT_BDR - Enables COUNTER_BDA_IA on INT2 pin
+ */
+#define LSM6DSL_INT2_CTRL 0x0e
+#define LSM6DSL_INT_DRDY_XL_MASK 0x01
+#define LSM6DSL_INT_DRDY_G_MASK 0x02
+#define LSM6DSL_INT1_BOOT_MASK 0x04
+#define LSM6DSL_INT2_DRDY_TEMP_MASK 0x04
+#define LSM6DSL_INT_FIFO_TH_MASK 0x08
+#define LSM6DSL_INT_FIFO_OVR_MASK 0x10
+#define LSM6DSL_INT_FIFO_FULL_MASK 0x20
+#define LSM6DSL_INT_CNT_BDR_MASK 0x40
+#define LSM6DSL_DEN_DRDY_FLAG_MASK 0x80
+
+/* Who Am I */
+#define LSM6DSL_WHO_AM_I_REG 0x0f
+#define LSM6DSL_WHO_AM_I 0x6a
+
+/*
+ * Accelerometer control register 1
+ *
+ * LPF2_XL_EN - Accelerometer high-resolution selection
+ * FS[1:0]_XL - Accelerometer full-scale selection
+ * ODR_XL[3:0] - Accelerometer ODR selection
+ */
+#define LSM6DSL_CTRL1_XL_REG 0x10
+#define LSM6DSL_BW0_XL_MASK 0x01
+#define LSM6DSL_LPF1_BW_SEL_MASK 0x02
+#define LSM6DSL_FS_XL_MASK 0x0c
+#define LSM6DSL_ODR_XL_MASK 0xf0
+
+/*
+ * Gyroscope control register 2
+ *
+ * FS_125 - Select gyro UI chain full-scale 125 dps
+ * FS[1:0]_G - Gyroscope full-scale selection
+ * ODR_G[3:0] - Gyroscope ODR selection
+ */
+#define LSM6DSL_CTRL2_G_REG 0x11
+#define LSM6DSL_FS_125_MASK 0x02
+#define LSM6DSL_FS_G_MASK 0x0e
+#define LSM6DSL_ODR_G_MASK 0xf0
+
+/*
+ * Control register 3
+ *
+ * SW_RESET - Software reset
+ * IF_INC - Register address automatically incremented during a multiple byte
+ * access with a serial interface (I2C or SPI).
+ * SIM - SPI serial interface Mode selection
+ * PP_OD - Push-pull/open-drain selection on INT1 and INT2 pins
+ * H_LACTIVE - Interrupt activation level
+ * BDU - Block Data Update
+ * BOOT - Reboots memory content
+ */
+#define LSM6DSL_CTRL3_C_REG 0x12
+#define LSM6DSL_SW_RESET_MASK 0x01
+#define LSM6DSL_BLE_MASK 0x02
+#define LSM6DSL_IF_INC_MASK 0x04
+#define LSM6DSL_SIM_MASK 0x08
+#define LSM6DSL_PP_OD_MASK 0x10
+#define LSM6DSL_H_LACTIVE_MASK 0x20
+#define LSM6DSL_BDU_MASK 0x40
+#define LSM6DSL_BOOT_MASK 0x80
+
+/*
+ * Control register 4
+ *
+ * LPF1_SEL_G - Enables gyroscope digital LPF1.
+ * I2C_disable - Disables I2C interface.
+ * DRDY_MASK - Configuration 1 data available enable bit.
+ * DEN_DRDY_INT1 - DEN DRDY signal on INT1 pad.
+ * INT2_on_INT1 - All interrupt signals available on INT1 pin enable.
+ * SLEEP - Enables gyroscope Sleep mode.
+ * DEN_XL_EN - Extend DEN functionality to accelerometer sensor.
+ */
+#define LSM6DSL_CTRL4_C_REG 0x13
+#define LSM6DSL_LPF1_SEL_G_MASK 0x02
+#define LSM6DSL_I2C_DISABLE_MASK 0x04
+#define LSM6DSL_DRDY_MASK_MASK 0x08
+#define LSM6DSL_DEN_DRDY_INT1_MASK 0x10
+#define LSM6DSL_INT2_ON_INT1_MASK 0x20
+#define LSM6DSL_SLEEP_MASK 0x40
+#define LSM6DSL_DEN_XL_EN_MASK 0x80
+
+/*
+ * Control register 5
+ *
+ * ST[1:0]_XL - Linear accelerometer sensor self-test enable
+ * ST[1:0]_G - Angular rate sensor self-test enable
+ * DEN_LH - DEN active level configuration
+ * ROUNDING[2:0] - Circular burst-mode (rounding) read from the output registers
+ */
+#define LSM6DSL_CTRL5_C_REG 0x14
+#define LSM6DSL_ST_XL_MASK 0x03
+#define LSM6DSL_ST_G_MASK 0x0c
+#define LSM6DSL_DEN_LH_MASK 0x10
+#define LSM6DSL_ROUNDING_MASK 0xE0
+
+#define LSM6DSL_XL_SELF_TEST_POS_SIGN 0x01
+#define LSM6DSL_XL_SELF_TEST_NEG_SIGN 0x02
+#define LSM6DSL_G_SELF_TEST_POS_SIGN 0x01
+#define LSM6DSL_G_SELF_TEST_NEG_SIGN 0x03
+
+/*
+ * Control register 6
+ *
+ * FTYPE[2:0] - Gyroscope's low-pass filter (LPF1) bandwidth selection
+ * USR_OFF_W - Weight of XL user offset bits
+ * XL_HM_MODE - High-performance operating mode disable for accelerometer
+ * LVL2_EN - DEN level-sensitive latched enable
+ * LVL1_EN - DEN data level-sensitive latched enable
+ * TRIG_EN - DEN data edge-sensitive latched enable
+ */
+#define LSM6DSL_CTRL6_C_REG 0x15
+#define LSM6DSL_FTYPE_MASK 0x03
+#define LSM6DSL_USR_OFF_W_MASK 0x08
+#define LSM6DSL_XL_HM_MODE_MASK 0x10
+#define LSM6DSL_LVL2_EN_MASK 0x20
+#define LSM6DSL_LVL_EN_MASK 0x40
+#define LSM6DSL_TRIG_EN_MASK 0x80
+
+/*
+ * Control register 7
+ *
+ * USR_OFF_ON_OUT - Enables accelerometer user offset correction block
+ * HPM_G[1:0] - Gyroscope digital HP filter cutoff selection
+ * HP_EN_G - Enables gyroscope digital high-pass filter.
+ * The filter is enabled only if the gyro is in HP mode
+ * G_HM_MODE - Disables high-performance operating mode for gyroscope
+ * ROUNDING_STATUS - Source register rounding function on WAKE_UP_SRC
+ */
+#define LSM6DSL_CTRL7_G_REG 0x16
+#define LSM6DSL_ROUNDING_STATUS_MASK 0x04
+#define LSM6DSL_HPM_G_MASK 0x30
+#define LSM6DSL_HP_EN_G_MASK 0x40
+#define LSM6DSL_G_HM_MODE_MASK 0x80
+
+/*
+ * Control register 8
+ *
+ * LOW_PASS_ON_6D - LPF2 on 6D function selection
+ * XL_FS_MODE - Accelerometer full-scale management between UI chain and
+ * OIS chain
+ * HP_SLOPE_XL_EN - Accelerometer slope filter / high-pass filter selection
+ * INPUT_COMPOSITE - Composite filter input selection
+ * HP_REF_MODE_XL - Enables accelerometer high-pass filter reference mode
+ * HPCF_XL_[1:0] - Accelerometer LPF2 and HP filter configuration and
+ * cutoff setting
+ * LPF2_XL_EN - Accelerometer low-pass filter LPF2 selection
+ */
+#define LSM6DSL_CTRL8_XL_REG 0x17
+#define LSM6DSL_LOW_PASS_ON_6D_MASK 0x01
+#define LSM6DSL_HP_SLOPE_XL_EN_MASK 0x04
+#define LSM6DSL_INPUT_COMPOSITE_MASK 0x08
+#define LSM6DSL_HP_REF_MODE_XL_MASK 0x10
+#define LSM6DSL_HPCF_XL_MASK 0x60
+#define LSM6DSL_LPF2_XL_EN_MASK 0x80
+
+/*
+ * Control register 9
+ *
+ * I3C_disable - Disables MIPI I3C SM communication protocol
+ * DEN_LH - DEN active level configuration
+ * DEN_XL_EN - Extends DEN functionality to accelerometer sensor
+ * DEN_XL_G - DEN stamping sensor selection
+ * DEN_Z - DEN value stored in LSB of Z-axis
+ * DEN_Y - DEN value stored in LSB of Y-axis
+ * DEN_X - DEN value stored in LSB of X-axis
+ */
+#define LSM6DSL_CTRL9_XL_REG 0x18
+#define LSM6DSL_SOFT_EN_MASK 0x04
+#define LSM6DSL_DEN_XL_G_MASK 0x10
+#define LSM6DSL_DEN_Z_MASK 0x20
+#define LSM6DSL_DEN_Y_MASK 0x40
+#define LSM6DSL_DEN_X_MASK 0x80
+#define LSM6DSL_DEN_ALL_MASK (LSM6DSL_DEN_X_MASK | \
+ LSM6DSL_DEN_Y_MASK | \
+ LSM6DSL_DEN_Z_MASK)
+
+/*
+ * Control register 10
+ *
+ * SIGN_MOTION_EN - Enable significant motion detection function
+ * PEDO_RST - Reset pedometer step counter
+ * FUNC_EN - Enable embedded functionalities (pedometer, tilt, significant motion
+ * detection, sensor hub and ironing)
+ * TILT_EN - Enable tilt calculation
+ * PEDO_EN - Enable pedometer algorithm
+ * TIMER_EN - Enable timestamp count
+ * WRIST_TILT_EN - Enable wrist tilt algorithm
+ */
+#define LSM6DSL_CTRL10_C_REG 0x19
+#define LSM6DSL_SIGN_MOTION_EN_MASK 0x01
+#define LSM6DSL_PEDO_RST_MASK 0x02
+#define LSM6DSL_FUNC_EN_MASK 0x04
+#define LSM6DSL_TILT_EN_MASK 0x08
+#define LSM6DSL_PEDO_EN_MASK 0x10
+#define LSM6DSL_TIMER_EN_MASK 0x20
+#define LSM6DSL_WRIST_TILT_EN_MASK 0x80
+
+
+/*
+ * Wake-up interrupt source register
+ *
+ * Z_WU - Wakeup event detection status on Z-axis
+ * Y_WU - Wakeup event detection status on Y-axis
+ * X_WU - Wakeup event detection status on X-axis
+ * WU_IA - Wakeup event detection status
+ * SLEEP_STATE_IA - Sleep status bit
+ * FF_IA - Free-fall event status
+ */
+#define LSM6DSL_WAKE_UP_SRC_REG 0x1b
+#define LSM6DSL_Z_WU_MASK 0x01
+#define LSM6DSL_Y_WU_MASK 0x02
+#define LSM6DSL_X_WU_MASK 0x04
+#define LSM6DSL_WU_IA_MASK 0x08
+#define LSM6DSL_SLEEP_STATE_IA_MASK 0x10
+#define LSM6DSL_FF_IA_MASK 0x20
+
+/*
+ * Tap source register
+ *
+ * Z_TAP - Tap event detection status on Z-axis
+ * Y_TAP - Tap event detection status on Y-axis
+ * X_TAP - Tap event detection status on X-axis
+ * TAP_SIGN - Sign of acceleration detected by tap event
+ * DOUBLE_TAP - Double-tap event detection status
+ * SINGLE_TAP - Single-tap event status
+ * TAP_IA - Tap event detection status
+ */
+#define LSM6DSL_TAP_SRC_REG 0x1c
+#define LSM6DSL_Z_TAP_MASK 0x01
+#define LSM6DSL_Y_TAP_MASK 0x02
+#define LSM6DSL_X_TAP_MASK 0x04
+#define LSM6DSL_TAP_SIGN_MASK 0x08
+#define LSM6DSL_DOUBLE_TAP_MASK 0x10
+#define LSM6DSL_SINGLE_TAP_MASK 0x20
+#define LSM6DSL_TAP_IA_MASK 0x40
+
+/*
+ * Portrait, landscape, face-up and face-down source register
+ *
+ * XL - X-axis low event (under threshold)
+ * XH - X-axis high event (over threshold)
+ * YL - Y-axis low event (under threshold)
+ * YH - Y-axis high event (over threshold)
+ * ZL - Z-axis low event (under threshold)
+ * ZH - Z-axis high event (over threshold)
+ * D6D_IA - Interrupt active for change position portrait, landscape,
+ * face-up, face-down
+ * DEN_DRDY - DEN data-ready signal
+ */
+#define LSM6DSL_D6D_SRC_REG 0x1d
+#define LSM6DSL_XL_MASK 0x01
+#define LSM6DSL_XH_MASK 0x02
+#define LSM6DSL_YL_MASK 0x04
+#define LSM6DSL_YH_MASK 0x08
+#define LSM6DSL_ZL_MASK 0x10
+#define LSM6DSL_ZH_MASK 0x20
+#define LSM6DSL_D6D_IA_MASK 0x40
+#define LSM6DSL_DEN_DRDY_MASK 0x80
+
+/*
+ * Status register
+ *
+ * XLDA - Accelerometer new data available
+ * GDA - Gyroscope new data available
+ * TDA - Temperature new data available
+ */
+#define LSM6DSL_STATUS_REG 0x1e
+#define LSM6DSL_STS_XLDA_UP_MASK 0x01
+#define LSM6DSL_STS_GDA_UP_MASK 0x02
+#define LSM6DSL_STS_TDA_UP_MASK 0x04
+
+/*
+ * Temperature data output register
+ *
+ * L and H registers together express a 16-bit word in two’s complement
+ */
+#define LSM6DSL_OUT_TEMP_L_REG 0x20
+#define LSM6DSL_OUT_TEMP_H_REG 0x21
+
+/*
+ * Angular rate sensor pitch axis (X) angular rate output register
+ *
+ * The value is expressed as a 16-bit word in two’s complement
+ */
+#define LSM6DSL_OUTX_L_G_REG 0x22
+#define LSM6DSL_OUTX_H_G_REG 0x23
+
+/*
+ * Linear acceleration sensor X-axis output register
+ *
+ * The value is expressed as a 16-bit word in two’s complement.
+ */
+#define LSM6DSL_OUTX_L_XL_REG 0x28
+#define LSM6DSL_OUTX_H_XL_REG 0x29
+
+/*
+ * FIFO status register 1
+ *
+ * DIFF_FIFO_[7:0] - Number of unread words (16-bit axes) stored in FIFO
+ */
+#define LSM6DSL_FIFO_STATUS1_REG 0x3a
+//#define LSM6DSL_FIFO_DIFF_MASK 0x03ff
+
+/*
+ * FIFO status register 2
+ *
+ * DIFF_FIFO_[9:8] - Number of unread words (16-bit axes) stored in FIFO
+ * FIFO_OVR_LATCHED - Latched FIFO overrun status
+ * COUNTER_BDR_IA - Counter BDR reaches the threshold
+ * FIFO_FULL_IA - Smart FIFO full status
+ * FIFO_OVR_IA - FIFO overrun status
+ * FIFO_WTM_IA - FIFO watermark status
+ */
+#define LSM6DSL_FIFO_STATUS2_REG 0x3b
+#define LSM6DSL_DIFF_FIFO_MASK 0x07
+#define LSM6DSL_FIFO_EMPTY_MASK 0x10
+#define LSM6DSL_FIFO_FULL_SMART_MASK 0x20
+#define LSM6DSL_OVER_RUN_MASK 0x40
+#define LSM6DSL_WaterM_MASK 0x80
+
+/*
+ * FIFO status register 3
+ *
+ * FIFO_PATTERN_[7:0] - Number of unread words (16-bit axes) stored in FIFO
+ */
+#define LSM6DSL_FIFO_STATUS3_REG 0x3c
+
+/*
+ * FIFO status register 3
+ *
+ * FIFO_PATTERN_[9:8] - Number of unread words (16-bit axes) stored in FIFO
+ */
+#define LSM6DSL_FIFO_STATUS4_REG 0x3d
+
+/*
+ * FIFO data out registers
+ */
+#define LSM6DSL_FIFO_DATA_OUT_L_REG 0x3e
+#define LSM6DSL_FIFO_DATA_OUT_H_REG 0x3f
+
+/*
+ * Timestamp first data output register
+ *
+ * The value is expressed as a 32-bit word and the bit resolution is 25 μs or
+ * 6.4ms depending on (WAKE_UP_DUR:TIMER_HR).
+ */
+#define LSM6DSL_TIMESTAMP0_REG 0x40
+#define LSM6DSL_TIMESTAMP1_REG 0x41
+#define LSM6DSL_TIMESTAMP2_REG 0x42
+
+/*
+ * Activity/inactivity functions, configuration of filtering, and tap
+ * recognition functions
+ *
+ * LIR - Latched Interrupt
+ * TAP_Z_EN - Enable Z direction in tap recognition
+ * TAP_Y_EN - Enable Y direction in tap recognition
+ * TAP_X_EN - Enable X direction in tap recognition
+ * SLOPE_FDS - HPF or SLOPE filter selection on wake-up and
+ * Activity/Inactivity functions
+ * INACT_EN - Enable inactivity function
+ * 00: disabled
+ * 01: sets accelerometer ODR to 12.5 Hz (low-power mode),
+ * gyro does not change
+ * 10: sets accelerometer ODR to 12.5 Hz (low-power mode),
+ * gyro to sleep mode
+ * 11: sets accelerometer ODR to 12.5 Hz (low-power mode),
+ * gyro to power-down mode
+ * INTERRUPTS_ENABLE - Enable basic interrupts (6D/4D, free-fall, wake-up,
+ * tap, inactivity).
+ * SLEEP_STS_ON_INT - [SLEEP_STATUS_ON_INT] Activity/inactivity interrupt
+ * mode configuration
+ * INT_CLR_ON_READ - This bit allows immediately clearing the latched
+ * interrupts of an event detection upon the read of the
+ * corresponding status register. It must be set to 1
+ * together with LIR
+ */
+#define LSM6DSL_TAP_CFG_REG 0x58
+#define LSM6DSL_LIR_MASK 0x01
+#define LSM6DSL_TAP_Z_EN_MASK 0x02
+#define LSM6DSL_TAP_Y_EN_MASK 0x04
+#define LSM6DSL_TAP_X_EN_MASK 0x08
+#define LSM6DSL_SLOPE_FDS_MASK 0x10
+#define LSM6DSL_INACT_EN_MASK 0x60
+#define LSM6DSL_INTERRUPTS_ENABLE_MASK 0x80
+
+#define LSM6DSL_TAP_XYZ_EN_MASK 0x0E
+
+#define LSM6DSL_SLEEP_STS_ON_INT_MASK 0x20
+#define LSM6DSL_INT_CLR_ON_READ_MASK 0x40
+
+/*
+ * Portrait/landscape position and tap function threshold register
+ *
+ * TAP_THS_Z_[4:0] - Z-axis tap recognition threshold 1 LSB = FS_XL / (2^5)
+ * SIXD_THS[1:0] - Threshold for 4D/6D function
+ * D4D_EN - 4D orientation detection enable. Z-axis position detection
+ * is disabled
+ */
+#define LSM6DSL_TAP_THS_6D_REG 0x59
+#define LSM6DSL_TAP_THS_MASK 0x1f
+#define LSM6DSL_SIXD_THS_MASK 0x60
+#define LSM6DSL_D4D_EN_MASK 0x80
+
+/*
+ * Tap recognition function setting register
+ *
+ * SHOCK[1:0] - Maximum duration of overthreshold event
+ * QUIET[1:0] - Expected quiet time after a tap detection
+ * DUR[3:0] - Duration of maximum time gap for double tap recognition
+ */
+#define LSM6DSL_INT_DUR2_REG 0x5a
+#define LSM6DSL_SHOCK_MASK 0x03
+#define LSM6DSL_QUIET_MASK 0x0c
+#define LSM6DSL_DUR_MASK 0xf0
+
+/*
+ * Single/double-tap selection and wake-up configuration
+ *
+ * WK_THS[5:0] - Threshold for wakeup
+ * SINGLE_DOUBLE_TAP - Single/double-tap event enable
+ */
+#define LSM6DSL_WAKE_UP_THS_REG 0x5b
+#define LSM6DSL_WK_THS_MASK 0x3f
+#define LSM6DSL_SINGLE_DOUBLE_TAP_MASK 0x80
+
+/*
+ * Free-fall, wakeup and sleep mode functions duration setting register
+ *
+ * SLEEP_DUR[3:0] - Duration to go in sleep mode
+ * TIMER_HR - Timestamp register resolution setting
+ * (0: 1LSB = 6.4 ms; 1: 1LSB = 25 μs)
+ * WAKE_DUR[1:0] - Wakeup duration event (in ODR)
+ * FF_DUR5 - Free fall duration event (bit 5)
+ */
+#define LSM6DSL_WAKE_UP_DUR_REG 0x5c
+#define LSM6DSL_SLEEP_DUR_MASK 0x0f
+#define LSM6DSL_TIMER_HR_MASK 0x10
+#define LSM6DSL_WAKE_DUR_MASK 0x60
+#define LSM6DSL_FF_DUR5_MASK 0x80
+
+/*
+ * Free-fall function duration setting register
+ *
+ * FF_DUR[4:0] - Free-fall duration event
+ * FF_THS[2:0] - Free fall threshold setting
+ */
+#define LSM6DSL_FREE_FALL_REG 0x5d
+#define LSM6DSL_FF_THS_MASK 0x07
+#define LSM6DSL_FF_DUR_MASK 0xf8
+
+/*
+ * Functions routing on INT1 register
+ *
+ * INT1_TIMER - Routing of end counter event of timer on INT1
+ * INT1_TILT - Routing of tilt event on INT1
+ * INT1_6D - Routing of 6D event on INT1
+ * INT1_DOUBLE_TAP - Routing of TAP event on INT1
+ * INT1_FF - Routing of Free-Fall event on INT1
+ * INT1_WU - Routing of Wake-up event on INT1
+ * INT1_SINGLE_TAP - Routing of Single-Tap event on INT1
+ * INT1_INACT_STATE - Routing on INT1 of inactivity mode
+ */
+#define LSM6DSL_MD1_CFG_REG 0x5e
+#define LSM6DSL_INT1_TIMER_MASK 0x01
+#define LSM6DSL_INT1_TILT_MASK 0x02
+#define LSM6DSL_INT1_6D_MASK 0x04
+#define LSM6DSL_INT1_DOUBLE_TAP_MASK 0x08
+#define LSM6DSL_INT1_FF_MASK 0x10
+#define LSM6DSL_INT1_WU_MASK 0x20
+#define LSM6DSL_INT1_SINGLE_TAP_MASK 0x40
+#define LSM6DSL_INT1_INACT_STATE_MASK 0x80
+
+/*
+ * Functions routing on INT2 register
+ *
+ * INT2_IRON - Routing of soft-iron/hard-iron algorithm end event on INT2
+ * INT2_TILT - Routing of tilt event on INT2
+ * INT2_6D - Routing of 6D event on INT2
+ * INT2_DOUBLE_TAP - Routing of TAP event on INT2
+ * INT2_FF - Routing of Free-Fall event on INT2
+ * INT2_WU - Routing of Wake-up event on INT2
+ * INT2_SINGLE_TAP - Routing of Single-Tap event on INT2
+ * INT2_INACT_STATE - Routing on INT2 of inactivity mode
+ */
+#define LSM6DSL_MD2_CFG_REG 0x5f
+#define LSM6DSL_INT2_IRON_MASK 0x01
+#define LSM6DSL_INT2_TILT_MASK 0x02
+#define LSM6DSL_INT2_6D_MASK 0x04
+#define LSM6DSL_INT2_DOUBLE_TAP_MASK 0x08
+#define LSM6DSL_INT2_FF_MASK 0x10
+#define LSM6DSL_INT2_WU_MASK 0x20
+#define LSM6DSL_INT2_SINGLE_TAP_MASK 0x40
+#define LSM6DSL_INT2_INACT_STATE_MASK 0x80
+
+#define LSM6DSL_X_OFS_USR_REG 0x73
+#define LSM6DSL_Y_OFS_USR_REG 0x74
+#define LSM6DSL_Z_OFS_USR_REG 0x75
+
+/* SensorHub registers */
+#define LSM6DSL_SENSORHUB1_REG 0x2e
+#define LSM6DSL_SENSORHUB2_REG 0x2f
+#define LSM6DSL_SENSORHUB3_REG 0x30
+#define LSM6DSL_SENSORHUB4_REG 0x31
+#define LSM6DSL_SENSORHUB5_REG 0x32
+#define LSM6DSL_SENSORHUB6_REG 0x33
+#define LSM6DSL_SENSORHUB7_REG 0x34
+#define LSM6DSL_SENSORHUB8_REG 0x35
+#define LSM6DSL_SENSORHUB9_REG 0x36
+#define LSM6DSL_SENSORHUB10_REG 0x37
+#define LSM6DSL_SENSORHUB11_REG 0x38
+#define LSM6DSL_SENSORHUB12_REG 0x39
+#define LSM6DSL_SENSORHUB13_REG 0x4d
+#define LSM6DSL_SENSORHUB14_REG 0x4e
+#define LSM6DSL_SENSORHUB15_REG 0x4f
+#define LSM6DSL_SENSORHUB16_REG 0x50
+#define LSM6DSL_SENSORHUB17_REG 0x51
+#define LSM6DSL_SENSORHUB18_REG 0x52
+
+/* Bank A registers */
+#define LSM6DSL_SLV0_ADD_REG 0x02
+#define LSM6DSL_SLV0_SUBADD_REG 0x03
+#define LSM6DSL_SLV0_CONFIG_REG 0x04
+#define LSM6DSL_SLV1_ADD_REG 0x05
+#define LSM6DSL_SLV1_SUBADD_REG 0x06
+#define LSM6DSL_SLV1_CONFIG_REG 0x07
+#define LSM6DSL_SLV2_ADD_REG 0x08
+#define LSM6DSL_SLV2_SUBADD_REG 0x09
+#define LSM6DSL_SLV2_CONFIG_REG 0x0a
+#define LSM6DSL_SLV3_ADD_REG 0x0b
+#define LSM6DSL_SLV3_SUBADD_REG 0x0c
+#define LSM6DSL_SLV3_CONFIG_REG 0x0d
+#define LSM6DSL_DATAWRITE_SRC_MODE_SUB_SLV0_REG 0x0e
+#define LSM6DSL_CONFIG_PEDO_THS_MIN_REG 0x0f
+#define LSM6DSL_SM_THS_REG 0x13
+#define LSM6DSL_PEDO_DEB_REG_REG 0x14
+#define LSM6DSL_STEP_COUNT_DELTA_REG 0x15
+#define LSM6DSL_MAG_SI_XX_REG 0x24
+#define LSM6DSL_MAG_SI_XY_REG 0x25
+#define LSM6DSL_MAG_SI_XZ_REG 0x26
+#define LSM6DSL_MAG_SI_YX_REG 0x27
+#define LSM6DSL_MAG_SI_YY_REG 0x28
+#define LSM6DSL_MAG_SI_YZ_REG 0x29
+#define LSM6DSL_MAG_SI_ZX_REG 0x2a
+#define LSM6DSL_MAG_SI_ZY_REG 0x2b
+#define LSM6DSL_MAG_SI_ZZ_REG 0x2c
+#define LSM6DSL_MAG_OFFX_L_REG 0x2d
+#define LSM6DSL_MAG_OFFX_H_REG 0x2e
+#define LSM6DSL_MAG_OFFY_L_REG 0x2f
+#define LSM6DSL_MAG_OFFY_H_REG 0x30
+#define LSM6DSL_MAG_OFFZ_L_REG 0x31
+#define LSM6DSL_MAG_OFFZ_H_REG 0x32
+
+/* Bank 2 registers */
+
+#define LSM6DSL_A_WRIST_TILT_LAT_REG 0x50
+#define LSM6DSL_A_WRIST_TILT_THS_REG 0x54
+#define LSM6DSL_A_WRIST_TILT_MASK_REG 0x59
+
+#define LSM6DSL_MAX_FIFO_DEPTH 2048
+
+/* Self Test output converted in LSB */
+#define LSM6DSL_XL_ST_MIN 819
+#define LSM6DSL_XL_ST_MAX 27868
+#define LSM6DSL_G_ST_MIN 2285
+#define LSM6DSL_G_ST_MAX 9142
+
+#define LSM6DSL_GET_OUT_REG(_type) \
+ (_type & SENSOR_TYPE_ACCELEROMETER) ? LSM6DSL_OUTX_L_XL_REG : \
+ LSM6DSL_OUTX_L_G_REG;
+
+/* Set Read operation bit in SPI communication */
+#define LSM6DSL_SPI_READ_CMD_BIT(_reg) (_reg |= 0x80)
+
+/* Max time to wait for interrupt */
+#define LSM6DSL_MAX_INT_WAIT (4 * OS_TICKS_PER_SEC)
+
+/* Shift data with mask */
+#define LSM6DSL_SHIFT_DATA_MASK(data, mask) \
+ ((data << __builtin_ctz(mask)) & mask)
+#define LSM6DSL_DESHIFT_DATA_MASK(data, mask) \
+ ((data & mask) >> __builtin_ctz(mask))
+
+struct lsm6dsl;
+
+struct lsm6dsl_cfg_regs1 {
+ union {
+ uint8_t regs[29];
+ struct {
+ uint8_t func_cfg_access;
+ uint8_t reserved1[2];
+ uint8_t sensor_sync_time_frame;
+ uint8_t sensor_sync_res_ratio;
+ uint8_t fifo_ctrl1;
+ uint8_t fifo_ctrl2;
+ uint8_t fifo_ctrl3;
+ uint8_t fifo_ctrl4;
+ uint8_t fifo_ctrl5;
+ uint8_t drdy_pulse_cfg_g;
+ uint8_t reserved2[1];
+ uint8_t int1_ctrl;
+ uint8_t int2_ctrl;
+ uint8_t who_am_i;
+ uint8_t ctrl1_xl;
+ uint8_t ctrl2_g;
+ uint8_t ctrl3_c;
+ uint8_t ctrl4_c;
+ uint8_t ctrl5_c;
+ uint8_t ctrl6_c;
+ uint8_t ctrl7_g;
+ uint8_t ctrl8_xl;
+ uint8_t ctrl9_xl;
+ uint8_t ctrl10_c;
+ uint8_t master_config;
+ uint8_t wauke_up_src;
+ uint8_t tap_src;
+ uint8_t d6d_src;
+ };
+ };
+};
+
+struct lsm6dsl_cfg_regs2 {
+ union {
+ uint8_t regs[8];
+ struct {
+ uint8_t tap_cfg;
+ uint8_t tap_ths_6d;
+ uint8_t int_dur2;
+ uint8_t wake_up_ths;
+ uint8_t wake_up_dur;
+ uint8_t free_fall;
+ uint8_t md1_cfg;
+ uint8_t md2_cfg;
+ };
+ };
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __LSM6DSL_PRIV_H_ */
diff --git a/hw/drivers/sensors/lsm6dsl/src/lsm6dsl_shell.c b/hw/drivers/sensors/lsm6dsl/src/lsm6dsl_shell.c
new file mode 100644
index 0000000..2047564
--- /dev/null
+++ b/hw/drivers/sensors/lsm6dsl/src/lsm6dsl_shell.c
@@ -0,0 +1,324 @@
+/*
+ * 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.
+ */
+
+#include <string.h>
+#include <errno.h>
+#include "os/mynewt.h"
+#include "console/console.h"
+#include "shell/shell.h"
+#include "sensor/accel.h"
+#include "lsm6dsl/lsm6dsl.h"
+#include "lsm6dsl_priv.h"
+#include "parse/parse.h"
+
+typedef struct {
+ uint8_t addr;
+ char *regname;
+} reg_name_t;
+
+static int lsm6dsl_shell_cmd(int argc, char **argv);
+
+/* Human readable register map for page 0 */
+static const reg_name_t reg_name[] = {
+ { .addr = 0x02, .regname = "FUNC_CFG_ACCESS" },
+ { .addr = 0x04, .regname = "SENSOR_SYNC_TIME_FRAME" },
+ { .addr = 0x05, .regname = "SENSOR_SYNC_RES_RATIO" },
+ { .addr = 0x06, .regname = "FIFO_CTRL1" },
+ { .addr = 0x07, .regname = "FIFO_CTRL2" },
+ { .addr = 0x08, .regname = "FIFO_CTRL3" },
+ { .addr = 0x09, .regname = "FIFO_CTRL4" },
+ { .addr = 0x0a, .regname = "FIFO_CTRL5" },
+ { .addr = 0x0b, .regname = "DRDY_PULSE_CFG_G" },
+ { .addr = 0x0d, .regname = "INT1_CTRL" },
+ { .addr = 0x0e, .regname = "INT2_CTRL" },
+ { .addr = 0x0f, .regname = "WHO_AM_I" },
+ { .addr = 0x10, .regname = "CTRL1_XL" },
+ { .addr = 0x11, .regname = "CTRL2_G" },
+ { .addr = 0x12, .regname = "CTRL3_C" },
+ { .addr = 0x13, .regname = "CTRL4_C" },
+ { .addr = 0x14, .regname = "CTRL5_C" },
+ { .addr = 0x15, .regname = "CTRL6_C" },
+ { .addr = 0x16, .regname = "CTRL7_G" },
+ { .addr = 0x17, .regname = "CTRL8_XL" },
+ { .addr = 0x18, .regname = "CTRL9_XL" },
+ { .addr = 0x19, .regname = "CTRL10_C" },
+ { .addr = 0x1A, .regname = "MASTER_CONFIG" },
+ { .addr = 0x1B, .regname = "WAKE_UP_SRC" },
+ { .addr = 0x1C, .regname = "TAP_SRC" },
+ { .addr = 0x1D, .regname = "D6D_SRC" },
+ { .addr = 0x1E, .regname = "STATUS_REG" },
+ { .addr = 0x3a, .regname = "FIFO_STATUS1" },
+ { .addr = 0x3b, .regname = "FIFO_STATUS2" },
+ { .addr = 0x3c, .regname = "FIFO_STATUS3" },
+ { .addr = 0x3d, .regname = "FIFO_STATUS4" },
+ { .addr = 0x53, .regname = "FUNC_SRC1" },
+ { .addr = 0x54, .regname = "FUNC_SRC2" },
+ { .addr = 0x55, .regname = "WRIST_TILT_IA" },
+ { .addr = 0x58, .regname = "TAP_CFG" },
+ { .addr = 0x59, .regname = "TAP_THS_6D" },
+ { .addr = 0x5a, .regname = "INT_DUR2" },
+ { .addr = 0x5b, .regname = "WAKE_UP_THS" },
+ { .addr = 0x5c, .regname = "WAKE_UP_DUR" },
+ { .addr = 0x5d, .regname = "FREE_FALL" },
+ { .addr = 0x5e, .regname = "MD1_CFG" },
+ { .addr = 0x5f, .regname = "MD2_CFG" },
+};
+
+static struct shell_cmd lsm6dsl_shell_cmd_struct = {
+ .sc_cmd = "lsm6dsl",
+ .sc_cmd_func = lsm6dsl_shell_cmd
+};
+
+static struct lsm6dsl *g_lsm6dsl;
+
+static int
+lsm6dsl_shell_open_device(void)
+{
+ if (!g_lsm6dsl) {
+ g_lsm6dsl = (struct lsm6dsl *)os_dev_open(MYNEWT_VAL(LSM6DSL_SHELL_DEV_NAME),
+ 1000, NULL);
+ }
+
+ return g_lsm6dsl ? 0 : SYS_ENODEV;
+}
+
+static int
+lsm6dsl_shell_err_invalid_arg(char *cmd_name)
+{
+ console_printf("Error: invalid argument \"%s\"\n",
+ cmd_name);
+ return EINVAL;
+}
+
+static int
+lsm6dsl_shell_err_too_many_args(char *cmd_name)
+{
+ console_printf("Error: too many arguments for command \"%s\"\n",
+ cmd_name);
+ return EINVAL;
+}
+
+static int
+lsm6dsl_shell_err_unknown_arg(char *cmd_name)
+{
+ console_printf("Error: unknown argument \"%s\"\n",
+ cmd_name);
+ return EINVAL;
+}
+
+static void
+lsm6dsl_shell_help(void)
+{
+ console_printf("%s cmd [flags...]\n", lsm6dsl_shell_cmd_struct.sc_cmd);
+ console_printf("cmd:\n");
+ console_printf("\tdump\t[START\tEND]\n");
+ console_printf("\tread\tADD\n");
+ console_printf("\twrite\tADD\tDATA\n");
+ console_printf("\ttest\n");
+}
+
+static const reg_name_t *
+lsm6dsl_get_reg(int i)
+{
+ int j;
+
+ for (j = 0; j < ARRAY_SIZE(reg_name); j++)
+ if (i == reg_name[j].addr) {
+ return ®_name[j];
+ }
+
+ return NULL;
+}
+
+static int
+lsm6dsl_shell_cmd_dump(int argc, char **argv)
+{
+ int rc = 0;
+ uint8_t value;
+ int i;
+ int sreg, ereg;
+ const reg_name_t *regname;
+ bool all;
+
+ if (argc > 4) {
+ return lsm6dsl_shell_err_too_many_args(argv[1]);
+ }
+
+ all = argc == 3 && 0 == strcmp(argv[2], "all");
+
+ if (argc == 2 || all) {
+ for (i = 0; i < ARRAY_SIZE(reg_name); i++) {
+ rc = lsm6dsl_read(g_lsm6dsl, reg_name[i].addr, &value, 1);
+ if (rc) {
+ console_printf("dump failed %d\n", rc);
+ } else if (all || value != 0){
+ console_printf("%-22s(0x%02X) = 0x%02X\n",
+ reg_name[i].regname, reg_name[i].addr, value);
+ }
+ }
+ } else {
+ sreg = parse_ll_bounds(argv[2], 0x02, 0x7F, &rc);
+ if (rc != 0) {
+ return lsm6dsl_shell_err_invalid_arg(argv[2]);
+ }
+
+ ereg = parse_ll_bounds(argv[3], 0x02, 0x7F, &rc);
+ if (rc != 0) {
+ return lsm6dsl_shell_err_invalid_arg(argv[3]);
+ }
+
+ for (i = sreg; i <= ereg; i++) {
+ rc = lsm6dsl_read(g_lsm6dsl, i, &value, 1);
+ if (rc) {
+ console_printf("dump failed %d\n", rc);
+ } else {
+ regname = lsm6dsl_get_reg(i);
+ if (regname)
+ console_printf("reg %-22s(0x%02X) = 0x%02X\n",
+ regname->regname, i, value);
+ else
+ console_printf("reg 0x%02X = 0x%02X\n", i, value);
+ }
+ }
+ }
+
+ return rc;
+}
+
+static int
+lsm6dsl_shell_cmd_read(int argc, char **argv)
+{
+ int rc;
+ uint8_t value;
+ int reg;
+
+ if (argc > 3) {
+ return lsm6dsl_shell_err_too_many_args(argv[1]);
+ }
+
+ reg = parse_ll_bounds(argv[2], 0x02, 0x7F, &rc);
+ if (rc != 0) {
+ return lsm6dsl_shell_err_invalid_arg(argv[2]);
+ }
+
+ rc = lsm6dsl_read(g_lsm6dsl, reg, &value, 1);
+ if (rc) {
+ console_printf("read failed %d\n", rc);
+ } else {
+ console_printf("reg 0x%02X(%d) = 0x%02X\n", reg, reg, value);
+ }
+
+ return rc;
+}
+
+static int
+lsm6dsl_shell_cmd_write(int argc, char **argv)
+{
+ int rc;
+ uint8_t value;
+ int reg;
+
+ if (argc > 4) {
+ return lsm6dsl_shell_err_too_many_args(argv[1]);
+ }
+
+ reg = parse_ll_bounds(argv[2], 0x02, 0x7F, &rc);
+ if (rc != 0) {
+ return lsm6dsl_shell_err_invalid_arg(argv[2]);
+ }
+
+ value = parse_ll_bounds(argv[3], 0x00, 0xFF, &rc);
+ if (rc != 0) {
+ return lsm6dsl_shell_err_invalid_arg(argv[2]);
+ }
+
+ rc = lsm6dsl_write(g_lsm6dsl, reg, &value, 1);
+ if (rc) {
+ console_printf("write failed %d\n", rc);
+ }
+
+ return rc;
+}
+
+static int
+lsm6dsl_shell_cmd_test(int argc, char **argv)
+{
+ int rc;
+ int result;
+
+ rc = lsm6dsl_run_self_test(g_lsm6dsl, &result);
+ if (rc) {
+ console_printf("test not started %d\n", rc);
+ } else {
+ console_printf("Test Result: %x\n", result);
+ }
+
+ return rc;
+}
+
+static int
+lsm6dsl_shell_cmd(int argc, char **argv)
+{
+ if (argc == 1) {
+ lsm6dsl_shell_help();
+
+ return 0;
+ }
+
+ if (lsm6dsl_shell_open_device()) {
+ console_printf("Error: device not found \"%s\"\n",
+ MYNEWT_VAL(LSM6DSL_SHELL_DEV_NAME));
+ }
+
+ if (argc > 1 && strcmp(argv[1], "dump") == 0) {
+ return lsm6dsl_shell_cmd_dump(argc, argv);
+ }
+
+ if (argc > 1 && strcmp(argv[1], "read") == 0) {
+ return lsm6dsl_shell_cmd_read(argc, argv);
+ }
+
+ if (argc > 1 && strcmp(argv[1], "write") == 0) {
+ return lsm6dsl_shell_cmd_write(argc, argv);
+ }
+
+ if (argc > 1 && strcmp(argv[1], "test") == 0) {
+ return lsm6dsl_shell_cmd_test(argc, argv);
+ }
+
+ return lsm6dsl_shell_err_unknown_arg(argv[1]);
+}
+
+int
+lsm6dsl_shell_init(void)
+{
+ int rc;
+
+ rc = shell_cmd_register(&lsm6dsl_shell_cmd_struct);
+ SYSINIT_PANIC_ASSERT(rc == 0);
+
+ return rc;
+}
+
+void lsm6dsl_pkg_init(void)
+{
+#if MYNEWT_VAL(LSM6DSL_CLI)
+ (void)lsm6dsl_shell_init();
+#endif
+}
diff --git a/hw/drivers/sensors/lsm6dsl/syscfg.yml b/hw/drivers/sensors/lsm6dsl/syscfg.yml
new file mode 100644
index 0000000..56384d2
--- /dev/null
+++ b/hw/drivers/sensors/lsm6dsl/syscfg.yml
@@ -0,0 +1,83 @@
+#
+# 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.
+#
+
+syscfg.defs:
+ LSM6DSL_SPI_SUPPORT:
+ description: 'Whether LSM6DSL driver should support SPI interface'
+ value: 1
+ LSM6DSL_I2C_SUPPORT:
+ description: 'Whether LSM6DSL driver should support I2C interface'
+ value: 1
+ LSM6DSL_ITF_LOCK_TMO:
+ description: 'LSM6DSL interface lock timeout in milliseconds'
+ value: 1000
+ LSM6DSL_I2C_RETRIES:
+ description: >
+ Number of retries to use for failed I2C communication.
+ value: 2
+ LSM6DSL_I2C_TIMEOUT_TICKS:
+ description: >
+ Number of OS ticks to wait for each I2C transaction to complete.
+ value: 3
+ LSM6DSL_CLI:
+ description: 'Enable shell support for the LSM6DSL'
+ value: 0
+ LSM6DSL_SHELL_DEV_NAME:
+ description: 'Name of LSM6DSL device to use in shell'
+ value: '"lsm6dsl_0"'
+ LSM6DSL_NOTIF_STATS:
+ description: 'Enable notification stats'
+ value: 1
+ LSM6DSL_INT_ENABLE:
+ description: 'Enable interrupt support, necessary for events'
+ value: 1
+ LSM6DSL_INT_CFG_ACTIVE:
+ description: 'Set 0 for active-low, 1 for active-high'
+ value: 1
+ LSM6DSL_INT1_PIN_HOST:
+ description: 'Interrupt pin number on host device connected to INT1 on device'
+ value: -1
+ LSM6DSL_INT1_PIN_DEVICE:
+ description: 'Interrupt pin number 1 or 2 on accelerometer device'
+ value: 1
+ LSM6DSL_INT2_PIN_HOST:
+ description: 'Interrupt pin number on host device connected to INT1 on device'
+ value: -1
+ LSM6DSL_INT2_PIN_DEVICE:
+ description: 'Interrupt pin number 1 or 2 on accelerometer device'
+ value: 2
+
+ LSM6DSL_SYSINIT_STAGE:
+ description: >
+ Sysinit stage for the LSM6DSL package.
+ value: 1000
+
+ ### Log settings.
+
+ LSM6DSL_LOG_MODULE:
+ description: 'Numeric module ID to use for LSM6DSL log messages.'
+ value: 114
+ LSM6DSL_LOG_LVL:
+ description: 'Minimum level for the LSM6DSL log.'
+ value: 1
+
+syscfg.logs:
+ LSM6DSL_LOG:
+ module: MYNEWT_VAL(LSM6DSL_LOG_MODULE)
+ level: MYNEWT_VAL(LSM6DSL_LOG_LVL)