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         uint8_t hdrlen;
 120         struct xbee_hdr *hdr = buf;
 121         int i;
 122         uint8_t cksum = 0;
 123         int channel = XBEE_DEFAULT_CHANNEL;
 124
 125         dev->stats.rx_frame++;
 126
 127         switch (hdr->type) {
 128                 case XBEE_TYPE_MODEM_STATUS:
 129                 case XBEE_TYPE_RECV:
 130                 case XBEE_TYPE_EXPL_RECV:
 131                         hdrlen = sizeof(struct xbee_hdr) - 1; /* no frame ID */
 132                         break;
 133                 default:
 134                         hdrlen = sizeof(struct xbee_hdr);
 135                         break;
 136         }
 137
 138         /* check frame len */
 139         if (len < (hdrlen + 1)) {
 140                 dev->stats.rx_frame_too_small++;
 141                 fprintf(stderr, "Frame too small\n");
 142                 return -1;
 143         }
 144
 145         /* validate the cksum */
 146         for (i = 3; i < (len - 1); i++)
 147                 cksum += ((uint8_t *)buf)[i];
 148         cksum = 0xff - cksum;
 149         if (cksum != ((uint8_t *)buf)[len-1]) {
 150                 fprintf(stderr, "Invalid cksum\n");
 151                 dev->stats.rx_invalid_cksum++;
 152                 return -1;
 153         }
 154
 155         /* dispatch */
 156         switch (hdr->type) {
 157                 case XBEE_TYPE_MODEM_STATUS:
 158                         dev->stats.rx_modem_status++;
 159                         channel = XBEE_DEFAULT_CHANNEL;
 160                         break;
 161                 case XBEE_TYPE_ATRESP:
 162                         if (xbee_proto_parse_atresp(dev, buf, len) < 0)
 163                                 return -1;
 164                         channel = hdr->id;
 165                         break;
 166                 case XBEE_TYPE_RMT_ATRESP:
 167                         if (xbee_proto_parse_rmt_atresp(dev, buf, len) < 0)
 168                                 return -1;
 169                         channel = hdr->id;
 170                         break;
 171                 case XBEE_TYPE_XMIT_STATUS:
 172                         if (xbee_proto_parse_xmit_status(dev, buf, len) < 0)
 173                                 return -1;
 174                         channel = hdr->id;
 175                         break;
 176                 case XBEE_TYPE_RECV:
 177                         dev->stats.rx_data++;
 178                         channel = XBEE_DEFAULT_CHANNEL;
 179                         break;
 180                 case XBEE_TYPE_EXPL_RECV:
 181                         dev->stats.rx_expl_data++;
 182                         channel = XBEE_DEFAULT_CHANNEL;
 183                         break;
 184                 case XBEE_TYPE_NODE_ID:
 185                         dev->stats.rx_node_id++;
 186                         channel = hdr->id; //XXX
 187                         break;
 188                         /* invalid commands */
 189                 case XBEE_TYPE_ATCMD:
 190                 case XBEE_TYPE_ATCMD_Q:
 191                 case XBEE_TYPE_XMIT:
 192                 case XBEE_TYPE_EXPL_XMIT:
 193                 case XBEE_TYPE_RMT_ATCMD:
 194                 default:
 195                         dev->stats.rx_invalid_type++;
 196                         break;
 197         }
 198
 199         /* fallback to default channel if not registered */
 200         if (channel < 0 || channel >= XBEE_MAX_CHANNEL ||
 201             dev->channel[channel].registered == 0)
 202                 channel = XBEE_DEFAULT_CHANNEL;
 203
 204         /* execute the callback if any */
 205         if (dev->channel[channel].rx_cb != NULL)
 206                 dev->channel[channel].rx_cb(dev, channel, hdr->type,
 207                                             buf + hdrlen,
 208                                             len - hdrlen - 1,
 209                                             dev->channel[channel].arg);
 210
 211         return 0;
 212 }
 213
 214 int xbee_proto_xmit(struct xbee_dev *dev, uint8_t channel_id, uint8_t type,
 215                     void *buf, unsigned len)
 216 {
 217         struct xbee_hdr hdr;
 218         unsigned i;
 219         uint8_t cksum = 0;
 220
 221         /* there is no empty message, so return an error */
 222         if (len == 0)
 223                 return -1;
 224
 225         /* prepare an iovec to avoid a copy: prepend a header to the
 226          * buffer and append a checksum */
 227         hdr.delimiter = XBEE_DELIMITER;
 228         hdr.len = htons(len + 2);
 229         hdr.type = type;
 230         hdr.id = channel_id;
 231
 232         if (channel_id < 0 || channel_id >= XBEE_MAX_CHANNEL ||
 233             dev->channel[channel_id].registered == 0) {
 234                 dev->stats.tx_invalid_channel ++;
 235                 return -1;
 236         }
 237
 238         /* calculate the cksum */
 239         cksum = hdr.type;
 240         cksum += hdr.id;
 241         for (i = 0; i < len; i++)
 242                 cksum += ((uint8_t *)buf)[i];
 243         cksum = 0xff - cksum;
 244         dev->stats.tx_frame ++;
 245
 246         /* some additional checks before sending */
 247         switch (hdr.type) {
 248
 249                 case XBEE_TYPE_ATCMD:
 250                         // XXX some checks ?
 251                         dev->stats.tx_atcmd ++;
 252                         break;
 253                 case XBEE_TYPE_ATCMD_Q:
 254                         dev->stats.tx_atcmd_q ++;
 255                         break;
 256                 case XBEE_TYPE_XMIT:
 257                         dev->stats.tx_data ++;
 258                         break;
 259                 case XBEE_TYPE_EXPL_XMIT:
 260                         dev->stats.tx_expl_data ++;
 261                         break;
 262                 case XBEE_TYPE_RMT_ATCMD:
 263                         dev->stats.tx_rmt_atcmd ++;
 264                         break;
 265
 266                 /* invalid commands */
 267                 case XBEE_TYPE_XMIT_STATUS:
 268                 case XBEE_TYPE_MODEM_STATUS:
 269                 case XBEE_TYPE_ATRESP:
 270                 case XBEE_TYPE_RECV:
 271                 case XBEE_TYPE_EXPL_RECV:
 272                 case XBEE_TYPE_NODE_ID:
 273                 case XBEE_TYPE_RMT_ATRESP:
 274                 default:
 275                         dev->stats.tx_invalid_type ++;
 276                         fprintf(stderr, "unhandled xmit type=%x\n", hdr.type);
 277                         return -1;
 278         }
 279
 280         /* send the frame on the wire */
 281         fwrite((uint8_t *)&hdr, 1, sizeof(hdr), dev->file);
 282         fwrite((uint8_t *)buf, 1, len, dev->file);
 283         fwrite(&cksum, 1, 1, dev->file);
 284
 285         return 0;
 286 }
 287
 288 void xbee_proto_rx(struct xbee_dev *dev)
 289 {
 290         uint8_t framelen;
 291         struct xbee_hdr *hdr = (struct xbee_hdr *)dev->frame;
 292         int c;
 293
 294         while (1) {
 295
 296                 /* read from UART */
 297                 c = fgetc(dev->file);
 298                 if (c == EOF)
 299                         break;
 300
 301                 /* frame too long XXX stats */
 302                 if (dev->frame_len >= XBEE_MAX_FRAME_LEN) {
 303                         dev->frame_len = 0;
 304                         continue;
 305                 }
 306
 307                 if (dev->frame_len == 0 && c != XBEE_DELIMITER)
 308                         continue;
 309
 310                 dev->frame[dev->frame_len++] = c;
 311
 312                 /* not enough data to read len */
 313                 if (dev->frame_len < sizeof(*hdr))
 314                         continue;
 315
 316                 framelen = ntohs(hdr->len);
 317                 framelen += 4; /* 1 for delimiter, 2 for len, 1 for cksum */
 318
 319                 /* frame too long XXX stats */
 320                 if (framelen >= XBEE_MAX_FRAME_LEN) {
 321                         dev->frame_len = 0;
 322                         continue;
 323                 }
 324
 325                 /* not enough data */
 326                 if (dev->frame_len < framelen)
 327                         continue;
 328                 if (xbee_proto_parse_frame(dev) < 0)
 329                         ;//XXX stats
 330                 dev->frame_len = 0;
 331         }
 332 }