Line data Source code
1 : /* 2 : * Copyright (C) 2020 Siddharth B Purohit 3 : * 4 : * This file is free software: you can redistribute it and/or modify it 5 : * under the terms of the GNU General Public License as published by the 6 : * Free Software Foundation, either version 3 of the License, or 7 : * (at your option) any later version. 8 : * 9 : * This file is distributed in the hope that it will be useful, but 10 : * WITHOUT ANY WARRANTY; without even the implied warranty of 11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 : * See the GNU General Public License for more details. 13 : * 14 : * You should have received a copy of the GNU General Public License along 15 : * with this program. If not, see <http://www.gnu.org/licenses/>. 16 : */ 17 : 18 : #include "CANIface.h" 19 : #include "system.h" 20 : 21 0 : bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const 22 : { 23 0 : const uint32_t clean_id = id & MaskExtID; 24 0 : const uint32_t rhs_clean_id = rhs.id & MaskExtID; 25 : 26 : /* 27 : * STD vs EXT - if 11 most significant bits are the same, EXT loses. 28 : */ 29 0 : const bool ext = id & FlagEFF; 30 0 : const bool rhs_ext = rhs.id & FlagEFF; 31 0 : if (ext != rhs_ext) { 32 0 : const uint32_t arb11 = ext ? (clean_id >> 18) : clean_id; 33 0 : const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id; 34 0 : if (arb11 != rhs_arb11) { 35 0 : return arb11 < rhs_arb11; 36 : } else { 37 0 : return rhs_ext; 38 : } 39 : } 40 : 41 : /* 42 : * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses. 43 : */ 44 0 : const bool rtr = id & FlagRTR; 45 0 : const bool rhs_rtr = rhs.id & FlagRTR; 46 0 : if (clean_id == rhs_clean_id && rtr != rhs_rtr) { 47 0 : return rhs_rtr; 48 : } 49 : 50 : /* 51 : * Plain ID arbitration - greater value loses. 52 : */ 53 0 : return clean_id < rhs_clean_id; 54 : } 55 : 56 : /* 57 : parent class receive handling for forwarding received frames to registered callbacks 58 : */ 59 152 : int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags) 60 : { 61 152 : if ((out_flags & IsForwardedFrame) != 0) { 62 : // this frame was forwarded from another interface to this one, so we should not forward it back 63 0 : return 1; 64 : } 65 : #ifndef HAL_BOOTLOADER_BUILD 66 152 : WITH_SEMAPHORE(callbacks.sem); 67 : #endif 68 608 : for (auto &cb : callbacks.cb) { 69 456 : if (cb != nullptr) { 70 : // forward the frame to the registered callbacks and mark it as forwarded 71 0 : cb(get_iface_num(), out_frame, out_flags | IsForwardedFrame); 72 : } 73 : } 74 152 : return 1; 75 152 : } 76 : 77 : /* 78 : parent class send handling for Forwarded frames 79 : */ 80 18063 : int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) 81 : { 82 : #ifndef HAL_BOOTLOADER_BUILD 83 18063 : WITH_SEMAPHORE(callbacks.sem); 84 : #endif 85 18063 : bool added_to_rx_queue = false; 86 72252 : for (auto &cb : callbacks.cb) { 87 54189 : if (cb == nullptr) { 88 54189 : continue; 89 : } 90 0 : if ((flags & IsForwardedFrame) == 0) { 91 : // call the frame callback from send only if the frame originated from this node 92 0 : cb(get_iface_num(), frame, flags); 93 0 : } else if (!added_to_rx_queue) { 94 : // the frame was forwarded from another interface, so add it to the receive queue 95 0 : CanRxItem rx_item; 96 0 : rx_item.frame = frame; 97 0 : rx_item.timestamp_us = AP_HAL::micros64(); 98 0 : rx_item.flags = AP_HAL::CANIface::IsForwardedFrame; 99 0 : add_to_rx_queue(rx_item); 100 0 : added_to_rx_queue = true; 101 : } 102 : } 103 36126 : return 1; 104 18063 : } 105 : 106 : /* 107 : register a callback for for sending CAN_FRAME messages. 108 : On success the returned callback_id can be used to unregister the callback 109 : */ 110 0 : bool AP_HAL::CANIface::register_frame_callback(FrameCb cb, uint8_t &callback_id) 111 : { 112 : #ifndef HAL_BOOTLOADER_BUILD 113 0 : WITH_SEMAPHORE(callbacks.sem); 114 : #endif 115 0 : for (uint8_t i=0; i<ARRAY_SIZE(callbacks.cb); i++) { 116 0 : if (callbacks.cb[i] == nullptr) { 117 0 : callbacks.cb[i] = cb; 118 0 : callback_id = i+1; 119 0 : return true; 120 : } 121 : } 122 0 : return false; 123 0 : } 124 : 125 : /* 126 : unregister a callback for for sending CAN_FRAME messages 127 : */ 128 0 : void AP_HAL::CANIface::unregister_frame_callback(uint8_t callback_id) 129 : { 130 : #ifndef HAL_BOOTLOADER_BUILD 131 0 : WITH_SEMAPHORE(callbacks.sem); 132 : #endif 133 0 : const uint8_t idx = callback_id - 1; 134 0 : if (idx < ARRAY_SIZE(callbacks.cb)) { 135 0 : callbacks.cb[idx] = nullptr; 136 : } 137 0 : } 138 : 139 152 : AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) : 140 152 : id(can_id), 141 152 : canfd(canfd_frame) 142 : { 143 152 : const uint8_t data_len_max = canfd_frame ? MaxDataLen : NonFDCANMaxDataLen; 144 152 : if ((can_data == nullptr) || (data_len == 0) || (data_len > data_len_max)) { 145 0 : dlc = 0; 146 0 : memset(data, 0, MaxDataLen); 147 0 : return; 148 : } 149 152 : memcpy(this->data, can_data, data_len); 150 152 : memset(&this->data[data_len], 0, MaxDataLen-data_len); 151 152 : if (data_len <= NonFDCANMaxDataLen) { 152 152 : dlc = data_len; 153 : } else { 154 : /* 155 : Data Length Code 9 10 11 12 13 14 15 156 : Number of data bytes 12 16 20 24 32 48 64 157 : */ 158 0 : if (data_len <= 12) { 159 0 : dlc = 9; 160 0 : } else if (data_len <= 16) { 161 0 : dlc = 10; 162 0 : } else if (data_len <= 20) { 163 0 : dlc = 11; 164 0 : } else if (data_len <= 24) { 165 0 : dlc = 12; 166 0 : } else if (data_len <= 32) { 167 0 : dlc = 13; 168 0 : } else if (data_len <= 48) { 169 0 : dlc = 14; 170 0 : } else if (data_len <= 64) { 171 0 : dlc = 15; 172 : } 173 : } 174 : } 175 : 176 18064 : uint8_t AP_HAL::CANFrame::dataLengthToDlc(uint8_t data_length) 177 : { 178 18064 : if (data_length <= 8) { 179 18064 : return data_length; 180 0 : } else if (data_length <= 12) { 181 0 : return 9; 182 0 : } else if (data_length <= 16) { 183 0 : return 10; 184 0 : } else if (data_length <= 20) { 185 0 : return 11; 186 0 : } else if (data_length <= 24) { 187 0 : return 12; 188 0 : } else if (data_length <= 32) { 189 0 : return 13; 190 0 : } else if (data_length <= 48) { 191 0 : return 14; 192 : } 193 0 : return 15; 194 : } 195 : 196 : 197 18215 : uint8_t AP_HAL::CANFrame::dlcToDataLength(uint8_t dlc) 198 : { 199 : /* 200 : Data Length Code 9 10 11 12 13 14 15 201 : Number of data bytes 12 16 20 24 32 48 64 202 : */ 203 18215 : if (dlc <= 8) { 204 18215 : return dlc; 205 0 : } else if (dlc == 9) { 206 0 : return 12; 207 0 : } else if (dlc == 10) { 208 0 : return 16; 209 0 : } else if (dlc == 11) { 210 0 : return 20; 211 0 : } else if (dlc == 12) { 212 0 : return 24; 213 0 : } else if (dlc == 13) { 214 0 : return 32; 215 0 : } else if (dlc == 14) { 216 0 : return 48; 217 : } 218 0 : return 64; 219 : }
