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