shared/util: Add iovec helpers

This adds iovec helper functions for handling byteorder and alignment
in place.

diff --git a/src/shared/util.c b/src/shared/util.c
index 2e0ac16..55c01d3 100755 (executable)
--- 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
  *
  *
  */
@@ -27,6 +28,8 @@
 #include <sys/random.h>
 #endif
 
+#include <lib/bluetooth.h>
+
 /* define MAX_INPUT for musl */
 #ifndef MAX_INPUT
 #define MAX_INPUT _POSIX_MAX_INPUT
@@ -273,6 +276,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)
@@ -297,6 +417,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 dc84f96..ce57b53 100755 (executable)
--- 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);