xbee_buf.c

   1 /*
   2  * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
   3  * All rights reserved.
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions are met:
   6  *
   7  *     * Redistributions of source code must retain the above copyright
   8  *       notice, this list of conditions and the following disclaimer.
   9  *     * Redistributions in binary form must reproduce the above copyright
  10  *       notice, this list of conditions and the following disclaimer in the
  11  *       documentation and/or other materials provided with the distribution.
  12  *     * Neither the name of the University of California, Berkeley nor the
  13  *       names of its contributors may be used to endorse or promote products
  14  *       derived from this software without specific prior written permission.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
  17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19  * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
  20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26  */
  27
  28 #include <string.h>
  29 #include <stdlib.h>
  30 #include <stdint.h>
  31 #include <sys/queue.h>
  32
  33 #include "xbee_neighbor.h"
  34 #include "xbee_stats.h"
  35 #include "xbee_buf.h"
  36 #include "xbee.h"
  37
  38 struct xbee_buf *xbee_buf_alloc(void)
  39 {
  40         struct xbee_buf *xbuf;
  41
  42         xbuf = malloc(sizeof(*xbuf));
  43         if (xbuf == NULL)
  44                 return NULL;
  45         memset(xbuf, 0, sizeof(*xbuf));
  46         xbuf->offset = 0;
  47         xbuf->len = 0;
  48         return xbuf;
  49 }
  50
  51 int xbee_buf_tailroom(struct xbee_buf *xbuf)
  52 {
  53         return XBEE_BUF_SIZE - xbuf->len - xbuf->offset;
  54 }
  55
  56 char *xbee_buf_data(struct xbee_buf *xbuf, unsigned off)
  57 {
  58         if (off >= xbuf->len)
  59                 return NULL;
  60         return xbuf->buf + xbuf->offset + off;
  61 }
  62
  63 char *xbee_buf_head(struct xbee_buf *xbuf)
  64 {
  65         return xbuf->buf + xbuf->offset;
  66 }
  67
  68 char *xbee_buf_tail(struct xbee_buf *xbuf)
  69 {
  70         return xbuf->buf + xbuf->offset + xbuf->len;
  71 }
  72
  73 void xbee_buf_enqueue(struct xbee_bufq *q, struct xbee_buf *xbuf)
  74 {
  75         CIRCLEQ_INSERT_TAIL(&q->xbq, xbuf, next);
  76         q->len += xbuf->len;
  77         q->nseg++;
  78 }
  79
  80 struct xbee_buf *xbee_bufq_last(struct xbee_bufq *q)
  81 {
  82         if (CIRCLEQ_EMPTY(&q->xbq))
  83                 return NULL;
  84         return CIRCLEQ_LAST(&q->xbq);
  85 }
  86
  87 void xbee_bufq_init(struct xbee_bufq *q)
  88 {
  89         CIRCLEQ_INIT(&q->xbq);
  90         q->len = 0;
  91         q->nseg = 0;
  92 }
  93
  94 void xbee_bufq_append(struct xbee_bufq *q, unsigned len)
  95 {
  96         struct xbee_buf *xbuf;
  97
  98         q->len += len;
  99         xbuf = CIRCLEQ_LAST(&q->xbq);
 100         xbuf->len += len;
 101 }
 102
 103 void xbee_bufq_flush(struct xbee_bufq *q)
 104 {
 105         struct xbee_buf *xbuf;
 106
 107         while (!CIRCLEQ_EMPTY(&q->xbq)) {
 108                 xbuf = CIRCLEQ_FIRST(&q->xbq);
 109                 CIRCLEQ_REMOVE(&q->xbq, xbuf, next);
 110                 q->nseg --;
 111                 q->len -= xbuf->len;
 112                 free(xbuf);
 113         }
 114 }
 115
 116 char *xbee_bufq_data(struct xbee_bufq *q, unsigned off)
 117 {
 118         struct xbee_buf *xbuf;
 119         char *data = NULL;
 120
 121         if (off >= q->len)
 122                 return NULL;
 123
 124         CIRCLEQ_FOREACH(xbuf, &q->xbq, next) {
 125                 data = xbee_buf_data(xbuf, off);
 126                 if (data != NULL)
 127                         return data;
 128                 off -= xbuf->len;
 129         }
 130
 131         return data;
 132 }
 133
 134 /* drop data in front of queue */
 135 int xbee_bufq_drop(struct xbee_bufq *q, unsigned len)
 136 {
 137         struct xbee_buf *xbuf;
 138
 139         if (len > q->len)
 140                 return -1;
 141
 142         while (!CIRCLEQ_EMPTY(&q->xbq)) {
 143                 xbuf = CIRCLEQ_FIRST(&q->xbq);
 144                 if (xbuf->len > len)
 145                         break;
 146                 CIRCLEQ_REMOVE(&q->xbq, xbuf, next);
 147                 len -= xbuf->len;
 148                 q->nseg --;
 149                 q->len -= xbuf->len;
 150                 free(xbuf);
 151                 xbuf = NULL;
 152         }
 153
 154         if (xbuf != NULL) {
 155                 xbuf->len -= len;
 156                 xbuf->offset += len;
 157                 q->len -= len;
 158         }
 159
 160         return 0;
 161 }
 162
 163 int xbee_bufq_copy(struct xbee_bufq *q, void *buf, unsigned len)
 164 {
 165         struct xbee_buf *xbuf;
 166         unsigned dstoff = 0, copylen;
 167
 168         if (len > q->len)
 169                 return -1;
 170
 171         CIRCLEQ_FOREACH(xbuf, &q->xbq, next) {
 172                 copylen = len;
 173                 if (xbuf->len < len)
 174                         copylen = xbuf->len;
 175                 memcpy(buf + dstoff, xbuf->buf + xbuf->offset, copylen);
 176                 len -= copylen;
 177                 if (len == 0)
 178                         break;
 179                 dstoff += copylen;
 180         }
 181
 182         return 0;
 183 }
 184