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/LICENSE
      23             :  *  modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
      24             :  */
      25             : 
      26             : 
      27             : #pragma once
      28             : 
      29             : #include "AP_HAL_Linux.h"
      30             : 
      31             : #if HAL_NUM_CAN_IFACES
      32             : 
      33             : #include <AP_HAL/CANIface.h>
      34             : 
      35             : #include <linux/can.h>
      36             : 
      37             : #include <string>
      38             : #include <queue>
      39             : #include <memory>
      40             : #include <map>
      41             : #include <unordered_set>
      42             : #include <poll.h>
      43             : 
      44             : namespace Linux {
      45             : 
      46             : enum class SocketCanError
      47             : {
      48             :     SocketReadFailure,
      49             :     SocketWriteFailure,
      50             :     TxTimeout
      51             : };
      52             : 
      53             : #define CAN_MAX_POLL_ITERATIONS_COUNT 100
      54             : #define CAN_MAX_INIT_TRIES_COUNT 100
      55             : 
      56             : class CANIface: public AP_HAL::CANIface {
      57             : public:
      58           0 :     CANIface(int index)
      59           0 :       : _self_index(index)
      60           0 :       , _max_frames_in_socket_tx_queue(2)
      61           0 :       , _frames_in_socket_tx_queue(0)
      62           0 :     { }
      63             : 
      64             :     ~CANIface() { }
      65             : 
      66             :     // Initialise CAN Peripheral
      67             :     bool init(const uint32_t bitrate) override;
      68             : 
      69             :     // Put frame into Tx FIFO returns negative on error, 0 on buffer full, 
      70             :     // 1 on successfully pushing a frame into FIFO
      71             :     int16_t send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
      72             :                  CanIOFlags flags) override;
      73             : 
      74             :     // Receive frame from Rx Buffer, returns negative on error, 0 on nothing available, 
      75             :     // 1 on successfully poping a frame
      76             :     int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
      77             :                     CanIOFlags& out_flags) override;
      78             : 
      79             :     // Always return false, there's no busoff condition in Linux CAN
      80           0 :     bool is_busoff() const override
      81             :     {
      82           0 :         return false;
      83             :     }
      84             : 
      85             :     void flush_tx() override;
      86             : 
      87             :     void clear_rx() override;
      88             : 
      89             :     // Get total number of Errors discovered
      90             :     uint32_t getErrorCount() const override;
      91             : 
      92             :     // returns true if init was successfully called
      93             :     bool is_initialized() const override;
      94             : 
      95             :     /******************************************
      96             :      * Select Method                          *
      97             :      * ****************************************/
      98             :     // wait until selected event is available, false when timed out waiting else true
      99             :     bool select(bool &read, bool &write,
     100             :                 const AP_HAL::CANFrame* const pending_tx,
     101             :                 uint64_t blocking_deadline) override;
     102             :     
     103             :     // setup event handle for waiting on events
     104             :     bool set_event_handle(AP_HAL::BinarySemaphore *handle) override;
     105             : 
     106             :     // fetch stats text and return the size of the same,
     107             :     // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt 
     108             :     void get_stats(ExpandingString &str) override;
     109             : 
     110             : private:
     111             :     void _pollWrite();
     112             : 
     113             :     bool _pollRead();
     114             : 
     115             :     int _write(const AP_HAL::CANFrame& frame) const;
     116             : 
     117             :     int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const;
     118             : 
     119             :     void _incrementNumFramesInSocketTxQueue();
     120             : 
     121             :     void _confirmSentFrame();
     122             : 
     123             :     bool _wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame);
     124             : 
     125             :     bool _hasReadyTx();
     126             : 
     127             :     bool _hasReadyRx();
     128             : 
     129             :     void _poll(bool read, bool write);
     130             : 
     131             :     int _openSocket(const std::string& iface_name);
     132             : 
     133             :     void _updateDownStatusFromPollResult(const pollfd& pfd);
     134             : 
     135             :     uint32_t _bitrate;
     136             : 
     137             :     bool _down;
     138             :     bool _initialized;
     139             : 
     140             :     int _fd;
     141             : 
     142             :     const uint8_t _self_index;
     143             : 
     144             :     const unsigned _max_frames_in_socket_tx_queue;
     145             :     unsigned _frames_in_socket_tx_queue;
     146             :     uint32_t _tx_frame_counter;
     147             :     AP_HAL::BinarySemaphore *sem_handle;
     148             : 
     149             :     pollfd _pollfd;
     150             :     std::map<SocketCanError, uint64_t> _errors;
     151             :     std::priority_queue<CanTxItem> _tx_queue;
     152             :     std::queue<CanRxItem> _rx_queue;
     153             :     std::unordered_multiset<uint32_t> _pending_loopback_ids;
     154             : 
     155             :     struct bus_stats : public AP_HAL::CANIface::bus_stats_t {
     156             :         uint32_t tx_confirmed;
     157             :         uint32_t num_downs;
     158             :         uint32_t num_rx_poll_req;
     159             :         uint32_t num_tx_poll_req;
     160             :         uint32_t num_poll_waits;
     161             :         uint32_t num_poll_tx_events;
     162             :         uint32_t num_poll_rx_events;
     163             :     } stats;
     164             : 
     165             : protected:
     166           0 :     bool add_to_rx_queue(const CanRxItem &rx_item) override {
     167           0 :         _rx_queue.push(rx_item);
     168           0 :         return true;
     169             :     }
     170             : 
     171           0 :     int8_t get_iface_num(void) const override {
     172           0 :         return _self_index;
     173             :     }
     174             :     HAL_Semaphore sem;
     175             : };
     176             : 
     177             : }
     178             : 
     179             : #endif //#if HAL_NUM_CAN_IFACES