lpcmanager

Moved CanFestival plugin customization code to LPCCanFestival.py
revamp
2018-02-05, Edouard Tisserant
163b46d0412b
Moved CanFestival plugin customization code to LPCCanFestival.py
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <sys/select.h>
#include <native/task.h>
#include <native/mutex.h>
#include <native/timer.h>
#include "beremiz.h"
static struct termios oldterminfo;
void closeserial(int fd)
{
tcsetattr(fd, TCSANOW, &oldterminfo);
if (close(fd) < 0)
perror("closeserial()");
}
/*------------------------- GPIO -------------------------------------*/
/* from armadeus/target/packages/as_devices/c/as_gpio* */
//#ifdef DEBUG
# define ERROR(fmt, ...) printf(fmt, ##__VA_ARGS__)
//#else
//# define ERROR(fmt, ...) /*fmt, ##__VA_ARGS__*/
//#endif
struct gpio_device {
int port_num;
int pin_file; /* pin file for 2.6.29 interface*/
};
struct gpio_device *RS485_GPIO_dev;
static int write_file_bool(int fd, int value)
{
int ret;
ret = write(fd, value?"1":"0", 1);
if (ret < 0) {
ERROR("write error\n");
return ret;
}
if (lseek(fd, 0, SEEK_SET) < 0) {
ERROR("lseek error\n");
return ret;
}
return ret;
}
#define BUFF_SIZE 256
static struct gpio_device *gpio_open(int aGpioNum)
{
struct gpio_device *dev;
int pin_file;
int export_file;
int gpio_dir_fd;
int retval;
char buf[BUFF_SIZE];
int ret = 0;
export_file = open("/sys/class/gpio/export", O_WRONLY);
if (export_file < 0) {
ERROR("Can't open /sys/class/gpio/export\nBe sure that gpiolib is under your kernel\n");
return NULL;
}
snprintf(buf, BUFF_SIZE, "%%d", aGpioNum);
retval = write(export_file, buf, strlen(buf));
close(export_file);
if (retval < 0) {
ERROR("/sys/class/gpio/export can't be written\n");
return NULL;
}
snprintf(buf, BUFF_SIZE, "/sys/class/gpio/gpio%%d/direction", aGpioNum);
gpio_dir_fd = open(buf, O_WRONLY);
if (gpio_dir_fd < 0) {
ERROR("Can't open gpio%%d direction\n", aGpioNum);
return NULL;
}
ret = write(gpio_dir_fd, "out", 3);
close(gpio_dir_fd);
if (ret < 0){
ERROR("Error writing direction\n");
return NULL;
}
snprintf(buf, BUFF_SIZE, "/sys/class/gpio/gpio%%d/value", aGpioNum);
pin_file = open(buf, O_RDWR);
if (pin_file < 0) {
ERROR("Can't export gpio number %%d\n", aGpioNum);
return NULL;
}
dev = malloc(sizeof(struct gpio_device));
if (dev == NULL) {
ERROR("Can't allocate gpio_device structure\n");
close(pin_file);
return NULL;
}
dev->port_num = aGpioNum;
dev->pin_file = pin_file;
return dev;
}
static int gpio_close(struct gpio_device *aDev)
{
int unexport_file;
char buf[BUFF_SIZE];
int retval;
if(aDev == NULL){
ERROR("device is NULL\n");
return -1;
}
unexport_file = open("/sys/class/gpio/unexport", O_WRONLY);
if (unexport_file < 0) {
ERROR("Can't open /sys/class/gpio/unexport\nBe sure that gpiolib is under your kernel\n");
return -1;
}
snprintf(buf, BUFF_SIZE, "%%d", aDev->port_num);
retval = write(unexport_file, buf, strlen(buf));
close(unexport_file);
if (retval < 0) {
ERROR("/sys/class/gpio/unexport can't be written\n");
return -1;
}
close(aDev->pin_file);
return 0;
}
static int gpio_set_pin_value(struct gpio_device *aDev, int aValue)
{
int pin_file = aDev->pin_file;
int retval;
retval = write_file_bool(pin_file, aValue);
if (retval < 0) {
ERROR("Can't write value\n");
close(pin_file);
return -1;
}
return aValue;
}
#define TransmitMode() gpio_set_pin_value(RS485_GPIO_dev, 1)
#define RecieveMode() gpio_set_pin_value(RS485_GPIO_dev, 0)
/*--------------------------- Serial Port handling ---------------------------*/
int openserial(char *devicename, unsigned long baudrate)
{
int fd;
struct termios attr;
speed_t baud = B115200; /* baud rate */
switch(baudrate)
{
case 19200:
baud = B19200;
break;
case 115200:
baud = B115200;
break;
default:
baud = B115200;
}
if ((fd = open(devicename, O_RDWR)) == -1) {
perror("openserial(): open()");
return 0;
}
if (tcgetattr(fd, &oldterminfo) == -1) {
perror("openserial(): tcgetattr()");
return 0;
}
attr = oldterminfo;
attr.c_oflag = 0;
attr.c_iflag = 0;
attr.c_lflag = 0;
attr.c_cflag = 0;
attr.c_cflag |= CREAD;
attr.c_cflag |= CLOCAL;
attr.c_cflag |= CS8 ; /* 8 bits */
/* no parity, 1 stop bit */
cfsetspeed(&attr, baud); /* baud rate */
if (tcflush(fd, TCIOFLUSH) == -1) {
perror("openserial(): tcflush()");
return 0;
}
if (tcsetattr(fd, TCSANOW, &attr) == -1) {
perror("initserial(): tcsetattr()");
return 0;
}
TransmitMode();
return fd;
}
#define EMPTY 0
#define LOCKED 1
#define FULL 2
#define MAX_UART_DEVICES 32
#define UART_BUFSIZE_SHORT 26 /* UART bus read & write buffer size - short buffer for LPC-2 */
#define UART_BUFSIZE_LONG 48 /* UART bus read & write buffer size - long buffer for LHC-2 */
#define UART_RETRY_NUM 0
char uartBufSize = UART_BUFSIZE_SHORT; /* Smarteh uart bus: buffer size */
unsigned long uartBaudrate = 19200; /* Smarteh uart bus: baudrate */
struct timeval uartTimeout = {0,40000}; /* Smarteh uart bus: timeout */
/* Tables containing information about connected devices on UART port
(initialized by Composer) */
unsigned char uartDev[MAX_UART_DEVICES][2];
unsigned char uartDevNum = MAX_UART_DEVICES;
/* Buffers for reading data from UART port devices */
typedef char uartDevReadBuf_t[MAX_UART_DEVICES][UART_BUFSIZE_LONG];
uartDevReadBuf_t uartDevReadBufA;
uartDevReadBuf_t uartDevReadBufB;
uartDevReadBuf_t *uartDevReadBuf_drv;
uartDevReadBuf_t *uartDevReadBuf_plc;
/* MC8 compatibility */
uartDevReadBuf_t uartDevReadBuf;
/* Buffers for writing data to UART port devices */
typedef char uartDevWriteBuf_t[MAX_UART_DEVICES][UART_BUFSIZE_LONG];
uartDevWriteBuf_t uartDevWriteBufA;
uartDevWriteBuf_t uartDevWriteBufB;
uartDevWriteBuf_t *uartDevWriteBuf_drv;
uartDevWriteBuf_t *uartDevWriteBuf_plc;
/* MC8 compatibility */
#define uartDevWriteBuf (*uartDevWriteBuf_drv)
int uartDevWriteBuf_plc_state;
int uartDevReadBuf_plc_state;
/* Buffers for communication statuses with UART port devices */
/* 2D arrays due to compatibility with Composer (LpcSmartehIDE) */
char uartCommErrCntBuf [MAX_UART_DEVICES][1];
char uartCommStatusBuf[MAX_UART_DEVICES][1];
/* Function interface definition for modules on UART (RS485) bus */
typedef unsigned char (*uartPortFunct)(char*, char*, char);
/* Table describing module positions on UART (RS485) bus (parsed from Composer data) */
uartPortFunct uartPortDevices[MAX_UART_DEVICES] = {NULL};
/* Table of timers (one for each position) */
static commTimer uartPortTim[MAX_UART_DEVICES];
static RT_TASK UART_task;
static RT_MUTEX UART_WriteMutex;
static RT_MUTEX UART_ReadMutex;
void UART_task_proc(void *arg)
{
static unsigned char i=0;
char commStat;
uint64_t actTime;
static uint64_t lastCommTime=0;
while (rt_task_sleep_until(TM_INFINITE) == -EINTR){
// Communicate only with initialised UART devices
if(uartPortDevices[i] != NULL)
{
// Timers for UART port modules
if(uartPortTim[i].status != TIM_DISABLED)
{
actTime = (uint64_t)rt_timer_read();
uartPortTim[i].actValue = actTime - uartPortTim[i].oldTime;
if((uartPortTim[i].actValue < uartPortTim[i].toValue)
|| (actTime - lastCommTime < 50000000))
uartPortTim[i].status = TIM_EN_RUNNING;
else {
uartPortTim[i].status = TIM_EN_EXPIRED;
uartPortTim[i].oldTime = actTime;
lastCommTime = actTime;
}
}
// Check timer status
if((uartPortTim[i].status == TIM_DISABLED)
|| (uartPortTim[i].status == TIM_EN_EXPIRED))
{
if(!rt_mutex_acquire(&UART_WriteMutex, TM_INFINITE )){
if(uartDevWriteBuf_plc_state == FULL){
uartDevWriteBuf_t *uartDevWriteBuf_tmp;
uartDevWriteBuf_tmp = uartDevWriteBuf_plc;
uartDevWriteBuf_plc = uartDevWriteBuf_drv;
uartDevWriteBuf_drv = uartDevWriteBuf_tmp;
uartDevWriteBuf_plc_state = EMPTY;
}
rt_mutex_release(&UART_WriteMutex);
}
// Communicate with device
commStat = (*uartPortDevices[i])(
&uartDevReadBuf[i][0],
&uartDevWriteBuf[i][0],
uartDev[i][1]);
memcpy(uartDevReadBuf_drv, &uartDevReadBuf, sizeof(uartDevReadBuf_t));
if(!rt_mutex_acquire(&UART_ReadMutex, TM_INFINITE )){
if(uartDevReadBuf_plc_state == EMPTY){
uartDevReadBuf_t *uartDevReadBuf_tmp;
uartDevReadBuf_tmp = uartDevReadBuf_plc;
uartDevReadBuf_plc = uartDevReadBuf_drv;
uartDevReadBuf_drv = uartDevReadBuf_tmp;
uartDevReadBuf_plc_state = FULL;
}
rt_mutex_release(&UART_ReadMutex);
}
// Check communication status:
if(commStat == TRUE)
{
uartCommStatusBuf[i][0] = TRUE;
uartCommErrCntBuf[i][0] = 0;
}
else
{
if(uartCommErrCntBuf[i][0] < UART_RETRY_NUM)
uartCommErrCntBuf[i][0]++;
else
uartCommStatusBuf[i][0] = FALSE;
}
// If timer is enabled, reset it's value,
// otherwise keep it disabled
if(uartPortTim[i].status != TIM_DISABLED)
{
uartPortTim[i].actValue = 0;
uartPortTim[i].status = TIM_EN_RUNNING;
}
else /* Keep timer disabled */
uartPortTim[i].status = TIM_DISABLED;
// Procede with next UART device only after
// communication with the current one is done
if(i < (uartDevNum-1))
i++;
else /* Go back to the first UART device */
i=0;
}
}
else i=0;
}
}
#define TAIL_LEN 3 /* Length of data tail in bytes */
/**************************************************************************//**
* Calculate checksum of a buffer
* @param [in] buffer Pointer to buffer
* @param [in] bufLen Buffer length
* @return Checksum values on buffer locations bufLen-TAIL_LEN &
* bufLen-TAIL_LEN+1
******************************************************************************/
void Checksum(unsigned char *buffer, unsigned char bufLen)
{
unsigned char i=0, j=0;
unsigned char checksum1=0, checksum2=0;
unsigned char checksum1Temp=0;
for(i=0;i<bufLen-TAIL_LEN;i++)
{
checksum1Temp=buffer[i];
for(j=0;j<8;j++) /* Compute number of '1' of whole buff. */
{
if((checksum1Temp & 0x01)>0)
checksum1++;
checksum1Temp = checksum1Temp >> 1;
}
checksum2 = checksum2 ^ buffer[i]; /* Compute XOR of whole buffer */
}
buffer[bufLen-TAIL_LEN] = checksum2; /* Write number of '1' to buffer */
buffer[bufLen-TAIL_LEN+1] = checksum1; /* Write XOR to buffer */
}
/**************************************************************************//**
* Check if checksum values of received buffer are valid
* @param [in] buffer Pointer to buffer
* @param [in] bufLen Buffer length
* @return TRUE if valid, otherwise FALSE
******************************************************************************/
unsigned char ChecksumValid(unsigned char *buffer, unsigned char bufLen)
{
unsigned char i=0, j=0;
unsigned char checksum1=0, checksum2=0;
unsigned char checksum1Temp=0;
for(i=0;i<bufLen-TAIL_LEN;i++)
{
checksum1Temp = buffer[i];
for(j=0;j<8;j++) /* Compute number of '1' of whole buff. */
{
if((checksum1Temp & 0x01)>0)
checksum1++;
checksum1Temp = checksum1Temp >> 1; /* Compute XOR of whole buffer */
}
checksum2 = checksum2 ^ buffer[i];
}
/* Check if computed checksums are the same as those in buffer (=> no error) */
if((checksum2==buffer[bufLen-TAIL_LEN]) && (checksum1==buffer[bufLen-TAIL_LEN+1]))
return TRUE;
else
return FALSE;
}
static int UART_fd;
/*************************************************************************//**
* Support for UART modules
* @param [in] readBuf Pointer to read buffer (for previously polled UART device!)
* @param [out] writeBuf Pointer to write buffer (for current device)
* @param [in] address UART device address
* @return TRUE if communication was successful, otherwise FALSE
*****************************************************************************/
unsigned char UARTDevice(char* readBuf, char* writeBuf, char address)
{
fd_set set;
struct timeval timeout;
int rv;
RTIME now;
int count = 0;
char tmp[uartBufSize];
/* Prepare transmit buffer */
memcpy(tmp+1,writeBuf,uartBufSize-TAIL_LEN-1);
tmp[0] = 'S';
tmp[uartBufSize-TAIL_LEN] = address;
Checksum((unsigned char*)tmp+1, uartBufSize);
tcflush(UART_fd, TCIOFLUSH);
now = rt_timer_read();
if(write(UART_fd, tmp, uartBufSize) != uartBufSize){
goto UARTDevfail;
}
/* Sleep until transmission completes + 0.5ms safety */
while(rt_task_sleep_until(
now + rt_timer_ns2ticks(
1000000000LL * uartBufSize * 10 / uartBaudrate
+ 500000)) == -EINTR);
/* Turn to listen mode*/
RecieveMode();
FD_ZERO(&set); /* clear the set */
FD_SET(UART_fd, &set); /* add our file descriptor to the set */
timeout.tv_sec = uartTimeout.tv_sec;
timeout.tv_usec = uartTimeout.tv_usec;
while(count < uartBufSize){
rv = select(UART_fd + 1, &set, NULL, NULL, &timeout);
if(rv == -1)
printf("RS485 select error\n");
else if(rv == 0)
{
//printf("timeout\n");
break;
}
else {
int rr = read(UART_fd, tmp + count, uartBufSize - count);
if(rr > 0){
count += rr;
}else{
printf("RS485 read error %%d\n",rr);
}
}
}
/* Turn to transmit mode*/
TransmitMode();
/* Copy received buffer */
if((count == uartBufSize) && (tmp[uartBufSize-TAIL_LEN] == address)){
if(ChecksumValid((unsigned char*)tmp+1, uartBufSize)){
memcpy(readBuf,tmp+1,uartBufSize-TAIL_LEN-1);
return(TRUE);
}
}
UARTDevfail:
tcflush(UART_fd, TCIOFLUSH);
return(FALSE);
}
/* Macro to transform milliseconds to ns */
#define msTOns(ms) (1000000L*ms)
void InitUartPortDevices_longBuffer(void)
{
unsigned char i=0;
uartTimeout.tv_sec = 0;
uartTimeout.tv_usec = 20000; /* 20 ms timeout */
for(i=0;i<MAX_UART_DEVICES;i++)
{
switch(uartDev[i][1])
{
case(2): /* MU1 */
case(4):
case(6):
case(8):
case(10):
case(12):
case(14):
case(16):
case(18):
case(20):
case(22):
case(24):
case(26):
case(28):
case(30):
case(32):
case(34):
case(36):
case(38):
case(40):
case(42):
case(44):
case(46):
case(48):
case(66): /* MU2 */
case(68):
case(70):
case(72):
case(74): /* MU3 */
case(76):
case(78):
case(80):
case(82): /* MU4 */
case(84):
case(86):
case(88):
case(90): /* MU5 */
case(92):
case(94):
case(96):
case(98): /* MU6 */
case(100):
case(102):
case(104):
case(106): /* M4U */
case(108):
case(110):
case(112):
case(114): /* TH1 */
case(116):
case(118):
case(120):
case(122):
case(124):
case(126):
case(128):
case(130):
case(132):
case(134):
case(136):
uartPortDevices[i] = &UARTDevice;
uartPortTim[i].toValue = msTOns(50); /* 50ms */
uartPortTim[i].status = TIM_EN_RUNNING;
break;
case(240): /* 240-254 are reserved for EEPROM settings */
case(242):
case(244):
case(246):
case(248):
case(250):
case(252):
case(254):
default:
uartPortDevices[i] = NULL; /* "Empty" or unknown module */
uartPortTim[i].status = TIM_DISABLED;
uartDevNum--; /* Substract unused devices from MAX_UART_DEVICES */
break;
}
}
}
void InitUartPortDevices_shortBuffer(void)
{
unsigned char i=0;
uartTimeout.tv_sec = 0;
uartTimeout.tv_usec = 40000; /* 40 ms timeout */
for(i=0;i<MAX_UART_DEVICES;i++)
{
switch(uartDev[i][1])
{
case(114): /* U0x */
case(116):
case(118):
case(120):
case(122):
case(124):
case(126):
case(128):
case(130): /* P01, P02, P01V, P02V */
case(132):
case(134):
case(136):
case(138): /* CA1 */
case(140):
case(142):
case(144):
case(154): /* CR1 */
case(156):
case(158):
case(160):
case(162): /* IR1V */
case(164):
case(166):
case(168):
case(178): /* AQ1, SM1, SM5, SM6, SM7 */
case(180):
case(182):
case(184):
case(186): /* TH1V */
case(188):
case(190):
case(192):
uartPortDevices[i] = &UARTDevice;
uartPortTim[i].toValue = msTOns(250); /* 250ms */
uartPortTim[i].status = TIM_EN_RUNNING;
break;
case(146): /* CH1 */
case(148):
case(150):
case(152):
uartPortDevices[i] = &UARTDevice;
uartPortTim[i].toValue = msTOns(650); /* 650ms */
uartPortTim[i].status = TIM_EN_RUNNING;
break;
case(170): /* ID1, ID2, ID3 */
case(172):
case(174):
case(176):
case(194): /* DP1V */
case(196):
case(198):
case(200):
case(202): /* DP2V */
case(204):
case(206):
case(208):
case(210): /* DT1V */
case(212):
case(214):
case(216):
case(218): /* DU1V */
case(220):
case(222):
case(224):
case(226): /* WP1 */
case(228):
case(230):
case(232):
case(234): /* WP2 */
case(236):
case(238):
case(240):
case(242): /* WT1 */
case(244):
case(246):
case(248):
uartPortDevices[i] = &UARTDevice;
uartPortTim[i].toValue = msTOns(450); /* 450ms */
uartPortTim[i].status = TIM_EN_RUNNING;
break;
default:
uartPortDevices[i] = NULL; /* "Empty" or unknown module */
uartPortTim[i].status = TIM_DISABLED;
break;
}
}
}