motion: Add commands for calibration

Move calibration in the EC to returns raw value with offset already
applied, as recommended by iio interface.

BRANCH=smaug
TEST=On Samus, with kernel change CL:283103
Check raw value are returned properly.
BUG=chromium:506101

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

4 files changed, 133 insertions, 4 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index a4efbaa8d6..8a34d1a613 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -466,7 +466,7 @@ 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_range.sensor_num);
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;
 
@@ -490,6 +490,28 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 		out->sensor_range.ret = data;
 		args->response_size = sizeof(out->sensor_range);
 		break;
+	case MOTIONSENSE_CMD_SENSOR_OFFSET:
+		/* Verify sensor number is valid. */
+		sensor = host_sensor_id_to_motion_sensor(
+				in->sensor_offset.sensor_num);
+		if (sensor == NULL)
+			return EC_RES_INVALID_PARAM;
+
+		/* Set new range if the data arg has a value. */
+		if (in->sensor_offset.flags & MOTION_SENSE_SET_OFFSET) {
+			if (sensor->drv->set_offset(sensor,
+						in->sensor_offset.offset,
+						in->sensor_offset.temp)
+					!= EC_SUCCESS) {
+				CPRINTS("MS bad sensor offsets");
+				return EC_RES_INVALID_PARAM;
+			}
+		}
+
+		sensor->drv->get_offset(sensor, out->sensor_offset.offset,
+				&out->sensor_offset.temp);
+		args->response_size = sizeof(out->sensor_offset);
+		break;
 	default:
 		/* Call other users of the motion task */
 #ifdef CONFIG_LID_ANGLE
diff --git a/include/accelgyro.h b/include/accelgyro.h
index 5ac486c660..723efa569c 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -77,6 +77,20 @@ struct accelgyro_drv {
 	int (*get_data_rate)(const struct motion_sensor_t *s,
 				int *rate);
 
+
+	/**
+	 * Setter and getter methods for the sensor offset.
+	 * @s Pointer to sensor data.
+	 * @offset: offset to apply to raw data.
+	 * @temp: temperature when calibration was done.
+	 * @return EC_SUCCESS if successful, non-zero if error.
+	 */
+	int (*set_offset)(const struct motion_sensor_t *s,
+				const int16_t    *offset,
+				int16_t    temp);
+	int (*get_offset)(const struct motion_sensor_t *s,
+				int16_t    *offset,
+				int16_t    *temp);
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	/**
 	 * Setup a one-time accel interrupt. If the threshold is low enough, the
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 6ef13c408b..c0b1c3cecd 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -1675,6 +1675,20 @@ enum motionsense_command {
 	 */
 	MOTIONSENSE_CMD_FIFO_READ = 9,
 
+	/*
+	 * Perform low level calibration.
+	 * On sensors that support it, ask to do offset calibration.
+	 */
+	MOTIONSENSE_CMD_PERFORM_CALIB = 10,
+
+	/*
+	 * Sensor Offset command is a setter/getter command for the offset
+	 * used for calibration.
+	 * The offsets can be calculated by the host, or via
+	 * PERFORM_CALIB command.
+	 */
+	MOTIONSENSE_CMD_SENSOR_OFFSET = 11,
+
 	/* Number of motionsense sub-commands. */
 	MOTIONSENSE_NUM_CMDS
 };
@@ -1745,6 +1759,12 @@ struct ec_response_motion_sense_fifo_data {
  */
 #define EC_MOTION_SENSE_NO_VALUE -1
 
+#define EC_MOTION_SENSE_INVALID_CALIB_TEMP 0x8000
+
+/* MOTIONSENSE_CMD_SENSOR_OFFSET subcommand flag */
+/* Set Calibration information */
+#define MOTION_SENSE_SET_OFFSET 1
+
 struct ec_params_motion_sense {
 	uint8_t cmd;
 	union {
@@ -1770,10 +1790,11 @@ struct ec_params_motion_sense {
 			int16_t data;
 		} ec_rate, kb_wake_angle;
 
-		/* Used for MOTIONSENSE_CMD_INFO, MOTIONSENSE_CMD_DATA. */
+		/* Used for MOTIONSENSE_CMD_INFO, MOTIONSENSE_CMD_DATA
+		 * and MOTIONSENSE_CMD_PERFORM_CALIB. */
 		struct {
 			uint8_t sensor_num;
-		} info, data, fifo_flush;
+		} info, data, fifo_flush, perform_calib;
 
 		/*
 		 * Used for MOTIONSENSE_CMD_SENSOR_ODR and
@@ -1790,8 +1811,40 @@ struct ec_params_motion_sense {
 			/* Data to set or EC_MOTION_SENSE_NO_VALUE to read. */
 			int32_t data;
 		} sensor_odr, sensor_range;
+
+		/* Used for MOTIONSENSE_CMD_SENSOR_OFFSET */
+		struct __attribute__((__packed__)) {
+			uint8_t sensor_num;
+
+			/*
+			 * bit 0: If set (MOTION_SENSE_SET_OFFSET), set
+			 * the calibration information in the EC.
+			 * If unset, just retrieve calibration information.
+			 */
+			uint16_t flags;
+
+			/*
+			 * Temperature at calibration, in units of 0.01 C
+			 * 0x8000: invalid / unknown.
+			 * 0x0: 0C
+			 * 0x7fff: +327.67C
+			 */
+			int16_t temp;
+
+			/*
+			 * Offset for calibration.
+			 * Unit:
+			 * Accelerometer: 1/1024 g
+			 * Gyro:          1/1024 deg/s
+			 */
+			int16_t offset[3];
+		} sensor_offset;
+
+		/* Used for MOTIONSENSE_CMD_FIFO_INFO */
 		struct {
 		} fifo_info;
+
+		/* Used for MOTIONSENSE_CMD_FIFO_READ */
 		struct {
 			/*
 			 * Number of expected vector to return.
@@ -1842,7 +1895,14 @@ struct ec_response_motion_sense {
 		struct {
 			/* Current value of the parameter queried. */
 			int32_t ret;
-		} ec_rate, sensor_odr, sensor_range, kb_wake_angle;
+		} ec_rate, sensor_odr, sensor_range, kb_wake_angle,
+		  perform_calib;
+
+		/* Used for MOTIONSENSE_CMD_SENSOR_OFFSET */
+		struct {
+			int16_t temp;
+			int16_t offset[3];
+		} sensor_offset;
 
 		struct ec_response_motion_sense_fifo_info fifo_info, fifo_flush;
 
diff --git a/util/ectool.c b/util/ectool.c
index 3faea0ffbe..d14caca71e 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -3224,6 +3224,8 @@ static const struct {
 	MS_SIZES(fifo_flush),
 	MS_SIZES(fifo_info),
 	MS_SIZES(fifo_read),
+	MS_SIZES(perform_calib),
+	MS_SIZES(sensor_offset),
 };
 BUILD_ASSERT(ARRAY_SIZE(ms_command_sizes) == MOTIONSENSE_NUM_CMDS);
 #undef MS_SIZES
@@ -3237,6 +3239,7 @@ static int ms_help(const char *cmd)
 	printf("  %s ec_rate [RATE_MS]          - set/get sample rate\n", cmd);
 	printf("  %s odr NUM [ODR [ROUNDUP]]    - set/get sensor ODR\n", cmd);
 	printf("  %s range NUM [RANGE [ROUNDUP]]- set/get sensor range\n", cmd);
+	printf("  %s offset NUM                 - get sensor offset\n", cmd);
 	printf("  %s kb_wake NUM                - set/get KB wake ang\n", cmd);
 	printf("  %s data NUM                   - read sensor latest data\n",
 			cmd);
@@ -3578,6 +3581,36 @@ static int cmd_motionsense(int argc, char **argv)
 		return rv < 0 ? rv : 0;
 	}
 
+	if (argc == 3 && !strcasecmp(argv[1], "offset")) {
+		param.cmd = MOTIONSENSE_CMD_SENSOR_OFFSET;
+		param.sensor_offset.flags = 0;
+
+		param.sensor_offset.sensor_num = strtol(argv[2], &e, 0);
+		if (e && *e) {
+			fprintf(stderr, "Bad %s arg.\n", argv[2]);
+			return -1;
+		}
+
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 1,
+				&param, ms_command_sizes[param.cmd].outsize,
+				resp, ms_command_sizes[param.cmd].insize);
+
+		if (rv < 0)
+			return rv;
+
+		printf("Offset vector: X:%d, Y:%d, Z:%d\n",
+			resp->sensor_offset.offset[0],
+			resp->sensor_offset.offset[1],
+			resp->sensor_offset.offset[2]);
+		if (resp->sensor_offset.temp ==
+		    EC_MOTION_SENSE_INVALID_CALIB_TEMP)
+			printf("temperature at calibration unknown\n");
+		else
+			printf("temperature at calibration: %d.%02d C\n",
+			       resp->sensor_offset.temp / 100,
+			       resp->sensor_offset.temp % 100);
+		return 0;
+	}
 	return ms_help(argv[0]);
 }