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 11 void *my_mem_start; 12 uint32_t my_count; 13 uint8_t *my_flag; 14 typedef struct { 15 void *addrBlock; 16 uint32_t sizeBlock; 17 uint8_t used; 18 } ALLOC_FLAG; 19 ALLOC_FLAG alloc_flags[50]; 20 21 void my_malloc_init(void *start, uint32_t size) 22 { 23 memset(start, 0, size); 24 my_mem_start = start; 25 my_count = size/4096; 26 my_flag = malloc(size/4096); 27 memset(my_flag, 0, size/4096); 28 memset(alloc_flags, 0, 50 * sizeof(ALLOC_FLAG)); 29 } 30 31 void write_usedblock(void *addr, uint32_t size) 32 { 33 uint8_t k; 34 for (k = 0; k < 50; k++) { 35 if (alloc_flags[k].used == 0) { 36 alloc_flags[k].used = 1; 37 alloc_flags[k].addrBlock = addr; 38 alloc_flags[k].sizeBlock = size; 39 break; 40 } 41 } 42 } 43 44 uint32_t find_sizeblock(void *addr) 45 { 46 uint8_t k; 47 for (k = 0; k < 50; k++) 48 if (alloc_flags[k].used == 1 && 49 alloc_flags[k].addrBlock == addr) 50 return alloc_flags[k].sizeBlock; 51 52 return 0; 53 } 54 55 void free_usedblock(void *addr) 56 { 57 uint8_t k; 58 for (k = 0; k < 50; k++) { 59 if (alloc_flags[k].used == 1 && 60 alloc_flags[k].addrBlock == addr) { 61 alloc_flags[k].used = 0; 62 alloc_flags[k].addrBlock = 0; 63 alloc_flags[k].sizeBlock = 0; 64 break; 65 } 66 } 67 } 68 69 void *my_malloc(uint32_t size) 70 { 71 uint32_t i, j, k, num; 72 73 num = (size - 1) / 4096 + 1; 74 75 for (i = 0; i < my_count - num; i++) { 76 if (*(my_flag + i) == 0) { 77 for (j = 0; j < num; j++) { 78 if (*(my_flag + i + j) != 0) 79 break; 80 } 81 if (j == num) { 82 for (k = 0; k < num; k++) { 83 *(my_flag + i + k) = 1; 84 memset(my_mem_start + 85 (i + k) * 4096, 0, 4096); 86 } 87 debug(" malloc is: 0x%X 0x%X\n", 88 (int)i, (int)num); 89 write_usedblock((my_mem_start + i * 4096), 90 num * 4096); 91 92 return my_mem_start + (i * 4096); 93 } 94 } 95 } 96 97 return 0; 98 } 99 100 void my_free(void *ptr) 101 { 102 uint32_t i, j, num, size; 103 104 if (ptr < my_mem_start) 105 return; 106 107 i = (ptr - my_mem_start) / 4096; 108 size = find_sizeblock(ptr); 109 free_usedblock(ptr); 110 if (size == 0) 111 return; 112 113 num = (size-1)/4096+1; 114 debug(" free is: 0x%X 0x%X\n", i, num); 115 116 for (j = 0; j < num; j++) { 117 *(my_flag + i + j) = 0; 118 memset(my_mem_start + (i + j) * 4096, 0, 4096); 119 } 120 } 121 122 /* 123 * Initlialize the memory component, for example providing the 124 * containing drivers handle. 125 */ 126 void OpteeClientMemInit(void) 127 { 128 #ifdef CONFIG_ROCKCHIP_RK3328 129 debug(" OpteeClientMemInit 64\n"); 130 my_malloc_init(0x09200000, 0x00200000); 131 #endif 132 #ifdef CONFIG_ROCKCHIP_RK322X 133 debug(" OpteeClientMemInit 32\n"); 134 my_malloc_init((void *)0x6910a000, 0x000e0000); 135 #endif 136 } 137 138 /* 139 * Allocate a page aligned block of memory from the TrustZone 140 * shared memory block. 141 */ 142 void *OpteeClientMemAlloc(uint32_t length) 143 { 144 return my_malloc(length); 145 } 146 147 /* 148 * Free a block of memory previously allocated using the 149 * OpteeClientMemAlloc function. 150 */ 151 void OpteeClientMemFree(void *mem) 152 { 153 my_free(mem); 154 } 155