[PATCH v6 7/9] tools rpmb: add RPBM access tool

Tomas Winkler
2016-09-13T13:23:02Z

Add simple RPMB host testing tool. It can be used
to program key, write and read data block, and retrieve
write counter.

Signed-off-by: Tomas Winkler <tomas.winkler@intel.com>
---
V2: resend
V3: fix missing objtool
V4: add verbose option
V5: 1. Adjust to the new API.
2. Exercise both request and sequence ioctls.
V6: 1. Add includes to openssl/rand.h and endian.h
2. Fix some signed, unsigned comparisons
3. Check results more thoroughly
4. use HOSTCFLAGS in compilation
5. Allocate frames dynamically.

MAINTAINERS | 1 +
tools/Makefile | 14 +-
tools/rpmb/.gitignore | 2 +
tools/rpmb/Makefile | 34 ++
tools/rpmb/rpmb.c | 1031 +++++++++++++++++++++++++++++++++++++++++++++++++
5 files changed, 1077 insertions(+), 5 deletions(-)
create mode 100644 tools/rpmb/.gitignore
create mode 100644 tools/rpmb/Makefile
create mode 100644 tools/rpmb/rpmb.c

diff --git a/MAINTAINERS b/MAINTAINERS
index 3a092b90add3..786ed2a1d2bf 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -10002,6 +10002,7 @@ F: drivers/char/rpmb/*
F: include/uapi/linux/rpmb.h
F: include/linux/rpmb.h
F: Documentation/ABI/testing/sysfs-class-rpmb
+F: tools/rpmb/

RTL2830 MEDIA DRIVER
M: Antti Palosaari <crope@iki.fi>
diff --git a/tools/Makefile b/tools/Makefile
index daa8fb3e4363..1d481b78063f 100644
--- a/tools/Makefile
+++ b/tools/Makefile
@@ -20,6 +20,7 @@ help:
@echo ' lguest - a minimal 32-bit x86 hypervisor'
@echo ' net - misc networking tools'
@echo ' perf - Linux performance measurement and analysis tool'
+ @echo ' rpmb - Replay protected memory block access tool'
@echo ' selftests - various kernel selftests'
@echo ' spi - spi tools'
@echo ' objtool - an ELF object analysis tool'
@@ -56,7 +57,7 @@ acpi: FORCE
cpupower: FORCE
$(call descend,power/$@)

-cgroup firewire hv guest spi usb virtio vm net iio gpio objtool: FORCE
+cgroup firewire hv guest rpmb spi usb virtio vm net iio gpio objtool: FORCE
$(call descend,$@)

liblockdep: FORCE
@@ -86,7 +87,7 @@ freefall: FORCE
$(call descend,laptop/$@)

all: acpi cgroup cpupower gpio hv firewire lguest \
- perf selftests turbostat usb \
+ perf rpmb selftests turbostat usb \
virtio vm net x86_energy_perf_policy \
tmon freefall objtool

@@ -96,7 +97,7 @@ acpi_install:
cpupower_install:
$(call descend,power/$(@:_install=),install)

-cgroup_install firewire_install gpio_install hv_install lguest_install perf_install usb_install virtio_install vm_install net_install objtool_install:
+cgroup_install firewire_install gpio_install hv_install lguest_install perf_install rpmb_install usb_install virtio_install vm_install net_install objtool_install:
$(call descend,$(@:_install=),install)

selftests_install:
@@ -116,7 +117,7 @@ kvm_stat_install:

install: acpi_install cgroup_install cpupower_install gpio_install \
hv_install firewire_install lguest_install \
- perf_install selftests_install turbostat_install usb_install \
+ perf_install rpmb_install selftests_install turbostat_install usb_install \
virtio_install vm_install net_install x86_energy_perf_policy_install \
tmon_install freefall_install objtool_install kvm_stat_install

@@ -145,6 +146,9 @@ perf_clean:
$(Q)mkdir -p $(PERF_O) .
$(Q)$(MAKE) --no-print-directory -C perf O=$(PERF_O) subdir= clean

+rpmb_clean:
+ $(call descend,$(@:_clean=),clean)
+
selftests_clean:
$(call descend,testing/$(@:_clean=),clean)

@@ -161,7 +165,7 @@ build_clean:
$(call descend,build,clean)

clean: acpi_clean cgroup_clean cpupower_clean hv_clean firewire_clean lguest_clean \
- perf_clean selftests_clean turbostat_clean spi_clean usb_clean virtio_clean \
+ perf_clean rpmb_clean selftests_clean turbostat_clean spi_clean usb_clean virtio_clean \
vm_clean net_clean iio_clean x86_energy_perf_policy_clean tmon_clean \
freefall_clean build_clean libbpf_clean libsubcmd_clean liblockdep_clean \
gpio_clean objtool_clean
diff --git a/tools/rpmb/.gitignore b/tools/rpmb/.gitignore
new file mode 100644
index 000000000000..218f680548e6
--- /dev/null
+++ b/tools/rpmb/.gitignore
@@ -0,0 +1,2 @@
+*.o
+rpmb
diff --git a/tools/rpmb/Makefile b/tools/rpmb/Makefile
new file mode 100644
index 000000000000..debb5a6bc208
--- /dev/null
+++ b/tools/rpmb/Makefile
@@ -0,0 +1,34 @@
+CC = $(CROSS_COMPILE)gcc
+LD = $(CROSS_COMPILE)ld
+PKG_CONFIG = $(CROSS_COMPILE)pkg-config
+
+ifeq ($(srctree),)
+srctree := $(patsubst %/,%,$(dir $(shell pwd)))
+srctree := $(patsubst %/,%,$(dir $(srctree)))
+#$(info Determined 'srctree' to be $(srctree))
+endif
+
+INSTALL = install
+prefix ?= /usr/local
+bindir = $(prefix)/bin
+
+
+CFLAGS += $(HOSTCFLAGS)
+CFLAGS += -D__EXPORTED_HEADERS__ -g -static
+LDFLAGS += -static
+CFLAGS += -I$(srctree)/include/uapi -I$(srctree)/include
+LDLIBS += $(shell $(PKG_CONFIG) --libs --static libcrypto)
+
+prog := rpmb
+
+all : $(prog)
+
+$(prog): rpmb.o
+
+clean :
+ $(RM) $(prog) *.o
+
+install: $(prog)
+ $(INSTALL) -m755 -d $(DESTDIR)$(bindir)
+ $(INSTALL) $(prog) $(DESTDIR)$(bindir)
+
diff --git a/tools/rpmb/rpmb.c b/tools/rpmb/rpmb.c
new file mode 100644
index 000000000000..4365a4a2c9eb
--- /dev/null
+++ b/tools/rpmb/rpmb.c
@@ -0,0 +1,1031 @@
+/******************************************************************************
+ * This file is provided under a dual BSD/GPLv2 license. When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2016 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * Intel Corporation.
+ * linux-mei@linux.intel.com
+ * http://www.intel.com
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2016 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name Intel Corporation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <libgen.h>
+#include <limits.h>
+#include <ctype.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <endian.h>
+
+#include <openssl/engine.h>
+#include <openssl/hmac.h>
+#include <openssl/rand.h>
+
+#include "linux/rpmb.h"
+
+#define RPMB_KEY_SIZE 32
+#define RPMB_MAC_SIZE 32
+#define RPMB_NONCE_SIZE 16
+
+bool verbose;
+#define rpmb_dbg(fmt, ARGS...) do { \
+ if (verbose) \
+ fprintf(stderr, "rpmb: " fmt, ##ARGS); \
+} while (0)
+
+#define rpmb_msg(fmt, ARGS...) \
+ fprintf(stderr, "rpmb: " fmt, ##ARGS)
+
+#define rpmb_err(fmt, ARGS...) \
+ fprintf(stderr, "rpmb: error: " fmt, ##ARGS)
+
+static const char *rpmb_op_str(uint16_t op)
+{
+#define RPMB_OP(_op) case RPMB_##_op: return #_op
+
+ switch (op) {
+ RPMB_OP(PROGRAM_KEY);
+ RPMB_OP(GET_WRITE_COUNTER);
+ RPMB_OP(WRITE_DATA);
+ RPMB_OP(READ_DATA);
+ RPMB_OP(RESULT_READ);
+ break;
+ default:
+ return "unknown";
+ }
+#undef RPMB_OP
+}
+
+static const char *rpmb_result_str(enum rpmb_op_result result)
+{
+#define str(x) #x
+#define RPMB_ERR(_res) case RPMB_ERR_##_res: \
+ { if (result & RPMB_ERR_COUNTER_EXPIRED) \
+ return "COUNTER_EXPIRE:" str(_res); \
+ else \
+ return #_res; \
+ }
+
+ switch (result & 0x000F) {
+ RPMB_ERR(OK);
+ RPMB_ERR(GENERAL);
+ RPMB_ERR(AUTH);
+ RPMB_ERR(COUNTER);
+ RPMB_ERR(ADDRESS);
+ RPMB_ERR(WRITE);
+ RPMB_ERR(READ);
+ RPMB_ERR(NO_KEY);
+ break;
+ default:
+ return "unknown";
+ }
+#undef RPMB_ERR
+#undef str
+};
+
+static inline void __dump_buffer(const char *buf)
+{
+ fprintf(stderr, "%s\n", buf);
+}
+
+static void
+dump_hex_buffer(const char *title, const void *buf, size_t len)
+{
+ const unsigned char *_buf = (const unsigned char *)buf;
+ const size_t pbufsz = 16 * 3;
+ char pbuf[pbufsz];
+ int j = 0;
+
+ if (title)
+ fprintf(stderr, "%s\n", title);
+ while (len-- > 0) {
+ snprintf(&pbuf[j], pbufsz - j, "%02X ", *_buf++);
+ j += 3;
+ if (j == 16 * 3) {
+ __dump_buffer(pbuf);
+ j = 0;
+ }
+ }
+ if (j)
+ __dump_buffer(pbuf);
+}
+
+static void dbg_dump_frame(const char *title, const struct rpmb_frame *f)
+{
+ uint16_t result, req_resp;
+
+ if (!verbose)
+ return;
+
+ if (!f)
+ return;
+
+ if (title)
+ fprintf(stderr, "%s\n", title);
+
+ result = be16toh(f->result);
+ req_resp = be16toh(f->req_resp);
+ if (req_resp & 0xf00)
+ req_resp = RPMB_RESP2REQ(req_resp);
+
+ fprintf(stderr, "ptr: %p\n", f);
+ dump_hex_buffer("key_mac: ", f->key_mac, 32);
+ dump_hex_buffer("data: ", f->data, 256);
+ dump_hex_buffer("nonce: ", f->nonce, 16);
+ fprintf(stderr, "write_counter: %u\n", be32toh(f->write_counter));
+ fprintf(stderr, "address: %0X\n", be16toh(f->addr));
+ fprintf(stderr, "block_count: %u\n", be16toh(f->block_count));
+ fprintf(stderr, "result %s:%d\n", rpmb_result_str(result), result);
+ fprintf(stderr, "req_resp %s\n", rpmb_op_str(req_resp));
+}
+
+static int open_dev_file(const char *devfile)
+{
+ int fd;
+
+ fd = open(devfile, O_RDWR);
+ if (fd < 0)
+ rpmb_err("Cannot open: %s: %s\n", devfile, strerror(errno));
+ return fd;
+}
+
+static int open_rd_file(const char *datafile, const char *type)
+{
+ int fd;
+
+ if (!strcmp(datafile, "-"))
+ fd = STDIN_FILENO;
+ else
+ fd = open(datafile, O_RDONLY);
+
+ if (fd < 0)
+ rpmb_err("Cannot open %s: %s: %s\n",
+ type, datafile, strerror(errno));
+
+ return fd;
+}
+
+static int open_wr_file(const char *datafile, const char *type)
+{
+ int fd;
+
+ if (!strcmp(datafile, "-"))
+ fd = STDOUT_FILENO;
+ else
+ fd = open(datafile, O_WRONLY | O_CREAT | O_APPEND,
+ S_IRUSR | S_IWUSR);
+ if (fd < 0)
+ rpmb_err("Cannot open %s: %s: %s\n",
+ type, datafile, strerror(errno));
+ return fd;
+}
+
+static void close_fd(int fd)
+{
+ if (fd > 0 && fd != STDIN_FILENO && fd != STDOUT_FILENO)
+ close(fd);
+}
+
+/* blocking rw wrapper */
+#define rw(func, fd, buf, size) \
+({ \
+ ssize_t nread = 0, n; \
+ char *_buf = (char *)buf; \
+ do { \
+ n = func(fd, _buf + nread, size - nread); \
+ if (n == -1 && errno != EINTR) { \
+ nread = -1; \
+ break; \
+ } else if (n > 0) { \
+ nread += n; \
+ } \
+ } while (n != 0 && (size_t)nread != size); \
+ nread; \
+})
+
+static ssize_t read_file(int fd, unsigned char *data, size_t size)
+{
+ ssize_t ret;
+
+ ret = rw(read, fd, data, size);
+ if (ret < 0) {
+ rpmb_err("cannot read file: %s\n", strerror(errno));
+ } else if ((size_t)ret != size) {
+ rpmb_err("read %zd but must be %zu bytes length\n", ret, size);
+ ret = -EINVAL;
+ }
+
+ return ret;
+}
+
+static ssize_t write_file(int fd, unsigned char *data, size_t size)
+{
+ ssize_t ret;
+
+ ret = rw(write, fd, data, size);
+ if (ret < 0) {
+ rpmb_err("cannot read file: %s\n", strerror(errno));
+ } else if ((size_t)ret != size) {
+ rpmb_err("data is %zd but must be %zu bytes length\n",
+ ret, size);
+ ret = -EINVAL;
+ }
+ return ret;
+}
+
+static struct rpmb_frame *rpmb_alloc_frames(unsigned int cnt)
+{
+ return calloc(1, sizeof(struct rpmb_frame));
+}
+
+static int rpmb_calc_hmac_sha256(struct rpmb_frame *frames, size_t blocks_cnt,
+ const unsigned char key[],
+ unsigned int key_size,
+ unsigned char mac[],
+ unsigned int mac_size)
+{
+ HMAC_CTX ctx;
+ int ret;
+ unsigned int i;
+
+ /* SSL returns 1 on success 0 on failure */
+
+ HMAC_CTX_init(&ctx);
+ ret = HMAC_Init_ex(&ctx, key, key_size, EVP_sha256(), NULL);
+ if (ret == 0)
+ goto out;
+ for (i = 0; i < blocks_cnt; i++)
+ HMAC_Update(&ctx, frames[i].data, hmac_data_len);
+
+ ret = HMAC_Final(&ctx, mac, &mac_size);
+ if (ret == 0)
+ goto out;
+ if (mac_size != RPMB_MAC_SIZE)
+ ret = 0;
+
+ ret = 1;
+out:
+ HMAC_CTX_cleanup(&ctx);
+ return ret == 1 ? 0 : -1;
+}
+
+static int rpmb_check_req_resp(uint16_t req, struct rpmb_frame *frame_out)
+{
+ if (RPMB_REQ2RESP(req) != be16toh(frame_out->req_resp)) {
+ rpmb_err("RPMB response mismatch %04X != %04X\n",
+ RPMB_REQ2RESP(req), be16toh(frame_out->req_resp));
+ return -1;
+ }
+ return 0;
+}
+
+static int rpmb_check_mac(const unsigned char *key,
+ struct rpmb_frame *frames_out,
+ unsigned int cnt_out)
+{
+ unsigned char mac[RPMB_MAC_SIZE];
+
+ if (cnt_out == 0) {
+ rpmb_err("RPMB 0 output frames\n");
+ return -1;
+ }
+
+ rpmb_calc_hmac_sha256(frames_out, cnt_out,
+ key, RPMB_KEY_SIZE,
+ mac, RPMB_MAC_SIZE);
+
+ if (memcmp(mac, frames_out[cnt_out - 1].key_mac, RPMB_MAC_SIZE)) {
+ rpmb_err("RPMB hmac mismatch\n");
+ dump_hex_buffer("Result MAC: ",
+ frames_out[cnt_out - 1].key_mac, RPMB_MAC_SIZE);
+ dump_hex_buffer("Expected MAC: ", mac, RPMB_MAC_SIZE);
+ return -1;
+ }
+
+ return 0;
+}
+
+static int (*rpmb_ioctl)(int fd, uint16_t req,
+ const struct rpmb_frame *frames_in,
+ unsigned int cnt_in,
+ struct rpmb_frame *frames_out,
+ unsigned int cnt_out);
+
+static int rpmb_ioctl_seq(int fd, uint16_t req,
+ const struct rpmb_frame *frames_in,
+ unsigned int cnt_in,
+ struct rpmb_frame *frames_out,
+ unsigned int cnt_out)
+{
+ int ret;
+ struct {
+ struct rpmb_ioc_seq_cmd h;
+ struct rpmb_ioc_cmd cmd[3];
+ } iseq = {};
+ struct rpmb_frame *frame_res = NULL;
+ int i;
+ uint32_t flags;
+
+ rpmb_dbg("RPMB OP: %s\n", rpmb_op_str(req));
+ dbg_dump_frame("In Frame: ", frames_in);
+
+ i = 0;
+ flags = RPMB_F_WRITE;
+ if (req == RPMB_WRITE_DATA || req == RPMB_PROGRAM_KEY)
+ flags |= RPMB_F_REL_WRITE;
+ rpmb_ioc_cmd_set(iseq.cmd[i], flags, frames_in, cnt_in);
+ i++;
+
+ if (req == RPMB_WRITE_DATA || req == RPMB_PROGRAM_KEY) {
+ frame_res = rpmb_alloc_frames(1);
+ if (!frame_res)
+ return -ENOMEM;
+ frame_res->req_resp = htobe16(RPMB_RESULT_READ);
+ rpmb_ioc_cmd_set(iseq.cmd[i], RPMB_F_WRITE, frame_res, 1);
+ i++;
+ }
+
+ rpmb_ioc_cmd_set(iseq.cmd[i], 0, frames_out, cnt_out);
+ i++;
+
+ iseq.h.num_of_cmds = i;
+ ret = ioctl(fd, RPMB_IOC_SEQ_CMD, &iseq);
+ if (ret < 0)
+ rpmb_err("ioctl failure %d: %s\n", ret, strerror(errno));
+
+ ret = rpmb_check_req_resp(req, frames_out);
+
+ dbg_dump_frame("Res Frame: ", frame_res);
+ dbg_dump_frame("Out Frame: ", frames_out);
+ free(frame_res);
+ return ret;
+}
+
+static int rpmb_ioctl_req(int fd, uint16_t req,
+ const struct rpmb_frame *frames_in,
+ unsigned int cnt_in,
+ struct rpmb_frame *frames_out,
+ unsigned int cnt_out)
+{
+ struct rpmb_ioc_req_cmd ireq;
+ int ret;
+
+ ireq.req_type = req;
+ rpmb_ioc_cmd_set(ireq.icmd, RPMB_F_WRITE, frames_in, cnt_in);
+ rpmb_ioc_cmd_set(ireq.ocmd, 0, frames_out, cnt_out);
+
+ rpmb_dbg("RPMB OP: %s\n", rpmb_op_str(req));
+ dbg_dump_frame("In Frame: ", frames_in);
+ ret = ioctl(fd, RPMB_IOC_REQ_CMD, &ireq);
+ if (ret < 0)
+ rpmb_err("ioctl failure %d: %s\n", ret, strerror(errno));
+
+ ret = rpmb_check_req_resp(req, frames_out);
+ dbg_dump_frame("Out Frame: ", frames_out);
+
+ return ret;
+}
+
+static int op_rpmb_program_key(int nargs, char *argv[])
+{
+ int ret;
+ int dev_fd = -1, key_fd = -1;
+ uint16_t req = RPMB_PROGRAM_KEY;
+ struct rpmb_frame *frame_in = NULL, *frame_out = NULL;
+
+ ret = -EINVAL;
+ if (nargs != 2)
+ return ret;
+
+ dev_fd = open_dev_file(argv[0]);
+ if (dev_fd < 0)
+ goto out;
+ argv++;
+
+ key_fd = open_rd_file(argv[0], "key file");
+ if (key_fd < 0)
+ goto out;
+ argv++;
+
+ frame_in = rpmb_alloc_frames(1);
+ frame_out = rpmb_alloc_frames(1);
+ if (!frame_in || !frame_out) {
+ ret = -ENOMEM;
+ goto out;
+ }
+
+ frame_in->req_resp = htobe16(req);
+
+ read_file(key_fd, frame_in->key_mac, RPMB_KEY_SIZE);
+
+ ret = rpmb_ioctl(dev_fd, req, frame_in, 1, frame_out, 1);
+ if (ret)
+ goto out;
+
+ if (RPMB_REQ2RESP(req) != be16toh(frame_out->req_resp)) {
+ rpmb_err("RPMB response mismatch\n");
+ ret = -1;
+ goto out;
+ }
+
+ ret = be16toh(frame_out->result);
+ if (ret)
+ rpmb_err("RPMB operation %s failed, %s[0x%04x]\n",
+ rpmb_op_str(req), rpmb_result_str(ret), ret);
+
+out:
+ free(frame_in);
+ free(frame_out);
+ close_fd(dev_fd);
+ close_fd(key_fd);
+
+ return ret;
+}
+
+static int rpmb_get_write_counter(int dev_fd, unsigned int *cnt,
+ const unsigned char *key)
+{
+ int ret;
+ uint16_t res = 0x000F;
+ uint16_t req = RPMB_GET_WRITE_COUNTER;
+ struct rpmb_frame *frame_in = NULL;
+ struct rpmb_frame *frame_out = NULL;
+
+ frame_in = rpmb_alloc_frames(1);
+ frame_out = rpmb_alloc_frames(1);
+ if (!frame_in || !frame_out) {
+ ret = -ENOMEM;
+ goto out;
+ }
+
+ frame_in->req_resp = htobe16(req);
+ RAND_bytes(frame_in->nonce, RPMB_NONCE_SIZE);
+
+ ret = rpmb_ioctl(dev_fd, req, frame_in, 1, frame_out, 1);
+ if (ret != 0)
+ return ret;
+
+ res = be16toh(frame_out->result);
+ if (res != RPMB_ERR_OK) {
+ ret = -1;
+ goto out;
+ }
+
+ if (memcmp(&frame_in->nonce, &frame_out->nonce, RPMB_NONCE_SIZE)) {
+ rpmb_err("RPMB NONCE mismatch\n");
+ dump_hex_buffer("Result NONCE:",
+ &frame_out->nonce, RPMB_NONCE_SIZE);
+ dump_hex_buffer("Expected NONCE: ",
+ &frame_in->nonce, RPMB_NONCE_SIZE);
+ ret = -1;
+ goto out;
+ }
+
+ if (key) {
+ ret = rpmb_check_mac(key, frame_out, 1);
+ if (ret)
+ goto out;
+ }
+
+ *cnt = be32toh(frame_out->write_counter);
+
+out:
+ if (ret)
+ rpmb_err("RPMB operation %s failed, %s[0x%04x]\n",
+ rpmb_op_str(req), rpmb_result_str(res), res);
+ return ret;
+}
+
+static int op_rpmb_get_write_counter(int nargs, char **argv)
+{
+ int ret;
+ int dev_fd = -1, key_fd = -1;
+ bool has_key;
+ unsigned char key[RPMB_KEY_SIZE];
+ unsigned int cnt;
+
+ if (nargs == 2)
+ has_key = true;
+ else if (nargs == 1)
+ has_key = false;
+ else
+ return -EINVAL;
+
+ ret = -EINVAL;
+ dev_fd = open_dev_file(argv[0]);
+ if (dev_fd < 0)
+ return ret;
+ argv++;
+
+ if (has_key) {
+ key_fd = open_rd_file(argv[0], "key file");
+ if (key_fd < 0)
+ goto out;
+ argv++;
+
+ ret = read_file(key_fd, key, RPMB_KEY_SIZE);
+ if (ret < 0)
+ goto out;
+
+ ret = rpmb_get_write_counter(dev_fd, &cnt, key);
+ } else {
+ ret = rpmb_get_write_counter(dev_fd, &cnt, NULL);
+ }
+
+ if (!ret)
+ printf("Counter value: 0x%08x\n", cnt);
+
+out:
+ close_fd(dev_fd);
+ close_fd(key_fd);
+ return ret;
+}
+
+static int op_rpmb_read_blocks(int nargs, char **argv)
+{
+ int i, ret;
+ int dev_fd = -1, data_fd = -1, key_fd = -1;
+ uint16_t req = RPMB_READ_DATA;
+ uint16_t addr, blocks_cnt;
+ unsigned char key[RPMB_KEY_SIZE];
+ unsigned long numarg;
+ bool has_key;
+ struct rpmb_frame *frame_in = NULL;
+ struct rpmb_frame *frames_out = NULL;
+ struct rpmb_frame *frame_out;
+
+ ret = -EINVAL;
+ if (nargs == 4)
+ has_key = false;
+ else if (nargs == 5)
+ has_key = true;
+ else
+ return ret;
+
+ dev_fd = open_dev_file(argv[0]);
+ if (dev_fd < 0)
+ goto out;
+ argv++;
+
+ errno = 0;
+ numarg = strtoul(argv[0], NULL, 0);
+ if (errno || numarg > USHRT_MAX) {
+ rpmb_err("wrong block address\n");
+ goto out;
+ }
+ addr = (uint16_t)numarg;
+ argv++;
+
+ errno = 0;
+ numarg = strtoul(argv[0], NULL, 0);
+ if (errno || numarg > USHRT_MAX) {
+ rpmb_err("wrong blocks count\n");
+ goto out;
+ }
+ blocks_cnt = (uint16_t)numarg;
+ argv++;
+
+ if (blocks_cnt == 0) {
+ rpmb_err("wrong blocks count\n");
+ goto out;
+ }
+
+ data_fd = open_wr_file(argv[0], "output data");
+ if (data_fd < 0)
+ goto out;
+ argv++;
+
+ if (has_key) {
+ key_fd = open_rd_file(argv[0], "key file");
+ if (key_fd < 0)
+ goto out;
+ argv++;
+
+ ret = read_file(key_fd, key, RPMB_KEY_SIZE);
+ if (ret < 0)
+ goto out;
+ }
+
+ ret = 0;
+ frames_out = rpmb_alloc_frames(blocks_cnt);
+ frame_in = rpmb_alloc_frames(1);
+ if (!frames_out || !frame_in) {
+ rpmb_err("Cannot allocate %d RPMB frames\n", blocks_cnt);
+ ret = -ENOMEM;
+ goto out;
+ }
+
+ frame_in->req_resp = htobe16(req);
+ frame_in->addr = htobe16(addr);
+ /* eMMc spec ask for 0 here this will be translated by the rpmb layer */
+ frame_in->block_count = htobe16(blocks_cnt);
+ if (has_key)
+ RAND_bytes(frame_in->nonce, RPMB_NONCE_SIZE);
+
+ ret = rpmb_ioctl(dev_fd, req, frame_in, 1, frames_out, blocks_cnt);
+ if (ret)
+ goto out;
+
+ frame_out = &frames_out[blocks_cnt - 1];
+ ret = be16toh(frame_out->result);
+ if (ret) {
+ rpmb_err("RPMB operation %s failed, %s[0x%04x]\n",
+ rpmb_op_str(req), rpmb_result_str(ret), ret);
+ goto out;
+ }
+
+ if (has_key) {
+ ret = rpmb_check_mac(key, frames_out, blocks_cnt);
+ if (ret)
+ goto out;
+ }
+
+ for (i = 0; i < blocks_cnt; i++) {
+ ret = write_file(data_fd, frames_out[i].data,
+ sizeof(frames_out[i].data));
+ if (ret < 0)
+ goto out;
+ }
+
+out:
+ free(frame_in);
+ free(frames_out);
+ close_fd(dev_fd);
+ close_fd(data_fd);
+ close_fd(key_fd);
+
+ return ret;
+}
+
+static int op_rpmb_write_blocks(int nargs, char **argv)
+{
+ int ret;
+ int dev_fd = -1, key_fd = -1, data_fd = -1;
+ int i;
+ uint16_t req = RPMB_WRITE_DATA;
+ unsigned char key[RPMB_KEY_SIZE];
+ unsigned char mac[RPMB_MAC_SIZE];
+ unsigned long numarg;
+ uint16_t addr, blocks_cnt;
+ uint32_t write_counter;
+ struct rpmb_frame *frames_in = NULL;
+ struct rpmb_frame *frame_out = NULL;
+
+ ret = -EINVAL;
+ if (nargs != 5)
+ goto out;
+
+ dev_fd = open_dev_file(argv[0]);
+ if (dev_fd < 0)
+ goto out;
+ argv++;
+
+ errno = 0;
+ numarg = strtoul(argv[0], NULL, 0);
+ if (errno || numarg > USHRT_MAX) {
+ rpmb_err("wrong block address %s\n", argv[0]);
+ goto out;
+ }
+ addr = (uint16_t)numarg;
+ argv++;
+
+ errno = 0;
+ numarg = strtoul(argv[0], NULL, 0);
+ if (errno || numarg > USHRT_MAX) {
+ rpmb_err("wrong blocks count\n");
+ goto out;
+ }
+ blocks_cnt = (uint16_t)numarg;
+ argv++;
+
+ if (blocks_cnt == 0) {
+ rpmb_err("wrong blocks count\n");
+ goto out;
+ }
+
+ data_fd = open_rd_file(argv[0], "data file");
+ if (data_fd < 0)
+ goto out;
+ argv++;
+
+ key_fd = open_rd_file(argv[0], "key file");
+ if (key_fd < 0)
+ goto out;
+ argv++;
+
+ ret = read_file(key_fd, key, RPMB_KEY_SIZE);
+ if (ret < 0)
+ goto out;
+
+ frames_in = rpmb_alloc_frames(blocks_cnt);
+ frame_out = rpmb_alloc_frames(1);
+ if (!frames_in || !frame_out) {
+ rpmb_err("can't allocate memory for RPMB outer frames\n");
+ ret = -ENOMEM;
+ goto out;
+ }
+
+ ret = rpmb_get_write_counter(dev_fd, &write_counter, key);
+ if (ret)
+ goto out;
+
+ for (i = 0; i < blocks_cnt; i++) {
+ frames_in[i].req_resp = htobe16(req);
+ frames_in[i].block_count = htobe16(blocks_cnt);
+ frames_in[i].addr = htobe16(addr);
+ frames_in[i].write_counter = htobe32(write_counter);
+ }
+
+ for (i = 0; i < blocks_cnt; i++) {
+ ret = read_file(data_fd, frames_in[i].data,
+ sizeof(frames_in[0].data));
+ if (ret < 0)
+ goto out;
+ }
+
+ rpmb_calc_hmac_sha256(frames_in, blocks_cnt,
+ key, RPMB_KEY_SIZE,
+ mac, RPMB_MAC_SIZE);
+ memcpy(frames_in[blocks_cnt - 1].key_mac, mac, RPMB_MAC_SIZE);
+ ret = rpmb_ioctl(dev_fd, req, frames_in, blocks_cnt, frame_out, 1);
+ if (ret != 0)
+ goto out;
+
+ ret = be16toh(frame_out->result);
+ if (ret) {
+ rpmb_err("RPMB operation %s failed, %s[0x%04x]\n",
+ rpmb_op_str(req), rpmb_result_str(ret), ret);
+ ret = -1;
+ }
+
+ if (be16toh(frame_out->addr) != addr) {
+ rpmb_err("RPMB addr mismatchs res=%04x req=%04x\n",
+ be16toh(frame_out->addr), addr);
+ ret = -1;
+ }
+
+ if (be32toh(frame_out->write_counter) <= write_counter) {
+ rpmb_err("RPMB write counter not incremeted res=%x req=%x\n",
+ be32toh(frame_out->write_counter), write_counter);
+ ret = -1;
+ }
+
+ ret = rpmb_check_mac(key, frame_out, 1);
+out:
+ free(frames_in);
+ free(frame_out);
+ close_fd(dev_fd);
+ close_fd(data_fd);
+ close_fd(key_fd);
+ return ret;
+}
+
+typedef int (*rpmb_op)(int argc, char *argv[]);
+
+struct rpmb_cmd {
+ const char *op_name;
+ rpmb_op op;
+ const char *usage; /* usage title */
+ const char *help; /* help */
+};
+
+static const struct rpmb_cmd cmds[] = {
+ {
+ "program-key",
+ op_rpmb_program_key,
+ "<RPMB_DEVICE> <KEY_FILE>",
+ " Program authentication key of 32 bytes length from the KEY_FILE\n"
+ " when KEY_FILE is -, read standard input.\n"
+ " NOTE: This is a one-time programmable irreversible change.\n",
+ },
+ {
+ "write-counter",
+ op_rpmb_get_write_counter,
+ "<RPMB_DEVICE> [KEY_FILE]",
+ " Rertrive write counter value from the <RPMB_DEVICE> to stdout.\n"
+ " When KEY_FILE is present data is verified via HMAC\n"
+ " when KEY_FILE is -, read standard input.\n"
+ },
+ {
+ "write-blocks",
+ op_rpmb_write_blocks,
+ "<RPMB_DEVICE> <block_count> <DATA_FILE> <KEY_FILE>",
+ " <block count> of 256 bytes will be written from the DATA_FILE\n"
+ " to the <RPMB_DEVICE> at block offset .\n"
+ " When DATA_FILE is -, read from standard input.\n",
+ },
+ {
+ "read-blocks",
+ op_rpmb_read_blocks,
+ "<RPMB_DEVICE> <blocks count> <OUTPUT_FILE> [KEY_FILE]",
+ " <block count> of 256 bytes will be read from <RPMB_DEVICE>\n"
+ " to the OUTPUT_FILE\n"
+ " When KEY_FILE is present data is verified via HMAC\n"
+ " When OUTPUT/KEY_FILE is -, read from standard input.\n"
+ " When OUTPUT_FILE is -, write to standard output\n",
+ },
+
+ { NULL, NULL, NULL, NULL }
+};
+
+static void help(const char *prog, const struct rpmb_cmd *cmd)
+{
+ printf("%s %s %s\n", prog, cmd->op_name, cmd->usage);
+ printf("%s\n", cmd->help);
+}
+
+static void usage(const char *prog)
+{
+ int i;
+
+ printf("\n");
+ printf("Usage: %s [-v] [-r|-s] <command> <args>\n\n", prog);
+ for (i = 0; cmds[i].op_name; i++)
+ printf(" %s %s %s\n",
+ prog, cmds[i].op_name, cmds[i].usage);
+
+ printf("\n");
+ printf(" %s -v/--verbose: runs in verbose mode\n", prog);
+ printf(" %s -s/ --sequence: use RPMB_IOC_SEQ_CMD\n", prog);
+ printf(" %s -r/--request: use RPMB_IOC_REQ_CMD\n", prog);
+ printf(" %s help : shows this help\n", prog);
+ printf(" %s help <command>: shows detailed help\n", prog);
+}
+
+static bool call_for_help(const char *arg)
+{
+ return !strcmp(arg, "help") ||
+ !strcmp(arg, "-h") ||
+ !strcmp(arg, "--help");
+}
+
+static bool parse_verbose(const char *arg)
+{
+ return !strcmp(arg, "-v") ||
+ !strcmp(arg, "--verbose");
+}
+
+static bool parse_req(const char *arg)
+{
+ return !strcmp(arg, "-r") ||
+ !strcmp(arg, "--request");
+}
+
+static bool parse_seq(const char *arg)
+{
+ return !strcmp(arg, "-s") ||
+ !strcmp(arg, "--sequence");
+}
+
+static const
+struct rpmb_cmd *parse_args(const char *prog, int *_argc, char **_argv[])
+{
+ int i;
+ int argc = *_argc;
+ char **argv = *_argv;
+ const struct rpmb_cmd *cmd = NULL;
+ bool need_help = false;
+
+ argc--; argv++;
+
+ if (argc == 0)
+ goto out;
+
+ if (call_for_help(argv[0])) {
+ argc--; argv++;
+ if (argc == 0)
+ goto out;
+
+ need_help = true;
+ }
+
+ if (parse_verbose(argv[0])) {
+ argc--; argv++;
+ if (argc == 0)
+ goto out;
+
+ verbose = true;
+ }
+
+ if (parse_req(argv[0])) {
+ argc--; argv++;
+ if (argc == 0)
+ goto out;
+
+ rpmb_ioctl = rpmb_ioctl_req;
+ }
+
+ if (parse_seq(argv[0])) {
+ argc--; argv++;
+ if (argc == 0)
+ goto out;
+
+ rpmb_ioctl = rpmb_ioctl_seq;
+ }
+
+ if (!rpmb_ioctl)
+ rpmb_ioctl = rpmb_ioctl_req;
+
+ for (i = 0; cmds[i].op_name; i++) {
+ if (!strncmp(argv[0], cmds[i].op_name,
+ strlen(cmds[i].op_name))) {
+ cmd = &cmds[i];
+ argc--; argv++;
+ break;
+ }
+ }
+
+ if (!cmd)
+ goto out;
+
+ if (need_help || (argc > 0 && call_for_help(argv[0]))) {
+ help(prog, cmd);
+ argc--; argv++;
+ return NULL;
+ }
+
+out:
+ *_argc = argc;
+ *_argv = argv;
+
+ if (!cmd)
+ usage(prog);
+
+ return cmd;
+}
+
+int main(int argc, char *argv[])
+{
+ const char *prog = basename(argv[0]);
+ const struct rpmb_cmd *cmd;
+ int ret;
+
+ cmd = parse_args(prog, &argc, &argv);
+ if (!cmd)
+ exit(EXIT_SUCCESS);
+
+ ret = cmd->op(argc, argv);
+ if (ret == -EINVAL)
+ help(prog, cmd);
+
+ if (ret)
+ exit(EXIT_FAILURE);
+
+ exit(EXIT_SUCCESS);
+}
--
2.7.4