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 <sys/socket.h>

    44

#include <sys/ioctl.h>

    45

#include "linux/can.h"

    46

#include "linux/can/raw.h"

    47

#include "net/if.h"

    48

#define PF_CAN 29

    49

#define AF_CAN PF_CAN

    50

//#include "af_can.h"

    51

#define CAN_IFNAME     "can%s"

    52

#define CAN_SOCKET     socket

    53

#define CAN_CLOSE      close

    54

#define CAN_RECV       recv

    55

#define CAN_SEND       send

    56

#define CAN_BIND       bind

    57

#define CAN_IOCTL      ioctl

    58

#define CAN_ERRNO(err) errno

    59

#define CAN_SETSOCKOPT setsockopt

    60

#endif

    61

    62

#include "can_driver.h"

    63

    64

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

    65

UNS8

    66

canReceive_driver (CAN_HANDLE fd0, Message * m)

    67

{

    68

  int res;

    69

  struct can_frame frame;

    70

    71

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

    72

  if (res < 0)

    73

    {

    74

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

    75

      return 1;

    76

    }

    77

    78

  m->cob_id = frame.can_id & CAN_EFF_MASK;

    79

  m->len = frame.can_dlc;

    80

  if (frame.can_id & CAN_RTR_FLAG)

    81

    m->rtr = 1;

    82

  else

    83

    m->rtr = 0;

    84

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

    85

    86

#if defined DEBUG_MSG_CONSOLE_ON

    87

  MSG("in : ");

    88

  print_message(m);

    89

#endif

    90

  return 0;

    91

}

    92

    93

    94

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

    95

UNS8

    96

canSend_driver (CAN_HANDLE fd0, Message * m)

    97

{

    98

  int res;

    99

  struct can_frame frame;

   100

   101

  frame.can_id = m->cob_id;

   102

  if (frame.can_id >= 0x800)

   103

    frame.can_id |= CAN_EFF_FLAG;

   104

  frame.can_dlc = m->len;

   105

  if (m->rtr)

   106

    frame.can_id |= CAN_RTR_FLAG;

   107

  else

   108

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

   109

   110

#if defined DEBUG_MSG_CONSOLE_ON

   111

  MSG("out : ");

   112

  print_message(m);

   113

#endif

   114

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

   115

  if (res < 0)

   116

    {

   117

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

   118

      return 1;

   119

    }

   120

   121

  return 0;

   122

}

   123

   124

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

   125

#ifdef RTCAN_SOCKET

   126

int

   127

TranslateBaudRate (const char *optarg)

   128

{

   129

  int baudrate;

   130

  int val, len;

   131

  char *pos = NULL;

   132

   133

  len = strlen (optarg);

   134

  if (!len)

   135

    return 0;

   136

   137

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

   138

    {

   139

    case 'M':

   140

      baudrate = 1000000;

   141

      break;

   142

    case 'K':

   143

      baudrate = 1000;

   144

      break;

   145

    default:

   146

      baudrate = 1;

   147

      break;

   148

    }

   149

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

   150

    baudrate *= val;

   151

  else

   152

    baudrate = 0;;

   153

   154

  return baudrate;

   155

}

   156

#endif

   157

   158

UNS8 canChangeBaudRate_driver( CAN_HANDLE fd, char* baud)

   159

{

   160

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

   161

	return 0;

   162

}

   163

   164

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

   165

CAN_HANDLE

   166

canOpen_driver (s_BOARD * board)

   167

{

   168

  struct ifreq ifr;

   169

  struct sockaddr_can addr;

   170

  int err;

   171

  CAN_HANDLE fd0 = malloc (sizeof (int));

   172

#ifdef RTCAN_SOCKET

   173

  can_baudrate_t *baudrate;

   174

  can_mode_t *mode;

   175

#endif

   176

   177

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

   178

  if (*(int *) fd0 < 0)

   179

    {

   180

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

   181

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

   182

      goto error_ret;

   183

    }

   184

   185

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

   186

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

   187

  else

   188

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

   189

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

   190

  if (err)

   191

    {

   192

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

   193

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

   194

      goto error_close;

   195

    }

   196

   197

  {

   198

    int loopback = 1;

   199

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

   200

               &loopback, sizeof(loopback));

   201

    if (err) {

   202

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

   203

        goto error_close;

   204

    }

   205

  }

   206

   207

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

   208

  {

   209

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

   210

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

   211

               &recv_own_msgs, sizeof(recv_own_msgs));

   212

    if (err) {

   213

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

   214

        goto error_close;

   215

    }

   216

  }

   217

#endif

   218

   219

  addr.can_family = AF_CAN;

   220

  addr.can_ifindex = ifr.ifr_ifindex;

   221

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

   222

  if (err)

   223

    {

   224

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

   225

      goto error_close;

   226

    }

   227

   228

#ifdef RTCAN_SOCKET

   229

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   230

  *baudrate = TranslateBaudRate (board->baudrate);

   231

  if (!*baudrate)

   232

    goto error_close;

   233

   234

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

   235

  if (err)

   236

    {

   237

      fprintf (stderr,

   238

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

   239

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

   240

      goto error_close;

   241

    }

   242

   243

  mode = (can_mode_t *) & ifr.ifr_ifru;

   244

  *mode = CAN_MODE_START;

   245

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

   246

  if (err)

   247

    {

   248

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

   249

	       strerror (CAN_ERRNO (err)));

   250

      goto error_close;

   251

    }

   252

#endif

   253

   254

  return fd0;

   255

   256

error_close:

   257

  CAN_CLOSE (*(int *) fd0);

   258

   259

error_ret:

   260

  free (fd0);

   261

  return NULL;

   262

}

   263

   264

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

   265

int

   266

canClose_driver (CAN_HANDLE fd0)

   267

{

   268

  if (fd0)

   269

    {

   270

      CAN_CLOSE (*(int *) fd0);

   271

      free (fd0);

   272

    }

   273

  return 0;

   274

}