src/can-message.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 #include "can-message.hpp"
  19
  20 #include <cstring>
  21
  22 #include "low-can-binding.hpp"
  23
  24 /********************************************************************************
  25 *
  26 *               CanMessage method implementation
  27 *
  28 *********************************************************************************/
  29
  30 can_message_t::can_message_t()
  31         : id_{0}, rtr_flag_{false}, length_{0}, flags_{0}, format_{CanMessageFormat::ERROR}
  32 {}
  33
  34 uint32_t can_message_t::get_id() const
  35 {
  36         return id_;
  37 }
  38
  39 bool can_message_t::get_rtr_flag_() const
  40 {
  41         return rtr_flag_;
  42 }
  43
  44 int can_message_t::get_format() const
  45 {
  46         if (format_ != CanMessageFormat::STANDARD || format_ != CanMessageFormat::EXTENDED)
  47                 return CanMessageFormat::ERROR;
  48         return format_;
  49 }
  50
  51 uint8_t can_message_t::get_flags() const
  52 {
  53         return flags_;
  54 }
  55
  56 const uint8_t* can_message_t::get_data() const
  57 {
  58         return data_.data();
  59 }
  60
  61 uint8_t can_message_t::get_length() const
  62 {
  63         return length_;
  64 }
  65
  66 void can_message_t::set_max_data_length(size_t nbytes)
  67 {
  68         maxdlen_ = 0;
  69
  70         switch(nbytes)
  71         {
  72                 case CANFD_MTU:
  73                         DEBUG(binder_interface, "set_max_data_length: Got an CAN FD frame");
  74                         maxdlen_ = CANFD_MAX_DLEN;
  75                         break;
  76                 case CAN_MTU:
  77                         DEBUG(binder_interface, "set_max_data_length: Got a legacy CAN frame");
  78                         maxdlen_ = CAN_MAX_DLEN;
  79                         break;
  80                 default:
  81                         ERROR(binder_interface, "set_max_data_length: unsupported CAN frame");
  82                         break;
  83         }
  84 }
  85
  86 bool can_message_t::is_correct_to_send()
  87 {
  88         if (id_ != 0 && length_ != 0 && format_ != CanMessageFormat::ERROR)
  89         {
  90                 int i;
  91                 for(i=0;i<CAN_MESSAGE_SIZE;i++)
  92                         if(data_[i] != 0)
  93                                 return true;
  94         }
  95         return false;
  96 }
  97
  98 void can_message_t::set_id_and_format(const uint32_t new_id)
  99 {
 100         set_format(new_id);
 101         switch(format_)
 102         {
 103                 case CanMessageFormat::STANDARD:
 104                         id_ = new_id & CAN_SFF_MASK;
 105                         break;
 106                 case CanMessageFormat::EXTENDED:
 107                         id_ = new_id & CAN_EFF_MASK;
 108                         break;
 109                 case CanMessageFormat::ERROR:
 110                         id_ = new_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 111                         break;
 112                 default:
 113                         ERROR(binder_interface, "ERROR: Can set id, not a compatible format or format not set prior to set id.");
 114                         break;
 115         }
 116 }
 117
 118 void can_message_t::set_format(const CanMessageFormat new_format)
 119 {
 120         if(new_format == CanMessageFormat::STANDARD || new_format == CanMessageFormat::EXTENDED || new_format == CanMessageFormat::ERROR)
 121                 format_ = new_format;
 122         else
 123                 ERROR(binder_interface, "ERROR: Can set format, wrong format chosen");
 124 }
 125
 126 void can_message_t::set_format(const uint32_t can_id)
 127 {
 128         if (can_id & CAN_ERR_FLAG)
 129                 format_ = CanMessageFormat::ERROR;
 130         else if (can_id & CAN_EFF_FLAG)
 131                 format_ = CanMessageFormat::EXTENDED;
 132         else
 133                 format_ = CanMessageFormat::STANDARD;
 134 }
 135
 136 void can_message_t::set_flags(const uint8_t flags)
 137 {
 138         flags_ = flags & 0xF;
 139 }
 140
 141 void can_message_t::set_length(const uint8_t new_length)
 142 {
 143         if(rtr_flag_)
 144                 length_ = new_length & 0xF;
 145         else
 146         {
 147                 length_ = (new_length > maxdlen_) ? maxdlen_ : new_length;
 148         }
 149 }
 150
 151 void can_message_t::set_data(const __u8* new_data)
 152 {
 153                 int i;
 154
 155                 /* maxdlen_ is now set at CAN_MAX_DLEN or CANFD_MAX_DLEN, respectively 8 and 64 bytes*/
 156                 for(i=0;i<maxdlen_;i++)
 157                 {
 158                         data_.push_back(new_data[i]);
 159                 }
 160 }
 161
 162 void can_message_t::convert_from_canfd_frame(const std::pair<struct canfd_frame&, size_t>args)
 163 {
 164         // May be it's overkill to assign member of the pair... May be it will change...
 165         struct canfd_frame frame = args.first;
 166         size_t nbytes = args.second;
 167         set_max_data_length(nbytes);
 168         set_length(frame.len);
 169         set_id_and_format(frame.can_id);
 170
 171         /* Overwrite lenght_ if RTR flags is detected.
 172          * standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
 173         if (frame.can_id & CAN_RTR_FLAG)
 174         {
 175                 rtr_flag_ = true;
 176                 if(frame.len && frame.len <= CAN_MAX_DLC)
 177                         set_length(frame.len);
 178                 return;
 179         }
 180
 181         /* Flags field only present for CAN FD frames*/
 182         if(maxdlen_ == CANFD_MAX_DLEN)
 183                 set_flags(frame.flags);
 184
 185         if ( data_.capacity() < maxdlen_)
 186                 data_.reserve(maxdlen_);
 187         set_data(frame.data);
 188
 189         DEBUG(binder_interface, "convert_from_canfd_frame: Found id: %X, format: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", id_, format_, length_,
 190                                                         data_[0], data_[1], data_[2], data_[3], data_[4], data_[5], data_[6], data_[7]);
 191 }
 192
 193 canfd_frame can_message_t::convert_to_canfd_frame()
 194 {
 195         canfd_frame frame;
 196
 197         if(is_correct_to_send())
 198         {
 199                 frame.can_id = get_id();
 200                 frame.len = get_length();
 201                 ::memcpy(frame.data, get_data(), length_);
 202         }
 203         else
 204                 ERROR(binder_interface, "can_message_t not correctly initialized to be sent");
 205
 206         return frame;
 207 }
 208