path: root/Master/Real-Time Systems/mki/src/libcanio.cpp

//============================================================================
// 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;
}