etisserant@47: /* etisserant@47: This file is part of CanFestival, a library implementing CanOpen Stack. etisserant@47: etisserant@47: Copyright (C): Edouard TISSERANT and Francis DUPIN etisserant@47: etisserant@47: See COPYING file for copyrights details. etisserant@47: etisserant@47: This library is free software; you can redistribute it and/or etisserant@47: modify it under the terms of the GNU Lesser General Public etisserant@47: License as published by the Free Software Foundation; either etisserant@47: version 2.1 of the License, or (at your option) any later version. etisserant@47: etisserant@47: This library is distributed in the hope that it will be useful, etisserant@47: but WITHOUT ANY WARRANTY; without even the implied warranty of etisserant@47: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU etisserant@47: Lesser General Public License for more details. etisserant@47: etisserant@47: You should have received a copy of the GNU Lesser General Public etisserant@47: License along with this library; if not, write to the Free Software etisserant@47: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA etisserant@47: */ etisserant@47: etisserant@47: #include etisserant@47: #include etisserant@47: #include etisserant@196: #include /* for NULL */ etisserant@192: #include etisserant@47: etisserant@47: #include "config.h" etisserant@47: etisserant@47: #ifdef RTCAN_SOCKET etisserant@47: #include "rtdm/rtcan.h" etisserant@47: #define CAN_IFNAME "rtcan%s" etisserant@47: #define CAN_SOCKET rt_dev_socket etisserant@47: #define CAN_CLOSE rt_dev_close etisserant@47: #define CAN_RECV rt_dev_recv etisserant@47: #define CAN_SEND rt_dev_send etisserant@47: #define CAN_BIND rt_dev_bind etisserant@47: #define CAN_IOCTL rt_dev_ioctl etisserant@196: #define CAN_ERRNO(err) (-err) etisserant@47: #else etisserant@187: #include etisserant@187: #include etisserant@187: #include "linux/can.h" etisserant@187: #include "linux/can/raw.h" etisserant@187: #include "net/if.h" etisserant@187: #define PF_CAN 29 etisserant@187: #define AF_CAN PF_CAN etisserant@187: //#include "af_can.h" etisserant@47: #define CAN_IFNAME "can%s" etisserant@47: #define CAN_SOCKET socket etisserant@47: #define CAN_CLOSE close etisserant@47: #define CAN_RECV recv etisserant@47: #define CAN_SEND send etisserant@47: #define CAN_BIND bind etisserant@47: #define CAN_IOCTL ioctl etisserant@196: #define CAN_ERRNO(err) errno etisserant@47: #endif etisserant@47: etisserant@47: #include "can_driver.h" etisserant@47: etisserant@290: #if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON etisserant@290: #include "def.h" etisserant@290: etisserant@290: #define MyCase(fc) case fc: printf(#fc);break; etisserant@290: void print_message(Message *m) etisserant@290: { etisserant@290: int i; etisserant@290: switch(m->cob_id.w >> 7) etisserant@290: { etisserant@290: MyCase(SYNC) etisserant@290: MyCase(TIME_STAMP) etisserant@290: MyCase(PDO1tx) etisserant@290: MyCase(PDO1rx) etisserant@290: MyCase(PDO2tx) etisserant@290: MyCase(PDO2rx) etisserant@290: MyCase(PDO3tx) etisserant@290: MyCase(PDO3rx) etisserant@290: MyCase(PDO4tx) etisserant@290: MyCase(PDO4rx) etisserant@290: MyCase(SDOtx) etisserant@290: MyCase(SDOrx) etisserant@290: MyCase(NODE_GUARD) etisserant@290: MyCase(NMT) etisserant@290: } etisserant@290: printf(" rtr:%d", m->rtr); etisserant@290: printf(" len:%d", m->len); etisserant@290: for (i = 0 ; i < m->len ; i++) etisserant@290: printf(" %02x", m->data[i]); etisserant@290: printf("\n"); etisserant@290: } etisserant@290: etisserant@290: #endif etisserant@47: /*********functions which permit to communicate with the board****************/ etisserant@196: UNS8 etisserant@196: canReceive_driver (CAN_HANDLE fd0, Message * m) etisserant@196: { etisserant@196: int res; etisserant@196: struct can_frame frame; etisserant@196: etisserant@196: res = CAN_RECV (*(int *) fd0, &frame, sizeof (frame), 0); etisserant@196: if (res < 0) etisserant@196: { etisserant@196: fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res))); etisserant@196: return 1; etisserant@196: } etisserant@196: etisserant@196: m->cob_id.w = frame.can_id & CAN_EFF_MASK; etisserant@196: m->len = frame.can_dlc; etisserant@196: if (frame.can_id & CAN_RTR_FLAG) etisserant@196: m->rtr = 1; etisserant@196: else etisserant@196: m->rtr = 0; etisserant@196: memcpy (m->data, frame.data, 8); etisserant@196: etisserant@290: #if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON etisserant@290: printf("in : "); etisserant@290: print_message(m); etisserant@290: #endif etisserant@196: return 0; etisserant@196: } etisserant@196: etisserant@196: etisserant@196: /***************************************************************************/ etisserant@196: UNS8 etisserant@196: canSend_driver (CAN_HANDLE fd0, Message * m) etisserant@196: { etisserant@196: int res; etisserant@196: struct can_frame frame; etisserant@196: etisserant@196: frame.can_id = m->cob_id.w; etisserant@196: if (frame.can_id >= 0x800) etisserant@196: frame.can_id |= CAN_EFF_FLAG; etisserant@196: frame.can_dlc = m->len; etisserant@196: if (m->rtr) etisserant@196: frame.can_id |= CAN_RTR_FLAG; etisserant@196: else etisserant@196: memcpy (frame.data, m->data, 8); etisserant@196: etisserant@290: #if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON etisserant@290: printf("out : "); etisserant@290: print_message(m); etisserant@290: #endif etisserant@196: res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0); etisserant@196: if (res < 0) etisserant@196: { etisserant@196: fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res))); etisserant@196: return 1; etisserant@196: } etisserant@196: etisserant@196: return 0; etisserant@196: } etisserant@196: etisserant@196: /***************************************************************************/ etisserant@196: #ifdef RTCAN_SOCKET etisserant@196: int etisserant@196: TranslateBaudRate (const char *optarg) etisserant@196: { etisserant@196: int baudrate; etisserant@196: int val, len; etisserant@196: char *pos = NULL; etisserant@196: etisserant@196: len = strlen (optarg); etisserant@196: if (!len) etisserant@196: return 0; etisserant@196: etisserant@196: switch ((int) optarg[len - 1]) etisserant@196: { etisserant@196: case 'M': etisserant@196: baudrate = 1000000; etisserant@196: break; etisserant@196: case 'K': etisserant@196: baudrate = 1000; etisserant@196: break; etisserant@196: default: etisserant@196: baudrate = 1; etisserant@196: break; etisserant@196: } etisserant@196: if ((sscanf (optarg, "%i", &val)) == 1) etisserant@196: baudrate *= val; etisserant@196: else etisserant@196: baudrate = 0;; etisserant@196: etisserant@196: return baudrate; etisserant@196: } etisserant@196: #endif etisserant@196: etisserant@196: /***************************************************************************/ etisserant@196: CAN_HANDLE etisserant@196: canOpen_driver (s_BOARD * board) etisserant@196: { etisserant@196: struct ifreq ifr; etisserant@196: struct sockaddr_can addr; etisserant@196: int err; etisserant@196: CAN_HANDLE fd0 = malloc (sizeof (int)); etisserant@196: #ifdef RTCAN_SOCKET etisserant@196: can_baudrate_t *baudrate; etisserant@196: can_mode_t *mode; etisserant@196: #endif etisserant@196: etisserant@196: *(int *) fd0 = CAN_SOCKET (PF_CAN, SOCK_RAW, CAN_RAW); etisserant@196: if (*(int *) fd0 < 0) etisserant@196: { etisserant@196: fprintf (stderr, "Socket creation failed: %s\n", etisserant@196: strerror (CAN_ERRNO (*(int *) fd0))); etisserant@196: goto error_ret; etisserant@196: } etisserant@196: etisserant@196: if (*board->busname >= '0' && *board->busname <= '9') etisserant@196: snprintf (ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname); etisserant@196: else etisserant@196: strncpy (ifr.ifr_name, board->busname, IFNAMSIZ); etisserant@196: err = CAN_IOCTL (*(int *) fd0, SIOCGIFINDEX, &ifr); etisserant@196: if (err) etisserant@196: { etisserant@196: fprintf (stderr, "Getting IF index for %s failed: %s\n", etisserant@196: ifr.ifr_name, strerror (CAN_ERRNO (err))); etisserant@196: goto error_close; etisserant@196: } etisserant@290: etisserant@290: { etisserant@290: int loopback = 1; etisserant@290: setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_LOOPBACK, etisserant@290: &loopback, sizeof(loopback)); etisserant@290: } etisserant@290: etisserant@290: { etisserant@290: int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ etisserant@290: etisserant@290: setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, etisserant@290: &recv_own_msgs, sizeof(recv_own_msgs)); etisserant@290: } etisserant@290: etisserant@196: addr.can_family = AF_CAN; etisserant@196: addr.can_ifindex = ifr.ifr_ifindex; etisserant@196: err = CAN_BIND (*(int *) fd0, (struct sockaddr *) &addr, sizeof (addr)); etisserant@196: if (err) etisserant@196: { etisserant@196: fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err))); etisserant@196: goto error_close; etisserant@196: } etisserant@196: etisserant@196: #ifdef RTCAN_SOCKET etisserant@196: baudrate = (can_baudrate_t *) & ifr.ifr_ifru; etisserant@196: *baudrate = TranslateBaudRate (board->baudrate); etisserant@196: if (!*baudrate) etisserant@196: goto error_close; etisserant@196: etisserant@196: err = CAN_IOCTL (*(int *) fd0, SIOCSCANBAUDRATE, &ifr); etisserant@196: if (err) etisserant@196: { etisserant@196: fprintf (stderr, etisserant@196: "Setting baudrate %d failed: %s\n", etisserant@196: *baudrate, strerror (CAN_ERRNO (err))); etisserant@196: goto error_close; etisserant@196: } etisserant@196: etisserant@196: mode = (can_mode_t *) & ifr.ifr_ifru; etisserant@196: *mode = CAN_MODE_START; etisserant@196: err = CAN_IOCTL (*(int *) fd0, SIOCSCANMODE, &ifr); etisserant@196: if (err) etisserant@196: { etisserant@196: fprintf (stderr, "Starting CAN device failed: %s\n", etisserant@196: strerror (CAN_ERRNO (err))); etisserant@196: goto error_close; etisserant@196: } etisserant@196: #endif etisserant@196: etisserant@196: return fd0; etisserant@196: etisserant@196: error_close: etisserant@196: CAN_CLOSE (*(int *) fd0); etisserant@196: etisserant@196: error_ret: etisserant@196: free (fd0); etisserant@196: return NULL; etisserant@196: } etisserant@196: etisserant@196: /***************************************************************************/ etisserant@196: int etisserant@196: canClose_driver (CAN_HANDLE fd0) etisserant@196: { etisserant@196: if (fd0) etisserant@196: { etisserant@196: CAN_CLOSE (*(int *) fd0); etisserant@196: free (fd0); etisserant@196: } etisserant@196: return 0; etisserant@196: }