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

  can_baudrate_t *baudrate;

   179

  can_mode_t *mode;

   180

#endif

   181

   182

  if(!fd0)

   183

    {

   184

      return NULL;

   185

    }

   186

   187

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

   188

  if (*(int *) fd0 < 0)

   189

    {

   190

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

   191

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

   192

      goto error_ret;

   193

    }

   194

   195

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

   196

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

   197

  else

   198

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

   199

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

   200

  if (err)

   201

    {

   202

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

   203

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

   204

      goto error_close;

   205

    }

   206

   207

  {

   208

    int loopback = 1;

   209

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

   210

               &loopback, sizeof(loopback));

   211

    if (err) {

   212

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

   213

        goto error_close;

   214

    }

   215

  }

   216

   217

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

   218

  {

   219

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

   220

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

   221

               &recv_own_msgs, sizeof(recv_own_msgs));

   222

    if (err) {

   223

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

   224

        goto error_close;

   225

    }

   226

  }

   227

#endif

   228

   229

  addr.can_family = AF_CAN;

   230

  addr.can_ifindex = ifr.ifr_ifindex;

   231

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

   232

  if (err)

   233

    {

   234

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

   235

      goto error_close;

   236

    }

   237

   238

#ifdef RTCAN_SOCKET

   239

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   240

  *baudrate = TranslateBaudRate (board->baudrate);

   241

  if (!*baudrate)

   242

    goto error_close;

   243

   244

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

   245

  if (err)

   246

    {

   247

      fprintf (stderr,

   248

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

   249

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

   250

      goto error_close;

   251

    }

   252

   253

  mode = (can_mode_t *) & ifr.ifr_ifru;

   254

  *mode = CAN_MODE_START;

   255

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

   256

  if (err)

   257

    {

   258

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

   259

	       strerror (CAN_ERRNO (err)));

   260

      goto error_close;

   261

    }

   262

#endif

   263

   264

  return fd0;

   265

   266

error_close:

   267

  CAN_CLOSE (*(int *) fd0);

   268

   269

error_ret:

   270

  free (fd0);

   271

  return NULL;

   272

}

   273

   274

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

   275

int

   276

canClose_driver (CAN_HANDLE fd0)

   277

{

   278

  if (fd0)

   279

    {

   280

      CAN_CLOSE (*(int *) fd0);

   281

      free (fd0);

   282

    }

   283

  return 0;

   284

}