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/can.h"

    33

#define CAN_IFNAME     "rtcan%s"

    34

#else

    35

#include <unistd.h>

    36

#include <sys/socket.h>

    37

#include <sys/ioctl.h>

    38

#include "linux/can.h"

    39

#include "linux/can/raw.h"

    40

#include "net/if.h"

    41

#ifndef PF_CAN

    42

#define PF_CAN 29

    43

#endif

    44

#ifndef AF_CAN

    45

#define AF_CAN PF_CAN

    46

#endif

    47

//#include "af_can.h"

    48

#define CAN_IFNAME     "can%s"

    49

#endif

    50

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

    58

#define CAN_ERRNO(err) errno

    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 const * 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

  nanosecs_rel_t tx_timeout;

   172

  can_baudrate_t *baudrate;

   173

  can_mode_t *mode;

   174

#endif

   175

   176

  if(!fd0)

   177

    {

   178

      return NULL;

   179

    }

   180

   181

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

   182

  if (*(int *) fd0 < 0)

   183

    {

   184

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

   185

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

   186

      goto error_ret;

   187

    }

   188

   189

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

   190

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

   191

  else

   192

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

   193

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

   194

  if (err)

   195

    {

   196

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

   197

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

   198

      goto error_close;

   199

    }

   200

   201

  {

   202

    int loopback = 1;

   203

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

   204

               &loopback, sizeof(loopback));

   205

    if (err) {

   206

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

   207

        goto error_close;

   208

    }

   209

  }

   210

   211

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

   212

  {

   213

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

   214

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

   215

               &recv_own_msgs, sizeof(recv_own_msgs));

   216

    if (err) {

   217

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

   218

        goto error_close;

   219

    }

   220

  }

   221

#endif

   222

   223

  addr.can_family = AF_CAN;

   224

  addr.can_ifindex = ifr.ifr_ifindex;

   225

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

   226

  if (err)

   227

    {

   228

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

   229

      goto error_close;

   230

    }

   231

   232

#ifdef RTCAN_SOCKET

   233

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

   234

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

   235

  if (err)

   236

    {

   237

      fprintf (stderr,

   238

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

   239

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

   240

      goto error_close;

   241

    }

   242

   243

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   244

  *baudrate = TranslateBaudRate (board->baudrate);

   245

  if (!*baudrate)

   246

    goto error_close;

   247

   248

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

   249

  if (err)

   250

    {

   251

      fprintf (stderr,

   252

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

   253

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

   254

      goto error_close;

   255

    }

   256

   257

  mode = (can_mode_t *) & ifr.ifr_ifru;

   258

  *mode = CAN_MODE_START;

   259

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

   260

  if (err)

   261

    {

   262

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

   263

	       strerror (CAN_ERRNO (err)));

   264

      goto error_close;

   265

    }

   266

#endif

   267

   268

  return fd0;

   269

   270

error_close:

   271

  CAN_CLOSE (*(int *) fd0);

   272

   273

error_ret:

   274

  free (fd0);

   275

  return NULL;

   276

}

   277

   278

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

   279

int

   280

canClose_driver (CAN_HANDLE fd0)

   281

{

   282

  if (fd0)

   283

    {

   284

      CAN_CLOSE (*(int *) fd0);

   285

      free (fd0);

   286

    }

   287

  return 0;

   288

}