2 * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions are met:
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.
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.
34 #include <sys/queue.h>
35 #include <arpa/inet.h>
42 #include "xbee_neighbor.h"
43 #include "xbee_stats.h"
45 #include "xbee_proto.h"
48 static void xbee_proto_drop_garbage(struct xbee_dev *dev)
50 struct xbee_bufq *q = &dev->queue;
53 /* drop all data != delimiter */
54 while ((data = xbee_bufq_data(q, 0))) {
59 if (*data == XBEE_DELIMITER)
62 dev->stats.rx_no_delim++;
67 /* return negative on error, 0 if there is not frame, or framelen */
68 int xbee_proto_get_frame(struct xbee_dev *dev, void *buf, unsigned len)
72 struct xbee_bufq *q = &dev->queue;
74 xbee_proto_drop_garbage(dev);
76 if (xbee_bufq_copy(q, &hdr, sizeof(hdr)) < 0)
79 framelen = ntohs(hdr.len);
80 framelen += 4; /* 1 for delimiter, 2 for len, 1 for cksum */
82 /* not enough data to read */
83 if (q->len < framelen)
86 /* arf, provided buffer is to small */
88 fprintf(stderr, "drop packet, buffer too small\n");
89 dev->stats.rx_frame_too_large++;
90 xbee_bufq_drop(q, framelen);
94 xbee_bufq_copy(q, buf, framelen);
95 xbee_bufq_drop(q, framelen);
99 /* return -1 if the frame is invalid */
100 static int xbee_proto_parse_atresp(struct xbee_dev *dev, void *buf,
103 struct xbee_atresp_hdr *atresp_hdr;
105 dev->stats.rx_atresp++;
107 if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_atresp_hdr)) {
108 dev->stats.rx_frame_too_small++;
112 atresp_hdr = buf + sizeof(struct xbee_hdr);
114 /* bad status, but let the frame continue */
115 if (atresp_hdr->status != 0)
116 dev->stats.rx_atresp_error++;
121 /* return -1 if the frame is invalid */
122 static int xbee_proto_parse_rmt_atresp(struct xbee_dev *dev, void *buf,
125 struct xbee_rmt_atresp_hdr *rmt_atresp_hdr;
127 dev->stats.rx_rmt_atresp++;
129 if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_rmt_atresp_hdr)) {
130 dev->stats.rx_frame_too_small++;
134 rmt_atresp_hdr = buf + sizeof(struct xbee_hdr);
136 /* bad status, but let the frame continue */
137 if (rmt_atresp_hdr->status != 0)
138 dev->stats.rx_rmt_atresp_error++;
143 /* return -1 if the frame is invalid */
144 static int xbee_proto_parse_xmit_status(struct xbee_dev *dev, void *buf,
147 struct xbee_xmit_status_hdr *xmit_status_hdr;
149 dev->stats.rx_xmit_status++;
151 if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_xmit_status_hdr)) {
152 dev->stats.rx_frame_too_small++;
156 xmit_status_hdr = buf + sizeof(struct xbee_hdr);
157 dev->stats.tx_xmit_retries += xmit_status_hdr->xmit_retry_cnt;
159 /* bad status, but let the frame continue */
160 if (xmit_status_hdr->delivery_status != 0)
161 dev->stats.rx_xmit_status_error++;
166 /* parse a frame: return 0 if the frame is valid, else a negative value */
168 int xbee_proto_parse_frame(struct xbee_dev *dev, void *buf, unsigned len)
170 struct xbee_hdr *hdr = buf;
176 dev->stats.rx_frame++;
178 /* check frame len: we must be able to read frame type */
179 if (len < (offsetof(struct xbee_hdr, type) + 1)) {
180 dev->stats.rx_frame_too_small++;
181 fprintf(stderr, "Frame too small\n");
186 case XBEE_TYPE_MODEM_STATUS:
188 case XBEE_TYPE_EXPL_RECV:
189 hdrlen = sizeof(struct xbee_hdr) - 1; /* no frame ID */
192 hdrlen = sizeof(struct xbee_hdr);
196 /* check frame len */
197 if (len < (hdrlen + 1)) {
198 dev->stats.rx_frame_too_small++;
199 fprintf(stderr, "Frame too small\n");
203 /* validate the cksum */
204 for (i = 3; i < (len - 1); i++)
205 cksum += ((uint8_t *)buf)[i];
206 cksum = 0xff - cksum;
207 if (cksum != ((uint8_t *)buf)[len-1]) {
208 fprintf(stderr, "Invalid cksum\n");
209 dev->stats.rx_invalid_cksum++;
215 case XBEE_TYPE_MODEM_STATUS:
216 dev->stats.rx_modem_status++;
217 channel = XBEE_DEFAULT_CHANNEL;
219 case XBEE_TYPE_ATRESP:
220 if (xbee_proto_parse_atresp(dev, buf, len) < 0)
224 case XBEE_TYPE_RMT_ATRESP:
225 if (xbee_proto_parse_rmt_atresp(dev, buf, len) < 0)
229 case XBEE_TYPE_XMIT_STATUS:
230 if (xbee_proto_parse_xmit_status(dev, buf, len) < 0)
235 dev->stats.rx_data++;
236 channel = XBEE_DEFAULT_CHANNEL;
238 case XBEE_TYPE_EXPL_RECV:
239 dev->stats.rx_expl_data++;
240 channel = XBEE_DEFAULT_CHANNEL;
242 case XBEE_TYPE_NODE_ID:
243 dev->stats.rx_node_id++;
244 channel = hdr->id; //XXX
246 /* invalid commands */
247 case XBEE_TYPE_ATCMD:
248 case XBEE_TYPE_ATCMD_Q:
250 case XBEE_TYPE_EXPL_XMIT:
251 case XBEE_TYPE_RMT_ATCMD:
253 dev->stats.rx_invalid_type++;
257 /* fallback to default channel if not registered */
258 if (channel < 0 || channel >= XBEE_MAX_CHANNEL ||
259 dev->channel[channel].registered == 0)
260 channel = XBEE_DEFAULT_CHANNEL;
262 /* execute the callback if any */
263 if (dev->channel[channel].rx_cb != NULL)
264 dev->channel[channel].rx_cb(dev, channel, hdr->type,
265 buf + hdrlen, len - hdrlen - 1,
266 dev->channel[channel].arg);
271 int xbee_proto_xmit(struct xbee_dev *dev, uint8_t channel_id, uint8_t type,
272 void *buf, unsigned len)
279 /* there is no empty message, so return an error */
283 /* prepare an iovec to avoid a copy: prepend a header to the
284 * buffer and append a checksum */
285 hdr.delimiter = XBEE_DELIMITER;
286 hdr.len = htons(len + 2);
290 iov[0].iov_base = &hdr;
291 iov[0].iov_len = sizeof(hdr);
292 iov[1].iov_base = buf;
293 iov[1].iov_len = len;
294 iov[2].iov_base = &cksum;
297 if (channel_id < 0 || channel_id >= XBEE_MAX_CHANNEL ||
298 dev->channel[channel_id].registered == 0) {
299 dev->stats.tx_invalid_channel ++;
303 /* calculate the cksum */
306 for (i = 0; i < len; i++)
307 cksum += ((uint8_t *)buf)[i];
308 cksum = 0xff - cksum;
309 dev->stats.tx_frame ++;
311 /* some additional checks before sending */
314 case XBEE_TYPE_ATCMD:
316 dev->stats.tx_atcmd ++;
318 case XBEE_TYPE_ATCMD_Q:
319 dev->stats.tx_atcmd_q ++;
322 dev->stats.tx_data ++;
324 case XBEE_TYPE_EXPL_XMIT:
325 dev->stats.tx_expl_data ++;
327 case XBEE_TYPE_RMT_ATCMD:
328 dev->stats.tx_rmt_atcmd ++;
331 /* invalid commands */
332 case XBEE_TYPE_XMIT_STATUS:
333 case XBEE_TYPE_MODEM_STATUS:
334 case XBEE_TYPE_ATRESP:
336 case XBEE_TYPE_EXPL_RECV:
337 case XBEE_TYPE_NODE_ID:
338 case XBEE_TYPE_RMT_ATRESP:
340 dev->stats.tx_invalid_type ++;
341 fprintf(stderr, "unhandled xmit type=%x\n", hdr.type);
345 return writev(dev->fd, iov, 3);