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                         return -1;
  64
  65                 struct can_isotp_options opts;
  66                 memset(&opts, 0, sizeof(opts));
  67                 setopt(SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
  68
  69                 if(bind((struct sockaddr *)&tx_address_, sizeof(tx_address_)) < 0)
  70                 {
  71                         AFB_ERROR("Bind failed. %s", strerror(errno));
  72                         close();
  73                         return -1;
  74                 }
  75
  76                 return socket_;
  77         }
  78
  79         /**
  80          * @brief Allows to read message
  81          *
  82          * @return std::shared_ptr<message_t> The message that was read
  83          */
  84         std::shared_ptr<message_t> socketcan_isotp_t::read_message()
  85         {
  86
  87                 std::shared_ptr<can_message_t> cm = std::make_shared<can_message_t>();
  88                 uint8_t msg[MAX_ISOTP_FRAMES];
  89                 ssize_t nbytes = read(socket(), msg, MAX_ISOTP_FRAMES);
  90
  91                 cm->set_id(tx_address_.can_addr.tp.rx_id);
  92
  93                 if(nbytes < 0)
  94                 {
  95                         AFB_ERROR("Can't read the next message from socket '%d'. '%s'", socket(), strerror(errno));
  96                         return cm;
  97                 }
  98
  99                 std::vector<uint8_t> data;
 100                 for (int i=0; i < nbytes; i++)
 101                         data.push_back(msg[i]);
 102
 103                 std::string data_string;
 104                 data_string = converter_t::to_hex(msg, nbytes);
 105                 AFB_DEBUG("DATA ISO TP for id : %x = %s", cm->get_id(), data_string.c_str());
 106
 107
 108                 cm->set_data(data);
 109                 cm->erase_flags();
 110                 cm->set_flags(ISOTP_PROTOCOL);
 111                 cm->set_length((uint32_t)nbytes);
 112                 cm->set_sub_id((int)socket());
 113                 // Need to define behaviour
 114
 115                 return cm;
 116         }
 117
 118         /**
 119          * @brief Allows to write can message
 120          *
 121          * @param m The message to send
 122          * @return int 0 if ok else -1
 123          */
 124         int socketcan_isotp_t::write_message(message_t& m)
 125         {
 126                 size_t size = m.get_length();
 127                 if(size < MAX_ISOTP_FRAMES)
 128                 {
 129                         ssize_t ret = write(socket(), m.get_data(), size);
 130                         if(ret < 0)
 131                         {
 132                                 AFB_ERROR("Error sending : %i %s", errno, ::strerror(errno));
 133                                 return -1;
 134                         }
 135
 136                         if(ret != size)
 137                                 AFB_WARNING("ISOTP wrote only %zd byte", ret);
 138                 }
 139                 else
 140                 {
 141                         AFB_ERROR("Error sending too much data");
 142                         return -1;
 143                 }
 144
 145                 return 0;
 146         }
 147
 148         /**
 149          * @brief Define the tx address for the bind function
 150          *
 151          * @param device_name The device can that you want to bind
 152          * @param rx_id The source can_id
 153          * @param tx_id The destination can_id
 154          * @return int 0 if ok else -1
 155          */
 156         int socketcan_isotp_t::define_tx_address(std::string device_name, canid_t rx_id, canid_t tx_id)
 157         {
 158                 struct ifreq ifr;
 159                 ::strcpy(ifr.ifr_name, device_name.c_str());
 160                 AFB_DEBUG("ifr_name is : %s", ifr.ifr_name);
 161
 162                 if(::ioctl(socket_, SIOCGIFINDEX, &ifr) < 0)
 163                 {
 164                         AFB_ERROR("ioctl failed. Error was : %s", strerror(errno));
 165                         close();
 166                 }
 167                 else
 168                         tx_address_.can_ifindex = ifr.ifr_ifindex;
 169
 170                 tx_address_.can_family = AF_CAN;
 171
 172                 if(tx_id == NO_CAN_ID || rx_id == NO_CAN_ID)
 173                 {
 174                         AFB_ERROR("Error tx_id or rx_id");
 175                         return -1;
 176                 }
 177
 178                 tx_address_.can_addr.tp.rx_id = rx_id;
 179                 tx_address_.can_addr.tp.tx_id = tx_id;
 180
 181                 return 0;
 182         }
 183 }