HAL_Linux: reimplement CAN with BinarySemaphore

libraries/AP_HAL_Linux/CANSocketIface.cpp

@@ -51,8 +51,6 @@ using namespace Linux;
 #define Debug(fmt, args...)
 #endif
 
-CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES];
-
 static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame)
 {
     can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } };
@@ -193,6 +191,9 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
         out_flags        = rx.flags;
     }
     (void)_rx_queue.pop();
+    if (sem_handle != nullptr) {
+        sem_handle->signal();
+    }
     return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
 }
 
@@ -507,8 +508,9 @@ bool CANIface::select(bool &read_select, bool &write_select,
                 stats.num_tx_poll_req++;
             }
         }
-        if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) {
+        const uint64_t now_us = AP_HAL::micros64();
-            _evt_handle->wait(blocking_deadline - AP_HAL::micros64());
+        if (sem_handle != nullptr && blocking_deadline > now_us) {
+            IGNORE_RETURN(sem_handle->wait(blocking_deadline - now_us));
         }
     }
 
@@ -528,67 +530,13 @@ bool CANIface::select(bool &read_select, bool &write_select,
     return true;
 }
 
-bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) {
+bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle)
-    _evt_handle = handle;
-    evt_can_socket[_self_index]._ifaces[_self_index] = this;
-    _evt_handle->set_source(&evt_can_socket[_self_index]);
-    return true;
-}
-
-
-bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle)
 {
-    if (evt_handle == nullptr) {
+    sem_handle = handle;
-        return false;
-    }
-    pollfd pollfds[HAL_NUM_CAN_IFACES] {};
-    uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {};
-    unsigned long int num_pollfds = 0;
-    
-    // Poll FD set setup
-    for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) {
-        if (_ifaces[i] == nullptr) {
-            continue;
-        }
-        if (_ifaces[i]->_down) {
-            continue;
-        }
-        pollfds[num_pollfds] = _ifaces[i]->_pollfd;
-        pollfd_iface_map[num_pollfds] = i;
-        num_pollfds++;
-        _ifaces[i]->stats.num_poll_waits++;
-    }
-
-    if (num_pollfds == 0) {
-        return true;
-    }
-
-    // Timeout conversion
-    auto ts = timespec();
-    ts.tv_sec = 0;
-    ts.tv_nsec = duration_us * 1000UL;
-
-    // Blocking here
-    const int res = ppoll(pollfds, num_pollfds, &ts, nullptr);
-
-    if (res < 0) {
-        return false;
-    }
-
-    // Handling poll output
-    for (unsigned i = 0; i < num_pollfds; i++) {
-        if (_ifaces[pollfd_iface_map[i]] == nullptr) {
-            continue;
-        }
-        _ifaces[pollfd_iface_map[i]]->_updateDownStatusFromPollResult(pollfds[i]);
-
-        const bool poll_read  = pollfds[i].revents & POLLIN;
-        const bool poll_write = pollfds[i].revents & POLLOUT;
-        _ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write);
-    }
     return true;
 }
 
+
 void CANIface::get_stats(ExpandingString &str)
 {
     str.printf("tx_requests:    %u\n"

libraries/AP_HAL_Linux/CANSocketIface.h

@@ -109,22 +109,12 @@ public:
                 uint64_t blocking_deadline) override;
     
     // setup event handle for waiting on events
-    bool set_event_handle(AP_HAL::EventHandle* handle) override;
+    bool set_event_handle(AP_HAL::BinarySemaphore *handle) override;
 
     // fetch stats text and return the size of the same,
     // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt 
     void get_stats(ExpandingString &str) override;
 
-    class CANSocketEventSource : public AP_HAL::EventSource {
-        friend class CANIface;
-        CANIface *_ifaces[HAL_NUM_CAN_IFACES];
-        
-    public:
-        // we just poll fd, no signaling is done
-        void signal(uint32_t evt_mask) override { return; }
-        bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override;
-    };
-
 private:
     void _pollWrite();
 
@@ -164,8 +154,7 @@ private:
     const unsigned _max_frames_in_socket_tx_queue;
     unsigned _frames_in_socket_tx_queue;
     uint32_t _tx_frame_counter;
-    AP_HAL::EventHandle *_evt_handle;
+    AP_HAL::BinarySemaphore *sem_handle;
-    static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES];
 
     pollfd _pollfd;
     std::map<SocketCanError, uint64_t> _errors;