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/cache_helpers.h>
8 #include <malloc.h>
9 #include <mm/core_memprot.h>
10 #include <string.h>
11 #include <trace.h>
12
13 /*
14 * Allocate an area of given size in bytes and set it to zero. Add the memory
15 * allocator information in the newly allocated area.
16 * Return a cacheline aligned buffer.
17 *
18 * @size Size in bytes to allocate
19 */
dcp_alloc_memalign(size_t size)20 static void *dcp_alloc_memalign(size_t size)
21 {
22 void *ptr = NULL;
23 size_t alloc_size = 0;
24 uint32_t cacheline_size = 0;
25
26 /*
27 * Buffer must be aligned on a cache line:
28 * - Buffer start address aligned on a cache line
29 * - End of Buffer inside a cache line.
30 *
31 * If the size to be allocated is already aligned on a cache line add a
32 * another cache line.
33 */
34 cacheline_size = dcache_get_line_size();
35 if (ROUNDUP_OVERFLOW(size, cacheline_size, &alloc_size))
36 return NULL;
37
38 ptr = memalign(cacheline_size, alloc_size);
39 if (!ptr)
40 return NULL;
41
42 memset(ptr, 0, size);
43
44 return ptr;
45 }
46
dcp_calloc_align_buf(struct dcp_align_buf * buf,size_t size)47 TEE_Result dcp_calloc_align_buf(struct dcp_align_buf *buf, size_t size)
48 {
49 if (!buf) {
50 EMSG("Error, buf is null");
51 return TEE_ERROR_BAD_PARAMETERS;
52 }
53
54 buf->data = dcp_alloc_memalign(size);
55 if (!buf->data)
56 return TEE_ERROR_OUT_OF_MEMORY;
57
58 buf->paddr = virt_to_phys(buf->data);
59
60 if (!buf->paddr) {
61 dcp_free(buf);
62 return TEE_ERROR_OUT_OF_MEMORY;
63 }
64
65 buf->size = size;
66
67 return TEE_SUCCESS;
68 }
69
dcp_free(struct dcp_align_buf * buf)70 void dcp_free(struct dcp_align_buf *buf)
71 {
72 free(buf->data);
73 }
74
dcp_left_shift_buffer(uint8_t * input,uint8_t * result,size_t buffer_size)75 void dcp_left_shift_buffer(uint8_t *input, uint8_t *result, size_t buffer_size)
76 {
77 unsigned int i = 0;
78 uint8_t overflow = 0;
79
80 /* For each byte */
81 for (i = 0; i < buffer_size; i++) {
82 /* Left shift a bytes by one */
83 result[buffer_size - 1 - i] =
84 input[buffer_size - 1 - i] << 1 | overflow;
85
86 overflow = input[buffer_size - 1 - i] >> 7;
87 }
88 }
89
dcp_udelay(uint32_t time)90 void dcp_udelay(uint32_t time)
91 {
92 uint32_t counter = time * 500;
93
94 /* Implementation of a Software loop assuming CPU clock of 500MHz */
95 while (counter--) {
96 isb();
97 dsb();
98 };
99 }
100
dcp_reverse(uint8_t * in,uint8_t * out,size_t size)101 void dcp_reverse(uint8_t *in, uint8_t *out, size_t size)
102 {
103 unsigned int i = 0;
104
105 for (i = 0; i < size; i++)
106 out[i] = in[size - 1 - i];
107 }
108
dcp_xor(uint8_t * a,uint8_t * b,uint8_t * out,size_t size)109 void dcp_xor(uint8_t *a, uint8_t *b, uint8_t *out, size_t size)
110 {
111 unsigned int i = 0;
112
113 for (i = 0; i < size; i++)
114 out[i] = a[i] ^ b[i];
115 }
116
dcp_cmac_padding(uint8_t * block,size_t len)117 void dcp_cmac_padding(uint8_t *block, size_t len)
118 {
119 unsigned int i = 0;
120
121 for (i = len; i < DCP_AES128_BLOCK_SIZE; i++) {
122 if (i == len)
123 block[i] = BIT(7);
124 else
125 block[i] = 0x0;
126 }
127 }
128