1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright 2020 NXP
4 */
5 #include <dcp_utils.h>
6 #include <drivers/imx/dcp.h>
7 #include <kernel/tee_misc.h>
8 #include <malloc.h>
9 #include <mm/core_memprot.h>
10 #include <string.h>
11 #include <trace.h>
12
dcp_calloc_align_buf(struct dcp_align_buf * buf,size_t size)13 TEE_Result dcp_calloc_align_buf(struct dcp_align_buf *buf, size_t size)
14 {
15 if (!buf) {
16 EMSG("Error, buf is null");
17 return TEE_ERROR_BAD_PARAMETERS;
18 }
19
20 buf->data = alloc_cache_aligned(size);
21 if (!buf->data)
22 return TEE_ERROR_OUT_OF_MEMORY;
23
24 buf->paddr = virt_to_phys(buf->data);
25
26 if (!buf->paddr) {
27 dcp_free(buf);
28 return TEE_ERROR_OUT_OF_MEMORY;
29 }
30
31 buf->size = size;
32
33 return TEE_SUCCESS;
34 }
35
dcp_free(struct dcp_align_buf * buf)36 void dcp_free(struct dcp_align_buf *buf)
37 {
38 free(buf->data);
39 }
40
dcp_left_shift_buffer(uint8_t * input,uint8_t * result,size_t buffer_size)41 void dcp_left_shift_buffer(uint8_t *input, uint8_t *result, size_t buffer_size)
42 {
43 unsigned int i = 0;
44 uint8_t overflow = 0;
45
46 /* For each byte */
47 for (i = 0; i < buffer_size; i++) {
48 /* Left shift a bytes by one */
49 result[buffer_size - 1 - i] =
50 input[buffer_size - 1 - i] << 1 | overflow;
51
52 overflow = input[buffer_size - 1 - i] >> 7;
53 }
54 }
55
dcp_udelay(uint32_t time)56 void dcp_udelay(uint32_t time)
57 {
58 uint32_t counter = time * 500;
59
60 /* Implementation of a Software loop assuming CPU clock of 500MHz */
61 while (counter--) {
62 isb();
63 dsb();
64 };
65 }
66
dcp_reverse(uint8_t * in,uint8_t * out,size_t size)67 void dcp_reverse(uint8_t *in, uint8_t *out, size_t size)
68 {
69 unsigned int i = 0;
70
71 for (i = 0; i < size; i++)
72 out[i] = in[size - 1 - i];
73 }
74
dcp_xor(uint8_t * a,uint8_t * b,uint8_t * out,size_t size)75 void dcp_xor(uint8_t *a, uint8_t *b, uint8_t *out, size_t size)
76 {
77 unsigned int i = 0;
78
79 for (i = 0; i < size; i++)
80 out[i] = a[i] ^ b[i];
81 }
82
dcp_cmac_padding(uint8_t * block,size_t len)83 void dcp_cmac_padding(uint8_t *block, size_t len)
84 {
85 unsigned int i = 0;
86
87 for (i = len; i < DCP_AES128_BLOCK_SIZE; i++) {
88 if (i == len)
89 block[i] = BIT(7);
90 else
91 block[i] = 0x0;
92 }
93 }
94