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