| /* |
| * Copyright (c) 2018 Foundries.io |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #ifndef ZEPHYR_INCLUDE_BUF_UTIL_H_ |
| #define ZEPHYR_INCLUDE_BUF_UTIL_H_ |
| |
| #include <zephyr/types.h> |
| #include <errno.h> |
| #include <string.h> |
| |
| #ifdef __cplusplus |
| extern "C" { |
| #endif |
| |
| /* append */ |
| static inline int buf_append(u8_t *dst, u16_t *dst_len, u16_t dst_size, |
| u8_t *src, u16_t src_len) |
| { |
| if (!dst || !src) { |
| return -EINVAL; |
| } |
| |
| if (*dst_len + src_len > dst_size) { |
| return -ENOMEM; |
| } |
| |
| memcpy(dst + *dst_len, src, src_len); |
| *dst_len += src_len; |
| return 0; |
| } |
| |
| /* insert */ |
| static inline int buf_insert(u8_t *dst, u16_t *dst_len, u16_t dst_size, |
| u16_t offset, u8_t *src, u16_t src_len) |
| { |
| if (!dst || !src) { |
| return -EINVAL; |
| } |
| |
| if (*dst_len + src_len > dst_size) { |
| return -ENOMEM; |
| } |
| |
| /* shift everything in fbuf after offset by len */ |
| memmove(dst + offset + src_len, dst + offset, *dst_len - offset); |
| |
| /* copy src into fbuf at offset */ |
| memcpy(dst + offset, src, src_len); |
| *dst_len += src_len; |
| return 0; |
| } |
| |
| /* read */ |
| static inline int buf_read(u8_t *dst, u16_t len, u8_t *src, u16_t src_len, |
| u16_t *offset) |
| { |
| if (!src) { |
| return -EINVAL; |
| } |
| |
| if (*offset + len > src_len) { |
| return -ENOMEM; |
| } |
| |
| if (dst) { |
| /* copy data at offset into dst */ |
| memcpy(dst, src + *offset, len); |
| } |
| |
| *offset += len; |
| return 0; |
| } |
| |
| static inline int buf_skip(u16_t len, u8_t *src, u16_t src_len, u16_t *offset) |
| { |
| return buf_read(NULL, len, src, src_len, offset); |
| } |
| |
| static inline int buf_read_u8(u8_t *value, u8_t *src, u16_t src_len, |
| u16_t *offset) |
| { |
| return buf_read(value, sizeof(u8_t), src, src_len, offset); |
| } |
| |
| static inline int buf_read_u16(u16_t *value, u8_t *src, u16_t src_len, |
| u16_t *offset) |
| { |
| return buf_read((u8_t *)value, sizeof(u16_t), src, src_len, offset); |
| } |
| |
| static inline int buf_read_be16(u16_t *value, u8_t *src, u16_t src_len, |
| u16_t *offset) |
| { |
| int ret; |
| u8_t v16[2]; |
| |
| ret = buf_read(v16, sizeof(u16_t), src, src_len, offset); |
| *value = v16[0] << 8 | v16[1]; |
| |
| return ret; |
| } |
| |
| static inline int buf_read_u32(u32_t *value, u8_t *src, u16_t src_len, |
| u16_t *offset) |
| { |
| return buf_read((u8_t *)value, sizeof(u32_t), src, src_len, offset); |
| } |
| |
| static inline int buf_read_be32(u32_t *value, u8_t *src, u16_t src_len, |
| u16_t *offset) |
| { |
| int ret; |
| u8_t v32[4]; |
| |
| ret = buf_read(v32, sizeof(u32_t), src, src_len, offset); |
| *value = v32[0] << 24 | v32[1] << 16 | v32[2] << 8 | v32[3]; |
| |
| return ret; |
| } |
| |
| #ifdef __cplusplus |
| } |
| #endif |
| |
| #endif /* ZEPHYR_INCLUDE_BUF_UTIL_H_ */ |