xbee_user.c

   1 /*
   2  * Copyright (c) 2014, 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 <stdint.h>
  30 #include <stdlib.h>
  31 #include <string.h>
  32 #include <stddef.h>
  33 #include <ctype.h>
  34
  35 #include <aversive.h>
  36 #include <aversive/endian.h>
  37 #include <aversive/wait.h>
  38 #include <aversive/pgmspace.h>
  39
  40 #include <callout.h>
  41 #include <rdline.h>
  42 #include <xbee.h>
  43 #include <xbee_rxtx.h>
  44
  45 #include "rc_proto.h"
  46 #include "xbee_user.h"
  47 #include "main.h"
  48
  49 #define XBEE_TIMEOUT_MS    1000
  50 #define XBEE_POLL_TIMER_MS 5
  51
  52 static struct xbee_ctx xbee_ctx[XBEE_MAX_CHANNEL];
  53
  54 int xbee_cmdline_input_enabled = 1;
  55
  56 /* parameters */
  57 int xbee_raw = 0;
  58
  59 static void __hexdump(const void *buf, unsigned int len)
  60 {
  61         unsigned int i, out, ofs;
  62         const unsigned char *data = buf;
  63 #define LINE_LEN 80
  64         char line[LINE_LEN];    /* space needed 8+16*3+3+16 == 75 */
  65
  66         ofs = 0;
  67         while (ofs < len) {
  68                 /* format 1 line in the buffer, then use printk to print them */
  69                 out = snprintf_P(line, LINE_LEN, PSTR("%08X"), ofs);
  70                 for (i=0; ofs+i < len && i<16; i++)
  71                         out += snprintf_P(line+out, LINE_LEN - out,
  72                                           PSTR(" %02X"),
  73                                           data[ofs+i]&0xff);
  74                 for (;i<=16;i++)
  75                         out += snprintf(line+out, LINE_LEN - out, "   ");
  76                 for (i=0; ofs < len && i<16; i++, ofs++) {
  77                         unsigned char c = data[ofs];
  78                         if (!isascii(c) || !isprint(c))
  79                                 c = '.';
  80                         out += snprintf_P(line+out,
  81                                           LINE_LEN - out,
  82                                           PSTR("%c"), c);
  83                 }
  84                 DEBUG(E_USER_XBEE, "%s");
  85         }
  86 }
  87
  88 static void hexdump_msg(const char *title, const struct xbee_msg *msg)
  89 {
  90         unsigned i;
  91
  92         DEBUG(E_USER_XBEE, "dump %s", title);
  93         for (i = 0; i < msg->iovlen; i++) {
  94                 DEBUG(E_USER_XBEE, "iovec %d at %p, len=%d", i,
  95                         msg->iov[i].buf, msg->iov[i].len);
  96                 __hexdump(msg->iov[i].buf, msg->iov[i].len);
  97         }
  98 }
  99
 100 static void hexdump(const char *title, const void *buf, unsigned int len)
 101 {
 102         DEBUG(E_USER_XBEE, "dump %s at [%p], len=%d", title, buf, len);
 103         __hexdump(buf, len);
 104 }
 105
 106 static int parse_xmit_status(struct xbee_ctx *ctx,
 107         struct xbee_xmit_status_hdr *frame, unsigned len)
 108 {
 109         (void)len;
 110
 111         if (ctx == NULL) {
 112                 ERROR(E_USER_XBEE, "no context");
 113                 return -1;
 114         }
 115
 116         /* see if it matches a xmit query (atcmd_query must be NULL) */
 117         if (ctx->atcmd_query[0] != '\0') {
 118                 ERROR(E_USER_XBEE, "invalid response 2");
 119                 return -1;
 120         }
 121
 122         /* XXX use defines for these values */
 123         if (frame->delivery_status == 0x00)
 124                 NOTICE(E_USER_XBEE, "Success");
 125         else if (frame->delivery_status == 0x01)
 126                 WARNING(E_USER_XBEE, "MAC ACK Failure");
 127         else if (frame->delivery_status == 0x15)
 128                 WARNING(E_USER_XBEE, "Invalid destination endpoint");
 129         else if (frame->delivery_status == 0x21)
 130                 WARNING(E_USER_XBEE, "Network ACK Failure");
 131         else if (frame->delivery_status == 0x25)
 132                 WARNING(E_USER_XBEE, "Route Not Found");
 133
 134         return 0;
 135 }
 136
 137 static int dump_atcmd(struct xbee_ctx *ctx, struct xbee_atresp_hdr *frame,
 138         unsigned len)
 139 {
 140         char atcmd_str[3];
 141         const struct xbee_atcmd *cmd_pgm;
 142         struct xbee_atcmd cmd;
 143         union {
 144                 uint8_t u8;
 145                 uint16_t u16;
 146                 uint32_t u32;
 147                 int16_t s16;
 148         } __attribute__((packed)) *result;
 149
 150         if (ctx == NULL) {
 151                 ERROR(E_USER_XBEE, "no context");
 152                 return -1;
 153         }
 154
 155         /* get AT command from frame */
 156         memcpy(atcmd_str, &frame->cmd, 2);
 157         atcmd_str[2] = '\0';
 158
 159         /* see if it matches query */
 160         if (memcmp(atcmd_str, ctx->atcmd_query, 2)) {
 161                 ERROR(E_USER_XBEE, "invalid response <%c%c><%s><%s>",
 162                          frame->cmd & 0xFF,
 163                          (frame->cmd >> 8) & 0xFF,
 164                          atcmd_str, ctx->atcmd_query);
 165                 return -1;
 166         }
 167
 168         /* see if it exists */
 169         cmd_pgm = xbee_atcmd_lookup_name(atcmd_str);
 170         if (cmd_pgm == NULL) {
 171                 ERROR(E_USER_XBEE, "unknown response");
 172                 return -1;
 173         }
 174         memcpy_P(&cmd, cmd_pgm, sizeof(cmd));
 175
 176         /* bad status */
 177         if (frame->status == 1) {
 178                 WARNING(E_USER_XBEE, "Status is error");
 179                 return -1;
 180         }
 181         else if (frame->status == 2) {
 182                 WARNING(E_USER_XBEE, "Invalid command");
 183                 return -1;
 184         }
 185         else if (frame->status == 3) {
 186                 WARNING(E_USER_XBEE, "Invalid parameter");
 187                 return -1;
 188         }
 189         else if (frame->status != 0) {
 190                 WARNING(E_USER_XBEE, "Unknown status error %d",
 191                         frame->status);
 192                 return -1;
 193         }
 194
 195         /* callback */
 196         if (ctx->func != NULL)
 197                 ctx->func(frame, len, ctx->arg);
 198
 199         /* dump frame */
 200         result = (void *)frame->data;
 201         len -= offsetof(struct xbee_atresp_hdr, data);
 202         if (cmd.flags & XBEE_ATCMD_F_PARAM_U8 && len == sizeof(uint8_t))
 203                 NOTICE(E_USER_XBEE, "<%s> is 0x%x (%d)", atcmd_str,
 204                         result->u8, result->u8);
 205         else if (cmd.flags & XBEE_ATCMD_F_PARAM_U16 && len == sizeof(uint16_t))
 206                 NOTICE(E_USER_XBEE, "<%s> is 0x%x (%d)", atcmd_str,
 207                         ntohs(result->u16), ntohs(result->u16));
 208         else if (cmd.flags & XBEE_ATCMD_F_PARAM_U32 && len == sizeof(uint32_t))
 209                 NOTICE(E_USER_XBEE, "<%s> is 0x%"PRIx32" (%"PRIu32")",
 210                         atcmd_str, ntohl(result->u32), ntohs(result->u32));
 211         else if (cmd.flags & XBEE_ATCMD_F_PARAM_S16 && len == sizeof(int16_t))
 212                 NOTICE(E_USER_XBEE, "<%s> is %d",
 213                         atcmd_str, ntohs(result->s16));
 214         else if (len == 0)
 215                 NOTICE(E_USER_XBEE, "no data, status ok");
 216         else
 217                 hexdump("atcmd answer", frame->data, len);
 218
 219         return 0;
 220 }
 221
 222
 223 int xbee_recv_data(struct xbee_recv_hdr *recvframe, unsigned len)
 224 {
 225         unsigned int datalen;
 226         struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
 227
 228         if (len <  sizeof(*recvframe))
 229                 return -1;
 230
 231         datalen = len - sizeof(*recvframe);
 232         if (datalen < sizeof(struct rc_proto_hdr))
 233                 return -1;
 234
 235         switch (rch->type) {
 236 #if 0
 237                 case RC_PROTO_TYPE_CHANNEL: {
 238                         struct rc_proto_channel *rcc =
 239                                 (struct rc_proto_channel *) recvframe->data;
 240                         int16_t val;
 241                         if (datalen != sizeof(struct rc_proto_channel))
 242                                 return -1;
 243                         val = ntohs(rcc->axis[0]);
 244                         val >>= 6;
 245                         val += 512;
 246                         spi_servo_set(0, val);
 247                         break;
 248                 }
 249                 case RC_PROTO_TYPE_RANGE: {
 250                         struct rc_proto_range *rcr =
 251                                 (struct rc_proto_range *) recvframe->data;
 252
 253                         if (datalen != sizeof(struct rc_proto_range))
 254                                 return -1;
 255
 256                         if (rcr->power_level >= MAX_POWER_LEVEL)
 257                                 return -1;
 258
 259                         rc_proto_rx_range(rcr->power_level);
 260
 261                         break;
 262                 }
 263 #endif
 264                 case RC_PROTO_HELLO: {
 265                         struct rc_proto_hello *rch =
 266                                 (struct rc_proto_hello *) recvframe->data;
 267
 268                         NOTICE(E_USER_XBEE, "recv hello len=%d",
 269                                 rch->datalen);
 270                         /* XXX stats */
 271                         break;
 272                 }
 273                 default:
 274                         return -1;
 275         }
 276
 277         return 0;
 278 }
 279
 280 /* socat /dev/ttyUSB0,raw,echo=0,b115200 /dev/ttyACM1,raw,echo=0,b115200 */
 281 int8_t xbeeapp_rx(struct xbee_dev *dev, int channel, int type,
 282              void *frame, unsigned len, void *opaque)
 283 {
 284         struct xbee_ctx *ctx = opaque;
 285         int8_t ret = 0;
 286
 287         NOTICE(E_USER_XBEE, "type=0x%x, channel=%d, ctx=%p",
 288                 type, channel, ctx);
 289
 290         /* if ctx is !NULL, it is an answer to a query */
 291         if (ctx != NULL) {
 292                 /* XXX only delete timeout if answer matched */
 293                 xbee_unload_timeout(ctx);
 294                 if (ctx->atcmd_query)
 295                         NOTICE(E_USER_XBEE, "Received answer to query <%c%c>",
 296                                 ctx->atcmd_query[0], ctx->atcmd_query[1]);
 297         }
 298
 299         /* some additional checks before sending */
 300         switch (type) {
 301                 case XBEE_TYPE_MODEM_STATUS: {
 302                         NOTICE(E_USER_XBEE, "Received Modem Status frame");
 303                         break;
 304                 }
 305
 306                 case XBEE_TYPE_RMT_ATRESP: {
 307                         union {
 308                                 uint64_t u64;
 309                                 struct {
 310 #if BYTE_ORDER == LITTLE_ENDIAN
 311                                         uint32_t low;
 312                                         uint32_t high;
 313 #else
 314                                         uint32_t high;
 315                                         uint32_t low;
 316 #endif
 317                                 } u32;
 318                         } addr;
 319                         memcpy(&addr, frame, sizeof(addr));
 320                         addr.u64 = ntohll(addr.u64);
 321                         NOTICE(E_USER_XBEE, "from remote address %"PRIx32"%"PRIx32"",
 322                                  addr.u32.high, addr.u32.low);
 323
 324                         /* this answer contains an atcmd answer at offset 10 */
 325                         if (dump_atcmd(ctx, frame + 10, len - 10) < 0)
 326                                 ret = -1;
 327
 328                         break;
 329                 }
 330                 case XBEE_TYPE_ATRESP: {
 331                         if (dump_atcmd(ctx, frame, len) < 0)
 332                                 ret = -1;
 333
 334                         break;
 335                 }
 336
 337                 case XBEE_TYPE_XMIT_STATUS: {
 338                         if (parse_xmit_status(ctx, frame, len) < 0)
 339                                 ret = -1;
 340
 341                         break;
 342                 }
 343
 344                 case XBEE_TYPE_RECV: {
 345                         if (xbee_recv_data(frame, len) < 0)
 346                                 ret = -1;
 347
 348                         break;
 349                 }
 350
 351                 case XBEE_TYPE_ATCMD:
 352                 case XBEE_TYPE_ATCMD_Q:
 353                 case XBEE_TYPE_XMIT:
 354                 case XBEE_TYPE_EXPL_XMIT:
 355                 case XBEE_TYPE_RMT_ATCMD:
 356                 case XBEE_TYPE_EXPL_RECV:
 357                 case XBEE_TYPE_NODE_ID:
 358                 default:
 359                         ERROR(E_USER_XBEE, "Invalid frame");
 360                         ret = -1;
 361                         break;
 362         }
 363
 364         WARNING(E_USER_XBEE, "undecoded rx frame");
 365         hexdump("undecoded rx frame", frame, len);
 366
 367         /* restart command line if it was a blocking query */
 368         if (ctx != NULL) {
 369                 xbee_unregister_channel(dev, channel);
 370                 if (ctx->foreground) {
 371                         xbee_stdin_enable();
 372                         rdline_newline(&xbeeboard.rdl, xbeeboard.prompt);
 373                 }
 374         }
 375
 376         return ret;
 377 }
 378
 379 static int xbeeapp_send(struct xbee_ctx *ctx, int type, struct xbee_msg *msg,
 380         int foreground)
 381 {
 382         int ret;
 383         int channel;
 384
 385         /* register a channel */
 386         channel = xbee_register_channel(xbee_dev, XBEE_CHANNEL_ANY,
 387                                         xbeeapp_rx, NULL);
 388         if (channel < 0) {
 389                 ERROR(E_USER_XBEE, "cannot send: no free channel");
 390                 return -1;
 391         }
 392
 393         /* copy context in the static struct table (avoiding a malloc) */
 394         memcpy(&xbee_ctx[channel], ctx, sizeof(*ctx));
 395         ctx = &xbee_ctx[channel];
 396         xbee_set_opaque(xbee_dev, channel, ctx);
 397
 398         NOTICE(E_USER_XBEE, "send frame channel=%d type=0x%x",
 399                 channel, type);
 400         hexdump_msg("xmit frame", msg);
 401
 402         /* transmit the frame on this channel */
 403         ret = xbee_tx_iovec(xbee_dev, channel, type, msg);
 404         if (ret < 0) {
 405                 ERROR(E_USER_XBEE, "cannot send");
 406                 xbee_unregister_channel(xbee_dev, channel);
 407                 return -1;
 408         }
 409
 410         ctx->channel = channel;
 411         xbee_load_timeout(ctx);   /* load a timeout event */
 412
 413         /* suspend command line until we have answer or timeout */
 414         if (foreground) {
 415                 ctx->foreground = 1;
 416                 rdline_stop(&xbeeboard.rdl); /* don't display prompt when return */
 417                 xbee_stdin_disable();  /* unload file descriptor polling */
 418         }
 419
 420         return 0;
 421 }
 422
 423 /* send an AT command with parameters filled by caller. Disable
 424  * command line until we get the answer or until a timeout occurs */
 425 int xbeeapp_send_atcmd(char *atcmd_str, void *param,
 426         unsigned param_len, int foreground,
 427         int (*func)(void *frame, unsigned len, void *arg), void *arg)
 428 {
 429         struct xbee_ctx ctx;
 430         /* struct xbee_atcmd_hdr atcmd_hdr; -> no needed same than atcmd_str */
 431         struct xbee_msg msg;
 432         uint8_t prio;
 433         int ret;
 434
 435         memset(&ctx, 0, sizeof(ctx));
 436         ctx.atcmd_query[0] = atcmd_str[0];
 437         ctx.atcmd_query[1] = atcmd_str[1];
 438         ctx.func = func;
 439         ctx.arg = arg;
 440
 441         msg.iovlen = 2;
 442         msg.iov[0].buf = atcmd_str;
 443         msg.iov[0].len = 2;
 444         msg.iov[1].buf = param;
 445         msg.iov[1].len = param_len;
 446
 447         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 448         ret = xbeeapp_send(&ctx, XBEE_TYPE_ATCMD, &msg, foreground);
 449         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 450
 451         return ret;
 452 }
 453
 454 int xbeeapp_send_msg(uint64_t addr, struct xbee_msg *msg, int foreground)
 455 {
 456         struct xbee_ctx ctx;
 457         struct xbee_xmit_hdr xmit_hdr;
 458         struct xbee_msg msg2;
 459         unsigned i;
 460         uint8_t prio;
 461         int ret;
 462
 463         if (msg->iovlen + 2 > XBEE_MSG_MAXIOV) {
 464                 ERROR(E_USER_XBEE, "too many iovecs");
 465                 return -1;
 466         }
 467
 468
 469         xmit_hdr.dstaddr = htonll(addr);
 470         xmit_hdr.reserved = htons(0xFFFE);
 471         xmit_hdr.bcast_radius = 0;
 472         xmit_hdr.opts = 0;
 473
 474         msg2.iovlen = msg->iovlen + 1;
 475         msg2.iov[0].buf = &xmit_hdr;
 476         msg2.iov[0].len = sizeof(xmit_hdr);
 477         for (i = 0; i < msg->iovlen; i++)
 478                 msg2.iov[i+1] = msg->iov[i];
 479
 480         memset(&ctx, 0, sizeof(ctx));
 481         ctx.atcmd_query[0] = '\0';
 482
 483         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 484         ret = xbeeapp_send(&ctx, XBEE_TYPE_XMIT, &msg2, foreground);
 485         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 486
 487         return ret;
 488 }
 489
 490 static void evt_timeout(struct callout_mgr *cm, struct callout *clt,
 491         void *arg)
 492 {
 493         struct xbee_ctx *ctx = arg;
 494
 495         (void)cm;
 496         (void)clt;
 497
 498         WARNING(E_USER_XBEE, "Timeout");
 499
 500         /* restart command line */
 501         xbee_stdin_enable();
 502         rdline_newline(&xbeeboard.rdl, xbeeboard.prompt);
 503
 504         /* free event */
 505         xbee_unregister_channel(xbee_dev, ctx->channel);
 506
 507         callout_stop(cm, clt);
 508 }
 509
 510 void xbee_load_timeout(struct xbee_ctx *ctx)
 511 {
 512         uint8_t prio;
 513
 514         callout_init(&ctx->timeout, evt_timeout, ctx, XBEE_PRIO);
 515         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 516         callout_schedule(&xbeeboard.intr_cm, &ctx->timeout, XBEE_TIMEOUT_MS);
 517         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 518 }
 519
 520 void xbee_unload_timeout(struct xbee_ctx *ctx)
 521 {
 522         uint8_t prio;
 523
 524         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 525         callout_stop(&xbeeboard.intr_cm, &ctx->timeout);
 526         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 527 }
 528
 529 static void xbee_rx_poll_timer_cb(struct callout_mgr *cm, struct callout *tim,
 530         void *arg)
 531 {
 532         (void) arg;
 533         xbee_rx(xbee_dev);
 534         callout_reschedule(cm, tim, XBEE_POLL_TIMER_MS);
 535 }
 536
 537 void xbee_mainloop(void)
 538 {
 539         uint8_t prio;
 540
 541         while (1) {
 542                 if (xbee_raw) {
 543                         int16_t c;
 544
 545                         /* from xbee to cmdline */
 546                         c = xbee_dev_recv(NULL);
 547                         if (c >= 0)
 548                                 cmdline_dev_send((uint8_t)c, NULL);
 549
 550                         /* from cmdline to xbee */
 551                         c = cmdline_dev_recv(NULL);
 552                         if (c == 4) { /* CTRL-d */
 553                                 prio = callout_mgr_set_prio(&xbeeboard.intr_cm,
 554                                         XBEE_PRIO);
 555                                 xbee_dev_send('A', NULL);
 556                                 xbee_dev_send('T', NULL);
 557                                 xbee_dev_send('C', NULL);
 558                                 xbee_dev_send('N', NULL);
 559                                 xbee_dev_send('\n', NULL);
 560                                 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 561                                 xbee_raw = 0;
 562                                 rdline_newline(&xbeeboard.rdl,
 563                                                xbeeboard.prompt);
 564                         }
 565                         else if (c >= 0) {
 566                                 /* send to xbee */
 567                                 xbee_dev_send((uint8_t)c, NULL);
 568
 569                                 /* echo on cmdline */
 570                                 cmdline_dev_send((uint8_t)c, NULL);
 571                         }
 572                 }
 573                 else {
 574                         if (xbee_cmdline_input_enabled)
 575                                 cmdline_poll();
 576                         /* xbee rx polling is done in a timer, so we can block
 577                          * the cmdline without loosing incoming packets */
 578                 }
 579         }
 580 }
 581
 582 void xbee_stdin_enable(void)
 583 {
 584         xbee_cmdline_input_enabled = 1;
 585 }
 586
 587 void xbee_stdin_disable(void)
 588 {
 589         xbee_cmdline_input_enabled = 0;
 590 }
 591
 592 void xbeeapp_init(void)
 593 {
 594         callout_init(&xbeeboard.xbee_rx_poll_timer, xbee_rx_poll_timer_cb,
 595                 NULL, XBEE_PRIO);
 596         callout_schedule(&xbeeboard.intr_cm,
 597                 &xbeeboard.xbee_rx_poll_timer, XBEE_POLL_TIMER_MS);
 598 }