diff --git a/src/ble/sm_mbedtls_allocator.c b/src/ble/sm_mbedtls_allocator.c new file mode 100644 index 000000000..5949f3a0a --- /dev/null +++ b/src/ble/sm_mbedtls_allocator.c @@ -0,0 +1,271 @@ +/* + * Copyright (C) 2014 BlueKitchen GmbH + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS + * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS + * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + +#include "btstack_config.h" + +#include +#include +#include + +#include "sm_mbedtls_allocator.h" +#include "btstack_util.h" + +#ifdef ENABLE_LE_SECURE_CONNECTIONS +#ifdef HAVE_HCI_CONTROLLER_DHKEY_SUPPORT +#error "Support for DHKEY Support in HCI Controller not implemented yet. Please use software implementation" +#else +#define USE_MBEDTLS_FOR_ECDH +#endif +#endif + +#ifdef USE_MBEDTLS_FOR_ECDH +#include "mbedtls/config.h" + +#ifndef HAVE_MALLOC + +size_t mbed_memory_allocated_current; +size_t mbed_memory_allocated_max; +size_t mbed_memory_max; +size_t mbed_memory_smallest_buffer = 0xfffffff; +int mbed_memory_num_allocations; + +// customized allocator for use with BTstack's Security Manager +// assumptions: +// - allocations are multiple of 8 +// - smallest allocation is 8 + +static uint32_t sm_allocator_size; +static uint8_t * sm_allocator_buffer; +static uint32_t sm_allocator_used; + +static sm_allocator_node_t * sm_mbedtls_node_for_offset(uint32_t offset){ + if (offset > sm_allocator_size){ + printf("Error! offset %u\n", offset); + } + return (sm_allocator_node_t *) (sm_allocator_buffer + offset); +} + +void sm_mbedtls_allocator_status(void){ +#ifdef DEBUG_ALLOCATIONS + uint32_t current_pos = 0; + sm_allocator_node_t * current = sm_mbedtls_node_for_offset(current_pos); + int i = 1; + size_t bytes_free = 0; + printf("SM Status:\n"); + while (1){ + printf("- SM ALLOC: Free %u: pos %u, size %u, next %u\n", i++, current_pos, current->size, current->next); + bytes_free += current->size; + current_pos = current->next; + current = sm_mbedtls_node_for_offset(current_pos); + if (current_pos == 0) break; + } + size_t overhead = mbed_memory_num_allocations * sizeof(void*); + printf("- Summary: Allocations %u. Current %lu (+ %zu = %zu used), Max %lu. Free %zu. Total: %zu\n", + mbed_memory_num_allocations, + mbed_memory_allocated_current, overhead, mbed_memory_allocated_current + overhead, + mbed_memory_allocated_max, bytes_free, bytes_free + mbed_memory_allocated_current + overhead ); +#endif +} + +static inline void sm_mbedtls_allocator_update_max(){ +} + +void * sm_mbedtls_allocator_calloc(size_t count, size_t size){ + size_t num_bytes = count * size; + size_t total = num_bytes + sizeof(void *); + mbed_memory_allocated_current += num_bytes; + mbed_memory_allocated_max = btstack_max(mbed_memory_allocated_max, mbed_memory_allocated_current); + mbed_memory_smallest_buffer = btstack_min(mbed_memory_smallest_buffer, num_bytes); + mbed_memory_num_allocations++; + + uint32_t prev_pos, current_pos, node_pos; + sm_allocator_node_t * prev, * current, * node; + + // find exact block + prev_pos = 0; + prev = sm_mbedtls_node_for_offset(prev_pos); + current_pos = prev->next; + while (current_pos){ + current = sm_mbedtls_node_for_offset(current_pos); + if (current->size == total){ + prev->next = current->next; + memset(current, 0, total); + *(uint32_t*)current = total; + sm_mbedtls_allocator_update_max(); +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_calloc(%zu,%zu) = total %zu -> pos %u\n", count, size, total, current_pos); +#endif + sm_mbedtls_allocator_status(); + return &sm_allocator_buffer[current_pos + sizeof(void *)]; + } + // next + prev = current; + prev_pos = current_pos; + current_pos = current->next; + } + // find first large enough block + prev_pos = 0; + prev = sm_mbedtls_node_for_offset(prev_pos); + current_pos = prev->next; + while (current_pos){ + current = sm_mbedtls_node_for_offset(current_pos); + if (current->size > total){ +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_calloc splitting block at %u, size %u (prev %u)\n", current_pos, current->size, prev_pos); +#endif + node_pos = current_pos + total; + node = sm_mbedtls_node_for_offset(node_pos); + node->next = current->next; + node->size = current->size - total; +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_calloc new block at %u, size %u\n", node_pos, node->size); +#endif + prev->next = node_pos; + memset(current, 0, total); + *(uint32_t*)current = total; + sm_mbedtls_allocator_update_max(); +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_calloc(%zu,%zu) = total %zu -> pos %u\n", count, size, total, current_pos); +#endif + sm_mbedtls_allocator_status(); + return &sm_allocator_buffer[current_pos + sizeof(void *)]; + } + // next + prev = current; + prev_pos = current_pos; + current_pos = current->next; + } + +#ifdef DEBUG_ALLOCATIONS + // failed to allocate + printf("sm_mbedtls_allocator_calloc error, no free chunk found!\n"); + exit(10); +#endif + // !!!! + return 0; +} + +void sm_mbedtls_allocator_free(void * data){ + uint32_t prev_pos, current_pos, next_pos; + sm_allocator_node_t * current, * prev, * next; + current = (sm_allocator_node_t*) (((uint8_t *) data) - sizeof(void *)); + current_pos = ((uint8_t *) current) - ((uint8_t *) sm_allocator_buffer); + size_t total = *(uint32_t*) current; + size_t num_bytes = total - sizeof(void *); + mbed_memory_allocated_current -= num_bytes; + mbed_memory_num_allocations--; + +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_free: pos %u, total %zu\n", current_pos, total); +#endif + // find previous node + prev_pos = 0; + prev = sm_mbedtls_node_for_offset(prev_pos); + while (prev->next < current_pos){ + prev_pos = prev->next; + prev = sm_mbedtls_node_for_offset(prev_pos); + } + + // setup new node + current->next = prev->next; + current->size = total; + prev->next = current_pos; + + // merge with previous ? +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_free: prev %u, %u\n", prev_pos, prev->size); +#endif + if (prev_pos + prev->size == current_pos){ +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_free: merge with previous\n"); +#endif + prev->size += current->size; + prev->next = current->next; + current = prev; + current_pos = prev_pos; + } + + // merge with next node? + next_pos = current->next; + if (current_pos + current->size == next_pos){ + next = sm_mbedtls_node_for_offset(next_pos); +#ifdef DEBUG_ALLOCATIONS + printf("sm_mbedtls_allocator_free: merge with next at pos %u, size %u\n", next_pos, next->size); +#endif + current->size += next->size; + current->next = next->next; + } + sm_mbedtls_allocator_status(); +} + +void sm_mbedtls_allocator_init(uint8_t * buffer, uint32_t size){ + sm_allocator_buffer = buffer; + sm_allocator_size = size; + sm_allocator_node_t * anchor = sm_mbedtls_node_for_offset(0); + anchor->next = sizeof(sm_allocator_node_t); + anchor->size = 0; + sm_allocator_node_t * first = sm_mbedtls_node_for_offset(anchor->next); + first->next = 0; + first->size = size - sizeof(sm_allocator_node_t); + sm_allocator_used = 2 * sizeof(sm_allocator_node_t); + sm_mbedtls_allocator_status(); +} + +#if 0 +void * sm_mbedtls_allocator_calloc(size_t count, size_t size){ + size_t total = count * size; + mbed_memory_allocated_current += total; + mbed_memory_allocated_max = btstack_max(mbed_memory_allocated_max, mbed_memory_allocated_current); + mbed_memory_smallest_buffer = btstack_min(mbed_memory_smallest_buffer, total); + mbed_memory_num_allocations++; + void * result = calloc(4 + total, 1); + *(uint32_t*) result = total; + printf("sm_mbedtls_allocator_calloc(%zu, %zu) -> res %p. Total %lu, Max %lu, Smallest %lu, Count %u\n", count, size, result, mbed_memory_allocated_current, mbed_memory_allocated_max, mbed_memory_smallest_buffer, mbed_memory_num_allocations); + return ((uint8_t *) result) + 4; +} + +void sm_mbedtls_allocator_free(void * data){ + void * orig = ((uint8_t *) data) - 4; + size_t total = *(uint32_t *)orig; + mbed_memory_allocated_current -= total; + mbed_memory_num_allocations--; + printf("sm_mbedtls_allocator_free(%p) - %zu bytes. Total %lu, Count %u\n", data, total, mbed_memory_allocated_current, mbed_memory_num_allocations); + free(orig); +} +#endif +#endif +#endif diff --git a/src/ble/sm_mbedtls_allocator.h b/src/ble/sm_mbedtls_allocator.h new file mode 100644 index 000000000..84ebb10e2 --- /dev/null +++ b/src/ble/sm_mbedtls_allocator.h @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2014 BlueKitchen GmbH + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS + * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS + * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + +#ifndef __SM_MBEDTLS_ALLOCATOR_H +#define __SM_MBEDTLS_ALLOCATOR_H + +#if defined __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct sm_allocator_node { + uint32_t next; // offset from sm_allocator_buffer, 0 == last item + uint32_t size; // size of free chunk incl. header +} sm_allocator_node_t; + +void sm_mbedtls_allocator_init(uint8_t * buffer, uint32_t size); +void sm_mbedtls_allocator_status(void); + +void * sm_mbedtls_allocator_calloc(size_t count, size_t size); +void sm_mbedtls_allocator_free(void * data); + +#if defined __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/test/security_manager/Makefile b/test/security_manager/Makefile index 62618afec..00457c96f 100644 --- a/test/security_manager/Makefile +++ b/test/security_manager/Makefile @@ -36,7 +36,7 @@ MBEDTLS = \ ecp_curves.c \ bignum.c \ -all: security_manager aestest ectest aes_cmac_test +all: security_manager aestest ectest aes_cmac_test sm_mbedtls_allocator_test security_manager: ${CORE_OBJ} ${COMMON_OBJ} security_manager.c ${CC} ${CORE_OBJ} ${COMMON_OBJ} security_manager.c ${CFLAGS} ${CPPFLAGS} ${LDFLAGS} -o $@ @@ -50,6 +50,9 @@ ectest: ectest.o ${MBEDTLS} rijndael.o aes_cmac_test: aes_cmac_test.o aes_cmac.o rijndael.o gcc ${CFLAGS} $^ -o $@ +sm_mbedtls_allocator_test: sm_mbedtls_allocator.o hci_dump.o btstack_util.o sm_mbedtls_allocator_test.c + ${CC} sm_mbedtls_allocator.o btstack_util.o hci_dump.o sm_mbedtls_allocator_test.c ${CFLAGS} ${CPPFLAGS} ${LDFLAGS} -o $@ + test: all ./security_manager ./aes_cmac_test diff --git a/test/security_manager/sm_mbedtls_allocator_test.c b/test/security_manager/sm_mbedtls_allocator_test.c new file mode 100644 index 000000000..b83fe8351 --- /dev/null +++ b/test/security_manager/sm_mbedtls_allocator_test.c @@ -0,0 +1,147 @@ +#include +#include +#include +#include + +#include "ble/sm_mbedtls_allocator.h" + +#include "CppUTest/TestHarness.h" +#include "CppUTest/CommandLineTestRunner.h" + +static uint8_t mbedtls_memory_buffer[5000]; // experimental value on 64-bit system + +uint32_t offset_for_buffer(void * buffer){ + return (uint8_t*)buffer - mbedtls_memory_buffer; +} + +TEST_GROUP(sm_mbedtls_allocator_test){ +}; + +TEST(sm_mbedtls_allocator_test, init){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + sm_allocator_node_t * anchor = (sm_allocator_node_t*) mbedtls_memory_buffer; + sm_allocator_node_t * first = (sm_allocator_node_t*) &mbedtls_memory_buffer[8]; + CHECK_EQUAL(8, anchor->next); + CHECK_EQUAL(0, anchor->size); + CHECK_EQUAL(0, first->next); + CHECK_EQUAL(sizeof(mbedtls_memory_buffer) - 8, first->size); +} + +TEST(sm_mbedtls_allocator_test, alloc1){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + CHECK_EQUAL(8 + sizeof(void*), offset_for_buffer(buffer1)); +} + +TEST(sm_mbedtls_allocator_test, alloc_free){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + sm_mbedtls_allocator_free(buffer1); + sm_allocator_node_t * anchor = (sm_allocator_node_t*) mbedtls_memory_buffer; + sm_allocator_node_t * first = (sm_allocator_node_t*) &mbedtls_memory_buffer[8]; + CHECK_EQUAL(8, anchor->next); + CHECK_EQUAL(0, anchor->size); + CHECK_EQUAL(0, first->next); + CHECK_EQUAL(sizeof(mbedtls_memory_buffer) - 8, first->size); +} + +TEST(sm_mbedtls_allocator_test, alloc2){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(1,8); + CHECK_EQUAL(8 + sizeof(void*) + 8 + sizeof(void*), offset_for_buffer(buffer2)); +} + +TEST(sm_mbedtls_allocator_test, alloc_free_2){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(1,8); + sm_mbedtls_allocator_free(buffer1); + sm_mbedtls_allocator_free(buffer2); + sm_allocator_node_t * anchor = (sm_allocator_node_t*) mbedtls_memory_buffer; + sm_allocator_node_t * first = (sm_allocator_node_t*) &mbedtls_memory_buffer[8]; + CHECK_EQUAL(8, anchor->next); + CHECK_EQUAL(0, anchor->size); + CHECK_EQUAL(0, first->next); + CHECK_EQUAL(sizeof(mbedtls_memory_buffer) - 8, first->size); +} + +TEST(sm_mbedtls_allocator_test, alloc_free_3){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(1,8); + sm_mbedtls_allocator_free(buffer2); + sm_mbedtls_allocator_free(buffer1); + sm_allocator_node_t * anchor = (sm_allocator_node_t*) mbedtls_memory_buffer; + CHECK_EQUAL(8, anchor->next); + CHECK_EQUAL(0, anchor->size); +} + +TEST(sm_mbedtls_allocator_test, alloc_alloc_free_alloc_1){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(1,8); + sm_mbedtls_allocator_free(buffer1); + buffer1 = sm_mbedtls_allocator_calloc(1,8); + CHECK_EQUAL(8 + sizeof(void*), offset_for_buffer(buffer1)); +} + +TEST(sm_mbedtls_allocator_test, alloc_alloc_free_alloc_2){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(1,8); + sm_mbedtls_allocator_free(buffer2); + buffer2 = sm_mbedtls_allocator_calloc(1,8); + CHECK_EQUAL(8 + sizeof(void*) + 8 + sizeof(void*), offset_for_buffer(buffer2)); +} + +TEST(sm_mbedtls_allocator_test, alloc_alloc_alloc_free_free_free_1){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(2,8); + void * buffer3 = sm_mbedtls_allocator_calloc(3,8); + sm_mbedtls_allocator_free(buffer1); + sm_mbedtls_allocator_free(buffer2); + sm_mbedtls_allocator_free(buffer3); + sm_allocator_node_t * anchor = (sm_allocator_node_t*) mbedtls_memory_buffer; + sm_allocator_node_t * first = (sm_allocator_node_t*) &mbedtls_memory_buffer[8]; + CHECK_EQUAL(8, anchor->next); + CHECK_EQUAL(0, anchor->size); + CHECK_EQUAL(0, first->next); + CHECK_EQUAL(sizeof(mbedtls_memory_buffer) - 8, first->size); +} + +TEST(sm_mbedtls_allocator_test, alloc_alloc_alloc_free_alloc_1){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(2,8); + void * buffer3 = sm_mbedtls_allocator_calloc(3,8); + sm_mbedtls_allocator_free(buffer1); + buffer1 = sm_mbedtls_allocator_calloc(1,8); + CHECK_EQUAL(8 + sizeof(void*), offset_for_buffer(buffer1)); +} + +TEST(sm_mbedtls_allocator_test, alloc_alloc_alloc_free_alloc_2){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(2,8); + void * buffer3 = sm_mbedtls_allocator_calloc(3,8); + sm_mbedtls_allocator_free(buffer2); + buffer2 = sm_mbedtls_allocator_calloc(1,8); + CHECK_EQUAL(8 + 2 * sizeof(void*) + 8, offset_for_buffer(buffer2)); +} + +TEST(sm_mbedtls_allocator_test, alloc_alloc_alloc_free_alloc_3){ + sm_mbedtls_allocator_init(mbedtls_memory_buffer, sizeof(mbedtls_memory_buffer)); + void * buffer1 = sm_mbedtls_allocator_calloc(1,8); + void * buffer2 = sm_mbedtls_allocator_calloc(2,8); + void * buffer3 = sm_mbedtls_allocator_calloc(3,8); + sm_mbedtls_allocator_free(buffer3); + buffer3 = sm_mbedtls_allocator_calloc(1,8); + CHECK_EQUAL(8 + 3 * sizeof(void*) + 8 + 16, offset_for_buffer(buffer3)); +} + +int main (int argc, const char * argv[]){ + return CommandLineTestRunner::RunAllTests(argc, argv); +} +