src/shared/ringbuf.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29 #include <string.h>
  30 #include <sys/uio.h>
  31 #include <sys/param.h>
  32
  33 #include "src/shared/util.h"
  34 #include "src/shared/ringbuf.h"
  35
  36 #ifndef MIN
  37 #define MIN(x,y) ((x)<(y)?(x):(y))
  38 #endif
  39
  40 struct ringbuf {
  41         void *buffer;
  42         size_t size;
  43         size_t in;
  44         size_t out;
  45         ringbuf_tracing_func_t in_tracing;
  46         void *in_data;
  47 };
  48
  49 #define RINGBUF_RESET 0
  50
  51 /* Find last (most siginificant) set bit */
  52 static inline unsigned int fls(unsigned int x)
  53 {
  54         return x ? sizeof(x) * 8 - __builtin_clz(x) : 0;
  55 }
  56
  57 /* Round up to nearest power of two */
  58 static inline unsigned int align_power2(unsigned int u)
  59 {
  60         return 1 << fls(u - 1);
  61 }
  62
  63 struct ringbuf *ringbuf_new(size_t size)
  64 {
  65         struct ringbuf *ringbuf;
  66         size_t real_size;
  67
  68         if (size < 2 || size > UINT_MAX)
  69                 return NULL;
  70
  71         /* Find the next power of two for size */
  72         real_size = align_power2(size);
  73
  74         ringbuf = new0(struct ringbuf, 1);
  75         ringbuf->buffer = malloc(real_size);
  76         if (!ringbuf->buffer) {
  77                 free(ringbuf);
  78                 return NULL;
  79         }
  80
  81         ringbuf->size = real_size;
  82         ringbuf->in = RINGBUF_RESET;
  83         ringbuf->out = RINGBUF_RESET;
  84
  85         return ringbuf;
  86 }
  87
  88 void ringbuf_free(struct ringbuf *ringbuf)
  89 {
  90         if (!ringbuf)
  91                 return;
  92
  93         free(ringbuf->buffer);
  94         free(ringbuf);
  95 }
  96
  97 bool ringbuf_set_input_tracing(struct ringbuf *ringbuf,
  98                         ringbuf_tracing_func_t callback, void *user_data)
  99 {
 100         if (!ringbuf)
 101                 return false;
 102
 103         ringbuf->in_tracing = callback;
 104         ringbuf->in_data = user_data;
 105
 106         return true;
 107 }
 108
 109 size_t ringbuf_capacity(struct ringbuf *ringbuf)
 110 {
 111         if (!ringbuf)
 112                 return 0;
 113
 114         return ringbuf->size;
 115 }
 116
 117 size_t ringbuf_len(struct ringbuf *ringbuf)
 118 {
 119         if (!ringbuf)
 120                 return 0;
 121
 122         return ringbuf->in - ringbuf->out;
 123 }
 124
 125 size_t ringbuf_drain(struct ringbuf *ringbuf, size_t count)
 126 {
 127         size_t len;
 128
 129         if (!ringbuf)
 130                 return 0;
 131
 132         len = MIN(count, ringbuf->in - ringbuf->out);
 133         if (!len)
 134                 return 0;
 135
 136         ringbuf->out += len;
 137
 138         if (ringbuf->out == ringbuf->in) {
 139                 ringbuf->in = RINGBUF_RESET;
 140                 ringbuf->out = RINGBUF_RESET;
 141         }
 142
 143         return len;
 144 }
 145
 146 void *ringbuf_peek(struct ringbuf *ringbuf, size_t offset, size_t *len_nowrap)
 147 {
 148         if (!ringbuf)
 149                 return NULL;
 150
 151         offset = (ringbuf->out + offset) & (ringbuf->size - 1);
 152
 153         if (len_nowrap) {
 154                 size_t len = ringbuf->in - ringbuf->out;
 155                 *len_nowrap = MIN(len, ringbuf->size - offset);
 156         }
 157
 158         return ringbuf->buffer + offset;
 159 }
 160
 161 ssize_t ringbuf_write(struct ringbuf *ringbuf, int fd)
 162 {
 163         size_t len, offset, end;
 164         struct iovec iov[2];
 165         ssize_t consumed;
 166
 167         if (!ringbuf || fd < 0)
 168                 return -1;
 169
 170         /* Determine how much data is available */
 171         len = ringbuf->in - ringbuf->out;
 172         if (!len)
 173                 return 0;
 174
 175         /* Grab data from buffer starting at offset until the end */
 176         offset = ringbuf->out & (ringbuf->size - 1);
 177         end = MIN(len, ringbuf->size - offset);
 178
 179         iov[0].iov_base = ringbuf->buffer + offset;
 180         iov[0].iov_len = end;
 181
 182         /* Use second vector for remainder from the beginning */
 183         iov[1].iov_base = ringbuf->buffer;
 184         iov[1].iov_len = len - end;
 185
 186         consumed = writev(fd, iov, 2);
 187         if (consumed < 0)
 188                 return -1;
 189
 190         ringbuf->out += consumed;
 191
 192         if (ringbuf->out == ringbuf->in) {
 193                 ringbuf->in = RINGBUF_RESET;
 194                 ringbuf->out = RINGBUF_RESET;
 195         }
 196
 197         return consumed;
 198 }
 199
 200 size_t ringbuf_avail(struct ringbuf *ringbuf)
 201 {
 202         if (!ringbuf)
 203                 return 0;
 204
 205         return ringbuf->size - ringbuf->in + ringbuf->out;
 206 }
 207
 208 int ringbuf_printf(struct ringbuf *ringbuf, const char *format, ...)
 209 {
 210         va_list ap;
 211         int len;
 212
 213         va_start(ap, format);
 214         len = ringbuf_vprintf(ringbuf, format, ap);
 215         va_end(ap);
 216
 217         return len;
 218 }
 219
 220 int ringbuf_vprintf(struct ringbuf *ringbuf, const char *format, va_list ap)
 221 {
 222         size_t avail, offset, end;
 223         char *str;
 224         int len;
 225
 226         if (!ringbuf || !format)
 227                 return -1;
 228
 229         /* Determine maximum length available for string */
 230         avail = ringbuf->size - ringbuf->in + ringbuf->out;
 231         if (!avail)
 232                 return -1;
 233
 234         len = vasprintf(&str, format, ap);
 235         if (len < 0)
 236                 return -1;
 237
 238         if ((size_t) len > avail) {
 239                 free(str);
 240                 return -1;
 241         }
 242
 243         /* Determine possible length of string before wrapping */
 244         offset = ringbuf->in & (ringbuf->size - 1);
 245         end = MIN((size_t) len, ringbuf->size - offset);
 246         memcpy(ringbuf->buffer + offset, str, end);
 247
 248         if (ringbuf->in_tracing)
 249                 ringbuf->in_tracing(ringbuf->buffer + offset, end,
 250                                                         ringbuf->in_data);
 251
 252         if (len - end > 0) {
 253                 /* Put the remainder of string at the beginning */
 254                 memcpy(ringbuf->buffer, str + end, len - end);
 255
 256                 if (ringbuf->in_tracing)
 257                         ringbuf->in_tracing(ringbuf->buffer, len - end,
 258                                                         ringbuf->in_data);
 259         }
 260
 261         free(str);
 262
 263         ringbuf->in += len;
 264
 265         return len;
 266 }
 267
 268 ssize_t ringbuf_read(struct ringbuf *ringbuf, int fd)
 269 {
 270         size_t avail, offset, end;
 271         struct iovec iov[2];
 272         ssize_t consumed;
 273
 274         if (!ringbuf || fd < 0)
 275                 return -1;
 276
 277         /* Determine how much can actually be consumed */
 278         avail = ringbuf->size - ringbuf->in + ringbuf->out;
 279         if (!avail)
 280                 return -1;
 281
 282         /* Determine how much to consume before wrapping */
 283         offset = ringbuf->in & (ringbuf->size - 1);
 284         end = MIN(avail, ringbuf->size - offset);
 285
 286         iov[0].iov_base = ringbuf->buffer + offset;
 287         iov[0].iov_len = end;
 288
 289         /* Now put the remainder into the second vector */
 290         iov[1].iov_base = ringbuf->buffer;
 291         iov[1].iov_len = avail - end;
 292
 293         consumed = readv(fd, iov, 2);
 294         if (consumed < 0)
 295                 return -1;
 296
 297         if (ringbuf->in_tracing) {
 298                 size_t len = MIN((size_t) consumed, end);
 299                 ringbuf->in_tracing(ringbuf->buffer + offset, len,
 300                                                         ringbuf->in_data);
 301                 if (consumed - len > 0)
 302                         ringbuf->in_tracing(ringbuf->buffer, consumed - len,
 303                                                         ringbuf->in_data);
 304         }
 305
 306         ringbuf->in += consumed;
 307
 308         return consumed;
 309 }