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

#include "can_driver.h"

    52

    53

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

    54

UNS8

    55

canReceive_driver (CAN_HANDLE fd0, Message * m)

    56

{

    57

  int res = 0;

    58

  struct can_frame frame;

    59

    60

  while (res <= 0) {

    61

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

    62

    if (res < 0)

    63

    {

    64

        if (errno == EBADF)

    65

        {

    66

            /* this will exit recieve loop */

    67

            return 1;

    68

        }

    69

        else if (errno == EINTR) continue; /* retry immediatly */

    70

        else

    71

        {

    72

            /* print error and retry after 20ms, arbitrary */

    73

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

    74

            usleep(20000);

    75

            continue;

    76

        }

    77

    }

    78

    else if (res == 0)

    79

    {

    80

        /* this will exit recieve loop */

    81

        return 1;

    82

    }

    83

  } 

    84

  m->cob_id = frame.can_id & CAN_EFF_MASK;

    85

  m->len = frame.can_dlc;

    86

  if (frame.can_id & CAN_RTR_FLAG)

    87

    m->rtr = 1;

    88

  else

    89

    m->rtr = 0;

    90

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

    91

    92

#if defined DEBUG_MSG_CONSOLE_ON

    93

  MSG("in : ");

    94

  print_message(m);

    95

#endif

    96

  return 0;

    97

}

    98

    99

   100

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

   101

UNS8

   102

canSend_driver (CAN_HANDLE fd0, Message const * m)

   103

{

   104

  int res;

   105

  struct can_frame frame;

   106

   107

  frame.can_id = m->cob_id;

   108

  if (frame.can_id >= 0x800)

   109

    frame.can_id |= CAN_EFF_FLAG;

   110

  frame.can_dlc = m->len;

   111

  if (m->rtr)

   112

    frame.can_id |= CAN_RTR_FLAG;

   113

  else

   114

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

   115

   116

#if defined DEBUG_MSG_CONSOLE_ON

   117

  MSG("out : ");

   118

  print_message(m);

   119

#endif

   120

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

   121

  if (res <= 0)

   122

    {

   123

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

   124

      return 1;

   125

    }

   126

   127

  return 0;

   128

}

   129

   130

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

   131

#ifdef RTCAN_SOCKET

   132

int

   133

TranslateBaudRate (const char *optarg)

   134

{

   135

  int baudrate;

   136

  int val, len;

   137

  char *pos = NULL;

   138

   139

  len = strlen (optarg);

   140

  if (!len)

   141

    return 0;

   142

   143

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

   144

    {

   145

    case 'M':

   146

      baudrate = 1000000;

   147

      break;

   148

    case 'K':

   149

      baudrate = 1000;

   150

      break;

   151

    default:

   152

      baudrate = 1;

   153

      break;

   154

    }

   155

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

   156

    baudrate *= val;

   157

  else

   158

    baudrate = 0;;

   159

   160

  return baudrate;

   161

}

   162

#endif

   163

   164

UNS8 canChangeBaudRate_driver( CAN_HANDLE fd, char* baud)

   165

{

   166

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

   167

	return 0;

   168

}

   169

   170

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

   171

CAN_HANDLE

   172

canOpen_driver (s_BOARD * board)

   173

{

   174

  struct ifreq ifr;

   175

  struct sockaddr_can addr;

   176

  int err;

   177

  CAN_HANDLE fd0 = malloc (sizeof (int));

   178

#ifdef RTCAN_SOCKET

   179

  nanosecs_rel_t tx_timeout;

   180

  can_baudrate_t *baudrate;

   181

  can_mode_t *mode;

   182

#endif

   183

   184

  if(!fd0)

   185

    {

   186

      return NULL;

   187

    }

   188

   189

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

   190

  if (*(int *) fd0 < 0)

   191

    {

   192

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

   193

	       strerror (errno));

   194

      goto error_ret;

   195

    }

   196

   197

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

   198

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

   199

  else

   200

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

   201

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

   202

  if (err)

   203

    {

   204

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

   205

	       ifr.ifr_name, strerror (errno));

   206

      goto error_close;

   207

    }

   208

   209

  {

   210

    int loopback = 1;

   211

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

   212

               &loopback, sizeof(loopback));

   213

    if (err) {

   214

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

   215

        goto error_close;

   216

    }

   217

  }

   218

   219

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

   220

  {

   221

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

   222

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

   223

               &recv_own_msgs, sizeof(recv_own_msgs));

   224

    if (err) {

   225

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

   226

        goto error_close;

   227

    }

   228

  }

   229

#endif

   230

   231

  addr.can_family = AF_CAN;

   232

  addr.can_ifindex = ifr.ifr_ifindex;

   233

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

   234

  if (err)

   235

    {

   236

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

   237

      goto error_close;

   238

    }

   239

   240

#ifdef RTCAN_SOCKET

   241

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

   242

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

   243

  if (err)

   244

    {

   245

      fprintf (stderr,

   246

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

   247

          *baudrate, strerror (errno));

   248

      goto error_close;

   249

    }

   250

   251

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   252

  *baudrate = TranslateBaudRate (board->baudrate);

   253

  if (!*baudrate)

   254

    goto error_close;

   255

   256

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

   257

  if (err)

   258

    {

   259

      fprintf (stderr,

   260

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

   261

	       *baudrate, strerror (errno));

   262

      goto error_close;

   263

    }

   264

   265

  mode = (can_mode_t *) & ifr.ifr_ifru;

   266

  *mode = CAN_MODE_START;

   267

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

   268

  if (err)

   269

    {

   270

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

   271

	       strerror (errno));

   272

      goto error_close;

   273

    }

   274

#endif

   275

   276

  return fd0;

   277

   278

error_close:

   279

  close (*(int *) fd0);

   280

   281

error_ret:

   282

  free (fd0);

   283

  return NULL;

   284

}

   285

   286

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

   287

int

   288

canClose_driver (CAN_HANDLE fd0)

   289

{

   290

  if (fd0)

   291

    {

   292

      shutdown (*(int *) fd0, SHUT_RDWR);

   293

      close (*(int *) fd0);

   294

      free (fd0);

   295

    }

   296

  return 0;

   297

}