Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_MAVLinkCAN.h"
17 : #include <AP_HAL/utility/sparse-endian.h>
18 : #include <AP_Common/sorting.h>
19 :
20 : #if AP_MAVLINKCAN_ENABLED
21 :
22 : extern const AP_HAL::HAL& hal;
23 :
24 : static AP_MAVLinkCAN *singleton;
25 :
26 0 : AP_MAVLinkCAN *AP_MAVLinkCAN::ensure_singleton()
27 : {
28 0 : if (singleton == nullptr) {
29 0 : singleton = NEW_NOTHROW AP_MAVLinkCAN();
30 : }
31 0 : return singleton;
32 : }
33 :
34 0 : bool AP_MAVLinkCAN::handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg)
35 : {
36 0 : auto *s = ensure_singleton();
37 0 : if (s == nullptr) {
38 0 : return false;
39 : }
40 0 : return singleton->_handle_can_forward(chan, packet, msg);
41 : }
42 :
43 0 : void AP_MAVLinkCAN::handle_can_frame(const mavlink_message_t &msg)
44 : {
45 0 : auto *s = ensure_singleton();
46 0 : if (s == nullptr) {
47 0 : return;
48 : }
49 0 : singleton->_handle_can_frame(msg);
50 : }
51 :
52 0 : void AP_MAVLinkCAN::handle_can_filter_modify(const mavlink_message_t &msg)
53 : {
54 0 : auto *s = ensure_singleton();
55 0 : if (s == nullptr) {
56 0 : return;
57 : }
58 0 : singleton->_handle_can_filter_modify(msg);
59 : }
60 :
61 :
62 : /*
63 : handle MAV_CMD_CAN_FORWARD mavlink long command
64 : */
65 0 : bool AP_MAVLinkCAN::_handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg)
66 : {
67 0 : WITH_SEMAPHORE(can_forward.sem);
68 0 : const int8_t bus = int8_t(packet.param1)-1;
69 :
70 0 : if (bus == -1) {
71 : /*
72 : a request to stop forwarding
73 : */
74 0 : if (can_forward.callback_id != 0) {
75 0 : hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id);
76 0 : can_forward.callback_id = 0;
77 : }
78 0 : return true;
79 : }
80 :
81 0 : if (bus < 0 || bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) {
82 0 : return false;
83 : }
84 :
85 0 : if (can_forward.callback_id != 0 && can_forward.callback_bus != bus && can_forward.callback_bus < HAL_NUM_CAN_IFACES) {
86 : /*
87 : the client is changing which bus they are monitoring, unregister from the previous bus
88 : */
89 0 : hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id);
90 0 : can_forward.callback_id = 0;
91 : }
92 :
93 0 : if (can_forward.callback_id == 0 &&
94 0 : !hal.can[bus]->register_frame_callback(
95 0 : FUNCTOR_BIND_MEMBER(&AP_MAVLinkCAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags), can_forward.callback_id)) {
96 : // failed to register the callback
97 0 : return false;
98 : }
99 :
100 0 : can_forward.callback_bus = bus;
101 0 : can_forward.last_callback_enable_ms = AP_HAL::millis();
102 0 : can_forward.chan = chan;
103 0 : can_forward.system_id = msg.sysid;
104 0 : can_forward.component_id = msg.compid;
105 :
106 0 : return true;
107 0 : }
108 :
109 : /*
110 : handle a CAN_FRAME packet
111 : */
112 0 : void AP_MAVLinkCAN::_handle_can_frame(const mavlink_message_t &msg)
113 : {
114 0 : if (frame_buffer == nullptr) {
115 : // allocate frame buffer
116 : // 20 is good for firmware upload
117 0 : uint8_t buffer_size = 20;
118 0 : WITH_SEMAPHORE(frame_buffer_sem);
119 0 : while (frame_buffer == nullptr && buffer_size > 0) {
120 : // we'd like 20 frames, but will live with less
121 0 : frame_buffer = NEW_NOTHROW ObjectBuffer<BufferFrame>(buffer_size);
122 0 : if (frame_buffer != nullptr && frame_buffer->get_size() != 0) {
123 : // register a callback for when frames can't be sent immediately
124 0 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_MAVLinkCAN::process_frame_buffer, void));
125 0 : break;
126 : }
127 0 : delete frame_buffer;
128 0 : frame_buffer = nullptr;
129 0 : buffer_size /= 2;
130 : }
131 0 : if (frame_buffer == nullptr) {
132 : // discard the frames
133 0 : return;
134 : }
135 0 : }
136 :
137 0 : switch (msg.msgid) {
138 0 : case MAVLINK_MSG_ID_CAN_FRAME: {
139 : mavlink_can_frame_t p;
140 0 : mavlink_msg_can_frame_decode(&msg, &p);
141 0 : if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
142 0 : return;
143 : }
144 : struct BufferFrame frame {
145 0 : bus : p.bus,
146 0 : frame : AP_HAL::CANFrame(p.id, p.data, p.len)
147 0 : };
148 : {
149 0 : WITH_SEMAPHORE(frame_buffer_sem);
150 0 : frame_buffer->push(frame);
151 0 : }
152 0 : break;
153 : }
154 : #if HAL_CANFD_SUPPORTED
155 0 : case MAVLINK_MSG_ID_CANFD_FRAME: {
156 : mavlink_canfd_frame_t p;
157 0 : mavlink_msg_canfd_frame_decode(&msg, &p);
158 0 : if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
159 0 : return;
160 : }
161 : struct BufferFrame frame {
162 0 : bus : p.bus,
163 0 : frame : AP_HAL::CANFrame(p.id, p.data, p.len, true)
164 0 : };
165 : {
166 0 : WITH_SEMAPHORE(frame_buffer_sem);
167 0 : frame_buffer->push(frame);
168 0 : }
169 0 : break;
170 : }
171 : #endif
172 : }
173 0 : process_frame_buffer();
174 : }
175 :
176 : /*
177 : process the frame buffer
178 : */
179 0 : void AP_MAVLinkCAN::process_frame_buffer()
180 : {
181 0 : while (frame_buffer) {
182 0 : WITH_SEMAPHORE(frame_buffer_sem);
183 0 : struct BufferFrame frame;
184 0 : const uint16_t timeout_us = 2000;
185 0 : if (!frame_buffer->peek(frame)) {
186 : // no frames in the queue
187 0 : break;
188 : }
189 0 : const int16_t retcode = hal.can[frame.bus]->send(frame.frame,
190 0 : AP_HAL::micros64() + timeout_us,
191 : AP_HAL::CANIface::IsForwardedFrame);
192 0 : if (retcode == 0) {
193 : // no space in the CAN output slots, try again later
194 0 : break;
195 : }
196 : // retcode == 1 means sent, -1 means a frame that can't be
197 : // sent. Either way we should remove from the queue
198 0 : frame_buffer->pop();
199 0 : }
200 0 : }
201 :
202 : /*
203 : handle a CAN_FILTER_MODIFY packet
204 : */
205 0 : void AP_MAVLinkCAN::_handle_can_filter_modify(const mavlink_message_t &msg)
206 : {
207 : mavlink_can_filter_modify_t p;
208 0 : mavlink_msg_can_filter_modify_decode(&msg, &p);
209 0 : const int8_t bus = int8_t(p.bus)-1;
210 0 : if (bus < 0 || bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) {
211 0 : return;
212 : }
213 0 : if (p.num_ids > ARRAY_SIZE(p.ids)) {
214 0 : return;
215 : }
216 0 : uint16_t *new_ids = nullptr;
217 0 : uint16_t num_new_ids = 0;
218 0 : WITH_SEMAPHORE(can_forward.sem);
219 :
220 : // sort the list, so we can bisection search and the array
221 : // operations below are efficient
222 0 : insertion_sort_uint16(p.ids, p.num_ids);
223 :
224 0 : switch (p.operation) {
225 0 : case CAN_FILTER_REPLACE: {
226 0 : if (p.num_ids == 0) {
227 0 : can_forward.num_filter_ids = 0;
228 0 : delete[] can_forward.filter_ids;
229 0 : can_forward.filter_ids = nullptr;
230 0 : return;
231 : }
232 0 : if (p.num_ids == can_forward.num_filter_ids &&
233 0 : memcmp(p.ids, can_forward.filter_ids, p.num_ids*sizeof(uint16_t)) == 0) {
234 : // common case of replacing with identical list
235 0 : return;
236 : }
237 0 : new_ids = NEW_NOTHROW uint16_t[p.num_ids];
238 0 : if (new_ids != nullptr) {
239 0 : num_new_ids = p.num_ids;
240 0 : memcpy((void*)new_ids, (const void *)p.ids, p.num_ids*sizeof(uint16_t));
241 : }
242 0 : break;
243 : }
244 0 : case CAN_FILTER_ADD: {
245 0 : if (common_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids,
246 0 : p.ids, p.num_ids) == p.num_ids) {
247 : // nothing changing
248 0 : return;
249 : }
250 0 : new_ids = NEW_NOTHROW uint16_t[can_forward.num_filter_ids+p.num_ids];
251 0 : if (new_ids == nullptr) {
252 0 : return;
253 : }
254 0 : if (can_forward.num_filter_ids != 0) {
255 0 : memcpy(new_ids, can_forward.filter_ids, can_forward.num_filter_ids*sizeof(uint16_t));
256 : }
257 0 : memcpy(&new_ids[can_forward.num_filter_ids], p.ids, p.num_ids*sizeof(uint16_t));
258 0 : insertion_sort_uint16(new_ids, can_forward.num_filter_ids+p.num_ids);
259 0 : num_new_ids = remove_duplicates_uint16(new_ids, can_forward.num_filter_ids+p.num_ids);
260 0 : break;
261 : }
262 0 : case CAN_FILTER_REMOVE: {
263 0 : if (common_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids,
264 0 : p.ids, p.num_ids) == 0) {
265 : // nothing changing
266 0 : return;
267 : }
268 0 : can_forward.num_filter_ids = remove_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids,
269 0 : p.ids, p.num_ids);
270 0 : if (can_forward.num_filter_ids == 0) {
271 0 : delete[] can_forward.filter_ids;
272 0 : can_forward.filter_ids = nullptr;
273 : }
274 0 : break;
275 : }
276 : }
277 0 : if (new_ids != nullptr) {
278 : // handle common case of no change
279 0 : if (num_new_ids == can_forward.num_filter_ids &&
280 0 : memcmp(new_ids, can_forward.filter_ids, num_new_ids*sizeof(uint16_t)) == 0) {
281 0 : delete[] new_ids;
282 : } else {
283 : // put the new list in place
284 0 : delete[] can_forward.filter_ids;
285 0 : can_forward.filter_ids = new_ids;
286 0 : can_forward.num_filter_ids = num_new_ids;
287 : }
288 : }
289 0 : }
290 :
291 : /*
292 : handler for CAN frames from the registered callback, sending frames
293 : out as CAN_FRAME or CANFD_FRAME messages
294 : */
295 0 : void AP_MAVLinkCAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags)
296 : {
297 : mavlink_channel_t chan;
298 : uint8_t system_id;
299 : uint8_t component_id;
300 : {
301 0 : WITH_SEMAPHORE(can_forward.sem);
302 0 : if (bus != can_forward.callback_bus) {
303 : // we are not registered for forwarding this bus, discard frame
304 0 : return;
305 : }
306 0 : if (can_forward.frame_counter++ == 100) {
307 : // check every 100 frames for disabling CAN_FRAME send
308 : // we stop sending after 5s if the client stops
309 : // sending MAV_CMD_CAN_FORWARD requests
310 0 : if (can_forward.callback_id != 0 &&
311 0 : AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) {
312 0 : hal.can[bus]->unregister_frame_callback(can_forward.callback_id);
313 0 : can_forward.callback_id = 0;
314 0 : return;
315 : }
316 0 : can_forward.frame_counter = 0;
317 : }
318 0 : if (can_forward.filter_ids != nullptr) {
319 : // work out ID of this frame
320 0 : uint16_t id = 0;
321 0 : if ((frame.id&0xff) != 0) {
322 : // not anonymous
323 0 : if (frame.id & 0x80) {
324 : // service message
325 0 : id = uint8_t(frame.id>>16);
326 : } else {
327 : // message frame
328 0 : id = uint16_t(frame.id>>8);
329 : }
330 : }
331 0 : if (!bisect_search_uint16(can_forward.filter_ids, can_forward.num_filter_ids, id)) {
332 0 : return;
333 : }
334 : }
335 : // remeber destination while we hold the mutex
336 0 : chan = can_forward.chan;
337 0 : system_id = can_forward.system_id;
338 0 : component_id = can_forward.component_id;
339 0 : }
340 :
341 : // the rest is run without the can_forward.sem
342 0 : WITH_SEMAPHORE(comm_chan_lock(chan));
343 0 : const uint8_t data_len = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
344 : #if HAL_CANFD_SUPPORTED
345 0 : if (frame.isCanFDFrame()) {
346 0 : if (HAVE_PAYLOAD_SPACE(chan, CANFD_FRAME)) {
347 0 : mavlink_msg_canfd_frame_send(chan, system_id, component_id,
348 0 : bus, data_len, frame.id, const_cast<uint8_t*>(frame.data));
349 : }
350 : } else
351 : #endif
352 : {
353 0 : if (HAVE_PAYLOAD_SPACE(chan, CAN_FRAME)) {
354 0 : mavlink_msg_can_frame_send(chan, system_id, component_id,
355 0 : bus, data_len, frame.id, const_cast<uint8_t*>(frame.data));
356 : }
357 : }
358 0 : }
359 :
360 : #endif // AP_MAVLINKCAN_ENABLED
|
