S Morita / mbed-mros2

Dependents:   mbed-os-example-mros2 example-mbed-mros2-sub-pose example-mbed-mros2-pub-twist example-mbed-mros2-mturtle-teleop

embeddedRTPS/thirdparty/Micro-CDR/src/c/common.c@7:c80f65422d99, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 09:23:37 2022 +0900
Revision:
7:c80f65422d99
Parent:
0:580aba13d1a1 
Merge test_assortment_of_msgs branch.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:580aba13d1a1 1 // Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
smoritaemb 0:580aba13d1a1 2 //
smoritaemb 0:580aba13d1a1 3 // Licensed under the Apache License, Version 2.0 (the "License");
smoritaemb 0:580aba13d1a1 4 // you may not use this file except in compliance with the License.
smoritaemb 0:580aba13d1a1 5 // You may obtain a copy of the License at
smoritaemb 0:580aba13d1a1 6 //
smoritaemb 0:580aba13d1a1 7 // http://www.apache.org/licenses/LICENSE-2.0
smoritaemb 0:580aba13d1a1 8 //
smoritaemb 0:580aba13d1a1 9 // Unless required by applicable law or agreed to in writing, software
smoritaemb 0:580aba13d1a1 10 // distributed under the License is distributed on an "AS IS" BASIS,
smoritaemb 0:580aba13d1a1 11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
smoritaemb 0:580aba13d1a1 12 // See the License for the specific language governing permissions and
smoritaemb 0:580aba13d1a1 13 // limitations under the License.
smoritaemb 0:580aba13d1a1 14
smoritaemb 0:580aba13d1a1 15 #include <ucdr/common.h>
smoritaemb 0:580aba13d1a1 16
smoritaemb 0:580aba13d1a1 17 #include <string.h>
smoritaemb 0:580aba13d1a1 18
smoritaemb 0:580aba13d1a1 19 #if __BIG_ENDIAN__
smoritaemb 0:580aba13d1a1 20 const ucdrEndianness UCDR_MACHINE_ENDIANNESS = UCDR_BIG_ENDIANNESS;
smoritaemb 0:580aba13d1a1 21 #else
smoritaemb 0:580aba13d1a1 22 const ucdrEndianness UCDR_MACHINE_ENDIANNESS = UCDR_LITTLE_ENDIANNESS;
smoritaemb 0:580aba13d1a1 23 #endif
smoritaemb 0:580aba13d1a1 24
smoritaemb 0:580aba13d1a1 25 // -------------------------------------------------------------------
smoritaemb 0:580aba13d1a1 26 // INTERNAL UTIL IMPLEMENTATIONS
smoritaemb 0:580aba13d1a1 27 // -------------------------------------------------------------------
smoritaemb 0:580aba13d1a1 28 bool ucdr_check_buffer(ucdrBuffer* mb, const uint32_t bytes)
smoritaemb 0:580aba13d1a1 29 {
smoritaemb 0:580aba13d1a1 30 if(!mb->error)
smoritaemb 0:580aba13d1a1 31 {
smoritaemb 0:580aba13d1a1 32 bool fit = mb->iterator + bytes <= mb->final;
smoritaemb 0:580aba13d1a1 33 if(!fit)
smoritaemb 0:580aba13d1a1 34 {
smoritaemb 0:580aba13d1a1 35 mb->error = true;
smoritaemb 0:580aba13d1a1 36 }
smoritaemb 0:580aba13d1a1 37 }
smoritaemb 0:580aba13d1a1 38
smoritaemb 0:580aba13d1a1 39 return !mb->error;
smoritaemb 0:580aba13d1a1 40 }
smoritaemb 0:580aba13d1a1 41
smoritaemb 0:580aba13d1a1 42 // -------------------------------------------------------------------
smoritaemb 0:580aba13d1a1 43 // PUBLIC IMPLEMENTATION
smoritaemb 0:580aba13d1a1 44 // -------------------------------------------------------------------
smoritaemb 0:580aba13d1a1 45 void ucdr_init_buffer(ucdrBuffer* mb, const uint8_t* data, const uint32_t size)
smoritaemb 0:580aba13d1a1 46 {
smoritaemb 0:580aba13d1a1 47 ucdr_init_buffer_offset(mb, data, size, 0U);
smoritaemb 0:580aba13d1a1 48 }
smoritaemb 0:580aba13d1a1 49
smoritaemb 0:580aba13d1a1 50 void ucdr_init_buffer_offset(ucdrBuffer* mb, const uint8_t* data, const uint32_t size, uint32_t offset)
smoritaemb 0:580aba13d1a1 51 {
smoritaemb 0:580aba13d1a1 52 ucdr_init_buffer_offset_endian(mb, data, size, offset, UCDR_MACHINE_ENDIANNESS);
smoritaemb 0:580aba13d1a1 53 }
smoritaemb 0:580aba13d1a1 54
smoritaemb 0:580aba13d1a1 55 void ucdr_init_buffer_offset_endian(ucdrBuffer* mb, const uint8_t* data, const uint32_t size, uint32_t offset, ucdrEndianness endianness)
smoritaemb 0:580aba13d1a1 56 {
smoritaemb 0:580aba13d1a1 57 mb->init = data;
smoritaemb 0:580aba13d1a1 58 mb->final = mb->init + size;
smoritaemb 0:580aba13d1a1 59 mb->iterator = (uint8_t*)mb->init + offset;
smoritaemb 0:580aba13d1a1 60 mb->last_data_size = 0U;
smoritaemb 0:580aba13d1a1 61 mb->endianness = endianness;
smoritaemb 0:580aba13d1a1 62 mb->error = false;
smoritaemb 0:580aba13d1a1 63 }
smoritaemb 0:580aba13d1a1 64
smoritaemb 0:580aba13d1a1 65
smoritaemb 0:580aba13d1a1 66 void ucdr_copy_buffer(ucdrBuffer* mb_dest, const ucdrBuffer* mb_source)
smoritaemb 0:580aba13d1a1 67 {
smoritaemb 0:580aba13d1a1 68 memcpy(mb_dest, mb_source, sizeof(ucdrBuffer));
smoritaemb 0:580aba13d1a1 69 }
smoritaemb 0:580aba13d1a1 70
smoritaemb 0:580aba13d1a1 71 void ucdr_reset_buffer(ucdrBuffer* mb)
smoritaemb 0:580aba13d1a1 72 {
smoritaemb 0:580aba13d1a1 73 ucdr_reset_buffer_offset(mb, 0U);
smoritaemb 0:580aba13d1a1 74 }
smoritaemb 0:580aba13d1a1 75
smoritaemb 0:580aba13d1a1 76 void ucdr_reset_buffer_offset(ucdrBuffer* mb, const uint32_t offset)
smoritaemb 0:580aba13d1a1 77 {
smoritaemb 0:580aba13d1a1 78 mb->iterator = (uint8_t*) mb->init + offset;
smoritaemb 0:580aba13d1a1 79 mb->last_data_size = 0U;
smoritaemb 0:580aba13d1a1 80 mb->error = false;
smoritaemb 0:580aba13d1a1 81 }
smoritaemb 0:580aba13d1a1 82
smoritaemb 0:580aba13d1a1 83 void ucdr_align_to(ucdrBuffer* mb, const uint32_t size)
smoritaemb 0:580aba13d1a1 84 {
smoritaemb 0:580aba13d1a1 85 uint32_t offset = ucdr_buffer_alignment(mb, size);
smoritaemb 0:580aba13d1a1 86 mb->iterator += offset;
smoritaemb 0:580aba13d1a1 87 if(mb->iterator > mb->final)
smoritaemb 0:580aba13d1a1 88 {
smoritaemb 0:580aba13d1a1 89 mb->iterator = (uint8_t*)mb->final;
smoritaemb 0:580aba13d1a1 90 }
smoritaemb 0:580aba13d1a1 91
smoritaemb 0:580aba13d1a1 92 mb->last_data_size = size;
smoritaemb 0:580aba13d1a1 93 }
smoritaemb 0:580aba13d1a1 94
smoritaemb 0:580aba13d1a1 95 uint32_t ucdr_alignment(uint32_t current_alignment, const uint32_t data_size)
smoritaemb 0:580aba13d1a1 96 {
smoritaemb 0:580aba13d1a1 97 return ((data_size - (current_alignment % data_size)) & (data_size - 1));
smoritaemb 0:580aba13d1a1 98 }
smoritaemb 0:580aba13d1a1 99
smoritaemb 0:580aba13d1a1 100 uint32_t ucdr_buffer_alignment(const ucdrBuffer* mb, const uint32_t data_size)
smoritaemb 0:580aba13d1a1 101 {
smoritaemb 0:580aba13d1a1 102 if(data_size > mb->last_data_size)
smoritaemb 0:580aba13d1a1 103 {
smoritaemb 0:580aba13d1a1 104 return (data_size - ((uint32_t)(mb->iterator - mb->init) % data_size)) & (data_size - 1);
smoritaemb 0:580aba13d1a1 105 }
smoritaemb 0:580aba13d1a1 106
smoritaemb 0:580aba13d1a1 107 return 0;
smoritaemb 0:580aba13d1a1 108 }
smoritaemb 0:580aba13d1a1 109
smoritaemb 0:580aba13d1a1 110 size_t ucdr_buffer_size(const ucdrBuffer* mb)
smoritaemb 0:580aba13d1a1 111 {
smoritaemb 0:580aba13d1a1 112 return (size_t)(mb->final - mb->init);
smoritaemb 0:580aba13d1a1 113 }
smoritaemb 0:580aba13d1a1 114
smoritaemb 0:580aba13d1a1 115 size_t ucdr_buffer_length(const ucdrBuffer* mb)
smoritaemb 0:580aba13d1a1 116 {
smoritaemb 0:580aba13d1a1 117 return (size_t)(mb->iterator - mb->init);
smoritaemb 0:580aba13d1a1 118 }
smoritaemb 0:580aba13d1a1 119
smoritaemb 0:580aba13d1a1 120 size_t ucdr_buffer_remaining(const ucdrBuffer* mb)
smoritaemb 0:580aba13d1a1 121 {
smoritaemb 0:580aba13d1a1 122 return (size_t)(mb->final - mb->iterator);
smoritaemb 0:580aba13d1a1 123 }
smoritaemb 0:580aba13d1a1 124
smoritaemb 0:580aba13d1a1 125 ucdrEndianness ucdr_buffer_endianness(const ucdrBuffer* mb)
smoritaemb 0:580aba13d1a1 126 {
smoritaemb 0:580aba13d1a1 127 return mb->endianness;
smoritaemb 0:580aba13d1a1 128 }
smoritaemb 0:580aba13d1a1 129
smoritaemb 0:580aba13d1a1 130 bool ucdr_buffer_has_error(const ucdrBuffer* mb)
smoritaemb 0:580aba13d1a1 131 {
smoritaemb 0:580aba13d1a1 132 return mb->error;
smoritaemb 0:580aba13d1a1 133 }