drivers/can_socket/can_socket.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4 Copyright (C): Edouard TISSERANT and Francis DUPIN
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25 #include <stdlib.h>
  26 #include <stddef.h>             /* for NULL */
  27 #include <errno.h>
  28
  29 #include "config.h"
  30
  31 #ifdef RTCAN_SOCKET
  32 #include "rtdm/rtcan.h"
  33 #define CAN_IFNAME     "rtcan%s"
  34 #define CAN_SOCKET     rt_dev_socket
  35 #define CAN_CLOSE      rt_dev_close
  36 #define CAN_RECV       rt_dev_recv
  37 #define CAN_SEND       rt_dev_send
  38 #define CAN_BIND       rt_dev_bind
  39 #define CAN_IOCTL      rt_dev_ioctl
  40 #define CAN_ERRNO(err) (-err)
  41 #else
  42 #include <sys/socket.h>
  43 #include <sys/ioctl.h>
  44 #include "linux/can.h"
  45 #include "linux/can/raw.h"
  46 #include "net/if.h"
  47 #define PF_CAN 29
  48 #define AF_CAN PF_CAN
  49 //#include "af_can.h"
  50 #define CAN_IFNAME     "can%s"
  51 #define CAN_SOCKET     socket
  52 #define CAN_CLOSE      close
  53 #define CAN_RECV       recv
  54 #define CAN_SEND       send
  55 #define CAN_BIND       bind
  56 #define CAN_IOCTL      ioctl
  57 #define CAN_ERRNO(err) errno
  58 #endif
  59
  60 #include "can_driver.h"
  61
  62 /*********functions which permit to communicate with the board****************/
  63 UNS8
  64 canReceive_driver (CAN_HANDLE fd0, Message * m)
  65 {
  66   int res;
  67   struct can_frame frame;
  68
  69   res = CAN_RECV (*(int *) fd0, &frame, sizeof (frame), 0);
  70   if (res < 0)
  71     {
  72       fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res)));
  73       return 1;
  74     }
  75
  76   m->cob_id = frame.can_id & CAN_EFF_MASK;
  77   m->len = frame.can_dlc;
  78   if (frame.can_id & CAN_RTR_FLAG)
  79     m->rtr = 1;
  80   else
  81     m->rtr = 0;
  82   memcpy (m->data, frame.data, 8);
  83
  84 #if defined DEBUG_MSG_CONSOLE_ON
  85   printf("in : ");
  86   print_message(m);
  87 #endif
  88   return 0;
  89 }
  90
  91
  92 /***************************************************************************/
  93 UNS8
  94 canSend_driver (CAN_HANDLE fd0, Message * m)
  95 {
  96   int res;
  97   struct can_frame frame;
  98
  99   frame.can_id = m->cob_id;
 100   if (frame.can_id >= 0x800)
 101     frame.can_id |= CAN_EFF_FLAG;
 102   frame.can_dlc = m->len;
 103   if (m->rtr)
 104     frame.can_id |= CAN_RTR_FLAG;
 105   else
 106     memcpy (frame.data, m->data, 8);
 107
 108 #if defined DEBUG_MSG_CONSOLE_ON
 109   printf("out : ");
 110   print_message(m);
 111 #endif
 112   res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0);
 113   if (res < 0)
 114     {
 115       fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res)));
 116       return 1;
 117     }
 118
 119   return 0;
 120 }
 121
 122 /***************************************************************************/
 123 #ifdef RTCAN_SOCKET
 124 int
 125 TranslateBaudRate (const char *optarg)
 126 {
 127   int baudrate;
 128   int val, len;
 129   char *pos = NULL;
 130
 131   len = strlen (optarg);
 132   if (!len)
 133     return 0;
 134
 135   switch ((int) optarg[len - 1])
 136     {
 137     case 'M':
 138       baudrate = 1000000;
 139       break;
 140     case 'K':
 141       baudrate = 1000;
 142       break;
 143     default:
 144       baudrate = 1;
 145       break;
 146     }
 147   if ((sscanf (optarg, "%i", &val)) == 1)
 148     baudrate *= val;
 149   else
 150     baudrate = 0;;
 151
 152   return baudrate;
 153 }
 154 #endif
 155
 156 UNS8 canChangeBaudRate_driver( CAN_HANDLE fd, char* baud)
 157 {
 158         printf("canChangeBaudRate not yet supported by this driver\n");
 159         return 0;
 160 }
 161
 162 /***************************************************************************/
 163 CAN_HANDLE
 164 canOpen_driver (s_BOARD * board)
 165 {
 166   struct ifreq ifr;
 167   struct sockaddr_can addr;
 168   int err;
 169   CAN_HANDLE fd0 = malloc (sizeof (int));
 170 #ifdef RTCAN_SOCKET
 171   can_baudrate_t *baudrate;
 172   can_mode_t *mode;
 173 #endif
 174
 175   *(int *) fd0 = CAN_SOCKET (PF_CAN, SOCK_RAW, CAN_RAW);
 176   if (*(int *) fd0 < 0)
 177     {
 178       fprintf (stderr, "Socket creation failed: %s\n",
 179                strerror (CAN_ERRNO (*(int *) fd0)));
 180       goto error_ret;
 181     }
 182
 183   if (*board->busname >= '0' && *board->busname <= '9')
 184     snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname);
 185   else
 186     strncpy (ifr.ifr_name, board->busname, IFNAMSIZ);
 187   err = CAN_IOCTL (*(int *) fd0, SIOCGIFINDEX, &ifr);
 188   if (err)
 189     {
 190       fprintf (stderr, "Getting IF index for %s failed: %s\n",
 191                ifr.ifr_name, strerror (CAN_ERRNO (err)));
 192       goto error_close;
 193     }
 194
 195   {
 196     int loopback = 1;
 197     setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_LOOPBACK,
 198                &loopback, sizeof(loopback));
 199   }
 200
 201   {
 202     int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
 203
 204     setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
 205                &recv_own_msgs, sizeof(recv_own_msgs));
 206   }
 207
 208   addr.can_family = AF_CAN;
 209   addr.can_ifindex = ifr.ifr_ifindex;
 210   err = CAN_BIND (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr));
 211   if (err)
 212     {
 213       fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err)));
 214       goto error_close;
 215     }
 216
 217 #ifdef RTCAN_SOCKET
 218   baudrate = (can_baudrate_t *) & ifr.ifr_ifru;
 219   *baudrate = TranslateBaudRate (board->baudrate);
 220   if (!*baudrate)
 221     goto error_close;
 222
 223   err = CAN_IOCTL (*(int *) fd0, SIOCSCANBAUDRATE, &ifr);
 224   if (err)
 225     {
 226       fprintf (stderr,
 227                "Setting baudrate %d failed: %s\n",
 228                *baudrate, strerror (CAN_ERRNO (err)));
 229       goto error_close;
 230     }
 231
 232   mode = (can_mode_t *) & ifr.ifr_ifru;
 233   *mode = CAN_MODE_START;
 234   err = CAN_IOCTL (*(int *) fd0, SIOCSCANMODE, &ifr);
 235   if (err)
 236     {
 237       fprintf (stderr, "Starting CAN device failed: %s\n",
 238                strerror (CAN_ERRNO (err)));
 239       goto error_close;
 240     }
 241 #endif
 242
 243   return fd0;
 244
 245 error_close:
 246   CAN_CLOSE (*(int *) fd0);
 247
 248 error_ret:
 249   free (fd0);
 250   return NULL;
 251 }
 252
 253 /***************************************************************************/
 254 int
 255 canClose_driver (CAN_HANDLE fd0)
 256 {
 257   if (fd0)
 258     {
 259       CAN_CLOSE (*(int *) fd0);
 260       free (fd0);
 261     }
 262   return 0;
 263 }