/*
|
* Copyright 2017, Rockchip Electronics Co., Ltd
|
* hisping lin, <hisping.lin@rock-chips.com>
|
*
|
* SPDX-License-Identifier: GPL-2.0+
|
*/
|
#include <common.h>
|
#include <stdlib.h>
|
#include <optee_include/OpteeClientMem.h>
|
#include <optee_include/OpteeClientRPC.h>
|
#include <optee_include/teesmc.h>
|
#include <optee_include/teesmc_optee.h>
|
#include <optee_include/teesmc_v2.h>
|
|
void *my_mem_start;
|
uint32_t my_count;
|
uint8_t *my_flag = NULL;
|
typedef struct {
|
void *addrBlock;
|
uint32_t sizeBlock;
|
uint8_t used;
|
} ALLOC_FLAG;
|
ALLOC_FLAG alloc_flags[50];
|
|
int my_malloc_init(void *start, uint32_t size)
|
{
|
if (start == NULL || size == 0) {
|
printf("TEEC: malloc init fail!\n");
|
return -1;
|
}
|
memset(start, 0, size);
|
my_mem_start = start;
|
my_count = size/4096;
|
if (my_flag == NULL) {
|
my_flag = malloc(size/4096);
|
if (my_flag == NULL) {
|
printf("TEEC: malloc fail!\n");
|
return -1;
|
}
|
}
|
memset(my_flag, 0, size/4096);
|
memset(alloc_flags, 0, 50 * sizeof(ALLOC_FLAG));
|
return 0;
|
}
|
|
void write_usedblock(void *addr, uint32_t size)
|
{
|
uint8_t k;
|
for (k = 0; k < 50; k++) {
|
if (alloc_flags[k].used == 0) {
|
alloc_flags[k].used = 1;
|
alloc_flags[k].addrBlock = addr;
|
alloc_flags[k].sizeBlock = size;
|
break;
|
}
|
}
|
}
|
|
uint32_t find_sizeblock(void *addr)
|
{
|
uint8_t k;
|
for (k = 0; k < 50; k++)
|
if (alloc_flags[k].used == 1 &&
|
alloc_flags[k].addrBlock == addr)
|
return alloc_flags[k].sizeBlock;
|
|
return 0;
|
}
|
|
void free_usedblock(void *addr)
|
{
|
uint8_t k;
|
for (k = 0; k < 50; k++) {
|
if (alloc_flags[k].used == 1 &&
|
alloc_flags[k].addrBlock == addr) {
|
alloc_flags[k].used = 0;
|
alloc_flags[k].addrBlock = 0;
|
alloc_flags[k].sizeBlock = 0;
|
break;
|
}
|
}
|
}
|
|
void *my_malloc(uint32_t size)
|
{
|
uint32_t i, j, k, num;
|
|
num = (size - 1) / 4096 + 1;
|
if (my_count < num)
|
return 0;
|
for (i = 0; i < my_count - num; i++) {
|
if (*(my_flag + i) == 0) {
|
for (j = 0; j < num; j++) {
|
if (*(my_flag + i + j) != 0)
|
break;
|
}
|
if (j == num) {
|
for (k = 0; k < num; k++) {
|
*(my_flag + i + k) = 1;
|
memset(my_mem_start +
|
(i + k) * 4096, 0, 4096);
|
}
|
debug("TEEC: malloc is: 0x%X 0x%X\n",
|
(int)i, (int)num);
|
write_usedblock((my_mem_start + i * 4096),
|
num * 4096);
|
|
return my_mem_start + (i * 4096);
|
}
|
}
|
}
|
|
return 0;
|
}
|
|
void my_free(void *ptr)
|
{
|
uint32_t i, j, num, size;
|
|
if (ptr < my_mem_start)
|
return;
|
|
i = (ptr - my_mem_start) / 4096;
|
size = find_sizeblock(ptr);
|
free_usedblock(ptr);
|
if (size == 0)
|
return;
|
|
num = (size-1)/4096+1;
|
debug("TEEC: free is: 0x%X 0x%X\n", i, num);
|
|
for (j = 0; j < num; j++) {
|
*(my_flag + i + j) = 0;
|
memset(my_mem_start + (i + j) * 4096, 0, 4096);
|
}
|
}
|
|
/*
|
* Initlialize the memory component, for example providing the
|
* containing drivers handle.
|
*/
|
int OpteeClientMemInit(void)
|
{
|
ARM_SMC_ARGS ArmSmcArgs = {0};
|
|
ArmSmcArgs.Arg0 = OPTEE_SMC_GET_SHM_CONFIG_V2;
|
|
tee_smc_call(&ArmSmcArgs);
|
|
debug("TEEC: get share memory, arg0=0x%x arg1=0x%x arg2=0x%x arg3=0x%x\n",
|
ArmSmcArgs.Arg0, ArmSmcArgs.Arg1, ArmSmcArgs.Arg2, ArmSmcArgs.Arg3);
|
|
return my_malloc_init((void *)(size_t)ArmSmcArgs.Arg1, ArmSmcArgs.Arg2);
|
}
|
|
/*
|
* Allocate a page aligned block of memory from the TrustZone
|
* shared memory block.
|
*/
|
void *OpteeClientMemAlloc(uint32_t length)
|
{
|
return my_malloc(length);
|
}
|
|
/*
|
* Free a block of memory previously allocated using the
|
* OpteeClientMemAlloc function.
|
*/
|
void OpteeClientMemFree(void *mem)
|
{
|
my_free(mem);
|
}
|
