nhrpd/zbuf.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /* Stream/packet buffer API implementation
   3  * Copyright (c) 2014-2015 Timo Teräs
   4  */
   5
   6 #ifdef HAVE_CONFIG_H
   7 #include "config.h"
   8 #endif
   9
  10 #include <string.h>
  11 #include <unistd.h>
  12 #include <errno.h>
  13 #include <assert.h>
  14 #include "zbuf.h"
  15 #include "memory.h"
  16 #include "nhrpd.h"
  17
  18 #define ERRNO_IO_RETRY(EN) (((EN) == EAGAIN) || ((EN) == EWOULDBLOCK) || ((EN) == EINTR))
  19
  20 DEFINE_MTYPE_STATIC(NHRPD, ZBUF_DATA, "NHRPD zbuf data");
  21
  22 struct zbuf *zbuf_alloc(size_t size)
  23 {
  24         struct zbuf *zb;
  25
  26         zb = XCALLOC(MTYPE_ZBUF_DATA, sizeof(*zb) + size);
  27
  28         zbuf_init(zb, zb + 1, size, 0);
  29         zb->allocated = 1;
  30
  31         return zb;
  32 }
  33
  34 void zbuf_init(struct zbuf *zb, void *buf, size_t len, size_t datalen)
  35 {
  36         *zb = (struct zbuf){
  37                 .buf = buf,
  38                 .end = (uint8_t *)buf + len,
  39                 .head = buf,
  40                 .tail = (uint8_t *)buf + datalen,
  41         };
  42 }
  43
  44 void zbuf_free(struct zbuf *zb)
  45 {
  46         if (zb->allocated)
  47                 XFREE(MTYPE_ZBUF_DATA, zb);
  48 }
  49
  50 void zbuf_reset(struct zbuf *zb)
  51 {
  52         zb->head = zb->tail = zb->buf;
  53         zb->error = 0;
  54 }
  55
  56 void zbuf_reset_head(struct zbuf *zb, void *ptr)
  57 {
  58         assert((void *)zb->buf <= ptr && ptr <= (void *)zb->tail);
  59         zb->head = ptr;
  60 }
  61
  62 static void zbuf_remove_headroom(struct zbuf *zb)
  63 {
  64         ssize_t headroom = zbuf_headroom(zb);
  65         if (!headroom)
  66                 return;
  67         memmove(zb->buf, zb->head, zbuf_used(zb));
  68         zb->head -= headroom;
  69         zb->tail -= headroom;
  70 }
  71
  72 ssize_t zbuf_read(struct zbuf *zb, int fd, size_t maxlen)
  73 {
  74         ssize_t r;
  75
  76         if (zb->error)
  77                 return -3;
  78
  79         zbuf_remove_headroom(zb);
  80         if (maxlen > zbuf_tailroom(zb))
  81                 maxlen = zbuf_tailroom(zb);
  82
  83         r = read(fd, zb->tail, maxlen);
  84         if (r > 0)
  85                 zb->tail += r;
  86         else if (r == 0)
  87                 r = -2;
  88         else if (ERRNO_IO_RETRY(errno))
  89                 r = 0;
  90
  91         return r;
  92 }
  93
  94 ssize_t zbuf_write(struct zbuf *zb, int fd)
  95 {
  96         ssize_t r;
  97
  98         if (zb->error)
  99                 return -3;
 100
 101         r = write(fd, zb->head, zbuf_used(zb));
 102         if (r > 0) {
 103                 zb->head += r;
 104                 if (zb->head == zb->tail)
 105                         zbuf_reset(zb);
 106         } else if (r == 0)
 107                 r = -2;
 108         else if (ERRNO_IO_RETRY(errno))
 109                 r = 0;
 110
 111         return r;
 112 }
 113
 114 ssize_t zbuf_recv(struct zbuf *zb, int fd)
 115 {
 116         ssize_t r;
 117
 118         if (zb->error)
 119                 return -3;
 120
 121         zbuf_remove_headroom(zb);
 122         r = recv(fd, zb->tail, zbuf_tailroom(zb), 0);
 123         if (r > 0)
 124                 zb->tail += r;
 125         else if (r == 0)
 126                 r = -2;
 127         else if (ERRNO_IO_RETRY(errno))
 128                 r = 0;
 129         return r;
 130 }
 131
 132 ssize_t zbuf_send(struct zbuf *zb, int fd)
 133 {
 134         ssize_t r;
 135
 136         if (zb->error)
 137                 return -3;
 138
 139         r = send(fd, zb->head, zbuf_used(zb), 0);
 140         if (r >= 0)
 141                 zbuf_reset(zb);
 142
 143         return r;
 144 }
 145
 146 void *zbuf_may_pull_until(struct zbuf *zb, const char *sep, struct zbuf *msg)
 147 {
 148         size_t seplen = strlen(sep), len;
 149         uint8_t *ptr;
 150
 151         ptr = memmem(zb->head, zbuf_used(zb), sep, seplen);
 152         if (!ptr)
 153                 return NULL;
 154
 155         len = ptr - zb->head + seplen;
 156         zbuf_init(msg, zbuf_pulln(zb, len), len, len);
 157         return msg->head;
 158 }
 159
 160 void zbufq_init(struct zbuf_queue *zbq)
 161 {
 162         *zbq = (struct zbuf_queue){
 163                 .queue_head = INIT_DLIST(zbq->queue_head),
 164         };
 165 }
 166
 167 void zbufq_reset(struct zbuf_queue *zbq)
 168 {
 169         struct zbuf *buf;
 170
 171         frr_each_safe (zbuf_queue, &zbq->queue_head, buf) {
 172                 zbuf_queue_del(&zbq->queue_head, buf);
 173                 zbuf_free(buf);
 174         }
 175 }
 176
 177 void zbufq_queue(struct zbuf_queue *zbq, struct zbuf *zb)
 178 {
 179         zbuf_queue_add_tail(&zbq->queue_head, zb);
 180 }
 181
 182 int zbufq_write(struct zbuf_queue *zbq, int fd)
 183 {
 184         struct iovec iov[16];
 185         struct zbuf *zb;
 186         ssize_t r;
 187         size_t iovcnt = 0;
 188
 189         frr_each_safe (zbuf_queue, &zbq->queue_head, zb) {
 190                 iov[iovcnt++] = (struct iovec){
 191                         .iov_base = zb->head, .iov_len = zbuf_used(zb),
 192                 };
 193                 if (iovcnt >= array_size(iov))
 194                         break;
 195         }
 196
 197         r = writev(fd, iov, iovcnt);
 198         if (r < 0)
 199                 return r;
 200
 201         frr_each_safe (zbuf_queue, &zbq->queue_head, zb) {
 202                 if (r < (ssize_t)zbuf_used(zb)) {
 203                         zb->head += r;
 204                         return 1;
 205                 }
 206
 207                 r -= zbuf_used(zb);
 208                 zbuf_queue_del(&zbq->queue_head, zb);
 209                 zbuf_free(zb);
 210         }
 211
 212         return 0;
 213 }
 214
 215 void zbuf_copy(struct zbuf *zdst, struct zbuf *zsrc, size_t len)
 216 {
 217         const void *src;
 218         void *dst;
 219
 220         dst = zbuf_pushn(zdst, len);
 221         src = zbuf_pulln(zsrc, len);
 222         if (!dst || !src)
 223                 return;
 224         memcpy(dst, src, len);
 225 }
 226
 227 void zbuf_copy_peek(struct zbuf *zdst, struct zbuf *zsrc, size_t len)
 228 {
 229         const void *src;
 230         void *dst;
 231
 232         dst = zbuf_pushn(zdst, len);
 233         src = zbuf_pulln(zsrc, 0);
 234         if (!dst || !src)
 235                 return;
 236         memcpy(dst, src, len);
 237 }