motion: Add decoding for MOTION_CMD_DUMP v1 command

MOTIONSENSE_CMD_DUMP is deprecated, replaced with MOTIONSENSE_CMD_GET_DATA
Also use vector_3_t instead of x,y,z

ectool motionsense commands only work with newer firmware, to
handle a dynamic number of sensors.
- The host sends the number of sensor it has allocated space for.
- If 0, the EC just sends the number of sensors available
- Otherwise returns sensor information up to the limit imposed by the host.

Remove MOTIONSENSE_GET_STATUS: not needed. It is only useful for LPC,
to guarantee atomicity of the data.
Remove MOTIONSENSE_GET_DATA: not needed since we increase the version
number of MOTIONSENSE command.

BUG=chrome-os-partner:31071,chromium:430792
BRANCH=ToT
TEST=Compile. On a firmware that support the new command:
/usr/sbin/ectool --name=cros_sh motionsense
Motion sensing active
Sensor 0: 92    15      1030
Sensor 1: -94   -63     718
/usr/sbin/ectool --name=cros_sh motionsense active
0
On a machine with older firmware (samus), check these
functions are not working anymore.

Change-Id: I64b62afff96670fb93457760d43d4e64e26e029f
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/226880
Reviewed-by: Alec Berg <alecaberg@chromium.org>

9 files changed, 201 insertions, 296 deletions

diff --git a/board/samus/board.c b/board/samus/board.c
index 68c5146438..d68b2bb2e6 100644
--- a/board/samus/board.c
+++ b/board/samus/board.c
@@ -287,18 +287,18 @@ struct motion_sensor_t motion_sensors[] = {
 	 * Requriement: accelerometer sensor must init before gyro sensor
 	 * DO NOT change the order of the following table.
 	 */
-	{SENSOR_ACTIVE_S0_S3_S5, "Base", SENSOR_CHIP_LSM6DS0,
-		SENSOR_ACCELEROMETER, LOCATION_BASE,
+	{SENSOR_ACTIVE_S0_S3_S5, "Base", MOTIONSENSE_CHIP_LSM6DS0,
+		MOTIONSENSE_TYPE_ACCEL, MOTIONSENSE_LOC_BASE,
 		&lsm6ds0_drv, &g_base_mutex, NULL,
 		LSM6DS0_ADDR1, &base_standard_ref, 119000, 2},
 
-	{SENSOR_ACTIVE_S0, "Lid",  SENSOR_CHIP_KXCJ9,
-		SENSOR_ACCELEROMETER, LOCATION_LID,
+	{SENSOR_ACTIVE_S0, "Lid",  MOTIONSENSE_CHIP_KXCJ9,
+		MOTIONSENSE_TYPE_ACCEL, MOTIONSENSE_LOC_LID,
 		&kxcj9_drv, &g_lid_mutex, &g_kxcj9_data,
 		KXCJ9_ADDR0, &lid_standard_ref, 100000, 2},
 
-	{SENSOR_ACTIVE_S0, "Base Gyro", SENSOR_CHIP_LSM6DS0,
-		SENSOR_GYRO, LOCATION_BASE,
+	{SENSOR_ACTIVE_S0, "Base Gyro", MOTIONSENSE_CHIP_LSM6DS0,
+		MOTIONSENSE_TYPE_GYRO, MOTIONSENSE_LOC_BASE,
 		&lsm6ds0_drv, &g_base_mutex, NULL,
 		LSM6DS0_ADDR1, NULL, 119000, 2000},
 
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 21b36a1e7d..fd9c64bc7d 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -61,13 +61,12 @@ static int accel_disp;
 #endif
 
 /*
- * Angle threshold for how close the hinge aligns with gravity before
- * considering the lid angle calculation unreliable. For computational
- * efficiency, value is given unit-less, so if you want the threshold to be
- * at 15 degrees, the value would be cos(15 deg) = 0.96593.
+ * Mutex to protect sensor values between host command task and
+ * motion sense task:
+ * When we process CMD_DUMP, we want to be sure the motion sense
+ * task is not updating the sensor values at the same time.
  */
-#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F
-
+static struct mutex g_sensor_mutex;
 
 static void motion_sense_shutdown(void)
 {
@@ -197,21 +196,11 @@ static inline void motion_sense_init(struct motion_sensor_t *sensor)
 
 static int motion_sense_read(struct motion_sensor_t *sensor)
 {
-	int ret;
-
 	if (sensor->state != SENSOR_INITIALIZED)
 		return EC_ERROR_UNKNOWN;
 
 	/* Read all raw X,Y,Z accelerations. */
-	ret = sensor->drv->read(sensor,
-		&sensor->xyz[X],
-		&sensor->xyz[Y],
-		&sensor->xyz[Z]);
-
-	if (ret != EC_SUCCESS)
-		return EC_ERROR_UNKNOWN;
-
-	return EC_SUCCESS;
+	return sensor->drv->read(sensor, sensor->raw_xyz);
 }
 
 /*
@@ -279,11 +268,15 @@ void motion_sense_task(void)
 				 * Rotate the accel vector so the reference for
 				 * all sensors are in the same space.
 				 */
-				if (*sensor->rot_standard_ref != NULL) {
-					rotate(sensor->xyz,
+				mutex_lock(&g_sensor_mutex);
+				if (*sensor->rot_standard_ref != NULL)
+					rotate(sensor->raw_xyz,
 					       *sensor->rot_standard_ref,
 					       sensor->xyz);
-				}
+				else
+					memcpy(sensor->xyz, sensor->raw_xyz,
+						sizeof(vector_3_t));
+				mutex_unlock(&g_sensor_mutex);
 			}
 		}
 
@@ -294,7 +287,6 @@ void motion_sense_task(void)
 #ifdef CONFIG_LID_ANGLE
 		if (rd_cnt == motion_sensor_count)
 			motion_lid_calc();
-
 #endif
 #ifdef CONFIG_CMD_ACCEL_INFO
 		if (accel_disp) {
@@ -336,34 +328,11 @@ void motion_sense_task(void)
 static struct motion_sensor_t
 	*host_sensor_id_to_motion_sensor(int host_id)
 {
-	int i;
-	struct motion_sensor_t *sensor = NULL;
-
-	for (i = 0; i < motion_sensor_count; ++i) {
-
-		sensor = &motion_sensors[i];
-
-		if ((LOCATION_BASE == sensor->location)
-			&& (SENSOR_ACCELEROMETER == sensor->type)
-			&& (host_id == EC_MOTION_SENSOR_ACCEL_BASE)) {
-			break;
-		}
-
-		if ((LOCATION_LID == sensor->location)
-			&& (SENSOR_ACCELEROMETER == sensor->type)
-			&& (host_id == EC_MOTION_SENSOR_ACCEL_LID)) {
-			break;
-		}
-
-		if ((LOCATION_BASE == sensor->location)
-			&& (SENSOR_GYRO == sensor->type)
-			&& (host_id == EC_MOTION_SENSOR_GYRO)) {
-			break;
-		}
-	}
+	struct motion_sensor_t *sensor;
 
-	if (i == motion_sensor_count)
+	if (host_id >= motion_sensor_count)
 		return NULL;
+	sensor = &motion_sensors[host_id];
 
 	/* if sensor is powered and initialized, return match */
 	if ((sensor->active & sensor->active_mask)
@@ -379,54 +348,44 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	const struct ec_params_motion_sense *in = args->params;
 	struct ec_response_motion_sense *out = args->response;
 	struct motion_sensor_t *sensor;
-	int i, data, ret = EC_RES_INVALID_PARAM;
+	int i, data, ret = EC_RES_INVALID_PARAM, reported;
 
 	switch (in->cmd) {
 	case MOTIONSENSE_CMD_DUMP:
 		out->dump.module_flags =
 			(*(host_get_memmap(EC_MEMMAP_ACC_STATUS)) &
-				EC_MEMMAP_ACC_STATUS_PRESENCE_BIT) ?
-					MOTIONSENSE_MODULE_FLAG_ACTIVE : 0;
-
-		for (i = 0; i < motion_sensor_count; i++) {
+			 EC_MEMMAP_ACC_STATUS_PRESENCE_BIT) ?
+			MOTIONSENSE_MODULE_FLAG_ACTIVE : 0;
+		out->dump.sensor_count = motion_sensor_count;
+		args->response_size = sizeof(out->dump);
+		reported = MIN(motion_sensor_count, in->dump.max_sensor_count);
+		mutex_lock(&g_sensor_mutex);
+		for (i = 0; i < reported; i++) {
 			sensor = &motion_sensors[i];
-			out->dump.sensor_flags[i] =
+			out->dump.sensor[i].flags =
 				MOTIONSENSE_SENSOR_FLAG_PRESENT;
-			out->dump.data[0+3*i] = sensor->xyz[X];
-			out->dump.data[1+3*i] = sensor->xyz[Y];
-			out->dump.data[2+3*i] = sensor->xyz[Z];
+			/* casting from int to s16 */
+			out->dump.sensor[i].data[X] = sensor->xyz[X];
+			out->dump.sensor[i].data[Y] = sensor->xyz[Y];
+			out->dump.sensor[i].data[Z] = sensor->xyz[Z];
 		}
-
-		args->response_size = sizeof(out->dump);
+		mutex_unlock(&g_sensor_mutex);
+		args->response_size += reported *
+			sizeof(struct ec_response_motion_sensor_data);
 		break;
 
 	case MOTIONSENSE_CMD_INFO:
 		sensor = host_sensor_id_to_motion_sensor(
-			in->sensor_odr.sensor_num);
+				in->sensor_odr.sensor_num);
 
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;
 
-		if (sensor->type == SENSOR_ACCELEROMETER)
-			out->info.type = MOTIONSENSE_TYPE_ACCEL;
-
-		else if (sensor->type == SENSOR_GYRO)
-			out->info.type = MOTIONSENSE_TYPE_GYRO;
-
-		if (sensor->location == LOCATION_BASE)
-			out->info.location = MOTIONSENSE_LOC_BASE;
-
-		else if (sensor->location == LOCATION_LID)
-			out->info.location = MOTIONSENSE_LOC_LID;
-
-		if (sensor->chip == SENSOR_CHIP_KXCJ9)
-			out->info.chip = MOTIONSENSE_CHIP_KXCJ9;
-
-		if (sensor->chip == SENSOR_CHIP_LSM6DS0)
-			out->info.chip = MOTIONSENSE_CHIP_LSM6DS0;
+		out->info.type = sensor->type;
+		out->info.location = sensor->location;
+		out->info.chip = sensor->chip;
 
 		args->response_size = sizeof(out->info);
-
 		break;
 
 	case MOTIONSENSE_CMD_EC_RATE:
@@ -454,16 +413,16 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	case MOTIONSENSE_CMD_SENSOR_ODR:
 		/* Verify sensor number is valid. */
 		sensor = host_sensor_id_to_motion_sensor(
-			in->sensor_odr.sensor_num);
+				in->sensor_odr.sensor_num);
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;
 
 		/* Set new data rate if the data arg has a value. */
 		if (in->sensor_odr.data != EC_MOTION_SENSE_NO_VALUE) {
 			if (sensor->drv->set_data_rate(sensor,
-						      in->sensor_odr.data,
-						      in->sensor_odr.roundup)
-						      != EC_SUCCESS) {
+						in->sensor_odr.data,
+						in->sensor_odr.roundup)
+					!= EC_SUCCESS) {
 				CPRINTS("MS bad sensor rate %d",
 						in->sensor_odr.data);
 				return EC_RES_INVALID_PARAM;
@@ -482,16 +441,16 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	case MOTIONSENSE_CMD_SENSOR_RANGE:
 		/* Verify sensor number is valid. */
 		sensor = host_sensor_id_to_motion_sensor(
-			in->sensor_odr.sensor_num);
+				in->sensor_odr.sensor_num);
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;
 
 		/* Set new data rate if the data arg has a value. */
 		if (in->sensor_range.data != EC_MOTION_SENSE_NO_VALUE) {
 			if (sensor->drv->set_range(sensor,
-						   in->sensor_range.data,
-						   in->sensor_range.roundup)
-						   != EC_SUCCESS) {
+						in->sensor_range.data,
+						in->sensor_range.roundup)
+					!= EC_SUCCESS) {
 				CPRINTS("MS bad sensor range %d",
 						in->sensor_range.data);
 				return EC_RES_INVALID_PARAM;
@@ -522,7 +481,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 
 DECLARE_HOST_COMMAND(EC_CMD_MOTION_SENSE_CMD,
 		     host_cmd_motion_sense,
-		     EC_VER_MASK(0));
+		     EC_VER_MASK(1));
 
 /*****************************************************************************/
 /* Console commands */
@@ -672,8 +631,9 @@ DECLARE_CONSOLE_COMMAND(accelrate, command_accel_data_rate,
 static int command_accel_read_xyz(int argc, char **argv)
 {
 	char *e;
-	int id, x, y, z, n = 1;
+	int id, n = 1;
 	struct motion_sensor_t *sensor;
+	vector_3_t v;
 
 	if (argc < 2)
 		return EC_ERROR_PARAM_COUNT;
@@ -690,11 +650,11 @@ static int command_accel_read_xyz(int argc, char **argv)
 	sensor = &motion_sensors[id];
 
 	while ((n == -1) || (n-- > 0)) {
-		x = y = z = 0;
-		sensor->drv->read(sensor, &x, &y, &z);
-		ccprintf("Current raw data %d: %-5d %-5d %-5d\n", id, x, y, z);
-		ccprintf("Last calib. data %d: %-5d %-5d %-5d\n", id,
-			 sensor->xyz[X], sensor->xyz[Y], sensor->xyz[Z]);
+		sensor->drv->read(sensor, v);
+		ccprintf("Current raw data %d: %-5d %-5d %-5d\n",
+			 id, v[X], v[Y], v[Z]);
+		ccprintf("Last calib. data %d: %-5d %-5d %-5d\n",
+			 id, sensor->xyz[X], sensor->xyz[Y], sensor->xyz[Z]);
 		task_wait_event(MIN_MOTION_SENSE_WAIT_TIME);
 	}
 	return EC_SUCCESS;
diff --git a/driver/accel_kxcj9.c b/driver/accel_kxcj9.c
index 22acbad5bd..aa2a8ed9c5 100644
--- a/driver/accel_kxcj9.c
+++ b/driver/accel_kxcj9.c
@@ -363,10 +363,7 @@ error_enable_sensor:
 }
 #endif
 
-static int read(const struct motion_sensor_t *s,
-			int *x_acc,
-			int *y_acc,
-			int *z_acc)
+static int read(const struct motion_sensor_t *s, vector_3_t v)
 {
 	uint8_t acc[6];
 	uint8_t reg = KXCJ9_XOUT_L;
@@ -411,9 +408,9 @@ static int read(const struct motion_sensor_t *s,
 	 * acc[4] = KXCJ9_ZOUT_L
 	 * acc[5] = KXCJ9_ZOUT_H
 	 */
-	*x_acc = multiplier * (((int8_t)acc[1]) << 4) | (acc[0] >> 4);
-	*y_acc = multiplier * (((int8_t)acc[3]) << 4) | (acc[2] >> 4);
-	*z_acc = multiplier * (((int8_t)acc[5]) << 4) | (acc[4] >> 4);
+	v[0] = multiplier * (((int8_t)acc[1]) << 4) | (acc[0] >> 4);
+	v[1] = multiplier * (((int8_t)acc[3]) << 4) | (acc[2] >> 4);
+	v[2] = multiplier * (((int8_t)acc[5]) << 4) | (acc[4] >> 4);
 
 	return EC_SUCCESS;
 }
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c
index d25b91b594..e907995b70 100644
--- a/driver/accelgyro_lsm6ds0.c
+++ b/driver/accelgyro_lsm6ds0.c
@@ -48,9 +48,9 @@ const struct accel_param_pair dps_ranges[] = {
 };
 
 static inline const struct accel_param_pair *get_range_table(
-		enum sensor_type_t type, int *psize)
+		enum motionsensor_type type, int *psize)
 {
-	if (SENSOR_ACCELEROMETER == type) {
+	if (MOTIONSENSE_TYPE_ACCEL == type) {
 		if (psize)
 			*psize = ARRAY_SIZE(g_ranges);
 		return g_ranges;
@@ -84,9 +84,9 @@ const struct accel_param_pair gyro_off_odr[] = {
 };
 
 static inline const struct accel_param_pair *get_odr_table(
-		enum sensor_type_t type, int *psize)
+		enum motionsensor_type type, int *psize)
 {
-	if (SENSOR_ACCELEROMETER == type) {
+	if (MOTIONSENSE_TYPE_ACCEL == type) {
 		if (psize)
 			*psize = ARRAY_SIZE(gyro_off_odr);
 		return gyro_off_odr;
@@ -97,15 +97,15 @@ static inline const struct accel_param_pair *get_odr_table(
 	}
 }
 
-static inline int get_ctrl_reg(enum sensor_type_t type)
+static inline int get_ctrl_reg(enum motionsensor_type type)
 {
-	return (SENSOR_ACCELEROMETER == type) ?
+	return (MOTIONSENSE_TYPE_ACCEL == type) ?
 		LSM6DS0_CTRL_REG6_XL : LSM6DS0_CTRL_REG1_G;
 }
 
-static inline int get_xyz_reg(enum sensor_type_t type)
+static inline int get_xyz_reg(enum motionsensor_type type)
 {
-	return (SENSOR_ACCELEROMETER == type) ?
+	return (MOTIONSENSE_TYPE_ACCEL == type) ?
 		LSM6DS0_OUT_X_L_XL : LSM6DS0_OUT_X_L_G;
 }
 
@@ -254,7 +254,7 @@ static int set_data_rate(const struct motion_sensor_t *s,
 	 * [3:0] HPCF_G
 	 *       Table 48 Gyroscope high-pass filter cutoff frequency
 	 */
-	if (SENSOR_GYRO == s->type) {
+	if (MOTIONSENSE_TYPE_GYRO == s->type) {
 		ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG3_G, &val);
 		if (ret != EC_SUCCESS)
 			goto accel_cleanup;
@@ -308,7 +308,7 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready)
 		return ret;
 	}
 
-	if (SENSOR_ACCELEROMETER == s->type)
+	if (MOTIONSENSE_TYPE_ACCEL == s->type)
 		*ready = (LSM6DS0_STS_XLDA_UP == (tmp & LSM6DS0_STS_XLDA_MASK));
 	else
 		*ready = (LSM6DS0_STS_GDA_UP == (tmp & LSM6DS0_STS_GDA_MASK));
@@ -316,10 +316,7 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready)
 	return EC_SUCCESS;
 }
 
-static int read(const struct motion_sensor_t *s,
-			int *x,
-			int *y,
-			int *z)
+static int read(const struct motion_sensor_t *s, vector_3_t v)
 {
 	uint8_t data[6];
 	uint8_t xyz_reg;
@@ -335,9 +332,9 @@ static int read(const struct motion_sensor_t *s,
 	 * to get the latest updated sensor data quickly.
 	 */
 	if (!tmp) {
-		*x = s->xyz[0];
-		*y = s->xyz[1];
-		*z = s->xyz[2];
+		v[0] = s->xyz[0];
+		v[1] = s->xyz[1];
+		v[2] = s->xyz[2];
 		return EC_SUCCESS;
 	}
 
@@ -355,27 +352,27 @@ static int read(const struct motion_sensor_t *s,
 		return ret;
 	}
 
-	*x = ((int16_t)((data[1] << 8) | data[0]));
-	*y = ((int16_t)((data[3] << 8) | data[2]));
-	*z = ((int16_t)((data[5] << 8) | data[4]));
+	v[0] = ((int16_t)((data[1] << 8) | data[0]));
+	v[1] = ((int16_t)((data[3] << 8) | data[2]));
+	v[2] = ((int16_t)((data[5] << 8) | data[4]));
 
 	ret = get_range(s, &range);
 	if (ret)
 		return EC_ERROR_UNKNOWN;
 
-	*x *= range;
-	*y *= range;
-	*z *= range;
+	v[0] *= range;
+	v[1] *= range;
+	v[2] *= range;
 
 	/* normalize the accel scale: 1G = 1024 */
-	if (SENSOR_ACCELEROMETER == s->type) {
-		*x >>= 5;
-		*y >>= 5;
-		*z >>= 5;
+	if (MOTIONSENSE_TYPE_ACCEL == s->type) {
+		v[0] >>= 5;
+		v[1] >>= 5;
+		v[2] >>= 5;
 	} else {
-		*x >>= 8;
-		*y >>= 8;
-		*z >>= 8;
+		v[0] >>= 8;
+		v[1] >>= 8;
+		v[2] >>= 8;
 	}
 
 	return EC_SUCCESS;
@@ -404,7 +401,7 @@ static int init(const struct motion_sensor_t *s)
 	 * Requirement: Accel need be init before gyro.
 	 * SW_RESET is down for accel only!
 	 */
-	if (SENSOR_ACCELEROMETER == s->type) {
+	if (MOTIONSENSE_TYPE_ACCEL == s->type) {
 
 		mutex_lock(s->mutex);
 		ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG8, &tmp);
@@ -434,7 +431,7 @@ static int init(const struct motion_sensor_t *s)
 			return EC_ERROR_UNKNOWN;
 	}
 
-	if (SENSOR_GYRO == s->type) {
+	if (MOTIONSENSE_TYPE_GYRO == s->type) {
 		/* Config GYRO Range */
 		ret = set_range(s, s->range, 1);
 		if (ret)
diff --git a/include/accelgyro.h b/include/accelgyro.h
index 6118f6dc0c..5ac486c660 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -27,15 +27,10 @@ struct accelgyro_drv {
 	 * three accelerations come back in counts, where ACCEL_G can be used
 	 * to convert counts to engineering units.
 	 * @s Pointer to sensor data.
-	 * @x_acc Pointer to store X-axis acceleration (in counts).
-	 * @y_acc Pointer to store Y-axis acceleration (in counts).
-	 * @z_acc Pointer to store Z-axis acceleration (in counts).
+	 * @v Vector to store acceleration (in units of counts).
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*read)(const struct motion_sensor_t *s,
-			int *x_acc,
-			int *y_acc,
-			int *z_acc);
+	int (*read)(const struct motion_sensor_t *s, vector_3_t v);
 
 	/**
 	 * Setter and getter methods for the sensor range. The sensor range
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 380259eec4..630b837f90 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -1301,34 +1301,10 @@ enum motionsense_command {
 	 */
 	MOTIONSENSE_CMD_KB_WAKE_ANGLE = 5,
 
-	/*
-	 * Sensor subsytem status.
-	 * Same format as EC_MEMMAP_ACC_STATUS
-	 * - for system without LPC -
-	 */
-	MOTIONSENSE_CMD_GET_STATUS = 6,
-
-	/*
-	 * Retrieve data and flags from all accel/gyro sensors.
-	 */
-	MOTIONSENSE_CMD_GET_DATA = 7,
-
 	/* Number of motionsense sub-commands. */
 	MOTIONSENSE_NUM_CMDS
 };
 
-enum motionsensor_id {
-	EC_MOTION_SENSOR_ACCEL_BASE = 0,
-	EC_MOTION_SENSOR_ACCEL_LID = 1,
-	EC_MOTION_SENSOR_GYRO = 2,
-
-	/*
-	 * Note, if more sensors are added and this count changes, the padding
-	 * in ec_response_motion_sense dump command must be modified.
-	 */
-	EC_MOTION_SENSOR_COUNT = 3
-};
-
 /* List of motion sensor types. */
 enum motionsensor_type {
 	MOTIONSENSE_TYPE_ACCEL = 0,
@@ -1363,10 +1339,15 @@ enum motionsensor_chip {
 struct ec_params_motion_sense {
 	uint8_t cmd;
 	union {
-		/* Used for MOTIONSENSE_CMD_DUMP, GET_STATUS, GET_DATA. */
+		/* Used for MOTIONSENSE_CMD_DUMP */
 		struct {
-			/* no args */
-		} data, dump, status;
+			/*
+			 * Maximal number of sensor the host is expecting.
+			 * 0 means the host is only interested in the number
+			 * of sensors controlled by the EC.
+			 */
+			uint8_t max_sensor_count;
+		} dump;
 
 		/*
 		 * Used for MOTIONSENSE_CMD_EC_RATE and
@@ -1400,6 +1381,15 @@ struct ec_params_motion_sense {
 	};
 } __packed;
 
+struct ec_response_motion_sensor_data {
+	/* Flags for each sensor. */
+	uint8_t flags;
+	uint8_t padding;
+
+	/* Each sensor is up to 3-axis. */
+	int16_t data[3];
+} __packed;
+
 struct ec_response_motion_sense {
 	union {
 		/* Used for MOTIONSENSE_CMD_DUMP */
@@ -1407,34 +1397,15 @@ struct ec_response_motion_sense {
 			/* Flags representing the motion sensor module. */
 			uint8_t module_flags;
 
-			/* Flags for each sensor. */
-			uint8_t sensor_flags[EC_MOTION_SENSOR_COUNT];
-
-			/* Array of all sensor data. Each sensor is 3-axis. */
-			int16_t data[3*EC_MOTION_SENSOR_COUNT];
-		} dump;
-
-		/* Used for MOTIONSENSE_CMD_GET_DATA */
-		struct {
-			/* Flags representing the motion sensor module. */
-			uint8_t module_flags;
-
 			/* Number of sensors managed directly by the EC */
-			uint8_t sensor_number;
+			uint8_t sensor_count;
 
 			/*
 			 * sensor data is truncated if response_max is too small
 			 * for holding all the data.
 			 */
-			struct sensor_data {
-				/* Flags for each sensor. */
-				uint8_t flags;
-				uint8_t padding;
-
-				/* Each sensor is up to 3-axis. */
-				int16_t data[3];
-			} sensor[0];
-		} data;
+			struct ec_response_motion_sensor_data sensor[0];
+		} dump;
 
 		/* Used for MOTIONSENSE_CMD_INFO. */
 		struct {
@@ -1448,11 +1419,6 @@ struct ec_response_motion_sense {
 			uint8_t chip;
 		} info;
 
-		/* Used for MOTIONSENSE_CMD_GET_STATUS */
-		struct {
-			uint8_t value;
-		} status;
-
 		/*
 		 * Used for MOTIONSENSE_CMD_EC_RATE, MOTIONSENSE_CMD_SENSOR_ODR,
 		 * MOTIONSENSE_CMD_SENSOR_RANGE, and
diff --git a/include/motion_sense.h b/include/motion_sense.h
index bb5acc308c..9f971bb184 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -8,24 +8,11 @@
 #ifndef __CROS_EC_MOTION_SENSE_H
 #define __CROS_EC_MOTION_SENSE_H
 
+#include "chipset.h"
+#include "common.h"
+#include "ec_commands.h"
 #include "gpio.h"
 #include "math_util.h"
-#include "chipset.h"
-
-enum sensor_location_t {
-	LOCATION_BASE = 0,
-	LOCATION_LID  = 1,
-};
-
-enum sensor_type_t {
-	SENSOR_ACCELEROMETER = 0x1,
-	SENSOR_GYRO          = 0x2,
-};
-
-enum sensor_chip_t {
-	SENSOR_CHIP_KXCJ9 = 0,
-	SENSOR_CHIP_LSM6DS0 = 1,
-};
 
 enum sensor_state {
 	SENSOR_NOT_INITIALIZED = 0,
@@ -43,9 +30,9 @@ struct motion_sensor_t {
 	/* RO fields */
 	uint32_t active_mask;
 	char *name;
-	enum sensor_chip_t chip;
-	enum sensor_type_t type;
-	enum sensor_location_t location;
+	enum motionsensor_chip chip;
+	enum motionsensor_type type;
+	enum motionsensor_location location;
 	const struct accelgyro_drv *drv;
 	struct mutex *mutex;
 	void *drv_data;
@@ -63,6 +50,7 @@ struct motion_sensor_t {
 	/* state parameters */
 	enum sensor_state state;
 	enum chipset_state_mask active;
+	vector_3_t raw_xyz;
 	vector_3_t xyz;
 };
 
diff --git a/test/motion_lid.c b/test/motion_lid.c
index e8880e85a8..e0f1e68e7c 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -30,12 +30,11 @@ static int accel_init(const struct motion_sensor_t *s)
 	return EC_SUCCESS;
 }
 
-static int accel_read(const struct motion_sensor_t *s,
-	int *x_acc, int *y_acc, int *z_acc)
+static int accel_read(const struct motion_sensor_t *s, vector_3_t v)
 {
-	*x_acc = s->xyz[X];
-	*y_acc = s->xyz[Y];
-	*z_acc = s->xyz[Z];
+	v[X] = s->xyz[X];
+	v[Y] = s->xyz[Y];
+	v[Z] = s->xyz[Z];
 	return EC_SUCCESS;
 }
 
@@ -102,12 +101,12 @@ const matrix_3x3_t lid_standard_ref = {
 };
 
 struct motion_sensor_t motion_sensors[] = {
-	{SENSOR_ACTIVE_S0_S3_S5, "base", SENSOR_CHIP_LSM6DS0,
-		SENSOR_ACCELEROMETER, LOCATION_BASE,
+	{SENSOR_ACTIVE_S0_S3_S5, "base", MOTIONSENSE_CHIP_LSM6DS0,
+		MOTIONSENSE_TYPE_ACCEL, MOTIONSENSE_LOC_BASE,
 		&test_motion_sense, NULL, NULL,
 		0, &base_standard_ref, 119000, 2},
-	{SENSOR_ACTIVE_S0, "lid", SENSOR_CHIP_KXCJ9,
-		SENSOR_ACCELEROMETER, LOCATION_LID,
+	{SENSOR_ACTIVE_S0, "lid", MOTIONSENSE_CHIP_KXCJ9,
+		MOTIONSENSE_TYPE_ACCEL, MOTIONSENSE_LOC_LID,
 		&test_motion_sense, NULL, NULL,
 		0, &lid_standard_ref, 100000, 2},
 };
diff --git a/util/ectool.c b/util/ectool.c
index 08e50ab58a..0222720d00 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -2360,23 +2360,22 @@ static int cmd_lightbar(int argc, char **argv)
  * memory depending on the number of sensors.
  */
 #define ECTOOL_MAX_SENSOR 16
-#define MS_DATA_SIZE() { \
-		sizeof(((struct ec_params_motion_sense *)0)->data) \
+#define MS_DUMP_SIZE() { \
+		sizeof(((struct ec_params_motion_sense *)0)->dump) \
 		+ sizeof(((struct ec_params_motion_sense *)0)->cmd), \
-		sizeof(((struct ec_response_motion_sense *)0)->data) \
-		+ ECTOOL_MAX_SENSOR * sizeof(struct sensor_data) }
+		sizeof(((struct ec_response_motion_sense *)0)->dump) \
+		+ sizeof(struct ec_response_motion_sensor_data) * \
+		  ECTOOL_MAX_SENSOR}
 static const struct {
 	uint8_t insize;
 	uint8_t outsize;
 } ms_command_sizes[] = {
-	MS_SIZES(dump),
+	MS_DUMP_SIZE(),
 	MS_SIZES(info),
 	MS_SIZES(ec_rate),
 	MS_SIZES(sensor_odr),
 	MS_SIZES(sensor_range),
 	MS_SIZES(kb_wake_angle),
-	MS_SIZES(status),
-	MS_DATA_SIZE(),
 };
 BUILD_ASSERT(ARRAY_SIZE(ms_command_sizes) == MOTIONSENSE_NUM_CMDS);
 #undef MS_SIZES
@@ -2397,67 +2396,68 @@ static int ms_help(const char *cmd)
 
 static int cmd_motionsense(int argc, char **argv)
 {
-	int i, rv;
+	int i, rv, status_only = (argc == 2);
 	struct ec_params_motion_sense param;
-	struct ec_response_motion_sense resp;
+	uint8_t resp_buffer[ms_command_sizes[MOTIONSENSE_CMD_DUMP].outsize];
+	struct ec_response_motion_sense *resp =
+		(struct ec_response_motion_sense *)resp_buffer;
 	char *e;
+	/*
+	 * Warning: the following strings printed out are read in an
+	 * autotest. Do not change string without consulting autotest
+	 * for kernel_CrosECSysfsAccel.
+	 */
+	const char *motion_status_string[2][2] = {
+		{ "Motion sensing inactive", "0"},
+		{ "Motion sensing active", "1"},
+	};
 
 	/* No motionsense command has more than 5 args. */
 	if (argc > 5)
 		return ms_help(argv[0]);
 
-	if (argc == 1) {
-		/* No args, dump motion data. */
+	if ((argc == 1) ||
+	    (argc == 2 && !strcasecmp(argv[1], "active"))) {
 		param.cmd = MOTIONSENSE_CMD_DUMP;
-		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 0,
-				&param, ms_command_sizes[param.cmd].insize,
-				&resp, ms_command_sizes[param.cmd].outsize);
-
-		if (rv < 0)
-			return rv;
-
-		if (resp.dump.module_flags & MOTIONSENSE_MODULE_FLAG_ACTIVE)
-			printf("Motion sensing active\n");
-		else
-			printf("Motion sensing inactive\n");
-
-		for (i = 0; i < EC_MOTION_SENSOR_COUNT; i++) {
-			printf("Sensor %d: ", i);
-			if (resp.dump.sensor_flags[i] &
-					MOTIONSENSE_SENSOR_FLAG_PRESENT)
-				printf("%d\t%d\t%d\n", resp.dump.data[3*i],
-							resp.dump.data[3*i+1],
-							resp.dump.data[3*i+2]);
-			else
+		param.dump.max_sensor_count = ECTOOL_MAX_SENSOR;
+		rv = ec_command(
+			EC_CMD_MOTION_SENSE_CMD, 1,
+			&param, ms_command_sizes[param.cmd].insize,
+			resp, ms_command_sizes[param.cmd].outsize);
+		if (rv > 0) {
+			printf("%s\n", motion_status_string[
+					!!(resp->dump.module_flags &
+					   MOTIONSENSE_MODULE_FLAG_ACTIVE)][
+					status_only]);
+			if (status_only)
+				return 0;
+
+			if (resp->dump.sensor_count > ECTOOL_MAX_SENSOR) {
+				printf("Too many sensors to handle: %d",
+						resp->dump.sensor_count);
+				return -1;
+			}
+			for (i = 0; i < resp->dump.sensor_count; i++) {
 				/*
 				 * Warning: the following string printed out
 				 * is read by an autotest. Do not change string
 				 * without consulting autotest for
 				 * kernel_CrosECSysfsAccel.
 				 */
-				printf("None\n");
+				printf("Sensor %d: ", i);
+				if (resp->dump.sensor[i].flags &
+						MOTIONSENSE_SENSOR_FLAG_PRESENT)
+					printf("%d\t%d\t%d\n",
+						resp->dump.sensor[i].data[0],
+						resp->dump.sensor[i].data[1],
+						resp->dump.sensor[i].data[2]);
+				else
+					printf("None\n");
+			}
+			return 0;
+		} else {
+			return rv;
 		}
-
-		return 0;
-	}
-
-	if (argc == 2 && !strcasecmp(argv[1], "active")) {
-		param.cmd = MOTIONSENSE_CMD_DUMP;
-		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 0,
-				&param, ms_command_sizes[param.cmd].insize,
-				&resp, ms_command_sizes[param.cmd].outsize);
-
-		/*
-		 * Warning: the following strings printed out are read in an
-		 * autotest. Do not change string without consulting autotest
-		 * for kernel_CrosECSysfsAccel.
-		 */
-		if (resp.dump.module_flags & MOTIONSENSE_MODULE_FLAG_ACTIVE)
-			printf("1\n");
-		else
-			printf("0\n");
-
-		return 0;
 	}
 
 	if (argc == 3 && !strcasecmp(argv[1], "info")) {
@@ -2469,15 +2469,15 @@ static int cmd_motionsense(int argc, char **argv)
 			return -1;
 		}
 
-		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 0,
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 1,
 				&param, ms_command_sizes[param.cmd].insize,
-				&resp, ms_command_sizes[param.cmd].outsize);
+				resp, ms_command_sizes[param.cmd].outsize);
 
 		if (rv < 0)
 			return rv;
 
 		printf("Type:     ");
-		switch (resp.info.type) {
+		switch (resp->info.type) {
 		case MOTIONSENSE_TYPE_ACCEL:
 			printf("accel\n");
 			break;
@@ -2489,7 +2489,7 @@ static int cmd_motionsense(int argc, char **argv)
 		}
 
 		printf("Location: ");
-		switch (resp.info.location) {
+		switch (resp->info.location) {
 		case MOTIONSENSE_LOC_BASE:
 			printf("base\n");
 			break;
@@ -2501,10 +2501,13 @@ static int cmd_motionsense(int argc, char **argv)
 		}
 
 		printf("Chip:     ");
-		switch (resp.info.chip) {
+		switch (resp->info.chip) {
 		case MOTIONSENSE_CHIP_KXCJ9:
 			printf("kxcj9\n");
 			break;
+		case MOTIONSENSE_CHIP_LSM6DS0:
+			printf("lsm6ds0\n");
+			break;
 		default:
 			printf("unknown\n");
 		}
@@ -2524,14 +2527,14 @@ static int cmd_motionsense(int argc, char **argv)
 			}
 		}
 
-		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 0,
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 1,
 				&param, ms_command_sizes[param.cmd].insize,
-				&resp, ms_command_sizes[param.cmd].outsize);
+				resp, ms_command_sizes[param.cmd].outsize);
 
 		if (rv < 0)
 			return rv;
 
-		printf("%d\n", resp.ec_rate.ret);
+		printf("%d\n", resp->ec_rate.ret);
 		return 0;
 	}
 
@@ -2562,14 +2565,14 @@ static int cmd_motionsense(int argc, char **argv)
 			}
 		}
 
-		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 0,
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 1,
 				&param, ms_command_sizes[param.cmd].insize,
-				&resp, ms_command_sizes[param.cmd].outsize);
+				resp, ms_command_sizes[param.cmd].outsize);
 
 		if (rv < 0)
 			return rv;
 
-		printf("%d\n", resp.sensor_odr.ret);
+		printf("%d\n", resp->sensor_odr.ret);
 		return 0;
 	}
 
@@ -2600,14 +2603,14 @@ static int cmd_motionsense(int argc, char **argv)
 			}
 		}
 
-		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 0,
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 1,
 				&param, ms_command_sizes[param.cmd].insize,
-				&resp, ms_command_sizes[param.cmd].outsize);
+				resp, ms_command_sizes[param.cmd].outsize);
 
 		if (rv < 0)
 			return rv;
 
-		printf("%d\n", resp.sensor_range.ret);
+		printf("%d\n", resp->sensor_range.ret);
 		return 0;
 	}
 
@@ -2623,14 +2626,14 @@ static int cmd_motionsense(int argc, char **argv)
 			}
 		}
 
-		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 0,
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 1,
 				&param, ms_command_sizes[param.cmd].insize,
-				&resp, ms_command_sizes[param.cmd].outsize);
+				resp, ms_command_sizes[param.cmd].outsize);
 
 		if (rv < 0)
 			return rv;
 
-		printf("%d\n", resp.kb_wake_angle.ret);
+		printf("%d\n", resp->kb_wake_angle.ret);
 		return 0;
 	}