can-utils.cpp

   1 /*
   2  * Copyright (C) 2015, 2016 "IoT.bzh"
   3  * Author "Romain Forlot" <romain.forlot@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 #pragma once
  19
  20 /********************************************************************************
  21 *
  22 *               CanBus method implementation
  23 *
  24 *********************************************************************************/
  25
  26 CanBus_c::CanBus_c(afb_binding_interface *itf, const std:string& dev_name)
  27 {
  28         interface = itf;
  29         deviceName = dev_name;
  30 }
  31
  32 int CanBus_c::open()
  33 {
  34         const int canfd_on = 1;
  35         struct ifreq ifr;
  36         struct timeval timeout = {1, 0};
  37
  38         DEBUG(interface, "open_can_dev: CAN Handler socket : %d", socket);
  39         if (socket >= 0)
  40                 close(socket);
  41
  42         socket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  43         if (socket < 0)
  44         {
  45                 ERROR(interface, "open_can_dev: socket could not be created");
  46         }
  47         else
  48         {
  49                 /* Set timeout for read */
  50                 setsockopt(socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
  51                 /* try to switch the socket into CAN_FD mode */
  52                 if (setsockopt(socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0)
  53                 {
  54                         NOTICE(interface, "open_can_dev: Can not switch into CAN Extended frame format.");
  55                         is_fdmode_on = false;
  56                 } else {
  57                         is_fdmode_on = true;
  58                 }
  59
  60                 /* Attempts to open a socket to CAN bus */
  61                 strcpy(ifr.ifr_name, device);
  62                 if(ioctl(socket, SIOCGIFINDEX, &ifr) < 0)
  63                         ERROR(interface, "open_can_dev: ioctl failed");
  64                 else
  65                 {
  66                         txAddress.can_family = AF_CAN;
  67                         txAddress.can_ifindex = ifr.ifr_ifindex;
  68
  69                         /* And bind it to txAddress */
  70                         if (bind(socket, (struct sockaddr *)&txAddress, sizeof(txAddress)) < 0)
  71                         {
  72                                 ERROR(interface, "open_can_dev: bind failed");
  73                         }
  74                         else
  75                         {
  76                                 fcntl(socket, F_SETFL, O_NONBLOCK);
  77                                 return 0;
  78                         }
  79                 }
  80                 close(socket);
  81                 socket = -1;
  82         }
  83         return -1;
  84 }
  85
  86 int CanBus_c::close()
  87 {
  88         close(socket);
  89         socket = -1;
  90 }
  91
  92 void CanBus_c::start_threads()
  93 {
  94         std::queue <CanMessage_c> can_message_q;
  95
  96         th_reading = std::thread(can_reader, interface, socket, can_message_q);
  97         th_decoding = std::thread(can_decoder, interface, can_message_q, can_message_q);
  98         th_pushing = std::thread(can_event_push, interface, can_message_q);
  99 }
 100
 101 /*
 102  * Send a can message from a CanMessage_c object.
 103  */
 104 int CanBus_c::send_can_message(CanMessage_c can_msg)
 105 {
 106         int nbytes;
 107         canfd_frame *f;
 108
 109         f = can_msg.convert_to_canfd_frame();
 110
 111         if(socket >= 0)
 112         {
 113                 nbytes = sendto(socket, &f, sizeof(struct canfd_frame), 0,
 114                                 (struct sockaddr*)&txAddress, sizeof(txAddress));
 115
 116                 if (nbytes == -1)
 117                 {
 118                         ERROR(interface, "send_can_message: Sending CAN frame failed.");
 119                         return -1;
 120                 }
 121                 return nbytes;
 122         }
 123         else
 124         {
 125                 ERROR(interface, "send_can_message: socket not initialized. Attempt to reopen can device socket.");
 126                 open_can_dev();
 127         }
 128         return 0;
 129 }
 130
 131 /********************************************************************************
 132 *
 133 *               CanMessage method implementation
 134 *
 135 *********************************************************************************/
 136
 137 uint32_t CanMessage_c::get_id()
 138 {
 139         return id;
 140 }
 141
 142 int CanMessage_c::get_format()
 143 {
 144         return format;
 145 }
 146
 147 uint8_t CanMessage_c::get_data()
 148 {
 149         return data;
 150 }
 151 uint8_t CanMessage_c::get_lenght()
 152 {
 153         return lenght;
 154 }
 155
 156 void CanMessage_c::set_id(uint32_t new_id)
 157 {
 158         switch(format):
 159                 case CanMessageFormat::SIMPLE:
 160                         id = new_id & CAN_SFF_MASK;
 161                 case CanMessageFormat::EXTENDED:
 162                         id = new_id & CAN_EFF_MASK;
 163                 default:
 164                         ERROR(interface, "ERROR: Can set id, not a compatible format or format not set prior to set id.");
 165 }
 166
 167 void CanMessage_c::set_format(CanMessageFormat new_format)
 168 {
 169         if(new_format == CanMessageFormat::SIMPLE || new_format == CanMessageFormat::EXTENDED)
 170                 format = new_format;
 171         else
 172                 ERROR(interface, "ERROR: Can set format, wrong format chosen");
 173 }
 174
 175 void CanMessage_c::set_data(uint8_t new_data)
 176 {
 177         data = new_data;
 178 }
 179
 180 void CanMessage_c::set_lenght(uint8_t new_length)
 181 {
 182         lenght = new_lenght;
 183 }
 184
 185 /*
 186  * This is the preferred way to initialize a CanMessage object
 187  * from a read canfd_frame message.
 188  *
 189  * params: canfd_frame pointer
 190  */
 191 void CanMessage_c::convert_from_canfd_frame(canfd_frame *frame)
 192 {
 193
 194         lenght = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
 195
 196         switch (canfd_frame->can_id):
 197                 case (canfd_frame->can_id & CAN_ERR_FLAG):
 198                         id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 199                         break;
 200                 case (canfd_frame->can_id & CAN_EFF_FLAG):
 201                         id = canfd_frame->can_id & CAN_EFF_MASK;
 202                         format = CanMessageFormat::EXTENDED;
 203                         break;
 204                 default:
 205                         format = CanMessageFormat::STANDARD;
 206                         id = canfd_frame->can_id & CAN_SFF_MASK;
 207                         break;
 208
 209         if (sizeof(canfd_frame->data) <= sizeof(data))
 210         {
 211                 memcpy(data, canfd_frame->data, lenght);
 212                 return 0;
 213         } else if (sizeof(canfd_frame->data) >= CAN_MAX_DLEN)
 214                 ERROR(interface, "CanMessage_c: canfd_frame data too long to be stored into CanMessage object");
 215 }
 216
 217 canfd_frame* convert_to_canfd_frame()
 218 {
 219         canfd_frame frame;
 220
 221         frame.id = can_msg.get_id();
 222         frame.len = can_msg.get_lenght();
 223         frame.data = can_msg.get_data();
 224
 225         return &frame;
 226 }