support servo control through SPI
[protos/xbee-avr.git] / 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