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/tee_misc.h> 8 #include <malloc.h> 9 #include <mm/core_memprot.h> 10 #include <string.h> 11 #include <trace.h> 12 13 TEE_Result dcp_calloc_align_buf(struct dcp_align_buf *buf, size_t size) 14 { 15 if (!buf) { 16 EMSG("Error, buf is null"); 17 return TEE_ERROR_BAD_PARAMETERS; 18 } 19 20 buf->data = alloc_cache_aligned(size); 21 if (!buf->data) 22 return TEE_ERROR_OUT_OF_MEMORY; 23 24 buf->paddr = virt_to_phys(buf->data); 25 26 if (!buf->paddr) { 27 dcp_free(buf); 28 return TEE_ERROR_OUT_OF_MEMORY; 29 } 30 31 buf->size = size; 32 33 return TEE_SUCCESS; 34 } 35 36 void dcp_free(struct dcp_align_buf *buf) 37 { 38 free(buf->data); 39 } 40 41 void dcp_left_shift_buffer(uint8_t *input, uint8_t *result, size_t buffer_size) 42 { 43 unsigned int i = 0; 44 uint8_t overflow = 0; 45 46 /* For each byte */ 47 for (i = 0; i < buffer_size; i++) { 48 /* Left shift a bytes by one */ 49 result[buffer_size - 1 - i] = 50 input[buffer_size - 1 - i] << 1 | overflow; 51 52 overflow = input[buffer_size - 1 - i] >> 7; 53 } 54 } 55 56 void dcp_udelay(uint32_t time) 57 { 58 uint32_t counter = time * 500; 59 60 /* Implementation of a Software loop assuming CPU clock of 500MHz */ 61 while (counter--) { 62 isb(); 63 dsb(); 64 }; 65 } 66 67 void dcp_reverse(uint8_t *in, uint8_t *out, size_t size) 68 { 69 unsigned int i = 0; 70 71 for (i = 0; i < size; i++) 72 out[i] = in[size - 1 - i]; 73 } 74 75 void dcp_xor(uint8_t *a, uint8_t *b, uint8_t *out, size_t size) 76 { 77 unsigned int i = 0; 78 79 for (i = 0; i < size; i++) 80 out[i] = a[i] ^ b[i]; 81 } 82 83 void dcp_cmac_padding(uint8_t *block, size_t len) 84 { 85 unsigned int i = 0; 86 87 for (i = len; i < DCP_AES128_BLOCK_SIZE; i++) { 88 if (i == len) 89 block[i] = BIT(7); 90 else 91 block[i] = 0x0; 92 } 93 } 94