low-can-binding/utils/socketcan-isotp.cpp

   1 /*
   2  * Copyright (C) 2019, 2020 "IoT.bzh"
   3  * Author "Arthur Guyader" <arthur.guyader@iot.bzh>
   4  *
   5  * Licensed under the Apache License, Version 2.0 (the "License");
   6  * you may not use this file except in compliance with the License.
   7  * You may obtain a copy of the License at
   8  *
   9  *       http://www.apache.org/licenses/LICENSE-2.0
  10  *
  11  * Unless required by applicable law or agreed to in writing, software
  12  * distributed under the License is distributed on an "AS IS" BASIS,
  13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14  * See the License for the specific language governing permissions and
  15  * limitations under the License.
  16  */
  17
  18 #include "socketcan-isotp.hpp"
  19
  20 #include <net/if.h>
  21 #include <sys/ioctl.h>
  22 #include <fcntl.h>
  23 #include <unistd.h>
  24 #include "../utils/converter.hpp"
  25 #include "../binding/application.hpp"
  26
  27 namespace utils
  28 {
  29         /**
  30          * @brief Open ISOTP socket this default function will not work, because
  31          * isotp need rx_id and tx_id
  32          *
  33          * @param device_name The device name where to open socket
  34          * @return int -1 fail
  35          */
  36         int socketcan_isotp_t::open(std::string device_name)
  37         {
  38                 AFB_WARNING("NOT USE THIS FUNCTION !");
  39                 return open(device_name,NO_CAN_ID,NO_CAN_ID);
  40         }
  41
  42         /**
  43          * @brief Open ISOTP socket, the socket will be open and bind
  44          * with rx_id and tx_id
  45          *
  46          * @param device_name The device name where to open socket
  47          * @param rx_id The source can_id
  48          * @param tx_id The destination can_id
  49          * @return int 0 if ok else -1
  50          */
  51         int socketcan_isotp_t::open(std::string device_name, canid_t rx_id, canid_t tx_id)
  52         {
  53                 close();
  54                 socket_ = socketcan_t::open(PF_CAN, SOCK_DGRAM, CAN_ISOTP);
  55
  56                 if(socket_ < 0)
  57                 {
  58                         AFB_ERROR("Error open ISO TP socket");
  59                         return -1;
  60                 }
  61
  62                 if(define_tx_address(device_name,rx_id,tx_id) < 0)
  63                 {
  64                         return -1;
  65                 }
  66
  67                 struct can_isotp_options opts;
  68                 memset(&opts,0,sizeof(opts));
  69                 setopt(SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
  70
  71                 if(bind((struct sockaddr *)&tx_address_, sizeof(tx_address_)) < 0)
  72                 {
  73                         AFB_ERROR("Bind failed. %s", strerror(errno));
  74                         close();
  75                         return -1;
  76                 }
  77
  78                 return socket_;
  79         }
  80
  81         /**
  82          * @brief Allows to read message
  83          *
  84          * @return std::shared_ptr<message_t> The message that was read
  85          */
  86         std::shared_ptr<message_t> socketcan_isotp_t::read_message()
  87         {
  88
  89                 std::shared_ptr<can_message_t> cm = std::make_shared<can_message_t>();
  90                 uint8_t msg[MAX_ISOTP_FRAMES];
  91                 ssize_t nbytes = read(socket(),msg,MAX_ISOTP_FRAMES);
  92
  93                 cm->set_id(tx_address_.can_addr.tp.rx_id);
  94
  95                 if(nbytes < 0)
  96                 {
  97                         AFB_ERROR("Can't read the next message from socket '%d'. '%s'", socket(), strerror(errno));
  98                         return cm;
  99                 }
 100
 101                 std::vector<uint8_t> data;
 102                 for (int i=0; i < nbytes; i++)
 103                 {
 104                         data.push_back(msg[i]);
 105                 }
 106
 107                 std::string data_string;
 108                 data_string = converter_t::to_hex(msg,nbytes);
 109                 AFB_DEBUG("DATA ISO TP for id : %x = %s",cm->get_id(),data_string.c_str());
 110
 111
 112                 cm->set_data(data);
 113                 cm->erase_flags();
 114                 cm->set_flags(ISOTP_PROTOCOL);
 115                 cm->set_length((uint32_t)nbytes);
 116                 cm->set_sub_id((int)socket());
 117                 // Need to define behaviour
 118
 119                 return cm;
 120         }
 121
 122         /**
 123          * @brief Allows to write can message
 124          *
 125          * @param m The message to send
 126          * @return int 0 if ok else -1
 127          */
 128         int socketcan_isotp_t::write_message(message_t& m)
 129         {
 130                 size_t size = m.get_length();
 131                 if(size < MAX_ISOTP_FRAMES)
 132                 {
 133                         ssize_t ret = write(socket(),m.get_data(),size);
 134                         if(ret < 0)
 135                         {
 136                                 AFB_ERROR("Error sending : %i %s", errno, ::strerror(errno));
 137                                 return -1;
 138                         }
 139
 140                         if(ret != size)
 141                         {
 142                                 AFB_WARNING("ISOTP wrote only %zd byte",ret);
 143                         }
 144                 }
 145                 else
 146                 {
 147                         AFB_ERROR("Error sending too much data");
 148                         return -1;
 149                 }
 150
 151                 return 0;
 152         }
 153
 154         /**
 155          * @brief Define the tx address for the bind function
 156          *
 157          * @param device_name The device can that you want to bind
 158          * @param rx_id The source can_id
 159          * @param tx_id The destination can_id
 160          * @return int 0 if ok else -1
 161          */
 162         int socketcan_isotp_t::define_tx_address(std::string device_name, canid_t rx_id, canid_t tx_id)
 163         {
 164                 struct ifreq ifr;
 165                 ::strcpy(ifr.ifr_name, device_name.c_str());
 166                 AFB_DEBUG("ifr_name is : %s", ifr.ifr_name);
 167
 168                 if(::ioctl(socket_, SIOCGIFINDEX, &ifr) < 0)
 169                 {
 170                         AFB_ERROR("ioctl failed. Error was : %s", strerror(errno));
 171                         close();
 172                 }
 173                 else
 174                 {
 175                         tx_address_.can_ifindex = ifr.ifr_ifindex;
 176                 }
 177
 178                 tx_address_.can_family = AF_CAN;
 179
 180                 if(tx_id == NO_CAN_ID || rx_id == NO_CAN_ID)
 181                 {
 182                         AFB_ERROR("Error tx_id or rx_id");
 183                         return -1;
 184                 }
 185
 186                 tx_address_.can_addr.tp.rx_id = rx_id;
 187                 tx_address_.can_addr.tp.tx_id = tx_id;
 188
 189                 return 0;
 190         }
 191 }