1 /*

     2 This file is part of CanFestival, a library implementing CanOpen Stack.

     3 

     4 Copyright (C): Edouard TISSERANT and Francis DUPIN

     5 

     6 See COPYING file for copyrights details.

     7 

     8 This library is free software; you can redistribute it and/or

     9 modify it under the terms of the GNU Lesser General Public

    10 License as published by the Free Software Foundation; either

    11 version 2.1 of the License, or (at your option) any later version.

    12 

    13 This library is distributed in the hope that it will be useful,

    14 but WITHOUT ANY WARRANTY; without even the implied warranty of

    15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    16 Lesser General Public License for more details.

    17 

    18 You should have received a copy of the GNU Lesser General Public

    19 License along with this library; if not, write to the Free Software

    20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

    21 */

    22 

    23 #include <stdio.h>

    24 #include <string.h>

    25 #include <stdlib.h>

    26 #include <stddef.h> /* for NULL */

    27 

    28 #include "config.h"

    29 

    30 #ifdef RTCAN_SOCKET

    31 #include "rtdm/rtcan.h"

    32 #define CAN_IFNAME     "rtcan%s"

    33 #define CAN_SOCKET     rt_dev_socket

    34 #define CAN_CLOSE      rt_dev_close

    35 #define CAN_RECV       rt_dev_recv

    36 #define CAN_SEND       rt_dev_send

    37 #define CAN_BIND       rt_dev_bind

    38 #define CAN_IOCTL      rt_dev_ioctl

    39 #else

    40 #include "af_can.h"

    41 #define CAN_IFNAME     "can%s"

    42 #define CAN_SOCKET     socket

    43 #define CAN_CLOSE      close

    44 #define CAN_RECV       recv

    45 #define CAN_SEND       send

    46 #define CAN_BIND       bind

    47 #define CAN_IOCTL      ioctl

    48 #endif

    49 

    50 struct CANPort;

    51 #define CAN_HANDLE struct CANPort *

    52 

    53 #include <applicfg.h>

    54 

    55 #include "timer.h"

    56 #include "can_driver.h"

    57 #include "timers_driver.h"

    58 

    59 typedef struct CANPort {

    60        int fd;

    61        TASK_HANDLE receiveTask;

    62        CO_Data* d;

    63 } CANPort;

    64 

    65 /*********functions which permit to communicate with the board****************/

    66 UNS8 canReceive(CAN_HANDLE fd0, Message *m)

    67 {

    68        int res;

    69        struct can_frame frame;

    70 

    71        res = CAN_RECV(fd0->fd, &frame, sizeof(frame), 0);

    72        if (res < 0)

    73                return 1;

    74 

    75        m->cob_id.w = frame.can_id & CAN_EFF_MASK;

    76        m->len      = frame.can_dlc;

    77        if (frame.can_id & CAN_RTR_FLAG)

    78                m->rtr = 1;

    79        else

    80                m->rtr = 0;

    81        memcpy(m->data, frame.data, 8);

    82 

    83        return 0;

    84 }

    85 

    86 void canReceiveLoop(CAN_HANDLE fd0)

    87 {

    88        CO_Data* d = fd0->d;

    89        Message m;

    90 

    91        while (1) {

    92                if (canReceive(fd0, &m) != 0)

    93                        break;

    94 

    95                EnterMutex();

    96                canDispatch(d, &m);

    97                LeaveMutex();

    98        }

    99 }

   100 

   101 /***************************************************************************/

   102 UNS8 canSend(CAN_HANDLE fd0, Message *m)

   103 {

   104        int res;

   105        struct can_frame frame;

   106 

   107        frame.can_id = m->cob_id.w;

   108        if (frame.can_id >= 0x800)

   109                frame.can_id |= CAN_EFF_FLAG;

   110        frame.can_dlc = m->len;

   111        if (m->rtr)

   112                frame.can_id |= CAN_RTR_FLAG;

   113        else

   114                memcpy(frame.data, m->data, 8);

   115 

   116        res = CAN_SEND(fd0->fd, &frame, sizeof(frame), 0);

   117        if (res < 0)

   118                return 1;

   119 

   120        return 0;

   121 }

   122 

   123 /***************************************************************************/

   124 CAN_HANDLE canOpen(s_BOARD *board)

   125 {

   126        CAN_HANDLE fd0;

   127        struct ifreq ifr;

   128        struct sockaddr_can addr;

   129        int err;

   130 

   131        fd0 = malloc(sizeof(*fd0));

   132        if (!fd0)

   133                return NULL;

   134 

   135        fd0->fd = CAN_SOCKET(PF_CAN, SOCK_RAW, 0);

   136        if(fd0->fd < 0){

   137                fprintf(stderr,"Socket creation failed.\n");

   138                goto error_ret;

   139        }

   140 

   141        snprintf(ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname);

   142        err = CAN_IOCTL(fd0->fd, SIOCGIFINDEX, &ifr);

   143        if (err) {

   144                fprintf(stderr, "Unknown device: %s\n", ifr.ifr_name);

   145                goto error_close;

   146        }

   147 

   148        addr.can_family  = AF_CAN;

   149        addr.can_ifindex = ifr.ifr_ifindex;

   150        err = CAN_BIND(fd0->fd, (struct sockaddr *)&addr,

   151                              sizeof(addr));

   152        if (err) {

   153                fprintf(stderr, "Binding failed.\n");

   154                goto error_close;

   155        }

   156 

   157        fd0->d = board->d;

   158        CreateReceiveTask(fd0, &fd0->receiveTask);

   159        return fd0;

   160 

   161  error_close:

   162        CAN_CLOSE(fd0->fd);

   163 

   164  error_ret:

   165        free(fd0);

   166        return NULL;

   167 }

   168 

   169 /***************************************************************************/

   170 int canClose(CAN_HANDLE fd0)

   171 {

   172        if (fd0) {

   173                WaitReceiveTaskEnd(&fd0->receiveTask);

   174                CAN_CLOSE(fd0->fd);

   175                free(fd0);

   176        }

   177        return 0;

   178 }