Goby v2
rudics_packet.cpp
1 // Copyright 2009-2018 Toby Schneider (http://gobysoft.org/index.wt/people/toby)
2 // GobySoft, LLC (2013-)
3 // Massachusetts Institute of Technology (2007-2014)
4 // Community contributors (see AUTHORS file)
5 //
6 //
7 // This file is part of the Goby Underwater Autonomy Project Libraries
8 // ("The Goby Libraries").
9 //
10 // The Goby Libraries are free software: you can redistribute them and/or modify
11 // them under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 2.1 of the License, or
13 // (at your option) any later version.
14 //
15 // The Goby Libraries are distributed in the hope that they will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with Goby. If not, see <http://www.gnu.org/licenses/>.
22 
23 #include <boost/algorithm/string.hpp>
24 #include <boost/crc.hpp>
25 #include <netinet/in.h>
26 
27 #include "goby/util/base_convert.h"
28 #include "goby/util/binary.h"
29 #include "rudics_packet.h"
30 
31 void goby::acomms::serialize_rudics_packet(std::string bytes, std::string* rudics_pkt,
32  const std::string& reserved, bool include_crc)
33 {
34  if (include_crc)
35  {
36  // 1. append CRC
37  boost::crc_32_type crc;
38  crc.process_bytes(bytes.data(), bytes.length());
39  bytes += uint32_to_byte_string(crc.checksum());
40  }
41 
42  // 2. convert to base (256 minus reserved)
43  const int reduced_base = 256 - reserved.size();
44 
45  goby::util::base_convert(bytes, rudics_pkt, 256, reduced_base);
46 
47  // 3. replace reserved characters
48  for (int i = 0, n = reserved.size(); i < n; ++i)
49  {
50  std::replace(rudics_pkt->begin(), rudics_pkt->end(), reserved[i],
51  static_cast<char>(reduced_base + i));
52  }
53 
54  // 4. append CR
55  *rudics_pkt += "\r";
56 }
57 
58 void goby::acomms::parse_rudics_packet(std::string* bytes, std::string rudics_pkt,
59  const std::string& reserved, bool include_crc)
60 {
61  const unsigned CR_SIZE = 1;
62  if (rudics_pkt.size() < CR_SIZE)
63  throw(RudicsPacketException("Packet too short for <CR>"));
64 
65  // 4. remove CR
66  rudics_pkt = rudics_pkt.substr(0, rudics_pkt.size() - 1);
67 
68  const int reduced_base = 256 - reserved.size();
69 
70  // get rid of extra junk
71  rudics_pkt.erase(
72  std::remove_if(rudics_pkt.begin(), rudics_pkt.end(), boost::is_any_of(reserved)),
73  rudics_pkt.end());
74 
75  // 3. replace reserved characters
76  for (int i = 0, n = reserved.size(); i < n; ++i)
77  {
78  std::replace(rudics_pkt.begin(), rudics_pkt.end(), static_cast<char>(reduced_base + i),
79  reserved[i]);
80  }
81 
82  // 2. convert to base
83  goby::util::base_convert(rudics_pkt, bytes, reduced_base, 256);
84 
85  if (include_crc)
86  {
87  // 1. check CRC
88  const unsigned CRC_BYTE_SIZE = 4;
89  if (bytes->size() < CRC_BYTE_SIZE)
90  throw(RudicsPacketException("Packet too short for CRC32"));
91 
92  std::string crc_str = bytes->substr(bytes->size() - 4, 4);
93  uint32_t given_crc = byte_string_to_uint32(crc_str);
94  *bytes = bytes->substr(0, bytes->size() - 4);
95 
96  boost::crc_32_type crc;
97  crc.process_bytes(bytes->data(), bytes->length());
98  uint32_t computed_crc = crc.checksum();
99 
100  if (given_crc != computed_crc)
101  throw(RudicsPacketException("Bad CRC32"));
102  }
103 }
104 
105 std::string goby::acomms::uint32_to_byte_string(uint32_t i)
106 {
107  union u_t {
108  uint32_t i;
109  char c[4];
110  } u;
111  u.i = htonl(i);
112  return std::string(u.c, 4);
113 }
114 
115 uint32_t goby::acomms::byte_string_to_uint32(std::string s)
116 {
117  union u_t {
118  uint32_t i;
119  char c[4];
120  } u;
121  memcpy(u.c, s.c_str(), 4);
122  return ntohl(u.i);
123 }