1 /*
2 * Copyright 2017, Rockchip Electronics Co., Ltd
3 * hisping lin, <hisping.lin@rock-chips.com>
4 *
5 * SPDX-License-Identifier: GPL-2.0+
6 */
7 #include <common.h>
8 #include <stdlib.h>
9 #include <optee_include/OpteeClientMem.h>
10 #include <optee_include/OpteeClientRPC.h>
11 #include <optee_include/teesmc.h>
12 #include <optee_include/teesmc_optee.h>
13 #include <optee_include/teesmc_v2.h>
14
15 void *my_mem_start;
16 uint32_t my_count;
17 uint8_t *my_flag = NULL;
18 typedef struct {
19 void *addrBlock;
20 uint32_t sizeBlock;
21 uint8_t used;
22 } ALLOC_FLAG;
23 ALLOC_FLAG alloc_flags[50];
24
my_malloc_init(void * start,uint32_t size)25 int my_malloc_init(void *start, uint32_t size)
26 {
27 if (start == NULL || size == 0) {
28 printf("TEEC: malloc init fail!\n");
29 return -1;
30 }
31 memset(start, 0, size);
32 my_mem_start = start;
33 my_count = size/4096;
34 if (my_flag == NULL) {
35 my_flag = malloc(size/4096);
36 if (my_flag == NULL) {
37 printf("TEEC: malloc fail!\n");
38 return -1;
39 }
40 }
41 memset(my_flag, 0, size/4096);
42 memset(alloc_flags, 0, 50 * sizeof(ALLOC_FLAG));
43 return 0;
44 }
45
write_usedblock(void * addr,uint32_t size)46 void write_usedblock(void *addr, uint32_t size)
47 {
48 uint8_t k;
49 for (k = 0; k < 50; k++) {
50 if (alloc_flags[k].used == 0) {
51 alloc_flags[k].used = 1;
52 alloc_flags[k].addrBlock = addr;
53 alloc_flags[k].sizeBlock = size;
54 break;
55 }
56 }
57 }
58
find_sizeblock(void * addr)59 uint32_t find_sizeblock(void *addr)
60 {
61 uint8_t k;
62 for (k = 0; k < 50; k++)
63 if (alloc_flags[k].used == 1 &&
64 alloc_flags[k].addrBlock == addr)
65 return alloc_flags[k].sizeBlock;
66
67 return 0;
68 }
69
free_usedblock(void * addr)70 void free_usedblock(void *addr)
71 {
72 uint8_t k;
73 for (k = 0; k < 50; k++) {
74 if (alloc_flags[k].used == 1 &&
75 alloc_flags[k].addrBlock == addr) {
76 alloc_flags[k].used = 0;
77 alloc_flags[k].addrBlock = 0;
78 alloc_flags[k].sizeBlock = 0;
79 break;
80 }
81 }
82 }
83
my_malloc(uint32_t size)84 void *my_malloc(uint32_t size)
85 {
86 uint32_t i, j, k, num;
87
88 num = (size - 1) / 4096 + 1;
89 if (my_count < num)
90 return 0;
91 for (i = 0; i < my_count - num; i++) {
92 if (*(my_flag + i) == 0) {
93 for (j = 0; j < num; j++) {
94 if (*(my_flag + i + j) != 0)
95 break;
96 }
97 if (j == num) {
98 for (k = 0; k < num; k++) {
99 *(my_flag + i + k) = 1;
100 memset(my_mem_start +
101 (i + k) * 4096, 0, 4096);
102 }
103 debug("TEEC: malloc is: 0x%X 0x%X\n",
104 (int)i, (int)num);
105 write_usedblock((my_mem_start + i * 4096),
106 num * 4096);
107
108 return my_mem_start + (i * 4096);
109 }
110 }
111 }
112
113 return 0;
114 }
115
my_free(void * ptr)116 void my_free(void *ptr)
117 {
118 uint32_t i, j, num, size;
119
120 if (ptr < my_mem_start)
121 return;
122
123 i = (ptr - my_mem_start) / 4096;
124 size = find_sizeblock(ptr);
125 free_usedblock(ptr);
126 if (size == 0)
127 return;
128
129 num = (size-1)/4096+1;
130 debug("TEEC: free is: 0x%X 0x%X\n", i, num);
131
132 for (j = 0; j < num; j++) {
133 *(my_flag + i + j) = 0;
134 memset(my_mem_start + (i + j) * 4096, 0, 4096);
135 }
136 }
137
138 /*
139 * Initlialize the memory component, for example providing the
140 * containing drivers handle.
141 */
OpteeClientMemInit(void)142 int OpteeClientMemInit(void)
143 {
144 ARM_SMC_ARGS ArmSmcArgs = {0};
145
146 ArmSmcArgs.Arg0 = OPTEE_SMC_GET_SHM_CONFIG_V2;
147
148 tee_smc_call(&ArmSmcArgs);
149
150 debug("TEEC: get share memory, arg0=0x%x arg1=0x%x arg2=0x%x arg3=0x%x\n",
151 ArmSmcArgs.Arg0, ArmSmcArgs.Arg1, ArmSmcArgs.Arg2, ArmSmcArgs.Arg3);
152
153 return my_malloc_init((void *)(size_t)ArmSmcArgs.Arg1, ArmSmcArgs.Arg2);
154 }
155
156 /*
157 * Allocate a page aligned block of memory from the TrustZone
158 * shared memory block.
159 */
OpteeClientMemAlloc(uint32_t length)160 void *OpteeClientMemAlloc(uint32_t length)
161 {
162 return my_malloc(length);
163 }
164
165 /*
166 * Free a block of memory previously allocated using the
167 * OpteeClientMemAlloc function.
168 */
OpteeClientMemFree(void * mem)169 void OpteeClientMemFree(void *mem)
170 {
171 my_free(mem);
172 }
173