From 18d49830b3e9ba0e346890bc3c0e9dd500eb0c2f Mon Sep 17 00:00:00 2001 From: Iulia Tanasescu Date: Fri, 5 May 2023 11:29:41 +0300 Subject: shared/util: Add iovec helpers This adds iovec helper functions for handling byteorder and alignment in place. --- src/shared/util.c | 228 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/shared/util.h | 24 ++++++ 2 files changed, 252 insertions(+) (limited to 'src') diff --git a/src/shared/util.c b/src/shared/util.c index 9a4a8d77a..4d1c0d005 100644 --- a/src/shared/util.c +++ b/src/shared/util.c @@ -4,6 +4,7 @@ * BlueZ - Bluetooth protocol stack for Linux * * Copyright (C) 2012-2014 Intel Corporation. All rights reserved. + * Copyright 2023 NXP * * */ @@ -28,6 +29,8 @@ #include #endif +#include + /* define MAX_INPUT for musl */ #ifndef MAX_INPUT #define MAX_INPUT _POSIX_MAX_INPUT @@ -274,6 +277,123 @@ void *util_iov_push_mem(struct iovec *iov, size_t len, const void *data) return p; } +void *util_iov_push_le64(struct iovec *iov, uint64_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(val)); + if (!p) + return NULL; + + put_le64(val, p); + + return p; +} + +void *util_iov_push_be64(struct iovec *iov, uint64_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(val)); + if (!p) + return NULL; + + put_be64(val, p); + + return p; +} + +void *util_iov_push_le32(struct iovec *iov, uint32_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(val)); + if (!p) + return NULL; + + put_le32(val, p); + + return p; +} + +void *util_iov_push_be32(struct iovec *iov, uint32_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(val)); + if (!p) + return NULL; + + put_be32(val, p); + + return p; +} + +void *util_iov_push_le24(struct iovec *iov, uint32_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(uint24_t)); + if (!p) + return NULL; + + put_le24(val, p); + + return p; +} + +void *util_iov_push_be24(struct iovec *iov, uint32_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(uint24_t)); + if (!p) + return NULL; + + put_le24(val, p); + + return p; +} + +void *util_iov_push_le16(struct iovec *iov, uint16_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(val)); + if (!p) + return NULL; + + put_le16(val, p); + + return p; +} + +void *util_iov_push_be16(struct iovec *iov, uint16_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(val)); + if (!p) + return NULL; + + put_be16(val, p); + + return p; +} + +void *util_iov_push_u8(struct iovec *iov, uint8_t val) +{ + void *p; + + p = util_iov_push(iov, sizeof(val)); + if (!p) + return NULL; + + put_u8(val, p); + + return p; +} + void *util_iov_pull(struct iovec *iov, size_t len) { if (!iov) @@ -298,6 +418,114 @@ void *util_iov_pull_mem(struct iovec *iov, size_t len) return NULL; } +void *util_iov_pull_le64(struct iovec *iov, uint64_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(*val))) { + *val = get_le64(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_be64(struct iovec *iov, uint64_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(*val))) { + *val = get_be64(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_le32(struct iovec *iov, uint32_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(*val))) { + *val = get_le32(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_be32(struct iovec *iov, uint32_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(*val))) { + *val = get_be32(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_le24(struct iovec *iov, uint32_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(uint24_t))) { + *val = get_le24(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_be24(struct iovec *iov, uint32_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(uint24_t))) { + *val = get_be24(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_le16(struct iovec *iov, uint16_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(*val))) { + *val = get_le16(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_be16(struct iovec *iov, uint16_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(*val))) { + *val = get_be16(data); + return data; + } + + return NULL; +} + +void *util_iov_pull_u8(struct iovec *iov, uint8_t *val) +{ + void *data = iov->iov_base; + + if (util_iov_pull(iov, sizeof(*val))) { + *val = get_u8(data); + return data; + } + + return NULL; +} + static const struct { uint16_t uuid; const char *str; diff --git a/src/shared/util.h b/src/shared/util.h index dc84f9635..ce57b53be 100644 --- a/src/shared/util.h +++ b/src/shared/util.h @@ -4,6 +4,7 @@ * BlueZ - Bluetooth protocol stack for Linux * * Copyright (C) 2012-2014 Intel Corporation. All rights reserved. + * Copyright 2023 NXP * * */ @@ -115,8 +116,26 @@ int util_iov_memcmp(const struct iovec *iov1, const struct iovec *iov2); void util_iov_memcpy(struct iovec *iov, void *src, size_t len); void *util_iov_push(struct iovec *iov, size_t len); void *util_iov_push_mem(struct iovec *iov, size_t len, const void *data); +void *util_iov_push_le64(struct iovec *iov, uint64_t val); +void *util_iov_push_be64(struct iovec *iov, uint64_t val); +void *util_iov_push_le32(struct iovec *iov, uint32_t val); +void *util_iov_push_be32(struct iovec *iov, uint32_t val); +void *util_iov_push_le24(struct iovec *iov, uint32_t val); +void *util_iov_push_be24(struct iovec *iov, uint32_t val); +void *util_iov_push_le16(struct iovec *iov, uint16_t val); +void *util_iov_push_be16(struct iovec *iov, uint16_t val); +void *util_iov_push_u8(struct iovec *iov, uint8_t val); void *util_iov_pull(struct iovec *iov, size_t len); void *util_iov_pull_mem(struct iovec *iov, size_t len); +void *util_iov_pull_le64(struct iovec *iov, uint64_t *val); +void *util_iov_pull_be64(struct iovec *iov, uint64_t *val); +void *util_iov_pull_le32(struct iovec *iov, uint32_t *val); +void *util_iov_pull_be32(struct iovec *iov, uint32_t *val); +void *util_iov_pull_le24(struct iovec *iov, uint32_t *val); +void *util_iov_pull_be24(struct iovec *iov, uint32_t *val); +void *util_iov_pull_le16(struct iovec *iov, uint16_t *val); +void *util_iov_pull_be16(struct iovec *iov, uint16_t *val); +void *util_iov_pull_u8(struct iovec *iov, uint8_t *val); void util_iov_free(struct iovec *iov, size_t cnt); const char *bt_uuid16_to_str(uint16_t uuid); @@ -179,6 +198,11 @@ static inline uint64_t get_be64(const void *ptr) return be64_to_cpu(get_unaligned((const uint64_t *) ptr)); } +static inline void put_u8(uint8_t val, void *dst) +{ + put_unaligned(val, (uint8_t *) dst); +} + static inline void put_le16(uint16_t val, void *dst) { put_unaligned(cpu_to_le16(val), (uint16_t *) dst); -- cgit v1.2.1