1// SPDX-License-Identifier: GPL-2.0+
2/*
3 * Copyright 2014, Staubli Faverges
4 * Pierre Aubert
5 *
6 * eMMC- Replay Protected Memory Block
7 * According to JEDEC Standard No. 84-A441
8 */
9
10#include <config.h>
11#include <log.h>
12#include <memalign.h>
13#include <mmc.h>
14#include <sdhci.h>
15#include <u-boot/sha256.h>
16#include "mmc_private.h"
17
18/* Request codes */
19#define RPMB_REQ_KEY		1
20#define RPMB_REQ_WCOUNTER	2
21#define RPMB_REQ_WRITE_DATA	3
22#define RPMB_REQ_READ_DATA	4
23#define RPMB_REQ_STATUS		5
24
25/* Response code */
26#define RPMB_RESP_KEY		0x0100
27#define RPMB_RESP_WCOUNTER	0x0200
28#define RPMB_RESP_WRITE_DATA	0x0300
29#define RPMB_RESP_READ_DATA	0x0400
30
31/* Error codes */
32#define RPMB_OK			0
33#define RPMB_ERR_GENERAL	1
34#define RPMB_ERR_AUTH	2
35#define RPMB_ERR_COUNTER	3
36#define RPMB_ERR_ADDRESS	4
37#define RPMB_ERR_WRITE		5
38#define RPMB_ERR_READ		6
39#define RPMB_ERR_KEY		7
40#define RPMB_ERR_CNT_EXPIRED	0x80
41#define RPMB_ERR_MSK		0x7
42
43/* Sizes of RPMB data frame */
44#define RPMB_SZ_STUFF		196
45#define RPMB_SZ_MAC		32
46#define RPMB_SZ_DATA		256
47#define RPMB_SZ_NONCE		16
48
49#define SHA256_BLOCK_SIZE	64
50
51/* Error messages */
52static const char * const rpmb_err_msg[] = {
53	"",
54	"General failure",
55	"Authentication failure",
56	"Counter failure",
57	"Address failure",
58	"Write failure",
59	"Read failure",
60	"Authentication key not yet programmed",
61};
62
63
64/* Structure of RPMB data frame. */
65struct s_rpmb {
66	unsigned char stuff[RPMB_SZ_STUFF];
67	unsigned char mac[RPMB_SZ_MAC];
68	unsigned char data[RPMB_SZ_DATA];
69	unsigned char nonce[RPMB_SZ_NONCE];
70	unsigned int write_counter;
71	unsigned short address;
72	unsigned short block_count;
73	unsigned short result;
74	unsigned short request;
75};
76
77static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
78			      bool is_rel_write)
79{
80	struct mmc_cmd cmd = {0};
81
82	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
83	cmd.cmdarg = blockcount & 0x0000FFFF;
84	if (is_rel_write)
85		cmd.cmdarg |= 1 << 31;
86	cmd.resp_type = MMC_RSP_R1;
87
88	return mmc_send_cmd(mmc, &cmd, NULL);
89}
90static int mmc_rpmb_request(struct mmc *mmc, const struct s_rpmb *s,
91			    unsigned int count, bool is_rel_write)
92{
93	struct mmc_cmd cmd = {0};
94	struct mmc_data data;
95	struct sdhci_host *host = mmc->priv;
96	int ret;
97
98	ret = mmc_set_blockcount(mmc, count, is_rel_write);
99	if (ret) {
100#ifdef CONFIG_MMC_RPMB_TRACE
101		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
102#endif
103		return 1;
104	}
105
106	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
107	cmd.cmdarg = 0;
108	cmd.resp_type = MMC_RSP_R1;
109
110	if (host->quirks & SDHCI_QUIRK_BROKEN_R1B)
111		cmd.resp_type = MMC_RSP_R1;
112
113	data.src = (const char *)s;
114	data.blocks = 1;
115	data.blocksize = MMC_MAX_BLOCK_LEN;
116	data.flags = MMC_DATA_WRITE;
117
118	ret = mmc_send_cmd(mmc, &cmd, &data);
119	if (ret) {
120#ifdef CONFIG_MMC_RPMB_TRACE
121		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
122#endif
123		return 1;
124	}
125	return 0;
126}
127static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
128			     unsigned short expected)
129{
130	struct mmc_cmd cmd = {0};
131	struct mmc_data data;
132	int ret;
133
134	ret = mmc_set_blockcount(mmc, 1, false);
135	if (ret) {
136#ifdef CONFIG_MMC_RPMB_TRACE
137		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
138#endif
139		return -1;
140	}
141	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
142	cmd.cmdarg = 0;
143	cmd.resp_type = MMC_RSP_R1;
144
145	data.dest = (char *)s;
146	data.blocks = 1;
147	data.blocksize = MMC_MAX_BLOCK_LEN;
148	data.flags = MMC_DATA_READ;
149
150	ret = mmc_send_cmd(mmc, &cmd, &data);
151	if (ret) {
152#ifdef CONFIG_MMC_RPMB_TRACE
153		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
154#endif
155		return -1;
156	}
157	/* Check the response and the status */
158	if (be16_to_cpu(s->request) != expected) {
159#ifdef CONFIG_MMC_RPMB_TRACE
160		printf("%s:response= %x\n", __func__,
161		       be16_to_cpu(s->request));
162#endif
163		return -1;
164	}
165	ret = be16_to_cpu(s->result);
166	if (ret) {
167		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
168		       (ret & RPMB_ERR_CNT_EXPIRED) ?
169		       "Write counter has expired" : "");
170	}
171
172	/* Return the status of the command */
173	return ret;
174}
175static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
176{
177	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
178
179	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
180	rpmb_frame->request = cpu_to_be16(RPMB_REQ_STATUS);
181	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
182		return -1;
183
184	/* Read the result */
185	return mmc_rpmb_response(mmc, rpmb_frame, expected);
186}
187static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
188		      unsigned char *output)
189{
190	sha256_context ctx;
191	int i;
192	unsigned char k_ipad[SHA256_BLOCK_SIZE];
193	unsigned char k_opad[SHA256_BLOCK_SIZE];
194
195	sha256_starts(&ctx);
196
197	/* According to RFC 4634, the HMAC transform looks like:
198	   SHA(K XOR opad, SHA(K XOR ipad, text))
199
200	   where K is an n byte key.
201	   ipad is the byte 0x36 repeated blocksize times
202	   opad is the byte 0x5c repeated blocksize times
203	   and text is the data being protected.
204	*/
205
206	for (i = 0; i < RPMB_SZ_MAC; i++) {
207		k_ipad[i] = key[i] ^ 0x36;
208		k_opad[i] = key[i] ^ 0x5c;
209	}
210	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
211	for ( ; i < SHA256_BLOCK_SIZE; i++) {
212		k_ipad[i] = 0x36;
213		k_opad[i] = 0x5c;
214	}
215	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
216	sha256_update(&ctx, buff, len);
217	sha256_finish(&ctx, output);
218
219	/* Init context for second pass */
220	sha256_starts(&ctx);
221
222	/* start with outer pad */
223	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);
224
225	/* then results of 1st hash */
226	sha256_update(&ctx, output, RPMB_SZ_MAC);
227
228	/* finish up 2nd pass */
229	sha256_finish(&ctx, output);
230}
231int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
232{
233	int ret;
234	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
235
236	/* Fill the request */
237	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
238	rpmb_frame->request = cpu_to_be16(RPMB_REQ_WCOUNTER);
239	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
240		return -1;
241
242	/* Read the result */
243	ret = mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_WCOUNTER);
244	if (ret)
245		return ret;
246
247	*pcounter = be32_to_cpu(rpmb_frame->write_counter);
248	return 0;
249}
250int mmc_rpmb_set_key(struct mmc *mmc, void *key)
251{
252	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
253	/* Fill the request */
254	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
255	rpmb_frame->request = cpu_to_be16(RPMB_REQ_KEY);
256	memcpy(rpmb_frame->mac, key, RPMB_SZ_MAC);
257
258	if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
259		return -1;
260
261	/* read the operation status */
262	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
263}
264int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
265		  unsigned short cnt, unsigned char *key)
266{
267	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
268	int i;
269
270	for (i = 0; i < cnt; i++) {
271		/* Fill the request */
272		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
273		rpmb_frame->address = cpu_to_be16(blk + i);
274		rpmb_frame->request = cpu_to_be16(RPMB_REQ_READ_DATA);
275		if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
276			break;
277
278		/* Read the result */
279		if (mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_READ_DATA))
280			break;
281
282		/* Check the HMAC if key is provided */
283		if (key) {
284			unsigned char ret_hmac[RPMB_SZ_MAC];
285
286			rpmb_hmac(key, rpmb_frame->data, 284, ret_hmac);
287			if (memcmp(ret_hmac, rpmb_frame->mac, RPMB_SZ_MAC)) {
288				printf("MAC error on block #%d\n", i);
289				break;
290			}
291		}
292		/* Copy data */
293		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame->data, RPMB_SZ_DATA);
294	}
295	return i;
296}
297int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
298		  unsigned short cnt, unsigned char *key)
299{
300	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
301	unsigned long wcount;
302	int i;
303
304	for (i = 0; i < cnt; i++) {
305		if (mmc_rpmb_get_counter(mmc, &wcount)) {
306			printf("Cannot read RPMB write counter\n");
307			break;
308		}
309
310		/* Fill the request */
311		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
312		memcpy(rpmb_frame->data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
313		rpmb_frame->address = cpu_to_be16(blk + i);
314		rpmb_frame->block_count = cpu_to_be16(1);
315		rpmb_frame->write_counter = cpu_to_be32(wcount);
316		rpmb_frame->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
317		/* Computes HMAC */
318		rpmb_hmac(key, rpmb_frame->data, 284, rpmb_frame->mac);
319
320		if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
321			break;
322
323		/* Get status */
324		if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
325			break;
326	}
327	return i;
328}
329
330static int send_write_mult_block(struct mmc *mmc, const struct s_rpmb *frm,
331				 unsigned short cnt)
332{
333	struct mmc_cmd cmd = {
334		.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK,
335		.resp_type = MMC_RSP_R1,
336	};
337	struct mmc_data data = {
338		.src = (const void *)frm,
339		.blocks = cnt,
340		.blocksize = sizeof(*frm),
341		.flags = MMC_DATA_WRITE,
342	};
343
344	return mmc_send_cmd(mmc, &cmd, &data);
345}
346
347static int send_read_mult_block(struct mmc *mmc, struct s_rpmb *frm,
348				unsigned short cnt)
349{
350	struct mmc_cmd cmd = {
351		.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK,
352		.resp_type = MMC_RSP_R1,
353	};
354	struct mmc_data data = {
355		.dest = (void *)frm,
356		.blocks = cnt,
357		.blocksize = sizeof(*frm),
358		.flags = MMC_DATA_READ,
359	};
360
361	return mmc_send_cmd(mmc, &cmd, &data);
362}
363
364static int rpmb_route_write_req(struct mmc *mmc, struct s_rpmb *req,
365				unsigned short req_cnt, struct s_rpmb *rsp,
366				unsigned short rsp_cnt)
367{
368	int ret;
369
370	/*
371	 * Send the write request.
372	 */
373	ret = mmc_set_blockcount(mmc, req_cnt, true);
374	if (ret)
375		return ret;
376
377	ret = send_write_mult_block(mmc, req, req_cnt);
378	if (ret)
379		return ret;
380
381	/*
382	 * Read the result of the request.
383	 */
384	ret = mmc_set_blockcount(mmc, 1, false);
385	if (ret)
386		return ret;
387
388	memset(rsp, 0, sizeof(*rsp));
389	rsp->request = cpu_to_be16(RPMB_REQ_STATUS);
390	ret = send_write_mult_block(mmc, rsp, 1);
391	if (ret)
392		return ret;
393
394	ret = mmc_set_blockcount(mmc, 1, false);
395	if (ret)
396		return ret;
397
398	return send_read_mult_block(mmc, rsp, 1);
399}
400
401static int rpmb_route_read_req(struct mmc *mmc, struct s_rpmb *req,
402			       unsigned short req_cnt, struct s_rpmb *rsp,
403			       unsigned short rsp_cnt)
404{
405	int ret;
406
407	/*
408	 * Send the read request.
409	 */
410	ret = mmc_set_blockcount(mmc, 1, false);
411	if (ret)
412		return ret;
413
414	ret = send_write_mult_block(mmc, req, 1);
415	if (ret)
416		return ret;
417
418	/*
419	 * Read the result of the request.
420	 */
421
422	ret = mmc_set_blockcount(mmc, rsp_cnt, false);
423	if (ret)
424		return ret;
425
426	return send_read_mult_block(mmc, rsp, rsp_cnt);
427}
428
429static int rpmb_route_frames(struct mmc *mmc, struct s_rpmb *req,
430			     unsigned short req_cnt, struct s_rpmb *rsp,
431			     unsigned short rsp_cnt)
432{
433	unsigned short n;
434
435	/*
436	 * If multiple request frames are provided, make sure that all are
437	 * of the same type.
438	 */
439	for (n = 1; n < req_cnt; n++)
440		if (req[n].request != req->request)
441			return -EINVAL;
442
443	switch (be16_to_cpu(req->request)) {
444	case RPMB_REQ_KEY:
445		if (req_cnt != 1 || rsp_cnt != 1)
446			return -EINVAL;
447		return rpmb_route_write_req(mmc, req, req_cnt, rsp, rsp_cnt);
448
449	case RPMB_REQ_WRITE_DATA:
450		if (!req_cnt || rsp_cnt != 1)
451			return -EINVAL;
452		return rpmb_route_write_req(mmc, req, req_cnt, rsp, rsp_cnt);
453
454	case RPMB_REQ_WCOUNTER:
455		if (req_cnt != 1 || rsp_cnt != 1)
456			return -EINVAL;
457		return rpmb_route_read_req(mmc, req, req_cnt, rsp, rsp_cnt);
458
459	case RPMB_REQ_READ_DATA:
460		if (req_cnt != 1 || !req_cnt)
461			return -EINVAL;
462		return rpmb_route_read_req(mmc, req, req_cnt, rsp, rsp_cnt);
463
464	default:
465		debug("Unsupported message type: %d\n",
466		      be16_to_cpu(req->request));
467		return -EINVAL;
468	}
469}
470
471int mmc_rpmb_route_frames(struct mmc *mmc, void *req, unsigned long reqlen,
472			  void *rsp, unsigned long rsplen)
473{
474	/*
475	 * Whoever crafted the data supplied to this function knows how to
476	 * format the PRMB frames and which response is expected. If
477	 * there's some unexpected mismatch it's more helpful to report an
478	 * error immediately than trying to guess what was the intention
479	 * and possibly just delay an eventual error which will be harder
480	 * to track down.
481	 */
482	void *rpmb_data = NULL;
483	int ret;
484
485	if (reqlen % sizeof(struct s_rpmb) || rsplen % sizeof(struct s_rpmb))
486		return -EINVAL;
487
488	if (!IS_ALIGNED((uintptr_t)req, ARCH_DMA_MINALIGN)) {
489		/* Memory alignment is required by MMC driver */
490		rpmb_data = malloc(reqlen);
491		if (!rpmb_data)
492			return -ENOMEM;
493
494		memcpy(rpmb_data, req, reqlen);
495		req = rpmb_data;
496	}
497
498	ret = rpmb_route_frames(mmc, req, reqlen / sizeof(struct s_rpmb),
499				rsp, rsplen / sizeof(struct s_rpmb));
500	free(rpmb_data);
501	return ret;
502}
503