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

}