src/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  * Get a CanMessage from can_message_q and return it
 133  * then point to the next CanMessage in queue.
 134  *
 135  * Return the next queue element or NULL if queue is empty.
 136  */
 137 CanMessage_c* CanBus_c::next_can_message()
 138 {
 139         if(! can_message_q.empty())
 140         {
 141                 CanMessage_c can_msg = can_message_q.front();
 142                 can_message_q.pop()
 143                 return &can_msg;
 144         }
 145
 146         return nullptr;
 147 }
 148
 149 void CanBus_c::insert_new_can_message(CanMessage_c *can_msg)
 150 {
 151         can_message_q.push(can_msg);
 152 }
 153
 154 /*
 155  * Get a VehicleMessage from vehicle_message_q and return it
 156  * then point to the next VehicleMessage in queue.
 157  *
 158  * Return the next queue element or NULL if queue is empty.
 159  */
 160 openxc_VehicleMessage* CanBus_c::next_vehicle_message()
 161 {
 162         if(! vehicle_message_q.empty())
 163         {
 164                 openxc_VehicleMessage v_msg = vehicle_message_q.front();
 165                 vehicle_message_q.pop()
 166                 return &v_msg;
 167         }
 168
 169         return nullptr;
 170 }
 171
 172 void CanBus_c::insert_new_vehicle_message(openxc_VehicleMessage *v_msg)
 173 {
 174         vehicle_message_q.push(v_msg);
 175 }
 176 /********************************************************************************
 177 *
 178 *               CanMessage method implementation
 179 *
 180 *********************************************************************************/
 181
 182 uint32_t CanMessage_c::get_id()
 183 {
 184         return id;
 185 }
 186
 187 int CanMessage_c::get_format()
 188 {
 189         return format;
 190 }
 191
 192 uint8_t CanMessage_c::get_data()
 193 {
 194         return data;
 195 }
 196 uint8_t CanMessage_c::get_lenght()
 197 {
 198         return lenght;
 199 }
 200
 201 void CanMessage_c::set_id(uint32_t new_id)
 202 {
 203         switch(format):
 204                 case CanMessageFormat::SIMPLE:
 205                         id = new_id & CAN_SFF_MASK;
 206                 case CanMessageFormat::EXTENDED:
 207                         id = new_id & CAN_EFF_MASK;
 208                 default:
 209                         ERROR(interface, "ERROR: Can set id, not a compatible format or format not set prior to set id.");
 210 }
 211
 212 void CanMessage_c::set_format(CanMessageFormat new_format)
 213 {
 214         if(new_format == CanMessageFormat::SIMPLE || new_format == CanMessageFormat::EXTENDED)
 215                 format = new_format;
 216         else
 217                 ERROR(interface, "ERROR: Can set format, wrong format chosen");
 218 }
 219
 220 void CanMessage_c::set_data(uint8_t new_data)
 221 {
 222         data = new_data;
 223 }
 224
 225 void CanMessage_c::set_lenght(uint8_t new_length)
 226 {
 227         lenght = new_lenght;
 228 }
 229
 230 /*
 231  * This is the preferred way to initialize a CanMessage object
 232  * from a read canfd_frame message.
 233  *
 234  * params: canfd_frame pointer
 235  */
 236 void CanMessage_c::convert_from_canfd_frame(canfd_frame *frame)
 237 {
 238
 239         lenght = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
 240
 241         switch (canfd_frame->can_id):
 242                 case (canfd_frame->can_id & CAN_ERR_FLAG):
 243                         id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 244                         break;
 245                 case (canfd_frame->can_id & CAN_EFF_FLAG):
 246                         id = canfd_frame->can_id & CAN_EFF_MASK;
 247                         format = CanMessageFormat::EXTENDED;
 248                         break;
 249                 default:
 250                         format = CanMessageFormat::STANDARD;
 251                         id = canfd_frame->can_id & CAN_SFF_MASK;
 252                         break;
 253
 254         if (sizeof(canfd_frame->data) <= sizeof(data))
 255         {
 256                 memcpy(data, canfd_frame->data, lenght);
 257                 return 0;
 258         } else if (sizeof(canfd_frame->data) >= CAN_MAX_DLEN)
 259                 ERROR(interface, "CanMessage_c: canfd_frame data too long to be stored into CanMessage object");
 260 }
 261
 262 canfd_frame* convert_to_canfd_frame()
 263 {
 264         canfd_frame frame;
 265
 266         frame.id = can_msg.get_id();
 267         frame.len = can_msg.get_lenght();
 268         frame.data = can_msg.get_data();
 269
 270         return &frame;
 271 }