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 }