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

#include <errno.h>

    28

    29

#include "config.h"

    30

    31

#ifdef RTCAN_SOCKET

    32

#include "rtdm/rtcan.h"

    33

#define CAN_IFNAME     "rtcan%s"

    34

#define CAN_SOCKET     rt_dev_socket

    35

#define CAN_CLOSE      rt_dev_close

    36

#define CAN_RECV       rt_dev_recv

    37

#define CAN_SEND       rt_dev_send

    38

#define CAN_BIND       rt_dev_bind

    39

#define CAN_IOCTL      rt_dev_ioctl

    40

#else

    41

#include <sys/socket.h>

    42

#include <sys/ioctl.h>

    43

#include "linux/can.h"

    44

#include "linux/can/raw.h"

    45

#include "net/if.h"

    46

#define PF_CAN 29

    47

#define AF_CAN PF_CAN

    48

//#include "af_can.h"

    49

#define CAN_IFNAME     "can%s"

    50

#define CAN_SOCKET     socket

    51

#define CAN_CLOSE      close

    52

#define CAN_RECV       recv

    53

#define CAN_SEND       send

    54

#define CAN_BIND       bind

    55

#define CAN_IOCTL      ioctl

    56

#endif

    57

    58

#include "can_driver.h"

    59

    60

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

    61

UNS8 canReceive_driver(CAN_HANDLE fd0, Message *m)

    62

{

    63

       int res;

    64

       struct can_frame frame;

    65

    66

       res = CAN_RECV(*(int*)fd0, &frame, sizeof(frame), 0);

    67

       if (res < 0)

    68

               return 1;

    69

    70

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

    71

       m->len      = frame.can_dlc;

    72

       if (frame.can_id & CAN_RTR_FLAG)

    73

               m->rtr = 1;

    74

       else

    75

               m->rtr = 0;

    76

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

    77

    78

       return 0;

    79

}

    80

    81

    82

/***************************************************************************/

    83

UNS8 canSend_driver(CAN_HANDLE fd0, Message *m)

    84

{

    85

       int res;

    86

       struct can_frame frame;

    87

    88

       frame.can_id = m->cob_id.w;

    89

       if (frame.can_id >= 0x800)

    90

               frame.can_id |= CAN_EFF_FLAG;

    91

       frame.can_dlc = m->len;

    92

       if (m->rtr)

    93

               frame.can_id |= CAN_RTR_FLAG;

    94

       else

    95

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

    96

    97

       res = CAN_SEND(*(int*)fd0, &frame, sizeof(frame), 0);

    98

       if (res < 0)

    99

               return 1;

   100

   101

       return 0;

   102

}

   103

   104

/***************************************************************************/

   105

#ifdef RTCAN_SOCKET

   106

int TranslateBaudRate(const char* optarg) {

   107

       int baudrate;

   108

       int val, len;

   109

       char *pos = NULL;

   110

   111

       len = strlen(optarg);

   112

       if (!len)

   113

               return 0;

   114

   115

       switch ((int)optarg[len - 1]) {

   116

       case 'M':

   117

               baudrate =  1000000;

   118

               break;

   119

       case 'K':

   120

               baudrate =  1000;

   121

               break;

   122

       default:

   123

               baudrate = 1;

   124

               break;

   125

       }

   126

       if ((sscanf(optarg, "%i", &val)) == 1)

   127

               baudrate *= val;

   128

       else

   129

               baudrate = 0;;

   130

   131

       printf("baudrate=%d (%s)\n", baudrate, optarg);

   132

       return baudrate;

   133

}

   134

#endif

   135

   136

/***************************************************************************/

   137

CAN_HANDLE canOpen_driver(s_BOARD *board)

   138

{

   139

       struct ifreq ifr;

   140

       struct sockaddr_can addr;

   141

       int err;

   142

       CAN_HANDLE fd0 = malloc(sizeof(int));

   143

#ifdef RTCAN_SOCKET

   144

       can_baudrate_t *baudrate;

   145

       can_mode_t *mode;

   146

#endif

   147

   148

       *(int*)fd0 = CAN_SOCKET(PF_CAN, SOCK_RAW, CAN_RAW);

   149

       if (*(int*)fd0 < 0) {

   150

               perror("Socket creation failed");

   151

               goto error_ret;

   152

       }

   153

   154

       if (*board->busname >= '0' && *board->busname <= '9')

   155

               snprintf(ifr.ifr_name, IFNAMSIZ, CAN_IFNAME,

   156

                        board->busname);

   157

       else

   158

               strncpy(ifr.ifr_name, board->busname, IFNAMSIZ);

   159

       err = CAN_IOCTL(*(int*)fd0, SIOCGIFINDEX, &ifr);

   160

       if (err) {

   161

               fprintf(stderr, "Getting IF index for %s failed: %s\n",

   162

                       ifr.ifr_name, strerror(errno));

   163

               goto error_close;

   164

       }

   165

   166

       addr.can_family  = AF_CAN;

   167

       addr.can_ifindex = ifr.ifr_ifindex;

   168

       err = CAN_BIND(*(int*)fd0, (struct sockaddr *)&addr,

   169

                             sizeof(addr));

   170

       if (err) {

   171

               perror("Binding failed");

   172

               goto error_close;

   173

       }

   174

   175

#ifdef RTCAN_SOCKET

   176

       baudrate = (can_baudrate_t *)&ifr.ifr_ifru;

   177

       *baudrate = TranslateBaudRate(board->baudrate);

   178

       if (!*baudrate)

   179

               goto error_close;

   180

   181

       err = CAN_IOCTL(*(int*)fd0, SIOCSCANBAUDRATE, &ifr);

   182

       if (err) {

   183

               fprintf(stderr,

   184

                       "Setting baudrate %d failed: %s\n",

   185

                       *baudrate, strerror(errno));

   186

               goto error_close;

   187

       }

   188

   189

       mode = (can_mode_t *)&ifr.ifr_ifru;

   190

       *mode = CAN_MODE_START;

   191

       err = CAN_IOCTL(*(int*)fd0, SIOCSCANMODE, &ifr);

   192

       if (err) {

   193

               perror("Starting CAN device failed");

   194

               goto error_close;

   195

       }

   196

#endif

   197

   198

       return fd0;

   199

   200

 error_close:

   201

       CAN_CLOSE(*(int*)fd0);

   202

   203

 error_ret:

   204

       free(fd0);

   205

       return NULL;

   206

}

   207

   208

/***************************************************************************/

   209

int canClose_driver(CAN_HANDLE fd0)

   210

{

   211

       if (fd0) {

   212

               CAN_CLOSE(*(int*)fd0);

   213

               free(fd0);

   214

       }

   215

       return 0;

   216

}