xref: /OK3568_Linux_fs/u-boot/drivers/mmc/rpmb.c (revision 4882a59341e53eb6f0b4789bf948001014eff981)

/*
 * Copyright 2014, Staubli Faverges
 * Pierre Aubert
 *
 * eMMC- Replay Protected Memory Block
 * According to JEDEC Standard No. 84-A441
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include <config.h>
#include <common.h>
#include <memalign.h>
#include <mmc.h>
#include <u-boot/sha256.h>
#include "mmc_private.h"

/* Request codes */
#define RPMB_REQ_KEY		1
#define RPMB_REQ_WCOUNTER	2
#define RPMB_REQ_WRITE_DATA	3
#define RPMB_REQ_READ_DATA	4
#define RPMB_REQ_STATUS		5

/* Response code */
#define RPMB_RESP_KEY		0x0100
#define RPMB_RESP_WCOUNTER	0x0200
#define RPMB_RESP_WRITE_DATA	0x0300
#define RPMB_RESP_READ_DATA	0x0400

/* Error codes */
#define RPMB_OK			0
#define RPMB_ERR_GENERAL	1
#define RPMB_ERR_AUTH	2
#define RPMB_ERR_COUNTER	3
#define RPMB_ERR_ADDRESS	4
#define RPMB_ERR_WRITE		5
#define RPMB_ERR_READ		6
#define RPMB_ERR_KEY		7
#define RPMB_ERR_CNT_EXPIRED	0x80
#define RPMB_ERR_MSK		0x7

#define SHA256_BLOCK_SIZE	64

/* Error messages */
static const char * const rpmb_err_msg[] = {
	"",
	"General failure",
	"Authentication failure",
	"Counter failure",
	"Address failure",
	"Write failure",
	"Read failure",
	"Authentication key not yet programmed",
};

static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
			      bool is_rel_write)
{
	struct mmc_cmd cmd = {0};

	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
	cmd.cmdarg = blockcount & 0x0000FFFF;
	if (is_rel_write)
		cmd.cmdarg |= 1 << 31;
	cmd.resp_type = MMC_RSP_R1;

	return mmc_send_cmd(mmc, &cmd, NULL);
}
static int mmc_rpmb_request(struct mmc *mmc, const void *s,
			    unsigned int count, bool is_rel_write)
{
	struct mmc_cmd cmd = {0};
	struct mmc_data data;
	int ret;

	ret = mmc_set_blockcount(mmc, count, is_rel_write);
	if (ret) {
#ifdef CONFIG_MMC_RPMB_TRACE
		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
#endif
		return 1;
	}

	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
	cmd.cmdarg = 0;
	cmd.resp_type = MMC_RSP_R1;

	data.src = (const char *)s;
	data.blocks = count;
	data.blocksize = MMC_MAX_BLOCK_LEN;
	data.flags = MMC_DATA_WRITE;

	ret = mmc_send_cmd(mmc, &cmd, &data);
	if (ret) {
#ifdef CONFIG_MMC_RPMB_TRACE
		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
#endif
		return 1;
	}
	return 0;
}
static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
			     unsigned short expected, unsigned short cnt)
{
	struct mmc_cmd cmd = {0};
	struct mmc_data data;
	int ret;

	ret = mmc_set_blockcount(mmc, cnt, false);
	if (ret) {
#ifdef CONFIG_MMC_RPMB_TRACE
		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
#endif
		return -1;
	}
	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
	cmd.cmdarg = 0;
	cmd.resp_type = MMC_RSP_R1;

	data.dest = (char *)s;
	data.blocks = cnt;
	data.blocksize = MMC_MAX_BLOCK_LEN;
	data.flags = MMC_DATA_READ;

	ret = mmc_send_cmd(mmc, &cmd, &data);
	if (ret) {
#ifdef CONFIG_MMC_RPMB_TRACE
		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
#endif
		return -1;
	}
	/* Check the response and the status */
	if (be16_to_cpu(s->request) != expected) {
#ifdef CONFIG_MMC_RPMB_TRACE
		printf("%s:response= %x\n", __func__,
		       be16_to_cpu(s->request));
#endif
		return -1;
	}
	ret = be16_to_cpu(s->result);
	if (ret) {
		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
		       (ret & RPMB_ERR_CNT_EXPIRED) ?
		       "Write counter has expired" : "");
	}

	/* Return the status of the command */
	return ret;
}
static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
{
	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);

	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
	rpmb_frame->request = cpu_to_be16(RPMB_REQ_STATUS);
	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
		return -1;

	/* Read the result */
	return mmc_rpmb_response(mmc, rpmb_frame, expected, 1);
}
static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
		      unsigned char *output)
{
	sha256_context ctx;
	int i;
	unsigned char k_ipad[SHA256_BLOCK_SIZE];
	unsigned char k_opad[SHA256_BLOCK_SIZE];

	sha256_starts(&ctx);

	/* According to RFC 4634, the HMAC transform looks like:
	   SHA(K XOR opad, SHA(K XOR ipad, text))

	   where K is an n byte key.
	   ipad is the byte 0x36 repeated blocksize times
	   opad is the byte 0x5c repeated blocksize times
	   and text is the data being protected.
	*/

	for (i = 0; i < RPMB_SZ_MAC; i++) {
		k_ipad[i] = key[i] ^ 0x36;
		k_opad[i] = key[i] ^ 0x5c;
	}
	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
	for ( ; i < SHA256_BLOCK_SIZE; i++) {
		k_ipad[i] = 0x36;
		k_opad[i] = 0x5c;
	}
	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
	sha256_update(&ctx, buff, len);
	sha256_finish(&ctx, output);

	/* Init context for second pass */
	sha256_starts(&ctx);

	/* start with outer pad */
	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);

	/* then results of 1st hash */
	sha256_update(&ctx, output, RPMB_SZ_MAC);

	/* finish up 2nd pass */
	sha256_finish(&ctx, output);
}
int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
{
	int ret;
	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);

	/* Fill the request */
	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
	rpmb_frame->request = cpu_to_be16(RPMB_REQ_WCOUNTER);
	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
		return -1;

	/* Read the result */
	ret = mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_WCOUNTER, 1);
	if (ret)
		return ret;

	*pcounter = be32_to_cpu(rpmb_frame->write_counter);
	return 0;
}
int mmc_rpmb_set_key(struct mmc *mmc, void *key)
{
	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
	/* Fill the request */
	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
	rpmb_frame->request = cpu_to_be16(RPMB_REQ_KEY);
	memcpy(rpmb_frame->mac, key, RPMB_SZ_MAC);

	if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
		return -1;

	/* read the operation status */
	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
}
int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
		  unsigned short cnt, unsigned char *key)
{
	ALLOC_CACHE_ALIGN_BUFFER
		(char, rpmb_frame_data,
		sizeof(struct s_rpmb) * cnt);
	ALLOC_CACHE_ALIGN_BUFFER
		(char, rpmb_frame_data_verify,
		sizeof(struct s_rpmb_verify) * cnt);
	struct s_rpmb *rpmb_frame;
	struct s_rpmb_verify *rpmb_frame_vrify;
	int i;

	memset(rpmb_frame_data, 0, sizeof(struct s_rpmb) * cnt);
	memset(rpmb_frame_data_verify, 0, sizeof(struct s_rpmb_verify) * cnt);
	rpmb_frame = (struct s_rpmb *)rpmb_frame_data;
	rpmb_frame->address = cpu_to_be16(blk);
	rpmb_frame->request = cpu_to_be16(RPMB_REQ_READ_DATA);
	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false)) {
		printf("mmc_rpmb_read request error\n");
		return -1;
	}

	if (mmc_rpmb_response
			(mmc,
			(struct s_rpmb *)rpmb_frame_data,
			RPMB_RESP_READ_DATA, cnt)) {
		printf("mmc_rpmb_read response error\n");
		return -1;
	}

	for (i = 0; i < cnt; i++) {
		rpmb_frame = (struct s_rpmb *)
					(rpmb_frame_data +
					i * sizeof(struct s_rpmb));

		rpmb_frame_vrify = (struct s_rpmb_verify *)
					(rpmb_frame_data_verify +
					i * sizeof(struct s_rpmb_verify));
		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame->data, RPMB_SZ_DATA);
		memcpy(rpmb_frame_vrify->data, rpmb_frame->data, 284);
	}

	if (key) {
		unsigned char ret_hmac[RPMB_SZ_MAC];
		rpmb_hmac
			(key, (unsigned char *)rpmb_frame_data_verify,
			284 * cnt, ret_hmac);
		if (memcmp(ret_hmac, rpmb_frame->mac, RPMB_SZ_MAC)) {
			printf("MAC error on block #%d\n", i);
			return -1;
		}
	}

	return cnt;
}
int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
		  unsigned short cnt, unsigned char *key)
{
	struct s_rpmb *rpmb_frame;
	struct s_rpmb_verify *rpmb_frame_vrify;
	ALLOC_CACHE_ALIGN_BUFFER
		(char, rpmb_frame_data,
		sizeof(struct s_rpmb) * cnt);
	ALLOC_CACHE_ALIGN_BUFFER
		(char, rpmb_frame_data_verify,
		sizeof(struct s_rpmb_verify) * cnt);

	unsigned long wcount;
	unsigned short i;
	unsigned short temp;

	temp = cnt - 1;
	memset(rpmb_frame_data, 0, sizeof(struct s_rpmb) * cnt);
	memset(rpmb_frame_data_verify, 0, sizeof(struct s_rpmb_verify) * cnt);
	for (i = 0; i < cnt; i++) {
		if (i == 0) {
			if (mmc_rpmb_get_counter(mmc, &wcount)) {
				printf("Cannot read RPMB write counter\n");
				break;
			}
		}

		rpmb_frame = (struct s_rpmb *)
			(rpmb_frame_data +
			i * sizeof(struct s_rpmb));
		rpmb_frame_vrify = (struct s_rpmb_verify *)
			(rpmb_frame_data_verify +
			i * sizeof(struct s_rpmb_verify));
		memcpy(rpmb_frame->data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
		memcpy(rpmb_frame_vrify->data, addr +
			i * RPMB_SZ_DATA, RPMB_SZ_DATA);
		rpmb_frame->address = cpu_to_be16(blk);
		rpmb_frame_vrify->address = cpu_to_be16(blk);
		rpmb_frame->block_count = cpu_to_be16(cnt);
		rpmb_frame_vrify->block_count = cpu_to_be16(cnt);
		rpmb_frame->write_counter = cpu_to_be32(wcount);
		rpmb_frame_vrify->write_counter = cpu_to_be32(wcount);
		rpmb_frame->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
		rpmb_frame_vrify->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
		if (i == temp) {
			rpmb_hmac
				(key, (unsigned char *)rpmb_frame_data_verify,
				284 * cnt, rpmb_frame->mac);
		}
	}
	if (mmc_rpmb_request(mmc, rpmb_frame_data, cnt, true))
		return -1;

	if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
		return -1;
	return cnt;
}

int read_counter(struct mmc *mmc, struct s_rpmb *requestpackets)
{
	if (mmc_rpmb_request(mmc, requestpackets, 1, false))
		return -1;

	if (mmc_rpmb_response(mmc, requestpackets, RPMB_RESP_WCOUNTER, 1))
		return -1;

	return 0;
}

int program_key(struct mmc *mmc, struct s_rpmb *requestpackets)
{
	if (mmc_rpmb_request(mmc, requestpackets, 1, true))
		return -1;

	memset(requestpackets, 0, sizeof(struct s_rpmb));

	requestpackets->request = cpu_to_be16(RPMB_REQ_STATUS);

	if (mmc_rpmb_request(mmc, requestpackets, 1, false))
		return -1;

	return mmc_rpmb_response(mmc, requestpackets, RPMB_RESP_KEY, 1);
}

int authenticated_read(struct mmc *mmc,
	struct s_rpmb *requestpackets, uint16_t block_count)
{
	if (mmc_rpmb_request(mmc, requestpackets, 1, false))
		return -1;

	if (mmc_rpmb_response
		(mmc, requestpackets, RPMB_RESP_READ_DATA, block_count))
		return -1;

	return 0;
}

int authenticated_write(struct mmc *mmc, struct s_rpmb *requestpackets)
{
	if (mmc_rpmb_request(mmc, requestpackets, 1, true))
		return -1;

	memset(requestpackets, 0, sizeof(struct s_rpmb));

	requestpackets->request = cpu_to_be16(RPMB_REQ_STATUS);

	if (mmc_rpmb_request(mmc, requestpackets, 1, false))
		return -1;

	return mmc_rpmb_response(mmc, requestpackets, RPMB_RESP_WRITE_DATA, 1);
}