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; 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 void my_malloc_init(void *start, uint32_t size) 26 { 27 memset(start, 0, size); 28 my_mem_start = start; 29 my_count = size/4096; 30 my_flag = malloc(size/4096); 31 memset(my_flag, 0, size/4096); 32 memset(alloc_flags, 0, 50 * sizeof(ALLOC_FLAG)); 33 } 34 35 void write_usedblock(void *addr, uint32_t size) 36 { 37 uint8_t k; 38 for (k = 0; k < 50; k++) { 39 if (alloc_flags[k].used == 0) { 40 alloc_flags[k].used = 1; 41 alloc_flags[k].addrBlock = addr; 42 alloc_flags[k].sizeBlock = size; 43 break; 44 } 45 } 46 } 47 48 uint32_t find_sizeblock(void *addr) 49 { 50 uint8_t k; 51 for (k = 0; k < 50; k++) 52 if (alloc_flags[k].used == 1 && 53 alloc_flags[k].addrBlock == addr) 54 return alloc_flags[k].sizeBlock; 55 56 return 0; 57 } 58 59 void free_usedblock(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 alloc_flags[k].used = 0; 66 alloc_flags[k].addrBlock = 0; 67 alloc_flags[k].sizeBlock = 0; 68 break; 69 } 70 } 71 } 72 73 void *my_malloc(uint32_t size) 74 { 75 uint32_t i, j, k, num; 76 77 num = (size - 1) / 4096 + 1; 78 79 for (i = 0; i < my_count - num; i++) { 80 if (*(my_flag + i) == 0) { 81 for (j = 0; j < num; j++) { 82 if (*(my_flag + i + j) != 0) 83 break; 84 } 85 if (j == num) { 86 for (k = 0; k < num; k++) { 87 *(my_flag + i + k) = 1; 88 memset(my_mem_start + 89 (i + k) * 4096, 0, 4096); 90 } 91 debug(" malloc is: 0x%X 0x%X\n", 92 (int)i, (int)num); 93 write_usedblock((my_mem_start + i * 4096), 94 num * 4096); 95 96 return my_mem_start + (i * 4096); 97 } 98 } 99 } 100 101 return 0; 102 } 103 104 void my_free(void *ptr) 105 { 106 uint32_t i, j, num, size; 107 108 if (ptr < my_mem_start) 109 return; 110 111 i = (ptr - my_mem_start) / 4096; 112 size = find_sizeblock(ptr); 113 free_usedblock(ptr); 114 if (size == 0) 115 return; 116 117 num = (size-1)/4096+1; 118 debug(" free is: 0x%X 0x%X\n", i, num); 119 120 for (j = 0; j < num; j++) { 121 *(my_flag + i + j) = 0; 122 memset(my_mem_start + (i + j) * 4096, 0, 4096); 123 } 124 } 125 126 /* 127 * Initlialize the memory component, for example providing the 128 * containing drivers handle. 129 */ 130 void OpteeClientMemInit(void) 131 { 132 ARM_SMC_ARGS ArmSmcArgs = {0}; 133 134 #ifdef CONFIG_OPTEE_V1 135 ArmSmcArgs.Arg0 = TEESMC32_OPTEE_FASTCALL_GET_SHM_CONFIG; 136 #endif 137 #ifdef CONFIG_OPTEE_V2 138 ArmSmcArgs.Arg0 = OPTEE_SMC_GET_SHM_CONFIG_V2; 139 #endif 140 141 tee_smc_call(&ArmSmcArgs); 142 143 printf("get share memory, arg0=0x%x arg1=0x%x arg2=0x%x arg3=0x%x\n", 144 ArmSmcArgs.Arg0, ArmSmcArgs.Arg1, ArmSmcArgs.Arg2, ArmSmcArgs.Arg3); 145 146 my_malloc_init((void *)(size_t)ArmSmcArgs.Arg1, ArmSmcArgs.Arg2); 147 } 148 149 /* 150 * Allocate a page aligned block of memory from the TrustZone 151 * shared memory block. 152 */ 153 void *OpteeClientMemAlloc(uint32_t length) 154 { 155 return my_malloc(length); 156 } 157 158 /* 159 * Free a block of memory previously allocated using the 160 * OpteeClientMemAlloc function. 161 */ 162 void OpteeClientMemFree(void *mem) 163 { 164 my_free(mem); 165 } 166