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 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 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 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 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 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 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 */ 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 */ 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 */ 169 void OpteeClientMemFree(void *mem) 170 { 171 my_free(mem); 172 } 173