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 }
-