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 <aversive.h>
  29 #include <aversive/queue.h>
  30 #include <aversive/endian.h>
  31
  32 #include <uart.h>
  33
  34 #include <stdio.h>
  35 #include <stdlib.h>
  36 #include <stdint.h>
  37 #include <string.h>
  38 #include <ctype.h>
  39
  40 #include "xbee_neighbor.h"
  41 #include "xbee_stats.h"
  42 #include "xbee_buf.h"
  43 #include "xbee_proto.h"
  44 #include "xbee.h"
  45
  46 /* return -1 if the frame is invalid */
  47 static int xbee_proto_parse_atresp(struct xbee_dev *dev, void *buf,
  48                                    unsigned len)
  49 {
  50         struct xbee_atresp_hdr *atresp_hdr;
  51
  52         dev->stats.rx_atresp++;
  53
  54         if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_atresp_hdr)) {
  55                 dev->stats.rx_frame_too_small++;
  56                 return -1;
  57         }
  58
  59         atresp_hdr = buf + sizeof(struct xbee_hdr);
  60
  61         /* bad status, but let the frame continue */
  62         if (atresp_hdr->status != 0)
  63                 dev->stats.rx_atresp_error++;
  64
  65         return 0;
  66 }
  67
  68 /* return -1 if the frame is invalid */
  69 static int xbee_proto_parse_rmt_atresp(struct xbee_dev *dev, void *buf,
  70                                    unsigned len)
  71 {
  72         struct xbee_rmt_atresp_hdr *rmt_atresp_hdr;
  73
  74         dev->stats.rx_rmt_atresp++;
  75
  76         if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_rmt_atresp_hdr)) {
  77                 dev->stats.rx_frame_too_small++;
  78                 return -1;
  79         }
  80
  81         rmt_atresp_hdr = buf + sizeof(struct xbee_hdr);
  82
  83         /* bad status, but let the frame continue */
  84         if (rmt_atresp_hdr->status != 0)
  85                 dev->stats.rx_rmt_atresp_error++;
  86
  87         return 0;
  88 }
  89
  90 /* return -1 if the frame is invalid */
  91 static int xbee_proto_parse_xmit_status(struct xbee_dev *dev, void *buf,
  92                                         unsigned len)
  93 {
  94         struct xbee_xmit_status_hdr *xmit_status_hdr;
  95
  96         dev->stats.rx_xmit_status++;
  97
  98         if (len < sizeof(struct xbee_hdr) + sizeof(struct xbee_xmit_status_hdr)) {
  99                 dev->stats.rx_frame_too_small++;
 100                 return -1;
 101         }
 102
 103         xmit_status_hdr = buf + sizeof(struct xbee_hdr);
 104         dev->stats.tx_xmit_retries += xmit_status_hdr->xmit_retry_cnt;
 105
 106         /* bad status, but let the frame continue */
 107         if (xmit_status_hdr->delivery_status != 0)
 108                 dev->stats.rx_xmit_status_error++;
 109
 110         return 0;
 111 }
 112
 113 /* parse the frame stored in the device: return 0 if the frame is
 114  * valid, else a negative value */
 115 static int xbee_proto_parse_frame(struct xbee_dev *dev)
 116 {
 117         void *buf = dev->frame;
 118         uint8_t len = dev->frame_len;
 119         struct xbee_hdr *hdr = buf;
 120         int i;
 121         uint8_t cksum = 0;
 122         int channel = XBEE_DEFAULT_CHANNEL;
 123
 124         dev->stats.rx_frame++;
 125
 126         /* check frame len */
 127         if (len < (sizeof(*hdr) + 1)) {
 128                 dev->stats.rx_frame_too_small++;
 129                 fprintf(stderr, "Frame too small\n");
 130                 return -1;
 131         }
 132
 133         /* validate the cksum */
 134         for (i = 3; i < (len - 1); i++)
 135                 cksum += ((uint8_t *)buf)[i];
 136         cksum = 0xff - cksum;
 137         if (cksum != ((uint8_t *)buf)[len-1]) {
 138                 fprintf(stderr, "Invalid cksum\n");
 139                 dev->stats.rx_invalid_cksum++;
 140                 return -1;
 141         }
 142
 143         /* dispatch */
 144         switch (hdr->type) {
 145                 case XBEE_TYPE_MODEM_STATUS:
 146                         dev->stats.rx_modem_status++;
 147                         channel = XBEE_DEFAULT_CHANNEL;
 148                         break;
 149                 case XBEE_TYPE_ATRESP:
 150                         if (xbee_proto_parse_atresp(dev, buf, len) < 0)
 151                                 return -1;
 152                         channel = hdr->id;
 153                         break;
 154                 case XBEE_TYPE_RMT_ATRESP:
 155                         if (xbee_proto_parse_rmt_atresp(dev, buf, len) < 0)
 156                                 return -1;
 157                         channel = hdr->id;
 158                         break;
 159                 case XBEE_TYPE_XMIT_STATUS:
 160                         if (xbee_proto_parse_xmit_status(dev, buf, len) < 0)
 161                                 return -1;
 162                         channel = hdr->id;
 163                         break;
 164                 case XBEE_TYPE_RECV:
 165                         dev->stats.rx_data++;
 166                         channel = XBEE_DEFAULT_CHANNEL;
 167                         break;
 168                 case XBEE_TYPE_EXPL_RECV:
 169                         dev->stats.rx_expl_data++;
 170                         channel = XBEE_DEFAULT_CHANNEL;
 171                         break;
 172                 case XBEE_TYPE_NODE_ID:
 173                         dev->stats.rx_node_id++;
 174                         channel = hdr->id; //XXX
 175                         break;
 176                         /* invalid commands */
 177                 case XBEE_TYPE_ATCMD:
 178                 case XBEE_TYPE_ATCMD_Q:
 179                 case XBEE_TYPE_XMIT:
 180                 case XBEE_TYPE_EXPL_XMIT:
 181                 case XBEE_TYPE_RMT_ATCMD:
 182                 default:
 183                         dev->stats.rx_invalid_type++;
 184                         break;
 185         }
 186
 187         /* fallback to default channel if not registered */
 188         if (channel < 0 || channel >= XBEE_MAX_CHANNEL ||
 189             dev->channel[channel].registered == 0)
 190                 channel = XBEE_DEFAULT_CHANNEL;
 191
 192         /* execute the callback if any */
 193         if (dev->channel[channel].rx_cb != NULL)
 194                 dev->channel[channel].rx_cb(dev, channel, hdr->type,
 195                                             buf + sizeof(struct xbee_hdr),
 196                                             len - sizeof(struct xbee_hdr) - 1,
 197                                             dev->channel[channel].arg);
 198
 199         return 0;
 200 }
 201
 202 int xbee_proto_xmit(struct xbee_dev *dev, uint8_t channel_id, uint8_t type,
 203                     void *buf, unsigned len)
 204 {
 205         struct xbee_hdr hdr;
 206         unsigned i;
 207         uint8_t cksum = 0;
 208
 209         /* there is no empty message, so return an error */
 210         if (len == 0)
 211                 return -1;
 212
 213         /* prepare an iovec to avoid a copy: prepend a header to the
 214          * buffer and append a checksum */
 215         hdr.delimiter = XBEE_DELIMITER;
 216         hdr.len = htons(len + 2);
 217         hdr.type = type;
 218         hdr.id = channel_id;
 219
 220         if (channel_id < 0 || channel_id >= XBEE_MAX_CHANNEL ||
 221             dev->channel[channel_id].registered == 0) {
 222                 dev->stats.tx_invalid_channel ++;
 223                 return -1;
 224         }
 225
 226         /* calculate the cksum */
 227         cksum = hdr.type;
 228         cksum += hdr.id;
 229         for (i = 0; i < len; i++)
 230                 cksum += ((uint8_t *)buf)[i];
 231         cksum = 0xff - cksum;
 232         dev->stats.tx_frame ++;
 233
 234         /* some additional checks before sending */
 235         switch (hdr.type) {
 236
 237                 case XBEE_TYPE_ATCMD:
 238                         // XXX some checks ?
 239                         dev->stats.tx_atcmd ++;
 240                         break;
 241                 case XBEE_TYPE_ATCMD_Q:
 242                         dev->stats.tx_atcmd_q ++;
 243                         break;
 244                 case XBEE_TYPE_XMIT:
 245                         dev->stats.tx_data ++;
 246                         break;
 247                 case XBEE_TYPE_EXPL_XMIT:
 248                         dev->stats.tx_expl_data ++;
 249                         break;
 250                 case XBEE_TYPE_RMT_ATCMD:
 251                         dev->stats.tx_rmt_atcmd ++;
 252                         break;
 253
 254                 /* invalid commands */
 255                 case XBEE_TYPE_XMIT_STATUS:
 256                 case XBEE_TYPE_MODEM_STATUS:
 257                 case XBEE_TYPE_ATRESP:
 258                 case XBEE_TYPE_RECV:
 259                 case XBEE_TYPE_EXPL_RECV:
 260                 case XBEE_TYPE_NODE_ID:
 261                 case XBEE_TYPE_RMT_ATRESP:
 262                 default:
 263                         dev->stats.tx_invalid_type ++;
 264                         fprintf(stderr, "unhandled xmit type=%x\n", hdr.type);
 265                         return -1;
 266         }
 267
 268         /* send the frame on the wire */
 269         fwrite((uint8_t *)&hdr, 1, sizeof(hdr), dev->file);
 270         fwrite((uint8_t *)buf, 1, len, dev->file);
 271         fwrite(&cksum, 1, 1, dev->file);
 272
 273         return 0;
 274 }
 275
 276 void xbee_proto_rx(struct xbee_dev *dev)
 277 {
 278         uint8_t framelen;
 279         struct xbee_hdr *hdr = (struct xbee_hdr *)dev->frame;
 280         int c;
 281
 282         while (1) {
 283
 284                 /* read from UART */
 285                 c = fgetc(dev->file);
 286                 if (c == EOF)
 287                         break;
 288
 289                 /* frame too long XXX stats */
 290                 if (dev->frame_len >= XBEE_MAX_FRAME_LEN) {
 291                         dev->frame_len = 0;
 292                         continue;
 293                 }
 294
 295                 dev->frame[dev->frame_len++] = c;
 296
 297                 /* not enough data to read len */
 298                 if (dev->frame_len < sizeof(*hdr))
 299                         continue;
 300
 301                 framelen = ntohs(hdr->len);
 302                 framelen += 4; /* 1 for delimiter, 2 for len, 1 for cksum */
 303
 304                 /* not enough data */
 305                 if (dev->frame_len < framelen)
 306                         continue;
 307                 if (xbee_proto_parse_frame(dev) < 0)
 308                         ;//XXX stats
 309                 dev->frame_len = 0;
 310         }
 311 }