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

#define CAN_ERRNO(err) (-err)

    41

#else

    42

#include <sys/socket.h>

    43

#include <sys/ioctl.h>

    44

#include "linux/can.h"

    45

#include "linux/can/raw.h"

    46

#include "net/if.h"

    47

#define PF_CAN 29

    48

#define AF_CAN PF_CAN

    49

//#include "af_can.h"

    50

#define CAN_IFNAME     "can%s"

    51

#define CAN_SOCKET     socket

    52

#define CAN_CLOSE      close

    53

#define CAN_RECV       recv

    54

#define CAN_SEND       send

    55

#define CAN_BIND       bind

    56

#define CAN_IOCTL      ioctl

    57

#define CAN_ERRNO(err) errno

    58

#endif

    59

    60

#include "can_driver.h"

    61

    62

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

    63

UNS8

    64

canReceive_driver (CAN_HANDLE fd0, Message * m)

    65

{

    66

  int res;

    67

  struct can_frame frame;

    68

    69

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

    70

  if (res < 0)

    71

    {

    72

      fprintf (stderr, "Recv failed: %s\n", strerror (CAN_ERRNO (res)));

    73

      return 1;

    74

    }

    75

    76

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

    77

  m->len = frame.can_dlc;

    78

  if (frame.can_id & CAN_RTR_FLAG)

    79

    m->rtr = 1;

    80

  else

    81

    m->rtr = 0;

    82

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

    83

    84

#if defined DEBUG_MSG_CONSOLE_ON

    85

  printf("in : ");

    86

  print_message(m);

    87

#endif

    88

  return 0;

    89

}

    90

    91

    92

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

    93

UNS8

    94

canSend_driver (CAN_HANDLE fd0, Message * m)

    95

{

    96

  int res;

    97

  struct can_frame frame;

    98

    99

  frame.can_id = m->cob_id.w;

   100

  if (frame.can_id >= 0x800)

   101

    frame.can_id |= CAN_EFF_FLAG;

   102

  frame.can_dlc = m->len;

   103

  if (m->rtr)

   104

    frame.can_id |= CAN_RTR_FLAG;

   105

  else

   106

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

   107

   108

#if defined DEBUG_MSG_CONSOLE_ON

   109

  printf("out : ");

   110

  print_message(m);

   111

#endif

   112

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

   113

  if (res < 0)

   114

    {

   115

      fprintf (stderr, "Send failed: %s\n", strerror (CAN_ERRNO (res)));

   116

      return 1;

   117

    }

   118

   119

  return 0;

   120

}

   121

   122

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

   123

#ifdef RTCAN_SOCKET

   124

int

   125

TranslateBaudRate (const char *optarg)

   126

{

   127

  int baudrate;

   128

  int val, len;

   129

  char *pos = NULL;

   130

   131

  len = strlen (optarg);

   132

  if (!len)

   133

    return 0;

   134

   135

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

   136

    {

   137

    case 'M':

   138

      baudrate = 1000000;

   139

      break;

   140

    case 'K':

   141

      baudrate = 1000;

   142

      break;

   143

    default:

   144

      baudrate = 1;

   145

      break;

   146

    }

   147

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

   148

    baudrate *= val;

   149

  else

   150

    baudrate = 0;;

   151

   152

  return baudrate;

   153

}

   154

#endif

   155

   156

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

   157

CAN_HANDLE

   158

canOpen_driver (s_BOARD * board)

   159

{

   160

  struct ifreq ifr;

   161

  struct sockaddr_can addr;

   162

  int err;

   163

  CAN_HANDLE fd0 = malloc (sizeof (int));

   164

#ifdef RTCAN_SOCKET

   165

  can_baudrate_t *baudrate;

   166

  can_mode_t *mode;

   167

#endif

   168

   169

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

   170

  if (*(int *) fd0 < 0)

   171

    {

   172

      fprintf (stderr, "Socket creation failed: %s\n",

   173

	       strerror (CAN_ERRNO (*(int *) fd0)));

   174

      goto error_ret;

   175

    }

   176

   177

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

   178

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

   179

  else

   180

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

   181

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

   182

  if (err)

   183

    {

   184

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

   185

	       ifr.ifr_name, strerror (CAN_ERRNO (err)));

   186

      goto error_close;

   187

    }

   188

   189

  {

   190

    int loopback = 1;

   191

    setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_LOOPBACK,

   192

               &loopback, sizeof(loopback));

   193

  }

   194

   195

  {

   196

    int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */

   197

   198

    setsockopt(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,

   199

               &recv_own_msgs, sizeof(recv_own_msgs));

   200

  }

   201

   202

  addr.can_family = AF_CAN;

   203

  addr.can_ifindex = ifr.ifr_ifindex;

   204

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

   205

  if (err)

   206

    {

   207

      fprintf (stderr, "Binding failed: %s\n", strerror (CAN_ERRNO (err)));

   208

      goto error_close;

   209

    }

   210

   211

#ifdef RTCAN_SOCKET

   212

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   213

  *baudrate = TranslateBaudRate (board->baudrate);

   214

  if (!*baudrate)

   215

    goto error_close;

   216

   217

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

   218

  if (err)

   219

    {

   220

      fprintf (stderr,

   221

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

   222

	       *baudrate, strerror (CAN_ERRNO (err)));

   223

      goto error_close;

   224

    }

   225

   226

  mode = (can_mode_t *) & ifr.ifr_ifru;

   227

  *mode = CAN_MODE_START;

   228

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

   229

  if (err)

   230

    {

   231

      fprintf (stderr, "Starting CAN device failed: %s\n",

   232

	       strerror (CAN_ERRNO (err)));

   233

      goto error_close;

   234

    }

   235

#endif

   236

   237

  return fd0;

   238

   239

error_close:

   240

  CAN_CLOSE (*(int *) fd0);

   241

   242

error_ret:

   243

  free (fd0);

   244

  return NULL;

   245

}

   246

   247

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

   248

int

   249

canClose_driver (CAN_HANDLE fd0)

   250

{

   251

  if (fd0)

   252

    {

   253

      CAN_CLOSE (*(int *) fd0);

   254

      free (fd0);

   255

    }

   256

  return 0;

   257

}