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 NULL;
 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 *
 156 *               CanMessage method implementation
 157 *
 158 *********************************************************************************/
 159
 160 uint32_t CanMessage_c::get_id()
 161 {
 162         return id;
 163 }
 164
 165 int CanMessage_c::get_format()
 166 {
 167         return format;
 168 }
 169
 170 uint8_t CanMessage_c::get_data()
 171 {
 172         return data;
 173 }
 174 uint8_t CanMessage_c::get_lenght()
 175 {
 176         return lenght;
 177 }
 178
 179 void CanMessage_c::set_id(uint32_t new_id)
 180 {
 181         switch(format):
 182                 case CanMessageFormat::SIMPLE:
 183                         id = new_id & CAN_SFF_MASK;
 184                 case CanMessageFormat::EXTENDED:
 185                         id = new_id & CAN_EFF_MASK;
 186                 default:
 187                         ERROR(interface, "ERROR: Can set id, not a compatible format or format not set prior to set id.");
 188 }
 189
 190 void CanMessage_c::set_format(CanMessageFormat new_format)
 191 {
 192         if(new_format == CanMessageFormat::SIMPLE || new_format == CanMessageFormat::EXTENDED)
 193                 format = new_format;
 194         else
 195                 ERROR(interface, "ERROR: Can set format, wrong format chosen");
 196 }
 197
 198 void CanMessage_c::set_data(uint8_t new_data)
 199 {
 200         data = new_data;
 201 }
 202
 203 void CanMessage_c::set_lenght(uint8_t new_length)
 204 {
 205         lenght = new_lenght;
 206 }
 207
 208 /*
 209  * This is the preferred way to initialize a CanMessage object
 210  * from a read canfd_frame message.
 211  *
 212  * params: canfd_frame pointer
 213  */
 214 void CanMessage_c::convert_from_canfd_frame(canfd_frame *frame)
 215 {
 216
 217         lenght = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
 218
 219         switch (canfd_frame->can_id):
 220                 case (canfd_frame->can_id & CAN_ERR_FLAG):
 221                         id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 222                         break;
 223                 case (canfd_frame->can_id & CAN_EFF_FLAG):
 224                         id = canfd_frame->can_id & CAN_EFF_MASK;
 225                         format = CanMessageFormat::EXTENDED;
 226                         break;
 227                 default:
 228                         format = CanMessageFormat::STANDARD;
 229                         id = canfd_frame->can_id & CAN_SFF_MASK;
 230                         break;
 231
 232         if (sizeof(canfd_frame->data) <= sizeof(data))
 233         {
 234                 memcpy(data, canfd_frame->data, lenght);
 235                 return 0;
 236         } else if (sizeof(canfd_frame->data) >= CAN_MAX_DLEN)
 237                 ERROR(interface, "CanMessage_c: canfd_frame data too long to be stored into CanMessage object");
 238 }
 239
 240 canfd_frame* convert_to_canfd_frame()
 241 {
 242         canfd_frame frame;
 243
 244         frame.id = can_msg.get_id();
 245         frame.len = can_msg.get_lenght();
 246         frame.data = can_msg.get_data();
 247
 248         return &frame;
 249 }