1 /* 2 * Copyright (C) 2024 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN 24 * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define BTSTACK_FILE__ "btstack_ltv_builder.c" 39 40 #include "btstack_ltv_builder.h" 41 42 #include "btstack_debug.h" 43 #include "btstack_util.h" 44 45 #include <stdint.h> 46 #include <string.h> 47 48 static void btstack_ltv_builder_increase_tag(btstack_ltv_builder_context_t * context, uint16_t size){ 49 btstack_assert((context->write_pos + size) < context->size); 50 context->write_pos += size; 51 btstack_assert((((uint16_t)context->buffer[context->len_pos]) + size) <= 255); 52 context->buffer[context->len_pos] += size; 53 } 54 55 void btstack_ltv_builder_init(btstack_ltv_builder_context_t * context, uint8_t * buffer, uint16_t size){ 56 btstack_assert(buffer != NULL); 57 context->buffer = buffer; 58 context->size = size; 59 context->write_pos = 0; 60 } 61 62 uint16_t btstack_ltv_builder_remaining_space(btstack_ltv_builder_context_t * context){ 63 return context->size - context->write_pos; 64 } 65 66 uint16_t btstack_ltv_builder_get_length(btstack_ltv_builder_context_t * context){ 67 return context->write_pos; 68 } 69 70 void btstack_ltv_builder_add_tag(btstack_ltv_builder_context_t * context, uint8_t tag){ 71 // update write pos 72 btstack_assert((context->write_pos + 2) < context->size); 73 // track len field position 74 context->len_pos = context->write_pos; 75 // add empty tag 76 context->buffer[context->write_pos++] = 0; 77 context->buffer[context->write_pos++] = tag; 78 } 79 80 void btstack_ltv_builder_add_08(btstack_ltv_builder_context_t * context, uint8_t value){ 81 btstack_assert(context->write_pos != 0); 82 uint16_t write_pos = context->write_pos; 83 btstack_ltv_builder_increase_tag(context, 1); 84 context->buffer[write_pos] = value; 85 } 86 87 void btstack_ltv_builder_add_little_endian_16(btstack_ltv_builder_context_t * context, uint16_t value) { 88 btstack_assert(context->write_pos != 0); 89 uint16_t write_pos = context->write_pos; 90 btstack_ltv_builder_increase_tag(context, 2); 91 little_endian_store_16(context->buffer, write_pos, value); 92 } 93 94 void btstack_ltv_builder_add_little_endian_24(btstack_ltv_builder_context_t * context, uint32_t value) { 95 btstack_assert(context->write_pos != 0); 96 uint16_t write_pos = context->write_pos; 97 btstack_ltv_builder_increase_tag(context, 3); 98 little_endian_store_24(context->buffer, write_pos, value); 99 } 100 101 void btstack_ltv_builder_add_little_endian_32(btstack_ltv_builder_context_t * context, uint32_t value) { 102 btstack_assert(context->write_pos != 0); 103 uint16_t write_pos = context->write_pos; 104 btstack_ltv_builder_increase_tag(context, 4); 105 little_endian_store_32(context->buffer, write_pos, value); 106 } 107 108 void btstack_ltv_builder_add_bytes(btstack_ltv_builder_context_t * context, const uint8_t * data, uint16_t length) { 109 btstack_assert(context->write_pos != 0); 110 btstack_assert(data != NULL); 111 btstack_ltv_builder_increase_tag(context, length); 112 memcpy(&context->buffer[context->write_pos], data, length); 113 context->write_pos += length; 114 } 115 116 void btstack_ltv_builder_add_string(btstack_ltv_builder_context_t * context, const char * text) { 117 uint16_t length = (uint16_t)strlen(text); 118 btstack_ltv_builder_add_bytes(context, (const uint8_t *) text, length); 119 } 120