diff --git a/src/btstack_util.c b/src/btstack_util.c index 6f769a7f0..f160007e9 100644 --- a/src/btstack_util.c +++ b/src/btstack_util.c @@ -559,3 +559,38 @@ void btstack_strcat(char * dst, uint16_t dst_size, const char * src){ (void) memcpy( &dst[dst_len], src, bytes_to_copy); dst[dst_len + bytes_to_copy] = 0; } + +uint16_t btstack_virtual_memcpy( + const uint8_t * field_data, uint16_t field_len, uint16_t field_offset, // position of field in complete data block + uint8_t * buffer, uint16_t buffer_size, uint16_t buffer_offset){ + + uint16_t after_buffer = buffer_offset + buffer_size ; + // bail before buffer + if ((field_offset + field_len) < buffer_offset){ + return 0; + } + // bail after buffer + if (field_offset >= after_buffer){ + return 0; + } + // calc overlap + uint16_t bytes_to_copy = field_len; + + uint16_t skip_at_start = 0; + if (field_offset < buffer_offset){ + skip_at_start = buffer_offset - field_offset; + bytes_to_copy -= skip_at_start; + } + + uint16_t skip_at_end = 0; + if ((field_offset + field_len) > after_buffer){ + skip_at_end = (field_offset + field_len) - after_buffer; + bytes_to_copy -= skip_at_end; + } + + btstack_assert((skip_at_end + skip_at_start) <= field_len); + btstack_assert(bytes_to_copy <= field_len); + + memcpy(&buffer[(field_offset + skip_at_start) - buffer_offset], &field_data[skip_at_start], bytes_to_copy); + return bytes_to_copy; +} \ No newline at end of file diff --git a/src/btstack_util.h b/src/btstack_util.h index e7c1cf72c..3b6cf66a1 100644 --- a/src/btstack_util.h +++ b/src/btstack_util.h @@ -343,6 +343,36 @@ void btstack_strcat(char * dst, uint16_t dst_size, const char * src); */ uint8_t btstack_clz(uint32_t value); +/** + * @brief Copy chunk of data into a virtual buffer backed by a physical buffer. + * Used to provide chunk of data of larger buffer that is constructed on the fly, e.g. serializing data struct + * + * For example, copy field2 to buffer, with buffer_offset = 11 + * + * field1 field2 field3 field4 field5 filed6 + * struct: -------|-------|----------|-------------|-------|-------------- + * buffer: ------------------ + * result: ----| + * + * When also copying field3 and field4 to buffer, with buffer_offset = 11 + * + * field1 field2 field3 field4 field5 filed6 + * struct: -------|-------|----------|-------------|-------|-------------- + * buffer: ------------------ + * result: ----|----------|-- + * + * @param field_data + * @param field_len + * @param field_offset position of field in complete data block + * @param buffer_data + * @param buffer_len + * @param buffer_offset position of buffer in complete data block + * @return bytes_copied number of bytes actually stored in buffer + */ +uint16_t btstack_virtual_memcpy( + const uint8_t * field_data, uint16_t field_len, uint16_t field_offset, + uint8_t * buffer, uint16_t buffer_size, uint16_t buffer_offset); + /* API_END */