From 8918247f37bbb8c5d917b8d28cb6dec4861b971e Mon Sep 17 00:00:00 2001 From: Reto Schneider Date: Wed, 13 Mar 2024 17:07:28 +0100 Subject: [PATCH] net: buf_simple: Add support for 40 bit data type This enables pulling and pushing values in 40 bit format. Signed-off-by: Reto Schneider --- include/zephyr/net/buf.h | 94 +++++++++++++++++++++++++++++++++ subsys/net/buf_simple.c | 78 +++++++++++++++++++++++++++ tests/net/buf_simple/src/main.c | 73 +++++++++++++++++++++++++ 3 files changed, 245 insertions(+) diff --git a/include/zephyr/net/buf.h b/include/zephyr/net/buf.h index 18ff833a38..21de2cdf15 100644 --- a/include/zephyr/net/buf.h +++ b/include/zephyr/net/buf.h @@ -299,6 +299,30 @@ void net_buf_simple_add_le32(struct net_buf_simple *buf, uint32_t val); */ void net_buf_simple_add_be32(struct net_buf_simple *buf, uint32_t val); +/** + * @brief Add 40-bit value at the end of the buffer + * + * Adds 40-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 40-bit value to be added. + */ +void net_buf_simple_add_le40(struct net_buf_simple *buf, uint64_t val); + +/** + * @brief Add 40-bit value at the end of the buffer + * + * Adds 40-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 40-bit value to be added. + */ +void net_buf_simple_add_be40(struct net_buf_simple *buf, uint64_t val); + /** * @brief Add 48-bit value at the end of the buffer * @@ -443,6 +467,30 @@ uint32_t net_buf_simple_remove_le32(struct net_buf_simple *buf); */ uint32_t net_buf_simple_remove_be32(struct net_buf_simple *buf); +/** + * @brief Remove and convert 40 bits from the end of the buffer. + * + * Same idea as with net_buf_simple_remove_mem(), but a helper for operating + * on 40-bit little endian data. + * + * @param buf A valid pointer on a buffer. + * + * @return 40-bit value converted from little endian to host endian. + */ +uint64_t net_buf_simple_remove_le40(struct net_buf_simple *buf); + +/** + * @brief Remove and convert 40 bits from the end of the buffer. + * + * Same idea as with net_buf_simple_remove_mem(), but a helper for operating + * on 40-bit big endian data. + * + * @param buf A valid pointer on a buffer. + * + * @return 40-bit value converted from big endian to host endian. + */ +uint64_t net_buf_simple_remove_be40(struct net_buf_simple *buf); + /** * @brief Remove and convert 48 bits from the end of the buffer. * @@ -595,6 +643,28 @@ void net_buf_simple_push_le32(struct net_buf_simple *buf, uint32_t val); */ void net_buf_simple_push_be32(struct net_buf_simple *buf, uint32_t val); +/** + * @brief Push 40-bit value to the beginning of the buffer + * + * Adds 40-bit value in little endian format to the beginning of the + * buffer. + * + * @param buf Buffer to update. + * @param val 40-bit value to be pushed to the buffer. + */ +void net_buf_simple_push_le40(struct net_buf_simple *buf, uint64_t val); + +/** + * @brief Push 40-bit value to the beginning of the buffer + * + * Adds 40-bit value in big endian format to the beginning of the + * buffer. + * + * @param buf Buffer to update. + * @param val 40-bit value to be pushed to the buffer. + */ +void net_buf_simple_push_be40(struct net_buf_simple *buf, uint64_t val); + /** * @brief Push 48-bit value to the beginning of the buffer * @@ -749,6 +819,30 @@ uint32_t net_buf_simple_pull_le32(struct net_buf_simple *buf); */ uint32_t net_buf_simple_pull_be32(struct net_buf_simple *buf); +/** + * @brief Remove and convert 40 bits from the beginning of the buffer. + * + * Same idea as with net_buf_simple_pull(), but a helper for operating + * on 40-bit little endian data. + * + * @param buf A valid pointer on a buffer. + * + * @return 40-bit value converted from little endian to host endian. + */ +uint64_t net_buf_simple_pull_le40(struct net_buf_simple *buf); + +/** + * @brief Remove and convert 40 bits from the beginning of the buffer. + * + * Same idea as with net_buf_simple_pull(), but a helper for operating + * on 40-bit big endian data. + * + * @param buf A valid pointer on a buffer. + * + * @return 40-bit value converted from big endian to host endian. + */ +uint64_t net_buf_simple_pull_be40(struct net_buf_simple *buf); + /** * @brief Remove and convert 48 bits from the beginning of the buffer. * diff --git a/subsys/net/buf_simple.c b/subsys/net/buf_simple.c index 8937a63615..3e9db256e8 100644 --- a/subsys/net/buf_simple.c +++ b/subsys/net/buf_simple.c @@ -127,6 +127,20 @@ void net_buf_simple_add_be32(struct net_buf_simple *buf, uint32_t val) sys_put_be32(val, net_buf_simple_add(buf, sizeof(val))); } +void net_buf_simple_add_le40(struct net_buf_simple *buf, uint64_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); + + sys_put_le40(val, net_buf_simple_add(buf, 5)); +} + +void net_buf_simple_add_be40(struct net_buf_simple *buf, uint64_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); + + sys_put_be40(val, net_buf_simple_add(buf, 5)); +} + void net_buf_simple_add_le48(struct net_buf_simple *buf, uint64_t val) { NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); @@ -246,6 +260,32 @@ uint32_t net_buf_simple_remove_be32(struct net_buf_simple *buf) return sys_be32_to_cpu(val); } +uint64_t net_buf_simple_remove_le40(struct net_buf_simple *buf) +{ + struct uint40 { + uint64_t u40: 40; + } __packed val; + void *ptr; + + ptr = net_buf_simple_remove_mem(buf, sizeof(val)); + val = UNALIGNED_GET((struct uint40 *)ptr); + + return sys_le40_to_cpu(val.u40); +} + +uint64_t net_buf_simple_remove_be40(struct net_buf_simple *buf) +{ + struct uint40 { + uint64_t u40: 40; + } __packed val; + void *ptr; + + ptr = net_buf_simple_remove_mem(buf, sizeof(val)); + val = UNALIGNED_GET((struct uint40 *)ptr); + + return sys_be40_to_cpu(val.u40); +} + uint64_t net_buf_simple_remove_le48(struct net_buf_simple *buf) { struct uint48 { @@ -362,6 +402,20 @@ void net_buf_simple_push_be32(struct net_buf_simple *buf, uint32_t val) sys_put_be32(val, net_buf_simple_push(buf, sizeof(val))); } +void net_buf_simple_push_le40(struct net_buf_simple *buf, uint64_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); + + sys_put_le40(val, net_buf_simple_push(buf, 5)); +} + +void net_buf_simple_push_be40(struct net_buf_simple *buf, uint64_t val) +{ + NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); + + sys_put_be40(val, net_buf_simple_push(buf, 5)); +} + void net_buf_simple_push_le48(struct net_buf_simple *buf, uint64_t val) { NET_BUF_SIMPLE_DBG("buf %p val %" PRIu64, buf, val); @@ -488,6 +542,30 @@ uint32_t net_buf_simple_pull_be32(struct net_buf_simple *buf) return sys_be32_to_cpu(val); } +uint64_t net_buf_simple_pull_le40(struct net_buf_simple *buf) +{ + struct uint40 { + uint64_t u40: 40; + } __packed val; + + val = UNALIGNED_GET((struct uint40 *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); + + return sys_le40_to_cpu(val.u40); +} + +uint64_t net_buf_simple_pull_be40(struct net_buf_simple *buf) +{ + struct uint40 { + uint64_t u40: 40; + } __packed val; + + val = UNALIGNED_GET((struct uint40 *)buf->data); + net_buf_simple_pull(buf, sizeof(val)); + + return sys_be40_to_cpu(val.u40); +} + uint64_t net_buf_simple_pull_le48(struct net_buf_simple *buf) { struct uint48 { diff --git a/tests/net/buf_simple/src/main.c b/tests/net/buf_simple/src/main.c index def63b38aa..de5edd4dfe 100644 --- a/tests/net/buf_simple/src/main.c +++ b/tests/net/buf_simple/src/main.c @@ -18,6 +18,8 @@ static const uint8_t le24[3] = { 0x03, 0x02, 0x01 }; static const uint8_t be24[3] = { 0x01, 0x02, 0x03 }; static const uint8_t le32[4] = { 0x04, 0x03, 0x02, 0x01 }; static const uint8_t be32[4] = { 0x01, 0x02, 0x03, 0x04 }; +static const uint8_t le40[5] = { 0x05, 0x04, 0x03, 0x02, 0x01 }; +static const uint8_t be40[5] = { 0x01, 0x02, 0x03, 0x04, 0x05 }; static const uint8_t le48[6] = { 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 }; static const uint8_t be48[6] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06 }; static const uint8_t le64[8] = { 0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 }; @@ -25,6 +27,7 @@ static const uint8_t be64[8] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08 static const uint16_t u16 = 0x0102; static const uint32_t u24 = 0x010203; static const uint32_t u32 = 0x01020304; +static const uint64_t u40 = 0x0102030405; static const uint64_t u48 = 0x010203040506; static const uint64_t u64 = 0x0102030405060708; @@ -141,6 +144,38 @@ ZTEST(net_buf_simple_test_suite, test_net_buf_simple_add_be32) sizeof(be32), "Invalid 32 bits byte order"); } +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_pull_le40) +{ + net_buf_simple_add_mem(&buf, &le40, sizeof(le40)); + + zassert_equal(u40, net_buf_simple_pull_le40(&buf), + "Invalid 40 bits byte order"); +} + +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_pull_be40) +{ + net_buf_simple_add_mem(&buf, &be40, sizeof(be40)); + + zassert_equal(u40, net_buf_simple_pull_be40(&buf), + "Invalid 40 bits byte order"); +} + +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_add_le40) +{ + net_buf_simple_add_le40(&buf, u40); + + zassert_mem_equal(le40, net_buf_simple_pull_mem(&buf, sizeof(le40)), + sizeof(le40), "Invalid 40 bits byte order"); +} + +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_add_be40) +{ + net_buf_simple_add_be40(&buf, u40); + + zassert_mem_equal(be40, net_buf_simple_pull_mem(&buf, sizeof(be40)), + sizeof(be40), "Invalid 40 bits byte order"); +} + ZTEST(net_buf_simple_test_suite, test_net_buf_simple_pull_le48) { net_buf_simple_add_mem(&buf, &le48, sizeof(le48)); @@ -325,6 +360,44 @@ ZTEST(net_buf_simple_test_suite, test_net_buf_simple_push_be32) sizeof(be32), "Invalid 32 bits byte order"); } +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_remove_le40) +{ + net_buf_simple_reserve(&buf, 16); + + net_buf_simple_push_mem(&buf, &le40, sizeof(le40)); + + zassert_equal(u40, net_buf_simple_remove_le40(&buf), "Invalid 40 bits byte order"); +} + +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_remove_be40) +{ + net_buf_simple_reserve(&buf, 16); + + net_buf_simple_push_mem(&buf, &be40, sizeof(be40)); + + zassert_equal(u40, net_buf_simple_remove_be40(&buf), "Invalid 40 bits byte order"); +} + +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_push_le40) +{ + net_buf_simple_reserve(&buf, 16); + + net_buf_simple_push_le40(&buf, u40); + + zassert_mem_equal(le40, net_buf_simple_remove_mem(&buf, sizeof(le40)), sizeof(le40), + "Invalid 40 bits byte order"); +} + +ZTEST(net_buf_simple_test_suite, test_net_buf_simple_push_be40) +{ + net_buf_simple_reserve(&buf, 16); + + net_buf_simple_push_be40(&buf, u40); + + zassert_mem_equal(be40, net_buf_simple_remove_mem(&buf, sizeof(be40)), sizeof(be40), + "Invalid 40 bits byte order"); +} + ZTEST(net_buf_simple_test_suite, test_net_buf_simple_remove_le48) { net_buf_simple_reserve(&buf, 16);