diff options
-rw-r--r-- | board/samus/board.c | 3 | ||||
-rw-r--r-- | driver/accel_kionix.c | 37 | ||||
-rw-r--r-- | driver/accelgyro_lsm6ds0.c | 38 | ||||
-rw-r--r-- | include/motion_sense.h | 4 | ||||
-rw-r--r-- | test/motion_lid.c | 2 |
5 files changed, 48 insertions, 36 deletions
diff --git a/board/samus/board.c b/board/samus/board.c index 0904850b9a..c586f2e3dd 100644 --- a/board/samus/board.c +++ b/board/samus/board.c @@ -299,6 +299,7 @@ struct motion_sensor_t motion_sensors[] = { .drv = &lsm6ds0_drv, .mutex = &g_base_mutex, .drv_data = &g_saved_data[0], + .port = I2C_PORT_ACCEL, .addr = LSM6DS0_ADDR1, .rot_standard_ref = &base_standard_ref, .default_range = 2, /* g, enough for laptop. */ @@ -333,6 +334,7 @@ struct motion_sensor_t motion_sensors[] = { .drv = &kionix_accel_drv, .mutex = &g_lid_mutex, .drv_data = &g_kxcj9_data, + .port = I2C_PORT_ACCEL, .addr = KXCJ9_ADDR0, .rot_standard_ref = &lid_standard_ref, .default_range = 2, /* g, enough for laptop. */ @@ -367,6 +369,7 @@ struct motion_sensor_t motion_sensors[] = { .drv = &lsm6ds0_drv, .mutex = &g_base_mutex, .drv_data = &g_saved_data[1], + .port = I2C_PORT_ACCEL, .addr = LSM6DS0_ADDR1, .rot_standard_ref = NULL, .default_range = 2000, /* g, enough for laptop. */ diff --git a/driver/accel_kionix.c b/driver/accel_kionix.c index 8d86a5bbce..0a3754b4dd 100644 --- a/driver/accel_kionix.c +++ b/driver/accel_kionix.c @@ -125,17 +125,18 @@ static int find_param_index(const int eng_val, const int round_up, /** * Read register from accelerometer. */ -static int raw_read8(const int addr, const int reg, int *data_ptr) +static int raw_read8(const int port, const int addr, const int reg, + int *data_ptr) { - return i2c_read8(I2C_PORT_ACCEL, addr, reg, data_ptr); + return i2c_read8(port, addr, reg, data_ptr); } /** * Write register from accelerometer. */ -static int raw_write8(const int addr, const int reg, int data) +static int raw_write8(const int port, const int addr, const int reg, int data) { - return i2c_write8(I2C_PORT_ACCEL, addr, reg, data); + return i2c_write8(port, addr, reg, data); } /** @@ -162,13 +163,13 @@ static int disable_sensor(const struct motion_sensor_t *s, int *reg_val) * so that we can restore it later. */ for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) { - ret = raw_read8(s->addr, reg, reg_val); + ret = raw_read8(s->port, s->addr, reg, reg_val); if (ret != EC_SUCCESS) continue; *reg_val &= ~pc1_field; - ret = raw_write8(s->addr, reg, *reg_val); + ret = raw_write8(s->port, s->addr, reg, *reg_val); if (ret == EC_SUCCESS) return EC_SUCCESS; } @@ -194,12 +195,12 @@ static int enable_sensor(const struct motion_sensor_t *s, int reg_val) pc1_field = KIONIX_PC1_FIELD(data->variant); for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) { - ret = raw_read8(s->addr, reg, ®_val); + ret = raw_read8(s->port, s->addr, reg, ®_val); if (ret != EC_SUCCESS) continue; /* Enable accelerometer based on reg_val value. */ - ret = raw_write8(s->addr, reg, + ret = raw_write8(s->port, s->addr, reg, reg_val | pc1_field); /* On first success, we are done. */ @@ -230,7 +231,7 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd) /* Determine new value of control reg and attempt to write it. */ reg_val_new = (reg_val & ~range_field) | range_val; - ret = raw_write8(s->addr, reg, reg_val_new); + ret = raw_write8(s->port, s->addr, reg, reg_val_new); /* If successfully written, then save the range. */ if (ret == EC_SUCCESS) { @@ -274,7 +275,7 @@ static int set_resolution(const struct motion_sensor_t *s, int res, int rnd) /* Determine new value of the control reg and attempt to write it. */ reg_val_new = (reg_val & ~res_field) | res_val; - ret = raw_write8(s->addr, reg, reg_val_new); + ret = raw_write8(s->port, s->addr, reg, reg_val_new); /* If successfully written, then save the range. */ if (ret == EC_SUCCESS) { @@ -318,14 +319,14 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) } /* Determine the new value of control reg and attempt to write it. */ - ret = raw_read8(s->addr, reg, &odr_reg_val); + ret = raw_read8(s->port, s->addr, reg, &odr_reg_val); if (ret != EC_SUCCESS) { mutex_unlock(s->mutex); return ret; } odr_val_new = (odr_reg_val & ~odr_field) | odr_val; /* Set output data rate. */ - ret = raw_write8(s->addr, reg, odr_val_new); + ret = raw_write8(s->port, s->addr, reg, odr_val_new); /* If successfully written, then save the new data rate. */ if (ret == EC_SUCCESS) @@ -377,10 +378,10 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) /* Read 6 bytes starting at XOUT_L. */ reg = KIONIX_XOUT_L(data->variant); mutex_lock(s->mutex); - i2c_lock(I2C_PORT_ACCEL, 1); - ret = i2c_xfer(I2C_PORT_ACCEL, s->addr, ®, 1, acc, 6, + i2c_lock(s->port, 1); + ret = i2c_xfer(s->port, s->addr, ®, 1, acc, 6, I2C_XFER_SINGLE); - i2c_lock(I2C_PORT_ACCEL, 0); + i2c_lock(s->port, 0); mutex_unlock(s->mutex); if (ret != EC_SUCCESS) @@ -439,13 +440,13 @@ static int init(const struct motion_sensor_t *s) mutex_unlock(s->mutex); return ret; } - ret = raw_read8(s->addr, reg, &val); + ret = raw_read8(s->port, s->addr, reg, &val); if (ret != EC_SUCCESS) { mutex_unlock(s->mutex); return ret; } val |= reset_field; - ret = raw_write8(s->addr, reg, val); + ret = raw_write8(s->port, s->addr, reg, val); if (ret != EC_SUCCESS) { mutex_unlock(s->mutex); return ret; @@ -456,7 +457,7 @@ static int init(const struct motion_sensor_t *s) do { msleep(1); - ret = raw_read8(s->addr, reg, &val); + ret = raw_read8(s->port, s->addr, reg, &val); if (ret != EC_SUCCESS) { mutex_unlock(s->mutex); return ret; diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c index 62996825ac..7948e651ce 100644 --- a/driver/accelgyro_lsm6ds0.c +++ b/driver/accelgyro_lsm6ds0.c @@ -150,17 +150,19 @@ static int get_engineering_val(const int reg_val, /** * Read register from accelerometer. */ -static inline int raw_read8(const int addr, const int reg, int *data_ptr) +static inline int raw_read8(const int port, const int addr, const int reg, + int *data_ptr) { - return i2c_read8(I2C_PORT_ACCEL, addr, reg, data_ptr); + return i2c_read8(port, addr, reg, data_ptr); } /** * Write register from accelerometer. */ -static inline int raw_write8(const int addr, const int reg, int data) +static inline int raw_write8(const int port, const int addr, const int reg, + int data) { - return i2c_write8(I2C_PORT_ACCEL, addr, reg, data); + return i2c_write8(port, addr, reg, data); } static int set_range(const struct motion_sensor_t *s, @@ -183,12 +185,12 @@ static int set_range(const struct motion_sensor_t *s, */ mutex_lock(s->mutex); - ret = raw_read8(s->addr, ctrl_reg, &ctrl_val); + ret = raw_read8(s->port, s->addr, ctrl_reg, &ctrl_val); if (ret != EC_SUCCESS) goto accel_cleanup; ctrl_val = (ctrl_val & ~LSM6DS0_RANGE_MASK) | reg_val; - ret = raw_write8(s->addr, ctrl_reg, ctrl_val); + ret = raw_write8(s->port, s->addr, ctrl_reg, ctrl_val); /* Now that we have set the range, update the driver's value. */ if (ret == EC_SUCCESS) @@ -239,12 +241,12 @@ static int set_data_rate(const struct motion_sensor_t *s, */ mutex_lock(s->mutex); - ret = raw_read8(s->addr, ctrl_reg, &val); + ret = raw_read8(s->port, s->addr, ctrl_reg, &val); if (ret != EC_SUCCESS) goto accel_cleanup; val = (val & ~LSM6DS0_ODR_MASK) | reg_val; - ret = raw_write8(s->addr, ctrl_reg, val); + ret = raw_write8(s->port, s->addr, ctrl_reg, val); /* Now that we have set the odr, update the driver's value. */ if (ret == EC_SUCCESS) @@ -259,14 +261,14 @@ static int set_data_rate(const struct motion_sensor_t *s, * Table 48 Gyroscope high-pass filter cutoff frequency */ if (MOTIONSENSE_TYPE_GYRO == s->type) { - ret = raw_read8(s->addr, LSM6DS0_CTRL_REG3_G, &val); + ret = raw_read8(s->port, s->addr, LSM6DS0_CTRL_REG3_G, &val); if (ret != EC_SUCCESS) goto accel_cleanup; val &= ~(0x3 << 4); /* clear bit [5:4] */ val = (rate > 119000) ? (val | (1<<7)) /* set high-power mode */ : (val & ~(1<<7)); /* set low-power mode */ - ret = raw_write8(s->addr, LSM6DS0_CTRL_REG3_G, val); + ret = raw_write8(s->port, s->addr, LSM6DS0_CTRL_REG3_G, val); } accel_cleanup: @@ -318,7 +320,7 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready) { int ret, tmp; - ret = raw_read8(s->addr, LSM6DS0_STATUS_REG, &tmp); + ret = raw_read8(s->port, s->addr, LSM6DS0_STATUS_REG, &tmp); if (ret != EC_SUCCESS) { CPRINTF("[%T %s type:0x%X RS Error]", s->name, s->type); @@ -358,10 +360,10 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) xyz_reg = get_xyz_reg(s->type); /* Read 6 bytes starting at xyz_reg */ - i2c_lock(I2C_PORT_ACCEL, 1); - ret = i2c_xfer(I2C_PORT_ACCEL, s->addr, + i2c_lock(s->port, 1); + ret = i2c_xfer(s->port, s->addr, &xyz_reg, 1, raw, 6, I2C_XFER_SINGLE); - i2c_lock(I2C_PORT_ACCEL, 0); + i2c_lock(s->port, 0); if (ret != EC_SUCCESS) { CPRINTF("[%T %s type:0x%X RD XYZ Error]", @@ -386,7 +388,7 @@ static int init(const struct motion_sensor_t *s) { int ret = 0, tmp; - ret = raw_read8(s->addr, LSM6DS0_WHO_AM_I_REG, &tmp); + ret = raw_read8(s->port, s->addr, LSM6DS0_WHO_AM_I_REG, &tmp); if (ret) return EC_ERROR_UNKNOWN; @@ -408,20 +410,20 @@ static int init(const struct motion_sensor_t *s) if (MOTIONSENSE_TYPE_ACCEL == s->type) { mutex_lock(s->mutex); - ret = raw_read8(s->addr, LSM6DS0_CTRL_REG8, &tmp); + ret = raw_read8(s->port, s->addr, LSM6DS0_CTRL_REG8, &tmp); if (ret) { mutex_unlock(s->mutex); return EC_ERROR_UNKNOWN; } tmp |= (1 | LSM6DS0_BDU_ENABLE); - ret = raw_write8(s->addr, LSM6DS0_CTRL_REG8, tmp); + ret = raw_write8(s->port, s->addr, LSM6DS0_CTRL_REG8, tmp); mutex_unlock(s->mutex); if (ret) return EC_ERROR_UNKNOWN; /* Power Down Gyro */ - ret = raw_write8(s->addr, + ret = raw_write8(s->port, s->addr, LSM6DS0_CTRL_REG1_G, 0x0); if (ret) return EC_ERROR_UNKNOWN; diff --git a/include/motion_sense.h b/include/motion_sense.h index 707a561049..afa635e90d 100644 --- a/include/motion_sense.h +++ b/include/motion_sense.h @@ -72,8 +72,12 @@ struct motion_sensor_t { const struct accelgyro_drv *drv; struct mutex *mutex; void *drv_data; + + /* i2c port */ + uint8_t port; /* i2c address or SPI slave logic GPIO. */ uint8_t addr; + const matrix_3x3_t *rot_standard_ref; /* diff --git a/test/motion_lid.c b/test/motion_lid.c index 224b12a26b..81ab132300 100644 --- a/test/motion_lid.c +++ b/test/motion_lid.c @@ -117,6 +117,7 @@ struct motion_sensor_t motion_sensors[] = { .drv = &test_motion_sense, .mutex = NULL, .drv_data = NULL, + .port = 0, .addr = 0, .rot_standard_ref = &base_standard_ref, .default_range = 2, /* g, enough for laptop. */ @@ -150,6 +151,7 @@ struct motion_sensor_t motion_sensors[] = { .drv = &test_motion_sense, .mutex = NULL, .drv_data = NULL, + .port = 0, .addr = 0, .rot_standard_ref = &lid_standard_ref, .default_range = 2, /* g, enough for laptop. */ |