diff --git a/src/dynamic_diag.rs b/src/dynamic_diag.rs index 1c7db92..9dfc20a 100644 --- a/src/dynamic_diag.rs +++ b/src/dynamic_diag.rs @@ -273,6 +273,9 @@ impl DynamicDiagSession { let (mut tx_resp, rx_resp) = mpsc::channel::(); let is_connected = Arc::new(AtomicBool::new(true)); let is_connected_inner = is_connected.clone(); + let command_cooldown_ms = advanced_opts + .map(|o| o.command_cooldown_ms as u128) + .unwrap_or_else(|| 0); let noti_session_mode = Arc::new(RwLock::new(current_session_mode.clone())); let noti_session_mode_t = noti_session_mode.clone(); @@ -281,182 +284,186 @@ impl DynamicDiagSession { let is_running_c = is_running.clone(); std::thread::spawn(move || { let mut last_tp_time = Instant::now(); + let mut last_cmd_time = Instant::now(); while is_running.load(Ordering::Relaxed) { - // Incomming ECU request - if let Ok(req) = rx_req.recv_timeout(Duration::from_millis(100)) { - let mut tx_addr = basic_opts.send_id; - match protocol.process_req_payload(&req.payload) { - DiagAction::SetSessionMode(mode) => { - let mut needs_response = true; - let mut ext_id = None; - if let Some(adv) = advanced_opts { - if adv.global_session_control && adv.global_tp_id != 0 { - tx_addr = adv.global_tp_id; - ext_id = adv.tp_ext_id; - needs_response = false; - } else if adv.global_session_control && adv.global_tp_id == 0 { - log::warn!("Global session control is enabled but global TP ID is not specified"); + if last_cmd_time.elapsed().as_millis() > command_cooldown_ms { + // Incomming ECU request + if let Ok(req) = rx_req.recv_timeout(Duration::from_millis(100)) { + let mut tx_addr = basic_opts.send_id; + match protocol.process_req_payload(&req.payload) { + DiagAction::SetSessionMode(mode) => { + let mut needs_response = true; + let mut ext_id = None; + if let Some(adv) = advanced_opts { + if adv.global_session_control && adv.global_tp_id != 0 { + tx_addr = adv.global_tp_id; + ext_id = adv.tp_ext_id; + needs_response = false; + } else if adv.global_session_control && adv.global_tp_id == 0 { + log::warn!("Global session control is enabled but global TP ID is not specified"); + } } + let res = send_recv_ecu_req::( + tx_addr, + ext_id, + &req.payload, + needs_response, + Some(&mut tx_resp), + basic_opts, + 0, + &mut channel, + &is_connected_inner, + ); + if res.is_ok() { + // Send OK! We can set diag mode in the server side + requested_session_mode = Some(mode); + *noti_session_mode_t.write().unwrap() = + requested_session_mode.clone(); + last_tp_time = Instant::now(); + } + tx_resp.send(res); } - let res = send_recv_ecu_req::( - tx_addr, - ext_id, - &req.payload, - needs_response, - Some(&mut tx_resp), - basic_opts, - 0, - &mut channel, - &is_connected_inner, - ); - if res.is_ok() { - // Send OK! We can set diag mode in the server side - requested_session_mode = Some(mode); - *noti_session_mode_t.write().unwrap() = - requested_session_mode.clone(); - last_tp_time = Instant::now(); - } - tx_resp.send(res); - } - DiagAction::EcuReset => { - let res = send_recv_ecu_req::( - tx_addr, - None, - &req.payload, - req.response_require, - Some(&mut tx_resp), - basic_opts, - 0, - &mut channel, - &is_connected_inner, - ); - if res.is_ok() { - log::debug!("ECU Reset detected. Setting default session mode"); - // Send OK! We have to set default session mode as the ECU has been rebooted - // Internally, the 'current session' does not change, as diag server will try on the next - // request to change modes back - *noti_session_mode_t.write().unwrap() = - protocol.get_basic_session_mode(); - current_session_mode = protocol.get_basic_session_mode(); - std::thread::sleep(Duration::from_millis(500)); // Await ECU to reboot - TODO. Maybe we should let this be configured? + DiagAction::EcuReset => { + let res = send_recv_ecu_req::( + tx_addr, + None, + &req.payload, + req.response_require, + Some(&mut tx_resp), + basic_opts, + 0, + &mut channel, + &is_connected_inner, + ); + if res.is_ok() { + log::debug!("ECU Reset detected. Setting default session mode"); + // Send OK! We have to set default session mode as the ECU has been rebooted + // Internally, the 'current session' does not change, as diag server will try on the next + // request to change modes back + *noti_session_mode_t.write().unwrap() = + protocol.get_basic_session_mode(); + current_session_mode = protocol.get_basic_session_mode(); + std::thread::sleep(Duration::from_millis(500)); // Await ECU to reboot - TODO. Maybe we should let this be configured? + } + tx_resp.send(res); } - tx_resp.send(res); - } - _ => { - let mut resp = send_recv_ecu_req::( - tx_addr, - None, - &req.payload, - req.response_require, - Some(&mut tx_resp), - basic_opts, - 0, - &mut channel, - &is_connected_inner, - ); - if let DiagServerRx::EcuError { b, desc: _ } = &resp { - if NRC::from(*b).is_wrong_diag_mode() { - log::debug!("Trying to switch ECU modes"); - // Wrong diag mode. We need to see if we need to change modes - // Switch modes! - tx_resp.send(DiagServerRx::EcuBusy); // Until we have a hook for this specific scenerio - // Now create new diag server request message - let mut needs_response = true; - let mut ext_id = None; - if let Some(adv) = advanced_opts { - if adv.global_session_control && adv.global_tp_id != 0 { - tx_addr = adv.global_tp_id; - ext_id = adv.tp_ext_id; - needs_response = false; - } else if adv.global_session_control - && adv.global_tp_id == 0 - { - log::warn!("Global session control is enabled but global TP ID is not specified"); + _ => { + let mut resp = send_recv_ecu_req::( + tx_addr, + None, + &req.payload, + req.response_require, + Some(&mut tx_resp), + basic_opts, + 0, + &mut channel, + &is_connected_inner, + ); + if let DiagServerRx::EcuError { b, desc: _ } = &resp { + if NRC::from(*b).is_wrong_diag_mode() { + log::debug!("Trying to switch ECU modes"); + // Wrong diag mode. We need to see if we need to change modes + // Switch modes! + tx_resp.send(DiagServerRx::EcuBusy); // Until we have a hook for this specific scenerio + // Now create new diag server request message + let mut needs_response = true; + let mut ext_id = None; + if let Some(adv) = advanced_opts { + if adv.global_session_control && adv.global_tp_id != 0 { + tx_addr = adv.global_tp_id; + ext_id = adv.tp_ext_id; + needs_response = false; + } else if adv.global_session_control + && adv.global_tp_id == 0 + { + log::warn!("Global session control is enabled but global TP ID is not specified"); + } } - } - if send_recv_ecu_req::( - tx_addr, - ext_id, - &protocol.make_session_control_msg( - ¤t_session_mode.clone().unwrap(), - ), - needs_response, - None, // None, internally handled - basic_opts, - 0, - &mut channel, - &is_connected_inner, - ) - .is_ok() - { - log::debug!("ECU mode switch OK. Resending the request"); - *noti_session_mode_t.write().unwrap() = - current_session_mode.clone(); - last_tp_time = Instant::now(); - // Resend our request - resp = send_recv_ecu_req::( + if send_recv_ecu_req::( tx_addr, - None, - &req.payload, - req.response_require, - Some(&mut tx_resp), + ext_id, + &protocol.make_session_control_msg( + ¤t_session_mode.clone().unwrap(), + ), + needs_response, + None, // None, internally handled basic_opts, 0, &mut channel, &is_connected_inner, - ); - } else { - // Diag session mode req failed. Set session data - *noti_session_mode_t.write().unwrap() = - protocol.get_basic_session_mode(); - current_session_mode = protocol.get_basic_session_mode() + ) + .is_ok() + { + log::debug!("ECU mode switch OK. Resending the request"); + *noti_session_mode_t.write().unwrap() = + current_session_mode.clone(); + last_tp_time = Instant::now(); + // Resend our request + resp = send_recv_ecu_req::( + tx_addr, + None, + &req.payload, + req.response_require, + Some(&mut tx_resp), + basic_opts, + 0, + &mut channel, + &is_connected_inner, + ); + } else { + // Diag session mode req failed. Set session data + *noti_session_mode_t.write().unwrap() = + protocol.get_basic_session_mode(); + current_session_mode = protocol.get_basic_session_mode() + } } } + tx_resp.send(resp); } - tx_resp.send(resp); } - } - } else { - if current_session_mode != requested_session_mode { - current_session_mode = requested_session_mode.clone(); - } - // Nothing to process, so sleep and/or tester present processing - // Logic for handling session control TP present requests - if session_control { - let c_mode = current_session_mode.clone().unwrap(); - let aops = advanced_opts.unwrap(); - if c_mode.tp_require - && last_tp_time.elapsed().as_millis() as u32 - >= aops.tester_present_interval_ms - { - let tx_payload = P::create_tp_msg(aops.tester_present_require_response); - let tx_addr = if aops.global_tp_id != 0 { - aops.global_tp_id - } else { - basic_opts.send_id - }; - if send_recv_ecu_req::( - tx_addr, - aops.tp_ext_id, - &tx_payload.to_bytes(), - aops.tester_present_require_response, - None, - basic_opts, - 0, - &mut channel, - &is_connected_inner, - ) - .is_err() + } else { + if current_session_mode != requested_session_mode { + current_session_mode = requested_session_mode.clone(); + } + // Nothing to process, so sleep and/or tester present processing + // Logic for handling session control TP present requests + if session_control { + let c_mode = current_session_mode.clone().unwrap(); + let aops = advanced_opts.unwrap(); + if c_mode.tp_require + && last_tp_time.elapsed().as_millis() as u32 + >= aops.tester_present_interval_ms { - log::warn!("Tester present send failure. Assuming default diag session state"); - current_session_mode = protocol.get_basic_session_mode(); - *noti_session_mode_t.write().unwrap() = - current_session_mode.clone(); - } else { - last_tp_time = Instant::now(); // OK, reset the timer + let tx_payload = P::create_tp_msg(aops.tester_present_require_response); + let tx_addr = if aops.global_tp_id != 0 { + aops.global_tp_id + } else { + basic_opts.send_id + }; + if send_recv_ecu_req::( + tx_addr, + aops.tp_ext_id, + &tx_payload.to_bytes(), + aops.tester_present_require_response, + None, + basic_opts, + 0, + &mut channel, + &is_connected_inner, + ) + .is_err() + { + log::warn!("Tester present send failure. Assuming default diag session state"); + current_session_mode = protocol.get_basic_session_mode(); + *noti_session_mode_t.write().unwrap() = + current_session_mode.clone(); + } else { + last_tp_time = Instant::now(); // OK, reset the timer + } } } } + last_cmd_time = Instant::now(); } } // Thread has exited, so tear everything down!