X-Git-Url: https://gerrit.automotivelinux.org/gerrit/gitweb?a=blobdiff_plain;f=src%2Fdiagnostic%2Fdiagnostic-manager.cpp;h=e3a78d0f69f431246b78ddb6ac0a3404f87806e7;hb=745ff334267ab92051bc8261a7daa0165c700fac;hp=69e2a7c45fbf99c46074a199d9fb0049da6575b4;hpb=d99b87ba4332fd0c1e51d2708e46c52bad713558;p=apps%2Fagl-service-can-low-level.git diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp index 69e2a7c4..e3a78d0f 100644 --- a/src/diagnostic/diagnostic-manager.cpp +++ b/src/diagnostic/diagnostic-manager.cpp @@ -25,6 +25,8 @@ #define MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ 10 #define MAX_SIMULTANEOUS_DIAG_REQUESTS 50 +// There are only 8 slots of in flight diagnostic requests +#define MAX_SIMULTANEOUS_IN_FLIGHT_REQUESTS 8 #define TIMERFD_ACCURACY 0 #define MICRO 1000000 @@ -81,7 +83,10 @@ void diagnostic_manager_t::reset() bool diagnostic_manager_t::shims_send(const uint32_t arbitration_id, const uint8_t* data, const uint8_t size) { std::shared_ptr can_bus_dev = can_bus_t::get_can_device(configuration_t::instance().get_diagnostic_manager().bus_); - return can_bus_dev->shims_send(arbitration_id, data, size); + if(can_bus_dev != nullptr) + return can_bus_dev->shims_send(arbitration_id, data, size); + ERROR(binder_interface, "shims_send: Can not retrieve diagnostic bus: %s", configuration_t::instance().get_diagnostic_manager().bus_.c_str()); + return false; } /// @brief The type signature for an optional logging function, if the user @@ -161,18 +166,17 @@ void diagnostic_manager_t::cancel_request(active_diagnostic_request_t* entry) /// @param[in] force - Force the cleaning or not ? void diagnostic_manager_t::cleanup_request(active_diagnostic_request_t* entry, bool force) { - if((force || (entry->get_in_flight() && entry->request_completed())) && entry != nullptr) + if((force || (entry != nullptr && entry->get_in_flight() && entry->request_completed()))) { entry->set_in_flight(false); char request_string[128] = {0}; diagnostic_request_to_string(&entry->get_handle()->request, request_string, sizeof(request_string)); - if(entry->get_recurring()) + if(force && entry->get_recurring()) { find_and_erase(entry, recurring_requests_); - if(force) - cancel_request(entry); + cancel_request(entry); DEBUG(binder_interface, "cleanup_request: Cancelling completed, recurring request: %s", request_string); } else @@ -268,13 +272,13 @@ bool diagnostic_manager_t::add_request(DiagnosticRequest* request, const std::st find_and_erase(entry, non_recurring_requests_); DEBUG(binder_interface, "Added one-time diagnostic request on bus %s: %s", - bus_, request_string); + bus_.c_str(), request_string); non_recurring_requests_.push_back(entry); } else { - WARNING(binder_interface, "There isn't enough request entry. Vector exhausted %d/%d", (int)non_recurring_requests_.size()); + WARNING(binder_interface, "There isn't enough request entry. Vector exhausted %d/%d", (int)non_recurring_requests_.size(), MAX_SIMULTANEOUS_DIAG_REQUESTS); non_recurring_requests_.resize(MAX_SIMULTANEOUS_DIAG_REQUESTS); added = false; } @@ -284,7 +288,7 @@ bool diagnostic_manager_t::add_request(DiagnosticRequest* request, const std::st bool diagnostic_manager_t::validate_optional_request_attributes(float frequencyHz) { if(frequencyHz > MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ) { - DEBUG(binder_interface, "Requested recurring diagnostic frequency %d is higher than maximum of %d", + DEBUG(binder_interface, "Requested recurring diagnostic frequency %lf is higher than maximum of %d", frequencyHz, MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ); return false; } @@ -365,17 +369,27 @@ bool diagnostic_manager_t::add_recurring_request(DiagnosticRequest* request, con wait_for_multiple_responses, decoder, callback, frequencyHz); entry->set_handle(shims_, request); - char request_string[128] = {0}; - diagnostic_request_to_string(&entry->get_handle()->request, request_string, - sizeof(request_string)); - - DEBUG(binder_interface, "add_recurring_request: Added recurring diagnostic request (freq: %f) on bus %s: %s", - frequencyHz, bus_.c_str(), request_string); + //start_diagnostic_request(&shims_, entry->get_handle()); + //char request_string[128] = {0}; + //diagnostic_request_to_string(&entry->get_handle()->request, request_string, + // sizeof(request_string)); uint64_t usec; - sd_event_now(afb_daemon_get_event_loop(binder_interface->daemon), CLOCK_MONOTONIC, &usec); + sd_event_now(afb_daemon_get_event_loop(binder_interface->daemon), CLOCK_BOOTTIME, &usec); + if(recurring_requests_.size() > 0) + { + DEBUG(binder_interface, "add_recurring_request: Added 100ms to usec to stagger sending requests"); + usec += 100000; + } + + DEBUG(binder_interface, "add_recurring_request: Added recurring diagnostic request (freq: %f) on bus %s at %ld. Event loop state: %d", + frequencyHz, + bus_.c_str(), + usec, + sd_event_get_state(afb_daemon_get_event_loop(binder_interface->daemon))); + if(sd_event_add_time(afb_daemon_get_event_loop(binder_interface->daemon), &source, - CLOCK_MONOTONIC, usec, TIMERFD_ACCURACY, send_request, request) < 0) + CLOCK_BOOTTIME, usec, TIMERFD_ACCURACY, send_request, request) < 0) { ERROR(binder_interface, "add_recurring_request: Request fails to be schedule through event loop"); added = false; @@ -406,23 +420,43 @@ bool diagnostic_manager_t::conflicting(active_diagnostic_request_t* request, act } -/// @brief Returns true if there are no other active requests to the same arbitration ID. +/// @brief Returns true if there are no other active requests to the same arbitration ID +/// and if there aren't more than 8 requests in flight at the same time. bool diagnostic_manager_t::clear_to_send(active_diagnostic_request_t* request) const { + int total_in_flight = 0; for ( auto entry : non_recurring_requests_) { if(conflicting(request, entry)) return false; + if(entry->get_in_flight()) + total_in_flight++; } for ( auto entry : recurring_requests_) { if(conflicting(request, entry)) return false; + if(entry->get_in_flight()) + total_in_flight++; } + + if(total_in_flight > MAX_SIMULTANEOUS_IN_FLIGHT_REQUESTS) + return false; return true; } +int diagnostic_manager_t::reschedule_request(sd_event_source *s, uint64_t usec, active_diagnostic_request_t* adr) +{ + usec = usec + (uint64_t)(adr->get_frequency_clock().frequency_to_period()); + DEBUG(binder_interface, "send_request: Event loop state: %d. usec: %ld", sd_event_get_state(afb_daemon_get_event_loop(binder_interface->daemon)), usec); + if(sd_event_source_set_time(s, usec) >= 0) + if(sd_event_source_set_enabled(s, SD_EVENT_ON) >= 0) + return 0; + sd_event_source_unref(s); + return -1; +} + /// @brief Systemd timer event callback use to send CAN messages at regular interval. Depending /// on the diagnostic message frequency. /// @@ -440,34 +474,30 @@ int diagnostic_manager_t::send_request(sd_event_source *s, uint64_t usec, void * DiagnosticRequest* request = (DiagnosticRequest*)userdata; active_diagnostic_request_t* adr = dm.find_recurring_request(request); -// if(adr != nullptr && adr->get_can_bus_dev() == dm.get_can_bus_dev() && adr->should_send() && -// dm.clear_to_send(adr)) - if(adr != nullptr && adr->get_can_bus_dev() == dm.get_can_bus_dev()) + dm.cleanup_active_requests(false); + if(adr != nullptr && adr->get_can_bus_dev() == dm.get_can_bus_dev() && adr->should_send() && + dm.clear_to_send(adr)) { adr->get_frequency_clock().tick(); start_diagnostic_request(&dm.shims_, adr->get_handle()); if(adr->get_handle()->completed && !adr->get_handle()->success) { - DEBUG(binder_interface, "send_request: Fatal error sending diagnostic request"); - sd_event_source_unref(s); - return -1; + ERROR(binder_interface, "send_request: Fatal error sending diagnostic request"); + sd_event_source_unref(s); + return -1; } + adr->get_timeout_clock().tick(); adr->set_in_flight(true); + } - if(adr->get_recurring()) - { - usec = usec + (uint64_t)(frequency_clock_t::frequency_to_period(adr->get_frequency_clock().get_frequency())*MICRO); - DEBUG(binder_interface, "send_request: Event loop state: %d. usec: %ld", sd_event_get_state(afb_daemon_get_event_loop(binder_interface->daemon)), usec); - if(sd_event_source_set_time(s, usec) >= 0) - if(sd_event_source_set_enabled(s, SD_EVENT_ON) >= 0) - return 0; - sd_event_source_unref(s); - return -1; - } + if(adr != nullptr && adr->get_recurring()) + { + return dm.reschedule_request(s, usec, adr); } + sd_event_source_unref(s); - ERROR(binder_interface, "send_request: Something goes wrong when submitting a new request to the CAN bus"); + NOTICE(binder_interface, "send_request: Request doesn't exist anymore. Canceling.'"); return -2; } @@ -509,6 +539,7 @@ openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_dia found_signals.front()->set_supported(false); cleanup_request(adr, true); NOTICE(binder_interface, "relay_diagnostic_response: PID not supported or ill formed. Please unsubscribe from it. Error code : %d", response.negative_response_code); + message = build_VehicleMessage(build_SimpleMessage(adr->get_name(), build_DynamicField("This PID isn't supported by your vehicle."))); } if(adr->get_callback() != nullptr)