xbee_proto.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 <stdio.h>
  29 #include <stdlib.h>
  30 #include <stdint.h>
  31 #include <stddef.h>
  32 #include <string.h>
  33 #include <ctype.h>
  34 #include <sys/queue.h>
  35 #include <arpa/inet.h>
  36 #include <sys/uio.h>
  37
  38 /* remove */
  39 #include <unistd.h>
  40 #include <fcntl.h>
  41
  42 #include "xbee_neighbor.h"
  43 #include "xbee_stats.h"
  44 #include "xbee_buf.h"
  45 #include "xbee_proto.h"
  46 #include "xbee.h"
  47
  48 static void xbee_proto_drop_garbage(struct xbee_dev *dev)
  49 {
  50         struct xbee_bufq *q = &dev->queue;
  51         char *data;
  52
  53         /* drop all data != delimiter */
  54         while ((data = xbee_bufq_data(q, 0))) {
  55                 /* no more data */
  56                 if (data == NULL)
  57                         break;
  58
  59                 if (*data == XBEE_DELIMITER)
  60                         break;
  61
  62                 dev->stats.rx_no_delim++;
  63                 xbee_bufq_drop(q, 1);
  64         }
  65 }
  66
  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)
  69 {
  70         uint16_t framelen;
  71         struct xbee_hdr hdr;
  72         struct xbee_bufq *q = &dev->queue;
  73
  74         xbee_proto_drop_garbage(dev);
  75
  76         if (xbee_bufq_copy(q, &hdr, sizeof(hdr)) < 0)
  77                 return 0;
  78
  79         framelen = ntohs(hdr.len);
  80         framelen += 4; /* 1 for delimiter, 2 for len, 1 for cksum */
  81
  82         /* not enough data to read */
  83         if (q->len < framelen)
  84                 return 0;
  85
  86         /* arf, provided buffer is to small */
  87         if (framelen > len) {
  88                 fprintf(stderr, "drop packet, buffer too small\n");
  89                 dev->stats.rx_frame_too_large++;
  90                 xbee_bufq_drop(q, framelen);
  91                 return -1;
  92         }
  93
  94         xbee_bufq_copy(q, buf, framelen);
  95         xbee_bufq_drop(q, framelen);
  96         return framelen;
  97 }
  98
  99 /* return -1 if the frame is invalid */
 100 static int xbee_proto_parse_atresp(struct xbee_dev *dev, void *buf,
 101                                    unsigned len)
 102 {
 103         struct xbee_atresp_hdr *atresp_hdr;
 104
 105         dev->stats.rx_atresp++;
 106
 107         if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_atresp_hdr)) {
 108                 dev->stats.rx_frame_too_small++;
 109                 return -1;
 110         }
 111
 112         atresp_hdr = buf + sizeof(struct xbee_hdr);
 113
 114         /* bad status, but let the frame continue */
 115         if (atresp_hdr->status != 0)
 116                 dev->stats.rx_atresp_error++;
 117
 118         return 0;
 119 }
 120
 121 /* return -1 if the frame is invalid */
 122 static int xbee_proto_parse_rmt_atresp(struct xbee_dev *dev, void *buf,
 123                                    unsigned len)
 124 {
 125         struct xbee_rmt_atresp_hdr *rmt_atresp_hdr;
 126
 127         dev->stats.rx_rmt_atresp++;
 128
 129         if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_rmt_atresp_hdr)) {
 130                 dev->stats.rx_frame_too_small++;
 131                 return -1;
 132         }
 133
 134         rmt_atresp_hdr = buf + sizeof(struct xbee_hdr);
 135
 136         /* bad status, but let the frame continue */
 137         if (rmt_atresp_hdr->status != 0)
 138                 dev->stats.rx_rmt_atresp_error++;
 139
 140         return 0;
 141 }
 142
 143 /* return -1 if the frame is invalid */
 144 static int xbee_proto_parse_xmit_status(struct xbee_dev *dev, void *buf,
 145                                         unsigned len)
 146 {
 147         struct xbee_xmit_status_hdr *xmit_status_hdr;
 148
 149         dev->stats.rx_xmit_status++;
 150
 151         if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_xmit_status_hdr)) {
 152                 dev->stats.rx_frame_too_small++;
 153                 return -1;
 154         }
 155
 156         xmit_status_hdr = buf + sizeof(struct xbee_hdr);
 157         dev->stats.tx_xmit_retries += xmit_status_hdr->xmit_retry_cnt;
 158
 159         /* bad status, but let the frame continue */
 160         if (xmit_status_hdr->delivery_status != 0)
 161                 dev->stats.rx_xmit_status_error++;
 162
 163         return 0;
 164 }
 165
 166 /* parse a frame: return 0 if the frame is valid, else a negative value */
 167 // XXX rename
 168 int xbee_proto_parse_frame(struct xbee_dev *dev, void *buf, unsigned len)
 169 {
 170         struct xbee_hdr *hdr = buf;
 171         int i;
 172         uint8_t cksum = 0;
 173         int channel;
 174         unsigned hdrlen;
 175
 176         dev->stats.rx_frame++;
 177
 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");
 182                 return -1;
 183         }
 184
 185         switch (hdr->type) {
 186                 case XBEE_TYPE_MODEM_STATUS:
 187                 case XBEE_TYPE_RECV:
 188                 case XBEE_TYPE_EXPL_RECV:
 189                         hdrlen = sizeof(struct xbee_hdr) - 1; /* no frame ID */
 190                         break;
 191                 default:
 192                         hdrlen = sizeof(struct xbee_hdr);
 193                         break;
 194         }
 195
 196         /* check frame len */
 197         if (len < (hdrlen + 1)) {
 198                 dev->stats.rx_frame_too_small++;
 199                 fprintf(stderr, "Frame too small\n");
 200                 return -1;
 201         }
 202
 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++;
 210                 return -1;
 211         }
 212
 213         /* dispatch */
 214         switch (hdr->type) {
 215                 case XBEE_TYPE_MODEM_STATUS:
 216                         dev->stats.rx_modem_status++;
 217                         channel = XBEE_DEFAULT_CHANNEL;
 218                         break;
 219                 case XBEE_TYPE_ATRESP:
 220                         if (xbee_proto_parse_atresp(dev, buf, len) < 0)
 221                                 return -1;
 222                         channel = hdr->id;
 223                         break;
 224                 case XBEE_TYPE_RMT_ATRESP:
 225                         if (xbee_proto_parse_rmt_atresp(dev, buf, len) < 0)
 226                                 return -1;
 227                         channel = hdr->id;
 228                         break;
 229                 case XBEE_TYPE_XMIT_STATUS:
 230                         if (xbee_proto_parse_xmit_status(dev, buf, len) < 0)
 231                                 return -1;
 232                         channel = hdr->id;
 233                         break;
 234                 case XBEE_TYPE_RECV:
 235                         dev->stats.rx_data++;
 236                         channel = XBEE_DEFAULT_CHANNEL;
 237                         break;
 238                 case XBEE_TYPE_EXPL_RECV:
 239                         dev->stats.rx_expl_data++;
 240                         channel = XBEE_DEFAULT_CHANNEL;
 241                         break;
 242                 case XBEE_TYPE_NODE_ID:
 243                         dev->stats.rx_node_id++;
 244                         channel = hdr->id; //XXX
 245                         break;
 246                         /* invalid commands */
 247                 case XBEE_TYPE_ATCMD:
 248                 case XBEE_TYPE_ATCMD_Q:
 249                 case XBEE_TYPE_XMIT:
 250                 case XBEE_TYPE_EXPL_XMIT:
 251                 case XBEE_TYPE_RMT_ATCMD:
 252                 default:
 253                         dev->stats.rx_invalid_type++;
 254                         break;
 255         }
 256
 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;
 261
 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);
 267
 268         return 0;
 269 }
 270
 271 static void hexdump(const char *title, const void *buf, unsigned int len)
 272 {
 273         unsigned int i, out, ofs;
 274         const unsigned char *data = buf;
 275 #define LINE_LEN 80
 276         char line[LINE_LEN];    /* space needed 8+16*3+3+16 == 75 */
 277
 278         printf("%s at [%p], len=%d\n", title, data, len);
 279         ofs = 0;
 280         while (ofs < len) {
 281                 /* format 1 line in the buffer, then use printk to print them */
 282                 out = snprintf(line, LINE_LEN, "%08X", ofs);
 283                 for (i=0; ofs+i < len && i<16; i++)
 284                         out += snprintf(line+out, LINE_LEN - out, " %02X",
 285                                         data[ofs+i]&0xff);
 286                 for (;i<=16;i++)
 287                         out += snprintf(line+out, LINE_LEN - out, "   ");
 288                 for (i=0; ofs < len && i<16; i++, ofs++) {
 289                         unsigned char c = data[ofs];
 290                         if (!isascii(c) || !isprint(c))
 291                                 c = '.';
 292                         out += snprintf(line+out, LINE_LEN - out, "%c", c);
 293                 }
 294                 printf("%s\n", line);
 295         }
 296 }
 297
 298 int xbee_proto_xmit(struct xbee_dev *dev, uint8_t channel_id, uint8_t type,
 299                     void *buf, unsigned len)
 300 {
 301         struct xbee_hdr hdr;
 302         struct iovec iov[3];
 303         int i;
 304         uint8_t cksum = 0;
 305
 306         /* there is no empty message, so return an error */
 307         if (len == 0)
 308                 return -1;
 309
 310         /* prepare an iovec to avoid a copy: prepend a header to the
 311          * buffer and append a checksum */
 312         hdr.delimiter = XBEE_DELIMITER;
 313         hdr.len = htons(len + 2);
 314         hdr.type = type;
 315         hdr.id = channel_id;
 316
 317         iov[0].iov_base = &hdr;
 318         iov[0].iov_len = sizeof(hdr);
 319         iov[1].iov_base = buf;
 320         iov[1].iov_len = len;
 321         iov[2].iov_base = &cksum;
 322         iov[2].iov_len = 1;
 323
 324         if (channel_id < 0 || channel_id >= XBEE_MAX_CHANNEL ||
 325             dev->channel[channel_id].registered == 0) {
 326                 dev->stats.tx_invalid_channel ++;
 327                 return -1;
 328         }
 329
 330         /* calculate the cksum */
 331         cksum = hdr.type;
 332         cksum += hdr.id;
 333         for (i = 0; i < len; i++)
 334                 cksum += ((uint8_t *)buf)[i];
 335         cksum = 0xff - cksum;
 336         dev->stats.tx_frame ++;
 337
 338         /* some additional checks before sending */
 339         switch (hdr.type) {
 340
 341                 case XBEE_TYPE_ATCMD:
 342                         // XXX some checks ?
 343                         dev->stats.tx_atcmd ++;
 344                         break;
 345                 case XBEE_TYPE_ATCMD_Q:
 346                         dev->stats.tx_atcmd_q ++;
 347                         break;
 348                 case XBEE_TYPE_XMIT:
 349                         dev->stats.tx_data ++;
 350                         break;
 351                 case XBEE_TYPE_EXPL_XMIT:
 352                         dev->stats.tx_expl_data ++;
 353                         break;
 354                 case XBEE_TYPE_RMT_ATCMD:
 355                         dev->stats.tx_rmt_atcmd ++;
 356                         break;
 357
 358                 /* invalid commands */
 359                 case XBEE_TYPE_XMIT_STATUS:
 360                 case XBEE_TYPE_MODEM_STATUS:
 361                 case XBEE_TYPE_ATRESP:
 362                 case XBEE_TYPE_RECV:
 363                 case XBEE_TYPE_EXPL_RECV:
 364                 case XBEE_TYPE_NODE_ID:
 365                 case XBEE_TYPE_RMT_ATRESP:
 366                 default:
 367                         dev->stats.tx_invalid_type ++;
 368                         fprintf(stderr, "unhandled xmit type=%x\n", hdr.type);
 369                         return -1;
 370         }
 371
 372         hexdump("hdr", (uint8_t *)&hdr, sizeof(hdr));
 373         hexdump("buf", (uint8_t *)buf, len);
 374         hexdump("cksum", &cksum, 1);
 375
 376         return writev(dev->fd, iov, 3);
 377 }
 378