1 /*
2 * Copyright (c) 2018-2022 Intel Corporation.
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 * 1. Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer in
9 * this position and unchanged.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 */
26
27 #include <errno.h>
28 #include <stdio.h>
29 #include <string.h>
30 #include <inttypes.h>
31 #include <unistd.h>
32 #include <openssl/hmac.h>
33 #include <openssl/opensslv.h>
34 #if OPENSSL_VERSION_NUMBER >= 0x30000000L
35 #include <openssl/evp.h>
36 #endif
37
38 #include "rpmb.h"
39 #include "rpmb_sim.h"
40 #include "log.h"
41
42 static FILE *rpmb_fd = NULL;
43
44 /*
45 * 0~6 is magic
46 * 7~38 is rpmb key
47 * 39~41 is write counter
48 */
49 #define KEY_MAGIC "key_sim"
50 #define KEY_MAGIC_ADDR 0
51 #define KEY_MAGIC_LENGTH 7
52
53 #define KEY_ADDR 7
54 #define KEY_LENGTH 32
55
56 #define WRITER_COUNTER_ADDR 39
57
58 #define TEEDATA_SIZE (4*1024*1024) //4M
59 #define TEEDATA_BLOCK_COUNT (TEEDATA_SIZE/256)
60
61 #ifndef offsetof
62 #define offsetof(s, m) (size_t) &(((s *) 0)->m)
63 #endif
64
65 static int virtio_rpmb_debug = 1;
66 #define DPRINTF(params) do { if (virtio_rpmb_debug) pr_dbg params; } while (0)
67 #define WPRINTF(params) (pr_err params)
68
69 /* Make rpmb_mac compatible for different openssl versions */
70 #if OPENSSL_VERSION_NUMBER < 0x10100000L
rpmb_mac(const uint8_t * key,const struct rpmb_frame * frames,size_t frame_cnt,uint8_t * mac)71 int rpmb_mac(const uint8_t *key, const struct rpmb_frame *frames,
72 size_t frame_cnt, uint8_t *mac)
73 {
74 int i;
75 int hmac_ret;
76 unsigned int md_len;
77 HMAC_CTX hmac_ctx;
78
79 HMAC_CTX_init(&hmac_ctx);
80 hmac_ret = HMAC_Init_ex(&hmac_ctx, key, 32, EVP_sha256(), NULL);
81 if (!hmac_ret) {
82 DPRINTF(("HMAC_Init_ex failed\n"));
83 goto err;
84 }
85
86 for (i = 0; i < frame_cnt; i++) {
87 hmac_ret = HMAC_Update(&hmac_ctx, frames[i].data, 284);
88 if (!hmac_ret) {
89 DPRINTF(("HMAC_Update failed\n"));
90 goto err;
91 }
92 }
93
94 hmac_ret = HMAC_Final(&hmac_ctx, mac, &md_len);
95 if (md_len != 32) {
96 DPRINTF(("bad md_len %d != 32.\n", md_len));
97 goto err;
98 }
99
100 if (!hmac_ret) {
101 DPRINTF(("HMAC_Final failed\n"));
102 goto err;
103 }
104
105 err:
106 HMAC_CTX_cleanup(&hmac_ctx);
107
108 return hmac_ret ? 0 : -1;
109 }
110 #elif OPENSSL_VERSION_NUMBER >= 0x30000000L
rpmb_mac(const uint8_t * key,const struct rpmb_frame * frames,size_t frame_cnt,uint8_t * mac)111 int rpmb_mac(const uint8_t *key, const struct rpmb_frame *frames,
112 size_t frame_cnt, uint8_t *mac)
113 {
114 int i;
115 int hmac_ret;
116 size_t md_len;
117 EVP_MAC_CTX *hmac_ctx;
118 EVP_MAC *hmac = EVP_MAC_fetch(NULL, "HMAC", NULL);
119
120 hmac_ctx = EVP_MAC_CTX_new(hmac);
121 if (hmac_ctx == NULL) {
122 DPRINTF(("get hmac_ctx failed\n"));
123 EVP_MAC_free(hmac);
124 return -1;
125 }
126
127 hmac_ret = EVP_MAC_init(hmac_ctx, key, 32, NULL);
128 if (!hmac_ret) {
129 DPRINTF(("HMAC_Init_ex failed\n"));
130 goto err;
131 }
132
133 for (i = 0; i < frame_cnt; i++) {
134 hmac_ret = EVP_MAC_update(hmac_ctx, frames[i].data, 284);
135 if (!hmac_ret) {
136 DPRINTF(("HMAC_Update failed\n"));
137 goto err;
138 }
139 }
140
141 hmac_ret = EVP_MAC_final(hmac_ctx, mac, &md_len, 32);
142 if (md_len != 32) {
143 DPRINTF(("bad md_len %d != 32.\n", md_len));
144 goto err;
145 }
146
147 if (!hmac_ret) {
148 DPRINTF(("HMAC_Final failed\n"));
149 goto err;
150 }
151
152 err:
153 EVP_MAC_CTX_free(hmac_ctx);
154 EVP_MAC_free(hmac);
155
156 return hmac_ret ? 0 : -1;
157 }
158 #else
rpmb_mac(const uint8_t * key,const struct rpmb_frame * frames,size_t frame_cnt,uint8_t * mac)159 int rpmb_mac(const uint8_t *key, const struct rpmb_frame *frames,
160 size_t frame_cnt, uint8_t *mac)
161 {
162 int i;
163 int hmac_ret;
164 unsigned int md_len;
165 HMAC_CTX *hmac_ctx;
166
167 hmac_ctx = HMAC_CTX_new();
168 if (hmac_ctx == NULL) {
169 DPRINTF(("get hmac_ctx failed\n"));
170 return -1;
171 }
172
173 hmac_ret = HMAC_Init_ex(hmac_ctx, key, 32, EVP_sha256(), NULL);
174 if (!hmac_ret) {
175 DPRINTF(("HMAC_Init_ex failed\n"));
176 goto err;
177 }
178
179 for (i = 0; i < frame_cnt; i++) {
180 hmac_ret = HMAC_Update(hmac_ctx, frames[i].data, 284);
181 if (!hmac_ret) {
182 DPRINTF(("HMAC_Update failed\n"));
183 goto err;
184 }
185 }
186
187 hmac_ret = HMAC_Final(hmac_ctx, mac, &md_len);
188 if (md_len != 32) {
189 DPRINTF(("bad md_len %d != 32.\n", md_len));
190 goto err;
191 }
192
193 if (!hmac_ret) {
194 DPRINTF(("HMAC_Final failed\n"));
195 goto err;
196 }
197
198 err:
199 HMAC_CTX_free(hmac_ctx);
200
201 return hmac_ret ? 0 : -1;
202 }
203 #endif
204
rpmb_sim_close(void)205 static void rpmb_sim_close(void)
206 {
207 fclose(rpmb_fd);
208 rpmb_fd = NULL;
209 }
210
file_write(FILE * fp,const void * buf,size_t size,off_t offset)211 static int file_write(FILE *fp, const void *buf, size_t size, off_t offset)
212 {
213 size_t rc = 0;
214
215 if (fseek(fp, offset, SEEK_SET)) {
216 DPRINTF(("%s:Seek to %ld failed.\n", __func__, offset));
217 return -1;
218 }
219
220 rc = fwrite(buf, sizeof(char), size, fp);
221 if (rc != size) {
222 return -1;
223 }
224
225 /* The flow of file writing sync should be:
226 C lib caches--->fflush--->disk caches--->fsync--->disk */
227 if (fflush(fp) < 0) {
228 return -1;
229 }
230
231 if (fsync(fileno(fp)) < 0) {
232 DPRINTF(("%s: fsync failed\n", __func__));
233 return -1;
234 }
235
236 return rc;
237 }
238
file_read(FILE * fp,void * buf,size_t size,off_t offset)239 static int file_read(FILE *fp, void *buf, size_t size, off_t offset)
240 {
241 size_t rc = 0;
242
243 if (fseek(fp, offset, SEEK_SET)) {
244 DPRINTF(("%s:Seek to %ld failed.\n", __func__, offset));
245 return -1;
246 }
247
248 rc = fread(buf, sizeof(char), size, fp);
249 if (rc == size) {
250 return rc;
251 } else {
252 return -1;
253 }
254 }
255
rpmb_sim_open(const char * rpmb_devname)256 static int rpmb_sim_open(const char *rpmb_devname)
257 {
258 uint8_t data = 0;
259 rpmb_fd = fopen(rpmb_devname, "rb+");
260
261 if (rpmb_fd == NULL) {
262 /*if the rpmb device file does not exist, create a new file*/
263 rpmb_fd = fopen(rpmb_devname, "wb+");
264 DPRINTF(("rpmb device file(%s) does not exist, create a new file\n", rpmb_devname));
265 /* Write 0 to the last byte to enable 4MB length access */
266 if (rpmb_fd == NULL || file_write(rpmb_fd, &data, 1, TEEDATA_SIZE - 1) < 0) {
267 DPRINTF(("Failed to initialize simulated rpmb to 0.\n"));
268 rpmb_fd = NULL;
269 }
270 }
271
272 if (rpmb_fd == NULL) {
273 DPRINTF(("%s: unable (%d) to open rpmb device '%s': %s\n",
274 __func__, errno, rpmb_devname, strerror(errno)));
275 return -1;
276 }
277
278 return 0;
279 }
280
get_counter(uint32_t * counter)281 static int get_counter(uint32_t *counter)
282 {
283 int rc = 0;
284
285 rc = file_read(rpmb_fd, counter, sizeof(*counter), WRITER_COUNTER_ADDR);
286 if (rc < 0)
287 {
288 DPRINTF(("%s failed.\n", __func__));
289 return -1;
290 }
291
292 swap32(*counter);
293
294 return 0;
295 }
296
set_counter(const uint32_t * counter)297 static int set_counter(const uint32_t *counter)
298 {
299 int rc = 0;
300 uint32_t cnt = *counter;
301
302 swap32(cnt);
303 rc = file_write(rpmb_fd, &cnt, sizeof(cnt), WRITER_COUNTER_ADDR);
304 if (rc < 0)
305 {
306 DPRINTF(("%s failed.\n", __func__));
307 return -1;
308 }
309
310 return 0;
311 }
312
is_key_programmed(void)313 static int is_key_programmed(void)
314 {
315 int rc = 0;
316 uint8_t magic[KEY_MAGIC_LENGTH] = {0};
317
318 rc = file_read(rpmb_fd, magic, KEY_MAGIC_LENGTH, KEY_MAGIC_ADDR);
319 if (rc < 0)
320 {
321 DPRINTF(("%s read magic failed.\n", __func__));
322 return 0;
323 }
324
325 if (memcmp(KEY_MAGIC, magic, KEY_MAGIC_LENGTH))
326 return 0;
327
328 return 1;
329 }
330
get_key(uint8_t * key)331 static int get_key(uint8_t *key)
332 {
333 int rc = 0;
334
335 rc = file_read(rpmb_fd, key, 32, KEY_ADDR);
336 if (rc < 0)
337 {
338 DPRINTF(("%s failed.\n", __func__));
339 return -1;
340 }
341
342 return 0;
343 }
344
program_key(const uint8_t * key)345 static int program_key(const uint8_t *key)
346 {
347 int rc = 0;
348
349 rc = file_write(rpmb_fd, key, 32, KEY_ADDR);
350 if (rc < 0)
351 {
352 DPRINTF(("%s failed at set key.\n", __func__));
353 return -1;
354 }
355
356 rc = file_write(rpmb_fd, KEY_MAGIC, KEY_MAGIC_LENGTH, KEY_MAGIC_ADDR);
357 if (rc < 0)
358 {
359 DPRINTF(("%s failed at set magic.\n", __func__));
360 return -1;
361 }
362
363 return 0;
364 }
365
rpmb_sim_program_key(const struct rpmb_frame * in_frame,uint32_t in_cnt,struct rpmb_frame * out_frame,uint32_t out_cnt)366 static int rpmb_sim_program_key(const struct rpmb_frame* in_frame, uint32_t in_cnt,
367 struct rpmb_frame* out_frame, uint32_t out_cnt)
368 {
369 int ret = 0;
370 int err = RPMB_RES_WRITE_FAILURE;
371 uint32_t counter = 0;
372
373 if (in_cnt == 0 || in_frame == NULL)
374 return -EINVAL;
375
376 if (is_key_programmed())
377 err = RPMB_RES_GENERAL_FAILURE;
378 else
379 ret = program_key(in_frame->key_mac);
380
381 if (ret)
382 goto out;
383
384 ret = set_counter(&counter);
385 if (ret)
386 goto out;
387
388 err = RPMB_RES_OK;
389
390 out:
391 if (out_frame) {
392 memset(out_frame, 0, out_cnt*sizeof(*out_frame));
393 out_frame->req_resp = swap16(RPMB_RESP_PROGRAM_KEY);
394 out_frame->result = swap16(err);
395 }
396
397 return ret;
398 }
399
rpmb_sim_write(const struct rpmb_frame * in_frame,uint32_t in_cnt,struct rpmb_frame * out_frame,uint32_t out_cnt)400 static int rpmb_sim_write(const struct rpmb_frame* in_frame, uint32_t in_cnt,
401 struct rpmb_frame* out_frame, uint32_t out_cnt)
402 {
403 int ret = 0;
404 int err = RPMB_RES_WRITE_FAILURE;
405 uint32_t i;
406 uint8_t key[32];
407 uint8_t mac[32];
408 uint32_t counter;
409 uint16_t addr;
410 uint16_t block_count;
411 uint8_t data[256*in_cnt];
412
413 if (in_cnt == 0 || in_frame == NULL)
414 return -EINVAL;
415
416 if (in_frame[0].req_resp != swap16(RPMB_REQ_DATA_WRITE))
417 return -EINVAL;
418
419 if (in_cnt > 2) {
420 err = RPMB_RES_GENERAL_FAILURE;
421 goto out;
422 }
423
424 addr = swap16(in_frame[0].addr);
425 block_count = swap16(in_frame[0].block_count);
426
427 if (addr >= TEEDATA_BLOCK_COUNT) {
428 err = RPMB_RES_ADDR_FAILURE;
429 goto out;
430 }
431
432 if (addr + block_count > TEEDATA_BLOCK_COUNT)
433 goto out;
434
435 if (block_count == 0 || block_count > in_cnt) {
436 ret = -EINVAL;
437 err = RPMB_RES_GENERAL_FAILURE;
438 goto out;
439 }
440
441 if (!is_key_programmed()) {
442 err = RPMB_RES_NO_AUTH_KEY;
443 goto out;
444 }
445
446 if (get_counter(&counter))
447 goto out;
448
449 if (counter == 0xFFFFFFFF) {
450 err = RPMB_RES_WRITE_COUNTER_EXPIRED;
451 goto out;
452 }
453
454 if (counter != swap32(in_frame[0].write_counter)) {
455 err = RPMB_RES_COUNT_FAILURE;
456 goto out;
457 }
458
459 if (get_key(key)) {
460 err = RPMB_RES_GENERAL_FAILURE;
461 goto out;
462 }
463
464 if (rpmb_mac(key, in_frame, in_cnt, mac)) {
465 err = RPMB_RES_GENERAL_FAILURE;
466 goto out;
467 }
468
469 if (memcmp(in_frame[in_cnt - 1].key_mac, mac, 32)) {
470 DPRINTF(("%s wrong mac.\n", __func__));
471 err = RPMB_RES_AUTH_FAILURE;
472 goto out;
473 }
474
475 for (i = 0; i < in_cnt; i++)
476 memcpy(data + i * 256, in_frame[i].data, 256);
477
478 if (file_write(rpmb_fd, data, sizeof(data), 256 * addr) < 0) {
479 DPRINTF(("%s write_with_retry failed.\n", __func__));
480 goto out;
481 }
482
483 ++counter;
484 if (set_counter(&counter)) {
485 DPRINTF(("%s set_counter failed.\n", __func__));
486 goto out;
487 }
488
489 err = RPMB_RES_OK;
490
491 out:
492 if (out_frame) {
493 memset(out_frame, 0, out_cnt*sizeof(*out_frame));
494 out_frame->req_resp = swap16(RPMB_RESP_DATA_WRITE);
495 out_frame->result = swap16(err);
496 if (err == RPMB_RES_OK) {
497 out_frame->addr = swap16(addr);
498 out_frame->write_counter = swap32(counter);
499 rpmb_mac(key, out_frame, 1, out_frame->key_mac);
500 }
501 }
502
503 return ret;
504 }
505
rpmb_sim_read(const struct rpmb_frame * in_frame,uint32_t in_cnt,struct rpmb_frame * out_frame,uint32_t out_cnt)506 static int rpmb_sim_read(const struct rpmb_frame* in_frame, uint32_t in_cnt,
507 struct rpmb_frame* out_frame, uint32_t out_cnt)
508 {
509 int ret = 0;
510 uint32_t i;
511 int err = RPMB_RES_READ_FAILURE;
512 uint8_t key[32];
513 uint8_t mac[32];
514 uint16_t addr;
515 uint8_t data[256*out_cnt];
516
517 if (in_cnt != 1 || in_frame == NULL)
518 return -EINVAL;
519
520 if (in_frame->req_resp != swap16(RPMB_REQ_DATA_READ))
521 return -EINVAL;
522
523 addr = swap16(in_frame->addr);
524
525 if (addr >= TEEDATA_BLOCK_COUNT) {
526 err = RPMB_RES_ADDR_FAILURE;
527 goto out;
528 }
529
530 if (addr + out_cnt > TEEDATA_BLOCK_COUNT)
531 goto out;
532
533 if (!is_key_programmed()) {
534 err = RPMB_RES_NO_AUTH_KEY;
535 goto out;
536 }
537
538 if (file_read(rpmb_fd, data, sizeof(data), 256 * addr) < 0) {
539 DPRINTF(("%s read_with_retry failed.\n", __func__));
540 goto out;
541 }
542
543 err = RPMB_RES_OK;
544
545 out:
546 if (out_frame) {
547 memset(out_frame, 0, out_cnt*sizeof(*out_frame));
548 for (i = 0; i < out_cnt; i++) {
549 memcpy(out_frame[i].nonce, in_frame[0].nonce, sizeof(in_frame[0].nonce));
550 out_frame[i].req_resp = swap16(RPMB_RESP_DATA_READ);
551 out_frame[i].block_count = swap16(out_cnt);
552 out_frame[i].addr = in_frame[0].addr;
553 memcpy(out_frame[i].data, data+256*i, 256);
554 }
555 if (get_key(key))
556 DPRINTF(("%s, get_key failed.\n", __func__));
557
558 out_frame[out_cnt - 1].result = swap16(err);
559 rpmb_mac(key, out_frame, out_cnt, mac);
560 memcpy(out_frame[out_cnt - 1].key_mac, mac, sizeof(mac));
561 }
562
563 return ret;
564 }
565
rpmb_sim_get_counter(const struct rpmb_frame * in_frame,uint32_t in_cnt,struct rpmb_frame * out_frame,uint32_t out_cnt)566 static int rpmb_sim_get_counter(const struct rpmb_frame* in_frame, uint32_t in_cnt,
567 struct rpmb_frame* out_frame, uint32_t out_cnt)
568 {
569 int ret = 0;
570 int err = RPMB_RES_COUNT_FAILURE;
571 uint8_t key[32];
572 uint32_t counter;
573
574 if (in_cnt != 1 || in_frame == NULL)
575 return -EINVAL;
576
577 if (in_frame->req_resp != swap16(RPMB_REQ_GET_COUNTER))
578 return -EINVAL;
579
580 if (!is_key_programmed()) {
581 err = RPMB_RES_NO_AUTH_KEY;
582 goto out;
583 }
584
585 if (get_key(key))
586 goto out;
587
588 if (get_counter(&counter))
589 goto out;
590
591 err = RPMB_RES_OK;
592
593 out:
594 if (out_frame) {
595 memset(out_frame, 0, sizeof(*out_frame)*out_cnt);
596 out_frame->result = swap16(err);
597 out_frame->req_resp = swap16(RPMB_RESP_GET_COUNTER);
598 memcpy(out_frame->nonce, in_frame[0].nonce, sizeof(in_frame[0].nonce));
599
600 if (err == RPMB_RES_OK) {
601 out_frame->write_counter = swap32(counter);
602 rpmb_mac(key, out_frame, out_cnt, out_frame->key_mac);
603 }
604 }
605
606 return ret;
607 }
608
is_use_sim_rpmb(void)609 int is_use_sim_rpmb(void)
610 {
611 int ret;
612
613 ret = rpmb_sim_open(RPMB_SIM_PATH_NAME);
614 if (ret) {
615 DPRINTF(("%s: rpmb_sim_open failed\n", __func__));
616 return 0;
617 }
618
619 ret = is_key_programmed();
620
621 rpmb_sim_close();
622
623 return ret;
624 }
625
rpmb_sim_key_init(uint8_t * key)626 int rpmb_sim_key_init(uint8_t *key)
627 {
628 int ret;
629 uint32_t counter = 0;
630
631 ret = rpmb_sim_open(RPMB_SIM_PATH_NAME);
632 if (ret) {
633 DPRINTF(("%s: rpmb_sim_open failed\n", __func__));
634 return ret;
635 }
636
637 if (!is_key_programmed()) {
638 ret = program_key(key);
639 if (ret) {
640 DPRINTF(("%s: program_key failed\n", __func__));
641 goto out;
642 }
643 }
644
645 ret = get_counter(&counter);
646 if (ret) {
647 counter = 0;
648 ret = set_counter(&counter);
649 if (ret) {
650 DPRINTF(("%s: set_counter failed\n", __func__));
651 goto out;
652 }
653 }
654
655 out:
656 rpmb_sim_close();
657
658 return ret;
659 }
660
661 /*
662 * rel_write write read
663 * RPMB_READ 0 1 1~N
664 * RPMB_WRITE 1~N 1 1
665 * GET_COUNTER 0 1 1
666 * PROGRAM_KEY 1 1 1
667 */
rpmb_sim_operations(const void * rel_write_data,size_t rel_write_size,const void * write_data,size_t write_size,void * read_buf,size_t read_size)668 static int rpmb_sim_operations(const void *rel_write_data, size_t rel_write_size,
669 const void *write_data, size_t write_size,
670 void *read_buf, size_t read_size)
671 {
672 int ret = -1;
673
674 if (rel_write_size) {
675 size_t nframe = rel_write_size/RPMB_FRAME_SIZE;
676 struct rpmb_frame rel_write_frame[nframe];
677 memcpy(rel_write_frame, rel_write_data, sizeof(rel_write_frame));
678 if (rel_write_frame[0].req_resp == swap16(RPMB_REQ_DATA_WRITE)) {
679 if (write_size/RPMB_FRAME_SIZE &&
680 ((struct rpmb_frame*)write_data)->req_resp == swap16(RPMB_REQ_RESULT_READ))
681 ret = rpmb_sim_write(rel_write_frame, nframe, read_buf, read_size/RPMB_FRAME_SIZE);
682 else
683 ret = rpmb_sim_write(rel_write_frame, nframe, NULL, 0);
684 }
685 else if (rel_write_frame[0].req_resp == swap16(RPMB_REQ_PROGRAM_KEY)) {
686 if (write_size/RPMB_FRAME_SIZE &&
687 ((struct rpmb_frame*)write_data)->req_resp == swap16(RPMB_REQ_RESULT_READ))
688 ret = rpmb_sim_program_key(rel_write_frame, 1, read_buf, read_size/RPMB_FRAME_SIZE);
689 else
690 ret = rpmb_sim_program_key(rel_write_frame, 1, NULL, 0);
691 }
692 }
693 else if (write_size) {
694 struct rpmb_frame write_frame[write_size/RPMB_FRAME_SIZE];
695 memcpy(write_frame, write_data, sizeof(write_frame));
696 if (write_frame[0].req_resp == swap16(RPMB_REQ_DATA_READ)) {
697 ret = rpmb_sim_read(write_frame, 1, read_buf, read_size/RPMB_FRAME_SIZE);
698 }
699 else if (write_frame[0].req_resp == swap16(RPMB_REQ_GET_COUNTER)) {
700 ret = rpmb_sim_get_counter(write_frame, 1, read_buf, 1);
701 }
702 }
703
704 return ret;
705 }
706
rpmb_sim_send(const void * r)707 int rpmb_sim_send(const void *r)
708 {
709 int ret;
710 uint16_t i;
711 uint32_t write_size = 0;
712 uint32_t rel_write_size = 0;
713 uint32_t read_size = 0;
714 struct rpmb_frame *frame_write = NULL;
715 struct rpmb_frame *frame_rel_write = NULL;
716 struct rpmb_frame *frame_read = NULL;
717 struct rpmb_ioc_cmd *ioc_cmd = NULL;
718 const struct rpmb_ioc_seq_data *iseq = r;
719
720 for (i = 0; i < iseq->h.num_of_cmds; i++) {
721 ioc_cmd = (struct rpmb_ioc_cmd *)(&iseq->cmd[i]);
722 if (ioc_cmd->flags == 0) {
723 frame_read = (struct rpmb_frame *)ioc_cmd->frames_ptr;
724 read_size = ioc_cmd->nframes * RPMB_FRAME_SIZE;
725 } else if (ioc_cmd->flags == RPMB_F_WRITE) {
726 frame_write = (struct rpmb_frame *)ioc_cmd->frames_ptr;
727 write_size = ioc_cmd->nframes * RPMB_FRAME_SIZE;
728 } else if (ioc_cmd->flags == (RPMB_F_WRITE | RPMB_F_REL_WRITE)) {
729 frame_rel_write = (struct rpmb_frame *)ioc_cmd->frames_ptr;
730 rel_write_size = ioc_cmd->nframes * RPMB_FRAME_SIZE;
731 } else {
732 DPRINTF(("%s: rpmb_ioc_cmd is invalid in the rpmb_ioc_seq_data\n", __func__));
733 goto err_response;
734 }
735 }
736
737 ret = rpmb_sim_open(RPMB_SIM_PATH_NAME);
738 if (ret) {
739 DPRINTF(("%s: rpmb_sim_open failed\n", __func__));
740 goto err_response;
741 }
742
743 /* execute rpmb command */
744 ret = rpmb_sim_operations(frame_rel_write, rel_write_size,
745 frame_write, write_size,
746 frame_read, read_size);
747 rpmb_sim_close();
748
749 if (ret) {
750 DPRINTF(("%s: rpmb_sim_operations failed\n", __func__));
751 goto err_response;
752 }
753
754 return 0;
755
756 err_response:
757 return -1;
758 }
759