fpsensor: add enrollment and matching interface

Add the state machine and the interfacing to the enrollment and matching
algorithm providing by the private driver part.

Implement the host commands interface for it (based on MKBP event) along
with the console debug commands to exercise it.

Signed-off-by: Vincent Palatin <vpalatin@chromium.org>

BRANCH=none
BUG=b:72360575
TEST=On ZerbleBarn console, use 'fpenroll' then 'fpmatch' for several
fingers.
TEST=With ZerbleBarn and a servo-v2, retrieve and upload templates with
'ectool_servo fptemplate'.
TEST=On Meowth, exercise with the prototype of biod
CrosFpBiometricsManager.
CQ-DEPEND=CL:*555078

Change-Id: I10b0d76d3faa898a682cf9a2eb7fc7e212b0c20c
Reviewed-on: https://chromium-review.googlesource.com/886401
Commit-Ready: Vincent Palatin <vpalatin@chromium.org>
Tested-by: Vincent Palatin <vpalatin@chromium.org>
Reviewed-by: Nicolas Boichat <drinkcat@chromium.org>

4 files changed, 553 insertions, 59 deletions

diff --git a/common/fpsensor.c b/common/fpsensor.c
index c465dd6963..7d1993a61f 100644
--- a/common/fpsensor.c
+++ b/common/fpsensor.c
@@ -4,6 +4,7 @@
  */
 
 #include "atomic.h"
+#include "clock.h"
 #include "common.h"
 #include "console.h"
 #include "ec_commands.h"
@@ -26,15 +27,29 @@
 #define FP_SENSOR_IMAGE_SIZE 0
 #define FP_SENSOR_RES_X 0
 #define FP_SENSOR_RES_Y 0
+#define FP_ALGORITHM_TEMPLATE_SIZE 0
+#define FP_MAX_FINGER_COUNT 0
 #endif
 
 /* if no special memory regions are defined, fallback on regular SRAM */
 #ifndef FP_FRAME_SECTION
 #define FP_FRAME_SECTION
 #endif
+#ifndef FP_TEMPLATE_SECTION
+#define FP_TEMPLATE_SECTION
+#endif
 
 /* Last acquired frame */
 static uint8_t fp_buffer[FP_SENSOR_IMAGE_SIZE] FP_FRAME_SECTION;
+/* Fingers templates for the current user */
+static uint8_t fp_template[FP_MAX_FINGER_COUNT][FP_ALGORITHM_TEMPLATE_SIZE]
+	FP_TEMPLATE_SECTION;
+/* Number of used templates */
+static uint32_t templ_valid;
+/* Bitmap of the templates with local modifications */
+static uint32_t templ_dirty;
+/* Current user ID */
+static uint32_t user_id[FP_CONTEXT_USERID_WORDS];
 
 #define CPRINTF(format, args...) cprintf(CC_FP, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_FP, format, ## args)
@@ -48,9 +63,11 @@ static uint8_t fp_buffer[FP_SENSOR_IMAGE_SIZE] FP_FRAME_SECTION;
 #define TASK_EVENT_SENSOR_IRQ     TASK_EVENT_CUSTOM(1)
 #define TASK_EVENT_UPDATE_CONFIG  TASK_EVENT_CUSTOM(2)
 
+#define FP_MODE_ANY_CAPTURE (FP_MODE_CAPTURE | FP_MODE_ENROLL_IMAGE | \
+			     FP_MODE_MATCH)
 #define FP_MODE_ANY_DETECT_FINGER (FP_MODE_FINGER_DOWN | FP_MODE_FINGER_UP | \
-				   FP_MODE_CAPTURE)
-#define FP_MODE_ANY_WAIT_IRQ      (FP_MODE_FINGER_DOWN | FP_MODE_CAPTURE)
+				   FP_MODE_ANY_CAPTURE)
+#define FP_MODE_ANY_WAIT_IRQ      (FP_MODE_FINGER_DOWN | FP_MODE_ANY_CAPTURE)
 
 /* Delay between 2 s of the sensor to detect finger removal */
 #define FINGER_POLLING_DELAY (100*MSEC)
@@ -64,6 +81,21 @@ void fps_event(enum gpio_signal signal)
 	task_set_event(TASK_ID_FPSENSOR, TASK_EVENT_SENSOR_IRQ, 0);
 }
 
+static void send_mkbp_event(uint32_t event)
+{
+	atomic_or(&fp_events, event);
+	mkbp_send_event(EC_MKBP_EVENT_FINGERPRINT);
+}
+
+static inline int is_raw_capture(uint32_t mode)
+{
+	int capture_type = FP_CAPTURE_TYPE(mode);
+
+	return (capture_type == FP_CAPTURE_VENDOR_FORMAT
+	     || capture_type == FP_CAPTURE_QUALITY_TEST);
+}
+
+#ifdef HAVE_FP_PRIVATE_DRIVER
 static inline int is_test_capture(uint32_t mode)
 {
 	int capture_type = FP_CAPTURE_TYPE(mode);
@@ -73,19 +105,78 @@ static inline int is_test_capture(uint32_t mode)
 		    || capture_type == FP_CAPTURE_PATTERN1);
 }
 
-static inline int is_raw_capture(uint32_t mode)
+/*
+ * contains the bit FP_MODE_ENROLL_SESSION if a finger enrollment is on-going.
+ * It is used to detect the ENROLL_SESSION transition when sensor_mode is
+ * updated by the host.
+ */
+static uint32_t enroll_session;
+
+static uint32_t fp_process_enroll(void)
 {
-	int capture_type = FP_CAPTURE_TYPE(mode);
+	int percent = 0;
+	int res;
+
+	/* begin/continue enrollment */
+	CPRINTS("[%d]Enrolling ...", templ_valid);
+	res = fp_finger_enroll(fp_buffer, &percent);
+	CPRINTS("[%d]Enroll =>%d (%d%%)", templ_valid, res, percent);
+	if (res < 0)
+		return EC_MKBP_FP_ENROLL
+		     | EC_MKBP_FP_ERRCODE(EC_MKBP_FP_ERR_ENROLL_INTERNAL);
+	templ_dirty |= (1 << templ_valid);
+	if (percent == 100) {
+		res = fp_enrollment_finish(fp_template[templ_valid]);
+		if (res)
+			res = EC_MKBP_FP_ERR_ENROLL_INTERNAL;
+		else
+			templ_valid++;
+		sensor_mode &= ~FP_MODE_ENROLL_SESSION;
+		enroll_session &= ~FP_MODE_ENROLL_SESSION;
+	}
+	return EC_MKBP_FP_ENROLL | EC_MKBP_FP_ERRCODE(res)
+	     | (percent << EC_MKBP_FP_ENROLL_PROGRESS_OFFSET);
+}
 
-	return (capture_type == FP_CAPTURE_VENDOR_FORMAT
-	     || capture_type == FP_CAPTURE_QUALITY_TEST);
+static uint32_t fp_process_match(void)
+{
+	int res;
+	uint32_t updated = 0;
+
+	/* match finger against current templates */
+	CPRINTS("Matching/%d ...", templ_valid);
+	res = fp_finger_match(fp_template[0], templ_valid, fp_buffer, &updated);
+	CPRINTS("Match =>%d", res);
+	if (res < 0)
+		res = EC_MKBP_FP_ERR_MATCH_NO_INTERNAL;
+	if (res == EC_MKBP_FP_ERR_MATCH_YES_UPDATED)
+		templ_dirty |= updated;
+	return EC_MKBP_FP_MATCH | EC_MKBP_FP_ERRCODE(res);
 }
 
-static void send_mkbp_event(uint32_t event)
+static void fp_process_finger(void)
 {
-	atomic_or(&fp_events, event);
-	mkbp_send_event(EC_MKBP_EVENT_FINGERPRINT);
+	int res = fp_sensor_acquire_image_with_mode(fp_buffer,
+			FP_CAPTURE_TYPE(sensor_mode));
+	if (!res) {
+		uint32_t evt = EC_MKBP_FP_IMAGE_READY;
+
+		/* we need CPU power to do the computations */
+		clock_enable_module(MODULE_FAST_CPU, 1);
+
+		if (sensor_mode & FP_MODE_ENROLL_IMAGE)
+			evt = fp_process_enroll();
+		else if (sensor_mode & FP_MODE_MATCH)
+			evt = fp_process_match();
+
+		sensor_mode &= ~FP_MODE_ANY_CAPTURE;
+		send_mkbp_event(evt);
+
+		/* go back to lower power mode */
+		clock_enable_module(MODULE_FAST_CPU, 0);
+	}
 }
+#endif /* HAVE_FP_PRIVATE_DRIVER */
 
 void fp_task(void)
 {
@@ -107,10 +198,23 @@ void fp_task(void)
 		evt = task_wait_event(timeout_us);
 
 		if (evt & TASK_EVENT_UPDATE_CONFIG) {
+			uint32_t mode = sensor_mode;
+
 			gpio_disable_interrupt(GPIO_FPS_INT);
-			if (is_test_capture(sensor_mode)) {
+			if ((mode ^ enroll_session) & FP_MODE_ENROLL_SESSION) {
+				if (mode & FP_MODE_ENROLL_SESSION) {
+					if (fp_enrollment_begin())
+						sensor_mode &=
+							~FP_MODE_ENROLL_SESSION;
+				} else {
+					fp_enrollment_finish(NULL);
+				}
+				enroll_session =
+					sensor_mode & FP_MODE_ENROLL_SESSION;
+			}
+			if (is_test_capture(mode)) {
 				fp_sensor_acquire_image_with_mode(fp_buffer,
-					FP_CAPTURE_TYPE(sensor_mode));
+					FP_CAPTURE_TYPE(mode));
 				sensor_mode &= ~FP_MODE_CAPTURE;
 				send_mkbp_event(EC_MKBP_FP_IMAGE_READY);
 				continue;
@@ -126,7 +230,7 @@ void fp_task(void)
 				timeout_us = FINGER_POLLING_DELAY;
 			else
 				timeout_us = -1;
-			if (sensor_mode & FP_MODE_ANY_WAIT_IRQ)
+			if (mode & FP_MODE_ANY_WAIT_IRQ)
 				gpio_enable_interrupt(GPIO_FPS_INT);
 		} else if (evt & (TASK_EVENT_SENSOR_IRQ | TASK_EVENT_TIMER)) {
 			gpio_disable_interrupt(GPIO_FPS_INT);
@@ -147,16 +251,9 @@ void fp_task(void)
 			}
 
 			if (st == FINGER_PRESENT &&
-			    sensor_mode & FP_MODE_CAPTURE) {
-				int res = fp_sensor_acquire_image_with_mode(
-						fp_buffer,
-						FP_CAPTURE_TYPE(sensor_mode));
-
-				if (!res) {
-					sensor_mode &= ~FP_MODE_CAPTURE;
-					send_mkbp_event(EC_MKBP_FP_IMAGE_READY);
-				}
-			}
+			    sensor_mode & FP_MODE_ANY_CAPTURE)
+				fp_process_finger();
+
 			if (sensor_mode & FP_MODE_ANY_WAIT_IRQ) {
 				fp_sensor_configure_detect();
 				gpio_enable_interrupt(GPIO_FPS_INT);
@@ -172,6 +269,15 @@ void fp_task(void)
 #endif /* !HAVE_FP_PRIVATE_DRIVER */
 }
 
+static void fp_clear_context(void)
+{
+	templ_valid = 0;
+	templ_dirty = 0;
+	memset(fp_buffer, 0, sizeof(fp_buffer));
+	memset(fp_template, 0, sizeof(fp_template));
+	/* TODO maybe shutdown and re-init the private libraries ? */
+}
+
 static int fp_get_next_event(uint8_t *out)
 {
 	uint32_t event_out = atomic_read_clear(&fp_events);
@@ -248,31 +354,95 @@ static int fp_command_info(struct host_cmd_handler_args *args)
 #endif
 		return EC_RES_UNAVAILABLE;
 
-	args->response_size = sizeof(*r);
+	r->template_size = FP_ALGORITHM_TEMPLATE_SIZE;
+	r->template_max = FP_MAX_FINGER_COUNT;
+	r->template_valid = templ_valid;
+	r->template_dirty = templ_dirty;
+
+	/* V1 is identical to V0 with more information appended */
+	args->response_size = args->version ? sizeof(*r) :
+			sizeof(struct ec_response_fp_info_v0);
 	return EC_RES_SUCCESS;
 }
-DECLARE_HOST_COMMAND(EC_CMD_FP_INFO, fp_command_info, EC_VER_MASK(0));
+DECLARE_HOST_COMMAND(EC_CMD_FP_INFO, fp_command_info,
+		     EC_VER_MASK(0) | EC_VER_MASK(1));
 
 static int fp_command_frame(struct host_cmd_handler_args *args)
 {
 	const struct ec_params_fp_frame *params = args->params;
 	void *out = args->response;
-	uint32_t offset = params->offset;
-
-	if (!is_raw_capture(sensor_mode))
-		offset += FP_SENSOR_IMAGE_OFFSET;
+	uint32_t idx = FP_FRAME_TEMPLATE_INDEX(params->offset);
+	uint32_t offset = params->offset & FP_FRAME_OFFSET_MASK;
+	uint32_t max_size;
+	uint8_t *content;
+
+	if (idx == FP_FRAME_INDEX_RAW_IMAGE) {
+		if (!is_raw_capture(sensor_mode))
+			offset += FP_SENSOR_IMAGE_OFFSET;
+		max_size = sizeof(fp_buffer);
+		content = fp_buffer;
+	} else if (idx > FP_MAX_FINGER_COUNT) {
+		return EC_RES_INVALID_PARAM;
+	} else if (idx > templ_valid) {
+		return EC_RES_UNAVAILABLE;
+	} else { /* the host requested a template */
+		max_size = sizeof(fp_template[0]);
+		/* Templates are numbered from 1 in this host request. */
+		content = fp_template[idx - 1];
+		templ_dirty &= ~(1 << (idx - 1));
+	}
 
-	if (offset + params->size > sizeof(fp_buffer) ||
+	if (offset + params->size > max_size ||
 	    params->size > args->response_max)
 		return EC_RES_INVALID_PARAM;
 
-	memcpy(out, fp_buffer + offset, params->size);
+	memcpy(out, content + offset, params->size);
 
 	args->response_size = params->size;
 	return EC_RES_SUCCESS;
 }
 DECLARE_HOST_COMMAND(EC_CMD_FP_FRAME, fp_command_frame, EC_VER_MASK(0));
 
+static int fp_command_template(struct host_cmd_handler_args *args)
+{
+	const struct ec_params_fp_template *params = args->params;
+	uint32_t size = params->size & ~FP_TEMPLATE_COMMIT;
+	uint32_t idx = templ_valid;
+
+	/* Can we store one more template ? */
+	if (idx >= FP_MAX_FINGER_COUNT)
+		return EC_RES_OVERFLOW;
+
+	if ((args->params_size !=
+	     size + offsetof(struct ec_params_fp_template, data)) ||
+	    (params->offset + size > sizeof(fp_template[0])))
+		return EC_RES_INVALID_PARAM;
+
+	memcpy(&fp_template[idx][params->offset], params->data, size);
+
+	if (params->size & FP_TEMPLATE_COMMIT)
+		templ_valid++;
+
+	return EC_RES_SUCCESS;
+}
+DECLARE_HOST_COMMAND(EC_CMD_FP_TEMPLATE, fp_command_template, EC_VER_MASK(0));
+
+static int fp_command_context(struct host_cmd_handler_args *args)
+{
+	const struct ec_params_fp_context *params = args->params;
+	struct ec_response_fp_context *resp = args->response;
+
+	fp_clear_context();
+
+	memcpy(user_id, params->userid, sizeof(user_id));
+	/* TODO(b/73337313): real crypto protocol */
+	memcpy(resp->nonce, params->nonce, sizeof(resp->nonce));
+
+	args->response_size = sizeof(*resp);
+	return EC_RES_SUCCESS;
+}
+DECLARE_HOST_COMMAND(EC_CMD_FP_CONTEXT, fp_command_context, EC_VER_MASK(0));
+
 #ifdef CONFIG_CMD_FPSENSOR_DEBUG
 /* --- Debug console commands --- */
 
@@ -319,35 +489,102 @@ static void upload_pgm_image(uint8_t *frame)
 	ccprintf("\x04"); /* End Of Transmission */
 }
 
-int command_fptest(int argc, char **argv)
+static int fp_console_action(uint32_t mode)
 {
 	int tries = 200;
+	ccprintf("Waiting for finger ...\n");
+	sensor_mode = mode;
+	task_set_event(TASK_ID_FPSENSOR, TASK_EVENT_UPDATE_CONFIG, 0);
+
+	while (tries--) {
+		if (!(sensor_mode & FP_MODE_ANY_CAPTURE)) {
+			ccprintf("done (events:%x)\n", fp_events);
+			return 0;
+		}
+		usleep(100 * MSEC);
+	}
+	return EC_ERROR_TIMEOUT;
+}
+
+int command_fpcapture(int argc, char **argv)
+{
 	int capture_type = FP_CAPTURE_SIMPLE_IMAGE;
+	uint32_t mode;
+	int rc;
 
 	if (argc >= 2) {
 		char *e;
+		int capture_type = strtoi(argv[1], &e, 0);
 
-		capture_type = strtoi(argv[1], &e, 0);
-		if (*e || capture_type < 0 || capture_type > 3)
+		if (*e || capture_type < 0)
 			return EC_ERROR_PARAM1;
 	}
+	mode = FP_MODE_CAPTURE | ((capture_type & FP_MODE_CAPTURE_TYPE_MASK)
+				  << FP_MODE_CAPTURE_TYPE_SHIFT);
 
-	ccprintf("Waiting for finger ...\n");
-	sensor_mode = FP_MODE_CAPTURE |
-		(capture_type << FP_MODE_CAPTURE_TYPE_SHIFT);
+	rc = fp_console_action(mode);
+	if (rc == EC_SUCCESS)
+		upload_pgm_image(fp_buffer + FP_SENSOR_IMAGE_OFFSET);
+
+	return rc;
+}
+DECLARE_CONSOLE_COMMAND(fpcapture, command_fpcapture, "", "");
+
+int command_fpenroll(int argc, char **argv)
+{
+	int rc;
+	int percent = 0;
+	uint32_t event;
+	static const char * const enroll_str[] = {"OK", "Low Quality",
+						  "Immobile", "Low Coverage"};
+
+	do {
+		int tries = 1000;
+
+		rc = fp_console_action(FP_MODE_ENROLL_SESSION |
+				       FP_MODE_ENROLL_IMAGE);
+		if (rc != EC_SUCCESS)
+			break;
+		event = atomic_read_clear(&fp_events);
+		percent = EC_MKBP_FP_ENROLL_PROGRESS(event);
+		ccprintf("Enroll capture: %s (%d%%)\n",
+			 enroll_str[EC_MKBP_FP_ERRCODE(event) & 3], percent);
+		/* wait for finger release between captures */
+		sensor_mode = FP_MODE_ENROLL_SESSION | FP_MODE_FINGER_UP;
+		task_set_event(TASK_ID_FPSENSOR, TASK_EVENT_UPDATE_CONFIG, 0);
+		while (tries-- && sensor_mode & FP_MODE_FINGER_UP)
+			usleep(20 * MSEC);
+	} while (percent < 100);
+	sensor_mode = 0; /* reset FP_MODE_ENROLL_SESSION */
 	task_set_event(TASK_ID_FPSENSOR, TASK_EVENT_UPDATE_CONFIG, 0);
 
-	while (tries--) {
-		if (!(sensor_mode & FP_MODE_CAPTURE)) {
-			ccprintf("done\n");
-			upload_pgm_image(fp_buffer + FP_SENSOR_IMAGE_OFFSET);
-			return 0;
-		}
-		usleep(100 * MSEC);
+	return rc;
+}
+DECLARE_CONSOLE_COMMAND(fpenroll, command_fpenroll, "", "");
+
+
+int command_fpmatch(int argc, char **argv)
+{
+	int rc = fp_console_action(FP_MODE_MATCH);
+	uint32_t event = atomic_read_clear(&fp_events);
+
+	if (rc == EC_SUCCESS && event & EC_MKBP_FP_MATCH) {
+		uint32_t errcode = EC_MKBP_FP_ERRCODE(event);
+
+		ccprintf("Match: %s (%d)\n",
+			 errcode & EC_MKBP_FP_ERR_MATCH_YES ? "YES" : "NO",
+			 errcode);
 	}
 
-	return EC_ERROR_TIMEOUT;
+	return rc;
+}
+DECLARE_CONSOLE_COMMAND(fpmatch, command_fpmatch, "", "");
+
+int command_fpclear(int argc, char **argv)
+{
+	fp_clear_context();
+	return EC_SUCCESS;
 }
-DECLARE_CONSOLE_COMMAND(fptest, command_fptest, "", "");
+DECLARE_CONSOLE_COMMAND(fpclear, command_fpclear, "", "");
 
 #endif /* CONFIG_CMD_FPSENSOR_DEBUG */
diff --git a/include/ec_commands.h b/include/ec_commands.h
index e1b5a77688..223691be24 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -3218,9 +3218,32 @@ struct __ec_align2 ec_response_keyboard_factory_test {
 
 /* Fingerprint events in 'fp_events' for EC_MKBP_EVENT_FINGERPRINT */
 #define EC_MKBP_FP_RAW_EVENT(fp_events) ((fp_events) & 0x00FFFFFF)
+#define EC_MKBP_FP_ERRCODE(fp_events)   ((fp_events) & 0x0000000F)
+#define EC_MKBP_FP_ENROLL_PROGRESS_OFFSET 4
+#define EC_MKBP_FP_ENROLL_PROGRESS(fpe) (((fpe) & 0x00000FF0) \
+					 >> EC_MKBP_FP_ENROLL_PROGRESS_OFFSET)
+#define EC_MKBP_FP_ENROLL               (1 << 27)
+#define EC_MKBP_FP_MATCH                (1 << 28)
 #define EC_MKBP_FP_FINGER_DOWN          (1 << 29)
 #define EC_MKBP_FP_FINGER_UP            (1 << 30)
 #define EC_MKBP_FP_IMAGE_READY          (1 << 31)
+/* code given by EC_MKBP_FP_ERRCODE() when EC_MKBP_FP_ENROLL is set */
+#define EC_MKBP_FP_ERR_ENROLL_OK               0
+#define EC_MKBP_FP_ERR_ENROLL_LOW_QUALITY      1
+#define EC_MKBP_FP_ERR_ENROLL_IMMOBILE         2
+#define EC_MKBP_FP_ERR_ENROLL_LOW_COVERAGE     3
+#define EC_MKBP_FP_ERR_ENROLL_INTERNAL         5
+/* Can be used to detect if image was usable for enrollment or not. */
+#define EC_MKBP_FP_ERR_ENROLL_PROBLEM_MASK     1
+/* code given by EC_MKBP_FP_ERRCODE() when EC_MKBP_FP_MATCH is set */
+#define EC_MKBP_FP_ERR_MATCH_NO                0
+#define EC_MKBP_FP_ERR_MATCH_NO_INTERNAL       6
+#define EC_MKBP_FP_ERR_MATCH_NO_LOW_QUALITY    2
+#define EC_MKBP_FP_ERR_MATCH_NO_LOW_COVERAGE   4
+#define EC_MKBP_FP_ERR_MATCH_YES               1
+#define EC_MKBP_FP_ERR_MATCH_YES_UPDATED       3
+#define EC_MKBP_FP_ERR_MATCH_YES_UPDATE_FAILED 5
+
 
 /*****************************************************************************/
 /* Temperature sensor commands */
@@ -4694,8 +4717,14 @@ struct __ec_align2 ec_params_fp_sensor_config {
 /* Extracts the capture type from the sensor 'mode' word */
 #define FP_CAPTURE_TYPE(mode) (((mode) >> FP_MODE_CAPTURE_TYPE_SHIFT) \
 					& FP_MODE_CAPTURE_TYPE_MASK)
+/* Finger enrollment session on-going */
+#define FP_MODE_ENROLL_SESSION (1<<4)
+/* Enroll the current finger image */
+#define FP_MODE_ENROLL_IMAGE   (1<<5)
+/* Try to match the current finger image */
+#define FP_MODE_MATCH          (1<<6)
 /* special value: don't change anything just read back current mode */
-#define FP_MODE_DONT_CHANGE   (1<<31)
+#define FP_MODE_DONT_CHANGE    (1<<31)
 
 struct __ec_align4 ec_params_fp_mode {
 	uint32_t mode; /* as defined by FP_MODE_ constants */
@@ -4719,7 +4748,22 @@ struct __ec_align4 ec_response_fp_mode {
 /* Sensor initialization failed */
 #define FP_ERROR_INIT_FAIL (1 << 15)
 
-struct __ec_align2 ec_response_fp_info {
+struct __ec_align4 ec_response_fp_info_v0 {
+	/* Sensor identification */
+	uint32_t vendor_id;
+	uint32_t product_id;
+	uint32_t model_id;
+	uint32_t version;
+	/* Image frame characteristics */
+	uint32_t frame_size;
+	uint32_t pixel_format; /* using V4L2_PIX_FMT_ */
+	uint16_t width;
+	uint16_t height;
+	uint16_t bpp;
+	uint16_t errors; /* see FP_ERROR_ flags above */
+};
+
+struct __ec_align4 ec_response_fp_info {
 	/* Sensor identification */
 	uint32_t vendor_id;
 	uint32_t product_id;
@@ -4732,14 +4776,58 @@ struct __ec_align2 ec_response_fp_info {
 	uint16_t height;
 	uint16_t bpp;
 	uint16_t errors; /* see FP_ERROR_ flags above */
+	/* Template/finger current information */
+	uint32_t template_size;  /* max template size in bytes */
+	uint16_t template_max;   /* maximum number of fingers/templates */
+	uint16_t template_valid; /* number of valid fingers/templates */
+	uint32_t template_dirty; /* bitmap of templates with MCU side changes */
 };
 
-/* Get the last captured finger frame */
+/* Get the last captured finger frame or a template content */
 #define EC_CMD_FP_FRAME 0x0404
 
+/* constants defining the 'offset' field which also contains the frame index */
+#define FP_FRAME_INDEX_SHIFT       28
+#define FP_FRAME_INDEX_RAW_IMAGE    0
+#define FP_FRAME_TEMPLATE_INDEX(offset) ((offset) >> FP_FRAME_INDEX_SHIFT)
+#define FP_FRAME_OFFSET_MASK       0x0FFFFFFF
+
 struct __ec_align4 ec_params_fp_frame {
+	/*
+	 * The offset contains the template index or FP_FRAME_INDEX_RAW_IMAGE
+	 * in the high nibble, and the real offset within the frame in
+	 * FP_FRAME_OFFSET_MASK.
+	 */
+	uint32_t offset;
+	uint32_t size;
+};
+
+/* Load a template into the MCU */
+#define EC_CMD_FP_TEMPLATE 0x0405
+
+/* Flag in the 'size' field indicating that the full template has been sent */
+#define FP_TEMPLATE_COMMIT 0x80000000
+
+struct __ec_align4 ec_params_fp_template {
 	uint32_t offset;
 	uint32_t size;
+	uint8_t data[];
+};
+
+/* Clear the current fingerprint user context and set a new one */
+#define EC_CMD_FP_CONTEXT 0x0406
+
+#define FP_CONTEXT_USERID_WORDS (32 / sizeof(uint32_t))
+#define FP_CONTEXT_NONCE_WORDS (32 / sizeof(uint32_t))
+
+struct __ec_align4 ec_params_fp_context {
+	uint32_t userid[FP_CONTEXT_USERID_WORDS];
+	/* TODO(b/73337313) mostly a placeholder, details to be implemented */
+	uint32_t nonce[FP_CONTEXT_NONCE_WORDS];
+};
+
+struct __ec_align4 ec_response_fp_context {
+	uint32_t nonce[FP_CONTEXT_NONCE_WORDS];
 };
 
 /*****************************************************************************/
diff --git a/include/fpsensor.h b/include/fpsensor.h
index 3a77803223..5fc7f3b584 100644
--- a/include/fpsensor.h
+++ b/include/fpsensor.h
@@ -93,4 +93,61 @@ int fp_sensor_acquire_image(uint8_t *image_data);
  */
 int fp_sensor_acquire_image_with_mode(uint8_t *image_data, int mode);
 
+/*
+ * Compares given finger image against enrolled templates.
+ *
+ * The matching algorithm can update the template with additional biometric data
+ * from the image, if it chooses to do so.
+ *
+ * @param templ a pointer to the array of template buffers.
+ * @param templ_count the number of buffers in the array of templates.
+ * @param image the buffer containing the finger image
+ * @param update_bitmap contains one bit per template, the bit is set if the
+ * match has updated the given template.
+ * @return negative value on error, else one of the following code :
+ * - EC_MKBP_FP_ERR_MATCH_NO on non-match
+ * - EC_MKBP_FP_ERR_MATCH_YES for match when template was not updated with
+ *   new data
+ * - EC_MKBP_FP_ERR_MATCH_YES_UPDATED for match when template was updated
+ * - EC_MKBP_FP_ERR_MATCH_YES_UPDATE_FAILED match, but update failed (not saved)
+ * - EC_MKBP_FP_ERR_MATCH_LOW_QUALITY when matching could not be performed due
+ *   to low image quality
+ * - EC_MKBP_FP_ERR_MATCH_LOW_COVERAGE when matching could not be performed
+ *   due to finger covering too little area of the sensor
+ */
+int fp_finger_match(void *templ, uint32_t templ_count, uint8_t *image,
+		    uint32_t *update_bitmap);
+
+/*
+ * Start a finger enrollment session.
+ *
+ * @return 0 on success or a negative error code.
+ */
+int fp_enrollment_begin(void);
+
+/*
+ * Generate a template from the finger whose enrollment has just being
+ * completed.
+ *
+ * @param templ the buffer which will receive the template.
+ * templ can be set to NULL to abort the current enrollment process.
+ *
+ * @return 0 on success or a negative error code.
+ */
+int fp_enrollment_finish(void *templ);
+
+/*
+ * Adds fingerprint image to the current enrollment session.
+ *
+ * @return a negative value on error or one of the following codes:
+ * - EC_MKBP_FP_ERR_ENROLL_OK when image was successfully enrolled
+ * - EC_MKBP_FP_ERR_ENROLL_IMMOBILE when image added, but user should be
+ *   advised to move finger
+ * - EC_MKBP_FP_ERR_ENROLL_LOW_QUALITY when image could not be used due to low
+ *   image quality
+ * - EC_MKBP_FP_ERR_ENROLL_LOW_COVERAGE when image could not be used due to
+ *   finger covering too little area of the sensor
+ */
+int fp_finger_enroll(uint8_t *image, int *completion);
+
 #endif /* __CROS_EC_FPSENSOR_H */
diff --git a/util/ectool.c b/util/ectool.c
index 28b2eeb8c4..83b263d74b 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -124,6 +124,8 @@ const char help_str[] =
 	"      Prints information about the Fingerprint sensor\n"
 	"  fpmode [capture|deepsleep|fingerdown|fingerup]\n"
 	"      Configure/Read the fingerprint sensor current mode\n"
+	"  fptemplate [<infile>|<index 0..2>]\n"
+	"      Add a template if <infile> is provided, else dump it\n"
 	"  forcelidopen <enable>\n"
 	"      Forces the lid switch to open position\n"
 	"  gpioget <GPIO name>\n"
@@ -1092,22 +1094,51 @@ int cmd_rwsig_action(int argc, char *argv[])
 	return ec_command(EC_CMD_RWSIG_ACTION, 0, &req, sizeof(req), NULL, 0);
 }
 
-static void *fp_download_frame(struct ec_response_fp_info *info, int all)
+#define FP_FRAME_INDEX_SIMPLE_IMAGE -1
+
+/*
+ * Download a frame buffer from the FPMCU.
+ *
+ * Might be either the finger image or a finger template depending on 'index'.
+ *
+ * @param info a pointer to store the struct ec_response_fp_info retrieved by
+ * this command.
+ * @param index the specific frame to retrieve, might be:
+ *  -1 (aka FP_FRAME_INDEX_SIMPLE_IMAGE) for the a single grayscale image.
+ *   0  (aka FP_FRAME_INDEX_RAW_IMAGE) for the full vendor raw finger image.
+ *   1..n for a finger template.
+ *
+ * @returns a pointer to the buffer allocated to contain the frame or NULL
+ * if case of error. The caller must call free() once it no longer needs the
+ * buffer.
+ */
+static void *fp_download_frame(struct ec_response_fp_info *info, int index)
 {
 	struct ec_params_fp_frame p;
 	int rv = 0;
 	size_t stride, size;
 	void *buffer;
 	uint8_t *ptr;
+	int cmdver = ec_cmd_version_supported(EC_CMD_FP_INFO, 1) ? 1 : 0;
+	int rsize = cmdver == 1 ? sizeof(*info)
+				: sizeof(struct ec_response_fp_info_v0);
 
-	rv = ec_command(EC_CMD_FP_INFO, 0, NULL, 0, info, sizeof(*info));
+	/* templates not supported in command v0 */
+	if (index > 0 && cmdver == 0)
+		return NULL;
+
+	rv = ec_command(EC_CMD_FP_INFO, cmdver, NULL, 0, info, rsize);
 	if (rv < 0)
 		return NULL;
 
-	if (all)
-		size = info->frame_size;
-	else
+	if (index == FP_FRAME_INDEX_SIMPLE_IMAGE) {
 		size = (size_t)info->width * info->bpp/8 * info->height;
+		index = FP_FRAME_INDEX_RAW_IMAGE;
+	} else if (index == FP_FRAME_INDEX_RAW_IMAGE) {
+		size = info->frame_size;
+	} else {
+		size = info->template_size;
+	}
 
 	buffer = malloc(size);
 	if (!buffer) {
@@ -1116,7 +1147,7 @@ static void *fp_download_frame(struct ec_response_fp_info *info, int all)
 	}
 
 	ptr = buffer;
-	p.offset = 0;
+	p.offset = index << FP_FRAME_INDEX_SHIFT;
 	while (size) {
 		stride = MIN(ec_max_insize, size);
 		p.size = stride;
@@ -1153,7 +1184,7 @@ static int fp_pattern_frame(int capt_type, const char *title, int inv)
 		return -1;
 	/* ensure the capture has happened without using event support */
 	usleep(200000);
-	pattern = fp_download_frame(&info, 0);
+	pattern = fp_download_frame(&info, FP_FRAME_INDEX_SIMPLE_IMAGE);
 	if (!pattern)
 		return -1;
 
@@ -1239,6 +1270,12 @@ int cmd_fp_mode(int argc, char *argv[])
 			mode |= FP_MODE_FINGER_DOWN;
 		else if (!strncmp(argv[i], "fingerup", 8))
 			mode |= FP_MODE_FINGER_UP;
+		else if (!strncmp(argv[i], "enroll", 6))
+			mode |= FP_MODE_ENROLL_IMAGE | FP_MODE_ENROLL_SESSION;
+		else if (!strncmp(argv[i], "match", 5))
+			mode |= FP_MODE_MATCH;
+		else if (!strncmp(argv[i], "reset", 5))
+			mode = 0;
 		else if (!strncmp(argv[i], "capture", 7))
 			mode |= FP_MODE_CAPTURE;
 		/* capture types */
@@ -1266,6 +1303,11 @@ int cmd_fp_mode(int argc, char *argv[])
 		printf("finger-down ");
 	if (r.mode & FP_MODE_FINGER_UP)
 		printf("finger-up ");
+	if (r.mode & FP_MODE_ENROLL_SESSION)
+		printf("enroll%s ",
+			r.mode & FP_MODE_ENROLL_IMAGE ? "+image" : "");
+	if (r.mode & FP_MODE_MATCH)
+		printf("match ");
 	if (r.mode & FP_MODE_CAPTURE)
 		printf("capture ");
 	printf("\n");
@@ -1276,8 +1318,11 @@ int cmd_fp_info(int argc, char *argv[])
 {
 	struct ec_response_fp_info r;
 	int rv;
+	int cmdver = ec_cmd_version_supported(EC_CMD_FP_INFO, 1) ? 1 : 0;
+	int rsize = cmdver == 1 ? sizeof(r)
+				: sizeof(struct ec_response_fp_info_v0);
 
-	rv = ec_command(EC_CMD_FP_INFO, 0, NULL, 0, &r, sizeof(r));
+	rv = ec_command(EC_CMD_FP_INFO, cmdver, NULL, 0, &r, rsize);
 	if (rv < 0)
 		return rv;
 
@@ -1290,6 +1335,11 @@ int cmd_fp_info(int argc, char *argv[])
 	       r.errors & FP_ERROR_BAD_HWID ? "BAD_HWID " : "",
 	       r.errors & FP_ERROR_INIT_FAIL ? "INIT_FAIL " : "",
 	       FP_ERROR_DEAD_PIXELS(r.errors));
+	if (cmdver == 1) {
+		printf("Templates: size %d count %d/%d dirty bitmap %x\n",
+		       r.template_size, r.template_valid, r.template_max,
+		       r.template_dirty);
+	}
 
 	return 0;
 }
@@ -1297,8 +1347,9 @@ int cmd_fp_info(int argc, char *argv[])
 int cmd_fp_frame(int argc, char *argv[])
 {
 	struct ec_response_fp_info r;
-	int raw = (argc == 2 && !strcasecmp(argv[1], "raw"));
-	void *buffer = fp_download_frame(&r, raw);
+	int idx = (argc == 2 && !strcasecmp(argv[1], "raw")) ?
+		FP_FRAME_INDEX_RAW_IMAGE : FP_FRAME_INDEX_SIMPLE_IMAGE;
+	void *buffer = fp_download_frame(&r, idx);
 	uint8_t *ptr = buffer;
 	int x, y;
 
@@ -1307,7 +1358,7 @@ int cmd_fp_frame(int argc, char *argv[])
 		return -1;
 	}
 
-	if (raw) {
+	if (idx == FP_FRAME_INDEX_RAW_IMAGE) {
 		fwrite(buffer, r.frame_size, 1, stdout);
 		goto frame_done;
 	}
@@ -1326,6 +1377,66 @@ frame_done:
 	return 0;
 }
 
+int cmd_fp_template(int argc, char *argv[])
+{
+	struct ec_response_fp_info r;
+	struct ec_params_fp_template *p = ec_outbuf;
+	int max_chunk = ec_max_outsize
+			- offsetof(struct ec_params_fp_template, data);
+	int idx = -1;
+	char *e;
+	int size;
+	void *buffer = NULL;
+	uint32_t offset = 0;
+	int rv = 0;
+
+	if (argc < 2) {
+		fprintf(stderr, "Usage: %s [<infile>|<index>]\n", argv[0]);
+		return -1;
+	}
+
+	idx = strtol(argv[1], &e, 0);
+	if (!(e && *e)) {
+		buffer = fp_download_frame(&r, idx + 1);
+		if (!buffer) {
+			fprintf(stderr, "Failed to get FP template %d\n", idx);
+			return -1;
+		}
+		fwrite(buffer, r.template_size, 1, stdout);
+		free(buffer);
+		return 0;
+	}
+	/* not an index, is it a filename ? */
+	buffer = read_file(argv[1], &size);
+	if (!buffer) {
+		fprintf(stderr, "Invalid parameter: %s\n", argv[1]);
+		return -1;
+	}
+	printf("sending template from: %s (%d bytes)\n", argv[1], size);
+	while (size) {
+		uint32_t tlen = MIN(max_chunk, size);
+
+		p->offset = offset;
+		p->size = tlen;
+		size -= tlen;
+		if (!size)
+			p->size |= FP_TEMPLATE_COMMIT;
+		memcpy(p->data, buffer + offset, tlen);
+		rv = ec_command(EC_CMD_FP_TEMPLATE, 0, p, tlen +
+				offsetof(struct ec_params_fp_template, data),
+				NULL, 0);
+		if (rv < 0)
+			break;
+		offset += tlen;
+	}
+	if (rv < 0)
+		fprintf(stderr, "Failed with %d\n", rv);
+	else
+		rv = 0;
+	free(buffer);
+	return rv;
+}
+
 /**
  * determine if in GFU mode or not.
  *
@@ -7726,6 +7837,7 @@ const struct command commands[] = {
 	{"fpframe", cmd_fp_frame},
 	{"fpinfo", cmd_fp_info},
 	{"fpmode", cmd_fp_mode},
+	{"fptemplate", cmd_fp_template},
 	{"gpioget", cmd_gpio_get},
 	{"gpioset", cmd_gpio_set},
 	{"hangdetect", cmd_hang_detect},