arbracan.c

CANopen源代码

/*********************************************************
 *                                                       *
 *  Advanced Ring Buffer Redundant Application for CAN   *
 *                                                       *
 *********************************************************
 *                      ArbraCan.c                       *
 *********************************************************
 * This program is free software; you can redistribute   *
 * it and/or modify it under the terms of the GNU General*
 * Public License as published by the Free Software      *
 * Foundation; either version 2 of the License, or (at   *
 * your option) any later version.                       *
 *                                                       *
 * This program is distributed in the hope that it will  *
 * be useful, but WITHOUT ANY WARRANTY; without even the *
 * implied warranty of MERCHANTABILITY or FITNESS FOR A  *
 * PARTICULAR PURPOSE.  See the GNU General Public       *
 * License for more details.                             *
 *                                                       *
 * You should have received a copy of the GNU General    *
 * Public License along with this program; if not, write *
 * to 	The Free Software Foundation, Inc.               *
 *	675 Mass Ave                                     *
 *	Cambridge                                        *
 *	MA 02139                                         *
 * 	USA.                                             *
 *********************************************************
 *                                                       *
 *      Author: Edouard TISSERANT                        *
 *      Contact: [e-mail]            *
 *      Version: 1.0                                     *
 *      Modification date:                               *
 *      Description:                                     *
 *-------------------------------------------------------*
 * This is the main functions of the ArbraCan layer      *
 *                                                       *
 *********************************************************/

#define ArbraCan_C
//
// Kernel includes
//
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/ioport.h>
#include <linux/fs.h>
#include <linux/pci.h>
#include <asm/uaccess.h>
#include <asm/irq.h>
#include <asm/io.h>


#ifdef __RTL__
#include <rtl_sched.h>
#include <rtl_posixio.h>
#include <rtl_mutex.h>
#endif

//
// Other Includes
//
#include <can.h>
#include <arbraCanHW.h>
#include <arbraCan.h>

//
// Compilation parameters
//
//#define DEBUG

//
// module parameters and descriptions
//
static char *canboard_name = NULL;
MODULE_PARM(canboard_name, "s");
MODULE_PARM_DESC(canboard_name, "Name of device");

static int can_rate=CAN_SPEED; // Value in the makefile
MODULE_PARM(can_rate, "1i");
MODULE_PARM_DESC(can_rate, "Speed (Kbauds)");

static int can_silent=0;
MODULE_PARM(can_silent, "1i");
MODULE_PARM_DESC(can_silent, "Be invisble on this bus");

static int can_nosort=0;
MODULE_PARM(can_nosort, "1i");
MODULE_PARM_DESC(can_nosort, "Don't sort paquet to respect CAL");

#ifdef DEBUG
static int can_debug=0;
MODULE_PARM(can_debug, "1i");
MODULE_PARM_DESC(can_debug, "Enable debugging messages");
#endif

MODULE_DESCRIPTION("canboard driver");
MODULE_AUTHOR("[e-mail]");
MODULE_LICENSE("GPL");

//
// Debugging Macros
//
#ifdef DEBUG
#define MSGK(string, args...) if Debug_Cond {printk("%s:%d:",__FILE__,__LINE__);printk(string, ##args);}
#else
#define MSGK(string, args...)
#endif

#define Debug_Cond ((1<<bus_nb) & can_debug)

//
// Globals vars.
//
// IRQ Reminder
static int can_irq = 0;

// Openner tables (because of Linux and RTLinux cohabitation)
#define NOT_OPENED 0
#define OPENED_USER 1
#define OPENED_RTL 2
int openner[CAN_BUS_COUNT] = {[0 ... CAN_BUS_COUNT-1] = NOT_OPENED};

// Half bus Mutex's
struct {
	spinlock_t spin;
	unsigned long irq_state;
	} bus_lock[CAN_BUS_COUNT][2] = {[0 ... CAN_BUS_COUNT-1]= { [0 ... 1] = {SPIN_LOCK_UNLOCKED, 0}}};

// Wait queues.
union dbl_wq{
	wait_queue_head_t l;
#ifdef __RTL__
	rtl_wait_t rt;
#endif
} can_wq[CAN_BUS_COUNT][2];

// Communication Object Priority stuff
const int COB_P[] = {
	0,	// 0-220 -> CMS obj priority 0
	1,	// 221-440 -> CMS obj priority 1
	2,
	3,
	4,
	5,
	6,
	7,	// 1541-1760 -> CMS obj priority 7
	7,	// 1760-
	7	//     -2200 -> NMT Node Guarding, LMT, DBT (arbitrary set to 7)
};

inline int abs(int a) {return a < 0 ? -a : a ;};
#define GET_PRIO(m) ((1 << bus_nb) & can_nosort) ? 0 : COB_P[abs((m).cob_id.w - 1) / 220]

// Buffer stuff
static Message x_buff[2][CAN_BUS_COUNT][PRIORITY_LEVELS][MESS_BUFF_SIZE];
static int x_buff_begin[2][CAN_BUS_COUNT][PRIORITY_LEVELS]={[0 ... 1]= {[0 ... CAN_BUS_COUNT-1]= {[0 ... PRIORITY_LEVELS-1] = 0}}};
static int x_buff_count[2][CAN_BUS_COUNT][PRIORITY_LEVELS]={[0 ... 1]= {[0 ... CAN_BUS_COUNT-1]= {[0 ... PRIORITY_LEVELS-1] = 0}}};

//
// OS dependent Declarations
//
#ifdef __RTL__		/* Rtlinux version of the driver*/

// we have to declare a special rtl_wait_sleep, where irq are not allowed, to be sure to catch irq when we need it.
int rtl_wait_sleep_irqrestore (rtl_wait_t *wait, spinlock_t *lock, unsigned long irq_state)
{
	pthread_t self = pthread_self();
	struct rtl_wait_struct wait_struct;

	self->abort = 0;
	self->abortdata = 0;

	wait_struct.magic = RTL_WAIT_MAGIC;
	wait_struct.waiter = self;
	wait_struct.next = wait->queue;
	wait->queue = &wait_struct;
	wait->p_lock = lock;

	RTL_MARK_SUSPENDED (self);

	rtl_spin_unlock_irqrestore(lock, irq_state);

	return rtl_schedule();
}

#define wait_event_rt(wq, condition)\
	do{\
		unsigned long irq_state;\
		rtl_spin_lock_irqsave(wq->p_lock, irq_state);\
		if(!condition){\
			do{\
				rtl_wait_sleep_irqrestore(wq, wq->p_lock, irq_state);\
				rtl_spin_lock_irqsave(wq->p_lock, irq_state);\
			}while(!condition);\
		}\
		rtl_spin_unlock_irqrestore(wq->p_lock, irq_state);\
	}while(0)

inline int Bus_Lock(int bus_nb, int dir){
	if(openner[bus_nb]==OPENED_RTL){
		rtl_spin_lock_irqsave(&(bus_lock[bus_nb][dir].spin), bus_lock[bus_nb][dir].irq_state);
		return 1;
		/*return rtl_spin_trylock(&(bus_lock[bus_nb][dir].spin));*/
	}else{
		spin_lock_irqsave(&(bus_lock[bus_nb][dir].spin), bus_lock[bus_nb][dir].irq_state);
		return 1;
	}
}

/*		rtl_spin_unlock(&(bus_lock[bus_nb][dir].spin));\*/
#define Bus_Unlock(bus_nb, dir)\
	if(openner[bus_nb]==OPENED_RTL){\
		rtl_spin_unlock_irqrestore(&(bus_lock[bus_nb][dir].spin), bus_lock[bus_nb][dir].irq_state);\
	}else{\
		spin_unlock_irqrestore(&(bus_lock[bus_nb][dir].spin), bus_lock[bus_nb][dir].irq_state);\
	}


// Wait For Event
#define Wait_For_Event(bus_nb, dir, condition)\
	if(openner[bus_nb]==OPENED_RTL){\
		wait_event_rt((&(can_wq[bus_nb][dir].rt)), condition);\
	}else{\
		wait_event_interruptible(can_wq[bus_nb][dir].l, condition);\
	}

#define Caught_Signal\
	openner[bus_nb]==OPENED_USER && signal_pending(current)

#define Wake_Up(bus_nb, dir)\
	if(openner[bus_nb]==OPENED_RTL){\
		rtl_wait_wakeup(&(can_wq[bus_nb][dir].rt));\
		rtl_schedule();\
	}else{\
		wake_up(&(can_wq[bus_nb][dir].l));\
	}

//IRQ stuff
#define OS_enable_irq(...) rtl_hard_enable_irq(__VA_ARGS__)
#define OS_disable_irq(...) rtl_hard_disable_irq(__VA_ARGS__)
static struct sigaction oldact;
inline int rtl_request_irq_sig(int irq,void (*function)(int)){
	struct sigaction act;
	act.sa_handler = function;
	act.sa_flags = SA_FOCUS;
	act.sa_focus = 1 << rtl_getcpuid();
	return sigaction (RTL_SIGIRQMIN + irq, &act, &oldact);
}
#define OS_request_irq(irq, function, ...) rtl_request_irq_sig(irq, function)
inline int rtl_free_irq_sig(int irq){
	return sigaction (RTL_SIGIRQMIN + irq, &oldact, NULL);
}
#define OS_free_irq(irq,...) rtl_free_irq_sig(irq)

#else			/* Normal Linux Kernel Driver*/

inline int Bus_Lock(int bus_nb, int dir){
	spin_lock_irqsave(&(bus_lock[bus_nb][dir].spin), bus_lock[bus_nb][dir].irq_state);
	return 1;
}
#define Bus_Unlock(bus_nb, dir)\
	spin_unlock_irqrestore(&(bus_lock[bus_nb][dir].spin), bus_lock[bus_nb][dir].irq_state)

// Wait For Event
#define Wait_For_Event(bus_nb, dir, condition)\
	wait_event_interruptible(can_wq[bus_nb][dir].l, condition)

#define Caught_Signal\
	signal_pending(current)

#define Wake_Up(bus_nb, dir)\
	wake_up(&(can_wq[bus_nb][dir].l))

//IRQ stuff
#define OS_enable_irq(...)
#define OS_disable_irq(...)
#define OS_request_irq(irq, function, ...) request_irq(irq, function,__VA_ARGS__)
#define OS_free_irq(irq,...) free_irq(irq,__VA_ARGS__)

#endif

//
// Set of inline functions to get buffers states
//
inline int Sum(int n, int *chose){
        int i;
        int t=0;
        for(i = 0; i < n; i++){
                t+=chose[i];
        }
        return t;
}

inline int Seat_Left_In_x(int dir, int bus_nb, int prio){
	int res;
	if(Bus_Lock(bus_nb, dir)){
		res = (x_buff_count[dir][bus_nb][prio] < MESS_BUFF_SIZE);
		Bus_Unlock(bus_nb, dir);
	} else {
		res = 0;
	}
	return res;
}

inline int Is_Out_Of_x(int dir, int bus_nb, int prio, int place){
	int res;
	if(Bus_Lock(bus_nb, dir)){
		res = (x_buff_begin[dir][bus_nb][prio] == ((place + 1) % MESS_BUFF_SIZE));
		Bus_Unlock(bus_nb, dir);
	} else {
		res = 0;
	}
	return res;
}

inline int Message_Pending(int dir, int bus_nb){
	int res;
	if(Bus_Lock(bus_nb, dir)){
		res = Sum(PRIORITY_LEVELS, x_buff_count[dir][bus_nb])>0;
		Bus_Unlock(bus_nb, dir);
	} else {
		res = 0;
	}
	return res;
}

//
// Put the message in the Tx Ring buffer and return where
// he have been placed if so.
//
int Put_In_x(int dir, int bus_nb, Message *arg)
{
	int prio = GET_PRIO(*arg);
	MSGK("Entering Put_In_x(%d,%d,%d)\n", bus_nb, dir, prio);

	if(x_buff_count[dir][bus_nb][prio] < MESS_BUFF_SIZE) {
		int place = (x_buff_begin[dir][bus_nb][prio] + x_buff_count[dir][bus_nb][prio]++) % MESS_BUFF_SIZE;
		MSGK("Prio: %d\n", prio);
		memcpy(
			&x_buff[dir][bus_nb][prio][place],
			(void *)arg, sizeof(Message));
		MSGK("Exiting (success) Put_In_x\n");
		return place;
	}
	MSGK("Exiting (fault) Put_In_x\n");
	return -EFAULT;
}

//
// Get the most important message (lowest priority number)
//
int Get_From_x(int dir, int bus_nb, Message *arg)
{
	int prio;
	MSGK("Entering Get_From_x(%d,%d)\n", bus_nb, dir);

	// Seek to non-empty buffer
	for(prio=0;prio < PRIORITY_LEVELS ; prio++){
		if(x_buff_count[dir][bus_nb][prio] > 0){
			MSGK("Prio: %d\n", prio);
			memcpy(
				arg,
				&x_buff[dir][bus_nb][prio][x_buff_begin[dir][bus_nb][prio]++],
				sizeof(Message));
			x_buff_begin[dir][bus_nb][prio] = x_buff_begin[dir][bus_nb][prio] % MESS_BUFF_SIZE;
			x_buff_count[dir][bus_nb][prio]--;
			MSGK("Exiting (success) Get_From_x\n");
			return 0;
		}
	}
	MSGK("Exiting (fault) Get_From_x\n");
	return -EFAULT;
}

//
// Put_In_x but get bus lock before and free it after
//
int Put_In_x_Lock(int dir, int bus_nb, Message *arg)
{
	int res;
	MSGK("Entering Put_In_x_Lock(%d,%d)\n", bus_nb, dir);
	if(Bus_Lock(bus_nb, dir)){
		res = Put_In_x(dir, bus_nb, arg);
		Bus_Unlock(bus_nb, dir);
	} else {
		res = -EAGAIN ;
	}
	return res;
}

//
// Get_From_x but get bus lock before and free it after
//
int Get_From_x_Lock(int dir, int bus_nb, Message *arg)
{
	int res;
	MSGK("Entering Get_From_x_Lock(%d,%d)\n", bus_nb, dir);
	if(Bus_Lock(bus_nb, dir)){
		res = Get_From_x(dir, bus_nb, arg);
		Bus_Unlock(bus_nb, dir);
	} else {
		res = -EAGAIN ;
	}
	return res;
}

//
// Get from Rx buffer of controller
// and put it in the fifo
//
void Update_Board_Rx(int bus_nb)
{
	Message m;

	if(Bus_Lock(bus_nb, Rx)){
		MSGK("Update_Board_Rx(%d)\n",bus_nb);
		while(CAN_RX_MSG_READY(bus_nb)){
			CAN_GET(bus_nb, &m);
			Put_In_x(Rx, bus_nb,&m);
			Wake_Up(bus_nb, Rx);
		}
		Bus_Unlock(bus_nb, Rx);
		MSGK("Fin Update_Board_Rx(%d)\n",bus_nb);
	}
}

//
// Get a message in the fifo and
// put it in Tx buffer of controller
//
void Update_Board_Tx(int bus_nb)
{
	Message m;

	if(Bus_Lock(bus_nb, Tx)){
		MSGK("Update_Board_Tx(%d)\n",bus_nb);