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