diff --git a/components/bt/esp_ble_mesh/mesh_common/include/mesh_buf.h b/components/bt/esp_ble_mesh/mesh_common/include/mesh_buf.h index f990c2b354..10cd305a9d 100644 --- a/components/bt/esp_ble_mesh/mesh_common/include/mesh_buf.h +++ b/components/bt/esp_ble_mesh/mesh_common/include/mesh_buf.h @@ -313,6 +313,30 @@ void net_buf_simple_add_le48(struct net_buf_simple *buf, u64_t val); */ void net_buf_simple_add_be48(struct net_buf_simple *buf, u64_t val); +/** + * @brief Add 64-bit value at the end of the buffer + * + * Adds 64-bit value in little endian format at the end of buffer. + * Increments the data length of a buffer to account for more data + * at the end. + * + * @param buf Buffer to update. + * @param val 64-bit value to be added. + */ +void net_buf_simple_add_le64(struct net_buf_simple *buf, u64_t val); + +/** + * @brief Add 64-bit value at the end of the buffer + * + * Adds 64-bit value in big endian format at the end of buffer. + * Increments the data length of a buffer to account for more data + * at the end. + * + * @param buf Buffer to update. + * @param val 64-bit value to be added. + */ +void net_buf_simple_add_be64(struct net_buf_simple *buf, u64_t val); + /** * @brief Push data to the beginning of the buffer. * @@ -492,6 +516,30 @@ u64_t net_buf_simple_pull_le48(struct net_buf_simple *buf); */ u64_t net_buf_simple_pull_be48(struct net_buf_simple *buf); +/** + * @brief Remove and convert 64 bits from the beginning of the buffer. + * + * Same idea as with net_buf_simple_pull(), but a helper for operating + * on 64-bit little endian data. + * + * @param buf A valid pointer on a buffer. + * + * @return 64-bit value converted from little endian to host endian. + */ +u64_t net_buf_simple_pull_le64(struct net_buf_simple *buf); + +/** + * @brief Remove and convert 64 bits from the beginning of the buffer. + * + * Same idea as with net_buf_simple_pull(), but a helper for operating + * on 64-bit big endian data. + * + * @param buf A valid pointer on a buffer. + * + * @return 64-bit value converted from big endian to host endian. + */ +u64_t net_buf_simple_pull_be64(struct net_buf_simple *buf); + /** * @brief Get the tail pointer for a buffer. * @@ -1056,6 +1104,32 @@ static inline void *net_buf_user_data(struct net_buf *buf) */ #define net_buf_add_be48(buf, val) net_buf_simple_add_be48(&(buf)->b, val) +/** + * @def net_buf_add_le64 + * @brief Add 64-bit value at the end of the buffer + * + * Adds 64-bit value in little endian format at the end of buffer. + * Increments the data length of a buffer to account for more data + * at the end. + * + * @param buf Buffer to update. + * @param val 64-bit value to be added. + */ +#define net_buf_add_le64(buf, val) net_buf_simple_add_le64(&(buf)->b, val) + +/** + * @def net_buf_add_be64 + * @brief Add 64-bit value at the end of the buffer + * + * Adds 64-bit value in big endian format at the end of buffer. + * Increments the data length of a buffer to account for more data + * at the end. + * + * @param buf Buffer to update. + * @param val 64-bit value to be added. + */ +#define net_buf_add_be64(buf, val) net_buf_simple_add_be64(&(buf)->b, val) + /** * @def net_buf_push * @brief Push data to the beginning of the buffer. @@ -1250,6 +1324,32 @@ static inline void *net_buf_user_data(struct net_buf *buf) */ #define net_buf_pull_be48(buf) net_buf_simple_pull_be48(&(buf)->b) +/** + * @def net_buf_pull_le64 + * @brief Remove and convert 64 bits from the beginning of the buffer. + * + * Same idea as with net_buf_pull(), but a helper for operating on + * 64-bit little endian data. + * + * @param buf A valid pointer on a buffer. + * + * @return 64-bit value converted from little endian to host endian. + */ +#define net_buf_pull_le64(buf) net_buf_simple_pull_le64(&(buf)->b) + +/** + * @def net_buf_pull_be64 + * @brief Remove and convert 64 bits from the beginning of the buffer. + * + * Same idea as with net_buf_pull(), but a helper for operating on + * 64-bit big endian data. + * + * @param buf A valid pointer on a buffer + * + * @return 64-bit value converted from big endian to host endian. + */ +#define net_buf_pull_be64(buf) net_buf_simple_pull_be64(&(buf)->b) + /** * @def net_buf_tailroom * @brief Check buffer tailroom. diff --git a/components/bt/esp_ble_mesh/mesh_common/include/mesh_util.h b/components/bt/esp_ble_mesh/mesh_common/include/mesh_util.h index bf9fa3a0b0..de6e0ad1ec 100644 --- a/components/bt/esp_ble_mesh/mesh_common/include/mesh_util.h +++ b/components/bt/esp_ble_mesh/mesh_common/include/mesh_util.h @@ -431,6 +431,21 @@ static inline void sys_put_be48(u64_t val, u8_t dst[6]) sys_put_be32(val, &dst[2]); } +/** + * @brief Put a 64-bit integer as big-endian to arbitrary location. + * + * Put a 64-bit integer, originally in host endianness, to a + * potentially unaligned memory location in big-endian format. + * + * @param val 64-bit integer in host endianness. + * @param dst Destination memory address to store the result. + */ +static inline void sys_put_be64(u64_t val, u8_t dst[8]) +{ + sys_put_be32(val >> 32, dst); + sys_put_be32(val, &dst[4]); +} + /** * @brief Put a 16-bit integer as little-endian to arbitrary location. * @@ -566,6 +581,21 @@ static inline u64_t sys_get_be48(const u8_t src[6]) return ((u64_t)sys_get_be32(&src[0]) << 32) | sys_get_be16(&src[4]); } +/** + * @brief Get a 64-bit integer stored in big-endian format. + * + * Get a 64-bit integer, stored in big-endian format in a potentially + * unaligned memory location, and convert it to the host endianness. + * + * @param src Location of the big-endian 64-bit integer to get. + * + * @return 64-bit integer in host endianness. + */ +static inline u64_t sys_get_be64(const u8_t src[8]) +{ + return ((u64_t)sys_get_be32(&src[0]) << 32) | sys_get_be32(&src[4]); +} + /** * @brief Get a 16-bit integer stored in little-endian format. * diff --git a/components/bt/esp_ble_mesh/mesh_common/mesh_buf.c b/components/bt/esp_ble_mesh/mesh_common/mesh_buf.c index 1fe3189fba..b875d285ba 100644 --- a/components/bt/esp_ble_mesh/mesh_common/mesh_buf.c +++ b/components/bt/esp_ble_mesh/mesh_common/mesh_buf.c @@ -128,6 +128,20 @@ void net_buf_simple_add_be48(struct net_buf_simple *buf, u64_t val) sys_put_be48(val, net_buf_simple_add(buf, 6)); } +void net_buf_simple_add_le64(struct net_buf_simple *buf, u64_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); + + sys_put_le64(val, net_buf_simple_add(buf, sizeof(val))); +} + +void net_buf_simple_add_be64(struct net_buf_simple *buf, u64_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); + + sys_put_be64(val, net_buf_simple_add(buf, sizeof(val))); +} + void *net_buf_simple_push(struct net_buf_simple *buf, size_t len) { NET_BUF_SIMPLE_DBG("buf %p len %u", buf, len); @@ -284,6 +298,26 @@ u64_t net_buf_simple_pull_be48(struct net_buf_simple *buf) return sys_be48_to_cpu(val.u48); } +u64_t net_buf_simple_pull_le64(struct net_buf_simple *buf) +{ + u64_t val; + + val = UNALIGNED_GET((u64_t *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); + + return sys_le64_to_cpu(val); +} + +u64_t net_buf_simple_pull_be64(struct net_buf_simple *buf) +{ + u64_t val; + + val = UNALIGNED_GET((u64_t *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); + + return sys_be64_to_cpu(val); +} + size_t net_buf_simple_headroom(struct net_buf_simple *buf) { return buf->data - buf->__buf;