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             : /*
      17             :  * Many thanks to members of the UAVCAN project:
      18             :  *  Pavel Kirienko <[email protected]>
      19             :  *  Ilia Sheremet <[email protected]>
      20             :  *
      21             :  *  license info can be found in the uavcan submodule located:
      22             :  *  modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
      23             :  */
      24             : 
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include <AP_HAL/system.h>
      27             : 
      28             : #if HAL_NUM_CAN_IFACES
      29             : 
      30             : #include "CANSocketIface.h"
      31             : 
      32             : #include <unistd.h>
      33             : #include <fcntl.h>
      34             : 
      35             : #include <sys/ioctl.h>
      36             : #include <net/if.h>
      37             : #include <cstring>
      38             : #include <cstdint>
      39             : #include "Scheduler.h"
      40             : #include <AP_CANManager/AP_CANManager.h>
      41             : #include <AP_Common/ExpandingString.h>
      42             : #include "CAN_Multicast.h"
      43             : #include "CAN_SocketCAN.h"
      44             : 
      45             : extern const AP_HAL::HAL& hal;
      46             : 
      47             : using namespace HALSITL;
      48             : 
      49             : uint8_t CANIface::_num_interfaces;
      50             : 
      51           0 : bool CANIface::is_initialized() const
      52             : {
      53           0 :     return transport != nullptr;
      54             : }
      55             : 
      56       18063 : int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline,
      57             :                        const CANIface::CanIOFlags flags)
      58             : {
      59       18063 :     WITH_SEMAPHORE(sem);
      60       18063 :     CanTxItem tx_item {};
      61       18063 :     tx_item.frame = frame;
      62       18063 :     if (flags & Loopback) {
      63           0 :         tx_item.loopback = true;
      64             :     }
      65       18063 :     if (flags & AbortOnError) {
      66           0 :         tx_item.abort_on_error = true;
      67             :     }
      68       18063 :     tx_item.setup = true;
      69       18063 :     tx_item.index = _tx_frame_counter;
      70       18063 :     tx_item.deadline = tx_deadline;
      71       18063 :     if (_tx_queue.push(tx_item)) {
      72       18063 :         _tx_frame_counter++;
      73       18063 :         stats.tx_requests++;
      74             :     } else {
      75           0 :         stats.tx_overflow++;
      76             :     }
      77       18063 :     _pollRead();     // Read poll is necessary because it can release the pending TX flag
      78       18063 :     _pollWrite();
      79             : 
      80       18063 :     return AP_HAL::CANIface::send(frame, tx_deadline, flags);
      81       18063 : }
      82             : 
      83         152 : int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
      84             :                           CANIface::CanIOFlags& out_flags)
      85             : {
      86         152 :     WITH_SEMAPHORE(sem);
      87         152 :     if (_rx_queue.is_empty()) {
      88           0 :         _pollRead();            // This allows to use the socket not calling poll() explicitly.
      89           0 :         if (_rx_queue.is_empty()) {
      90           0 :             return 0;
      91             :         }
      92             :     }
      93             :     {
      94         152 :         const CanRxItem &rx = *_rx_queue[0];
      95         152 :         out_frame        = rx.frame;
      96         152 :         out_timestamp_us = rx.timestamp_us;
      97         152 :         out_flags        = rx.flags;
      98             :     }
      99         152 :     IGNORE_RETURN(_rx_queue.pop());
     100         152 :     return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
     101         152 : }
     102             : 
     103       98905 : bool CANIface::_hasReadyTx()
     104             : {
     105       98905 :     WITH_SEMAPHORE(sem);
     106       98905 :     return !_tx_queue.is_empty();
     107       98905 : }
     108             : 
     109      107494 : bool CANIface::_hasReadyRx()
     110             : {
     111      107494 :     WITH_SEMAPHORE(sem);
     112      107494 :     return !_rx_queue.is_empty();
     113      107494 : }
     114             : 
     115       62779 : void CANIface::_poll(bool read, bool write)
     116             : {
     117       62779 :     if (read) {
     118       62779 :         _pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue
     119             :     }
     120       62779 :     if (write) {
     121       62779 :         _pollWrite();
     122             :     }
     123       62779 : }
     124             : 
     125           0 : uint32_t CANIface::getErrorCount() const
     126             : {
     127           0 :     return 0;
     128             : }
     129             : 
     130       80842 : void CANIface::_pollWrite()
     131             : {
     132       80842 :     if (transport == nullptr) {
     133           0 :         return;
     134             :     }
     135       98905 :     while (_hasReadyTx()) {
     136       18063 :         WITH_SEMAPHORE(sem);
     137       18063 :         const CanTxItem *tx = _tx_queue[0];
     138       18063 :         if (tx == nullptr) {
     139           0 :             break;
     140             :         }
     141       18063 :         const uint64_t curr_time = AP_HAL::micros64();
     142       18063 :         if (tx->deadline >= curr_time) {
     143       18063 :             bool ok = transport->send(tx->frame);
     144       18063 :             if (ok) {
     145       18063 :                 stats.tx_success++;
     146       18063 :                 stats.last_transmit_us = curr_time;
     147             :             } else {
     148           0 :                 break;
     149             :             }
     150             :         } else {
     151           0 :             stats.tx_timedout++;
     152             :         }
     153             : 
     154             :         // Removing the frame from the queue
     155       18063 :         IGNORE_RETURN(_tx_queue.pop());
     156       18063 :     }
     157             : }
     158             : 
     159       80842 : bool CANIface::_pollRead()
     160             : {
     161       80842 :     if (transport == nullptr) {
     162           0 :         return false;
     163             :     }
     164       80842 :     CanRxItem rx {};
     165       80842 :     bool ok = transport->receive(rx.frame);
     166       80842 :     if (!ok) {
     167       80690 :         return false;
     168             :     }
     169         152 :     rx.timestamp_us = AP_HAL::micros64();
     170         152 :     WITH_SEMAPHORE(sem);
     171         152 :     add_to_rx_queue(rx);
     172         152 :     stats.rx_received++;
     173         152 :     return true;
     174         152 : }
     175             : 
     176             : // Might block forever, only to be used for testing
     177           0 : void CANIface::flush_tx()
     178             : {
     179           0 :     WITH_SEMAPHORE(sem);
     180             :     do {
     181           0 :         _poll(true, true);
     182           0 :     } while(!_tx_queue.is_empty());
     183           0 : }
     184             : 
     185           0 : void CANIface::clear_rx()
     186             : {
     187           0 :     WITH_SEMAPHORE(sem);
     188             :     // Clean Rx Queue
     189           0 :     _rx_queue.clear();
     190           0 : }
     191             : 
     192           0 : void CANIface::_confirmSentFrame()
     193             : {
     194           0 :     if (_frames_in_socket_tx_queue > 0) {
     195           0 :         _frames_in_socket_tx_queue--;
     196             :     }
     197           0 : }
     198             : 
     199           5 : bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate)
     200             : {
     201           5 :     return init(bitrate);
     202             : }
     203             : 
     204           5 : bool CANIface::init(const uint32_t bitrate)
     205             : {
     206           5 :     const auto *_sitl = AP::sitl();
     207           5 :     if (_sitl == nullptr) {
     208           0 :         return false;
     209             :     }
     210           5 :     if (_self_index >= HAL_NUM_CAN_IFACES) {
     211           0 :         return false;
     212             :     }
     213           5 :     const SITL::SIM::CANTransport can_type = _sitl->can_transport[_self_index];
     214           5 :     switch (can_type) {
     215           5 :     case SITL::SIM::CANTransport::MulticastUDP:
     216           5 :         transport = NEW_NOTHROW CAN_Multicast();
     217           5 :         break;
     218             : #if HAL_CAN_WITH_SOCKETCAN
     219           0 :     case SITL::SIM::CANTransport::SocketCAN:
     220           0 :         transport = NEW_NOTHROW CAN_SocketCAN();
     221           0 :         break;
     222             : #endif
     223           0 :     case SITL::SIM::CANTransport::None:
     224             :     default: // if user supplies an invalid value for the parameter
     225           0 :         transport = nullptr;
     226           0 :         break;
     227             :     }
     228           5 :     if (transport == nullptr) {
     229           0 :         return false;
     230             :     }
     231           5 :     if (!transport->init(_self_index)) {
     232           0 :         delete transport;
     233           0 :         transport = nullptr;
     234           0 :         return false;
     235             :     }
     236           5 :     if (sem_handle != nullptr) {
     237           0 :         transport->set_event_handle(sem_handle);
     238             :     }
     239           5 :     return true;
     240             : }
     241             : 
     242       62779 : bool CANIface::select(bool &read_select, bool &write_select,
     243             :                       const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline)
     244             : {
     245       62779 :     if (transport == nullptr) {
     246           0 :         return false;
     247             :     }
     248             :     // Detecting whether we need to block at all
     249       62779 :     bool need_block = !write_select;    // Write queue is infinite
     250             : 
     251             :     // call poll here to flush some tx
     252       62779 :     _poll(true, true);
     253             : 
     254       62779 :     if (read_select && _hasReadyRx()) {
     255         152 :         need_block = false;
     256             :     }
     257             : 
     258       62779 :     if (need_block) {
     259       44563 :         _pollfd.fd = transport->get_read_fd();
     260       44563 :         _pollfd.events |= POLLIN;
     261             :     }
     262       62779 :     const uint64_t now_us = AP_HAL::micros64();
     263       62779 :     if (sem_handle != nullptr && blocking_deadline > now_us) {
     264           0 :         IGNORE_RETURN(sem_handle->wait(blocking_deadline - now_us));
     265             :     }
     266             : 
     267             :     // Writing the output masks
     268       62779 :     write_select = true;
     269       62779 :     read_select = _hasReadyRx();
     270             : 
     271       62779 :     return true;
     272             : }
     273             : 
     274           5 : bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle)
     275             : {
     276           5 :     sem_handle = handle;
     277           5 :     if (transport != nullptr) {
     278           5 :         transport->set_event_handle(handle);
     279             :     }
     280           5 :     return true;
     281             : }
     282             : 
     283             : 
     284           0 : void CANIface::get_stats(ExpandingString &str)
     285             : {
     286           0 :     str.printf("tx_requests:    %u\n"
     287             :                "tx_rejected:    %u\n"
     288             :                "tx_success:     %u\n"
     289             :                "tx_timedout:    %u\n"
     290             :                "rx_received:    %u\n"
     291             :                "rx_errors:      %u\n",
     292             :                stats.tx_requests,
     293             :                stats.tx_rejected,
     294             :                stats.tx_success,
     295             :                stats.tx_timedout,
     296             :                stats.rx_received,
     297             :                stats.rx_errors);
     298           0 : }
     299             : 
     300             : #endif