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 <stdio.h>
  32 #include <sys/queue.h>
  33
  34 #include "xbee_neighbor.h"
  35 #include "xbee_stats.h"
  36 #include "xbee_buf.h"
  37 #include "xbee.h"
  38
  39 struct xbee_buf *xbee_buf_alloc(void)
  40 {
  41         struct xbee_buf *xbuf;
  42
  43         xbuf = malloc(sizeof(*xbuf));
  44         if (xbuf == NULL)
  45                 return NULL;
  46         memset(xbuf, 0, sizeof(*xbuf));
  47         xbuf->offset = 0;
  48         xbuf->len = 0;
  49         return xbuf;
  50 }
  51
  52 int xbee_buf_tailroom(struct xbee_buf *xbuf)
  53 {
  54         return XBEE_BUF_SIZE - xbuf->len - xbuf->offset;
  55 }
  56
  57 char *xbee_buf_data(struct xbee_buf *xbuf, unsigned off)
  58 {
  59         if (off >= xbuf->len)
  60                 return NULL;
  61         return xbuf->buf + xbuf->offset + off;
  62 }
  63
  64 char *xbee_buf_head(struct xbee_buf *xbuf)
  65 {
  66         return xbuf->buf + xbuf->offset;
  67 }
  68
  69 char *xbee_buf_tail(struct xbee_buf *xbuf)
  70 {
  71         return xbuf->buf + xbuf->offset + xbuf->len;
  72 }
  73
  74 void xbee_buf_enqueue(struct xbee_bufq *q, struct xbee_buf *xbuf)
  75 {
  76         CIRCLEQ_INSERT_TAIL(&q->xbq, xbuf, next);
  77         q->len += xbuf->len;
  78         q->nseg++;
  79 }
  80
  81 struct xbee_buf *xbee_bufq_last(struct xbee_bufq *q)
  82 {
  83         if (CIRCLEQ_EMPTY(&q->xbq))
  84                 return NULL;
  85         return CIRCLEQ_LAST(&q->xbq);
  86 }
  87
  88 void xbee_bufq_init(struct xbee_bufq *q)
  89 {
  90         CIRCLEQ_INIT(&q->xbq);
  91         q->len = 0;
  92         q->nseg = 0;
  93 }
  94
  95 void xbee_bufq_append(struct xbee_bufq *q, unsigned len)
  96 {
  97         struct xbee_buf *xbuf;
  98
  99         q->len += len;
 100         xbuf = CIRCLEQ_LAST(&q->xbq);
 101         xbuf->len += len;
 102 }
 103
 104 void xbee_bufq_flush(struct xbee_bufq *q)
 105 {
 106         struct xbee_buf *xbuf;
 107
 108         while (!CIRCLEQ_EMPTY(&q->xbq)) {
 109                 xbuf = CIRCLEQ_FIRST(&q->xbq);
 110                 CIRCLEQ_REMOVE(&q->xbq, xbuf, next);
 111                 q->nseg --;
 112                 q->len -= xbuf->len;
 113                 free(xbuf);
 114         }
 115 }
 116
 117 char *xbee_bufq_data(struct xbee_bufq *q, unsigned off)
 118 {
 119         struct xbee_buf *xbuf;
 120         char *data = NULL;
 121
 122         if (off >= q->len)
 123                 return NULL;
 124
 125         CIRCLEQ_FOREACH(xbuf, &q->xbq, next) {
 126                 data = xbee_buf_data(xbuf, off);
 127                 if (data != NULL)
 128                         return data;
 129                 off -= xbuf->len;
 130         }
 131
 132         return data;
 133 }
 134
 135 /* drop data in front of queue */
 136 int xbee_bufq_drop(struct xbee_bufq *q, unsigned len)
 137 {
 138         struct xbee_buf *xbuf;
 139
 140         if (len > q->len)
 141                 return -1;
 142
 143         while (!CIRCLEQ_EMPTY(&q->xbq)) {
 144                 xbuf = CIRCLEQ_FIRST(&q->xbq);
 145                 if (xbuf->len > len)
 146                         break;
 147                 CIRCLEQ_REMOVE(&q->xbq, xbuf, next);
 148                 len -= xbuf->len;
 149                 q->nseg --;
 150                 q->len -= xbuf->len;
 151                 free(xbuf);
 152                 xbuf = NULL;
 153         }
 154
 155         if (xbuf != NULL) {
 156                 xbuf->len -= len;
 157                 xbuf->offset += len;
 158                 q->len -= len;
 159         }
 160
 161         return 0;
 162 }
 163
 164 int xbee_bufq_copy(struct xbee_bufq *q, void *buf, unsigned len)
 165 {
 166         struct xbee_buf *xbuf;
 167         unsigned dstoff = 0, copylen;
 168
 169         if (len > q->len)
 170                 return -1;
 171
 172         CIRCLEQ_FOREACH(xbuf, &q->xbq, next) {
 173                 copylen = len;
 174                 if (xbuf->len < len)
 175                         copylen = xbuf->len;
 176                 memcpy(buf + dstoff, xbuf->buf + xbuf->offset, copylen);
 177                 len -= copylen;
 178                 if (len == 0)
 179                         break;
 180                 dstoff += copylen;
 181         }
 182
 183         return 0;
 184 }
 185