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