1 /*
2 * Copyright (c) 2018 Foundries.io
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #ifndef ZEPHYR_INCLUDE_BUF_UTIL_H_
8 #define ZEPHYR_INCLUDE_BUF_UTIL_H_
9
10 #include <zephyr/types.h>
11 #include <errno.h>
12 #include <string.h>
13
14 #ifdef __cplusplus
15 extern "C" {
16 #endif
17
18 /* append */
buf_append(uint8_t * dst,uint16_t * dst_len,uint16_t dst_size,const uint8_t * src,uint16_t src_len)19 static inline int buf_append(uint8_t *dst, uint16_t *dst_len, uint16_t dst_size,
20 const uint8_t *src, uint16_t src_len)
21 {
22 if (!dst || !src) {
23 return -EINVAL;
24 }
25
26 if (*dst_len + src_len > dst_size) {
27 return -ENOMEM;
28 }
29
30 memcpy(dst + *dst_len, src, src_len);
31 *dst_len += src_len;
32 return 0;
33 }
34
35 /* insert */
buf_insert(uint8_t * dst,uint16_t * dst_len,uint16_t dst_size,uint16_t offset,uint8_t * src,uint16_t src_len)36 static inline int buf_insert(uint8_t *dst, uint16_t *dst_len, uint16_t dst_size,
37 uint16_t offset, uint8_t *src, uint16_t src_len)
38 {
39 if (!dst || !src) {
40 return -EINVAL;
41 }
42
43 if (*dst_len + src_len > dst_size) {
44 return -ENOMEM;
45 }
46
47 /* shift everything in fbuf after offset by len */
48 memmove(dst + offset + src_len, dst + offset, *dst_len - offset);
49
50 /* copy src into fbuf at offset */
51 memcpy(dst + offset, src, src_len);
52 *dst_len += src_len;
53 return 0;
54 }
55
56 /* read */
buf_read(uint8_t * dst,uint16_t len,uint8_t * src,uint16_t src_len,uint16_t * offset)57 static inline int buf_read(uint8_t *dst, uint16_t len, uint8_t *src, uint16_t src_len,
58 uint16_t *offset)
59 {
60 if (!src) {
61 return -EINVAL;
62 }
63
64 if (*offset + len > src_len) {
65 return -ENODATA;
66 }
67
68 if (dst) {
69 /* copy data at offset into dst */
70 memcpy(dst, src + *offset, len);
71 }
72
73 *offset += len;
74 return 0;
75 }
76
buf_skip(uint16_t len,uint8_t * src,uint16_t src_len,uint16_t * offset)77 static inline int buf_skip(uint16_t len, uint8_t *src, uint16_t src_len, uint16_t *offset)
78 {
79 return buf_read(NULL, len, src, src_len, offset);
80 }
81
buf_read_u8(uint8_t * value,uint8_t * src,uint16_t src_len,uint16_t * offset)82 static inline int buf_read_u8(uint8_t *value, uint8_t *src, uint16_t src_len,
83 uint16_t *offset)
84 {
85 return buf_read(value, sizeof(uint8_t), src, src_len, offset);
86 }
87
buf_read_u16(uint16_t * value,uint8_t * src,uint16_t src_len,uint16_t * offset)88 static inline int buf_read_u16(uint16_t *value, uint8_t *src, uint16_t src_len,
89 uint16_t *offset)
90 {
91 return buf_read((uint8_t *)value, sizeof(uint16_t), src, src_len, offset);
92 }
93
buf_read_be16(uint16_t * value,uint8_t * src,uint16_t src_len,uint16_t * offset)94 static inline int buf_read_be16(uint16_t *value, uint8_t *src, uint16_t src_len,
95 uint16_t *offset)
96 {
97 int ret;
98 uint8_t v16[2];
99
100 ret = buf_read(v16, sizeof(uint16_t), src, src_len, offset);
101 *value = v16[0] << 8 | v16[1];
102
103 return ret;
104 }
105
buf_read_u32(uint32_t * value,uint8_t * src,uint16_t src_len,uint16_t * offset)106 static inline int buf_read_u32(uint32_t *value, uint8_t *src, uint16_t src_len,
107 uint16_t *offset)
108 {
109 return buf_read((uint8_t *)value, sizeof(uint32_t), src, src_len, offset);
110 }
111
buf_read_be32(uint32_t * value,uint8_t * src,uint16_t src_len,uint16_t * offset)112 static inline int buf_read_be32(uint32_t *value, uint8_t *src, uint16_t src_len,
113 uint16_t *offset)
114 {
115 int ret;
116 uint8_t v32[4];
117
118 ret = buf_read(v32, sizeof(uint32_t), src, src_len, offset);
119 *value = v32[0] << 24 | v32[1] << 16 | v32[2] << 8 | v32[3];
120
121 return ret;
122 }
123
124 #ifdef __cplusplus
125 }
126 #endif
127
128 #endif /* ZEPHYR_INCLUDE_BUF_UTIL_H_ */
129
