low-can-binding/utils/socketcan-j1939/socketcan-j1939-addressclaiming.cpp

   1 /*
   2  * Copyright (C) 2018, 2019 "IoT.bzh"
   3  * Author "Arthur Guyader" <arthur.guyader@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 <unistd.h>
  19 #include "./socketcan-j1939-addressclaiming.hpp"
  20
  21 namespace utils
  22 {
  23         /**
  24          * @brief Construct a new socketcan j1939 addressclaiming t::socketcan j1939 addressclaiming t object
  25          *
  26          */
  27         socketcan_j1939_addressclaiming_t::socketcan_j1939_addressclaiming_t():
  28         socketcan_j1939_t(),
  29         table_j1939_address_{{std::make_pair(0,false)}},
  30         signal_stop_{false},
  31         claiming_state_{claiming_state::INITIAL}
  32         {}
  33
  34         /**
  35          * @brief Allows to read the claiming message and react to them
  36          *
  37          * @return std::shared_ptr<message_t> Return an INVALID j1939 message because no need of return (only signature of the function)
  38          */
  39         std::shared_ptr<message_t> socketcan_j1939_addressclaiming_t::read_message()
  40         {
  41                 AFB_DEBUG("[socketcan_j1939_addressclaiming_t] read_message");
  42                 std::shared_ptr<message_t> invalid_message = std::make_shared<j1939_message_t>();
  43                 std::shared_ptr<message_t> m;
  44                 j1939_message_t* jm;
  45
  46                 m = socketcan_j1939_t::read_message();
  47                 jm = static_cast<j1939_message_t*>(m.get());
  48
  49                 if(jm->get_pgn() == J1939_PGN_ADDRESS_CLAIMED)
  50                 {
  51                         if(jm->get_addr() >= J1939_IDLE_ADDR)
  52                         {
  53                                 AFB_DEBUG("Get invalid address claiming by name : %x",(unsigned int)jm->get_name());
  54                                 return invalid_message;
  55                         }
  56
  57                         if(jm->get_name() == htole64(J1939_NAME_ECU))
  58                         {
  59                                 AFB_DEBUG("Get own address claiming");
  60                                 return invalid_message;
  61                         }
  62
  63                         AFB_DEBUG("Get address claiming from %x",(unsigned int)jm->get_name());
  64
  65                         if(jm->get_addr() != current_address_)
  66                         {
  67                                 save_addr_name(jm->get_addr(),jm->get_name());
  68                                 return invalid_message;
  69                         }
  70
  71                         if(claiming_state_ == claiming_state::CLAIMING)
  72                         {
  73                                 if(jm->get_name() > htole64(J1939_NAME_ECU))
  74                                 {
  75                                         AFB_WARNING("Error from %x to use j1939 protocol",(unsigned int)htole64(J1939_NAME_ECU));
  76                                         return invalid_message;
  77                                 }
  78
  79                                 save_addr_name(jm->get_addr(),jm->get_name());
  80
  81
  82                                 if(timer_handle_->evtSource)
  83                                 {
  84                                                 TimerEvtStop(timer_handle_);
  85                                                 timer_handle_ = nullptr;
  86                                 }
  87
  88                                 if(claim_address(false, true) < 0)
  89                                 {
  90                                         AFB_ERROR("Claim address failed");
  91                                         change_state(claiming_state::INVALID);
  92                                         return invalid_message;
  93                                 }
  94                         }
  95                         else if(claiming_state_ == claiming_state::OPERATIONAL)
  96                         {
  97                                 AFB_DEBUG("Address colision");
  98                                 if(jm->get_name() > htole64(J1939_NAME_ECU))
  99                                 {
 100                                         if(claim_address(false,false) < 0)
 101                                         {
 102                                                 AFB_ERROR("Claim address failed");
 103                                                 change_state(claiming_state::INVALID);
 104                                                 return invalid_message;
 105                                         }
 106                                         return invalid_message;
 107                                 }
 108
 109                                 save_addr_name(jm->get_addr(),jm->get_name());
 110
 111                                 if(claim_address(false,true) < 0)
 112                                 {
 113                                                 AFB_ERROR("Claim address failed");
 114                                                 change_state(claiming_state::INVALID);
 115                                                 return invalid_message;
 116                                 }
 117                         }
 118                 }
 119                 return invalid_message;
 120         }
 121
 122         /**
 123          * @brief Initialize the table j1939 with the valid address posible
 124          *
 125          */
 126         void socketcan_j1939_addressclaiming_t::initialize_table_j1939_address()
 127         {
 128                 int start_addr = 128;
 129                 int end_addr = 247;
 130                 if(start_addr < 0)
 131                 {
 132                         AFB_ERROR("[socketcan-j1939-addressclaiming][initialize_table_j1939_address] Invalid start address");
 133                 }
 134                 else
 135                 {
 136                         if(end_addr >= J1939_IDLE_ADDR)
 137                         {
 138                                 AFB_ERROR("[socketcan-j1939-addressclaiming][initialize_table_j1939_address] Invalid end address");
 139                         }
 140                         else
 141                         {
 142                                 for (int i = start_addr; i <= end_addr; i++) {
 143                                         table_j1939_address_[i] = std::make_pair(0,true);
 144                                 }
 145                         }
 146                 }
 147         }
 148
 149         /**
 150          * @brief Save at an address a name
 151          *
 152          * @param addr The adress where you want to save name
 153          * @param name The name of the ECU that is in the address
 154          * @return int 0 if save is ok
 155          */
 156         int socketcan_j1939_addressclaiming_t::save_addr_name(uint8_t addr,name_t name)
 157         {
 158                 if(addr < J1939_IDLE_ADDR)
 159                 {
 160                         if(table_j1939_address_[addr].first < name)
 161                         {
 162                                 table_j1939_address_[addr].first = name;
 163                                 AFB_DEBUG("[socketcan-j1939-addressclaiming][save_addr_name] NAME : %x <--> ADDR : %d",(unsigned int)name,addr);
 164                         }
 165                         else if(table_j1939_address_[addr].first == name)
 166                         {
 167                                 AFB_WARNING("Name %x has already adress %d",(unsigned int)name,addr);
 168                         }
 169                 }
 170                 else
 171                 {
 172                         AFB_ERROR("Invalid address to save");
 173                         return -1;
 174                 }
 175                 return 0;
 176         }
 177
 178         /**
 179          * @brief Choose new address for the ECU check in the table the best place to claim
 180          *
 181          * @return uint8_t The new address choosen
 182          */
 183         uint8_t socketcan_j1939_addressclaiming_t::choose_new_address()
 184         {
 185                 for (int i = 0; i < J1939_IDLE_ADDR; i++)
 186                 {
 187                         if(table_j1939_address_[i].second && i!=current_address_)
 188                         {
 189                                 if(     table_j1939_address_[i].first >= htole64(J1939_NAME_ECU) || table_j1939_address_[i].first == 0)
 190                                 {
 191                                         return (uint8_t) i;
 192                                 }
 193                         }
 194                 }
 195                 return J1939_IDLE_ADDR;
 196         }
 197
 198         /**
 199          * @brief The function that destoy the timer
 200          *
 201          * @param timer_context The timer context to destroy
 202          * @return int
 203          */
 204         int socketcan_j1939_addressclaiming_t::free_timer_handle(void *timer_context)
 205         {
 206                 socketcan_j1939_addressclaiming_t *addressclaiming_socket = (socketcan_j1939_addressclaiming_t*) timer_context;
 207                 addressclaiming_socket->timer_handle_ = nullptr;
 208                 return 0;
 209         }
 210
 211         /**
 212          * @brief The function is call when at the end of the timer, the socket has don't receive
 213          *
 214          * @param timerhandle The timerhandle of the timer
 215          * @return int 1 it's all it's ok
 216          */
 217         int socketcan_j1939_addressclaiming_t::no_response_claiming(TimerHandleT *timerhandle)
 218         {
 219                 socketcan_j1939_addressclaiming_t *addressclaiming_socket = (socketcan_j1939_addressclaiming_t*) timerhandle->context;
 220                 // If the cache is cleared :
 221                 addressclaiming_socket->change_state(claiming_state::OPERATIONAL);
 222                 addressclaiming_socket->save_addr_name(addressclaiming_socket->current_address_,htole64(J1939_NAME_ECU));
 223                 AFB_DEBUG("Get address %d for this ecu", addressclaiming_socket->current_address_);
 224                 /*Else :
 225
 226                 uint8_t data[3]= { 0, 0, 0, };
 227                 std::vector<uint8_t> data_v(data,data+3);
 228                 int res = addressclaiming_socket->write_j1939_message(J1939_PGN_REQUEST,data_v,3);
 229                 if(res < 0)
 230                 {
 231                         if(res == -99)
 232                         {
 233                                 addressclaiming_socket->save_addr_name(addressclaiming_socket->current_address_,htole64(1));
 234                                 AFB_DEBUG("Address busy but no claming request from other ECU");
 235                                 addressclaiming_socket->claim_address(false,true);
 236                         }
 237                         else
 238                         {
 239                                 AFB_ERROR("ERROR");
 240                         }
 241                 }
 242                 else
 243                 {
 244                         addressclaiming_socket->change_state(claiming_state::OPERATIONAL);
 245                         addressclaiming_socket->save_addr_name(addressclaiming_socket->current_address_,htole64(J1939_NAME_ECU));
 246                         AFB_DEBUG("Get address %d for this ecu", addressclaiming_socket->current_address_);
 247                 }*/
 248
 249                 return 1;
 250         }
 251
 252         /**
 253          * @brief Launch timer when an address is claimed
 254          *
 255          */
 256         void socketcan_j1939_addressclaiming_t::launch_timer()
 257         {
 258                 timer_handle_ = (TimerHandleT*) malloc(sizeof(TimerHandleT));
 259                 timer_handle_->uid = "claiming_wait";
 260                 timer_handle_->delay = 250;
 261                 timer_handle_->count = 1;
 262                 timer_handle_->freeCB = free_timer_handle;
 263                 TimerEvtStart(afbBindingV3root, timer_handle_, no_response_claiming, (void *) this);
 264         }
 265
 266
 267         /**
 268          * @brief Allows to claim a new address
 269          *
 270          * @param first_claim If true, the socket is open
 271          * @param new_address If true, claim a new address, else only resend a claim with same address
 272          * @return int -1 if fail
 273          */
 274         int socketcan_j1939_addressclaiming_t::claim_address(bool first_claim,bool new_address)
 275         {
 276                 if(new_address)
 277                 {
 278                         AFB_DEBUG("New address");
 279                         current_address_ = choose_new_address();
 280                         change_state(claiming_state::CLAIMING);
 281                         launch_timer();
 282                 }
 283
 284                 if(current_address_ == J1939_IDLE_ADDR)
 285                 {
 286                         AFB_ERROR("No address left");
 287                         return -1;
 288                 }
 289
 290                 if(first_claim)
 291                 {
 292                         int ret = socketcan_j1939_t::open(device_name_,htole64(J1939_NAME_ECU),J1939_NO_PGN,current_address_);
 293
 294                         if(ret < 0)
 295                         {
 296                                 AFB_ERROR("Error open socket address claiming");
 297                                 return -1;
 298                         }
 299
 300                         AFB_DEBUG("[socketcan-j1939-addressclaiming][claim_address] Success open socket address claiming");
 301                         add_filter(J1939_NO_NAME,J1939_PGN_ADDRESS_CLAIMED,J1939_NO_ADDR,J1939_NO_NAME,J1939_PGN_PDU1_MAX,J1939_NO_ADDR);
 302                         define_opt();
 303                 }
 304                 else
 305                 {
 306                         tx_address_.can_addr.j1939.addr = current_address_;
 307                         if(bind((struct sockaddr *)&tx_address_, sizeof(tx_address_)) < 0)
 308                         {
 309                                 AFB_ERROR("rebind() fail");
 310                                 return -1;
 311                         }
 312                 }
 313
 314                 uint64_t name = htole64(J1939_NAME_ECU);
 315                 uint8_t dat[8] = {0};
 316                 memcpy(dat, &name, 8);
 317                 struct sockaddr_can sockname;
 318                 memset(&sockname, 0, sizeof(sockname));
 319                 sockname.can_family = AF_CAN;
 320                 sockname.can_addr.j1939.pgn = J1939_PGN_ADDRESS_CLAIMED;
 321                 sockname.can_addr.j1939.addr = J1939_NO_ADDR;
 322                 socklen_t socklen = sizeof(sockname);
 323
 324                 ssize_t ret = sendto(socket_, &dat, sizeof(dat), 0, (const struct sockaddr *)&sockname, socklen);
 325
 326                 if(ret < 0 )
 327                 {
 328                         AFB_ERROR("Address claimed fail : %s", strerror(errno));
 329                         return -1;
 330                 }
 331
 332                 AFB_DEBUG("[socketcan-j1939-addressclaiming][claim_address] Send address claiming request");
 333
 334                 return (int)ret;
 335         }
 336
 337 /*      int socketcan_j1939_addressclaiming_t::pgn_request()
 338         {
 339                 static const uint8_t dat[3] = { 0, 0xee, 0, };
 340                 static struct sockaddr_can peername;
 341                 peername.can_family = AF_CAN;
 342                 peername.can_addr.j1939.pgn = J1939_PGN_REQUEST;
 343                 peername.can_addr.j1939.addr = J1939_NO_ADDR;
 344                 int ret = sendto(socket_, dat, sizeof(dat), 0, (const struct sockaddr *)&peername, sizeof(peername));
 345
 346                 if(ret < 0)
 347                 {
 348                         AFB_ERROR("Error pgn_request()");
 349                 }
 350                 return ret;
 351         }
 352         */
 353
 354         /**
 355          * @brief Return the address associate to a name
 356          *
 357          * @param name The name you are looking for
 358          * @return uint8_t The address if it is present, else J1939_IDLE_ADDR
 359          */
 360         uint8_t socketcan_j1939_addressclaiming_t::get_addr_table(name_t name)
 361         {
 362                 for(int i = 0; i < J1939_IDLE_ADDR; i++)
 363                 {
 364                         if(table_j1939_address_[i].first == name)
 365                         {
 366                                 return (uint8_t) i;
 367                         }
 368                 }
 369                 return J1939_IDLE_ADDR;
 370         }
 371
 372         /**
 373          * @brief Allows to open a J1939 socket address claiming
 374          *
 375          * @param device_name The name of the device on which to open the socket
 376          * @param pgn NO_PGN
 377          * @return int Return 0 if ok else -1
 378          */
 379         int socketcan_j1939_addressclaiming_t::open(std::string device_name, pgn_t pgn)
 380         {
 381                 device_name_ = device_name;
 382                 initialize_table_j1939_address();
 383                 if(claim_address(true,true) < 0)
 384                 {
 385                         AFB_ERROR("Claim address failed");
 386                         return -1;
 387                 }
 388                 return 0;
 389         }
 390
 391         /**
 392          * @brief Allows to change the state of the socket address claiming
 393          * When the state change a mutex is lock
 394          *
 395          * @param new_state The new state
 396          */
 397         void socketcan_j1939_addressclaiming_t::change_state(claiming_state new_state)
 398         {
 399                 std::unique_lock<std::mutex> lock(socketcan_j1939_t::mutex_claiming_);
 400                 claiming_state_ = new_state;
 401                 socketcan_j1939_t::signal_address_claiming_.notify_one();
 402         }
 403
 404         /**
 405          * @brief Allows to get the states of the socket
 406          *
 407          * @return claiming_state The state of the socket
 408          */
 409         claiming_state socketcan_j1939_addressclaiming_t::get_state()
 410         {
 411                 return claiming_state_;
 412         }
 413
 414 }