motion_sense: Add "spoof" mode

This commit adds a "spoof" mode feature to the motionsense stack.  It
allows the user to arbitrarily set the outputs of the sensor in order to
"spoof" the readings of the sensor.  This can be useful in emulating
tablet mode or device rotations.  A command is available from the EC
console named `accelspoof` and there is a corresponding motionsense
command in ectool called `spoof`.

The usage is as follows:

- EC console
 > accelspoof [id] [on/off] [X Y Z]

- ectool
 # ectool motionsense spoof -- [id] [0/1] [X Y Z]

If on or off(or 0/1) is not specified, the current spoof mode status of
the sensor is returned.  If on is specified, but no components are
provided, the sensor will lock the current values and provide those as
the spoofed values.  If the components are provided, those will be used
as the spoofed values.

BUG=chromium:675263
BRANCH=cyan,glados,gru,oak
TEST=Flash a DUT with accels.  From AP console, run `ectool motionsense
lid_angle` in a loop, use 'accelspoof' EC console command to set spoofed
values.  Verify that the angle is fixed regardless of the actual angle
of the DUT.
TEST=Flash a DUT with accels.  From AP console, use `ectool motionsense
spoof` to spoof values and verify that `ectool motionsense` reflects the
spoofed values.  Test with both provided component values and no
component values.

Change-Id: Ie30688d22f38054e7243b1af493a3092b2cdfb72
Signed-off-by: Aseda Aboagye <aaboagye@google.com>
Reviewed-on: https://chromium-review.googlesource.com/421280
Commit-Ready: Aseda Aboagye <aaboagye@chromium.org>
Tested-by: Aseda Aboagye <aaboagye@chromium.org>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

7 files changed, 291 insertions, 16 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 6715947ffb..68ad38a40c 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -72,6 +72,10 @@ static struct mutex g_sensor_mutex;
  */
 test_export_static enum chipset_state_mask sensor_active;
 
+#ifdef CONFIG_ACCEL_SPOOF_MODE
+static void print_spoof_mode_status(int id);
+#endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */
+
 #ifdef CONFIG_ACCEL_FIFO
 /* Need to wake up the AP */
 static int wake_up_needed;
@@ -400,6 +404,9 @@ static inline int motion_sense_init(struct motion_sensor_t *sensor)
 {
 	int ret, cnt = 3;
 
+	/* By default, report the actual sensor values. */
+	sensor->in_spoof_mode = 0;
+
 	/* Initialize accelerometers. */
 	do {
 		ret = sensor->drv->init(sensor);
@@ -600,7 +607,16 @@ static int motion_sense_read(struct motion_sensor_t *sensor)
 	if (sensor->drv->get_data_rate(sensor) == 0)
 		return EC_ERROR_NOT_POWERED;
 
-	/* Read all raw X,Y,Z accelerations. */
+#ifdef CONFIG_ACCEL_SPOOF_MODE
+	/*
+	 * If the sensor is in spoof mode, the readings are already present in
+	 * spoof_xyz.
+	 */
+	if (sensor->in_spoof_mode)
+		return EC_SUCCESS;
+#endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */
+
+	/* Otherwise, read all raw X,Y,Z accelerations. */
 	return sensor->drv->read(sensor, sensor->raw_xyz);
 }
 
@@ -622,13 +638,18 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		sensor->drv->load_fifo(sensor);
 	} else if (motion_sensor_time_to_read(ts, sensor)) {
 		struct ec_response_motion_sensor_data vector;
+		int *v = sensor->raw_xyz;
 		ret = motion_sense_read(sensor);
 		if (ret == EC_SUCCESS) {
 			vector.flags = 0;
 			vector.sensor_num = sensor - motion_sensors;
-			vector.data[X] = sensor->raw_xyz[X];
-			vector.data[Y] = sensor->raw_xyz[Y];
-			vector.data[Z] = sensor->raw_xyz[Z];
+#ifdef CONFIG_ACCEL_SPOOF_MODE
+			if (sensor->in_spoof_mode)
+				v = sensor->spoof_xyz;
+#endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */
+			vector.data[X] = v[X];
+			vector.data[Y] = v[Y];
+			vector.data[Z] = v[Z];
 			motion_sense_fifo_add_unit(&vector, sensor, 3);
 		}
 		sensor->last_collection = ts->le.lo;
@@ -1212,7 +1233,61 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 		args->response_size = sizeof(out->set_activity);
 		break;
 	}
-#endif
+#endif /* defined(CONFIG_GESTURE_HOST_DETECTION) */
+
+#ifdef CONFIG_ACCEL_SPOOF_MODE
+	case MOTIONSENSE_CMD_SPOOF: {
+		sensor = host_sensor_id_to_real_sensor(in->spoof.sensor_id);
+		if (sensor == NULL)
+			return EC_RES_INVALID_PARAM;
+
+		switch (in->spoof.spoof_enable) {
+		case MOTIONSENSE_SPOOF_MODE_DISABLE:
+			/* Disable spoof mode. */
+			sensor->in_spoof_mode = 0;
+			break;
+
+		case MOTIONSENSE_SPOOF_MODE_CUSTOM:
+			/*
+			 * Enable spoofing, but use provided component values.
+			 */
+			sensor->spoof_xyz[X] = (int)in->spoof.components[X];
+			sensor->spoof_xyz[Y] = (int)in->spoof.components[Y];
+			sensor->spoof_xyz[Z] = (int)in->spoof.components[Z];
+			sensor->in_spoof_mode = 1;
+			break;
+
+		case MOTIONSENSE_SPOOF_MODE_LOCK_CURRENT:
+			/*
+			 * Enable spoofing, but lock to current sensor
+			 * values.  raw_xyz already has the values we want.
+			 */
+			sensor->spoof_xyz[X] = sensor->raw_xyz[X];
+			sensor->spoof_xyz[Y] = sensor->raw_xyz[Y];
+			sensor->spoof_xyz[Z] = sensor->raw_xyz[Z];
+			sensor->in_spoof_mode = 1;
+			break;
+
+		case MOTIONSENSE_SPOOF_MODE_QUERY:
+			/* Querying the spoof status of the sensor. */
+			out->spoof.ret = sensor->in_spoof_mode;
+			args->response_size = sizeof(out->spoof);
+			break;
+
+		default:
+			return EC_RES_INVALID_PARAM;
+		}
+
+		/*
+		 * Only print the status when spoofing is enabled or disabled.
+		 */
+		if (in->spoof.spoof_enable != MOTIONSENSE_SPOOF_MODE_QUERY)
+			print_spoof_mode_status((int)(sensor - motion_sensors));
+
+		break;
+	}
+#endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */
+
 	default:
 		/* Call other users of the motion task */
 #ifdef CONFIG_LID_ANGLE
@@ -1521,6 +1596,73 @@ static int motion_sense_read_fifo(int argc, char **argv)
 DECLARE_CONSOLE_COMMAND(fiforead, motion_sense_read_fifo,
 	"id",
 	"Read Fifo sensor");
-#endif
-
+#endif /* defined(CONFIG_CMD_ACCEL_FIFO) */
 #endif /* CONFIG_CMD_ACCELS */
+
+#ifdef CONFIG_ACCEL_SPOOF_MODE
+static void print_spoof_mode_status(int id)
+{
+	CPRINTS("Sensor %d spoof mode is %s. <%d, %d, %d>", id,
+		motion_sensors[id].in_spoof_mode ? "enabled" : "disabled",
+		motion_sensors[id].spoof_xyz[X],
+		motion_sensors[id].spoof_xyz[Y],
+		motion_sensors[id].spoof_xyz[Z]);
+}
+
+#ifdef CONFIG_CMD_ACCELSPOOF
+static int command_accelspoof(int argc, char **argv)
+{
+	char *e;
+	int id, enable, i;
+	struct motion_sensor_t *s;
+
+	/* There must be at least 1 parameter, the sensor id. */
+	if (argc < 2)
+		return EC_ERROR_PARAM_COUNT;
+
+	/* First argument is sensor id. */
+	id = strtoi(argv[1], &e, 0);
+	if (id >= motion_sensor_count || id < 0)
+		return EC_ERROR_PARAM1;
+
+	s = &motion_sensors[id];
+
+	/* Print the sensor's current spoof status. */
+	if (argc == 2)
+		print_spoof_mode_status(id);
+
+	/* Enable/Disable spoof mode. */
+	if (argc >= 3) {
+		if (!parse_bool(argv[2], &enable))
+			return EC_ERROR_PARAM2;
+
+		if (enable) {
+			/*
+			 * If no components are provided, we'll just use the
+			 * current values as the spoofed values.  But if the
+			 * components are provided, use the provided ones as the
+			 * spoofed ones.
+			 */
+			if (argc == 6) {
+				for (i = 0; i < 3; i++)
+					s->spoof_xyz[i] = strtoi(argv[3 + i],
+								 &e, 0);
+			} else if (argc == 3) {
+				for (i = X; i <= Z; i++)
+					s->spoof_xyz[i] = s->raw_xyz[i];
+			} else {
+				/* It's either all or nothing. */
+				return EC_ERROR_PARAM_COUNT;
+			}
+		}
+		s->in_spoof_mode = enable;
+		print_spoof_mode_status(id);
+	}
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(accelspoof, command_accelspoof,
+			"id [on/off] [X] [Y] [Z]",
+			"Enable/Disable spoofing of sensor readings.");
+#endif /* defined(CONIFG_CMD_ACCELSPOOF) */
+#endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 0348e51bd9..a92cc1f54c 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -927,6 +927,10 @@ static int bmi160_decode_header(struct motion_sensor_t *s,
 				int *v = (s + i)->raw_xyz;
 				vector.flags = 0;
 				normalize(s + i, v, *bp);
+#ifdef CONFIG_ACCEL_SPOOF_MODE
+				if ((s+i)->in_spoof_mode)
+					v = (s+i)->spoof_xyz;
+#endif  /* defined(CONFIG_ACCEL_SPOOF_MODE) */
 				vector.data[X] = v[X];
 				vector.data[Y] = v[Y];
 				vector.data[Z] = v[Z];
diff --git a/include/accelgyro.h b/include/accelgyro.h
index 883516bb3e..f78af2bb90 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -108,6 +108,10 @@ struct accelgyro_drv {
 	 * - update sensor raw_xyz vector with the last information.
 	 * We put raw data in hub fifo and process data from there.
 	 * @s Pointer to sensor data.
+	 *
+	 * NOTE: If a new driver supports this function, be sure to add a check
+	 * for spoof_mode in order to load the sensor stack with the spoofed
+	 * data.  See accelgyro_bmi160.c::load_fifo for an example.
 	 */
 	int (*load_fifo)(struct motion_sensor_t *s);
 #endif
diff --git a/include/config.h b/include/config.h
index 0e926d6977..6241a3a5aa 100644
--- a/include/config.h
+++ b/include/config.h
@@ -36,11 +36,12 @@
  * BOARD_*, CHIP_*, and CHIP_FAMILY_*.
  */
 
-/* Enable accelerometer interrupts. */
-#undef CONFIG_ACCEL_INTERRUPTS
-/* Add support for sensor FIFO:
- * define the size of the global fifo, must be a power of 2. */
+/*
+ * Add support for sensor FIFO:
+ * define the size of the global fifo, must be a power of 2.
+ */
 #undef CONFIG_ACCEL_FIFO
+
 /* The amount of free entries that trigger an interrupt to the AP. */
 #undef CONFIG_ACCEL_FIFO_THRES
 
@@ -50,6 +51,15 @@
  */
 #undef CONFIG_ACCEL_FORCE_MODE_MASK
 
+/* Enable accelerometer interrupts. */
+#undef CONFIG_ACCEL_INTERRUPTS
+
+/*
+ * Support "spoof" mode for sensors.  This allows sensors to have their values
+ * spoofed to any arbitrary value.  This is useful for testing.
+ */
+#define CONFIG_ACCEL_SPOOF_MODE
+
 /* Specify type of accelerometers attached. */
 #undef CONFIG_ACCEL_BMA255
 #undef CONFIG_ACCEL_KXCJ9
@@ -606,6 +616,7 @@
 #undef  CONFIG_CMD_ACCELS
 #undef  CONFIG_CMD_ACCEL_FIFO
 #undef  CONFIG_CMD_ACCEL_INFO
+#define CONFIG_CMD_ACCELSPOOF
 #undef  CONFIG_CMD_ALS
 #define CONFIG_CMD_APTHROTTLE
 #undef  CONFIG_CMD_BATDEBUG
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 4f89475e26..6ea1a74c15 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -1909,6 +1909,12 @@ enum motionsense_command {
 	 */
 	MOTIONSENSE_CMD_FIFO_INT_ENABLE = 15,
 
+	/*
+	 * Spoof the readings of the sensors.  The spoofed readings can be set
+	 * to arbitrary values, or will lock to the last read actual values.
+	 */
+	MOTIONSENSE_CMD_SPOOF = 16,
+
 	/* Number of motionsense sub-commands. */
 	MOTIONSENSE_NUM_CMDS
 };
@@ -2030,6 +2036,20 @@ struct __ec_todo_unpacked ec_motion_sense_activity {
 
 #define LID_ANGLE_UNRELIABLE 500
 
+enum motionsense_spoof_mode {
+	/* Disable spoof mode. */
+	MOTIONSENSE_SPOOF_MODE_DISABLE = 0,
+
+	/* Enable spoof mode, but use provided component values. */
+	MOTIONSENSE_SPOOF_MODE_CUSTOM,
+
+	/* Enable spoof mode, but use the current sensor values. */
+	MOTIONSENSE_SPOOF_MODE_LOCK_CURRENT,
+
+	/* Query the current spoof mode status for the sensor. */
+	MOTIONSENSE_SPOOF_MODE_QUERY,
+};
+
 struct __ec_todo_packed ec_params_motion_sense {
 	uint8_t cmd;
 	union {
@@ -2131,6 +2151,20 @@ struct __ec_todo_packed ec_params_motion_sense {
 			 */
 			int8_t enable;
 		} fifo_int_enable;
+
+		/* Used for MOTIONSENSE_CMD_SPOOF */
+		struct __ec_todo_packed {
+			uint8_t sensor_id;
+
+			/* See enum motionsense_spoof_mode. */
+			uint8_t spoof_enable;
+
+			/* Ignored, used for alignment. */
+			uint8_t rsvd;
+
+			/* Individual component values to spoof. */
+			int16_t components[3];
+		} spoof;
 	};
 };
 
@@ -2169,14 +2203,15 @@ struct __ec_todo_packed ec_response_motion_sense {
 		/*
 		 * Used for MOTIONSENSE_CMD_EC_RATE, MOTIONSENSE_CMD_SENSOR_ODR,
 		 * MOTIONSENSE_CMD_SENSOR_RANGE,
-		 * MOTIONSENSE_CMD_KB_WAKE_ANGLE and
-		 * MOTIONSENSE_CMD_FIFO_INT_ENABLE.
+		 * MOTIONSENSE_CMD_KB_WAKE_ANGLE,
+		 * MOTIONSENSE_CMD_FIFO_INT_ENABLE and
+		 * MOTIONSENSE_CMD_SPOOF.
 		 */
 		struct __ec_todo_unpacked {
 			/* Current value of the parameter queried. */
 			int32_t ret;
 		} ec_rate, sensor_odr, sensor_range, kb_wake_angle,
-		  fifo_int_enable;
+		  fifo_int_enable, spoof;
 
 		/* Used for MOTIONSENSE_CMD_SENSOR_OFFSET */
 		struct __ec_todo_unpacked  {
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 98c27ae04d..30384374bd 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -78,6 +78,12 @@ struct motion_sensor_t {
 	/* i2c address or SPI slave logic GPIO. */
 	uint8_t addr;
 
+	/*
+	 * When non-zero, spoof mode will allow the EC to report arbitrary
+	 * values for any of the components.
+	 */
+	uint8_t in_spoof_mode;
+
 	const matrix_3x3_t *rot_standard_ref;
 
 	/*
@@ -108,6 +114,7 @@ struct motion_sensor_t {
 	enum sensor_state state;
 	vector_3_t raw_xyz;
 	vector_3_t xyz;
+	vector_3_t spoof_xyz;
 
 	/* How many flush events are pending */
 	uint32_t flush_pending;
diff --git a/util/ectool.c b/util/ectool.c
index 797670013e..08f0e77ea2 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -3435,6 +3435,7 @@ static const struct {
 	MS_SIZES(set_activity),
 	MS_SIZES(lid_angle),
 	MS_SIZES(fifo_int_enable),
+	MS_SIZES(spoof),
 };
 BUILD_ASSERT(ARRAY_SIZE(ms_command_sizes) == MOTIONSENSE_NUM_CMDS);
 #undef MS_SIZES
@@ -3463,6 +3464,8 @@ static int ms_help(const char *cmd)
 	printf("  %s set_activity NUM ACT EN    - enable/disable activity\n",
 			cmd);
 	printf("  %s lid_angle                  - print lid angle\n", cmd);
+	printf("  %s spoof -- NUM [0/1] [X Y Z] - enable/disable spoofing\n",
+	       cmd);
 
 	return 0;
 }
@@ -3496,8 +3499,8 @@ static int cmd_motionsense(int argc, char **argv)
 		{ "Motion sensing active", "1"},
 	};
 
-	/* No motionsense command has more than 5 args. */
-	if (argc > 5)
+	/* No motionsense command has more than 7 args. */
+	if (argc > 7)
 		return ms_help(argv[0]);
 
 	if ((argc == 1) ||
@@ -3949,6 +3952,75 @@ static int cmd_motionsense(int argc, char **argv)
 		return 0;
 	}
 
+	if (argc >= 3 && !strcasecmp(argv[1], "spoof")) {
+		param.cmd = MOTIONSENSE_CMD_SPOOF;
+		/* By default, just query the current spoof status. */
+		param.spoof.spoof_enable = MOTIONSENSE_SPOOF_MODE_QUERY;
+		param.spoof.sensor_id = strtol(argv[2], &e, 0);
+		if (e && *e) {
+			fprintf(stderr, "Bad %s arg.\n", argv[2]);
+			return -1;
+		}
+
+		if (argc >= 4) {
+			int enable, i;
+			int16_t val;
+
+			enable = strtol(argv[3], &e, 0);
+			if ((e && *e) || (enable != 0 && enable != 1)) {
+				fprintf(stderr, "Bad %s arg.\n", argv[3]);
+				return -1;
+			}
+
+			if ((enable == 1) && (argc == 4)) {
+				/*
+				 * Enable spoofing, but lock to current sensor
+				 * values.
+				 */
+				param.spoof.spoof_enable =
+					MOTIONSENSE_SPOOF_MODE_LOCK_CURRENT;
+			} else if ((enable == 1) && (argc == 7)) {
+				/*
+				 * Enable spoofing, but use provided component
+				 * values.
+				 */
+				param.spoof.spoof_enable =
+					MOTIONSENSE_SPOOF_MODE_CUSTOM;
+				for (i = 0; i < 3; i++) {
+					val = strtol(argv[4+i], &e, 0);
+					if (e && *e) {
+						fprintf(stderr, "Bad %s arg.\n",
+							argv[4+i]);
+						return -1;
+					}
+					param.spoof.components[i] = val;
+				}
+			} else if (enable == 0) {
+				param.spoof.spoof_enable =
+					MOTIONSENSE_SPOOF_MODE_DISABLE;
+			} else {
+				return ms_help(argv[0]);
+			}
+		}
+
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 2,
+				&param, ms_command_sizes[param.cmd].outsize,
+				resp, ms_command_sizes[param.cmd].insize);
+		if (rv < 0)
+			return rv;
+
+		if (param.spoof.spoof_enable == MOTIONSENSE_SPOOF_MODE_QUERY)
+			/*
+			 * Response is the current spoof status of the
+			 * sensor.
+			 */
+			printf("Sensor %d spoof mode is %s.\n",
+			       param.spoof.sensor_id,
+			       resp->spoof.ret ? "enabled" : "disabled");
+
+		return 0;
+	}
+
 	return ms_help(argv[0]);
 }