Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : base class for serially-attached simulated devices
17 : */
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #include <SITL/SITL.h>
21 :
22 : #include "SIM_SerialDevice.h"
23 :
24 : #include <stdio.h>
25 : #include <unistd.h>
26 : #include <fcntl.h>
27 :
28 : using namespace SITL;
29 :
30 25211 : SerialDevice::SerialDevice(uint16_t tx_bufsize, uint16_t rx_bufsize)
31 : {
32 25211 : to_autopilot = NEW_NOTHROW ByteBuffer{tx_bufsize};
33 25211 : from_autopilot = NEW_NOTHROW ByteBuffer{rx_bufsize};
34 : // devices attached to a simulated serial port have this set when
35 : // the autopilot opens that port:
36 25211 : autopilot_baud = 0;
37 25211 : }
38 :
39 374457021 : bool SerialDevice::init_sitl_pointer()
40 : {
41 374457021 : if (_sitl == nullptr) {
42 14684 : _sitl = AP::sitl();
43 14684 : if (_sitl == nullptr) {
44 0 : return false;
45 : }
46 : }
47 374457021 : return true;
48 : }
49 :
50 : #if AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
51 : ssize_t SerialDevice::corrupt_transfer(char *buffer, const ssize_t ret, const size_t size) const
52 : {
53 : if (ret > 0 && (rand() % 100) < 2) {
54 : // drop a random byte from returned data:
55 : const size_t byte_ofs_to_drop = rand() % ret;
56 : fprintf(stderr, "dropping byte at offset %u\n", unsigned(byte_ofs_to_drop));
57 : memmove(&buffer[byte_ofs_to_drop], &buffer[byte_ofs_to_drop+1], ret - byte_ofs_to_drop - 1);
58 : return ret - 1;
59 : }
60 :
61 : if (ret > 0 && size_t(ret) < size && (rand() % 100) < 2) {
62 : // add a random byte to the stream:
63 : const size_t byte_ofs_to_add = rand() % ret;
64 : fprintf(stderr, "adding byte at offset %u\n", unsigned(byte_ofs_to_add));
65 : memmove(&buffer[byte_ofs_to_add+1], &buffer[byte_ofs_to_add], ret - byte_ofs_to_add);
66 : buffer[byte_ofs_to_add] = rand()*256;
67 : return ret + 1;
68 : }
69 :
70 : if (ret > 0 && unsigned(ret) < size && (rand() % 100) < 2) {
71 : // corrupt a random byte in the stream:
72 : const size_t byte_ofs_to_corrupt = rand() % ret;
73 : fprintf(stderr, "corrupting byte at offset=%u\n", unsigned(byte_ofs_to_corrupt));
74 : buffer[byte_ofs_to_corrupt] = rand()*256;
75 : return ret;
76 : }
77 :
78 : return ret;
79 : }
80 : #endif
81 :
82 373481273 : ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) const
83 : {
84 373481273 : ssize_t ret = from_autopilot->read((uint8_t*)buffer, size);
85 : #if AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
86 : ret = corrupt_transfer(buffer, ret, size);
87 : #endif
88 :
89 : // if (ret > 0) {
90 : // ::fprintf(stderr, "SIM_SerialDevice: read from autopilot (%u): (", (unsigned)ret);
91 : // for (ssize_t i=0; i<ret; i++) {
92 : // const uint8_t x = buffer[i];
93 : // ::fprintf(stderr, "%02X", (unsigned)x);
94 : // }
95 : // ::fprintf(stderr, " ");
96 : // for (ssize_t i=0; i<ret; i++) {
97 : // ::fprintf(stderr, "%c", buffer[i]);
98 : // }
99 : // ::fprintf(stderr, ")\n");
100 : // }
101 373481273 : return ret;
102 : }
103 :
104 17843353 : ssize_t SerialDevice::write_to_autopilot(const char *buffer, const size_t size) const
105 : {
106 17843353 : if (!is_match_baud()) {
107 747 : return -1;
108 : }
109 :
110 17842606 : const ssize_t ret = to_autopilot->write((uint8_t*)buffer, size);
111 17842606 : return ret;
112 : }
113 :
114 491705065 : ssize_t SerialDevice::read_from_device(char *buffer, const size_t size) const
115 : {
116 491705065 : if (!is_match_baud()) {
117 26437 : return -1;
118 : }
119 :
120 491678628 : ssize_t ret = to_autopilot->read((uint8_t*)buffer, size);
121 : #if AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
122 : ret = corrupt_transfer(buffer, ret, size);
123 : #endif
124 : // ::fprintf(stderr, "read_from_device: (");
125 : // for (ssize_t i=0; i<ret; i++) {
126 : // ::fprintf(stderr, "%02X", (uint8_t)buffer[i]);
127 : // }
128 : // ::fprintf(stderr, ") (\n");
129 : // for (ssize_t i=0; i<ret; i++) {
130 : // ::fprintf(stderr, "%c", (uint8_t)buffer[i]);
131 : // }
132 : // ::fprintf(stderr, ")\n");
133 491678628 : return ret;
134 : }
135 3961493 : ssize_t SerialDevice::write_to_device(const char *buffer, const size_t size) const
136 : {
137 3961493 : const ssize_t ret = from_autopilot->write((uint8_t*)buffer, size);
138 3961493 : return ret;
139 : }
140 :
141 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
142 : /*
143 : attach this device to a TCP server socket. The autopilot connects to
144 : this socket (e.g. with a NET_Pn port configured as a TCP client)
145 : instead of talking to the device over a simulated serial port
146 : */
147 1 : bool SerialDevice::listen_on_tcp_port(const uint16_t port)
148 : {
149 1 : listener = NEW_NOTHROW SocketAPM_native(false);
150 1 : if (listener == nullptr) {
151 0 : return false;
152 : }
153 1 : listener->reuseaddress();
154 1 : if (!listener->bind("127.0.0.1", port) ||
155 2 : !listener->listen(1) ||
156 1 : !listener->set_blocking(false)) {
157 0 : ::fprintf(stderr, "SIM: failed to listen on TCP port %u: %m\n", unsigned(port));
158 0 : delete listener;
159 0 : listener = nullptr;
160 0 : return false;
161 : }
162 1 : ::printf("SIM: device listening for autopilot on TCP port %u\n", unsigned(port));
163 1 : return true;
164 : }
165 :
166 : /*
167 : move bytes between the network socket and this device. This performs
168 : the same role the SITL UART driver performs for serially-attached
169 : devices
170 : */
171 7245 : void SerialDevice::network_update()
172 : {
173 7245 : if (listener == nullptr) {
174 : // not attached to a socket
175 3055 : return;
176 : }
177 :
178 7245 : if (sock == nullptr) {
179 3056 : sock = listener->accept(0);
180 3056 : if (sock == nullptr) {
181 3055 : return;
182 : }
183 1 : sock->set_blocking(false);
184 : }
185 :
186 : char buffer[512];
187 :
188 : // device to autopilot:
189 : while (true) {
190 4239 : const ssize_t nread = read_from_device(buffer, sizeof(buffer));
191 4239 : if (nread <= 0) {
192 4190 : break;
193 : }
194 49 : if (sock->send(buffer, nread) != nread) {
195 : // if the autopilot is not keeping up we simply drop the data
196 0 : break;
197 : }
198 49 : }
199 :
200 : // autopilot to device:
201 : while (true) {
202 4226 : const ssize_t nread = sock->recv(buffer, sizeof(buffer), 0);
203 4226 : if (nread == 0) {
204 : // autopilot closed the connection; wait for it to reconnect
205 0 : delete sock;
206 0 : sock = nullptr;
207 0 : return;
208 : }
209 4226 : if (nread < 0) {
210 4190 : break;
211 : }
212 36 : write_to_device(buffer, nread);
213 36 : }
214 : }
215 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
216 :
217 : /**
218 : * baudrates match
219 : *
220 : * @retval true matched baudreate
221 : * @retval false unmatched baudreate
222 : */
223 509548418 : bool SerialDevice::is_match_baud() const
224 : {
225 509548418 : if (device_baud() != 0 && autopilot_baud != 0 && device_baud() != autopilot_baud) {
226 27184 : return false;
227 : }
228 509521234 : return true;
229 : }
|
