etisserant@47: /*
etisserant@47: This file is part of CanFestival, a library implementing CanOpen Stack.
etisserant@47: 
etisserant@47: Copyright (C): Edouard TISSERANT and Francis DUPIN
etisserant@47: 
etisserant@47: See COPYING file for copyrights details.
etisserant@47: 
etisserant@47: This library is free software; you can redistribute it and/or
etisserant@47: modify it under the terms of the GNU Lesser General Public
etisserant@47: License as published by the Free Software Foundation; either
etisserant@47: version 2.1 of the License, or (at your option) any later version.
etisserant@47: 
etisserant@47: This library is distributed in the hope that it will be useful,
etisserant@47: but WITHOUT ANY WARRANTY; without even the implied warranty of
etisserant@47: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
etisserant@47: Lesser General Public License for more details.
etisserant@47: 
etisserant@47: You should have received a copy of the GNU Lesser General Public
etisserant@47: License along with this library; if not, write to the Free Software
etisserant@47: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
etisserant@47: */
etisserant@47: 
etisserant@47: #include <stdio.h>
etisserant@47: #include <string.h>
etisserant@47: #include <stdlib.h>
etisserant@196: #include <stddef.h>		/* for NULL */
etisserant@192: #include <errno.h>
etisserant@47: 
etisserant@47: #include "config.h"
etisserant@47: 
etisserant@47: #ifdef RTCAN_SOCKET
etisserant@47: #include "rtdm/rtcan.h"
etisserant@47: #define CAN_IFNAME     "rtcan%s"
etisserant@47: #define CAN_SOCKET     rt_dev_socket
etisserant@47: #define CAN_CLOSE      rt_dev_close
etisserant@47: #define CAN_RECV       rt_dev_recv
etisserant@47: #define CAN_SEND       rt_dev_send
etisserant@47: #define CAN_BIND       rt_dev_bind
etisserant@47: #define CAN_IOCTL      rt_dev_ioctl
etisserant@196: #define CAN_ERRNO(err) (-err)
etisserant@47: #else
etisserant@187: #include <sys/socket.h>
etisserant@187: #include <sys/ioctl.h>
etisserant@187: #include "linux/can.h"
etisserant@187: #include "linux/can/raw.h"
etisserant@187: #include "net/if.h"
etisserant@187: #define PF_CAN 29
etisserant@187: #define AF_CAN PF_CAN
etisserant@187: //#include "af_can.h"
etisserant@47: #define CAN_IFNAME     "can%s"
etisserant@47: #define CAN_SOCKET     socket
etisserant@47: #define CAN_CLOSE      close
etisserant@47: #define CAN_RECV       recv
etisserant@47: #define CAN_SEND       send
etisserant@47: #define CAN_BIND       bind
etisserant@47: #define CAN_IOCTL      ioctl
etisserant@196: #define CAN_ERRNO(err) errno
etisserant@47: #endif
etisserant@47: 
etisserant@47: #include "can_driver.h"
etisserant@47: 
etisserant@290: #if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON
etisserant@290: #include "def.h"
etisserant@290: 
etisserant@290: #define MyCase(fc) case fc: printf(#fc);break;
etisserant@290: void print_message(Message *m)
etisserant@290: {
etisserant@290:     int i;
etisserant@290:     switch(m->cob_id.w >> 7)
etisserant@290:     {
etisserant@290:         MyCase(SYNC)
etisserant@290:         MyCase(TIME_STAMP)
etisserant@290:         MyCase(PDO1tx)
etisserant@290:         MyCase(PDO1rx)
etisserant@290:         MyCase(PDO2tx)
etisserant@290:         MyCase(PDO2rx)
etisserant@290:         MyCase(PDO3tx)
etisserant@290:         MyCase(PDO3rx)
etisserant@290:         MyCase(PDO4tx)
etisserant@290:         MyCase(PDO4rx)
etisserant@290:         MyCase(SDOtx)
etisserant@290:         MyCase(SDOrx)
etisserant@290:         MyCase(NODE_GUARD)
etisserant@290:         MyCase(NMT)
etisserant@290:     }
etisserant@290:     printf(" rtr:%d", m->rtr);
etisserant@290:     printf(" len:%d", m->len);
etisserant@290:     for (i = 0 ; i < m->len ; i++)
etisserant@290:         printf(" %02x", m->data[i]);
etisserant@290:     printf("\n");
etisserant@290: }
etisserant@290: 
etisserant@290: #endif
etisserant@47: /*********functions which permit to communicate with the board****************/
etisserant@196: UNS8
etisserant@196: canReceive_driver (CAN_HANDLE fd0, Message * m)
etisserant@196: {
etisserant@196:   int res;
etisserant@196:   struct can_frame frame;
etisserant@196: 
etisserant@196:   res = CAN_RECV (*(int *) fd0, &frame, sizeof (frame), 0);
etisserant@196:   if (res < 0)
etisserant@196:     {
etisserant@196:       fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res)));
etisserant@196:       return 1;
etisserant@196:     }
etisserant@196: 
etisserant@196:   m->cob_id.w = frame.can_id & CAN_EFF_MASK;
etisserant@196:   m->len = frame.can_dlc;
etisserant@196:   if (frame.can_id & CAN_RTR_FLAG)
etisserant@196:     m->rtr = 1;
etisserant@196:   else
etisserant@196:     m->rtr = 0;
etisserant@196:   memcpy (m->data, frame.data, 8);
etisserant@196: 
etisserant@290: #if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON
etisserant@290:   printf("in : ");
etisserant@290:   print_message(m);
etisserant@290: #endif
etisserant@196:   return 0;
etisserant@196: }
etisserant@196: 
etisserant@196: 
etisserant@196: /***************************************************************************/
etisserant@196: UNS8
etisserant@196: canSend_driver (CAN_HANDLE fd0, Message * m)
etisserant@196: {
etisserant@196:   int res;
etisserant@196:   struct can_frame frame;
etisserant@196: 
etisserant@196:   frame.can_id = m->cob_id.w;
etisserant@196:   if (frame.can_id >= 0x800)
etisserant@196:     frame.can_id |= CAN_EFF_FLAG;
etisserant@196:   frame.can_dlc = m->len;
etisserant@196:   if (m->rtr)
etisserant@196:     frame.can_id |= CAN_RTR_FLAG;
etisserant@196:   else
etisserant@196:     memcpy (frame.data, m->data, 8);
etisserant@196: 
etisserant@290: #if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON
etisserant@290:   printf("out : ");
etisserant@290:   print_message(m);
etisserant@290: #endif
etisserant@196:   res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0);
etisserant@196:   if (res < 0)
etisserant@196:     {
etisserant@196:       fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res)));
etisserant@196:       return 1;
etisserant@196:     }
etisserant@196: 
etisserant@196:   return 0;
etisserant@196: }
etisserant@196: 
etisserant@196: /***************************************************************************/
etisserant@196: #ifdef RTCAN_SOCKET
etisserant@196: int
etisserant@196: TranslateBaudRate (const char *optarg)
etisserant@196: {
etisserant@196:   int baudrate;
etisserant@196:   int val, len;
etisserant@196:   char *pos = NULL;
etisserant@196: 
etisserant@196:   len = strlen (optarg);
etisserant@196:   if (!len)
etisserant@196:     return 0;
etisserant@196: 
etisserant@196:   switch ((int) optarg[len - 1])
etisserant@196:     {
etisserant@196:     case 'M':
etisserant@196:       baudrate = 1000000;
etisserant@196:       break;
etisserant@196:     case 'K':
etisserant@196:       baudrate = 1000;
etisserant@196:       break;
etisserant@196:     default:
etisserant@196:       baudrate = 1;
etisserant@196:       break;
etisserant@196:     }
etisserant@196:   if ((sscanf (optarg, "%i", &val)) == 1)
etisserant@196:     baudrate *= val;
etisserant@196:   else
etisserant@196:     baudrate = 0;;
etisserant@196: 
etisserant@196:   return baudrate;
etisserant@196: }
etisserant@196: #endif
etisserant@196: 
etisserant@196: /***************************************************************************/
etisserant@196: CAN_HANDLE
etisserant@196: canOpen_driver (s_BOARD * board)
etisserant@196: {
etisserant@196:   struct ifreq ifr;
etisserant@196:   struct sockaddr_can addr;
etisserant@196:   int err;
etisserant@196:   CAN_HANDLE fd0 = malloc (sizeof (int));
etisserant@196: #ifdef RTCAN_SOCKET
etisserant@196:   can_baudrate_t *baudrate;
etisserant@196:   can_mode_t *mode;
etisserant@196: #endif
etisserant@196: 
etisserant@196:   *(int *) fd0 = CAN_SOCKET (PF_CAN, SOCK_RAW, CAN_RAW);
etisserant@196:   if (*(int *) fd0 < 0)
etisserant@196:     {
etisserant@196:       fprintf (stderr, "Socket creation failed: %s\n",
etisserant@196: 	       strerror (CAN_ERRNO (*(int *) fd0)));
etisserant@196:       goto error_ret;
etisserant@196:     }
etisserant@196: 
etisserant@196:   if (*board->busname >= '0' && *board->busname <= '9')
etisserant@196:     snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname);
etisserant@196:   else
etisserant@196:     strncpy (ifr.ifr_name, board->busname, IFNAMSIZ);
etisserant@196:   err = CAN_IOCTL (*(int *) fd0, SIOCGIFINDEX, &ifr);
etisserant@196:   if (err)
etisserant@196:     {
etisserant@196:       fprintf (stderr, "Getting IF index for %s failed: %s\n",
etisserant@196: 	       ifr.ifr_name, strerror (CAN_ERRNO (err)));
etisserant@196:       goto error_close;
etisserant@196:     }
etisserant@290:   
etisserant@290:   {
etisserant@290:     int loopback = 1;
etisserant@290:     setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_LOOPBACK,
etisserant@290:                &loopback, sizeof(loopback));
etisserant@290:   }
etisserant@290:   
etisserant@290:   {
etisserant@290:     int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
etisserant@290: 
etisserant@290:     setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
etisserant@290:                &recv_own_msgs, sizeof(recv_own_msgs));
etisserant@290:   }
etisserant@290:   
etisserant@196:   addr.can_family = AF_CAN;
etisserant@196:   addr.can_ifindex = ifr.ifr_ifindex;
etisserant@196:   err = CAN_BIND (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr));
etisserant@196:   if (err)
etisserant@196:     {
etisserant@196:       fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err)));
etisserant@196:       goto error_close;
etisserant@196:     }
etisserant@196: 
etisserant@196: #ifdef RTCAN_SOCKET
etisserant@196:   baudrate = (can_baudrate_t *) & ifr.ifr_ifru;
etisserant@196:   *baudrate = TranslateBaudRate (board->baudrate);
etisserant@196:   if (!*baudrate)
etisserant@196:     goto error_close;
etisserant@196: 
etisserant@196:   err = CAN_IOCTL (*(int *) fd0, SIOCSCANBAUDRATE, &ifr);
etisserant@196:   if (err)
etisserant@196:     {
etisserant@196:       fprintf (stderr,
etisserant@196: 	       "Setting baudrate %d failed: %s\n",
etisserant@196: 	       *baudrate, strerror (CAN_ERRNO (err)));
etisserant@196:       goto error_close;
etisserant@196:     }
etisserant@196: 
etisserant@196:   mode = (can_mode_t *) & ifr.ifr_ifru;
etisserant@196:   *mode = CAN_MODE_START;
etisserant@196:   err = CAN_IOCTL (*(int *) fd0, SIOCSCANMODE, &ifr);
etisserant@196:   if (err)
etisserant@196:     {
etisserant@196:       fprintf (stderr, "Starting CAN device failed: %s\n",
etisserant@196: 	       strerror (CAN_ERRNO (err)));
etisserant@196:       goto error_close;
etisserant@196:     }
etisserant@196: #endif
etisserant@196: 
etisserant@196:   return fd0;
etisserant@196: 
etisserant@196: error_close:
etisserant@196:   CAN_CLOSE (*(int *) fd0);
etisserant@196: 
etisserant@196: error_ret:
etisserant@196:   free (fd0);
etisserant@196:   return NULL;
etisserant@196: }
etisserant@196: 
etisserant@196: /***************************************************************************/
etisserant@196: int
etisserant@196: canClose_driver (CAN_HANDLE fd0)
etisserant@196: {
etisserant@196:   if (fd0)
etisserant@196:     {
etisserant@196:       CAN_CLOSE (*(int *) fd0);
etisserant@196:       free (fd0);
etisserant@196:     }
etisserant@196:   return 0;
etisserant@196: }