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
btstack_ltv_builder_increase_tag(btstack_ltv_builder_context_t * context,uint16_t size)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
btstack_ltv_builder_init(btstack_ltv_builder_context_t * context,uint8_t * buffer,uint16_t size)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
btstack_ltv_builder_remaining_space(btstack_ltv_builder_context_t * context)62 uint16_t btstack_ltv_builder_remaining_space(btstack_ltv_builder_context_t * context){
63 return context->size - context->write_pos;
64 }
65
btstack_ltv_builder_get_length(btstack_ltv_builder_context_t * context)66 uint16_t btstack_ltv_builder_get_length(btstack_ltv_builder_context_t * context){
67 return context->write_pos;
68 }
69
btstack_ltv_builder_add_tag(btstack_ltv_builder_context_t * context,uint8_t tag)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
btstack_ltv_builder_add_08(btstack_ltv_builder_context_t * context,uint8_t value)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
btstack_ltv_builder_add_little_endian_16(btstack_ltv_builder_context_t * context,uint16_t value)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
btstack_ltv_builder_add_little_endian_24(btstack_ltv_builder_context_t * context,uint32_t value)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
btstack_ltv_builder_add_little_endian_32(btstack_ltv_builder_context_t * context,uint32_t value)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
btstack_ltv_builder_add_bytes(btstack_ltv_builder_context_t * context,const uint8_t * data,uint16_t length)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
btstack_ltv_builder_add_string(btstack_ltv_builder_context_t * context,const char * text)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