From 048fcb8d510cdbf4cf6b207961ca59c82848e6fb Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 4 Mar 2013 21:38:38 +0100 Subject: digital/ucoolib/ucoolib/base/proto: add legacy proto handler --- digital/ucoolib/ucoolib/base/proto/proto.cc | 257 ++++++++++++++++++++++++++++ 1 file changed, 257 insertions(+) create mode 100644 digital/ucoolib/ucoolib/base/proto/proto.cc (limited to 'digital/ucoolib/ucoolib/base/proto/proto.cc') diff --git a/digital/ucoolib/ucoolib/base/proto/proto.cc b/digital/ucoolib/ucoolib/base/proto/proto.cc new file mode 100644 index 00000000..2e41a17f --- /dev/null +++ b/digital/ucoolib/ucoolib/base/proto/proto.cc @@ -0,0 +1,257 @@ +// ucoolib - Microcontroller object oriented library. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "proto.hh" + +#include + +namespace ucoo { + +Proto::Proto (Handler &handler, Stream &stream) + : handler_ (handler), stream_ (stream), step_ (IDLE) +{ +} + +void +Proto::accept () +{ + int c; + while ((c = stream_.getc ()) != -1) + { + if (c == '!') + step_ = BANG; + else + { + switch (step_) + { + case IDLE: + // Nothing received yet. + break; + case BANG: + // Bang received yet. + if (std::isalpha (c)) + { + cmd_ = c; + size_ = 0; + step_ = COMMAND; + } + else + { + handler_.proto_handle (*this, '?', 0, 0); + step_ = IDLE; + } + break; + case COMMAND: + // Command received yet. + if (c == '\r' || c == '\n') + { + handler_.proto_handle (*this, cmd_, args_, size_); + step_ = IDLE; + } + else if (c == '\'') + step_ = ARG_CHAR; + else if (c == '"') + step_ = ARG_STRING; + else + { + step_ = ARG_DIGIT; + accept_digit (c); + } + break; + case ARG_DIGIT: + step_ = COMMAND; + accept_digit (c); + break; + case ARG_CHAR: + step_ = COMMAND; + accept_char (c); + break; + case ARG_STRING: + if (c == '\r' || c == '\n') + { + handler_.proto_handle (*this, cmd_, args_, size_); + step_ = IDLE; + } + else + { + accept_char (c); + } + break; + } + } + } +} + +void +Proto::send (char cmd) +{ + char buf[3]; + char *p = buf; + *p++ = '!'; + *p++ = cmd; + *p++ = '\n'; + stream_.write (buf, p - buf); +} + +void +Proto::send (char cmd, const char *fmt, int a0) +{ + send (cmd, fmt, a0, 0, 0, 0); +} + +void +Proto::send (char cmd, const char *fmt, int a0, int a1) +{ + send (cmd, fmt, a0, a1, 0, 0); +} + +void +Proto::send (char cmd, const char *fmt, int a0, int a1, int a2) +{ + send (cmd, fmt, a0, a1, a2, 0); +} + +static inline void +send_byte (char *buf, uint8_t b) +{ + static const char hexchars[] = "0123456789abcdef"; + buf[0] = hexchars[b >> 4]; + buf[1] = hexchars[b & 0xf]; +} + +static char * +send_arg (char *buf, char fmt, int v) +{ + switch (fmt) + { + case 'L': + case 'l': + send_byte (buf, (v >> 24) & 0xff); + buf += 2; + send_byte (buf, (v >> 16) & 0xff); + buf += 2; + case 'H': + case 'h': + send_byte (buf, (v >> 8) & 0xff); + buf += 2; + case 'B': + case 'b': + send_byte (buf, v & 0xff); + buf += 2; + break; + default: + assert_unreachable (); + } + return buf; +} + +void +Proto::send (char cmd, const char *fmt, int a0, int a1, int a2, int a3) +{ + char buf[3 + 4 * 4 * 2]; // Large enough for the largest frame. + char *p = buf; + *p++ = '!'; + *p++ = cmd; + if (*fmt) + { + p = send_arg (p, *fmt++, a0); + if (*fmt) + { + p = send_arg (p, *fmt++, a1); + if (*fmt) + { + p = send_arg (p, *fmt++, a2); + if (*fmt) + { + p = send_arg (p, *fmt++, a3); + } + } + } + } + *p++ = '\n'; + stream_.write (buf, p - buf); +} + +void +Proto::send_buf (char cmd, const uint8_t *args, int size) +{ + int i; + char buf[3 + 2 * size]; + char *p = buf; + *p++ = '!'; + *p++ = cmd; + for (i = 0; i < size; i++) + { + send_byte (p, args[i]); + p += 2; + } + *p++ = '\n'; + stream_.write (buf, p - buf); +} + +void +Proto::accept_digit (int c) +{ + // Test for argument list overflow. + if (size_ >= UCOO_CONFIG_BASE_PROTO_ARGS_MAX_SIZE) + { + handler_.proto_handle (*this, '?', 0, 0); + step_ = IDLE; + return; + } + // Convert from hexa. + if ('0' <= c && c <= '9') + c -= '0'; + else if ('a' <= c && c <= 'f') + c -= 'a' - 10; + else if ('A' <= c && c <= 'F') + c -= 'A' - 10; + else + { + handler_.proto_handle (*this, '?', 0, 0); + step_ = IDLE; + return; + } + // Add digit. + args_[size_] <<= 4; + args_[size_] |= c; + if (step_ == COMMAND) + size_++; +} + +void +Proto::accept_char (int c) +{ + // Test for argument list overflow or unwanted char. + if (size_ >= UCOO_CONFIG_BASE_PROTO_ARGS_MAX_SIZE || !std::isprint (c)) + { + handler_.proto_handle (*this, '?', 0, 0); + step_ = IDLE; + return; + } + // Add char. + args_[size_] = c; + size_++; +} + +} // namespace ucoo -- cgit v1.2.3