From 33613a85afc4b1481367fbe92a17ee59c240250b Mon Sep 17 00:00:00 2001 From: Sven Eisenhauer Date: Fri, 10 Nov 2023 15:11:48 +0100 Subject: add new repo --- Master/Real-Time Systems/mki/src/libcanio.cpp | 680 ++++++++++++++++++++++++++ 1 file changed, 680 insertions(+) create mode 100644 Master/Real-Time Systems/mki/src/libcanio.cpp (limited to 'Master/Real-Time Systems/mki/src/libcanio.cpp') diff --git a/Master/Real-Time Systems/mki/src/libcanio.cpp b/Master/Real-Time Systems/mki/src/libcanio.cpp new file mode 100644 index 0000000..52cf12c --- /dev/null +++ b/Master/Real-Time Systems/mki/src/libcanio.cpp @@ -0,0 +1,680 @@ +//============================================================================ +// Name: +// libcanio.cpp +// +// Summary: +// A pretty simple library for the SocketCan layer, which trys to make +// the communication with the interfaces a bit easier, thus saving the +// tedious research time needed to get things up and running. +// +// Created on: +// Sep 24, 2010 +// +// Author: +// Christian Steiger +//============================================================================ + +// TODO CanIO interface - Read me first! +// As you can see there are a lot of "placeholder routine" TODOs in this file, +// normally filters can be set per setsocketopt, but not during runtime. +// Well, it was mentioned on the following page that this is possible... +// http://www.brownhat.org/docs/socketcan/llcf-api.html +// ...but i wasnt yet able to find the one command to do it, +// and my time for this project is rapidly running out. So the current +// filter implementation is a crude one in software that needs to be +// replaced with the real thing. Comments and function types already 'fit' +// more or less, so changes on the class interface shouldnt be neccesary. + +// Includes +#include "libcanio.h" + +// Constructor +CanIO::CanIO() +{ + // set default variables + m_socket = 0; + m_print_errors = 1; + m_filter = 0; + m_filter_size = 0; + m_filter_active = 0; +} + +// Destructor +CanIO::~CanIO() +{ + // properly close the socket + disconnect(); +} + +//============================================================================ +// Summary: +// Connects to an socket can interface +// Parameters: +// [in] interface - name of the can interface, e.g. can0, vcan0 +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::connect(const char* const interface) +{ + // disconnect if theres already an socket present + if (m_socket) + disconnect(); + + // create needed variables + struct sockaddr_can addr; + struct ifreq ifr; + int ret; + + // create a socket handler + m_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW); + + // invalid socket_handler? + if (m_socket < 0) + { + // print error message if wanted + if (m_print_errors) + perror("CanIO::connect, socket"); + + // return the result + return m_socket; + } + + // copy the name of the wanted interface into the ifreq struct + strcpy(ifr.ifr_name, interface); + + // and get the interface index via ioctl for further operations + ret = ioctl(m_socket, SIOCGIFINDEX, &ifr); + + // check for errors + if (ret < 0) + { + // prints errno message if wanted + if (m_print_errors) + perror("CanIO::connect, ioctl"); + + // close socket + disconnect(); + + // return the result + return ret; + } + + if (m_filter_active) + setsockopt( + m_socket, + SOL_CAN_RAW, + CAN_RAW_FILTER, + m_filter, + m_filter_size * sizeof(can_filter) + ); + + // copy the info we got from the ioctl call + addr.can_ifindex = ifr.ifr_ifindex; + + // set the protocol/implementation we want to use + addr.can_family = AF_CAN; + + // bind the socket to the device + ret = bind(m_socket, (struct sockaddr *)&addr, sizeof(addr)); + + // check for errors and prints errno message if wanted + if (ret < 0) + { + // print errormessage if wanted + if (m_print_errors) + perror("CanIO::connect, bind"); + + // close socket + disconnect(); + } + + // return the result + return ret; +} + +//============================================================================ +// Summary: +// Disconnects from a socket can interface +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::disconnect(void) +{ + // just abort if there isnt a socket + if (!m_socket) + return 0; + + // close socket connection + int ret = close(m_socket); + + // set m_socket to 0 + m_socket = 0; + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::disconnect, close"); + + // return the result + return ret; +} + +//============================================================================ +// Summary: +// Reads a can message from the device. The read ist blocking, +// if there arent new messages it will wait until one is available! +// Parameters: +// [in] frame - pointer to a can_frame where the message data is stored. +// take a look in /usr/include/linux/can.h for more details. +// The message type is also in the can_id field, use the masks +// CAN_EFF_FLAG, CAN_RTR_FLAG and CAN_ERR_FLAG to determine it. +// [in] timestamp - a pointer to a timeval struct, contains sec and usec. +// [out] 0 on success, -1 on failure (sets errno), -2 on incomplete frames. +//============================================================================ +ssize_t CanIO::readMsg(can_frame* frame, timeval* timestamp) +{ + // detect nullpointer + if (!frame) + { + // set errno, invalid argument + errno = EINVAL; + + // print error message if wanted + if (m_print_errors) + perror("CanIO::readMsg"); + + // return the failure + return -1; + } + + // create temporary variables + ssize_t nbytes; + + // look at the placeholder comment below + readmsg_loop: + + // read a can message from the bus + nbytes = read(m_socket, frame, sizeof(can_frame)); + + // check for errors + if (nbytes < 0) + { + // print errno message if wanted + if (m_print_errors) + perror("CanIO::readMsg, read"); + + // return the result + return nbytes; + } + + // check for a complete frame + // note: we cant fetch missing bytes, each read, even with a byte length of 1, + // will get the starting bytes of a new message instead of the missing ones. + if (nbytes < (signed int)sizeof(can_frame)) + { + // print error message if wanted + if (m_print_errors) + fprintf(stderr, "CanIO::readMsg, read: incomplete CAN frame.\n"); + + // returns -2 for error indication + return -2; + } + + // check if a timestamp is wanted + if ( timestamp ) + { + // misuse the nbyte variable to get ioctls result + nbytes = ioctl(m_socket, SIOCGSTAMP, timestamp); + + // was there an error? + if (nbytes < 0) + { + // print error if wanted + if (m_print_errors) + perror("CanIO::readMsg, ioctl"); + + // returns the result + return nbytes; + } + } + + // TODO CanIO::ReadMsg - Placeholder, remove this part when you rewrite the filter functions. + // Its a simple goto command, ugly, but saves code. Dont forget to remove the mark above too. + if (m_filter_active > 0) + while(!isVisible(frame->can_id)) + goto readmsg_loop; + + // everything ok, return 0 + return 0; +} + + +//============================================================================ +// Summary: +// Trys to write a can message to the device. +// Parameters: +// [in] frame - pointer to a can_frame struct which contains the message_data. +// [in] frame - pointer to a can_frame struct where the message data is stored. +// take a look in /usr/include/linux/can.h for more details. +// The message type is also in the can_id field, use the flags +// CAN_EFF_FLAG, CAN_RTR_FLAG and CAN_ERR_FLAG to set it. +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +ssize_t CanIO::writeMsg(const can_frame* const frame) +{ + // detect nullpointer + if (!frame) + { + // set errno, invalid argument + errno = EINVAL; + + // print error message if wanted + if (m_print_errors) + perror("CanIO::writeMsg"); + + // return the failure + return -1; + } + + // try to write to the device and return the result + int ret = write(m_socket, frame, sizeof(struct can_frame)); + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::writeMsg, write"); + + // return the result + return ret; +} + +//============================================================================ +// Summary: +// Dont print error messages on stderr. +// Useful if you do want to to the errorhandling yourself. +// Parameters: +// none +//============================================================================ +void CanIO::hideErrors(void) +{ + m_print_errors = 0; +} + +//============================================================================ +// Summary: +// Print error messages on stderr. +// Useful for debugging purposes, enabled per default. +// Parameters: +// none +//============================================================================ +void CanIO::showErrors(void) +{ + m_print_errors = 1; +} + + +//============================================================================ +// Summary: +// The driver is able to generate error frames which can be passed to the +// application in the same way like other can messages, +// This command enables this feature. +// Parameters: +// [in] mask - The errors are divided in different error classes which +// can be filtered with the appropiate masks. +// To get every error just ignore this argument, otherwise +// set the masks for the errors you would like to see. +// Take a look at /usr/include/linux/can/error.h for masks. +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::showCanErrors(const can_err_mask_t err_mask) +{ + // create temporary variables + can_err_mask_t mask = err_mask; + + // check the mask value + if (!mask) + mask = CAN_ERR_MASK; + + // return the results of setsockopt + int ret = setsockopt( + m_socket, + SOL_CAN_RAW, + CAN_RAW_ERR_FILTER, + &mask, + sizeof(mask) + ); + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::showCanErrors, setsockopt"); + + // return the result + return ret; + +} + +//============================================================================ +// Summary: +// The driver can generate error frames which can be passed to the +// application in the same way like other can messages, +// this command disables this feature (default). +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::hideCanErrors(void) +{ + // set the mask to zero + can_err_mask_t mask = 0; + + // return the results of setsockopt + int ret = setsockopt( + m_socket, + SOL_CAN_RAW, + CAN_RAW_ERR_FILTER, + &mask, + sizeof(mask) + ); + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::hideCanErrors, setsockopt"); + + // return the result + return ret; +} + +//============================================================================ +// Summary: +// TODO CanIO::enableLoopBack() - Describe me! +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::enableLoopBack(void) +{ + // create temporary variables + const int loopback = 1; + int ret; + + // set the loopback state + ret = setsockopt( + m_socket, + SOL_CAN_RAW, + CAN_RAW_LOOPBACK, + &loopback, + sizeof(loopback) + ); + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::enableLoopBack, setsockopt"); + + // return the result + return ret; +} + +//============================================================================ +// Summary: +// TODO CanIO::disableLoopBack() - Describe me! +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::disableLoopBack(void) +{ + // create temporary variables + const int loopback = 0; + int ret; + + // set the loopback state + ret = setsockopt( + m_socket, + SOL_CAN_RAW, + CAN_RAW_LOOPBACK, + &loopback, + sizeof(loopback) + ); + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::disableLoopBack, setsockopt"); + + // return the result + return ret; +} + + +//============================================================================ +// Summary: +// TODO CanIO::enableRecvOwnMsgs() - Describe me! +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::enableRecvOwnMsgs(void) +{ + // create temporary variables + const int recv_own_msgs = 1; + int ret; + + // set the loopback state + ret = setsockopt( + m_socket, + SOL_CAN_RAW, + CAN_RAW_RECV_OWN_MSGS, + &recv_own_msgs, + sizeof(recv_own_msgs) + ); + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::enableRecvOwnMsgs, setsockopt"); + + // return the result + return ret; +} + +//============================================================================ +// Summary: +// TODO CanIO::disableRecvOwnMsgs() - Describe me! +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +int CanIO::disableRecvOwnMsgs(void) +{ + // create temporary variables + const int recv_own_msgs = 0; + int ret; + + // set the loopback state + ret = setsockopt( + m_socket, + SOL_CAN_RAW, + CAN_RAW_RECV_OWN_MSGS, + &recv_own_msgs, + sizeof(recv_own_msgs) + ); + + // check for errors and prints errno message if wanted + if (ret < 0 && m_print_errors) + perror("CanIO::disableRecvOwnMsgs, setsockopt"); + + // return the result + return ret; +} + +//============================================================================ +// Summary: +// Adds a custom can id filter +// Parameters: +// [in] rfilter - Points to an can_filter struct which contains the filter. +// [in] size - The size of the rfilter. +// [out] 0 on success, -1 on failure, sets errno. +// +// Filter example: +// This shows messages with an id between 0x200 and 0x2FF: +// rfilter.can_id = 0x200; +// rfilter.can_mask = 0xF00; +// +// While this hides messages with in id between 0x200 and 0x2FF: +// rfilter.can_id = 0x200 | CAN_INV_FILTER; +// rfilter.can_mask = 0xF00; +// +// More filters can be combined like this: +// rfilter[0].can_id = 0x00001234; // exactly this EFF frame +// rfilter[0].can_mask = CAN_EFF_MASK; +// rfilter[1].can_id = 0x234; // exactly this SFF frame +// rfilter[1].can_mask = CAN_SFF_MASK; +//============================================================================ +// TODO CanIO::addFilter() - Placeholder routine, rewrite for setsockopt. +int CanIO::addFilter(const can_filter* const filter, const unsigned int size) +{ + // detect nullpointer and wrong size + if (!filter || size < sizeof(can_filter)) + { + // set errno, invalid argument + errno = EINVAL; + + // print error message if wanted + if (m_print_errors) + perror("CanIO::addFilter"); + + // return the failure + return -1; + } + + // calculate the arraysize + unsigned int arraysize = (size / sizeof(can_filter)); + + // if there arnt already rules defined + if (!m_filter) + { + // create a new array of pointers to can_filter + m_filter = new can_filter[arraysize]; + + // just copy the data + memcpy(m_filter, filter, size); + + // set the new arraysize + m_filter_size = arraysize; + } + + // if there are already rules, extend the set + else + { + // copy the pointer from the old array + can_filter *temp = m_filter; + + // create a new one, with the size of the two rulesets + m_filter = new can_filter[ arraysize + m_filter_size ]; + + // copy the old one + memcpy(m_filter, temp, m_filter_size * sizeof(can_filter)); + + // copy the new one after the end of the old one + memcpy(m_filter + m_filter_size, filter, size); + + // set the new filter size + m_filter_size += arraysize; + + // delete the content of the old pointer + delete temp; + } + + // enable the filter + if (!m_filter_active) + m_filter_active = 1; + + // everything okay + return 0; +} + +//============================================================================ +// Summary: +// Enables the filter, put already stored filter rules back to work. +// addFilter does this automatically, use disableFilter to prevent it. +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +// TODO CanIO:enableFilter() - Placeholder routine, rewrite for setsockopt. +int CanIO::enableFilter(void) +{ + // sets filter activity + m_filter_active = 1; + + // placeholder for setsockopt returns + return 0; +} + +//============================================================================ +// Summary: +// Disables the Filter, but keep the filter rules in the background. +// Stays disabled until you call enableFilter, regardless of addFilter calls. +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +// TODO CanIO:disableFilter() - Placeholder routine, rewrite for setsockopt. +int CanIO::disableFilter(void) +{ + // sets filter activity + m_filter_active = -1; + + // placeholder for setsockopt returns + return 0; +} + +//============================================================================ +// Summary: +// Simply resets the filter. +// Parameters: +// [out] 0 on success, -1 on failure, sets errno. +//============================================================================ +// TODO CanIO::clearFilter() - Placeholder routine, rewrite for setsockopt. +int CanIO::clearFilter(void) +{ + // reset filter size + m_filter_size = 0; + + // delete only when there is a pointer + if (m_filter) + delete m_filter; + + // Everything okay + return 0; +} + +//============================================================================ +// Summary: +// Checks if the given can id is visible or not. +// Dont use this function, it will be obsolete when +// the filter functions get a better implementation +// Parameters: +// [in] id - The can id +// [out] True or false, depending on visibility +//============================================================================ +// TODO CanIO::isVisible() - Placeholder help routine, remove it when you rewrite the filter functions. +char CanIO::isVisible(const unsigned int id) +{ + // declare variables + short unsigned int i; + char visible = 0; + + // go through the filter rules + for (i = 0; i < m_filter_size; i++) + { + // make things much easier to read, + // gcc -O2 will take hopefully care of it + const unsigned int rule = (m_filter + i)->can_mask & (m_filter + i)->can_id; + const unsigned int input = (m_filter + i)->can_mask & id; + + // inverted filter? + if ((m_filter + i)->can_id & CAN_INV_FILTER) + { + if (rule != input) + return 1; + } + else + { + if (rule == input) + return 1; + } + } + // Everything okay + return visible; +} + + -- cgit v1.2.3