diff --git a/include/common/net_helper.h b/include/common/net_helper.h index 7d5636df7..288ca824b 100644 --- a/include/common/net_helper.h +++ b/include/common/net_helper.h @@ -30,7 +30,7 @@ #include #include -/* Functions to read various integers that may be unaligned */ +/* Functions to read/write various integers that may be unaligned */ /* Read a uint16_t in native host order */ static inline uint16_t read_u16(const void *p) @@ -39,6 +39,13 @@ static inline uint16_t read_u16(const void *p) return u->u16; } +/* Write a uint16_t in native host order */ +static inline void write_u16(void *p, const uint16_t u16) +{ + union { uint16_t u16; } __attribute__((packed))*u = p; + u->u16 = u16; +} + /* Read a uint32_t in native host order */ static inline uint32_t read_u32(const void *p) { @@ -46,6 +53,13 @@ static inline uint32_t read_u32(const void *p) return u->u32; } +/* Write a uint32_t in native host order */ +static inline void write_u32(void *p, const uint32_t u32) +{ + union { uint32_t u32; } __attribute__((packed))*u = p; + u->u32 = u32; +} + /* Read a possibly wrapping number of bytes into destination . The * first segment is composed of bytes at p1. The remaining byte(s), if any, * are read from . may be zero and may also be larger than . The @@ -72,6 +86,27 @@ static void readv_bytes(void *dst, const size_t bytes, const void *p1, size_t s1 __asm__ volatile("" ::: "memory"); } +/* Write a possibly wrapping number of bytes from location . The + * first segment is composed of bytes at p1. The remaining byte(s), if any, + * are written to . may be zero and may also be larger than . + * The caller is always responsible for providing enough room. Note: the + * function is purposely *not* marked inline to let the compiler decide what to + * do with it, because it's around 34 bytes long, placed on critical path but + * rarely called, and uses uses a lot of arguments if not inlined. The compiler + * will thus decide what's best to do with it depending on the context. + */ +static void writev_bytes(const void *src, const size_t bytes, void *p1, size_t s1, void *p2) +{ + size_t idx; + + p2 -= s1; + for (idx = 0; idx < bytes; idx++) { + if (idx == s1) + p1 = p2; + ((uint8_t *)p1)[idx] = ((const uint8_t *)src)[idx]; + } +} + /* Read a possibly wrapping uint16_t in native host order. The first segment is * composed of bytes at p1. The remaining byte(s), if any, are read from * . may be zero and may be larger than the type. The caller is always @@ -94,6 +129,25 @@ static inline uint16_t readv_u16(const void *p1, size_t s1, const void *p2) } } +/* Write a possibly wrapping uint16_t in native host order. The first segment is + * composed of bytes at p1. The remaining byte(s), if any, are written to + * . may be zero and may be larger than the type. The caller is always + * responsible for providing enough room. + */ +static inline void writev_u16(void *p1, size_t s1, void *p2, const uint16_t u16) +{ + union { uint16_t u16; } __attribute__((packed)) *u; + + if (unlikely(s1 == 1)) { + *(uint8_t *)p1 = ((const uint8_t *)&u16)[0]; + *(uint8_t *)p2 = ((const uint8_t *)&u16)[1]; + } + else { + u = (s1 == 0) ? p2 : p1; + u->u16 = u16; + } +} + /* Read a possibly wrapping uint32_t in native host order. The first segment is * composed of bytes at p1. The remaining byte(s), if any, are read from * . may be zero and may be larger than the type. The caller is always @@ -110,6 +164,19 @@ static inline uint32_t readv_u32(const void *p1, size_t s1, const void *p2) return u32; } +/* Write a possibly wrapping uint32_t in native host order. The first segment is + * composed of bytes at p1. The remaining byte(s), if any, are written to + * . may be zero and may be larger than the type. The caller is always + * responsible for providing enough room. + */ +static inline void writev_u32(void *p1, size_t s1, void *p2, const uint32_t u32) +{ + if (!unlikely(s1 < sizeof(u32))) + write_u32(p1, u32); + else + writev_bytes(&u32, sizeof(u32), p1, s1, p2); +} + /* Signed integer versions : return the same data but signed */ /* Read an int16_t in native host order */ @@ -142,12 +209,24 @@ static inline uint16_t read_n16(const void *p) return ntohs(read_u16(p)); } +/* Write a uint16_t after converting it from host order to network order */ +static inline void write_n16(void *p, const uint16_t u16) +{ + write_u16(p, htons(u16)); +} + /* Read a uint32_t, and convert from network order to host order */ static inline uint32_t read_n32(const void *p) { return ntohl(read_u32(p)); } +/* Write a uint32_t after converting it from host order to network order */ +static inline void write_n32(void *p, const uint32_t u32) +{ + write_u32(p, htonl(u32)); +} + /* Read a possibly wrapping uint16_t in network order. The first segment is * composed of bytes at p1. The remaining byte(s), if any, are read from * . may be zero and may be larger than the type. The caller is always @@ -164,6 +243,23 @@ static inline uint16_t readv_n16(const void *p1, size_t s1, const void *p2) return (*(uint8_t *)p1 << 8) + *(uint8_t *)p2; } +/* Write a possibly wrapping uint16_t in network order. The first segment is + * composed of bytes at p1. The remaining byte(s), if any, are written to + * . may be zero and may be larger than the type. The caller is always + * responsible for providing enough room. + */ +static inline void writev_n16(const void *p1, size_t s1, const void *p2, const uint16_t u16) +{ + if (unlikely(s1 < 2)) { + if (s1 == 0) + p1 = p2++; + } + else + p2 = p1 + 1; + *(uint8_t *)p1 = u16 >> 8; + *(uint8_t *)p2 = u16; +} + /* Read a possibly wrapping uint32_t in network order. The first segment is * composed of bytes at p1. The remaining byte(s), if any, are read from * . may be zero and may be larger than the type. The caller is always @@ -174,4 +270,14 @@ static inline uint32_t readv_n32(const void *p1, size_t s1, const void *p2) return ntohl(readv_u32(p1, s1, p2)); } +/* Write a possibly wrapping uint32_t in network order. The first segment is + * composed of bytes at p1. The remaining byte(s), if any, are written to + * . may be zero and may be larger than the type. The caller is always + * responsible for providing enough room. + */ +static inline void writev_n32(void *p1, size_t s1, void *p2, const uint32_t u32) +{ + writev_u32(p1, s1, p2, htonl(u32)); +} + #endif /* COMMON_NET_HELPER_H */