1use std::{io::{Read, Write}, net::{TcpStream, UdpSocket, SocketAddr}};
2use iso13400_2::{*, request};
3use iso14229_1::{Configuration as Iso14229Cfg, response::Response as Iso14229Response, TryFromWithCfg};
4use crate::DoIpError;
5use super::{config::Configuration, context::{GatewayInfo, PL_TYPES}};
6
7type VerPayload = (Version, Payload);
8pub type RoutingActiveStatus = (ActiveCode, Option<u8>);
9
10#[derive(Debug)]
11pub struct DoIpClient {
12 config: Configuration,
13 server_udp_addr: SocketAddr,
14 udp_socket: UdpSocket,
15 tcp_stream: TcpStream,
16 gateway_info: Option<GatewayInfo>,
17}
18
19impl DoIpClient {
20 pub fn new(config: Configuration) -> Result<Self, DoIpError> {
21 let udp_socket = UdpSocket::bind(format!("{}:0", config.server_ip()))
22 .map_err(DoIpError::IoError)?;
23 let tcp_stream = TcpStream::connect(format!("{}:{}", config.server_ip(), TCP_SERVER_PORT))
24 .map_err(DoIpError::IoError)?;
25 let server_udp_addr = format!("{}:{}", config.server_ip(), UDP_SERVER_PORT)
26 .parse::<SocketAddr>()
27 .unwrap();
28
29 Ok(Self {
30 config,
31 server_udp_addr,
32 udp_socket,
33 tcp_stream,
34 gateway_info: None,
35 })
36 }
37
38 pub fn vehicle_identifier(&mut self) -> Result<(), DoIpError> {
39 let request = Message {
40 version: self.gateway_version(),
41 payload: Payload::ReqVehicleId(request::VehicleID)
42 };
43
44 let response = self.udp_send_recv(request)?;
45 self.vehicle_id_response(response)
46 }
47
48 pub fn vehicle_with_eid(&mut self, eid: Eid) -> Result<(), DoIpError> {
49 let request = Message {
50 version: self.gateway_version(),
51 payload: Payload::ReqVehicleWithEid(request::VehicleIDWithEID::new(eid)),
52 };
53
54 let response = self.udp_send_recv(request)?;
55 self.vehicle_id_response(response)
56 }
57
58 pub fn vehicle_with_vin(&mut self, vin: &str) -> Result<(), DoIpError> {
59 let payload = request::VehicleIDWithVIN::new(vin)
60 .map_err(DoIpError::Iso13400Error)?;
61 let request = Message {
62 version: self.gateway_version(),
63 payload: Payload::ReqVehicleWithVIN(payload)
64 };
65
66 let response = self.udp_send_recv(request)?;
67 self.vehicle_id_response(response)
68 }
69
70 pub fn routing_active(
71 &mut self,
72 r#type: RoutingActiveType,
73 user_def: Option<u32>,
74 ) -> Result<RoutingActiveStatus, DoIpError> {
75 let src_addr = self.config.address();
76 let request = Message {
77 version: self.gateway_version(),
78 payload: Payload::ReqRoutingActive(
79 request::RoutingActive::new(src_addr, r#type, user_def)
80 )
81 };
82 let (_, resp) = self.tcp_write_read(request)?;
83 match resp {
84 Payload::RespHeaderNegative(v) => {
85 Err(DoIpError::HeaderNegativeError(v.code()))
86 },
87 Payload::RespRoutingActive(v) => {
88 let dst_addr = v.dst_addr();
89 if src_addr != dst_addr {
90 log::warn!("DoIPClient - routing active receive a message that target address: {:?}", dst_addr);
91 }
92
93 let active_code = v.active_code();
94 match active_code {
95 ActiveCode::Activated |
96 ActiveCode::Success |
97 ActiveCode::NeedConfirm => Ok((active_code, None)),
98 ActiveCode::SourceAddressUnknown |
99 ActiveCode::SourceAddressInvalid |
100 ActiveCode::SocketInvalid |
101 ActiveCode::WithoutAuth |
102 ActiveCode::VehicleRefused |
103 ActiveCode::Unsupported |
104 ActiveCode::TLSRequired => Err(DoIpError::ActiveError(active_code)),
105 ActiveCode::VMSpecific(v) => {
106 log::info!("DoIPClient - routing active receive VM Specific value: {}", v);
107 Ok((active_code, Some(v)))
108 },
109 ActiveCode::Reserved(v) => {
110 log::warn!("DoIPClient - routing active receive Reserved value: {}", v);
111 Ok((active_code, Some(v)))
112 },
113 }
114 },
115 _ => unreachable!(),
116 }
117 }
118
119 pub fn alive_check(&mut self) -> Result<(), DoIpError> {
120 let request = Message {
121 version: self.gateway_version(),
122 payload: Payload::ReqAliveCheck(request::AliveCheck)
123 };
124 let (_, resp) = self.tcp_write_read(request)?;
125 match resp {
126 Payload::RespHeaderNegative(v) => {
127 Err(DoIpError::HeaderNegativeError(v.code()))
128 },
129 Payload::RespAliveCheck(v) => {
130 log::info!("DoIPClient - alive check: {:?}", v.src_addr());
131 Ok(())
132 },
133 _ => unreachable!(),
134 }
135 }
136
137 pub fn entity_status(&mut self) -> Result<response::EntityStatus, DoIpError> {
138 let request = Message {
139 version: self.gateway_version(),
140 payload: Payload::ReqEntityStatus(request::EntityStatus)
141 };
142 let (_, resp) = self.udp_send_recv(request)?;
143 match resp {
144 Payload::RespHeaderNegative(v) => {
145 Err(DoIpError::HeaderNegativeError(v.code()))
146 },
147 Payload::RespEntityStatus(v) => Ok(v),
148 _ => unreachable!(),
149 }
150 }
151
152 pub fn diag_power_mode(&mut self) -> Result<PowerMode, DoIpError> {
153 let request = Message {
154 version: self.gateway_version(),
155 payload: Payload::ReqDiagPowerMode(request::DiagnosticPowerMode)
156 };
157 let (_, resp) = self.udp_send_recv(request)?;
158 match resp {
159 Payload::RespHeaderNegative(v) => {
160 Err(DoIpError::HeaderNegativeError(v.code()))
161 },
162 Payload::RespDiagPowerMode(v) => {
163 Ok(v.mode())
164 },
165 _ => unreachable!(),
166 }
167 }
168
169 pub fn diagnostic(
170 &mut self,
171 address: LogicAddress,
172 data: Vec<u8>,
173 ) -> Result<Iso14229Response, DoIpError> {
174 let request = Message {
175 version: self.gateway_version(),
176 payload: Payload::Diagnostic(
177 Diagnostic::new(self.config.address(), address, data)
178 )
179 };
180 let (_, resp) = self.tcp_write_read(request)?;
181 match resp {
182 Payload::RespHeaderNegative(v) => {
183 Err(DoIpError::HeaderNegativeError(v.code()))
184 },
185 Payload::RespDiagNegative(v) => {
186 log::warn!("DoIPClient - {}", v);
187 Err(DoIpError::DiagnosticNegativeError {
188 code: v.code(),
189 data: hex::encode(v.previous_diagnostic_data())
190 })
191 },
192 Payload::RespDiagPositive(v) => {
193 log::debug!("DoIPClient - Diagnostic message ACK: {}", v);
194 let (_, payload) = self.tcp_read(&PL_TYPES.diag_data_payload_types)?;
195 match payload {
196 Payload::Diagnostic(v) => {
197 let data = v.data;
198 log::debug!("DoIPClient - diagnostic Data: {:?}", hex::encode(&data));
199 let cfg = Iso14229Cfg::default();
200 let resp = Iso14229Response::try_from_cfg(data, &cfg)
201 .map_err(DoIpError::Iso14229Error)?;
202
203 Ok(resp)
204 },
205 _ => unreachable!(),
206 }
207 },
208 _ => unreachable!(),
209 }
210 }
211
212 #[inline]
213 fn vehicle_id_response(&mut self, (ver, resp): VerPayload) -> Result<(), DoIpError> {
214 match resp {
215 Payload::RespHeaderNegative(v) => {
216 Err(DoIpError::HeaderNegativeError(v.code()))
217 },
218 Payload::RespVehicleId(v) => {
219 self.gateway_info = Some(GatewayInfo {
220 version: ver,
221 address: v.address(),
222 eid: v.eid(),
223 gid: v.gid(),
224 further_act: v.further_act(),
225 sync_status: v.sync_status(),
226 });
227
228 Ok(())
229 },
230 _ => unreachable!(),
231 }
232 }
233
234 fn udp_send_recv(&mut self, request: Message) -> Result<VerPayload, DoIpError> {
235 let payload_type = request.payload.payload_type();
236 let expect = match payload_type {
237 PayloadType::ReqVehicleId => Some(&PL_TYPES.vid_payload_types),
238 PayloadType::ReqVehicleWithEid => Some(&PL_TYPES.vid_payload_types),
239 PayloadType::ReqVehicleWithVIN => Some(&PL_TYPES.vid_payload_types),
240 PayloadType::ReqEntityStatus => Some(&PL_TYPES.es_payload_types),
241 PayloadType::ReqDiagPowerMode => Some(&PL_TYPES.dpm_payload_types),
242 _ => None
243 }
244 .ok_or(DoIpError::InputError(format!("invalid udp request payload: {:?}", payload_type)))?;
245 let data: Vec<_> = request.into();
246 log::trace!("DoIPClient - UDP writing data: {}", hex::encode(&data));
247 let size = self.udp_socket.send_to(&data, &self.server_udp_addr)
248 .map_err(DoIpError::IoError)?;
249 let data_len = data.len();
250 if size != data_len {
251 log::warn!("DoIPClient - UDP wrote {} bytes, expect {}", size, data_len);
252 }
253
254 let mut buffer = [0; 1024];
255 let size = self.udp_socket.recv(&mut buffer)
256 .map_err(DoIpError::IoError)?;
257
258 self.parse_response(&buffer[..size], expect)
259 }
260
261 fn tcp_write_read(&mut self, request: Message) -> Result<VerPayload, DoIpError> {
262 let payload_type = request.payload.payload_type();
263 let expect = match payload_type {
264 PayloadType::ReqRoutingActive => Some(&PL_TYPES.ra_payload_types),
265 PayloadType::ReqAliveCheck => Some(&PL_TYPES.ac_payload_types),
266 PayloadType::Diagnostic => Some(&PL_TYPES.diag_payload_types),
267 _ => None,
268 }
269 .ok_or(DoIpError::InputError(format!("invalid udp request payload: {:?}", payload_type)))?;
270 let data: Vec<_> = request.into();
271 log::trace!("DoIPClient - TCP writing data: {}", hex::encode(&data));
272 let size = self.tcp_stream.write(&data)
273 .map_err(DoIpError::IoError)?;
274 let data_len = data.len();
275 if size != data_len {
276 log::warn!("DoIPClient - TCP wrote {} bytes, expect {}", size, data_len);
277 Err(DoIpError::IoError(std::io::Error::last_os_error()))
278 }
279 else {
280 self.tcp_read(expect)
281 }
282 }
283
284 #[inline]
285 fn tcp_read(&mut self, expected: &Vec<PayloadType>) -> Result<VerPayload, DoIpError> {
286 let mut buffer = [0; 4096];
287 let size = self.tcp_stream.read(&mut buffer)
288 .map_err(DoIpError::IoError)?;
289
290 self.parse_response(&buffer[..size], expected)
291 }
292
293 #[inline]
294 fn parse_response(
295 &mut self,
296 data: &[u8],
297 expected: &Vec<PayloadType>,
298 ) -> Result<VerPayload, DoIpError> {
299 let response = Message::try_from(data)
300 .map_err(DoIpError::Iso13400Error)?;
301 let version = response.version;
302 self.version_check(version);
303
304 let actual = response.payload.payload_type();
305 if expected.contains(&actual) {
306 Ok((version, response.payload))
307 }
308 else {
309 Err(DoIpError::PayloadTypeError(actual))
310 }
311 }
312
313 fn version_check(&self, version: Version) {
314 match &self.gateway_info {
315 Some(info) => if info.version() != version {
316 log::warn!("DoIPClient - DoIP version mismatch!");
317 },
318 None => {},
319 }
320 }
321
322 #[inline]
323 fn gateway_version(&self) -> Version {
324 match &self.gateway_info {
325 Some(info) => info.version,
326 None => Version::Default,
327 }
328 }
329}