src/marnav/io/seatalk_reader.cpp

   1 #include "seatalk_reader.hpp"
   2 #include <algorithm>
   3
   4 namespace marnav
   5 {
   6 namespace io
   7 {
   8 seatalk_reader::seatalk_reader(std::unique_ptr<device> && dev)
   9         : dev(std::move(dev))
  10 {
  11         std::fill_n(reinterpret_cast<uint8_t *>(&ctx), sizeof(ctx), 0);
  12
  13         ctx.state = State::READ;
  14         ctx.remaining = 255;
  15         ctx.index = 0;
  16 }
  17
  18 seatalk_reader::~seatalk_reader() {}
  19
  20 void seatalk_reader::close()
  21 {
  22         if (dev)
  23                 dev->close();
  24         dev.reset();
  25 }
  26
  27 uint8_t seatalk_reader::parity(uint8_t a) const
  28 {
  29         int c = 0;
  30
  31         for (int i = 0; i < 8; ++i) {
  32                 if (a & 0x01)
  33                         ++c;
  34                 a >>= 1;
  35         }
  36         return (c % 2) == 0;
  37 }
  38
  39 void seatalk_reader::write_cmd(uint8_t c)
  40 {
  41         if (ctx.remaining > 0 && ctx.remaining < 254) {
  42                 ++ctx.collisions;
  43         }
  44
  45         ctx.data[0] = c;
  46         ctx.index = 1;
  47         ctx.remaining = 254;
  48 }
  49
  50 /// Writes data into the read context buffer.
  51 void seatalk_reader::write_data(uint8_t c)
  52 {
  53         if (ctx.index >= sizeof(ctx.data))
  54                 return;
  55
  56         if (ctx.remaining == 0)
  57                 return;
  58
  59         if (ctx.remaining == 255) // not yet in sync
  60                 return;
  61
  62         if (ctx.remaining == 254) {
  63                 // attribute byte, -1 because cmd is already consumed
  64                 ctx.remaining = 3 + (c & 0x0f) - 1;
  65         }
  66
  67         ctx.data[ctx.index] = c;
  68         ++ctx.index;
  69         --ctx.remaining;
  70 }
  71
  72 /// Processes SeaTalk data read from the device.
  73 ///
  74 /// This function contains a state machine, which does the handling
  75 /// of the SeaTalk specific feature: misusing the parity bit as
  76 /// indicator for command bytes.
  77 /// Since termios is in use, which provides parity error information
  78 /// as quoting bytes, a non-trivial implementation is needed to
  79 /// distinguish between normal and command bytes. Also, collision
  80 /// detection on this pseudo-bus (SeaTalk) is handled.
  81 ///
  82 /// @exception std::runtime_error Bus read error.
  83 void seatalk_reader::process_seatalk()
  84 {
  85         switch (ctx.state) {
  86                 case State::READ:
  87                         if (ctx.raw == 0xff) {
  88                                 ctx.state = State::ESCAPE;
  89                         } else {
  90                                 if (parity(ctx.raw)) {
  91                                         write_cmd(ctx.raw);
  92                                 } else {
  93                                         write_data(ctx.raw);
  94                                         if (ctx.remaining == 0)
  95                                                 emit_message();
  96                                 }
  97                         }
  98                         break;
  99
 100                 case State::ESCAPE:
 101                         if (ctx.raw == 0x00) {
 102                                 ctx.state = State::PARITY;
 103                         } else if (ctx.raw == 0xff) {
 104                                 write_data(ctx.raw);
 105                                 if (ctx.remaining == 0)
 106                                         emit_message();
 107                                 ctx.state = State::READ;
 108                         } else {
 109                                 throw std::runtime_error{"SeaTalk bus read error."};
 110                         }
 111                         break;
 112
 113                 case State::PARITY:
 114                         if (parity(ctx.raw)) {
 115                                 write_data(ctx.raw);
 116                                 if (ctx.remaining == 0)
 117                                         emit_message();
 118                         } else {
 119                                 write_cmd(ctx.raw);
 120                         }
 121                         ctx.state = State::READ;
 122                         break;
 123         }
 124 }
 125
 126 /// Reads data from the device.
 127 ///
 128 /// @retval true  Success.
 129 /// @retval false End of file.
 130 /// @exception std::runtime_error The device was invalid or read error.
 131 bool seatalk_reader::read_data()
 132 {
 133         if (!dev)
 134                 throw std::runtime_error{"device invalid"};
 135         int rc = dev->read(reinterpret_cast<char *>(&ctx.raw), sizeof(ctx.raw));
 136         if (rc == 0)
 137                 return false;
 138         if (rc < 0)
 139                 throw std::runtime_error{"read error"};
 140         if (rc != sizeof(ctx.raw))
 141                 throw std::runtime_error{"read error"};
 142         return true;
 143 }
 144
 145 /// Reads data from the device and processes it. If a complete SeaTalk
 146 /// message was received the method process_message will be executed.
 147 /// This method automatcially synchronizes with the SeaTalk bus.
 148 ///
 149 /// @retval true  Success.
 150 /// @retval false End of file.
 151 /// @exception std::runtime_error Device or processing error.
 152 bool seatalk_reader::read()
 153 {
 154         if (!read_data())
 155                 return false;
 156         process_seatalk();
 157         return true;
 158 }
 159
 160 void seatalk_reader::emit_message()
 161 {
 162         process_message(std::vector<uint8_t>{ctx.data, ctx.data + ctx.index});
 163 }
 164 }
 165 }