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          * @brief Save at an address a name
 150          *
 151          * @param addr The adress where you want to save name
 152          * @param name The name of the ECU that is in the address
 153          * @return int 0 if save is ok
 154          */
 155         int socketcan_j1939_addressclaiming_t::save_addr_name(uint8_t addr, name_t name)
 156         {
 157                 if(addr < J1939_IDLE_ADDR)
 158                 {
 159                         if(table_j1939_address_[addr].first < name)
 160                         {
 161                                 table_j1939_address_[addr].first = name;
 162                                 AFB_DEBUG("[socketcan-j1939-addressclaiming][save_addr_name] NAME : %x <--> ADDR : %d",(unsigned int)name, addr);
 163                         }
 164                         else if(table_j1939_address_[addr].first == name)
 165                         {
 166                                 AFB_WARNING("Name %x has already adress %d",(unsigned int)name, addr);
 167                         }
 168                 }
 169                 else
 170                 {
 171                         AFB_ERROR("Invalid address to save");
 172                         return -1;
 173                 }
 174                 return 0;
 175         }
 176
 177         /**
 178          * @brief Choose new address for the ECU check in the table the best place to claim
 179          *
 180          * @return uint8_t The new address choosen
 181          */
 182         uint8_t socketcan_j1939_addressclaiming_t::choose_new_address()
 183         {
 184                 for (int i = 0; i < J1939_IDLE_ADDR; i++)
 185                 {
 186                         if(table_j1939_address_[i].second && i!=current_address_)
 187                         {
 188                                 if(     table_j1939_address_[i].first >= htole64(J1939_NAME_ECU) || table_j1939_address_[i].first == 0)
 189                                         return (uint8_t) i;
 190                         }
 191                 }
 192                 return J1939_IDLE_ADDR;
 193         }
 194
 195         /**
 196          * @brief The function that destoy the timer
 197          *
 198          * @param timer_context The timer context to destroy
 199          * @return int
 200          */
 201         int socketcan_j1939_addressclaiming_t::free_timer_handle(void *timer_context)
 202         {
 203                 socketcan_j1939_addressclaiming_t *addressclaiming_socket = (socketcan_j1939_addressclaiming_t*) timer_context;
 204                 addressclaiming_socket->timer_handle_ = nullptr;
 205                 return 0;
 206         }
 207
 208         /**
 209          * @brief The function is call when at the end of the timer, the socket has don't receive
 210          *
 211          * @param timerhandle The timerhandle of the timer
 212          * @return int 1 it's all it's ok
 213          */
 214         int socketcan_j1939_addressclaiming_t::no_response_claiming(TimerHandleT *timerhandle)
 215         {
 216                 socketcan_j1939_addressclaiming_t *addressclaiming_socket = (socketcan_j1939_addressclaiming_t*) timerhandle->context;
 217                 // If the cache is cleared :
 218                 addressclaiming_socket->change_state(claiming_state::OPERATIONAL);
 219                 addressclaiming_socket->save_addr_name(addressclaiming_socket->current_address_, htole64(J1939_NAME_ECU));
 220                 AFB_DEBUG("Get address %d for this ecu", addressclaiming_socket->current_address_);
 221                 /*Else :
 222
 223                 uint8_t data[3]= { 0, 0, 0, };
 224                 std::vector<uint8_t> data_v(data, data+3);
 225                 int res = addressclaiming_socket->write_j1939_message(J1939_PGN_REQUEST, data_v, 3);
 226                 if(res < 0)
 227                 {
 228                         if(res == -99)
 229                         {
 230                                 addressclaiming_socket->save_addr_name(addressclaiming_socket->current_address_, htole64(1));
 231                                 AFB_DEBUG("Address busy but no claming request from other ECU");
 232                                 addressclaiming_socket->claim_address(false, true);
 233                         }
 234                         else
 235                         {
 236                                 AFB_ERROR("ERROR");
 237                         }
 238                 }
 239                 else
 240                 {
 241                         addressclaiming_socket->change_state(claiming_state::OPERATIONAL);
 242                         addressclaiming_socket->save_addr_name(addressclaiming_socket->current_address_, htole64(J1939_NAME_ECU));
 243                         AFB_DEBUG("Get address %d for this ecu", addressclaiming_socket->current_address_);
 244                 }*/
 245
 246                 return 1;
 247         }
 248
 249         /**
 250          * @brief Launch timer when an address is claimed
 251          *
 252          */
 253         void socketcan_j1939_addressclaiming_t::launch_timer()
 254         {
 255                 timer_handle_ = (TimerHandleT*) malloc(sizeof(TimerHandleT));
 256                 timer_handle_->uid = "claiming_wait";
 257                 timer_handle_->delay = 250;
 258                 timer_handle_->count = 1;
 259                 timer_handle_->freeCB = free_timer_handle;
 260                 TimerEvtStart(afbBindingV3root, timer_handle_, no_response_claiming, (void *) this);
 261         }
 262
 263
 264         /**
 265          * @brief Allows to claim a new address
 266          *
 267          * @param first_claim If true, the socket is open
 268          * @param new_address If true, claim a new address, else only resend a claim with same address
 269          * @return int -1 if fail
 270          */
 271         int socketcan_j1939_addressclaiming_t::claim_address(bool first_claim, bool new_address)
 272         {
 273                 if(new_address)
 274                 {
 275                         AFB_DEBUG("New address");
 276                         current_address_ = choose_new_address();
 277                         change_state(claiming_state::CLAIMING);
 278                         launch_timer();
 279                 }
 280
 281                 if(current_address_ == J1939_IDLE_ADDR)
 282                 {
 283                         AFB_ERROR("No address left");
 284                         return -1;
 285                 }
 286
 287                 if(first_claim)
 288                 {
 289                         int ret = socketcan_j1939_t::open(device_name_, htole64(J1939_NAME_ECU), J1939_NO_PGN, current_address_);
 290
 291                         if(ret < 0)
 292                         {
 293                                 AFB_ERROR("Error open socket address claiming");
 294                                 return -1;
 295                         }
 296
 297                         AFB_DEBUG("[socketcan-j1939-addressclaiming][claim_address] Success open socket address claiming");
 298                         add_filter(J1939_NO_NAME, J1939_PGN_ADDRESS_CLAIMED, J1939_NO_ADDR, J1939_NO_NAME, J1939_PGN_PDU1_MAX, J1939_NO_ADDR);
 299                         define_opt();
 300                 }
 301                 else
 302                 {
 303                         tx_address_.can_addr.j1939.addr = current_address_;
 304                         if(bind((struct sockaddr *)&tx_address_, sizeof(tx_address_)) < 0)
 305                         {
 306                                 AFB_ERROR("rebind() fail");
 307                                 return -1;
 308                         }
 309                 }
 310
 311                 uint64_t name = htole64(J1939_NAME_ECU);
 312                 uint8_t dat[8] = {0};
 313                 memcpy(dat, &name, 8);
 314                 struct sockaddr_can sockname;
 315                 memset(&sockname, 0, sizeof(sockname));
 316                 sockname.can_family = AF_CAN;
 317                 sockname.can_addr.j1939.pgn = J1939_PGN_ADDRESS_CLAIMED;
 318                 sockname.can_addr.j1939.addr = J1939_NO_ADDR;
 319                 socklen_t socklen = sizeof(sockname);
 320
 321                 ssize_t ret = sendto(socket_, &dat, sizeof(dat), 0, (const struct sockaddr *)&sockname, socklen);
 322
 323                 if(ret < 0 )
 324                 {
 325                         AFB_ERROR("Address claimed fail : %s", strerror(errno));
 326                         return -1;
 327                 }
 328
 329                 AFB_DEBUG("[socketcan-j1939-addressclaiming][claim_address] Send address claiming request");
 330
 331                 return (int)ret;
 332         }
 333
 334 /*      int socketcan_j1939_addressclaiming_t::pgn_request()
 335         {
 336                 static const uint8_t dat[3] = { 0, 0xee, 0, };
 337                 static struct sockaddr_can peername;
 338                 peername.can_family = AF_CAN;
 339                 peername.can_addr.j1939.pgn = J1939_PGN_REQUEST;
 340                 peername.can_addr.j1939.addr = J1939_NO_ADDR;
 341                 int ret = sendto(socket_, dat, sizeof(dat), 0, (const struct sockaddr *)&peername, sizeof(peername));
 342
 343                 if(ret < 0)
 344                         AFB_ERROR("Error pgn_request()");
 345
 346                 return ret;
 347         }
 348         */
 349
 350         /**
 351          * @brief Return the address associate to a name
 352          *
 353          * @param name The name you are looking for
 354          * @return uint8_t The address if it is present, else J1939_IDLE_ADDR
 355          */
 356         uint8_t socketcan_j1939_addressclaiming_t::get_addr_table(name_t name)
 357         {
 358                 for(int i = 0; i < J1939_IDLE_ADDR; i++)
 359                 {
 360                         if(table_j1939_address_[i].first == name)
 361                                 return (uint8_t) i;
 362                 }
 363                 return J1939_IDLE_ADDR;
 364         }
 365
 366         /**
 367          * @brief Allows to open a J1939 socket address claiming
 368          *
 369          * @param device_name The name of the device on which to open the socket
 370          * @param pgn NO_PGN
 371          * @return int Return 0 if ok else -1
 372          */
 373         int socketcan_j1939_addressclaiming_t::open(std::string device_name, pgn_t pgn)
 374         {
 375                 device_name_ = device_name;
 376                 initialize_table_j1939_address();
 377                 if(claim_address(true, true) < 0)
 378                 {
 379                         AFB_ERROR("Claim address failed");
 380                         return -1;
 381                 }
 382                 return 0;
 383         }
 384
 385         /**
 386          * @brief Allows to change the state of the socket address claiming
 387          * When the state change a mutex is lock
 388          *
 389          * @param new_state The new state
 390          */
 391         void socketcan_j1939_addressclaiming_t::change_state(claiming_state new_state)
 392         {
 393                 std::unique_lock<std::mutex> lock(socketcan_j1939_t::mutex_claiming_);
 394                 claiming_state_ = new_state;
 395                 socketcan_j1939_t::signal_address_claiming_.notify_one();
 396         }
 397
 398         /**
 399          * @brief Allows to get the states of the socket
 400          *
 401          * @return claiming_state The state of the socket
 402          */
 403         claiming_state socketcan_j1939_addressclaiming_t::get_state()
 404         {
 405                 return claiming_state_;
 406         }
 407
 408 }