00001
+00002
+00003
+00004
+00005
+00006
+00007
+00008
+00009
+00010
+00011
+00012
+00013
+00014
+00015
+00016
+00017
+00018
+00019
+00020
+00021
+00022
+00023 #include <stdio.h>
+00024 #include <string.h>
+00025 #include <stdlib.h>
+00026 #include <stddef.h>
+00027 #include <errno.h>
+00028
+00029 #include "config.h"
+00030
+00031 #ifdef RTCAN_SOCKET
+00032 #include "rtdm/rtcan.h"
+00033 #define CAN_IFNAME "rtcan%s"
+00034 #define CAN_SOCKET rt_dev_socket
+00035 #define CAN_CLOSE rt_dev_close
+00036 #define CAN_RECV rt_dev_recv
+00037 #define CAN_SEND rt_dev_send
+00038 #define CAN_BIND rt_dev_bind
+00039 #define CAN_IOCTL rt_dev_ioctl
+00040 #define CAN_ERRNO(err) (-err)
+00041 #else
+00042 #include <sys/socket.h>
+00043 #include <sys/ioctl.h>
+00044 #include "linux/can.h"
+00045 #include "linux/can/raw.h"
+00046 #include "net/if.h"
+00047 #define PF_CAN 29
+00048 #define AF_CAN PF_CAN
+00049
+00050 #define CAN_IFNAME "can%s"
+00051 #define CAN_SOCKET socket
+00052 #define CAN_CLOSE close
+00053 #define CAN_RECV recv
+00054 #define CAN_SEND send
+00055 #define CAN_BIND bind
+00056 #define CAN_IOCTL ioctl
+00057 #define CAN_ERRNO(err) errno
+00058 #endif
+00059
+00060 #include "can_driver.h"
+00061
+00062
+00063 UNS8
+00064 canReceive_driver (CAN_HANDLE fd0, Message * m)
+00065 {
+00066 int res;
+00067 struct can_frame frame;
+00068
+00069 res = CAN_RECV (*(int *) fd0, &frame, sizeof (frame), 0);
+00070 if (res < 0)
+00071 {
+00072 fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res)));
+00073 return 1;
+00074 }
+00075
+00076 m->cob_id.w = frame.can_id & CAN_EFF_MASK;
+00077 m->len = frame.can_dlc;
+00078 if (frame.can_id & CAN_RTR_FLAG)
+00079 m->rtr = 1;
+00080 else
+00081 m->rtr = 0;
+00082 memcpy (m->data, frame.data, 8);
+00083
+00084 return 0;
+00085 }
+00086
+00087
+00088
+00089 UNS8
+00090 canSend_driver (CAN_HANDLE fd0, Message * m)
+00091 {
+00092 int res;
+00093 struct can_frame frame;
+00094
+00095 frame.can_id = m->cob_id.w;
+00096 if (frame.can_id >= 0x800)
+00097 frame.can_id |= CAN_EFF_FLAG;
+00098 frame.can_dlc = m->len;
+00099 if (m->rtr)
+00100 frame.can_id |= CAN_RTR_FLAG;
+00101 else
+00102 memcpy (frame.data, m->data, 8);
+00103
+00104 res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0);
+00105 if (res < 0)
+00106 {
+00107 fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res)));
+00108 return 1;
+00109 }
+00110
+00111 return 0;
+00112 }
+00113
+00114
+00115 #ifdef RTCAN_SOCKET
+00116 int
+00117 TranslateBaudRate (const char *optarg)
+00118 {
+00119 int baudrate;
+00120 int val, len;
+00121 char *pos = NULL;
+00122
+00123 len = strlen (optarg);
+00124 if (!len)
+00125 return 0;
+00126
+00127 switch ((int) optarg[len - 1])
+00128 {
+00129 case 'M':
+00130 baudrate = 1000000;
+00131 break;
+00132 case 'K':
+00133 baudrate = 1000;
+00134 break;
+00135 default:
+00136 baudrate = 1;
+00137 break;
+00138 }
+00139 if ((sscanf (optarg, "%i", &val)) == 1)
+00140 baudrate *= val;
+00141 else
+00142 baudrate = 0;;
+00143
+00144 return baudrate;
+00145 }
+00146 #endif
+00147
+00148
+00149 CAN_HANDLE
+00150 canOpen_driver (s_BOARD * board)
+00151 {
+00152 struct ifreq ifr;
+00153 struct sockaddr_can addr;
+00154 int err;
+00155 CAN_HANDLE fd0 = malloc (sizeof (int));
+00156 #ifdef RTCAN_SOCKET
+00157 can_baudrate_t *baudrate;
+00158 can_mode_t *mode;
+00159 #endif
+00160
+00161 *(int *) fd0 = CAN_SOCKET (PF_CAN, SOCK_RAW, CAN_RAW);
+00162 if (*(int *) fd0 < 0)
+00163 {
+00164 fprintf (stderr, "Socket creation failed: %s\n",
+00165 strerror (CAN_ERRNO (*(int *) fd0)));
+00166 goto error_ret;
+00167 }
+00168
+00169 if (*board->busname >= '0' && *board->busname <= '9')
+00170 snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname);
+00171 else
+00172 strncpy (ifr.ifr_name, board->busname, IFNAMSIZ);
+00173 err = CAN_IOCTL (*(int *) fd0, SIOCGIFINDEX, &ifr);
+00174 if (err)
+00175 {
+00176 fprintf (stderr, "Getting IF index for %s failed: %s\n",
+00177 ifr.ifr_name, strerror (CAN_ERRNO (err)));
+00178 goto error_close;
+00179 }
+00180
+00181 addr.can_family = AF_CAN;
+00182 addr.can_ifindex = ifr.ifr_ifindex;
+00183 err = CAN_BIND (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr));
+00184 if (err)
+00185 {
+00186 fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err)));
+00187 goto error_close;
+00188 }
+00189
+00190 #ifdef RTCAN_SOCKET
+00191 baudrate = (can_baudrate_t *) & ifr.ifr_ifru;
+00192 *baudrate = TranslateBaudRate (board->baudrate);
+00193 if (!*baudrate)
+00194 goto error_close;
+00195
+00196 err = CAN_IOCTL (*(int *) fd0, SIOCSCANBAUDRATE, &ifr);
+00197 if (err)
+00198 {
+00199 fprintf (stderr,
+00200 "Setting baudrate %d failed: %s\n",
+00201 *baudrate, strerror (CAN_ERRNO (err)));
+00202 goto error_close;
+00203 }
+00204
+00205 mode = (can_mode_t *) & ifr.ifr_ifru;
+00206 *mode = CAN_MODE_START;
+00207 err = CAN_IOCTL (*(int *) fd0, SIOCSCANMODE, &ifr);
+00208 if (err)
+00209 {
+00210 fprintf (stderr, "Starting CAN device failed: %s\n",
+00211 strerror (CAN_ERRNO (err)));
+00212 goto error_close;
+00213 }
+00214 #endif
+00215
+00216 return fd0;
+00217
+00218 error_close:
+00219 CAN_CLOSE (*(int *) fd0);
+00220
+00221 error_ret:
+00222 free (fd0);
+00223 return NULL;
+00224 }
+00225
+00226
+00227 int
+00228 canClose_driver (CAN_HANDLE fd0)
+00229 {
+00230 if (fd0)
+00231 {
+00232 CAN_CLOSE (*(int *) fd0);
+00233 free (fd0);
+00234 }
+00235 return 0;
+00236 }
+