Line data Source code
1 : /********************************************************************************
2 : * Copyright (c) 2026 Accenture
3 : *
4 : * This program and the accompanying materials are made available under the
5 : * terms of the Apache License Version 2.0 which is available at
6 : * https://www.apache.org/licenses/LICENSE-2.0
7 : *
8 : * SPDX-License-Identifier: Apache-2.0
9 : ********************************************************************************/
10 :
11 : #include "someip/QueryManager.h"
12 :
13 : #include "someip/ServiceDescription.h"
14 : #include "someip/SomeIpConstants.h"
15 : #include "someip/init.h"
16 : #include "someip/logger.h"
17 :
18 : #include <etl/algorithm.h>
19 : #include <etl/intrusive_forward_list.h>
20 : #include <etl/intrusive_links.h>
21 : #include <algorithm>
22 :
23 : // Logger API uses printf-style varargs for fixed diagnostic messages in this module.
24 : // NOLINTBEGIN(cppcoreguidelines-pro-type-vararg)
25 :
26 : namespace someip
27 : {
28 : using ::util::logger::SOMEIP;
29 :
30 68 : QueryManager::QueryManager(QueryList& queryList, TcpClientChannelValidator& validator)
31 68 : : _queries(queryList)
32 68 : , _pServiceAnnouncer(nullptr)
33 68 : , _pRpcReceiver(nullptr)
34 68 : , _tcpClientChannelValidator(validator)
35 68 : {}
36 :
37 55 : void QueryManager::wire(IServiceAnnouncer* const serviceAnnouncer, IRpcReceiver* const rpcReceiver)
38 : {
39 55 : _pServiceAnnouncer = serviceAnnouncer;
40 55 : _pRpcReceiver = rpcReceiver;
41 55 : }
42 :
43 2 : void QueryManager::start() const
44 : {
45 2 : if (_pServiceAnnouncer == nullptr)
46 : {
47 1 : WARN_LOG(SOMEIP, "QueryManager::start() no service-announcer");
48 : }
49 2 : if (_pRpcReceiver == nullptr)
50 : {
51 0 : WARN_LOG(SOMEIP, "QueryManager::start() no rpc-receiver");
52 : }
53 :
54 4 : for (ServiceQuery* query : _queries)
55 : {
56 2 : if (query != nullptr)
57 : {
58 2 : ::someip::init(*query);
59 : }
60 : }
61 2 : }
62 :
63 1 : void QueryManager::stop() const
64 : {
65 2 : for (ServiceQuery* query : _queries)
66 : {
67 1 : IServiceListener* const listener = query->listener;
68 :
69 1 : if (listener != nullptr)
70 : {
71 0 : listener->serviceStatusChanged(
72 0 : query->description, IServiceListener::ServiceStatus::SERVICE_UNAVAILABLE);
73 : }
74 : else
75 : {
76 1 : if (_pServiceAnnouncer != nullptr)
77 : {
78 1 : _pServiceAnnouncer->unsubscribe(query->description, query->serviceDiscoveryAddress);
79 : }
80 : }
81 : }
82 1 : }
83 :
84 30 : bool QueryManager::registerQuery(ServiceQuery& query)
85 : {
86 30 : if (hasQuery(query))
87 : {
88 1 : return false;
89 : }
90 :
91 29 : if (_queries.full())
92 : {
93 1 : WARN_LOG(
94 : SOMEIP,
95 : "QueryManager full! Rejecting query (service: %d, version: %d, instance: %d, "
96 : "eventgroup: %d)",
97 : query.description.serviceId,
98 : query.description.majorVersion,
99 : query.description.instanceId,
100 : query.description.eventGroup);
101 1 : return false;
102 : }
103 :
104 28 : INFO_LOG(
105 : SOMEIP,
106 : "QueryManager::registerQuery(service: %d, version: %d, instance: %d, eventgroup: %d)",
107 : query.description.serviceId,
108 : query.description.majorVersion,
109 : query.description.instanceId,
110 : query.description.eventGroup);
111 :
112 28 : (void)_queries.insert(&query);
113 28 : ::someip::init(query);
114 :
115 28 : return true;
116 : }
117 :
118 25 : bool QueryManager::unregisterQuery(ServiceQuery& query)
119 : {
120 25 : if (!hasQuery(query))
121 : {
122 1 : return false;
123 : }
124 :
125 24 : INFO_LOG(
126 : SOMEIP,
127 : "QueryManager::unregisterQuery(service: %d, version: %d, instance: %d, eventgroup: %d)",
128 : query.description.serviceId,
129 : query.description.majorVersion,
130 : query.description.instanceId,
131 : query.description.eventGroup);
132 :
133 24 : if (containsEventGroup(query.description)
134 24 : && (query.subscriptionState != ServiceQuery::SubscriptionState::STATE_UNSUBSCRIBED))
135 : {
136 4 : if (_pServiceAnnouncer != nullptr)
137 : {
138 4 : _pServiceAnnouncer->unsubscribe(query.description, query.serviceDiscoveryAddress);
139 : }
140 :
141 4 : query.subscriptionState = ServiceQuery::SubscriptionState::STATE_UNSUBSCRIBED;
142 : }
143 :
144 24 : auto it = _queries.find(&query);
145 24 : if (it != _queries.end())
146 : {
147 24 : _queries.erase(it);
148 : }
149 24 : query.state = ServiceQuery::ServiceQueryState::QUERY_IDLE_PHASE;
150 :
151 24 : return true;
152 : }
153 :
154 62 : bool QueryManager::hasQuery(ServiceQuery& query) const { return _queries.contains(&query); }
155 :
156 1 : bool QueryManager::isEventgroupPort(
157 : service_id::type const serviceId,
158 : instance_id::type const instanceId,
159 : major_version::type const majorVersion,
160 : uint16_t const port) const
161 : {
162 1 : for (ServiceQuery* query : _queries)
163 : {
164 0 : auto service = ::someip::make<ServiceDescription>();
165 0 : service.serviceId = serviceId;
166 0 : service.instanceId = instanceId;
167 0 : service.majorVersion = majorVersion;
168 0 : if ((query != nullptr) && isEventgroupOfService(query->description, service))
169 : {
170 0 : if (port == query->description.port)
171 : {
172 0 : return true;
173 : }
174 : }
175 : }
176 1 : return false;
177 : }
178 :
179 : ServiceQuery const*
180 0 : QueryManager::getQuery(service_id::type const serviceId, instance_id::type const instanceId) const
181 : {
182 : QueryList::const_iterator queryItr = etl::find_if(
183 0 : _queries.cbegin(),
184 0 : _queries.cend(),
185 0 : [serviceId](ServiceQuery const* query)
186 0 : { return query && query->description.serviceId >= serviceId; });
187 0 : for (; queryItr != _queries.end(); ++queryItr)
188 : {
189 0 : ServiceQuery* const query = *queryItr;
190 :
191 0 : if ((query != nullptr) && (query->description.serviceId != serviceId))
192 : {
193 0 : break;
194 : }
195 0 : if ((query != nullptr) && (query->description.instanceId == instanceId))
196 : {
197 0 : return query;
198 : }
199 : }
200 0 : return nullptr;
201 : }
202 :
203 4 : bool QueryManager::hasServiceDescription(ServiceDescription const& service) const
204 : {
205 : QueryList::const_iterator query = etl::find_if(
206 4 : _queries.cbegin(),
207 0 : _queries.cend(),
208 8 : [serviceId = service.serviceId](ServiceQuery const* q)
209 12 : { return q && q->description.serviceId >= serviceId; });
210 4 : for (; query != _queries.cend(); ++query)
211 : {
212 3 : if (query != nullptr)
213 : {
214 3 : if ((*query)->description.serviceId != service.serviceId)
215 : {
216 2 : break;
217 : }
218 1 : if (isInstanceOf(service, (*query)->description))
219 : {
220 1 : return true;
221 : }
222 : }
223 : }
224 3 : return false;
225 : }
226 :
227 21 : void QueryManager::updateQueries(
228 : ServiceDescription const& service, IServiceListener::ServiceStatus const status) const
229 : {
230 21 : DEBUG_LOG(
231 : SOMEIP, "QueryManager::updateQueries(service: %d, status: %d)", service.serviceId, status);
232 :
233 21 : ::etl::intrusive_forward_list<IServiceListener, ::etl::forward_link<0>> listeners;
234 :
235 : QueryList::const_iterator queryItr = etl::find_if(
236 21 : _queries.cbegin(),
237 0 : _queries.cend(),
238 46 : [serviceId = service.serviceId](ServiceQuery const* q)
239 67 : { return q && q->description.serviceId >= serviceId; });
240 41 : for (; queryItr != _queries.end(); ++queryItr)
241 : {
242 23 : ServiceQuery* const query = *queryItr;
243 23 : if (query != nullptr)
244 : {
245 23 : if (query->description.serviceId != service.serviceId)
246 : {
247 3 : break;
248 : }
249 : // handle queries for services
250 20 : if (isInstanceOf(service, query->description))
251 : {
252 17 : if (IServiceListener::ServiceStatus::SERVICE_AVAILABLE == status)
253 : {
254 9 : query->state = ServiceQuery::ServiceQueryState::QUERY_MAIN_PHASE;
255 : }
256 : // offer service ttl is expired
257 8 : else if (
258 : (IServiceListener::ServiceStatus::SERVICE_UNAVAILABLE == status)
259 8 : && (service.ttl == 0U))
260 : {
261 5 : query->state = ServiceQuery::ServiceQueryState::QUERY_INITIAL_WAIT_PHASE;
262 : }
263 : else
264 : {
265 : // do nothing
266 : }
267 :
268 17 : IServiceListener* const listener = query->listener;
269 :
270 17 : if (listener != nullptr)
271 : {
272 14 : query->serviceDiscoveryAddress = service.ipAddress;
273 14 : listeners.push_front(*listener);
274 : }
275 : }
276 : // handle queries for eventgroups
277 3 : else if (isEventgroupOfService(query->description, service))
278 : {
279 3 : if (IServiceListener::ServiceStatus::SERVICE_UNAVAILABLE == status)
280 : {
281 1 : query->subscriptionState = ServiceQuery::SubscriptionState::STATE_UNSUBSCRIBED;
282 : }
283 : }
284 : else
285 : {
286 : ; // nothing else to do
287 : }
288 : }
289 : }
290 :
291 : // notifying listeners may change queries
292 35 : while (!listeners.empty())
293 : {
294 14 : IServiceListener* const listener = &listeners.front();
295 14 : listeners.pop_front();
296 :
297 14 : listener->serviceStatusChanged(service, status);
298 :
299 14 : auto const endItr = _queries.cend();
300 14 : queryItr = _queries.begin();
301 25 : for (; queryItr != endItr; ++queryItr)
302 : {
303 16 : ServiceQuery* const query = *queryItr;
304 16 : if (query != nullptr)
305 : {
306 16 : if (query->description.serviceId != service.serviceId)
307 : {
308 5 : break;
309 : }
310 11 : if (isEventgroupOfService(query->description, service))
311 : {
312 0 : listener->updateEventgroupDescription(query->description, status);
313 : }
314 : }
315 : }
316 : }
317 21 : }
318 :
319 7 : void QueryManager::updateQueryInInitialPhase(uint64_t const timestamp, ServiceQuery& query) const
320 : {
321 7 : if (query.timestamp == 0U)
322 : {
323 5 : query.timestamp = timestamp; // start waiting
324 : }
325 7 : uint32_t const timePassed = static_cast<uint32_t>(timestamp - query.timestamp);
326 7 : if (timePassed >= query.sdConfig._initialDelay)
327 : {
328 5 : if ((_pServiceAnnouncer != nullptr) && (!containsEventGroup(query.description)))
329 : {
330 2 : _pServiceAnnouncer->find(query.description);
331 : }
332 :
333 5 : query.state = ServiceQuery::ServiceQueryState::QUERY_REPETITION_PHASE;
334 5 : query.timestamp = timestamp;
335 5 : query.repetitionCount = 1U;
336 : }
337 7 : }
338 :
339 8 : void QueryManager::updateQueryInRepetitionPhase(uint64_t const timestamp, ServiceQuery& query) const
340 : {
341 8 : uint32_t const timePassed = static_cast<uint32_t>(timestamp - query.timestamp);
342 : uint32_t const repetitionCount
343 8 : = etl::min(query.repetitionCount, static_cast<uint32_t>(sizeof(uint32_t) * 8U));
344 8 : uint32_t const repetitionDelay = (static_cast<uint32_t>(1U) << (repetitionCount - 1U))
345 8 : * query.sdConfig._repetitionsBaseDelay;
346 8 : if (timePassed >= repetitionDelay)
347 : {
348 8 : if ((_pServiceAnnouncer != nullptr) && (!containsEventGroup(query.description)))
349 : {
350 4 : _pServiceAnnouncer->find(query.description);
351 : }
352 :
353 8 : query.timestamp = timestamp;
354 8 : ++query.repetitionCount;
355 :
356 8 : if (query.repetitionCount >= query.sdConfig._repetitionsMax)
357 : {
358 4 : query.state = ServiceQuery::ServiceQueryState::QUERY_MAIN_PHASE;
359 : }
360 : }
361 8 : }
362 :
363 11 : void QueryManager::updateQueries(uint64_t const timestamp) const
364 : {
365 31 : for (ServiceQuery* query : _queries)
366 : {
367 20 : if (query != nullptr)
368 : {
369 : // should never happen, but safety first
370 20 : if (timestamp < query->timestamp)
371 : {
372 1 : continue;
373 : }
374 :
375 19 : if (ServiceQuery::ServiceQueryState::QUERY_INITIAL_WAIT_PHASE == query->state)
376 : {
377 7 : updateQueryInInitialPhase(timestamp, *query);
378 : }
379 12 : else if (ServiceQuery::ServiceQueryState::QUERY_REPETITION_PHASE == query->state)
380 : {
381 8 : updateQueryInRepetitionPhase(timestamp, *query);
382 : }
383 : }
384 : }
385 11 : }
386 :
387 4 : void QueryManager::subscribeAckReceived(
388 : ServiceDescription const& service,
389 : ::ip::IPEndpoint const& multicastEndpoint,
390 : ::ip::IPAddress const& sourceAddress)
391 : {
392 : QueryList::const_iterator queryItr = etl::find_if(
393 4 : _queries.cbegin(),
394 0 : _queries.cend(),
395 8 : [serviceId = service.serviceId](ServiceQuery const* q)
396 12 : { return q && q->description.serviceId >= serviceId; });
397 8 : for (; queryItr != _queries.end(); ++queryItr)
398 : {
399 4 : ServiceQuery* const query = *queryItr;
400 4 : if (query != nullptr)
401 : {
402 4 : if (query->description.serviceId != service.serviceId)
403 : {
404 0 : break;
405 : }
406 8 : if (((query->state == ServiceQuery::ServiceQueryState::QUERY_REPETITION_PHASE)
407 1 : || (query->state == ServiceQuery::ServiceQueryState::QUERY_MAIN_PHASE))
408 3 : && matches(query->description, service)
409 8 : && query->serviceDiscoveryAddress == sourceAddress)
410 : {
411 3 : query->subscriptionState = ServiceQuery::SubscriptionState::STATE_ACK_RECEIVED;
412 :
413 3 : if ((multicastEndpoint.isSet())
414 3 : && (::ip::isMulticastAddress(multicastEndpoint.getAddress()))
415 6 : && (!query->multicastAddress.isSet()))
416 : {
417 1 : query->multicastAddress = multicastEndpoint;
418 :
419 1 : if (_pRpcReceiver != nullptr)
420 : {
421 1 : (void)_pRpcReceiver->requestMulticastReception(multicastEndpoint);
422 : }
423 : }
424 : }
425 : }
426 : }
427 4 : }
428 :
429 1 : void QueryManager::subscribeNackReceived(
430 : ServiceDescription const& service, ::ip::IPAddress const& sourceAddress)
431 : {
432 1 : ServiceQuery sq = make<ServiceQuery>();
433 1 : sq.description = service;
434 : LessThanComparator comp;
435 : QueryList::const_iterator queryItr = etl::find_if(
436 1 : _queries.cbegin(),
437 0 : _queries.cend(),
438 3 : [&sq, &comp](ServiceQuery const* query) { return !comp(query, &sq); });
439 0 : if ((service.proto == proto::SD_L4_PROTO_TCP) && (queryItr != _queries.cend())
440 1 : && (matches(service, (*queryItr)->description)))
441 : {
442 0 : ServiceQuery* const query = *queryItr;
443 0 : if ((nullptr != query) && query->serviceDiscoveryAddress == sourceAddress)
444 : {
445 0 : TcpClientChannelValidator::CachedValidator tcpValidator(_tcpClientChannelValidator);
446 0 : tcpValidator.checkClientChannel(
447 0 : ::ip::IPEndpoint(service.ipAddress, service.port), query->description.port);
448 : }
449 : }
450 :
451 3 : while ((queryItr != _queries.cend()) && service.serviceId == (*queryItr)->description.serviceId
452 1 : && service.instanceId == (*queryItr)->description.instanceId
453 3 : && (*queryItr)->serviceDiscoveryAddress == sourceAddress)
454 : {
455 1 : (*queryItr)->subscriptionState = ServiceQuery::SubscriptionState::STATE_UNSUBSCRIBED;
456 1 : if ((nullptr != (*queryItr)->listener))
457 : {
458 1 : (*queryItr)->listener->serviceStatusChanged(
459 : service, IServiceListener::ServiceStatus::SERVICE_SUBSCRIPTION_NACK);
460 : }
461 1 : ++queryItr;
462 : }
463 1 : }
464 :
465 15 : void QueryManager::offerReceived(
466 : ServiceDescription const& service, ::ip::IPAddress const& sourceAddress)
467 : {
468 15 : ::etl::optional<TcpClientChannelValidator::CachedValidator> tcpValidator;
469 15 : if (service.proto == proto::SD_L4_PROTO_TCP)
470 : {
471 4 : tcpValidator.emplace(_tcpClientChannelValidator);
472 : }
473 :
474 : QueryList::const_iterator queryItr = etl::find_if(
475 15 : _queries.cbegin(),
476 0 : _queries.cend(),
477 33 : [serviceId = service.serviceId](ServiceQuery const* q)
478 48 : { return q && q->description.serviceId >= serviceId; });
479 30 : for (; queryItr != _queries.end(); ++queryItr)
480 : {
481 17 : ServiceQuery* const query = *queryItr;
482 17 : if (query != nullptr)
483 : {
484 17 : if (query->description.serviceId != service.serviceId)
485 : {
486 2 : break;
487 : }
488 30 : if (((query->state == ServiceQuery::ServiceQueryState::QUERY_REPETITION_PHASE)
489 11 : || (query->state == ServiceQuery::ServiceQueryState::QUERY_MAIN_PHASE))
490 26 : && isEventgroupOfService(query->description, service))
491 : {
492 7 : if ((ServiceQuery::SubscriptionState::STATE_UNSUBSCRIBED
493 6 : == query->subscriptionState)
494 4 : && (service.proto == proto::SD_L4_PROTO_TCP)
495 12 : && (!tcpValidator->isChannelEstablished(
496 6 : ::ip::IPEndpoint(service.ipAddress, service.port),
497 2 : query->description.port)))
498 : {
499 : // cannot subscribe without data channel
500 1 : continue;
501 : }
502 :
503 5 : if (ServiceQuery::SubscriptionState::STATE_WAITING_FOR_ACK
504 5 : == query->subscriptionState)
505 : {
506 2 : if (_pServiceAnnouncer != nullptr)
507 : {
508 2 : _pServiceAnnouncer->unsubscribe(
509 2 : query->description, query->serviceDiscoveryAddress);
510 : }
511 : }
512 : else
513 : {
514 : // ServiceQuery::SubscriptionState::STATE_ACK_RECEIVED - subscription has been
515 : // made, updating existing
516 : }
517 5 : query->subscriptionState = ServiceQuery::SubscriptionState::STATE_WAITING_FOR_ACK;
518 5 : query->description.ipAddress = service.ipAddress;
519 5 : query->description.ttl = service.ttl;
520 5 : query->serviceDiscoveryAddress = sourceAddress;
521 :
522 5 : if (_pServiceAnnouncer != nullptr)
523 : {
524 5 : _pServiceAnnouncer->subscribe(
525 5 : query->description, query->serviceDiscoveryAddress);
526 : }
527 : }
528 : }
529 : }
530 15 : }
531 :
532 4 : void QueryManager::stopOfferReceived(ServiceDescription const& service) const
533 : {
534 : QueryList::const_iterator queryItr = etl::find_if(
535 4 : _queries.cbegin(),
536 0 : _queries.cend(),
537 8 : [serviceId = service.serviceId](ServiceQuery const* q)
538 12 : { return q && q->description.serviceId >= serviceId; });
539 8 : for (; queryItr != _queries.end(); ++queryItr)
540 : {
541 4 : ServiceQuery* const query = *queryItr;
542 4 : if (query != nullptr)
543 : {
544 4 : if (query->description.serviceId != service.serviceId)
545 : {
546 0 : break;
547 : }
548 8 : if (((query->state == ServiceQuery::ServiceQueryState::QUERY_INITIAL_WAIT_PHASE)
549 0 : || (query->state == ServiceQuery::ServiceQueryState::QUERY_REPETITION_PHASE))
550 4 : && (!containsEventGroup(query->description))
551 8 : && matches(query->description, service))
552 : {
553 : query->state
554 1 : = ServiceQuery::ServiceQueryState::QUERY_MAIN_PHASE; // don't send FINDs
555 : // after a StopOffer
556 : }
557 : }
558 : }
559 4 : }
560 :
561 206 : bool QueryManager::LessThanComparator::operator()(
562 : ServiceQuery const* const lhs, ServiceQuery const* const rhs) const
563 : {
564 206 : if ((lhs != nullptr) && (rhs != nullptr))
565 : {
566 206 : uint16_t const lhsServiceId = lhs->description.serviceId;
567 206 : uint16_t const rhsServiceId = rhs->description.serviceId;
568 206 : if (lhsServiceId != rhsServiceId)
569 : {
570 35 : return lhsServiceId < rhsServiceId;
571 : }
572 :
573 171 : uint16_t const lhsInstanceId = lhs->description.instanceId;
574 171 : uint16_t const rhsInstanceId = rhs->description.instanceId;
575 171 : if (lhsInstanceId != rhsInstanceId)
576 : {
577 0 : return lhsInstanceId < rhsInstanceId;
578 : }
579 :
580 171 : uint16_t const lhsEventGroup = lhs->description.eventGroup;
581 171 : uint16_t const rhsEventGroup = rhs->description.eventGroup;
582 171 : if (lhsEventGroup != rhsEventGroup)
583 : {
584 16 : return lhsEventGroup < rhsEventGroup;
585 : }
586 :
587 155 : uint8_t const lhsMajorVersion = lhs->description.majorVersion;
588 155 : uint8_t const rhsMajorVersion = rhs->description.majorVersion;
589 155 : if (lhsMajorVersion != rhsMajorVersion)
590 : {
591 0 : return lhsMajorVersion < rhsMajorVersion;
592 : }
593 :
594 155 : return lhs->timestamp < rhs->timestamp;
595 : }
596 0 : return false;
597 : }
598 :
599 0 : bool QueryManager::ServiceIdComparator::operator()(
600 : ServiceQuery const* const lhs, service_id::type const rhs) const
601 : {
602 0 : if (lhs != nullptr)
603 : {
604 0 : return lhs->description.serviceId < rhs;
605 : }
606 :
607 0 : return false;
608 : }
609 :
610 : } // namespace someip
611 :
612 : // NOLINTEND(cppcoreguidelines-pro-type-vararg)
|
