summaryrefslogtreecommitdiff
path: root/ucoo/arch/host/mex/mex_msg.host.cc
diff options
context:
space:
mode:
Diffstat (limited to 'ucoo/arch/host/mex/mex_msg.host.cc')
-rw-r--r--ucoo/arch/host/mex/mex_msg.host.cc290
1 files changed, 290 insertions, 0 deletions
diff --git a/ucoo/arch/host/mex/mex_msg.host.cc b/ucoo/arch/host/mex/mex_msg.host.cc
new file mode 100644
index 0000000..5876e22
--- /dev/null
+++ b/ucoo/arch/host/mex/mex_msg.host.cc
@@ -0,0 +1,290 @@
+// ucoolib - Microcontroller object oriented library. {{{
+//
+// Copyright (C) 2012 Nicolas Schodet
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+// }}}
+#define __STDC_LIMIT_MACROS 1
+#include "mex_msg.hh"
+
+namespace ucoo {
+namespace mex {
+
+const int default_head_skip = 3;
+
+/// Compute needed size to store given format.
+static int
+calcsize (const char *fmt)
+{
+ int size = 0;
+ for (; *fmt; fmt++)
+ {
+ switch (*fmt)
+ {
+ case 'l':
+ case 'L':
+ size += 4;
+ break;
+ case 'h':
+ case 'H':
+ size += 2;
+ break;
+ case 'b':
+ case 'B':
+ size += 1;
+ break;
+ }
+ }
+ return size;
+}
+
+Msg::Msg (mtype_t mtype)
+ : head_skip_ (default_head_skip),
+ mtype_ (mtype)
+{
+ payload_.resize (head_skip_);
+}
+
+Msg::Msg (MsgReader &reader)
+ : head_skip_ (default_head_skip),
+ mtype_ (MTYPE_IDLE)
+{
+ int size = reader.size ();
+ payload_.resize (head_skip_ - 1 + size);
+ reader.read (&payload_.front () + head_skip_ - 1);
+ mtype_ = static_cast<mtype_t> (payload_[head_skip_ - 1]);
+}
+
+void
+Msg::write (MsgWriter &writer)
+{
+ assert (head_skip_ >= 1);
+ payload_[head_skip_ - 1] = static_cast<int> (mtype_);
+ writer.write (&payload_.front () + head_skip_ - 1,
+ payload_.size () - head_skip_ + 1);
+}
+
+MsgPusher
+Msg::push (const char *fmt)
+{
+ int push_index = payload_.size ();
+ payload_.resize (push_index + calcsize (fmt));
+ return MsgPusher (payload_.begin () + push_index, fmt);
+}
+
+void
+Msg::push (const char *buffer, int size)
+{
+ payload_.insert (payload_.end (), buffer, buffer + size);
+}
+
+MsgPoper
+Msg::pop (const char *fmt)
+{
+ int pop_size = calcsize (fmt);
+ assert (pop_size <= (int) payload_.size () - head_skip_);
+ int pop_index = head_skip_;
+ head_skip_ += pop_size;
+ return MsgPoper (payload_.begin () + pop_index, fmt);
+}
+
+const char *
+Msg::pop (int size)
+{
+ assert (size <= (int) payload_.size () - head_skip_);
+ const char *r = &payload_.front () + head_skip_;
+ head_skip_ += size;
+ return r;
+}
+
+MsgPusher
+Msg::encapsulate (mtype_t mtype, const char *fmt)
+{
+ int head_size = calcsize (fmt) + 1;
+ assert (head_size <= head_skip_);
+ payload_[head_skip_ - 1] = static_cast<int> (mtype_);
+ mtype_ = mtype;
+ head_skip_ -= head_size;
+ return MsgPusher (payload_.begin () + head_skip_, fmt);
+}
+
+void
+Msg::decapsulate ()
+{
+ assert (1 <= payload_.size () - head_skip_);
+ mtype_ = static_cast<mtype_t> (payload_[head_skip_]);
+ head_skip_++;
+}
+
+int
+Msg::len () const
+{
+ return payload_.size () - head_skip_;
+}
+
+MsgPusher &
+MsgPusher::operator<< (int32_t v)
+{
+ push (v);
+ return *this;
+}
+
+MsgPusher &
+MsgPusher::operator<< (uint32_t v)
+{
+ push (v);
+ return *this;
+}
+
+MsgPusher::MsgPusher (std::vector<char>::iterator out, const char *fmt)
+ : out_ (out),
+ fmt_ (fmt)
+{
+}
+
+void
+MsgPusher::push (int64_t v)
+{
+ switch (*fmt_)
+ {
+ case 'l':
+ assert (v >= INT32_MIN && v <= INT32_MAX);
+ break;
+ case 'L':
+ assert (v >= 0 && v <= UINT32_MAX);
+ break;
+ case 'h':
+ assert (v >= INT16_MIN && v <= INT16_MAX);
+ break;
+ case 'H':
+ assert (v >= 0 && v <= UINT16_MAX);
+ break;
+ case 'b':
+ assert (v >= INT8_MIN && v <= INT8_MAX);
+ break;
+ case 'B':
+ assert (v >= 0 && v <= UINT8_MAX);
+ break;
+ default:
+ assert (0);
+ }
+ switch (*fmt_)
+ {
+ case 'l':
+ case 'L':
+ *out_++ = v >> 24;
+ *out_++ = v >> 16;
+ case 'h':
+ case 'H':
+ *out_++ = v >> 8;
+ case 'b':
+ case 'B':
+ *out_++ = v >> 0;
+ }
+ fmt_++;
+}
+
+MsgPoper &
+MsgPoper::operator>> (int32_t &v)
+{
+ v = pop (31, true);
+ return *this;
+}
+
+MsgPoper &
+MsgPoper::operator>> (uint32_t &v)
+{
+ v = pop (32, false);
+ return *this;
+}
+
+MsgPoper &
+MsgPoper::operator>> (int16_t &v)
+{
+ v = pop (15, true);
+ return *this;
+}
+
+MsgPoper &
+MsgPoper::operator>> (uint16_t &v)
+{
+ v = pop (16, false);
+ return *this;
+}
+
+MsgPoper &
+MsgPoper::operator>> (int8_t &v)
+{
+ v = pop (7, true);
+ return *this;
+}
+
+MsgPoper &
+MsgPoper::operator>> (uint8_t &v)
+{
+ v = pop (8, false);
+ return *this;
+}
+
+MsgPoper::MsgPoper (std::vector<char>::const_iterator in, const char *fmt)
+ : in_ (in),
+ fmt_ (fmt)
+{
+}
+
+uint32_t
+MsgPoper::pop_bytes (int bytes)
+{
+ uint32_t r = 0;
+ while (bytes--)
+ r = r << 8 | static_cast<unsigned char> (*in_++);
+ return r;
+}
+
+int64_t
+MsgPoper::pop (int bits, bool sign)
+{
+ switch (*fmt_++)
+ {
+ case 'l':
+ assert (bits >= 31 && sign);
+ return static_cast<int32_t> (pop_bytes (4));
+ case 'L':
+ assert (bits >= 32);
+ return static_cast<uint32_t> (pop_bytes (4));
+ case 'h':
+ assert (bits >= 15 && sign);
+ return static_cast<int16_t> (pop_bytes (2));
+ case 'H':
+ assert (bits >= 16);
+ return static_cast<uint16_t> (pop_bytes (2));
+ case 'b':
+ assert (bits >= 7 && sign);
+ return static_cast<int8_t> (pop_bytes (1));
+ case 'B':
+ assert (bits >= 8);
+ return static_cast<uint8_t> (pop_bytes (1));
+ default:
+ assert_unreachable ();
+ }
+}
+
+} // namespace mex
+} // namespace ucoo