[PATCH v2 6/8] tools rpmb: add RPBM access tool
From: Tomas Winkler
Date: Mon Apr 04 2016 - 07:13:20 EST
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@xxxxxxxxx>
---
V2: resend
MAINTAINERS | 1 +
tools/Makefile | 16 +-
tools/rpmb/.gitignore | 2 +
tools/rpmb/Makefile | 32 ++
tools/rpmb/rpmb.c | 807 ++++++++++++++++++++++++++++++++++++++++++++++++++
5 files changed, 853 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 07bd6f380460..b7966b95c265 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -9425,6 +9425,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@xxxxxx>
diff --git a/tools/Makefile b/tools/Makefile
index 60c7e6c8ff17..5b37ccd95cab 100644
--- a/tools/Makefile
+++ b/tools/Makefile
@@ -19,6 +19,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'
@@ -55,7 +56,8 @@ 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
+cgroup firewire hv guest rpmb spi usb virtio vm net iio: FORCE
$(call descend,$@)
liblockdep: FORCE
@@ -85,7 +87,7 @@ freefall: FORCE
$(call descend,laptop/$@)
all: acpi cgroup cpupower hv firewire lguest \
- perf selftests turbostat usb \
+ perf rpmb selftests turbostat usb \
virtio vm net x86_energy_perf_policy \
tmon freefall objtool
@@ -95,7 +97,8 @@ acpi_install:
cpupower_install:
$(call descend,power/$(@:_install=),install)
-cgroup_install firewire_install hv_install lguest_install perf_install usb_install virtio_install vm_install net_install objtool_install:
+cgroup_install firewire_install hv_install lguest_install perf_install rpmb_install usb_install virtio_install vm_install net_install objtool_install:
+cgroup_install firewire_install hv_install lguest_install perf_install rpmb_install usb_install virtio_install vm_install net_install:
$(call descend,$(@:_install=),install)
selftests_install:
@@ -111,7 +114,7 @@ freefall_install:
$(call descend,laptop/$(@:_install=),install)
install: acpi_install cgroup_install cpupower_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
@@ -139,6 +142,9 @@ libsubcmd_clean:
perf_clean:
$(call descend,$(@:_clean=),clean)
+rpmb_clean:
+ $(call descend,$(@:_clean=),clean)
+
selftests_clean:
$(call descend,testing/$(@:_clean=),clean)
@@ -155,7 +161,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..e0e51f306dac
--- /dev/null
+++ b/tools/rpmb/Makefile
@@ -0,0 +1,32 @@
+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 += -D__EXPORTED_HEADERS__
+CFLAGS += -I$(srctree)/include/uapi -I$(srctree)/include
+LDLIBS += $(shell $(PKG_CONFIG) --libs 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..e55f4d4e7ef7
--- /dev/null
+++ b/tools/rpmb/rpmb.c
@@ -0,0 +1,807 @@
+/******************************************************************************
+ * 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@xxxxxxxxxxxxxxx
+ * 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 <openssl/engine.h>
+#include <openssl/hmac.h>
+
+#include "linux/rpmb.h"
+
+#define RPMB_KEY_SIZE 32
+#define RPMB_MAC_SIZE 32
+#define RPMB_NONCE_SIZE 16
+
+#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";
+ }
+}
+
+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
+};
+
+static inline void __dump_buffer(const char *buf)
+{
+ fprintf(stderr, "%s\n", buf);
+}
+static void rpmb_dump_hex_buffer(const unsigned char *buf, size_t len)
+{
+ const size_t pbufsz = 16 * 3;
+ char pbuf[pbufsz];
+ int j = 0;
+
+ 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 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 (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 (ret != size) {
+ rpmb_err("data is %zd but must be %zu bytes length\n",
+ ret, size);
+ ret = -EINVAL;
+ }
+ return ret;
+}
+
+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;
+ 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_ioctl(int fd, uint16_t req,
+ const struct rpmb_frame *frames_in,
+ unsigned int in_cnt,
+ struct rpmb_frame *frames_out,
+ unsigned int out_cnt)
+{
+ int ret;
+ struct rpmb_ioc_cmd icmd;
+
+ icmd.req = req;
+ icmd.in_frames_count = in_cnt;
+ rpmb_ioc_cmd_set_in_frames(icmd, frames_in);
+ rpmb_ioc_cmd_set_out_frames(icmd, frames_out);
+ icmd.out_frames_count = out_cnt;
+
+ ret = ioctl(fd, RPMB_IOC_REQ, &icmd);
+ if (ret < 0)
+ rpmb_err("ioctl failure %d: %s\n", ret, strerror(errno));
+
+ 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, frame_out;
+
+ 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++;
+
+ memset(&frame_in, 0, sizeof(frame_in));
+ 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;
+
+ 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:
+ 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 req = RPMB_GET_WRITE_COUNTER;
+ unsigned char mac[RPMB_MAC_SIZE];
+ struct rpmb_frame frame_in;
+ struct rpmb_frame frame_out;
+
+ memset(&frame_in, 0, sizeof(frame_in));
+ 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;
+
+ ret = be16toh(frame_out.result);
+ if (ret)
+ goto out;
+
+ if (key) {
+ rpmb_calc_hmac_sha256(&frame_out, 1,
+ key, RPMB_KEY_SIZE,
+ mac, RPMB_MAC_SIZE);
+ if (memcmp(mac, frame_out.key_mac, RPMB_MAC_SIZE)) {
+ rpmb_err("RPMB hmac mismatch\n");
+ rpmb_dump_hex_buffer(frame_out.key_mac, RPMB_MAC_SIZE);
+ rpmb_dump_hex_buffer(mac, RPMB_MAC_SIZE);
+ ret = RPMB_ERR_AUTH;
+ 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(ret), ret);
+ }
+ 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 char mac[RPMB_MAC_SIZE];
+ unsigned long numarg;
+ bool has_key;
+ struct rpmb_frame frame_in;
+ 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;
+ memset(&frame_in, 0, sizeof(frame_in));
+ frame_in.req_resp = htobe16(req);
+ frame_in.addr = htobe16(addr);
+ frame_in.block_count = htobe16(blocks_cnt);
+ if (has_key)
+ RAND_bytes(frame_in.nonce, RPMB_NONCE_SIZE);
+
+ frames_out = calloc(blocks_cnt, sizeof(struct rpmb_frame));
+ if (!frames_out) {
+ rpmb_err("Cannot allocate %d RPMB frames\n", blocks_cnt);
+ ret = -ENOMEM;
+ goto out;
+ }
+
+ 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) {
+ rpmb_calc_hmac_sha256(frames_out, blocks_cnt,
+ key, RPMB_KEY_SIZE,
+ mac, RPMB_MAC_SIZE);
+ if (memcmp(mac, frame_out->key_mac, RPMB_MAC_SIZE)) {
+ rpmb_err("RPMB hmac mismatch\n");
+ rpmb_dump_hex_buffer(frame_out->key_mac, RPMB_MAC_SIZE);
+ rpmb_dump_hex_buffer(mac, RPMB_MAC_SIZE);
+ ret = RPMB_ERR_AUTH;
+ 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(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;
+ unsigned int cnt;
+ struct rpmb_frame *frames_in;
+ struct rpmb_frame frame_out;
+
+ 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 = calloc(blocks_cnt, sizeof(struct rpmb_frame));
+ if (!frames_in) {
+ rpmb_err("can't allocate memory for RPMB outer frames\n");
+ exit(EXIT_FAILURE);
+ }
+
+ ret = rpmb_get_write_counter(dev_fd, &cnt, 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(cnt);
+ }
+
+ 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);
+ }
+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> <address> <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 <address>.\n"
+ " When DATA_FILE is -, read from standard input.\n",
+ },
+ {
+ "read-blocks",
+ op_rpmb_read_blocks,
+ "<RPMB_DEVICE> <address> <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 <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 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 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;
+ else
+ need_help = true;
+ }
+
+ 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.4.3