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

    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_SETSOCKOPT rt_dev_setsockopt

    41

#define CAN_ERRNO(err) (-err)

    42

#else

    43

#include <unistd.h>

    44

#include <sys/socket.h>

    45

#include <sys/ioctl.h>

    46

#include "linux/can.h"

    47

#include "linux/can/raw.h"

    48

#include "net/if.h"

    49

#ifndef PF_CAN

    50

#define PF_CAN 29

    51

#endif

    52

#ifndef AF_CAN

    53

#define AF_CAN PF_CAN

    54

#endif

    55

//#include "af_can.h"

    56

#define CAN_IFNAME     "can%s"

    57

#define CAN_SOCKET     socket

    58

#define CAN_CLOSE      close

    59

#define CAN_RECV       recv

    60

#define CAN_SEND       send

    61

#define CAN_BIND       bind

    62

#define CAN_IOCTL      ioctl

    63

#define CAN_ERRNO(err) errno

    64

#define CAN_SETSOCKOPT setsockopt

    65

#endif

    66

    67

#include "can_driver.h"

    68

    69

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

    70

UNS8

    71

canReceive_driver (CAN_HANDLE fd0, Message * m)

    72

{

    73

  int res;

    74

  struct can_frame frame;

    75

    76

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

    77

  if (res < 0)

    78

    {

    79

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

    80

      return 1;

    81

    }

    82

    83

  m->cob_id = frame.can_id & CAN_EFF_MASK;

    84

  m->len = frame.can_dlc;

    85

  if (frame.can_id & CAN_RTR_FLAG)

    86

    m->rtr = 1;

    87

  else

    88

    m->rtr = 0;

    89

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

    90

    91

#if defined DEBUG_MSG_CONSOLE_ON

    92

  MSG("in : ");

    93

  print_message(m);

    94

#endif

    95

  return 0;

    96

}

    97

    98

    99

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

   100

UNS8

   101

canSend_driver (CAN_HANDLE fd0, Message const * m)

   102

{

   103

  int res;

   104

  struct can_frame frame;

   105

   106

  frame.can_id = m->cob_id;

   107

  if (frame.can_id >= 0x800)

   108

    frame.can_id |= CAN_EFF_FLAG;

   109

  frame.can_dlc = m->len;

   110

  if (m->rtr)

   111

    frame.can_id |= CAN_RTR_FLAG;

   112

  else

   113

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

   114

   115

#if defined DEBUG_MSG_CONSOLE_ON

   116

  MSG("out : ");

   117

  print_message(m);

   118

#endif

   119

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

   120

  if (res < 0)

   121

    {

   122

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

   123

      return 1;

   124

    }

   125

   126

  return 0;

   127

}

   128

   129

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

   130

#ifdef RTCAN_SOCKET

   131

int

   132

TranslateBaudRate (const char *optarg)

   133

{

   134

  int baudrate;

   135

  int val, len;

   136

  char *pos = NULL;

   137

   138

  len = strlen (optarg);

   139

  if (!len)

   140

    return 0;

   141

   142

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

   143

    {

   144

    case 'M':

   145

      baudrate = 1000000;

   146

      break;

   147

    case 'K':

   148

      baudrate = 1000;

   149

      break;

   150

    default:

   151

      baudrate = 1;

   152

      break;

   153

    }

   154

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

   155

    baudrate *= val;

   156

  else

   157

    baudrate = 0;;

   158

   159

  return baudrate;

   160

}

   161

#endif

   162

   163

UNS8 canChangeBaudRate_driver( CAN_HANDLE fd, char* baud)

   164

{

   165

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

   166

	return 0;

   167

}

   168

   169

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

   170

CAN_HANDLE

   171

canOpen_driver (s_BOARD * board)

   172

{

   173

  struct ifreq ifr;

   174

  struct sockaddr_can addr;

   175

  int err;

   176

  CAN_HANDLE fd0 = malloc (sizeof (int));

   177

#ifdef RTCAN_SOCKET

   178

  nanosecs_rel_t tx_timeout;

   179

  can_baudrate_t *baudrate;

   180

  can_mode_t *mode;

   181

#endif

   182

   183

  if(!fd0)

   184

    {

   185

      return NULL;

   186

    }

   187

   188

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

   189

  if (*(int *) fd0 < 0)

   190

    {

   191

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

   192

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

   193

      goto error_ret;

   194

    }

   195

   196

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

   197

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

   198

  else

   199

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

   200

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

   201

  if (err)

   202

    {

   203

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

   204

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

   205

      goto error_close;

   206

    }

   207

   208

  {

   209

    int loopback = 1;

   210

    err = CAN_SETSOCKOPT(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_LOOPBACK,

   211

               &loopback, sizeof(loopback));

   212

    if (err) {

   213

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

   214

        goto error_close;

   215

    }

   216

  }

   217

   218

#ifndef RTCAN_SOCKET /*CAN_RAW_RECV_OWN_MSGS not supported in rtsocketcan*/

   219

  {

   220

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

   221

    err = CAN_SETSOCKOPT(*(int *)fd0, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,

   222

               &recv_own_msgs, sizeof(recv_own_msgs));

   223

    if (err) {

   224

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

   225

        goto error_close;

   226

    }

   227

  }

   228

#endif

   229

   230

  addr.can_family = AF_CAN;

   231

  addr.can_ifindex = ifr.ifr_ifindex;

   232

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

   233

  if (err)

   234

    {

   235

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

   236

      goto error_close;

   237

    }

   238

   239

#ifdef RTCAN_SOCKET

   240

  tx_timeout = 10000000;    /* XXX Timeout is temporarily set to 10 ms */

   241

  err = CAN_IOCTL (*(int *) fd0, RTCAN_RTIOC_SND_TIMEOUT, &tx_timeout);

   242

  if (err)

   243

    {

   244

      fprintf (stderr,

   245

          "Setting TX timeout %d failed: %s\n",

   246

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

   247

      goto error_close;

   248

    }

   249

   250

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   251

  *baudrate = TranslateBaudRate (board->baudrate);

   252

  if (!*baudrate)

   253

    goto error_close;

   254

   255

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

   256

  if (err)

   257

    {

   258

      fprintf (stderr,

   259

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

   260

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

   261

      goto error_close;

   262

    }

   263

   264

  mode = (can_mode_t *) & ifr.ifr_ifru;

   265

  *mode = CAN_MODE_START;

   266

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

   267

  if (err)

   268

    {

   269

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

   270

	       strerror (CAN_ERRNO (err)));

   271

      goto error_close;

   272

    }

   273

#endif

   274

   275

  return fd0;

   276

   277

error_close:

   278

  CAN_CLOSE (*(int *) fd0);

   279

   280

error_ret:

   281

  free (fd0);

   282

  return NULL;

   283

}

   284

   285

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

   286

int

   287

canClose_driver (CAN_HANDLE fd0)

   288

{

   289

  if (fd0)

   290

    {

   291

      CAN_CLOSE (*(int *) fd0);

   292

      free (fd0);

   293

    }

   294

  return 0;

   295

}