BLE Host - use big endian for iBeacon ver fields.

3 files changed, 92 insertions, 2 deletions

diff --git a/net/nimble/host/src/ble_ibeacon.c b/net/nimble/host/src/ble_ibeacon.c
index 896dc102..db34dfbd 100644
--- a/net/nimble/host/src/ble_ibeacon.c
+++ b/net/nimble/host/src/ble_ibeacon.c
@@ -42,8 +42,8 @@ ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, uint16_t minor)
     memcpy(buf + 4, uuid128, 16);
 
     /** Version number. */
-    htole16(buf + 20, major);
-    htole16(buf + 22, minor);
+    htobe16(buf + 20, major);
+    htobe16(buf + 22, minor);
 
     /** Last byte (tx power level) filled in after HCI exchange. */
 
diff --git a/net/nimble/include/nimble/ble.h b/net/nimble/include/nimble/ble.h
index c0b5b742..9081b8c8 100644
--- a/net/nimble/include/nimble/ble.h
+++ b/net/nimble/include/nimble/ble.h
@@ -127,12 +127,18 @@ extern uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
 #undef le16toh
 #undef le32toh
 #undef le64toh
+#undef htobe16
+#undef htobe32
+#undef htobe64
 void htole16(void *buf, uint16_t x);
 void htole32(void *buf, uint32_t x);
 void htole64(void *buf, uint64_t x);
 uint16_t le16toh(void *buf);
 uint32_t le32toh(void *buf);
 uint64_t le64toh(void *buf);
+void htobe16(void *buf, uint16_t x);
+void htobe32(void *buf, uint32_t x);
+void htobe64(void *buf, uint64_t x);
 void swap_in_place(void *buf, int len);
 void swap_buf(uint8_t *dst, uint8_t *src, int len);
 /* XXX */
diff --git a/net/nimble/src/util.c b/net/nimble/src/util.c
index c6f2a43e..d0056df6 100644
--- a/net/nimble/src/util.c
+++ b/net/nimble/src/util.c
@@ -105,6 +105,90 @@ le64toh(void *buf)
 }
 
 void
+htobe16(void *buf, uint16_t x)
+{
+    uint8_t *u8ptr;
+
+    u8ptr = buf;
+    u8ptr[0] = (uint8_t)(x >> 8);
+    u8ptr[1] = (uint8_t)x;
+}
+
+void
+htobe32(void *buf, uint32_t x)
+{
+    uint8_t *u8ptr;
+
+    u8ptr = buf;
+    u8ptr[0] = (uint8_t)(x >> 24);
+    u8ptr[1] = (uint8_t)(x >> 16);
+    u8ptr[2] = (uint8_t)(x >> 8);
+    u8ptr[3] = (uint8_t)x;
+}
+
+void
+htobe64(void *buf, uint64_t x)
+{
+    uint8_t *u8ptr;
+
+    u8ptr = buf;
+    u8ptr[0] = (uint8_t)(x >> 56);
+    u8ptr[1] = (uint8_t)(x >> 48);
+    u8ptr[2] = (uint8_t)(x >> 40);
+    u8ptr[3] = (uint8_t)(x >> 32);
+    u8ptr[4] = (uint8_t)(x >> 24);
+    u8ptr[5] = (uint8_t)(x >> 16);
+    u8ptr[6] = (uint8_t)(x >> 8);
+    u8ptr[7] = (uint8_t)x;
+}
+
+uint16_t
+be16toh(void *buf)
+{
+    uint16_t x;
+    uint8_t *u8ptr;
+
+    u8ptr = buf;
+    x = (uint16_t)u8ptr[0] << 8;
+    x |= u8ptr[1];
+
+    return x;
+}
+
+uint32_t
+be32toh(void *buf)
+{
+    uint32_t x;
+    uint8_t *u8ptr;
+
+    u8ptr = buf;
+    x = (uint32_t)u8ptr[0] << 24;
+    x |= (uint32_t)u8ptr[1] << 16;
+    x |= (uint32_t)u8ptr[2] << 8;
+    x |= u8ptr[3];
+
+    return x;
+}
+
+uint64_t
+be64toh(void *buf)
+{
+    uint64_t x;
+    uint8_t *u8ptr;
+
+    u8ptr = buf;
+    x = (uint64_t)u8ptr[0] << 56;
+    x |= (uint64_t)u8ptr[1] << 48;
+    x |= (uint64_t)u8ptr[2] << 40;
+    x |= (uint64_t)u8ptr[3] << 32;
+    x |= (uint64_t)u8ptr[4] << 24;
+    x |= (uint64_t)u8ptr[5] << 16;
+    x |= (uint64_t)u8ptr[6] << 8;
+    x |= u8ptr[7];
+
+    return x;
+}
+void
 swap_in_place(void *buf, int len)
 {
     uint8_t *u8ptr;