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 int CanBus_c::open()
  27 {
  28         const int canfd_on = 1;
  29         struct ifreq ifr;
  30         struct timeval timeout = {1, 0};
  31
  32         DEBUG(interface, "open_can_dev: CAN Handler socket : %d", socket);
  33         if (socket >= 0)
  34                 close(socket);
  35
  36         socket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  37         if (socket < 0)
  38         {
  39                 ERROR(interface, "open_can_dev: socket could not be created");
  40         }
  41         else
  42         {
  43                 /* Set timeout for read */
  44                 setsockopt(socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
  45                 /* try to switch the socket into CAN_FD mode */
  46                 if (setsockopt(socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0)
  47                 {
  48                         NOTICE(interface, "open_can_dev: Can not switch into CAN Extended frame format.");
  49                         is_fdmode_on = false;
  50                 } else {
  51                         is_fdmode_on = true;
  52                 }
  53
  54                 /* Attempts to open a socket to CAN bus */
  55                 strcpy(ifr.ifr_name, device);
  56                 if(ioctl(socket, SIOCGIFINDEX, &ifr) < 0)
  57                         ERROR(interface, "open_can_dev: ioctl failed");
  58                 else
  59                 {
  60                         txAddress.can_family = AF_CAN;
  61                         txAddress.can_ifindex = ifr.ifr_ifindex;
  62
  63                         /* And bind it to txAddress */
  64                         if (bind(socket, (struct sockaddr *)&txAddress, sizeof(txAddress)) < 0)
  65                         {
  66                                 ERROR(interface, "open_can_dev: bind failed");
  67                         }
  68                         else
  69                         {
  70                                 fcntl(socket, F_SETFL, O_NONBLOCK);
  71                                 return 0;
  72                         }
  73                 }
  74                 close(socket);
  75                 socket = -1;
  76         }
  77         return -1;
  78 }
  79
  80 int CanBus_c::close()
  81 {
  82         close(socket);
  83         socket = -1;
  84 }
  85
  86 void CanBus_c::start_threads()
  87 {
  88         std::queue <CanMessage_t> can_message_q;
  89
  90     th_reading = std::thread(can_reader, interface, socket, can_message_q);
  91     th_decoding = std::thread(can_decoder, interface, can_message_q, can_message_queue);
  92     th_pushing = std::thread(can_event_push, interface, can_message_queue);
  93 }
  94
  95 /********************************************************************************
  96 *
  97 *               CanMessage method implementation
  98 *
  99 *********************************************************************************/
 100
 101 uint32_t CanMessage_c::get_id()
 102 {
 103     return id;
 104 }
 105
 106 int CanMessage_c::get_format()
 107 {
 108     return format;
 109 }
 110
 111 uint8_t CanMessage_c::get_data()
 112 {
 113     return data;
 114 }
 115 uint8_t CanMessage_c::get_lenght()
 116 {
 117     return lenght;
 118 }
 119
 120 void CanMessage_c::set_id(uint32_t new_id)
 121 {
 122     switch(format):
 123         case SIMPLE:
 124             id = new_id & CAN_SFF_MASK;
 125         case EXTENDED:
 126             id = new_id & CAN_EFF_MASK;
 127         default:
 128             ERROR(interface, "ERROR: Can set id, not a compatible format or format not set prior to set id.");
 129 }
 130
 131 void CanMessage_c::set_format(CanMessageFormat new_format)
 132 {
 133     if(new_format == SIMPLE || new_format == EXTENDED)
 134         format = new_format;
 135     else
 136         ERROR(interface, "ERROR: Can set format, wrong format chosen");
 137 }
 138
 139 void CanMessage_c::set_data(uint8_t new_data)
 140 {
 141     data = new_data;
 142 }
 143
 144 void CanMessage_c::set_lenght(uint8_t new_length)
 145 {
 146     lenght = new_lenght;
 147 }
 148
 149 /*
 150  * This is the prefered way to initialize a CanMessage object
 151  * from a read canfd_frame message.
 152  *
 153  * params: canfd_frame pointer
 154  */
 155 void CanMessage_c::convert_canfd_frame_to_CanMessage(canfd_frame *frame)
 156 {
 157
 158         lenght = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
 159
 160         switch (canfd_frame->can_id):
 161         case (canfd_frame->can_id & CAN_ERR_FLAG):
 162                     id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 163             break;
 164         case (canfd_frame->can_id & CAN_EFF_FLAG):
 165                     id = canfd_frame->can_id & CAN_EFF_MASK;
 166                 format = EXTENDED;
 167                         break;
 168                 default:
 169                         format = STANDARD;
 170                         id = canfd_frame->can_id & CAN_SFF_MASK;
 171                         break;
 172
 173         if (sizeof(canfd_frame->data) <= sizeof(data))
 174         {
 175                 for (i = 0; i < lenght; i++)
 176                         can_message->data.bytes[i] = canfd_frame->data[i];
 177                 return 0;
 178         } else if (sizeof(canfd_frame->data) >= CAN_MAX_DLEN)
 179                 ERROR(interface, "CanMessage_c: canfd_frame data too long to be stored into CanMessage object");
 180 }