zbuf.c 4.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219
  1. /* Stream/packet buffer API implementation
  2. * Copyright (c) 2014-2015 Timo Teräs
  3. *
  4. * This file is free software: you may copy, redistribute and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation, either version 2 of the License, or
  7. * (at your option) any later version.
  8. */
  9. #define _GNU_SOURCE
  10. #include <string.h>
  11. #include <unistd.h>
  12. #include <errno.h>
  13. #include "zassert.h"
  14. #include "zbuf.h"
  15. #include "memory.h"
  16. #include "memtypes.h"
  17. #include "nhrpd.h"
  18. #define ERRNO_IO_RETRY(EN) (((EN) == EAGAIN) || ((EN) == EWOULDBLOCK) || ((EN) == EINTR))
  19. struct zbuf *zbuf_alloc(size_t size)
  20. {
  21. struct zbuf *zb;
  22. zb = XMALLOC(MTYPE_STREAM_DATA, sizeof(*zb) + size);
  23. if (!zb)
  24. return NULL;
  25. zbuf_init(zb, zb+1, size, 0);
  26. zb->allocated = 1;
  27. return zb;
  28. }
  29. void zbuf_init(struct zbuf *zb, void *buf, size_t len, size_t datalen)
  30. {
  31. *zb = (struct zbuf) {
  32. .buf = buf,
  33. .end = (uint8_t *)buf + len,
  34. .head = buf,
  35. .tail = (uint8_t *)buf + datalen,
  36. };
  37. }
  38. void zbuf_free(struct zbuf *zb)
  39. {
  40. if (zb->allocated)
  41. XFREE(MTYPE_STREAM_DATA, zb);
  42. }
  43. void zbuf_reset(struct zbuf *zb)
  44. {
  45. zb->head = zb->tail = zb->buf;
  46. zb->error = 0;
  47. }
  48. void zbuf_reset_head(struct zbuf *zb, void *ptr)
  49. {
  50. zassert((void*)zb->buf <= ptr && ptr <= (void*)zb->tail);
  51. zb->head = ptr;
  52. }
  53. static void zbuf_remove_headroom(struct zbuf *zb)
  54. {
  55. ssize_t headroom = zbuf_headroom(zb);
  56. if (!headroom)
  57. return;
  58. memmove(zb->buf, zb->head, zbuf_used(zb));
  59. zb->head -= headroom;
  60. zb->tail -= headroom;
  61. }
  62. ssize_t zbuf_read(struct zbuf *zb, int fd, size_t maxlen)
  63. {
  64. ssize_t r;
  65. if (zb->error)
  66. return -3;
  67. zbuf_remove_headroom(zb);
  68. if (maxlen > zbuf_tailroom(zb))
  69. maxlen = zbuf_tailroom(zb);
  70. r = read(fd, zb->tail, maxlen);
  71. if (r > 0) zb->tail += r;
  72. else if (r == 0) r = -2;
  73. else if (r < 0 && ERRNO_IO_RETRY(errno)) r = 0;
  74. return r;
  75. }
  76. ssize_t zbuf_write(struct zbuf *zb, int fd)
  77. {
  78. ssize_t r;
  79. if (zb->error)
  80. return -3;
  81. r = write(fd, zb->head, zbuf_used(zb));
  82. if (r > 0) {
  83. zb->head += r;
  84. if (zb->head == zb->tail)
  85. zbuf_reset(zb);
  86. }
  87. else if (r == 0) r = -2;
  88. else if (r < 0 && ERRNO_IO_RETRY(errno)) r = 0;
  89. return r;
  90. }
  91. ssize_t zbuf_recv(struct zbuf *zb, int fd)
  92. {
  93. ssize_t r;
  94. if (zb->error)
  95. return -3;
  96. zbuf_remove_headroom(zb);
  97. r = recv(fd, zb->tail, zbuf_tailroom(zb), 0);
  98. if (r > 0) zb->tail += r;
  99. else if (r == 0) r = -2;
  100. else if (r < 0 && ERRNO_IO_RETRY(errno)) r = 0;
  101. return r;
  102. }
  103. ssize_t zbuf_send(struct zbuf *zb, int fd)
  104. {
  105. ssize_t r;
  106. if (zb->error)
  107. return -3;
  108. r = send(fd, zb->head, zbuf_used(zb), 0);
  109. if (r >= 0)
  110. zbuf_reset(zb);
  111. return r;
  112. }
  113. void *zbuf_may_pull_until(struct zbuf *zb, const char *sep, struct zbuf *msg)
  114. {
  115. size_t seplen = strlen(sep), len;
  116. uint8_t *ptr;
  117. ptr = memmem(zb->head, zbuf_used(zb), sep, seplen);
  118. if (!ptr) return NULL;
  119. len = ptr - zb->head + seplen;
  120. zbuf_init(msg, zbuf_pulln(zb, len), len, len);
  121. return msg->head;
  122. }
  123. void zbufq_init(struct zbuf_queue *zbq)
  124. {
  125. *zbq = (struct zbuf_queue) {
  126. .queue_head = LIST_INITIALIZER(zbq->queue_head),
  127. };
  128. }
  129. void zbufq_reset(struct zbuf_queue *zbq)
  130. {
  131. struct zbuf *buf, *bufn;
  132. list_for_each_entry_safe(buf, bufn, &zbq->queue_head, queue_list) {
  133. list_del(&buf->queue_list);
  134. zbuf_free(buf);
  135. }
  136. }
  137. void zbufq_queue(struct zbuf_queue *zbq, struct zbuf *zb)
  138. {
  139. list_add_tail(&zb->queue_list, &zbq->queue_head);
  140. }
  141. int zbufq_write(struct zbuf_queue *zbq, int fd)
  142. {
  143. struct iovec iov[16];
  144. struct zbuf *zb, *zbn;
  145. ssize_t r;
  146. size_t iovcnt = 0;
  147. list_for_each_entry_safe(zb, zbn, &zbq->queue_head, queue_list) {
  148. iov[iovcnt++] = (struct iovec) {
  149. .iov_base = zb->head,
  150. .iov_len = zbuf_used(zb),
  151. };
  152. if (iovcnt >= ZEBRA_NUM_OF(iov))
  153. break;
  154. }
  155. r = writev(fd, iov, iovcnt);
  156. if (r < 0)
  157. return r;
  158. list_for_each_entry_safe(zb, zbn, &zbq->queue_head, queue_list) {
  159. if (r < (ssize_t)zbuf_used(zb)) {
  160. zb->head += r;
  161. return 1;
  162. }
  163. r -= zbuf_used(zb);
  164. list_del(&zb->queue_list);
  165. zbuf_free(zb);
  166. }
  167. return 0;
  168. }
  169. void zbuf_copy(struct zbuf *zdst, struct zbuf *zsrc, size_t len)
  170. {
  171. const void *src;
  172. void *dst;
  173. dst = zbuf_pushn(zdst, len);
  174. src = zbuf_pulln(zsrc, len);
  175. if (!dst || !src) return;
  176. memcpy(dst, src, len);
  177. }