diff options
Diffstat (limited to 'low-can-binding/utils')
| -rw-r--r-- | low-can-binding/utils/config-parser.cpp | 67 | ||||
| -rw-r--r-- | low-can-binding/utils/config-parser.hpp | 44 | ||||
| -rw-r--r-- | low-can-binding/utils/openxc-utils.cpp | 351 | ||||
| -rw-r--r-- | low-can-binding/utils/openxc-utils.hpp | 47 | ||||
| -rw-r--r-- | low-can-binding/utils/signals.cpp | 77 | ||||
| -rw-r--r-- | low-can-binding/utils/signals.hpp | 86 | ||||
| -rw-r--r-- | low-can-binding/utils/socketcan-bcm.cpp | 101 | ||||
| -rw-r--r-- | low-can-binding/utils/socketcan-bcm.hpp | 53 | ||||
| -rw-r--r-- | low-can-binding/utils/socketcan-raw.cpp | 88 | ||||
| -rw-r--r-- | low-can-binding/utils/socketcan-raw.hpp | 35 | ||||
| -rw-r--r-- | low-can-binding/utils/socketcan.cpp | 97 | ||||
| -rw-r--r-- | low-can-binding/utils/socketcan.hpp | 71 | ||||
| -rw-r--r-- | low-can-binding/utils/timer.cpp | 129 | ||||
| -rw-r--r-- | low-can-binding/utils/timer.hpp | 58 |
14 files changed, 1304 insertions, 0 deletions
diff --git a/low-can-binding/utils/config-parser.cpp b/low-can-binding/utils/config-parser.cpp new file mode 100644 index 00000000..8ccf5bcb --- /dev/null +++ b/low-can-binding/utils/config-parser.cpp @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "config-parser.hpp" + +#include "../binding/low-can-hat.hpp" + +namespace utils +{ + /// @brief constructor using path to file + config_parser_t::config_parser_t(std::string conf_file) + : filepath_{conf_file}, config_content_{} + { + config_content_.read_file(conf_file); + } + + const std::string& config_parser_t::filepath() const + { + return filepath_; + } + + /// @brief read the conf_file_ and parse it into an INIReader object + /// to search into later. + bool config_parser_t::check_conf() + { + if (config_content_.size() <= 0) + { + ERROR("Can't load the INI config file."); + return false; + } + DEBUG("Configuration file parsed"); + return true; + } + + /// @brief Public method to access devices_name_ vector. If vector size equal 0 + /// then it will parses the configuration file content to fill it. It could be empty even + /// after parsing if content file just don't have a correct "canbus" directive so you + /// have to test the returned value. + /// + /// @return A const vector with string of linux CAN devices. + const std::vector<std::pair<std::string, std::string> > config_parser_t::get_devices_name() + { + std::vector<std::pair<std::string, std::string> > devices_name; + + std::map<std::string, std::string> bus_mapping = config_content_.get_keys("CANbus-mapping"); + for(const auto& busIt : bus_mapping ) + { + devices_name.push_back(std::make_pair(busIt.first, busIt.second)); + } + + return devices_name; + } +}
\ No newline at end of file diff --git a/low-can-binding/utils/config-parser.hpp b/low-can-binding/utils/config-parser.hpp new file mode 100644 index 00000000..3115e9bd --- /dev/null +++ b/low-can-binding/utils/config-parser.hpp @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include <string> +#include <vector> +#include <utility> +#include "ini-config.hpp" + +namespace utils +{ + /// @brief A configuration file parser that handle INI configuration + /// file format. + class config_parser_t + { + private: + const std::string filepath_; /*!< filepath_ - Path to the config file*/ + ini_config config_content_; /*!< config_content_ - Parsed content of INI file.*/ + + public: + config_parser_t(config_parser_t&&) = default; + config_parser_t(const config_parser_t&) = default; + explicit config_parser_t(std::string conf_file); + + const std::string& filepath() const; + bool check_conf(); + const std::vector<std::pair<std::string, std::string> > get_devices_name(); + }; +} diff --git a/low-can-binding/utils/openxc-utils.cpp b/low-can-binding/utils/openxc-utils.cpp new file mode 100644 index 00000000..f39f4184 --- /dev/null +++ b/low-can-binding/utils/openxc-utils.cpp @@ -0,0 +1,351 @@ +/* + * Copyright (C) 2015, 2016 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loic Collignon" <loic.collignon@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "openxc-utils.hpp" + +#include "../binding/application.hpp" + +/// +/// @brief Build a specific VehicleMessage containing a DiagnosticResponse. +/// +/// @param[in] request - Original request use to retrieve decoder and callback +/// @param[in] response - Response to the request that will be decoded if decoder set +/// and put into the DiagnosticResponse of the VehicleMessage. +/// @param[in] parsed_value - raw parsed value of the payload from CAN message +/// +/// @return a vehicle message including simple message that will be convert into +/// a JSON object before being pushed to the subscribers +/// +const openxc_VehicleMessage build_VehicleMessage(active_diagnostic_request_t* request, const DiagnosticResponse& response, float parsed_value) +{ + openxc_VehicleMessage message; + application_t& app = application_t::instance(); + + message.has_type = true; + message.type = openxc_VehicleMessage_Type::openxc_VehicleMessage_Type_DIAGNOSTIC; + message.has_diagnostic_response = true; + message.diagnostic_response.has_bus = true; + message.diagnostic_response.bus = app.get_can_bus_manager().get_can_device_index( + app.get_diagnostic_manager().get_bus_name()); + message.diagnostic_response.has_message_id = true; + + if(request->get_id() != OBD2_FUNCTIONAL_BROADCAST_ID) + { + message.diagnostic_response.message_id = response.arbitration_id + - DIAGNOSTIC_RESPONSE_ARBITRATION_ID_OFFSET; + } + else + { + // must preserve responding arb ID for responses to functional broadcast + // requests, as they are the actual module address and not just arb ID + + // 8. + message.diagnostic_response.message_id = response.arbitration_id; + } + + message.diagnostic_response.has_mode = true; + message.diagnostic_response.mode = response.mode; + message.diagnostic_response.has_pid = response.has_pid; + if(message.diagnostic_response.has_pid) + message.diagnostic_response.pid = response.pid; + message.diagnostic_response.has_success = true; + message.diagnostic_response.success = response.success; + message.diagnostic_response.has_negative_response_code = !response.success; + message.diagnostic_response.negative_response_code = + response.negative_response_code; + + if(response.payload_length > 0) + { + if(request->get_decoder() != nullptr) + { + message.diagnostic_response.has_value = true; + message.diagnostic_response.value = parsed_value; + } + else + { + message.diagnostic_response.has_payload = true; + ::memcpy(message.diagnostic_response.payload.bytes, response.payload, + response.payload_length); + message.diagnostic_response.payload.size = response.payload_length; + } + } + + return message; +} +/// +/// @brief Build a specific VehicleMessage containing a SimpleMessage with associated timestamp +/// +/// @param[in] message - simple message to include into openxc_VehicleMessage +/// @param[in] timestamp - timestamp from ioctl when reading the socket +/// +/// @return a vehicle message including simple message that will be convert into +/// a JSON object before being pushed to the subscribers +/// +const openxc_VehicleMessage build_VehicleMessage(const openxc_SimpleMessage& message, uint64_t timestamp) +{ + openxc_VehicleMessage v; + + v.has_type = true, + v.type = openxc_VehicleMessage_Type::openxc_VehicleMessage_Type_SIMPLE; + v.has_simple_message = true; + v.simple_message = message; + v.has_timestamp = true; + v.timestamp = timestamp; + + return v; +} + +/// +/// @brief Build a specific VehicleMessage containing a SimpleMessage. +/// +/// @param[in] message - simple message to include into openxc_VehicleMessage +/// +/// @return a vehicle message including simple message that will be convert into +/// a JSON object before being pushed to the subscribers +/// +const openxc_VehicleMessage build_VehicleMessage(const openxc_SimpleMessage& message) +{ + openxc_VehicleMessage v; + + v.has_type = true, + v.type = openxc_VehicleMessage_Type::openxc_VehicleMessage_Type_SIMPLE; + v.has_simple_message = true; + v.simple_message = message; + v.has_timestamp = true; + v.timestamp = system_time_us(); + + return v; +} + +/// +/// @brief Build an empty VehicleMessage that isn't usable by at least the struct +/// is initialized for the most part and can be use to check a false return value. +/// +/// @return A VehicleMessage with all boolean value to false. +/// +openxc_VehicleMessage build_VehicleMessage() +{ + openxc_VehicleMessage v; + + ::memset(&v, 0, sizeof(openxc_VehicleMessage)); + return v; +} + +bool is_valid(const openxc_VehicleMessage& v) +{ + if (v.has_type == false && + v.has_can_message == false && + v.has_simple_message == false && + v.has_diagnostic_response == false && + v.has_control_command == false && + v.has_command_response == false && + v.has_timestamp == false) + return false; + return true; +} + +/// +/// @brief Build an openxc_SimpleMessage associating a name to an openxc_DynamicField +/// +/// @param[in] name - const string reference name to assign to the created SimpleMessage +/// this will set has_name member to true and assign name to the name member. Maximum size for name is +/// set to 100 char. +/// @param[in] value - const reference with DynamicField to assign to SimpleMessage +/// value. +/// +/// @return an openxc_SimpleMessage struct initialized with name and value provided. +/// +const openxc_SimpleMessage build_SimpleMessage(const std::string& name, const openxc_DynamicField& value) +{ + openxc_SimpleMessage s; + + s.has_name = true; + ::strncpy(s.name, name.c_str(), 100); + s.has_value = true; + s.value = value; + + return s; +} + +/// +/// @brief Build an openxc_DynamicField with a string value +/// +/// @param[in] value - const string reference value to assign to builded +/// openxc_DynamicField. +/// +/// @return openxc_DynamicField initialized with a string value. +/// +const openxc_DynamicField build_DynamicField(const char* value) +{ + openxc_DynamicField d; + d.has_type = true; + d.type = openxc_DynamicField_Type_STRING; + + d.has_string_value = true; + d.has_numeric_value = false; + d.has_boolean_value = false; + ::strncpy(d.string_value, value, 100); + + return d; +} + +/// +/// @brief Build an openxc_DynamicField with a string value +/// +/// @param[in] value - const string reference value to assign to builded +/// openxc_DynamicField. +/// +/// @return openxc_DynamicField initialized with a string value. +/// +const openxc_DynamicField build_DynamicField(const std::string& value) +{ + openxc_DynamicField d; + d.has_type = true; + d.type = openxc_DynamicField_Type_STRING; + + d.has_string_value = true; + d.has_numeric_value = false; + d.has_boolean_value = false; + ::strncpy(d.string_value, value.c_str(), 100); + + return d; +} + +/// +/// @fn openxc_DynamicField build_DynamicField(double value); +/// +/// @brief Build an openxc_DynamicField with a double value +/// +/// @param[in] value - double value to assign to builded openxc_DynamicField. +/// +/// @return openxc_DynamicField initialized with a double value. +/// +const openxc_DynamicField build_DynamicField(double value) +{ + openxc_DynamicField d; + d.has_type = true; + d.type = openxc_DynamicField_Type_NUM; + + d.has_string_value = false; + d.has_numeric_value = true; + d.has_boolean_value = false; + d.numeric_value = value; + + return d; +} + +/// +/// @brief Build an openxc_DynamicField with a boolean value +/// +/// @param[in] value - boolean value to assign to builded openxc_DynamicField. +/// +/// @return openxc_DynamicField initialized with a boolean value. +/// +const openxc_DynamicField build_DynamicField(bool value) +{ + openxc_DynamicField d; + d.has_type = true; + d.type = openxc_DynamicField_Type_BOOL; + + d.has_string_value = false; + d.has_numeric_value = false; + d.has_boolean_value = true; + d.boolean_value = value; + + return d; +} + +int get_bool_from_DynamicField(const openxc_VehicleMessage& v_msg, bool* ret) +{ + if(v_msg.has_simple_message && v_msg.simple_message.has_value && v_msg.simple_message.value.has_boolean_value) + { + *ret = v_msg.simple_message.value.boolean_value; + return 0; + } + + return -1; +} + +double get_numerical_from_DynamicField(const openxc_VehicleMessage& v_msg) +{ + return (v_msg.has_simple_message && v_msg.simple_message.has_value && v_msg.simple_message.value.has_numeric_value) ? + v_msg.simple_message.value.numeric_value : -1.0; +} + +const std::string get_string_from_DynamicField(const openxc_VehicleMessage& v_msg) +{ +return (v_msg.has_simple_message && v_msg.simple_message.has_value && v_msg.simple_message.value.has_string_value) ? + v_msg.simple_message.value.string_value : ""; +} + +/// +/// @brief Extract the simple message value from an openxc_VehicleMessage +/// and return it. If there isn't SimpleMessage in the VehicleMessage then +/// returned value will be a SimpleMessage with all field set at false. +/// DynamicField from SimpleMessage will be boolean DynamicField set to false too. +/// +/// @param[in] v_msg - const reference to openxc_VehicleMessage +/// +/// @return A simpleMessage from the provided VehicleMessage. +/// +const openxc_SimpleMessage get_simple_message(const openxc_VehicleMessage& v_msg) +{ + if (v_msg.has_simple_message) + return v_msg.simple_message; + + openxc_SimpleMessage s_msg = { false, "", false, build_DynamicField(false), false, build_DynamicField(false)}; + return s_msg; +} + +/// +/// @brief Make a JSON object from a DynamicField +/// +/// @param[in] field - openxc_DynamicField struct to convert into +/// a json object. +/// @param[out] value - pointer to the object to set up. +/// +void jsonify_DynamicField(const openxc_DynamicField& field, json_object* value) +{ + if(field.has_numeric_value) + json_object_object_add(value, "value", json_object_new_double(field.numeric_value)); + else if(field.has_boolean_value) + json_object_object_add(value, "value", json_object_new_boolean(field.boolean_value)); + else if(field.has_string_value) + json_object_object_add(value, "value", json_object_new_string(field.string_value)); +} + +/// +/// @brief Make a JSON object from a SimpleMessage +/// +/// @param[in] s_msg - const reference to an openxc_SimpleMessage +/// struct to convert into a json object. +/// @param[out] json - pointer with the DynamicField converted into json object +/// +/// @return True if SimpleMessage has been transformed into json object +/// and false if not. In such case, a json object is returned { "error": "error msg"} +/// +bool jsonify_simple(const openxc_SimpleMessage& s_msg, json_object* json) +{ + if(s_msg.has_name) + { + json_object_object_add(json, "name", json_object_new_string(s_msg.name)); + jsonify_DynamicField(s_msg.value, json); + return true; + } + json_object_object_add(json, "error", json_object_new_string("openxc_SimpleMessage doesn't have name'")); + return false; +}
\ No newline at end of file diff --git a/low-can-binding/utils/openxc-utils.hpp b/low-can-binding/utils/openxc-utils.hpp new file mode 100644 index 00000000..83d63797 --- /dev/null +++ b/low-can-binding/utils/openxc-utils.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2015, 2016 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loic Collignon" <loic.collignon@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include <string> +#include <json-c/json.h> +#include <sys/timeb.h> + +#include "openxc.pb.h" +#include "../diagnostic/active-diagnostic-request.hpp" + +const openxc_VehicleMessage build_VehicleMessage(active_diagnostic_request_t* request, const DiagnosticResponse& response, float parsed_value); +const openxc_VehicleMessage build_VehicleMessage(const openxc_SimpleMessage& message, uint64_t timestamp); +const openxc_VehicleMessage build_VehicleMessage(const openxc_SimpleMessage& message); +openxc_VehicleMessage build_VehicleMessage(); +bool is_valid(const openxc_VehicleMessage& v); + +const openxc_SimpleMessage build_SimpleMessage(const std::string& name, const openxc_DynamicField& value); +const openxc_DynamicField build_DynamicField(const char* value); +const openxc_DynamicField build_DynamicField(const std::string& value); +const openxc_DynamicField build_DynamicField(double value); +const openxc_DynamicField build_DynamicField(bool value); + +int get_bool_from_DynamicField(const openxc_VehicleMessage& v_msg, bool& ret); +double get_numerical_from_DynamicField(const openxc_VehicleMessage& v_msg); +const std::string get_string_from_DynamicField(const openxc_VehicleMessage& v_msg); +const openxc_SimpleMessage get_simple_message(const openxc_VehicleMessage& v_msg); + +void jsonify_DynamicField(const openxc_DynamicField& field, json_object* value); + +bool jsonify_simple(const openxc_SimpleMessage& s_msg, json_object* json);
\ No newline at end of file diff --git a/low-can-binding/utils/signals.cpp b/low-can-binding/utils/signals.cpp new file mode 100644 index 00000000..fbc49044 --- /dev/null +++ b/low-can-binding/utils/signals.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2015, 2016 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "signals.hpp" + +namespace utils +{ + signals_manager_t::signals_manager_t() + {} + + /// @brief Return singleton instance of configuration object. + signals_manager_t& signals_manager_t::instance() + { + static signals_manager_t sm; + return sm; + } + + /// @brief Return Subscribed signals map mutex. + std::mutex& signals_manager_t::get_subscribed_signals_mutex() + { + return subscribed_signals_mutex_; + } + + /// + /// @brief return the subscribed_signals map. + /// + /// @return Map of subscribed signals. + std::map<int, std::shared_ptr<low_can_subscription_t> >& signals_manager_t::get_subscribed_signals() + { + return subscribed_signals_; + } + + /// + /// @fn std::vector<std::string> find_signals(const openxc_DynamicField &key) + /// @brief return signals name found searching through CAN_signals and OBD2 pid + /// + /// @param[in] key : can contain numeric or string value in order to search against + /// can signals or obd2 signals name. + /// + /// @return Vector of signals name found. + /// + struct signals_found signals_manager_t::find_signals(const openxc_DynamicField &key) + { + struct signals_found sf; + + switch(key.type) + { + case openxc_DynamicField_Type::openxc_DynamicField_Type_STRING: + lookup_signals_by_name(key.string_value, application_t::instance().get_all_can_signals(), sf.can_signals); + lookup_signals_by_name(key.string_value, application_t::instance().get_diagnostic_messages(), sf.diagnostic_messages); + break; + case openxc_DynamicField_Type::openxc_DynamicField_Type_NUM: + lookup_signals_by_id(key.numeric_value, application_t::instance().get_all_can_signals(), sf.can_signals); + lookup_signals_by_id(key.numeric_value, application_t::instance().get_diagnostic_messages(), sf.diagnostic_messages); + break; + default: + ERROR("wrong openxc_DynamicField specified. Use openxc_DynamicField_Type_NUM or openxc_DynamicField_Type_STRING type only."); + break; + } + DEBUG("Found %d signal(s)", (int)(sf.can_signals.size() + sf.diagnostic_messages.size())); + return sf; + } +}
\ No newline at end of file diff --git a/low-can-binding/utils/signals.hpp b/low-can-binding/utils/signals.hpp new file mode 100644 index 00000000..555c5dd6 --- /dev/null +++ b/low-can-binding/utils/signals.hpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2015, 2016 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include <vector> +#include <string> +#include <fnmatch.h> + +#include "openxc.pb.h" +#include "../binding/application.hpp" +#include "../can/can-signals.hpp" +#include "../diagnostic/diagnostic-message.hpp" + +#include "../binding/low-can-hat.hpp" +#include "../binding/low-can-subscription.hpp" + +namespace utils +{ + struct signals_found + { + std::vector<std::shared_ptr<can_signal_t> > can_signals; + std::vector<std::shared_ptr<diagnostic_message_t> > diagnostic_messages; + }; + + /// @brief Signal manager singleton hold subscription object with attached afb_event and its mutex + /// to read and write it safely. + /// It can be used to browse CAN signals and Diagnostic messages vectors and find a particular signal to + /// subscribe to. + class signals_manager_t + { + private: + std::mutex subscribed_signals_mutex_; + std::map<int, std::shared_ptr<low_can_subscription_t> > subscribed_signals_; ///< Map containing all subscribed signals, key is the socket int value. + + signals_manager_t(); ///< Private constructor to make singleton class. + + public: + static signals_manager_t& instance(); + + std::mutex& get_subscribed_signals_mutex(); + std::map<int, std::shared_ptr<low_can_subscription_t> >& get_subscribed_signals(); + + struct signals_found find_signals(const openxc_DynamicField &key); + void find_diagnostic_messages(const openxc_DynamicField &key, std::vector<std::shared_ptr<diagnostic_message_t> >& found_signals); + void find_can_signals(const openxc_DynamicField &key, std::vector<std::shared_ptr<can_signal_t> >& found_signals); + + template <typename T> + void lookup_signals_by_name(const std::string& key, std::vector<std::shared_ptr<T> > signals, std::vector<std::shared_ptr<T> >& found_signals) + { + for(std::shared_ptr<T> s : signals) + { + if(::fnmatch(key.c_str(), s->get_generic_name().c_str(), FNM_CASEFOLD) == 0) + found_signals.push_back(s); + else if(::fnmatch(key.c_str(), s->get_name().c_str(), FNM_CASEFOLD) == 0) + found_signals.push_back(s); + } + } + + template <typename T> + void lookup_signals_by_id(const double key, std::vector<std::shared_ptr<T> > signals, std::vector<std::shared_ptr<T> >& found_signals) + { + for(std::shared_ptr<T> s : signals) + { + if(application_t::instance().get_signal_id(*s) == key) + { + found_signals.push_back(s); + } + } + } + }; +}
\ No newline at end of file diff --git a/low-can-binding/utils/socketcan-bcm.cpp b/low-can-binding/utils/socketcan-bcm.cpp new file mode 100644 index 00000000..e16b246f --- /dev/null +++ b/low-can-binding/utils/socketcan-bcm.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loïc Collignon" <loic.collignon@iot.bzh> + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "socketcan-bcm.hpp" + +#include <net/if.h> +#include <sys/ioctl.h> +#include <fcntl.h> + +#include "../binding/application.hpp" + +namespace utils +{ + /// @brief Connect the socket. + /// @return 0 if success. + int socketcan_bcm_t::connect(const struct sockaddr* addr, socklen_t len) + { + return socket_ != INVALID_SOCKET ? ::connect(socket_, addr, len) : 0; + } + + /// @brief Open a raw socket CAN. + /// @param[in] device_name is the kernel network device name of the CAN interface. + /// + /// @return Upon successful completion, shall return a non-negative integer, the socket file descriptor. Otherwise, a value of -1 shall be returned and errno set to indicate the error. + int socketcan_bcm_t::open(std::string device_name) + { + close(); + + struct ifreq ifr; + socket_ = socketcan_t::open(PF_CAN, SOCK_DGRAM, CAN_BCM); + + // Attempts to open a socket to CAN bus + ::strcpy(ifr.ifr_name, device_name.c_str()); + DEBUG("BCM socket ifr_name is : %s", ifr.ifr_name); + if(::ioctl(socket_, SIOCGIFINDEX, &ifr) < 0) + { + ERROR("ioctl failed. Error was : %s", strerror(errno)); + close(); + } + else + { + tx_address_.can_family = AF_CAN; + tx_address_.can_ifindex = ifr.ifr_ifindex; + + if(connect((struct sockaddr *)&tx_address_, sizeof(tx_address_)) < 0) + { + ERROR("Connect failed. %s", strerror(errno)); + close(); + } + // Needed because of using systemD event loop. See sd_event_add_io manual. + fcntl(socketcan_t::socket_, F_SETFL, O_NONBLOCK); + } + return socket_; + } + + /// Read the socket to retrieve the associated CAN message. All the hard work is do into + /// convert_from_frame method and if there isn't CAN message retrieve, only BCM head struct, + /// then CAN message will be zeroed and must be handled later. + socketcan_bcm_t& operator>>(socketcan_bcm_t& s, can_message_t& cm) + { + struct utils::simple_bcm_msg msg; + + ::memset(&msg, 0, sizeof(msg)); + const struct sockaddr_can& addr = s.get_tx_address(); + socklen_t addrlen = sizeof(addr); + struct ifreq ifr; + + ssize_t nbytes = ::recvfrom(s.socket(), &msg, sizeof(msg), 0, (struct sockaddr*)&addr, &addrlen); + ifr.ifr_ifindex = addr.can_ifindex; + ioctl(s.socket(), SIOCGIFNAME, &ifr); + long unsigned int frame_size = nbytes-sizeof(struct bcm_msg_head); + + DEBUG("Data available: %li bytes read. BCM head, opcode: %i, can_id: %i, nframes: %i", frame_size, msg.msg_head.opcode, msg.msg_head.can_id, msg.msg_head.nframes); + DEBUG("read: Found on bus %s:\n id: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", ifr.ifr_name, msg.msg_head.can_id, msg.frames.can_dlc, + msg.frames.data[0], msg.frames.data[1], msg.frames.data[2], msg.frames.data[3], msg.frames.data[4], msg.frames.data[5], msg.frames.data[6], msg.frames.data[7]); + + struct timeval tv; + ioctl(s.socket(), SIOCGSTAMP, &tv); + uint64_t timestamp = 1000000 * tv.tv_sec + tv.tv_usec; + cm = ::can_message_t::convert_from_frame(msg.frames , + frame_size, + timestamp); + cm.set_sub_id((int)s.socket()); + + return s; + } +}
\ No newline at end of file diff --git a/low-can-binding/utils/socketcan-bcm.hpp b/low-can-binding/utils/socketcan-bcm.hpp new file mode 100644 index 00000000..97f093df --- /dev/null +++ b/low-can-binding/utils/socketcan-bcm.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loïc Collignon" <loic.collignon@iot.bzh> + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include "socketcan.hpp" +#include "../can/can-message.hpp" + +namespace utils +{ + struct simple_bcm_msg + { + struct bcm_msg_head msg_head; + struct can_frame frames; + }; + + struct canfd_bcm_msg + { + struct bcm_msg_head msg_head; + struct canfd_frame frames; + canfd_bcm_msg() { msg_head.flags |= CAN_FD_FRAME; } + }; + + /// @brief derivated socketcan class specialized for BCM CAN socket. + class socketcan_bcm_t : public socketcan_t + { + public: + using socketcan_t::socketcan_t; + + virtual int open(std::string device_name); + + private: + int connect(const struct sockaddr* addr, socklen_t len); + }; + + socketcan_bcm_t& operator>>(socketcan_bcm_t& s, can_message_t& cm); +// socketcan_bcm_t& operator<<(socketcan_bcm_t& s, const struct simple_bcm_msg& obj); +// socketcan_bcm_t& operator<<(socketcan_bcm_t& s, const struct canfd_bcm_msg& obj); +} diff --git a/low-can-binding/utils/socketcan-raw.cpp b/low-can-binding/utils/socketcan-raw.cpp new file mode 100644 index 00000000..31965c3d --- /dev/null +++ b/low-can-binding/utils/socketcan-raw.cpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loïc Collignon" <loic.collignon@iot.bzh> + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "socketcan-raw.hpp" + +#include <net/if.h> +#include <sys/ioctl.h> + +namespace utils +{ + /// @brief Bind the socket. + /// @return 0 if success. + int socketcan_raw_t::bind(const struct sockaddr* addr, socklen_t len) + { + return socket_ != INVALID_SOCKET ? ::bind(socket_, addr, len) : 0; + } + + /// @brief Open a raw socket CAN. + /// @param[in] device_name is the kernel network device name of the CAN interface. + /// + /// @return Upon successful completion, shall return a non-negative integer, the socket file descriptor. Otherwise, a value of -1 shall be returned and errno set to indicate the error. + int socketcan_raw_t::open(std::string device_name) + { + close(); + + struct ifreq ifr; + socket_ = socketcan_t::open(PF_CAN, SOCK_RAW, CAN_RAW); + + // Attempts to open a socket to CAN bus + ::strcpy(ifr.ifr_name, device_name.c_str()); + DEBUG("ifr_name is : %s", ifr.ifr_name); + if(::ioctl(socket_, SIOCGIFINDEX, &ifr) < 0) + { + ERROR("ioctl failed. Error was : %s", strerror(errno)); + close(); + } + else + { + tx_address_.can_family = AF_CAN; + tx_address_.can_ifindex = ifr.ifr_ifindex; + + if(bind((struct sockaddr *)&tx_address_, sizeof(tx_address_)) < 0) + { + ERROR("Bind failed. %s", strerror(errno)); + close(); + } + } + return socket_; + } + + socketcan_raw_t& operator>>(socketcan_raw_t& s, can_message_t& cm) + { + struct canfd_frame frame; + + const struct sockaddr_can& addr = s.get_tx_address(); + socklen_t addrlen = sizeof(addr); + struct ifreq ifr; + + ssize_t nbytes = ::recvfrom(s.socket(), &frame, sizeof(frame), 0, (struct sockaddr*)&addr, &addrlen); + ifr.ifr_ifindex = addr.can_ifindex; + ioctl(s.socket(), SIOCGIFNAME, &ifr); + + DEBUG("Data available: %i bytes read", (int)nbytes); + DEBUG("read: Found on bus %s:\n id: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", ifr.ifr_name, frame.can_id, frame.len, + frame.data[0], frame.data[1], frame.data[2], frame.data[3], frame.data[4], frame.data[5], frame.data[6], frame.data[7]); + + struct timeval tv; + ioctl(s.socket(), SIOCGSTAMP, &tv); + uint64_t timestamp = 1000000 * tv.tv_sec + tv.tv_usec; + cm = ::can_message_t::convert_from_frame(frame , nbytes, timestamp); + + return s; + } +}
\ No newline at end of file diff --git a/low-can-binding/utils/socketcan-raw.hpp b/low-can-binding/utils/socketcan-raw.hpp new file mode 100644 index 00000000..61672411 --- /dev/null +++ b/low-can-binding/utils/socketcan-raw.hpp @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loïc Collignon" <loic.collignon@iot.bzh> + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#pragma once + +#include "socketcan.hpp" +#include "../can/can-message.hpp" + +namespace utils +{ + class socketcan_raw_t : public socketcan_t + { + public: + using socketcan_t::socketcan_t; + virtual int open(std::string device_name); + + private: + int bind(const struct sockaddr* addr, socklen_t len); + }; + + socketcan_raw_t& operator>>(socketcan_raw_t& s, can_message_t& cm); +} diff --git a/low-can-binding/utils/socketcan.cpp b/low-can-binding/utils/socketcan.cpp new file mode 100644 index 00000000..71588a6e --- /dev/null +++ b/low-can-binding/utils/socketcan.cpp @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loïc Collignon" <loic.collignon@iot.bzh> + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include <unistd.h> +#include <string> +#include <linux/can/raw.h> +#include <net/if.h> +#include <sys/ioctl.h> + +#include "socketcan.hpp" + +namespace utils +{ + + /// @brief Construct a default, invalid, socket. + socketcan_t::socketcan_t() + : socket_{INVALID_SOCKET} + {} + + /// @brief Construct a socket by moving an existing one. + socketcan_t::socketcan_t(socketcan_t&& s) + : socket_{s.socket_} + { + ::memset(&tx_address_, 0, sizeof(tx_address_)); + } + + socketcan_t& socketcan_t::operator=(const socketcan_t& s) + { + socket_ = std::move(s.socket_); + return *this; + } + + socketcan_t::~socketcan_t() + { + close(); + socket_ = INVALID_SOCKET; + } + + const struct sockaddr_can& socketcan_t::get_tx_address() const + { + return tx_address_; + } + + /// @brief Test if socket is valid. + /// @return true if valid, false otherwise. + socketcan_t::operator bool() const + { + return socket_ != INVALID_SOCKET; + } + + /// @brief Open the socket. + /// @param[in] domain Specifies the communications domain in which a socket is to be created. + /// @param[in] type Specifies the type of socket to be created. + /// @param[in] protocol Specifies a particular protocol to be used with the socket. Specifying a protocol of 0 causes socket() to use an unspecified default protocol appropriate for the requested socket type. + /// @return Upon successful completion, shall return a non-negative integer, the socket file descriptor. Otherwise, a value of -1 shall be returned and errno set to indicate the error. + int socketcan_t::open(int domain, int type, int protocol) + { + close(); + socket_ = ::socket(domain, type, protocol); + return socket_; + } + + /// @brief Close the socket. + /// @return 0 if success. + int socketcan_t::close() + { + return socket_ != INVALID_SOCKET ? ::close(socket_) : 0; + } + + /// @brief Set socket option. + /// @return 0 if success. + int socketcan_t::setopt(int level, int optname, const void* optval, socklen_t optlen) + { + return socket_ != INVALID_SOCKET ? ::setsockopt(socket_, level, optname, optval, optlen) : 0; + } + + /// @brief Get the file descriptor. + /// @return The socket's file descriptor + int socketcan_t::socket() const + { + return socket_; + } +}
\ No newline at end of file diff --git a/low-can-binding/utils/socketcan.hpp b/low-can-binding/utils/socketcan.hpp new file mode 100644 index 00000000..82b797e0 --- /dev/null +++ b/low-can-binding/utils/socketcan.hpp @@ -0,0 +1,71 @@ +#pragma once + +/* + * Copyright (C) 2015, 2016 ,2017 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * Author "Loïc Collignon" <loic.collignon@iot.bzh> + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include <vector> + +#include <sys/socket.h> +#include <linux/can/bcm.h> +#include <string.h> + +#include "../binding/low-can-hat.hpp" + +#define INVALID_SOCKET -1 + +namespace utils +{ + class socketcan_t + { + public: + socketcan_t(); + socketcan_t(const socketcan_t& s) = delete; + socketcan_t(socketcan_t&&); + socketcan_t& operator=(const socketcan_t& s); + virtual ~socketcan_t(); + + const struct sockaddr_can& get_tx_address() const; + explicit operator bool() const; + + int socket() const; + virtual int open(std::string device_name) = 0; + int setopt(int level, int optname, const void* optval, socklen_t optlen); + int close(); + + protected: + int socket_; + struct sockaddr_can tx_address_; + + int open(int domain, int type, int protocol); + }; + + template <typename T> + socketcan_t& operator<<(socketcan_t& s, const std::vector<T>& vobj) + { + for(const auto& obj : vobj) + s << obj; + return s; + } + + template <typename T> + socketcan_t& operator<<(socketcan_t& s, const T& obj) + { + if (::sendto(s.socket(), &obj, sizeof(obj), 0, (const struct sockaddr*)&s.get_tx_address(), sizeof(s.get_tx_address())) < 0) + ERROR("Error sending : %i %s", errno, ::strerror(errno)); + return s; + } +} diff --git a/low-can-binding/utils/timer.cpp b/low-can-binding/utils/timer.cpp new file mode 100644 index 00000000..f3cdd5e7 --- /dev/null +++ b/low-can-binding/utils/timer.cpp @@ -0,0 +1,129 @@ +/* + * Copyright (C) 2015, 2016 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include <time.h> +#include <stdlib.h> +#include <cmath> + +#include "timer.hpp" + +long long int system_time_us() +{ + struct timespec t_usec; + long long int timestamp_usec = 0; + + if(!::clock_gettime(CLOCK_MONOTONIC, &t_usec)) + timestamp_usec = (t_usec.tv_nsec / 1000ll) + (t_usec.tv_sec* 1000000ll); + return timestamp_usec; +} + +long long int system_time_ms() +{ + struct timespec t_msec; + long long int timestamp_msec = 0; + + if(!::clock_gettime(CLOCK_MONOTONIC, &t_msec)) + timestamp_msec = (t_msec.tv_nsec / 1000000ll) + (t_msec.tv_sec* 1000ll); + return timestamp_msec; +} + +long long int system_time_s() +{ + struct timespec t_sec; + long long int timestamp_sec = 0; + + if(!::clock_gettime(CLOCK_MONOTONIC, &t_sec)) + timestamp_sec = t_sec.tv_sec; + return timestamp_sec; +} + +frequency_clock_t::frequency_clock_t() + : unit_{1000000}, frequency_{10.0}, last_tick_{0}, time_function_{nullptr} +{} + +frequency_clock_t::frequency_clock_t(float frequency) + : unit_{1000000}, frequency_{frequency}, last_tick_{0}, time_function_{nullptr} +{ + if(frequency_ <= 0) + frequency_ = 1; +} + +frequency_clock_t::frequency_clock_t(float frequency, uint64_t last_tick, time_function_t time_function) + : unit_{1000000}, frequency_{frequency}, last_tick_{0}, time_function_{nullptr} +{ + if(frequency_ <= 0) + frequency_ = 1; +} + +/// @brief Return the period in ms given the frequency in hertz. +/// @param[in] frequency - Frequency to convert, in hertz +float frequency_clock_t::frequency_to_period() const +{ + return frequency_ == 0 ? 0 : 1 / frequency_; +} + +/// @brief Return a timeval struct based on the frequency_ member. used to +/// specified CAN BCM timers. +const struct timeval frequency_clock_t::get_timeval_from_period() const +{ + struct timeval freq = {0, 0}; + float f; + freq.tv_usec = (long int)(std::modf(frequency_to_period(), &f) * unit_); + freq.tv_sec = (time_t)f; + + return freq; +} + +bool frequency_clock_t::started() +{ + return last_tick_ != 0; +} + +time_function_t frequency_clock_t::get_time_function() +{ + return time_function_ != nullptr ? time_function_ : system_time_us; +} + +bool frequency_clock_t::elapsed(bool stagger) +{ + float period = frequency_to_period(); + float elapsed_time = 0; + if(!started() && stagger) + last_tick_ = get_time_function()() - (rand() % int(period)); + + // Make sure it ticks the the first call + elapsed_time = !started() ? period : (float)get_time_function()() - (float)last_tick_; + + return frequency_ == 0 || elapsed_time >= period; +} + +float frequency_clock_t::get_frequency() const +{ + return frequency_; +} + +uint64_t frequency_clock_t::get_last_tick() const +{ + return last_tick_; +} + +/// @brief Force the clock to tick, regardless of it its time has actually +/// elapsed. +void frequency_clock_t::tick(uint64_t timestamp) +{ + last_tick_ = timestamp; +}
\ No newline at end of file diff --git a/low-can-binding/utils/timer.hpp b/low-can-binding/utils/timer.hpp new file mode 100644 index 00000000..a667022a --- /dev/null +++ b/low-can-binding/utils/timer.hpp @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2015, 2016 "IoT.bzh" + * Author "Romain Forlot" <romain.forlot@iot.bzh> + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include <sys/time.h> +#include <stdint.h> + +/// @brief return epoch in milliseconds +/// +/// @return long long int epoch in milliseconds +typedef long long int (*time_function_t)(); + +long long int system_time_us(); +long long int system_time_ms(); +long long int system_time_s(); + + +/// @brief A frequency counting clock. +/// Utility class allowing some time function. +class frequency_clock_t +{ +private: + float unit_; ///< unit_ - multiplicator to make operation to be in the right unit (milli, micro, nano, etc) + float frequency_; ///< the clock frequency in Hz. + uint64_t last_tick_; ///< the last time (in microseconds since startup) that the clock ticked. + time_function_t time_function_; ///< a function returning current time + +public: + frequency_clock_t(); + explicit frequency_clock_t(float frequency); + frequency_clock_t(float frequency, uint64_t last_tick, time_function_t time_function); + + float get_frequency() const; + const struct timeval get_timeval_from_period() const; + + float frequency_to_period() const; + bool started(); + time_function_t get_time_function(); + bool elapsed(bool stagger); + + uint64_t get_last_tick() const; + void tick(uint64_t timestamp); +};
\ No newline at end of file |