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_;
  59 }
  60 uint8_t can_message_t::get_length() const
  61 {
  62         return length_;
  63 }
  64
  65 void can_message_t::set_max_data_length(const struct canfd_frame& frame)
  66 {
  67         maxdlen_ = 0;
  68
  69         switch(sizeof(frame))
  70         {
  71                 case CANFD_MTU:
  72                         DEBUG(binder_interface, "convert_from_canfd_frame: Got an CAN FD frame with length %d and flags %d", frame.len, frame.flags);
  73                         maxdlen_ = CANFD_MAX_DLEN;
  74                         break;
  75                 case CAN_MTU:
  76                         DEBUG(binder_interface, "convert_from_canfd_frame: Got a legacy CAN frame with length %d", frame.len);
  77                         maxdlen_ = CAN_MAX_DLEN;
  78                         break;
  79                 default:
  80                         ERROR(binder_interface, "convert_from_canfd_frame: unsupported CAN frame");
  81                         break;
  82         }
  83 }
  84
  85 bool can_message_t::is_correct_to_send()
  86 {
  87         if (id_ != 0 && length_ != 0 && format_ != CanMessageFormat::ERROR)
  88         {
  89                 int i;
  90                 for(i=0;i<CAN_MESSAGE_SIZE;i++)
  91                         if(data_[i] != 0)
  92                                 return true;
  93         }
  94         return false;
  95 }
  96
  97 void can_message_t::set_id_and_format(const uint32_t new_id)
  98 {
  99         set_format(new_id);
 100         switch(format_)
 101         {
 102                 case CanMessageFormat::STANDARD:
 103                         id_ = new_id & CAN_SFF_MASK;
 104                         break;
 105                 case CanMessageFormat::EXTENDED:
 106                         id_ = new_id & CAN_EFF_MASK;
 107                         break;
 108                 case CanMessageFormat::ERROR:
 109                         id_ = new_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 110                         break;
 111                 default:
 112                         ERROR(binder_interface, "ERROR: Can set id, not a compatible format or format not set prior to set id.");
 113                         break;
 114         }
 115 }
 116
 117 void can_message_t::set_format(const CanMessageFormat new_format)
 118 {
 119         if(new_format == CanMessageFormat::STANDARD || new_format == CanMessageFormat::EXTENDED || new_format == CanMessageFormat::ERROR)
 120                 format_ = new_format;
 121         else
 122                 ERROR(binder_interface, "ERROR: Can set format, wrong format chosen");
 123 }
 124
 125 void can_message_t::set_format(const uint32_t can_id)
 126 {
 127         if (can_id & CAN_ERR_FLAG)
 128                 format_ = CanMessageFormat::ERROR;
 129         else if (can_id & CAN_EFF_FLAG)
 130                 format_ = CanMessageFormat::EXTENDED;
 131         else
 132                 format_ = CanMessageFormat::STANDARD;
 133 }
 134
 135 void can_message_t::set_flags(const uint8_t flags)
 136 {
 137         flags_ = flags & 0xF;
 138 }
 139
 140 void can_message_t::set_length(const uint8_t new_length)
 141 {
 142         if(rtr_flag_)
 143                 length_ = new_length & 0xF;
 144         else
 145         {
 146                 length_ = (new_length > maxdlen_) ? maxdlen_ : new_length;
 147         }
 148 }
 149
 150 void can_message_t::set_data(const __u8 new_data[], size_t dlen)
 151 {
 152         if (sizeof(dlen)/sizeof(__u8) > maxdlen_)
 153                 ERROR(binder_interface, "Can set data, too big ! It is a CAN frame ?");
 154         else
 155         {
 156                 int i;
 157                 /* Limiting to 8 bytes message for now on 64 bytes from fd frames*/
 158                 for(i=0;i<CAN_MESSAGE_SIZE;i++)
 159                 {
 160                         data_[i] = new_data[i];
 161                 }
 162         }
 163 }
 164
 165 void can_message_t::convert_from_canfd_frame(const struct canfd_frame& frame)
 166 {
 167         set_max_data_length(frame);
 168         set_length(frame.len);
 169         set_id_and_format(frame.can_id);
 170
 171         /* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
 172         if (frame.can_id & CAN_RTR_FLAG)
 173         {
 174                 rtr_flag_ = true;
 175                 if(frame.len && frame.len <= CAN_MAX_DLC)
 176                         set_length(frame.len);
 177                 return;
 178         }
 179
 180         /* Flags field only present for CAN FD frames*/
 181         if(maxdlen_ == CANFD_MAX_DLEN)
 182                 set_flags(frame.flags);
 183
 184         size_t dlen = sizeof(frame.data);
 185         memset(data_, 0, dlen);
 186         set_data(frame.data, dlen);
 187
 188         DEBUG(binder_interface, "convert_from_canfd_frame: Found id: %d, format: %d, length: %d, data %d%d%d%d%d%d%d%d", id_, format_, length_,
 189                                                         data_[0], data_[1], data_[2], data_[3], data_[4], data_[5], data_[6], data_[7]);
 190 }
 191
 192 canfd_frame can_message_t::convert_to_canfd_frame()
 193 {
 194         canfd_frame frame;
 195
 196         if(is_correct_to_send())
 197         {
 198                 frame.can_id = get_id();
 199                 frame.len = get_length();
 200                 ::memcpy(frame.data, get_data(), length_);
 201         }
 202         else
 203                 ERROR(binder_interface, "can_message_t not correctly initialized to be sent");
 204
 205         return frame;
 206 }
 207