1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright 2020 NXP 4 */ 5 #include <dcp_utils.h> 6 #include <drivers/imx/dcp.h> 7 #include <kernel/cache_helpers.h> 8 #include <malloc.h> 9 #include <mm/core_memprot.h> 10 #include <string.h> 11 #include <trace.h> 12 13 /* 14 * Allocate an area of given size in bytes and set it to zero. Add the memory 15 * allocator information in the newly allocated area. 16 * Return a cacheline aligned buffer. 17 * 18 * @size Size in bytes to allocate 19 */ 20 static void *dcp_alloc_memalign(size_t size) 21 { 22 void *ptr = NULL; 23 size_t alloc_size = 0; 24 uint32_t cacheline_size = 0; 25 26 /* 27 * Buffer must be aligned on a cache line: 28 * - Buffer start address aligned on a cache line 29 * - End of Buffer inside a cache line. 30 * 31 * If the size to be allocated is already aligned on a cache line add a 32 * another cache line. 33 */ 34 cacheline_size = dcache_get_line_size(); 35 if (ROUNDUP_OVERFLOW(size, cacheline_size, &alloc_size)) 36 return NULL; 37 38 ptr = memalign(cacheline_size, alloc_size); 39 if (!ptr) 40 return NULL; 41 42 memset(ptr, 0, size); 43 44 return ptr; 45 } 46 47 TEE_Result dcp_calloc_align_buf(struct dcp_align_buf *buf, size_t size) 48 { 49 if (!buf) { 50 EMSG("Error, buf is null"); 51 return TEE_ERROR_BAD_PARAMETERS; 52 } 53 54 buf->data = dcp_alloc_memalign(size); 55 if (!buf->data) 56 return TEE_ERROR_OUT_OF_MEMORY; 57 58 buf->paddr = virt_to_phys(buf->data); 59 60 if (!buf->paddr) { 61 dcp_free(buf); 62 return TEE_ERROR_OUT_OF_MEMORY; 63 } 64 65 buf->size = size; 66 67 return TEE_SUCCESS; 68 } 69 70 void dcp_free(struct dcp_align_buf *buf) 71 { 72 free(buf->data); 73 } 74 75 void dcp_left_shift_buffer(uint8_t *input, uint8_t *result, size_t buffer_size) 76 { 77 unsigned int i = 0; 78 uint8_t overflow = 0; 79 80 /* For each byte */ 81 for (i = 0; i < buffer_size; i++) { 82 /* Left shift a bytes by one */ 83 result[buffer_size - 1 - i] = 84 input[buffer_size - 1 - i] << 1 | overflow; 85 86 overflow = input[buffer_size - 1 - i] >> 7; 87 } 88 } 89 90 void dcp_udelay(uint32_t time) 91 { 92 uint32_t counter = time * 500; 93 94 /* Implementation of a Software loop assuming CPU clock of 500MHz */ 95 while (counter--) { 96 isb(); 97 dsb(); 98 }; 99 } 100 101 void dcp_reverse(uint8_t *in, uint8_t *out, size_t size) 102 { 103 unsigned int i = 0; 104 105 for (i = 0; i < size; i++) 106 out[i] = in[size - 1 - i]; 107 } 108 109 void dcp_xor(uint8_t *a, uint8_t *b, uint8_t *out, size_t size) 110 { 111 unsigned int i = 0; 112 113 for (i = 0; i < size; i++) 114 out[i] = a[i] ^ b[i]; 115 } 116 117 void dcp_cmac_padding(uint8_t *block, size_t len) 118 { 119 unsigned int i = 0; 120 121 for (i = len; i < DCP_AES128_BLOCK_SIZE; i++) { 122 if (i == len) 123 block[i] = BIT(7); 124 else 125 block[i] = 0x0; 126 } 127 } 128