nvme-rpmb.c

/*
 * Copyright (C) 2020 Micron Techology Inc. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License version
 * 2 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.
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 * nvme-rpmb.c - Implementation of NVMe RPMB support commands in Nvme
 */
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <linux/if_alg.h>
#include <linux/socket.h>

#include "nvme.h"
#include "nvme-print.h"
#include "nvme-ioctl.h"

#define CREATE_CMD


#ifndef AF_ALG
#define AF_ALG 38
#endif
#ifndef SOL_ALG
#define SOL_ALG 279
#endif

#define HMAC_SHA256_ALGO_NAME		"hmac(sha256)"
#define MD5_HASH_ALGO_NAME		"md5"
#define HMAC_SHA256_HASH_SIZE		32
#define MD5_HASH_HASH_SIZE		16

extern int nvme_show_id_ctrl_rpmbs(unsigned int);
/*
 * Utility function to create hash value of given data (with given key) using
 * given hash algorithm; this function uses kernel crypto services
 */
unsigned char *create_hash(const char *algo,
			   int hash_size,
			   unsigned char *data,
			   int datalen,
			   unsigned char *key,
			   int keylen)
{
	int error, infd, outfd = -1;
	unsigned char *hash = NULL;
	struct sockaddr_alg provider_sa = {
		.salg_family = AF_ALG,
		.salg_type = "hash",
		.salg_name = { 0 }
	};

	/* copy algorith name */
	memcpy(provider_sa.salg_name, algo, strlen(algo));

    	/* open netlink socket connection to algorigm provider and bind */
    	infd = socket(AF_ALG, SOCK_SEQPACKET, 0);
	if (infd < 0) {
		perror("socket");
		return hash;
	}
    	error = bind(infd, (struct sockaddr *)&provider_sa, sizeof(provider_sa));
	if (error < 0) {
		perror("bind");
		goto out;
	}

	/* if algorithm requires key, set it first - empty keys not accepted !*/
	if (key != NULL && keylen > 0) {
        	error = setsockopt(infd, SOL_ALG, ALG_SET_KEY, key, keylen);
		if (error < 0) {
			perror("setsockopt");
			goto out;
		}
	}
		
    	/* now send data to hash */
    	outfd = accept(infd, NULL, 0);
	if (outfd < 0) {
		perror("accept");
		goto out;
	}
    	error = send(outfd, data, datalen, 0);
	if (error < 0) {
		perror("send");
		goto out;
	}

	/* read computed hash */
    	hash = (unsigned char *)calloc(hash_size, 1);
	if (hash == NULL) {
        	perror("calloc");
		goto out;
    	}

    	error = read(outfd, hash, hash_size);
	if (error != hash_size) {
        	perror("read");
        	free(hash);
        	hash = NULL;
    	}
out:
    if (outfd > 0) close(outfd);
    if (infd > 0)  close(infd);

    return hash;
}

/* Function that computes hmac-sha256 hash of given data and key pair. Returns
 * byte stream (non-null terminated) upon success, NULL otherwise.
 */
unsigned char *
hmac_sha256(unsigned char *data, int datalen, unsigned char *key, int keylen)
{
	return create_hash(HMAC_SHA256_ALGO_NAME,
			   HMAC_SHA256_HASH_SIZE,
			   data,
			   datalen,
			   key,
			   keylen);
}

/* Function that computes md5 of given buffer - md5 hash is used as nonce
 * Returns byte stream (non-null terminated) upon success, NULL otherwise.
 */
unsigned char *
rpmb_md5(unsigned char *data, int datalen)
{
	return create_hash(MD5_HASH_ALGO_NAME,
			   MD5_HASH_HASH_SIZE,
			   data,
			   datalen,
			   NULL,
			   0);
}

/* Read data from given file into buffer and return its length */
static int read_file(const char *file, unsigned char **data, unsigned int *len)
{
	struct stat sb;
	size_t size;
	unsigned char   *buf = NULL;
	int fd;
	int err = -EINVAL;

	if (file == NULL) return err;

	if ((fd = open(file, O_RDONLY)) < 0) {
		fprintf(stderr, "Failed to open %s: %s\n", file, strerror(errno));
		return fd;
	}

	err = fstat(fd, &sb);
	if (err < 0) {
		perror("fstat");
		goto out;
	}

	size = sb.st_size;
	if (posix_memalign((void **)&buf, getpagesize(), size)) {
		fprintf(stderr, "No memory for reading file :%s\n", file);
		err = -ENOMEM;
		goto out;
	}

	err = read(fd, buf, size);
	if (err < 0) {
		err = -errno;
		fprintf(stderr, "Failed to read data from file"
				" %s with %s\n", file, strerror(errno));
		free(buf);
		goto out;
	}
	err -= size;
	*data = buf; 
	*len = size;
out:
	close(fd);
	return err;
}

/* Write given buffer data to specified file */
static void write_file(unsigned char *data, size_t len, const char *dir,
			  const char *file, const char *msg)
{
	char temp_folder[PATH_MAX] = { 0 };
	FILE *fp = NULL;

	if (dir != NULL)
		sprintf(temp_folder, "%s/%s", dir, file);
	else
		sprintf(temp_folder, "./%s", file);

	if ((fp = fopen(temp_folder, "ab+")) != NULL) {
		if (fwrite(data, 1, len,  fp) != len) {
			fprintf(stderr, "Failed to write %s data to %s\n",
				 msg ? msg : "", temp_folder);
		}
		fclose(fp);
	} else  {
		fprintf(stderr, "Failed to open %s file to write %s\n",
			temp_folder, msg ? msg : "");
	}
}

/* Various definitions used in RPMB related support */
enum rpmb_request_type {
	RPMB_REQ_AUTH_KEY_PROGRAM = 0x0001,
	RPMB_REQ_READ_WRITE_CNTR  = 0x0002,
	RPMB_REQ_AUTH_DATA_WRITE  = 0x0003,
	RPMB_REQ_AUTH_DATA_READ   = 0x0004,
	RPMB_REQ_READ_RESULT      = 0x0005,
	RPMB_REQ_AUTH_DCB_WRITE   = 0x0006,
	RPMB_REQ_AUTH_DCB_READ    = 0x0007
};
	
enum rpmb_response_type {
	RPMB_RSP_AUTH_KEY_PROGRAM = (RPMB_REQ_AUTH_KEY_PROGRAM << 8),
	RPMB_RSP_READ_WRITE_CNTR  = (RPMB_REQ_READ_WRITE_CNTR  << 8),
	RPMB_RSP_AUTH_DATA_WRITE  = (RPMB_REQ_AUTH_DATA_WRITE  << 8),
	RPMB_RSP_AUTH_DATA_READ   = (RPMB_REQ_AUTH_DATA_READ   << 8),
	RPMB_RSP_READ_RESULT      = (RPMB_REQ_READ_RESULT      << 8),
	RPMB_RSP_AUTH_DCB_WRITE   = (RPMB_REQ_AUTH_DCB_WRITE   << 8),
	RPMB_RSP_AUTH_DCB_READ    = (RPMB_REQ_AUTH_DCB_READ    << 8)
};

/* RPMB data frame structure */
#pragma pack(1)
struct rpmb_data_frame_t {
	unsigned char  pad[191];
	unsigned char  mac[32];
	unsigned char  target;     /* 0-6, should match with NSSF with SS, SR */
	unsigned char  nonce[16];
	unsigned int   write_counter;
	unsigned int   address;
	unsigned int   sectors;
	unsigned short result;
	unsigned short type;       /* req or response */
	unsigned char  data[0];    /* in sector count times */
};
#pragma pack()
	
struct rpmb_config_block_t {
	unsigned char  bp_enable;
	unsigned char  bp_lock;
	unsigned char  rsvd[510]; 
};

#define RPMB_DATA_FRAME_SIZE  256
#define RPMB_NVME_SECP        0xEA 
#define RPMB_NVME_SPSP        0x0001

#define SEND_RPMB_REQ(tgt, size, req) \
nvme_sec_send(fd, 0, tgt, RPMB_NVME_SPSP, RPMB_NVME_SECP, size, size, \
		(unsigned char *)(req))
	
#define RECV_RPMB_RSP(tgt, size, rsp) \
nvme_sec_recv(fd, 0, tgt, RPMB_NVME_SPSP, RPMB_NVME_SECP, size, size, \
		(unsigned char *)(rsp))
	
/* Initialize nonce value in rpmb request frame */
static void rpmb_nonce_init(struct rpmb_data_frame_t *req)
{
	int num = rand();
	unsigned char *hash = rpmb_md5((unsigned char *)&num, sizeof(num));
	if (hash) memcpy(req->nonce, hash, sizeof(req->nonce));
}

/* Read key from a given key buffer or key file */
static unsigned char *read_rpmb_key(char *keystr, char *keyfile, unsigned int *keysize)
{
	unsigned char *keybuf = NULL;
	
	if (keystr == NULL) {
		if (keyfile != NULL)
			read_file(keyfile, &keybuf, keysize);
	} else if ((keybuf = (unsigned char *)malloc(strlen(keystr))) != NULL) {
		*keysize = strlen(keystr);
		memcpy(keybuf, keystr, *keysize);
	}

	return keybuf;
}

/* Initialize RPMB request frame with given values */
static struct rpmb_data_frame_t *
rpmb_request_init(unsigned int   req_size,
		  unsigned short type,
		  unsigned char  target,
		  unsigned char  nonce,
		  unsigned int   addr,
		  unsigned int   sectors,
		  unsigned char  *data,
		  unsigned short data_offset,
		  unsigned int   data_size)
{
	struct rpmb_data_frame_t *req = NULL;

	if ((req = (struct rpmb_data_frame_t *)calloc(req_size, 1)) == NULL) {
		fprintf(stderr, "Memory allocation failed for request 0x%04x\n",
			type);
		return req;
	}

	req->type = type;
	req->target = target;
	req->address = addr;
	req->sectors = sectors;
	
	if (nonce) rpmb_nonce_init(req);
	if (data)  memcpy((unsigned char *)req + data_offset, data, data_size);

	return req;
}

/* Process rpmb response and print appropriate error message */
static int check_rpmb_response( struct rpmb_data_frame_t *req,
				struct rpmb_data_frame_t *rsp,
				char *msg)
{
	const char *rpmb_result_string [] = {
		"Operation successful", 
		"General failure",
		"Authentication (MAC) failure",
		"Counter failure (not matching/incrementing failure)",
		"Address failure (out of range or wrong alignment)",
		"Write (data/counter/result) failure",
		"Read (data/counter/result) failure",
		"Authentication key not yet programmed",
		"Invalid device configuration block",
		"Unknown error"
	};
	 
	/* check error status before comparing nonce and mac */
	if (rsp->result != 0)  {
		if (rsp->type != ((req->type << 8) & 0xFF00)) {
			fprintf(stderr, "%s ! non-matching response 0x%04x for"
				" 0x%04x\n", msg, rsp->type, req->type);
		} else if ((rsp->result & 0x80) == 0x80) {
			fprintf(stderr, "%s ! Expired write-counter !\n", msg);
		} else if (rsp->result) {
			fprintf(stderr, "%s ! %s\n", msg,
				rpmb_result_string[rsp->result & 0x7F]);
		} else if (memcmp(req->nonce, rsp->nonce, 16)) {
			fprintf(stderr, "%s ! non-matching nonce\n", msg);
		} else if (memcmp(req->mac, rsp->mac, 32)) {
			fprintf(stderr, "%s ! non-matching MAC\n", msg);
		} else if ((req->write_counter + 1) != rsp->write_counter) {
			fprintf(stderr, "%s ! out-of-sync write-counters\n", msg);
		}
	}
	
	return (int)(rsp->result);
}

/* send an initialized rpmb request to the controller and read its response
 * expected response size give in 'rsp_size'. returns response buffer upon
 * successful completion (caller must free), NULL otherwise
 */
static struct rpmb_data_frame_t *
rpmb_read_request(int fd, 
		  struct rpmb_data_frame_t *req,
	          int req_size,
	          int rsp_size)
{
	struct rpmb_data_frame_t *rsp = NULL;
	unsigned char msg[1024] = { 0 };
	int error;

	sprintf((char *)msg, "RPMB request 0x%04x to target 0x%x",
		req->type, req->target);

	error = SEND_RPMB_REQ(req->target, req_size, req);
	if (error != 0) {
		fprintf(stderr, "%s failed with error = 0x%x\n",
			msg, error);
		goto error_out;
	}

	/* read the result back */
	rsp = (struct rpmb_data_frame_t *)calloc(rsp_size, 1);
	if (rsp == NULL) {
		fprintf(stderr, "memory alloc failed for %s\n", msg);
		goto error_out;
	}

	/* Read result of previous request */
	error = RECV_RPMB_RSP(req->target, rsp_size, rsp);
	if (error) {
		fprintf(stderr, "error 0x%x receiving response for %s\n",
			error, msg);
		goto error_out;
	}

	/* validate response buffer - match target, nonce, and mac */
	error = check_rpmb_response(req, rsp, (char *)msg);
	if (error == 0) return rsp;

error_out:
	free(rsp);
	return NULL;
}

/* read current write counter value from controller */
static int rpmb_read_write_counter(int fd,
				   unsigned char target,
				   unsigned int *counter)
{
	int error = -1;
	int req_size = sizeof(struct rpmb_data_frame_t);
	struct rpmb_data_frame_t *req = NULL;
	struct rpmb_data_frame_t *rsp = NULL;

	req = rpmb_request_init(req_size, RPMB_REQ_READ_WRITE_CNTR,
				target, 1, 0, 0, NULL, 0, 0);
	if (req == NULL) goto out;
	if ((rsp = rpmb_read_request(fd, req, req_size, req_size)) == NULL) {
		goto out;
	}	
	*counter = rsp->write_counter; 
	error = 0;
	
out:
	free(req);
	free(rsp);
	return error;
}

/* Read current device configuration block into specified buffer. It also returns
 * current write counter value returned as part of response, in case of error it
 * returns 0
 */
static unsigned int rpmb_read_config_block(int fd, unsigned char **config_buf)
{
	int req_size = sizeof(struct rpmb_data_frame_t);
	int cfg_size = sizeof(struct rpmb_config_block_t);
	int rsp_size = req_size + cfg_size;
	
	struct rpmb_data_frame_t   *req = NULL;
	struct rpmb_data_frame_t   *rsp = NULL;
	struct rpmb_config_block_t *cfg = NULL;
	unsigned int retval = 0;

	/* initialize request with nonce, no data on input */
	req = rpmb_request_init(req_size, RPMB_REQ_AUTH_DCB_READ, 0, 1, 0, 1,
				0, 0, 0);
	if ((req == NULL) ||
	    (rsp = rpmb_read_request(fd, req, req_size, rsp_size)) == NULL)
	{
		goto out;
	}	

	/* copy configuration data to be sent back to caller */
	cfg = (struct rpmb_config_block_t *)calloc(cfg_size, 1);
	if (cfg == NULL) {
		fprintf(stderr, "failed to allocate RPMB config buffer\n");
		goto out;
	}

	memcpy(cfg, rsp->data, cfg_size);
	*config_buf = (unsigned char *)cfg;
	cfg = NULL;
	retval = rsp->write_counter;
out:
	free(req);
	free(rsp);
	free(cfg);
	return retval;
}


static int rpmb_auth_data_read(int fd, unsigned char target,
			       unsigned int offset,
			       unsigned char **msg_buf,
			       int msg_size, int acc_size)
{
	struct rpmb_data_frame_t *req = NULL;
	struct rpmb_data_frame_t *rsp = NULL;
	int req_size = sizeof(struct rpmb_data_frame_t);
	int chunk_size = (acc_size < msg_size) ? acc_size : msg_size;
	int xfer = chunk_size;
	unsigned char *bufp = (unsigned char *)malloc(msg_size * 512);
	unsigned char *tbufp = bufp;
	int data_size, rsp_size;
	int error = -1;

	if (bufp == NULL) {
		fprintf(stderr, "Failed to allocated memory for read-data req\n");
		goto out;
	}
	
	while (xfer > 0) {
		rsp_size = req_size + xfer * 512;
		req = rpmb_request_init(req_size, RPMB_REQ_AUTH_DATA_READ,
					target, 1, offset, xfer, 0, 0, 0);
		if (req == NULL) break;
		if ((rsp = rpmb_read_request(fd, req, req_size, rsp_size)) == NULL)
		{
			fprintf(stderr, "read_request failed\n");
			free(rsp);
			goto out;
		}

		data_size = rsp->sectors * 512;
		memcpy(tbufp, rsp->data, data_size);
		offset += rsp->sectors;
		tbufp += data_size;
		if (offset + chunk_size > msg_size)
			xfer = msg_size - offset;
		else 
			xfer = chunk_size;
		free(req);
		free(rsp);
	}
	
	*msg_buf = bufp;
	error = offset;
out:
	return error;
}

/* Implementation of programming authentication key to given RPMB target */
static int rpmb_program_auth_key(int fd, unsigned char target,
				 unsigned char *key_buf, int key_size)
{
	int req_size = sizeof(struct rpmb_data_frame_t);
	int rsp_size = sizeof(struct rpmb_data_frame_t);
	
	struct rpmb_data_frame_t *req = NULL;
	struct rpmb_data_frame_t *rsp = NULL;
	
	int err = -ENOMEM;
	
	req = rpmb_request_init(req_size, RPMB_REQ_AUTH_KEY_PROGRAM, target,
				0, 0, 0, key_buf, (223 - key_size), key_size);
	if (req == NULL) {
		fprintf(stderr, "failed to allocate request buffer memory\n");
		goto out;
	}

	/* send request and read the result first */
	rsp = rpmb_read_request(fd, req, req_size, rsp_size);
	if (rsp == NULL || rsp->result != 0) {
		goto out;
	}

	/* re-use response buffer */
	memset(rsp, 0, rsp_size);
	err = RECV_RPMB_RSP(req->target, rsp_size, (unsigned char *)rsp);
	if (err != 0) {
		err = check_rpmb_response(req, rsp, "Failed to Program Key");
	}
out:
	free(req);
	free(rsp);
	
	return err;
}


/* Implementation of RPMB authenticated data write command; this function
 * transfers msg_size bytes from msg_buf to controller 'addr'. Returns
 * number of bytes actually written to, otherwise negetive error code
 * on failures.
 */
static int auth_data_write_chunk(int fd, unsigned char tgt, unsigned int addr,
				 unsigned char *msg_buf, int msg_size,
				 unsigned char *keybuf, int keysize)
{
	int req_size = sizeof(struct rpmb_data_frame_t) + msg_size;
	int rsp_size = sizeof(struct rpmb_data_frame_t);
	
	struct rpmb_data_frame_t *req = NULL;
	struct rpmb_data_frame_t *rsp = NULL;
	
	unsigned int write_cntr = 0;
	unsigned char *mac = NULL;
	int error  = -ENOMEM;

	/* get current write counter and copy to the request  */
	error = rpmb_read_write_counter(fd, tgt, &write_cntr);
	if (error != 0) {
	   fprintf(stderr, "Failed to read write counter for write-data\n");
	    goto out;
	}
	
	req = rpmb_request_init(req_size, RPMB_REQ_AUTH_DATA_WRITE, tgt, 0,
				addr, (msg_size / 512), msg_buf,
				offsetof(struct rpmb_data_frame_t, data), msg_size);
	if (req == NULL) {
		fprintf(stderr, "Memory alloc failed for write-data command\n");
		goto out;
	}

	req->write_counter = write_cntr;

	/* compute HMAC hash */
	mac = hmac_sha256(((unsigned char *)req + 223), req_size - 223,
			   keybuf, keysize);
	if (mac == NULL) {
		fprintf(stderr, "failed to compute HMAC-SHA256\n");
		error = -1;
		goto out;
	}

	memcpy(req->mac, mac, 32);
	
	/* send the request and get response */
	error = SEND_RPMB_REQ(tgt, req_size, (unsigned char *)req);
	if (error != 0) {
	    fprintf(stderr, "RPMB request 0x%04x for 0x%x, error: %d\n",
		    req->type, tgt, error);
	    goto out;
	}
	
	/* send the request to get the result and then request to get the response */
        rsp = (struct rpmb_data_frame_t *)calloc(rsp_size, 1);
	rsp->target = req->target;
	rsp->type = RPMB_REQ_READ_RESULT;
	error = SEND_RPMB_REQ(tgt, rsp_size, (unsigned char *)rsp);
	if (error != 0 || rsp->result != 0) {
		fprintf(stderr, "Write-data read result 0x%x, error = 0x%x\n",
			rsp->result, error);
		goto out;
	}

	/* Read final response */
	memset(rsp, 0, rsp_size);
	error = RECV_RPMB_RSP(tgt, rsp_size, (unsigned char *)rsp);
	if (error != 0)
		fprintf(stderr, "Auth data write recv error = 0x%x\n", error);
	else 
    		error = check_rpmb_response(req, rsp, "Failed to write-data");
out:
	free(req);
	free(rsp);
	free(mac);

	return error;
}

/* send the request and get response */
static int rpmb_auth_data_write(int fd, unsigned char target,
				unsigned int addr, int acc_size,
				unsigned char *msg_buf, int msg_size,
				unsigned char *keybuf, int keysize)
{
	int chunk_size = acc_size < msg_size ? acc_size : msg_size;
	int xfer   = chunk_size;
	int offset = 0;

	while (xfer > 0 ) {
		if (auth_data_write_chunk(fd, target, (addr + offset / 512),
				          msg_buf + offset, xfer,
				          keybuf, keysize) != 0)
		{
			/* error writing chunk data */
			break;	
		}

		offset += xfer;
		if (offset + chunk_size > msg_size)
			xfer = msg_size - offset;
		else 
			xfer = chunk_size;
	}

	return offset;
}

/* writes given config_block buffer to the drive target 0 */
static int rpmb_write_config_block(int fd, unsigned char *cfg_buf,
				   unsigned char *keybuf, int keysize)
{
	int cfg_size = sizeof(struct rpmb_config_block_t);
	int rsp_size = sizeof(struct rpmb_data_frame_t);
	int req_size = rsp_size + cfg_size;
	
	struct rpmb_data_frame_t *req = NULL;
	struct rpmb_data_frame_t *rsp = NULL;
	unsigned char *cfg_buf_read = NULL, *mac = NULL;
	unsigned int write_cntr = 0;
	int   error = -ENOMEM;
	
	/* initialize request */
	req = rpmb_request_init(req_size, RPMB_REQ_AUTH_DCB_WRITE, 0, 0, 0, 1,
				cfg_buf, offsetof(struct rpmb_data_frame_t, data),
				cfg_size);
	if (req == NULL) {
		fprintf(stderr, "failed to allocate rpmb request buffer\n");
		goto out; 
	}

	/* read config block write_counter from controller */
	write_cntr = rpmb_read_config_block(fd, &cfg_buf_read);
	if (cfg_buf_read == NULL) {
	    	fprintf(stderr, "failed to read config block write counter\n");
		error = -EIO;
	    	goto out;
	}

	free(cfg_buf_read);
	req->write_counter = write_cntr;
	mac = hmac_sha256(((unsigned char *)req + 223), req_size - 223,
			   keybuf, keysize);
	if (mac == NULL) {
		fprintf(stderr, "failed to compute hmac-sha256 hash\n");
		error = -EINVAL;
	    	goto out;
	}
	
	memcpy(req->mac, mac, sizeof(req->mac)); 
	
	error = SEND_RPMB_REQ(0, req_size, (unsigned char *)req);
	if (error != 0) {
		fprintf(stderr, "Write-config RPMB request, error = 0x%x\n",
			error);
		goto out;
	}
	
	/* get response */
	rsp = (struct rpmb_data_frame_t *)calloc(rsp_size, 1);
	if (rsp == NULL) {
		fprintf(stderr, "failed to allocate response buffer memory\n");
		error = -ENOMEM;
		goto out;
	}

	/* get result first */
	memset(rsp, 0, rsp_size);
	rsp->target = req->target;
	rsp->type = RPMB_REQ_READ_RESULT;
	/* get the response and validate */
	error = RECV_RPMB_RSP(req->target, rsp_size, rsp);
	if (error != 0) {
		fprintf(stderr,"Failed getting write-config response\
			error = 0x%x\n", error);
		goto out;
	}
	error = check_rpmb_response(req, rsp,
				  "Failed to retrieve write-config response");
out:
	free(req);
	free(rsp);
	free(mac);
	
	return error;
}

static bool invalid_xfer_size(int blocks, unsigned int bpsz)
{
	return ((blocks <= 0) || 
		(blocks * 512) > ((bpsz + 1) * 128 * 1024));
}

/* Handling rpmb sub-command */
int rpmb_cmd_option(int argc, char **argv, struct command *cmd, struct plugin *plugin)
{
	const char *desc    = "Run RPMB command on the supporting controller";
	const char *msg     = "data to be written on write-data or write-config commands";
	const char *mfile   = "data file for read/write-data, read/write-config options";
	const char *kfile   = "key file that has authentication key to be used";
	const char *target  = "RPMB target - numerical value of 0 to 6, default 0";
	const char *address = "Sector offset to read from or write to for an RPMB target, default 0";
	const char *blocks  = "Number of 512 blocks to read or write";
	const char *key     = "key to be used for authentication";
	const char *opt     = "RPMB action - info, program-key, read-counter, write-data, " \
			      "read-data, write-config and read-config";
	
	struct config {
		char *cmd;
		char *key;
		char *msg;
		char *keyfile;
		char *msgfile;
		int  opt;
		int  address;
		int  blocks; 
		char target;
	};
	
	struct config cfg = {
		.cmd     = "info",
		.key     = NULL,
		.msg     = NULL,
		.msgfile = NULL,
		.keyfile = NULL,
		.opt     = 0,
		.address = 0,
		.blocks  = 0,
		.target  = 0,
	};
	
	OPT_ARGS(opts) = {
		OPT_STRING("cmd",     'c', "command", &cfg.cmd,     opt),
		OPT_STRING("msgfile", 'f', "FILE",    &cfg.msgfile, mfile),
		OPT_STRING("keyfile", 'g', "FILE",    &cfg.keyfile, kfile),
		OPT_STRING("key",     'k', "key",     &cfg.key,     key),
		OPT_STRING("msg",     'd', "data",    &cfg.msg,     msg),
		OPT_UINT("address",   'o', &cfg.address,  address),
		OPT_UINT("blocks",    'b', &cfg.blocks,   blocks),
		OPT_UINT("target",    't', &cfg.target,   target),
		OPT_END()
	};
	
	unsigned int write_cntr = 0;
	unsigned char *key_buf = NULL;
	unsigned char *msg_buf = NULL;
	unsigned int msg_size = 0;
	unsigned int key_size = 0;
	int fd = -1, err = -1;
	struct nvme_id_ctrl ctrl;

	union ctrl_rpmbs_reg {
		struct {
			unsigned int num_targets:3;
			unsigned int auth_method:3;
			unsigned int reserved:10;
			unsigned int total_size:8;   /* 128K units */
			unsigned int access_size:8;  /* in 512 byte count */
		};
		unsigned int rpmbs;
	} regs;
	
	if ((fd = parse_and_open(argc, argv, desc, opts)) < 0)
		goto out;
	
	/* before parsing  commands, check if controller supports any RPMB targets */
	err = nvme_identify_ctrl(fd, &ctrl);
	if (err)
		goto out;
	
	regs.rpmbs = le32_to_cpu(ctrl.rpmbs);
	if (regs.num_targets == 0) {
		fprintf(stderr, "No RPMB targets are supported by the drive\n");
		goto out;
	}
	
	/* parse and validate options; default print rpmb support info */
	if (cfg.cmd == 0 || strcmp(cfg.cmd, "info") == 0) {
		nvme_show_id_ctrl_rpmbs(regs.rpmbs);
		goto out;
	}
	
	if (strcmp(cfg.cmd, "program-key") == 0)
		cfg.opt = RPMB_REQ_AUTH_KEY_PROGRAM;
	else if (strcmp(cfg.cmd, "read-counter") == 0)
		cfg.opt = RPMB_REQ_READ_WRITE_CNTR;
	else if (strcmp(cfg.cmd, "write-data") == 0)
		cfg.opt = RPMB_REQ_AUTH_DATA_WRITE;
	else if (strcmp(cfg.cmd, "read-data") == 0)
		cfg.opt = RPMB_REQ_AUTH_DATA_READ;
	else if (strcmp(cfg.cmd, "write-config") == 0)
		cfg.opt = RPMB_REQ_AUTH_DCB_WRITE;
	else if (strcmp(cfg.cmd, "read-config") == 0)
		cfg.opt = RPMB_REQ_AUTH_DCB_READ;
	else {
		fprintf(stderr, "Invalid option %s for rpmb command\n", cfg.cmd);
		goto out;
	}
	
	/* input file/data processing */
	if (cfg.opt == RPMB_REQ_AUTH_DCB_WRITE || 
	    cfg.opt == RPMB_REQ_AUTH_DATA_WRITE ||
	    cfg.opt == RPMB_REQ_AUTH_KEY_PROGRAM)
	{
		key_buf = read_rpmb_key(cfg.key, cfg.keyfile, &key_size);
		if (key_buf == NULL) {
			fprintf(stderr, "Failed to read key\n");
			goto out;
		}
	
		if (key_size > 223 || key_size <= 0) {
			fprintf(stderr, "Invalid key size %d, valid input 1 to 223\n",
			key_size);
			goto out;
		}

		if (cfg.opt == RPMB_REQ_AUTH_DCB_WRITE ||
		    cfg.opt == RPMB_REQ_AUTH_DATA_WRITE) {
			if (cfg.msg != NULL) {
				msg_size = strlen(cfg.msg);
				msg_buf = (unsigned char *)malloc(msg_size);
				memcpy(msg_buf, cfg.msg, msg_size);
			} else {
				err = read_file(cfg.msgfile, &msg_buf, &msg_size);
				if (err || msg_size <= 0) {
					fprintf(stderr, "Failed to read file %s\n",
						cfg.msgfile);
					goto out;
				}
			}
		}
	}
	
	switch (cfg.opt) {
		case RPMB_REQ_READ_WRITE_CNTR:
			err = rpmb_read_write_counter(fd, cfg.target, &write_cntr);
			if (err == 0)
				printf("Write Counter is: %u\n", write_cntr);
			break;
	
		case RPMB_REQ_AUTH_DCB_READ:
			write_cntr = rpmb_read_config_block(fd, &msg_buf);
			if (msg_buf == NULL) {
				fprintf(stderr, "failed read config blk\n");
				goto out;
			}

			/* no output file is given, print the data on stdout */
			if (cfg.msgfile == 0) {
				struct rpmb_config_block_t *cfg =
						(struct rpmb_config_block_t *)msg_buf;
				printf("Boot Parition Protection is %s\n",
					((cfg->bp_enable & 0x1)  ? "Enabled" : "Disabled"));
				printf("Boot Parition 1 is %s\n",
					((cfg->bp_lock & 0x2) ? "Locked" : "Unlocked"));
				printf("Boot Parition 0 is %s\n",
					((cfg->bp_lock & 0x1) ? "Locked" : "Unlocked"));
			} else {
				printf("Saving received config data to %s file\n", cfg.msgfile);
				write_file(msg_buf, sizeof(struct rpmb_config_block_t), NULL,
					   cfg.msgfile, NULL);
			}
			err = (write_cntr == 0);
			break;
	
		case RPMB_REQ_AUTH_DATA_READ:
			/* check if requested data is beyond what target supports */
			msg_size = cfg.blocks * 512;
			if (invalid_xfer_size(cfg.blocks, regs.total_size)) {
				fprintf(stderr, "invalid transfer size %d \n",
					msg_size);
				break;
			}
			err = rpmb_auth_data_read(fd, cfg.target, cfg.address,
						  &msg_buf, cfg.blocks,
						  (regs.access_size + 1));
			if (err > 0 && msg_buf != NULL) {
				printf("Writting %d bytes to file %s\n",
					err * 512, cfg.msgfile);
				write_file(msg_buf, err * 512, NULL,
					   cfg.msgfile, NULL);
			}
			break;
	
		case RPMB_REQ_AUTH_DATA_WRITE:
			if (invalid_xfer_size(cfg.blocks, regs.total_size) || 
			    (cfg.blocks * 512) > msg_size) {
				fprintf(stderr, "invalid transfer size %d\n", 
					cfg.blocks * 512);
				break;
			} else if ((cfg.blocks * 512) < msg_size) {
				msg_size = cfg.blocks * 512;
			}
			err = rpmb_auth_data_write(fd, cfg.target, cfg.address,
						  ((regs.access_size + 1) * 512),
						   msg_buf, msg_size,
						   key_buf, key_size);

			/* print whatever extent of data written to target */
			printf("Written %d sectors out of %d @target(%d):0x%x\n",
				err/512, msg_size/512, cfg.target, cfg.address);
			break;

		case RPMB_REQ_AUTH_DCB_WRITE:
			err = rpmb_write_config_block(fd, msg_buf, key_buf, key_size);
			break;
	
		case RPMB_REQ_AUTH_KEY_PROGRAM:
			err = rpmb_program_auth_key(fd, cfg.target, key_buf, key_size);
			break;
		default:
			break;
	}
	 
out:
	/* release memory  */
	free(key_buf);
	free(msg_buf);
	
	/* close file descriptor */
	if (fd > 0) close(fd);
	
	return err;
}