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

  return 0;

    85

}

    86

    87

    88

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

    89

UNS8

    90

canSend_driver (CAN_HANDLE fd0, Message * m)

    91

{

    92

  int res;

    93

  struct can_frame frame;

    94

    95

  frame.can_id = m->cob_id.w;

    96

  if (frame.can_id >= 0x800)

    97

    frame.can_id |= CAN_EFF_FLAG;

    98

  frame.can_dlc = m->len;

    99

  if (m->rtr)

   100

    frame.can_id |= CAN_RTR_FLAG;

   101

  else

   102

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

   103

   104

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

   105

  if (res < 0)

   106

    {

   107

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

   108

      return 1;

   109

    }

   110

   111

  return 0;

   112

}

   113

   114

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

   115

#ifdef RTCAN_SOCKET

   116

int

   117

TranslateBaudRate (const char *optarg)

   118

{

   119

  int baudrate;

   120

  int val, len;

   121

  char *pos = NULL;

   122

   123

  len = strlen (optarg);

   124

  if (!len)

   125

    return 0;

   126

   127

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

   128

    {

   129

    case 'M':

   130

      baudrate = 1000000;

   131

      break;

   132

    case 'K':

   133

      baudrate = 1000;

   134

      break;

   135

    default:

   136

      baudrate = 1;

   137

      break;

   138

    }

   139

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

   140

    baudrate *= val;

   141

  else

   142

    baudrate = 0;;

   143

   144

  return baudrate;

   145

}

   146

#endif

   147

   148

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

   149

CAN_HANDLE

   150

canOpen_driver (s_BOARD * board)

   151

{

   152

  struct ifreq ifr;

   153

  struct sockaddr_can addr;

   154

  int err;

   155

  CAN_HANDLE fd0 = malloc (sizeof (int));

   156

#ifdef RTCAN_SOCKET

   157

  can_baudrate_t *baudrate;

   158

  can_mode_t *mode;

   159

#endif

   160

   161

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

   162

  if (*(int *) fd0 < 0)

   163

    {

   164

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

   165

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

   166

      goto error_ret;

   167

    }

   168

   169

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

   170

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

   171

  else

   172

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

   173

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

   174

  if (err)

   175

    {

   176

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

   177

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

   178

      goto error_close;

   179

    }

   180

   181

  addr.can_family = AF_CAN;

   182

  addr.can_ifindex = ifr.ifr_ifindex;

   183

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

   184

  if (err)

   185

    {

   186

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

   187

      goto error_close;

   188

    }

   189

   190

#ifdef RTCAN_SOCKET

   191

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   192

  *baudrate = TranslateBaudRate (board->baudrate);

   193

  if (!*baudrate)

   194

    goto error_close;

   195

   196

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

   197

  if (err)

   198

    {

   199

      fprintf (stderr,

   200

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

   201

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

   202

      goto error_close;

   203

    }

   204

   205

  mode = (can_mode_t *) & ifr.ifr_ifru;

   206

  *mode = CAN_MODE_START;

   207

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

   208

  if (err)

   209

    {

   210

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

   211

	       strerror (CAN_ERRNO (err)));

   212

      goto error_close;

   213

    }

   214

#endif

   215

   216

  return fd0;

   217

   218

error_close:

   219

  CAN_CLOSE (*(int *) fd0);

   220

   221

error_ret:

   222

  free (fd0);

   223

  return NULL;

   224

}

   225

   226

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

   227

int

   228

canClose_driver (CAN_HANDLE fd0)

   229

{

   230

  if (fd0)

   231

    {

   232

      CAN_CLOSE (*(int *) fd0);

   233

      free (fd0);

   234

    }

   235

  return 0;

   236

}