[RFC PATCH 14/19] tools/vhost-user-rpmb: implement VIRTIO_RPMB_REQ_GET_WRITE_COUNTER

["Alex Bennée" <alex.bennee@linaro.org>]

This is the first function with an implied response that doesn't need
a VIRTIO_RPMB_REQ_RESULT_READ.

Signed-off-by: Alex Bennée <alex.bennee@linaro.org>
---
 tools/vhost-user-rpmb/main.c | 44 ++++++++++++++++++++++++++++++++++++
 1 file changed, 44 insertions(+)

diff --git a/tools/vhost-user-rpmb/main.c b/tools/vhost-user-rpmb/main.c
index 9c98f6916f6f..88747c50fa44 100644
--- a/tools/vhost-user-rpmb/main.c
+++ b/tools/vhost-user-rpmb/main.c
@@ -121,8 +121,10 @@ typedef struct VuRpmb {
     int flash_fd;
     void *flash_map;
     uint8_t *key;
+    uint8_t  last_nonce[16];
     uint16_t last_result;
     uint16_t last_reqresp;
+    uint32_t write_count;
 } VuRpmb;
 
 /* refer to util/iov.c */
@@ -286,6 +288,42 @@ static void vrpmb_handle_program_key(VuDev *dev, struct virtio_rpmb_frame *frame
     return;
 }
 
+/*
+ * vrpmb_handle_get_write_counter:
+ *
+ * We respond straight away with re-using the frame as sent.
+ */
+static struct virtio_rpmb_frame *
+vrpmb_handle_get_write_counter(VuDev *dev, struct virtio_rpmb_frame *frame)
+{
+    VuRpmb *r = container_of(dev, VuRpmb, dev.parent);
+    struct virtio_rpmb_frame *resp = g_new0(struct virtio_rpmb_frame, 1);
+
+    /*
+     * Run the checks from:
+     * 5.12.6.1.2 Device Requirements: Device Operation: Get Write Counter
+     */
+
+    resp->req_resp = htobe16(VIRTIO_RPMB_RESP_GET_COUNTER);
+    if (!r->key) {
+        g_debug("no key programmed");
+        resp->result = htobe16(VIRTIO_RPMB_RES_NO_AUTH_KEY);
+        return resp;
+    } else if (be16toh(frame->block_count) > 1) { /* allow 0 (NONCONF) */
+        g_debug("invalid block count (%d)", be16toh(frame->block_count));
+        resp->result = htobe16(VIRTIO_RPMB_RES_GENERAL_FAILURE);
+    } else {
+        resp->write_counter = htobe32(r->write_count);
+    }
+    /* copy nonce */
+    memcpy(&resp->nonce, &frame->nonce, sizeof(frame->nonce));
+
+    /* calculate MAC */
+    vrpmb_update_mac_in_frame(r, resp);
+
+    return resp;
+}
+
 /*
  * Return the result of the last message. This is only valid if the
  * previous message was VIRTIO_RPMB_REQ_PROGRAM_KEY or
@@ -298,6 +336,9 @@ static struct virtio_rpmb_frame * vrpmb_handle_result_read(VuDev *dev)
     VuRpmb *r = container_of(dev, VuRpmb, dev.parent);
     struct virtio_rpmb_frame *resp = g_new0(struct virtio_rpmb_frame, 1);
 
+    g_info("%s: for request:%x result:%x", __func__,
+           r->last_reqresp, r->last_result);
+
     if (r->last_reqresp == VIRTIO_RPMB_RESP_PROGRAM_KEY ||
         r->last_reqresp == VIRTIO_RPMB_REQ_DATA_WRITE) {
         resp->result = htobe16(r->last_result);
@@ -393,6 +434,9 @@ vrpmb_handle_ctrl(VuDev *dev, int qidx)
             case VIRTIO_RPMB_REQ_PROGRAM_KEY:
                 vrpmb_handle_program_key(dev, f);
                 break;
+            case VIRTIO_RPMB_REQ_GET_WRITE_COUNTER:
+                resp = vrpmb_handle_get_write_counter(dev, f);
+                break;
             case VIRTIO_RPMB_REQ_RESULT_READ:
                 if (!responded) {
                     resp = vrpmb_handle_result_read(dev);
-- 
2.20.1