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/426373
Reviewed-by: Aseda Aboagye <aaboagye@chromium.org>
Commit-Queue: Aseda Aboagye <aaboagye@chromium.org>
Tested-by: Aseda Aboagye <aaboagye@chromium.org>

6 files changed, 289 insertions, 17 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 25ebf77847..cadaa3a525 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -71,6 +71,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;
@@ -387,6 +391,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);
@@ -577,7 +584,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);
 }
 
@@ -601,13 +617,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;
 		}
@@ -1191,7 +1212,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
@@ -1550,6 +1625,74 @@ static int motion_sense_read_fifo(int argc, char **argv)
 DECLARE_CONSOLE_COMMAND(fiforead, motion_sense_read_fifo,
 	"id",
 	"Read Fifo sensor", NULL);
-#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.",
+			NULL);
+#endif /* defined(CONIFG_CMD_ACCELSPOOF) */
+#endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */
diff --git a/include/accelgyro.h b/include/accelgyro.h
index dd7d50a6d9..9d4b4c0eda 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -118,6 +118,10 @@ struct accelgyro_drv {
 	 * - update sensor raw_xyz vector with the last information.
 	 * We put raw data in hub fifo and process data from theres.
 	 * @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 fa845b1dc2..1b7fcaabb2 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_KXCJ9
 #undef CONFIG_ACCEL_KX022
@@ -446,6 +456,7 @@
 
 #undef CONFIG_CMD_ACCELS
 #undef CONFIG_CMD_ACCEL_INFO
+#define CONFIG_CMD_ACCELSPOOF
 #undef CONFIG_CMD_BATDEBUG
 #undef CONFIG_CMD_CHGRAMP
 #undef CONFIG_CMD_CLOCKGATES
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 27d485416a..f290dbea9e 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -1730,6 +1730,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
 };
@@ -1845,7 +1851,21 @@ struct ec_motion_sense_activity {
 
 #define LID_ANGLE_UNRELIABLE 500
 
-struct ec_params_motion_sense {
+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 __packed ec_params_motion_sense {
 	uint8_t cmd;
 	union {
 		/* Used for MOTIONSENSE_CMD_DUMP */
@@ -1945,6 +1965,20 @@ struct ec_params_motion_sense {
 			 */
 			int8_t enable;
 		} fifo_int_enable;
+
+		/* Used for MOTIONSENSE_CMD_SPOOF */
+		struct __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;
 	};
 } __packed;
 
@@ -1983,14 +2017,15 @@ struct 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 {
 			/* 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 {
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 2aea41a698..910b95dc6e 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 bf8b04e063..03c7ee8573 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -3237,6 +3237,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
@@ -3265,6 +3266,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;
 }
@@ -3298,8 +3301,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) ||
@@ -3723,6 +3726,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]);
 }