add new repoHEADmaster

1 files changed, 680 insertions, 0 deletions

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;
+}
+
+