1 // Copyright (C) 2016-2017 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
2 // This Source Code Form is subject to the terms of the Mozilla Public
3 // License, v. 2.0. If a copy of the MPL was not distributed with this
4 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
5
6 #include <vsomeip/vsomeip.hpp>
7 #include <vsomeip/internal/logger.hpp>
8
9 #include "../implementation/service_discovery/include/constants.hpp"
10 #include "../implementation/utility/include/byteorder.hpp"
11
12 #include <cstdint>
13 #include <thread>
14 #include <cstring>
15 #include <condition_variable>
16 #include <iomanip>
17 #include <iostream>
18 #include <sstream>
19
20 namespace vsomeip_ctrl {
21
22 class vsomeip_sender {
23 public:
vsomeip_sender(bool _use_tcp,const std::vector<vsomeip::byte_t> & _user_message,vsomeip::instance_t _instance)24 vsomeip_sender(bool _use_tcp,
25 const std::vector<vsomeip::byte_t>& _user_message,
26 vsomeip::instance_t _instance) :
27 use_tcp_(_use_tcp),
28 user_message_(_user_message),
29 instance_(_instance),
30 app_(vsomeip::runtime::get()->create_application("vsomeip_ctrl")),
31 wait_service_available_(true),
32 send_thread_(std::bind(&vsomeip_sender::send, this)),
33 service_id_(0x0),
34 method_id_(0x0),
35 length_(0),
36 client_id_(0x0),
37 interface_version_(0x0),
38 message_type_(vsomeip::message_type_e::MT_UNKNOWN),
39 return_code_(vsomeip::return_code_e::E_UNKNOWN),
40 wait_for_answer_(true)
41 {
42 if (user_message_.size() < VSOMEIP_PAYLOAD_POS) {
43 VSOMEIP_ERROR << "Provided message is to short, min. length "
44 "is 16 Bytes, exiting.";
45 exit(EXIT_FAILURE);
46 }
47 service_id_ = VSOMEIP_BYTES_TO_WORD(user_message_[VSOMEIP_SERVICE_POS_MIN],
48 user_message_[VSOMEIP_SERVICE_POS_MAX]);
49 method_id_ = VSOMEIP_BYTES_TO_WORD(user_message_[VSOMEIP_METHOD_POS_MIN],
50 user_message_[VSOMEIP_METHOD_POS_MAX]);
51 length_ = VSOMEIP_BYTES_TO_LONG(user_message_[VSOMEIP_LENGTH_POS_MIN],
52 user_message_[VSOMEIP_LENGTH_POS_MIN+1],
53 user_message_[VSOMEIP_LENGTH_POS_MIN+2],
54 user_message_[VSOMEIP_LENGTH_POS_MAX]);
55 client_id_ = VSOMEIP_BYTES_TO_WORD(user_message_[VSOMEIP_CLIENT_POS_MIN],
56 user_message_[VSOMEIP_CLIENT_POS_MAX]);
57 interface_version_ = user_message_[VSOMEIP_INTERFACE_VERSION_POS];
58 message_type_ = static_cast<vsomeip::message_type_e>(user_message_[VSOMEIP_MESSAGE_TYPE_POS]);
59 return_code_ = static_cast<vsomeip::return_code_e>(user_message_[VSOMEIP_RETURN_CODE_POS]);
60
61 validate_message();
62
63 if (!app_->init()) {
64 VSOMEIP_ERROR << "Couldn't initialize application";
65 exit(EXIT_FAILURE);
66 }
67 app_->register_state_handler(
68 std::bind(&vsomeip_sender::on_state, this,
69 std::placeholders::_1));
70 app_->register_message_handler(vsomeip::ANY_SERVICE,
71 vsomeip::ANY_INSTANCE, vsomeip::ANY_METHOD,
72 std::bind(&vsomeip_sender::on_message, this,
73 std::placeholders::_1));
74 app_->register_availability_handler(service_id_, instance_,
75 std::bind(&vsomeip_sender::on_availability, this,
76 std::placeholders::_1, std::placeholders::_2,
77 std::placeholders::_3));
78 app_->start();
79 };
80
stop(int _exit_code)81 void stop(int _exit_code) {
82 app_->clear_all_handler();
83 app_->release_service(service_id_, instance_);
84 app_->stop();
85 exit(_exit_code);
86 }
87
~vsomeip_sender()88 ~vsomeip_sender() {
89 send_thread_.join();
90 }
91
on_state(vsomeip::state_type_e _state)92 void on_state(vsomeip::state_type_e _state) {
93 VSOMEIP_INFO << "Application " << app_->get_name() << " is "
94 << (_state == vsomeip::state_type_e::ST_REGISTERED ?
95 "registered." : "deregistered.");
96
97 if (_state == vsomeip::state_type_e::ST_REGISTERED) {
98 app_->request_service(service_id_, instance_);
99 }
100 }
101
on_availability(vsomeip::service_t _service,vsomeip::instance_t _instance,bool _is_available)102 void on_availability(vsomeip::service_t _service,
103 vsomeip::instance_t _instance, bool _is_available) {
104 if(_is_available) {
105 VSOMEIP_INFO << "Service [" << std::setw(4) << std::setfill('0')
106 << std::hex << _service << "." << _instance << "] is available.";
107 std::lock_guard<std::mutex> its_lock(mutex_);
108 wait_service_available_ = false;
109 condition_.notify_one();
110 }
111 }
112
on_message(const std::shared_ptr<vsomeip::message> & _response)113 void on_message(const std::shared_ptr<vsomeip::message> &_response) {
114 VSOMEIP_INFO << "Received a response from Service [" << std::setw(4)
115 << std::setfill('0') << std::hex << _response->get_service()
116 << "." << std::setw(4) << std::setfill('0') << std::hex
117 << _response->get_instance() << "]:";
118 VSOMEIP_INFO << "########## begin message";
119 VSOMEIP_INFO << std::hex << std::setw(4) << std::setfill('0')
120 << _response->get_service()
121 << std::hex << std::setw(4) << std::setfill('0')
122 << _response->get_method()
123 << " # service id / instance id";
124 VSOMEIP_INFO << std::hex << std::setw(8) << std::setfill('0')
125 << _response->get_length() << " # length";
126 VSOMEIP_INFO << std::hex << std::setw(4) << std::setfill('0')
127 << _response->get_client()
128 << std::hex << std::setw(4) << std::setfill('0')
129 << _response->get_session()
130 << " # client id / session id";
131 VSOMEIP_INFO << std::hex << std::setw(2) << std::setfill('0')
132 << static_cast<std::uint16_t>(_response->get_protocol_version())
133 << std::hex << std::setw(2) << std::setfill('0')
134 << static_cast<std::uint16_t>(_response->get_interface_version())
135 << std::hex << std::setw(2) << std::setfill('0')
136 << static_cast<std::uint16_t>(_response->get_message_type())
137 << std::hex << std::setw(2) << std::setfill('0')
138 << static_cast<std::uint16_t>(_response->get_return_code())
139 << " # protocol version / interface version / "
140 << "message type / return code";
141
142
143 std::stringstream stream;
144 std::string str;
145 for(unsigned int i = 0; i < _response->get_payload()->get_length(); i++) {
146 stream << std::hex << std::setw(2) << std::setfill('0')
147 << static_cast<std::uint32_t>((_response->get_payload()->get_data())[i]);
148 str.append(stream.str());
149 stream.str("");
150 stream.clear();
151 }
152 std::string str2;
153 int k=1;
154 for(unsigned int j = 0; j < str.length(); j+=2, k++) {
155 str2.append(str.substr(j,2));
156 if(k%4 == 0) {
157 if(k == 4) {
158 VSOMEIP_INFO << str2 << " # payload from here on";
159 } else {
160 VSOMEIP_INFO << str2;
161 }
162 str2.clear();
163 }
164 }
165 VSOMEIP_INFO << "########## end message";
166 VSOMEIP_INFO << "Payload as byte stream: " << str;
167 str.clear();
168 std::lock_guard<std::mutex> its_lock(mutex_);
169 wait_for_answer_ = false;
170 condition_.notify_one();
171 }
172
send()173 void send() {
174 std::unique_lock<std::mutex> its_lock(mutex_);
175 while (wait_service_available_) {
176 if(std::cv_status::timeout == condition_.wait_for(its_lock, std::chrono::seconds(6))) {
177 VSOMEIP_INFO << "Service [" << std::setw(4) << std::setfill('0')
178 << std::hex << service_id_ << "." << instance_ << "] isn't available. Exiting";
179 stop(EXIT_FAILURE);
180 }
181 }
182
183 std::shared_ptr<vsomeip::message> its_message =
184 vsomeip::runtime::get()->create_message(use_tcp_);
185 its_message->set_method(method_id_);
186 its_message->set_service(service_id_);
187 its_message->set_interface_version(interface_version_);
188 its_message->set_message_type(message_type_);
189 its_message->set_return_code(return_code_);
190 its_message->set_client(app_->get_client());
191 its_message->set_instance(instance_);
192
193 std::shared_ptr< vsomeip::payload > its_payload =
194 vsomeip::runtime::get()->create_payload();
195 its_payload->set_data(&user_message_[VSOMEIP_PAYLOAD_POS],
196 static_cast<vsomeip::length_t>(user_message_.size() - VSOMEIP_PAYLOAD_POS));
197 its_message->set_payload(its_payload);
198 VSOMEIP_INFO << "Sending";
199 app_->send(its_message);
200
201 while(wait_for_answer_) {
202 if(std::cv_status::timeout == condition_.wait_for(its_lock, std::chrono::seconds(5))) {
203 VSOMEIP_INFO << "Didn't receive answer within 5sec. Shutting down.";
204 stop(EXIT_SUCCESS);
205 break;
206 }
207 }
208
209 stop(EXIT_SUCCESS);
210 }
211
212 private:
validate_message()213 bool validate_message() {
214 if (!check_message_type()) {
215 VSOMEIP_ERROR << "Invalid message type 0x" << std::setw(2)
216 << std::setfill('0') << std::hex
217 << static_cast<std::uint8_t>(message_type_) << ", exiting.";
218 stop(EXIT_FAILURE);
219 }
220
221 if(!check_return_code()) {
222 VSOMEIP_ERROR << "Invalid return code 0x" << std::setw(2)
223 << std::setfill('0') << std::hex
224 << static_cast<std::uint8_t>(return_code_) << ", exiting.";
225 stop(EXIT_FAILURE);
226 }
227
228 if (service_id_ == vsomeip::sd::service &&
229 method_id_ == vsomeip::sd::method) {
230 VSOMEIP_ERROR << "Usage of reserved service id and method id "
231 "of service discovery, exiting.";
232 stop(EXIT_FAILURE);
233 }
234
235 if (user_message_.size() != length_ + 8) {
236 VSOMEIP_ERROR << "Provided length 0x" << std::setw(8)
237 << std::setfill('0') << std::hex << length_
238 << " doesn't match message size.";
239 VSOMEIP_ERROR << "Assuming the same payload the length field should"
240 " be set to 0x" << std::setw(8) << std::setfill('0')
241 << std::hex << user_message_.size() - 8 << " , exiting.";
242 stop(EXIT_FAILURE);
243 }
244
245 if (use_tcp_ && user_message_.size() > VSOMEIP_MAX_TCP_MESSAGE_SIZE) {
246 VSOMEIP_WARNING << "Max allowed message size for TCP is "
247 << std::dec << VSOMEIP_MAX_TCP_MESSAGE_SIZE
248 << ". Provided message size is: " << user_message_.size();
249 }
250 if (!use_tcp_ && user_message_.size() > VSOMEIP_MAX_UDP_MESSAGE_SIZE) {
251 VSOMEIP_WARNING << "Max allowed message size for UDP is "
252 << std::dec << VSOMEIP_MAX_UDP_MESSAGE_SIZE
253 << ". Provided message size is: " << user_message_.size();
254 }
255 return true;
256 }
257
check_message_type()258 bool check_message_type() {
259 switch (message_type_) {
260 case vsomeip::message_type_e::MT_REQUEST:
261 case vsomeip::message_type_e::MT_REQUEST_NO_RETURN:
262 case vsomeip::message_type_e::MT_NOTIFICATION:
263 case vsomeip::message_type_e::MT_RESPONSE:
264 case vsomeip::message_type_e::MT_ERROR:
265 return true;
266 break;
267 case vsomeip::message_type_e::MT_UNKNOWN:
268 case vsomeip::message_type_e::MT_ERROR_ACK:
269 case vsomeip::message_type_e::MT_RESPONSE_ACK:
270 case vsomeip::message_type_e::MT_NOTIFICATION_ACK:
271 case vsomeip::message_type_e::MT_REQUEST_NO_RETURN_ACK:
272 case vsomeip::message_type_e::MT_REQUEST_ACK:
273 default:
274 return false;
275 break;
276 }
277 }
278
check_return_code()279 bool check_return_code() {
280 if (static_cast<std::uint8_t>(return_code_) > 0x3F) {
281 VSOMEIP_ERROR << "Provided return code 0x" << std::setw(2)
282 << std::setfill('0') << std::hex
283 << static_cast<std::uint8_t>(return_code_) << " is out of range.";
284 return false;
285 }
286 if (static_cast<std::uint8_t>(return_code_) >
287 static_cast<std::uint8_t>(vsomeip::return_code_e::E_WRONG_MESSAGE_TYPE) &&
288 static_cast<std::uint8_t>(return_code_) <= 0x3f) {
289 VSOMEIP_ERROR << "Provided return code 0x" << std::setw(2)
290 << std::setfill('0') << std::hex <<
291 static_cast<std::uint8_t>(return_code_) << "is reserved.";
292 return false;
293 }
294 switch (message_type_) {
295 case vsomeip::message_type_e::MT_REQUEST:
296 case vsomeip::message_type_e::MT_REQUEST_NO_RETURN:
297 case vsomeip::message_type_e::MT_NOTIFICATION:
298 if(return_code_ != vsomeip::return_code_e::E_OK) {
299 VSOMEIP_ERROR << "Provided return code 0x" << std::setw(2)
300 << std::setfill('0') << std::hex
301 << static_cast<std::uint8_t>(return_code_)
302 << "is invalid in combination with message type 0x"
303 << std::setw(2) << std::setfill('0') << std::hex
304 << static_cast<std::uint8_t>(message_type_)
305 << " use 0x00 (E_OK).";
306 return false;
307 }
308 return true;
309 break;
310 default:
311 return true;
312 break;
313 }
314 }
315
316 private:
317 bool use_tcp_;
318 std::vector<vsomeip::byte_t> user_message_;
319 vsomeip::instance_t instance_;
320 std::shared_ptr<vsomeip::application> app_;
321 std::mutex mutex_;
322 std::condition_variable condition_;
323 bool wait_service_available_;
324 std::thread send_thread_;
325 vsomeip::service_t service_id_;
326 vsomeip::method_t method_id_;
327 std::uint32_t length_;
328 vsomeip::client_t client_id_;
329 vsomeip::interface_version_t interface_version_;
330 vsomeip::message_type_e message_type_;
331 vsomeip::return_code_e return_code_;
332 bool wait_for_answer_;
333 };
334 } // namespace vsomeip_ctrl
335
print_help(char * binary_name)336 static void print_help(char* binary_name) {
337 std::cout << "Usage example:" << std::endl;
338 std::cout << binary_name << " --instance 5678 "
339 << "--message 123480e800000015134300030100000000000009efbbbf576f726c6400\n"
340 << "This will send a message to service with service id 1234 and instance 5678."
341 << std::endl << std::endl;
342 std::cout <<
343 "Available options:\n"
344 "--help | -h : print this help\n"
345 "--instance | -i : instance id of target service in hex (required)\n"
346 "--tcp | -t : flag to enable sending over TCP, default off (= UDP)\n"
347 "--message | -m : vSomeIP message to send in hex (required)\n\n"
348 "Please note: the fields client id and session id in the provided message\n"
349 "will be overwritten by the stack with the required values\n"
350 "Please further make sure to use the same configuration file\n"
351 "as the target service, if the system is not using routingmanagerd" << std::endl;
352 }
353
main(int argc,char ** argv)354 int main(int argc, char** argv) {
355 if (argc < 2) {
356 std::cerr << "To few arguments, please see the help with : "
357 << argv[0] << " --help" << std::endl;
358 exit(EXIT_FAILURE);
359 }
360
361 std::vector<vsomeip::byte_t> user_message;
362 vsomeip::instance_t instance(vsomeip::ANY_INSTANCE);
363 bool use_tcp(false);
364
365 for (int i = 1; i < argc; i++) {
366 std::string arg(argv[i]);
367 if (arg == "--help" || arg == "-h") {
368 print_help(argv[0]);
369 exit(EXIT_SUCCESS);
370 break;
371 } else if (arg == "--message" || arg == "-m") {
372 std::string message(argv[i + 1]);
373 for (unsigned int i = 0; i < message.length(); i += 2) {
374 vsomeip::byte_t its_byte;
375 try {
376 std::uint64_t tmp = std::stoul(message.substr(i, 2), 0, 16);
377 tmp = (tmp > (std::numeric_limits<std::uint8_t>::max)()) ?
378 (std::numeric_limits<std::uint8_t>::max)() : tmp;
379 its_byte = static_cast<vsomeip::byte_t>(tmp);
380 } catch (std::invalid_argument &e) {
381 std::cerr << e.what() << ": Couldn't convert '"
382 << message.substr(i, 2) << "' to hex, exiting: "
383 << std::endl;
384 exit(EXIT_FAILURE);
385 }
386 user_message.push_back(its_byte);
387 }
388 } if (arg == "--tcp" || arg == "-t") {
389 use_tcp = true;
390 } else if (arg == "--instance" || arg == "-i") {
391 std::string instance_str(argv[i + 1]);
392 if(instance_str.length() > 4) {
393 std::cerr << "provided instance is to long, exiting." << std::endl;
394 exit(EXIT_FAILURE);
395 }
396 if(instance_str.length() < 4) {
397 while(instance_str.size() != 4) {
398 instance_str = std::string("0") += instance_str;
399 }
400 }
401 vsomeip::byte_t high(0x0);
402 vsomeip::byte_t low(0x0);
403 std::cout << "Instance: " << instance_str << std::endl;
404 for (unsigned int i = 0; i < instance_str.length(); i += 2) {
405 try {
406 std::uint64_t tmp = std::stoul(instance_str.substr(i, 2), 0, 16);
407 tmp = (tmp > (std::numeric_limits<std::uint8_t>::max)()) ?
408 (std::numeric_limits<std::uint8_t>::max)() : tmp;
409
410 vsomeip::byte_t its_byte = static_cast<vsomeip::byte_t>(tmp);
411 if (i == 0) {
412 high = its_byte;
413 } else {
414 low = its_byte;
415 }
416 } catch (std::invalid_argument &e) {
417 std::cerr << e.what() << ": Couldn't convert '"
418 << instance_str.substr(i, 2) << "' to hex, exiting: "
419 << std::endl;
420 exit(EXIT_FAILURE);
421 }
422 }
423 instance = VSOMEIP_BYTES_TO_WORD(high, low);
424 }
425 }
426
427 if(instance == vsomeip::ANY_INSTANCE) {
428 std::cerr << "Please provide a target instance (see --help)" << std::endl;
429 exit(EXIT_FAILURE);
430 }
431
432 if(user_message.empty()) {
433 std::cerr << "Please provide a message to send (see --help)" << std::endl;
434 exit(EXIT_FAILURE);
435 }
436
437 vsomeip_ctrl::vsomeip_sender sender(use_tcp, user_message, instance);
438 return EXIT_SUCCESS;
439 }
440