nhrpd/zbuf.c

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