mbuf.c 5.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246
  1. /*
  2. * Copyright (c) 2018-2019 Richard Braun.
  3. *
  4. * This program is free software: you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation, either version 3 of the License, or
  7. * (at your option) any later version.
  8. *
  9. * This program is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. * GNU General Public License for more details.
  13. *
  14. * You should have received a copy of the GNU General Public License
  15. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16. *
  17. * Upstream site with license notes :
  18. * http://git.sceen.net/rbraun/librbraun.git/
  19. */
  20. #include <assert.h>
  21. #include <errno.h>
  22. #include <limits.h>
  23. #include <stdbool.h>
  24. #include <stddef.h>
  25. #include <stdint.h>
  26. #include <kern/macros.h>
  27. #include <kern/mbuf.h>
  28. /*
  29. * Message header.
  30. *
  31. * The size denotes the size of the data, without the header.
  32. */
  33. union mbuf_hdr
  34. {
  35. uint8_t size1;
  36. uint16_t size2;
  37. uint32_t size4;
  38. #ifdef __LP64__
  39. uint64_t size8;
  40. #endif
  41. };
  42. static size_t
  43. mbuf_compute_hdr_size (unsigned int order)
  44. {
  45. return (order / CHAR_BIT);
  46. }
  47. static void*
  48. mbuf_hdr_get_msg_size_addr (union mbuf_hdr *hdr, unsigned int order)
  49. {
  50. switch (order)
  51. {
  52. case 8:
  53. return (&hdr->size1);
  54. case 16:
  55. return (&hdr->size2);
  56. default:
  57. #ifdef __LP64__
  58. return (&hdr->size8);
  59. case 32:
  60. #endif
  61. return (&hdr->size4);
  62. }
  63. }
  64. static size_t
  65. mbuf_hdr_get_msg_size (const union mbuf_hdr *hdr, unsigned int order)
  66. {
  67. switch (order)
  68. {
  69. case 8:
  70. return (hdr->size1);
  71. case 16:
  72. return (hdr->size2);
  73. default:
  74. #ifdef __LP64__
  75. return (hdr->size8);
  76. case 32:
  77. #endif
  78. return (hdr->size4);
  79. }
  80. }
  81. static void
  82. mbuf_hdr_init (union mbuf_hdr *hdr, unsigned int order, size_t size)
  83. {
  84. switch (order)
  85. {
  86. case 8:
  87. assert (size <= UINT8_MAX);
  88. hdr->size1 = size;
  89. break;
  90. case 16:
  91. assert (size <= UINT16_MAX);
  92. hdr->size2 = size;
  93. break;
  94. default:
  95. #ifdef __LP64__
  96. hdr->size8 = size;
  97. break;
  98. case 32:
  99. assert (size <= UINT32_MAX);
  100. #endif
  101. hdr->size4 = size;
  102. }
  103. }
  104. static size_t
  105. mbuf_hdr_get_total_msg_size (const union mbuf_hdr *hdr, unsigned int order)
  106. {
  107. return (mbuf_compute_hdr_size (order) + mbuf_hdr_get_msg_size (hdr, order));
  108. }
  109. static unsigned int
  110. mbuf_compute_order (size_t max_msg_size)
  111. {
  112. assert (max_msg_size);
  113. unsigned int order = (sizeof (max_msg_size) * CHAR_BIT) -
  114. __builtin_clzl (max_msg_size);
  115. return (P2ROUND (order, CHAR_BIT));
  116. }
  117. void
  118. mbuf_init (struct mbuf *mbuf, void *buf, size_t capacity, size_t max_msg_size)
  119. {
  120. cbuf_init (&mbuf->cbuf, buf, capacity);
  121. mbuf->max_msg_size = max_msg_size;
  122. mbuf->order = mbuf_compute_order (max_msg_size);
  123. }
  124. void
  125. mbuf_clear (struct mbuf *mbuf)
  126. {
  127. return (cbuf_clear (&mbuf->cbuf));
  128. }
  129. static void
  130. mbuf_clear_old_msgs (struct mbuf *mbuf, size_t total_size)
  131. {
  132. union mbuf_hdr hdr;
  133. size_t hdr_size = mbuf_compute_hdr_size (mbuf->order);
  134. void *msg_size_addr = mbuf_hdr_get_msg_size_addr (&hdr, mbuf->order);
  135. do
  136. {
  137. size_t size = hdr_size;
  138. int error = cbuf_pop (&mbuf->cbuf, msg_size_addr, &size);
  139. assert (! error);
  140. if (! size)
  141. break;
  142. size_t msg_size = mbuf_hdr_get_msg_size (&hdr, mbuf->order);
  143. size = msg_size;
  144. error = cbuf_pop (&mbuf->cbuf, NULL, &size);
  145. assert (!error && size == msg_size);
  146. }
  147. while (cbuf_avail_size (&mbuf->cbuf) < total_size);
  148. }
  149. int
  150. mbuf_push (struct mbuf *mbuf, const void *buf, size_t size, bool erase)
  151. {
  152. if (size > mbuf->max_msg_size)
  153. return (EINVAL);
  154. union mbuf_hdr hdr;
  155. mbuf_hdr_init (&hdr, mbuf->order, size);
  156. size_t total_size = mbuf_hdr_get_total_msg_size (&hdr, mbuf->order);
  157. if (total_size > cbuf_avail_size (&mbuf->cbuf))
  158. {
  159. if (!erase || total_size > cbuf_capacity (&mbuf->cbuf))
  160. return (EMSGSIZE);
  161. mbuf_clear_old_msgs (mbuf, total_size);
  162. }
  163. size_t hdr_size = mbuf_compute_hdr_size (mbuf->order);
  164. void *msg_size_addr = mbuf_hdr_get_msg_size_addr (&hdr, mbuf->order);
  165. int error = cbuf_push (&mbuf->cbuf, msg_size_addr, hdr_size, erase);
  166. assert (! error);
  167. error = cbuf_push (&mbuf->cbuf, buf, size, erase);
  168. assert (! error);
  169. return (0);
  170. }
  171. int
  172. mbuf_pop (struct mbuf *mbuf, void *buf, size_t *sizep)
  173. {
  174. size_t start = cbuf_start (&mbuf->cbuf);
  175. int error = mbuf_read (mbuf, &start, buf, sizep);
  176. if (! error)
  177. cbuf_set_start (&mbuf->cbuf, start);
  178. return (error);
  179. }
  180. int
  181. mbuf_read (const struct mbuf *mbuf, size_t *indexp, void *buf, size_t *sizep)
  182. {
  183. union mbuf_hdr hdr;
  184. size_t hdr_size = mbuf_compute_hdr_size (mbuf->order);
  185. void *msg_size_addr = mbuf_hdr_get_msg_size_addr (&hdr, mbuf->order);
  186. size_t size = hdr_size;
  187. int error = cbuf_read (&mbuf->cbuf, *indexp, msg_size_addr, &size);
  188. if (error)
  189. return (error);
  190. else if (size == 0)
  191. return (EAGAIN);
  192. assert (size == hdr_size);
  193. size_t msg_size = mbuf_hdr_get_msg_size (&hdr, mbuf->order);
  194. if (msg_size > *sizep)
  195. {
  196. error = EMSGSIZE;
  197. goto out;
  198. }
  199. size = msg_size;
  200. error = cbuf_read (&mbuf->cbuf, *indexp + hdr_size, buf, &size);
  201. if (error)
  202. goto out;
  203. assert (size == msg_size);
  204. *indexp += hdr_size + size;
  205. out:
  206. *sizep = msg_size;
  207. return (error);
  208. }