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_tlv_builder.c"
39
40 #include "btstack_tlv_builder.h"
41
42 #include "btstack_bool.h"
43 #include "btstack_config.h"
44 #include "btstack_debug.h"
45 #include "btstack_util.h"
46
47 #include <stdint.h>
48 #include <string.h>
49
btstack_tlv_builder_increase_tag(btstack_tlv_builder_context_t * context,uint16_t size)50 static void btstack_tlv_builder_increase_tag(btstack_tlv_builder_context_t * context, uint16_t size){
51 btstack_assert((context->write_pos + size) < context->size);
52 context->write_pos += size;
53 btstack_assert((((uint16_t)context->buffer[context->len_pos]) + size) <= 255);
54 context->buffer[context->len_pos] += size;
55 }
56
btstack_tlv_builder_init(btstack_tlv_builder_context_t * context,uint8_t * buffer,uint16_t size)57 void btstack_tlv_builder_init(btstack_tlv_builder_context_t * context, uint8_t * buffer, uint16_t size){
58 btstack_assert(buffer != NULL);
59 context->buffer = buffer;
60 context->size = size;
61 context->write_pos = 0;
62 }
63
btstack_tlv_builder_remaining_space(btstack_tlv_builder_context_t * context)64 uint16_t btstack_tlv_builder_remaining_space(btstack_tlv_builder_context_t * context){
65 return context->size - context->write_pos;
66 }
67
btstack_tlv_builder_get_length(btstack_tlv_builder_context_t * context)68 uint16_t btstack_tlv_builder_get_length(btstack_tlv_builder_context_t * context){
69 return context->write_pos;
70 }
71
btstack_tlv_builder_add_tag(btstack_tlv_builder_context_t * context,uint8_t tag)72 void btstack_tlv_builder_add_tag(btstack_tlv_builder_context_t * context, uint8_t tag){
73 // update write pos
74 btstack_assert((context->write_pos + 2) < context->size);
75 // track len pos
76 context->buffer[context->write_pos++] = tag;
77 // add empty tag
78 context->len_pos = context->write_pos;
79 context->buffer[context->write_pos++] = 0;
80 }
81
btstack_tlv_builder_add_08(btstack_tlv_builder_context_t * context,uint8_t value)82 void btstack_tlv_builder_add_08(btstack_tlv_builder_context_t * context, uint8_t value){
83 btstack_assert(context->write_pos != 0);
84 uint16_t write_pos = context->write_pos;
85 btstack_tlv_builder_increase_tag(context, 1);
86 context->buffer[write_pos] = value;
87 }
88
btstack_tlv_builder_add_big_endian_16(btstack_tlv_builder_context_t * context,uint16_t value)89 void btstack_tlv_builder_add_big_endian_16(btstack_tlv_builder_context_t * context, uint16_t value) {
90 btstack_assert(context->write_pos != 0);
91 uint16_t write_pos = context->write_pos;
92 btstack_tlv_builder_increase_tag(context, 2);
93 big_endian_store_16(context->buffer, write_pos, value);
94 }
95
btstack_tlv_builder_add_big_endian_24(btstack_tlv_builder_context_t * context,uint32_t value)96 void btstack_tlv_builder_add_big_endian_24(btstack_tlv_builder_context_t * context, uint32_t value) {
97 btstack_assert(context->write_pos != 0);
98 uint16_t write_pos = context->write_pos;
99 btstack_tlv_builder_increase_tag(context, 3);
100 big_endian_store_24(context->buffer, write_pos, value);
101 }
102
btstack_tlv_builder_add_big_endian_32(btstack_tlv_builder_context_t * context,uint32_t value)103 void btstack_tlv_builder_add_big_endian_32(btstack_tlv_builder_context_t * context, uint32_t value) {
104 btstack_assert(context->write_pos != 0);
105 uint16_t write_pos = context->write_pos;
106 btstack_tlv_builder_increase_tag(context, 4);
107 big_endian_store_32(context->buffer, write_pos, value);
108 }
109
btstack_tlv_builder_add_bytes(btstack_tlv_builder_context_t * context,const uint8_t * data,uint16_t length)110 void btstack_tlv_builder_add_bytes(btstack_tlv_builder_context_t * context, const uint8_t * data, uint16_t length) {
111 btstack_assert(context->write_pos != 0);
112 btstack_assert(data != NULL);
113 btstack_tlv_builder_increase_tag(context, length);
114 memcpy(&context->buffer[context->write_pos], data, length);
115 context->write_pos += length;
116 }
117
btstack_tlv_builder_add_string(btstack_tlv_builder_context_t * context,const char * text)118 void btstack_tlv_builder_add_string(btstack_tlv_builder_context_t * context, const char * text) {
119 uint16_t length = (uint16_t)strlen(text);
120 btstack_tlv_builder_add_bytes(context, (const uint8_t *) text, length);
121 }
122