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