nico@215: nico@215:
nico@215:00001 /* nico@215: 00002 This file is part of CanFestival, a library implementing CanOpen Stack. nico@215: 00003 nico@215: 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN nico@215: 00005 nico@215: 00006 See COPYING file for copyrights details. nico@215: 00007 nico@215: 00008 This library is free software; you can redistribute it and/or nico@215: 00009 modify it under the terms of the GNU Lesser General Public nico@215: 00010 License as published by the Free Software Foundation; either nico@215: 00011 version 2.1 of the License, or (at your option) any later version. nico@215: 00012 nico@215: 00013 This library is distributed in the hope that it will be useful, nico@215: 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of nico@215: 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@215: 00016 Lesser General Public License for more details. nico@215: 00017 nico@215: 00018 You should have received a copy of the GNU Lesser General Public nico@215: 00019 License along with this library; if not, write to the Free Software nico@215: 00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA nico@215: 00021 */ nico@215: 00022 nico@215: 00023 #include <stdio.h> nico@215: 00024 #include <string.h> nico@215: 00025 #include <stdlib.h> nico@215: 00026 #include <stddef.h> /* for NULL */ nico@215: 00027 #include <errno.h> nico@215: 00028 nico@215: 00029 #include "config.h" nico@215: 00030 nico@215: 00031 #ifdef RTCAN_SOCKET nico@215: 00032 #include "rtdm/rtcan.h" nico@215: 00033 #define CAN_IFNAME "rtcan%s" nico@215: 00034 #define CAN_SOCKET rt_dev_socket nico@215: 00035 #define CAN_CLOSE rt_dev_close nico@215: 00036 #define CAN_RECV rt_dev_recv nico@215: 00037 #define CAN_SEND rt_dev_send nico@215: 00038 #define CAN_BIND rt_dev_bind nico@215: 00039 #define CAN_IOCTL rt_dev_ioctl nico@215: 00040 #define CAN_ERRNO(err) (-err) nico@215: 00041 #else nico@215: 00042 #include <sys/socket.h> nico@215: 00043 #include <sys/ioctl.h> nico@215: 00044 #include "linux/can.h" nico@215: 00045 #include "linux/can/raw.h" nico@215: 00046 #include "net/if.h" etisserant@240: 00047 #define PF_CAN 29 etisserant@240: 00048 #define AF_CAN PF_CAN nico@215: 00049 //#include "af_can.h" etisserant@240: 00050 #define CAN_IFNAME "can%s" etisserant@240: 00051 #define CAN_SOCKET socket etisserant@240: 00052 #define CAN_CLOSE close etisserant@240: 00053 #define CAN_RECV recv etisserant@240: 00054 #define CAN_SEND send etisserant@240: 00055 #define CAN_BIND bind etisserant@240: 00056 #define CAN_IOCTL ioctl etisserant@240: 00057 #define CAN_ERRNO(err) errno nico@215: 00058 #endif nico@215: 00059 nico@215: 00060 #include "can_driver.h" nico@215: 00061 nico@215: 00062 /*********functions which permit to communicate with the board****************/ etisserant@240: 00063 UNS8 etisserant@240: 00064 canReceive_driver (CAN_HANDLE fd0, Message * m) nico@215: 00065 { nico@215: 00066 int res; nico@215: 00067 struct can_frame frame; nico@215: 00068 etisserant@240: 00069 res = CAN_RECV (*(int *) fd0, &frame, sizeof (frame), 0); nico@215: 00070 if (res < 0) nico@215: 00071 { etisserant@240: 00072 fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res))); nico@215: 00073 return 1; nico@215: 00074 } nico@215: 00075 etisserant@240: 00076 m->cob_id.w = frame.can_id & CAN_EFF_MASK; etisserant@240: 00077 m->len = frame.can_dlc; nico@215: 00078 if (frame.can_id & CAN_RTR_FLAG) etisserant@240: 00079 m->rtr = 1; nico@215: 00080 else etisserant@240: 00081 m->rtr = 0; etisserant@240: 00082 memcpy (m->data, frame.data, 8); nico@215: 00083 nico@215: 00084 return 0; nico@215: 00085 } nico@215: 00086 nico@215: 00087 nico@215: 00088 /***************************************************************************/ etisserant@240: 00089 UNS8 etisserant@240: 00090 canSend_driver (CAN_HANDLE fd0, Message * m) nico@215: 00091 { nico@215: 00092 int res; nico@215: 00093 struct can_frame frame; nico@215: 00094 etisserant@240: 00095 frame.can_id = m->cob_id.w; nico@215: 00096 if (frame.can_id >= 0x800) nico@215: 00097 frame.can_id |= CAN_EFF_FLAG; etisserant@240: 00098 frame.can_dlc = m->len; etisserant@240: 00099 if (m->rtr) nico@215: 00100 frame.can_id |= CAN_RTR_FLAG; nico@215: 00101 else etisserant@240: 00102 memcpy (frame.data, m->data, 8); nico@215: 00103 etisserant@240: 00104 res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0); nico@215: 00105 if (res < 0) nico@215: 00106 { etisserant@240: 00107 fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res))); nico@215: 00108 return 1; nico@215: 00109 } nico@215: 00110 nico@215: 00111 return 0; nico@215: 00112 } nico@215: 00113 nico@215: 00114 /***************************************************************************/ nico@215: 00115 #ifdef RTCAN_SOCKET nico@215: 00116 int etisserant@240: 00117 TranslateBaudRate (const char *optarg) nico@215: 00118 { nico@215: 00119 int baudrate; nico@215: 00120 int val, len; nico@215: 00121 char *pos = NULL; nico@215: 00122 nico@215: 00123 len = strlen (optarg); nico@215: 00124 if (!len) nico@215: 00125 return 0; nico@215: 00126 nico@215: 00127 switch ((int) optarg[len - 1]) nico@215: 00128 { nico@215: 00129 case 'M': nico@215: 00130 baudrate = 1000000; nico@215: 00131 break; nico@215: 00132 case 'K': nico@215: 00133 baudrate = 1000; nico@215: 00134 break; nico@215: 00135 default: nico@215: 00136 baudrate = 1; nico@215: 00137 break; nico@215: 00138 } nico@215: 00139 if ((sscanf (optarg, "%i", &val)) == 1) nico@215: 00140 baudrate *= val; nico@215: 00141 else nico@215: 00142 baudrate = 0;; nico@215: 00143 nico@215: 00144 return baudrate; nico@215: 00145 } nico@215: 00146 #endif nico@215: 00147 nico@215: 00148 /***************************************************************************/ etisserant@240: 00149 CAN_HANDLE etisserant@240: 00150 canOpen_driver (s_BOARD * board) nico@215: 00151 { nico@215: 00152 struct ifreq ifr; nico@215: 00153 struct sockaddr_can addr; nico@215: 00154 int err; etisserant@240: 00155 CAN_HANDLE fd0 = malloc (sizeof (int)); nico@215: 00156 #ifdef RTCAN_SOCKET nico@215: 00157 can_baudrate_t *baudrate; nico@215: 00158 can_mode_t *mode; nico@215: 00159 #endif nico@215: 00160 etisserant@240: 00161 *(int *) fd0 = CAN_SOCKET (PF_CAN, SOCK_RAW, CAN_RAW); nico@215: 00162 if (*(int *) fd0 < 0) nico@215: 00163 { nico@215: 00164 fprintf (stderr, "Socket creation failed: %s\n", etisserant@240: 00165 strerror (CAN_ERRNO (*(int *) fd0))); nico@215: 00166 goto error_ret; nico@215: 00167 } nico@215: 00168 etisserant@240: 00169 if (*board->busname >= '0' && *board->busname <= '9') etisserant@240: 00170 snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname); nico@215: 00171 else etisserant@240: 00172 strncpy (ifr.ifr_name, board->busname, IFNAMSIZ); etisserant@240: 00173 err = CAN_IOCTL (*(int *) fd0, SIOCGIFINDEX, &ifr); nico@215: 00174 if (err) nico@215: 00175 { nico@215: 00176 fprintf (stderr, "Getting IF index for %s failed: %s\n", etisserant@240: 00177 ifr.ifr_name, strerror (CAN_ERRNO (err))); nico@215: 00178 goto error_close; nico@215: 00179 } nico@215: 00180 etisserant@240: 00181 addr.can_family = AF_CAN; nico@215: 00182 addr.can_ifindex = ifr.ifr_ifindex; etisserant@240: 00183 err = CAN_BIND (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr)); nico@215: 00184 if (err) nico@215: 00185 { etisserant@240: 00186 fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err))); nico@215: 00187 goto error_close; nico@215: 00188 } nico@215: 00189 nico@215: 00190 #ifdef RTCAN_SOCKET nico@215: 00191 baudrate = (can_baudrate_t *) & ifr.ifr_ifru; etisserant@240: 00192 *baudrate = TranslateBaudRate (board->baudrate); nico@215: 00193 if (!*baudrate) nico@215: 00194 goto error_close; nico@215: 00195 etisserant@240: 00196 err = CAN_IOCTL (*(int *) fd0, SIOCSCANBAUDRATE, &ifr); nico@215: 00197 if (err) nico@215: 00198 { nico@215: 00199 fprintf (stderr, nico@215: 00200 "Setting baudrate %d failed: %s\n", etisserant@240: 00201 *baudrate, strerror (CAN_ERRNO (err))); nico@215: 00202 goto error_close; nico@215: 00203 } nico@215: 00204 nico@215: 00205 mode = (can_mode_t *) & ifr.ifr_ifru; nico@215: 00206 *mode = CAN_MODE_START; etisserant@240: 00207 err = CAN_IOCTL (*(int *) fd0, SIOCSCANMODE, &ifr); nico@215: 00208 if (err) nico@215: 00209 { nico@215: 00210 fprintf (stderr, "Starting CAN device failed: %s\n", etisserant@240: 00211 strerror (CAN_ERRNO (err))); nico@215: 00212 goto error_close; nico@215: 00213 } nico@215: 00214 #endif nico@215: 00215 nico@215: 00216 return fd0; nico@215: 00217 nico@215: 00218 error_close: etisserant@240: 00219 CAN_CLOSE (*(int *) fd0); nico@215: 00220 nico@215: 00221 error_ret: nico@215: 00222 free (fd0); nico@215: 00223 return NULL; nico@215: 00224 } nico@215: 00225 nico@215: 00226 /***************************************************************************/ nico@215: 00227 int etisserant@240: 00228 canClose_driver (CAN_HANDLE fd0) nico@215: 00229 { nico@215: 00230 if (fd0) nico@215: 00231 { etisserant@240: 00232 CAN_CLOSE (*(int *) fd0); nico@215: 00233 free (fd0); nico@215: 00234 } nico@215: 00235 return 0; nico@215: 00236 } etisserant@240: