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 = 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

  MSG("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;

   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

  MSG("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

UNS8 canChangeBaudRate_driver( CAN_HANDLE fd, char* baud)

   157

{

   158

	printf("canChangeBaudRate not yet supported by this driver\n");

   159

	return 0;

   160

}

   161

   162

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

   163

CAN_HANDLE

   164

canOpen_driver (s_BOARD * board)

   165

{

   166

  struct ifreq ifr;

   167

  struct sockaddr_can addr;

   168

  int err;

   169

  CAN_HANDLE fd0 = malloc (sizeof (int));

   170

#ifdef RTCAN_SOCKET

   171

  can_baudrate_t *baudrate;

   172

  can_mode_t *mode;

   173

#endif

   174

   175

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

   176

  if (*(int *) fd0 < 0)

   177

    {

   178

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

   179

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

   180

      goto error_ret;

   181

    }

   182

   183

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

   184

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

   185

  else

   186

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

   187

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

   188

  if (err)

   189

    {

   190

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

   191

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

   192

      goto error_close;

   193

    }

   194

   195

  {

   196

    int loopback = 1;

   197

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

   198

               &loopback, sizeof(loopback));

   199

  }

   200

   201

  {

   202

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

   203

   204

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

   205

               &recv_own_msgs, sizeof(recv_own_msgs));

   206

  }

   207

   208

  addr.can_family = AF_CAN;

   209

  addr.can_ifindex = ifr.ifr_ifindex;

   210

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

   211

  if (err)

   212

    {

   213

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

   214

      goto error_close;

   215

    }

   216

   217

#ifdef RTCAN_SOCKET

   218

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   219

  *baudrate = TranslateBaudRate (board->baudrate);

   220

  if (!*baudrate)

   221

    goto error_close;

   222

   223

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

   224

  if (err)

   225

    {

   226

      fprintf (stderr,

   227

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

   228

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

   229

      goto error_close;

   230

    }

   231

   232

  mode = (can_mode_t *) & ifr.ifr_ifru;

   233

  *mode = CAN_MODE_START;

   234

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

   235

  if (err)

   236

    {

   237

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

   238

	       strerror (CAN_ERRNO (err)));

   239

      goto error_close;

   240

    }

   241

#endif

   242

   243

  return fd0;

   244

   245

error_close:

   246

  CAN_CLOSE (*(int *) fd0);

   247

   248

error_ret:

   249

  free (fd0);

   250

  return NULL;

   251

}

   252

   253

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

   254

int

   255

canClose_driver (CAN_HANDLE fd0)

   256

{

   257

  if (fd0)

   258

    {

   259

      CAN_CLOSE (*(int *) fd0);

   260

      free (fd0);

   261

    }

   262

  return 0;

   263

}