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_ERRNO(err) (-err)

    41

#else

    42

#include <sys/socket.h>

    43

#include <sys/ioctl.h>

    44

#include "linux/can.h"

    45

#include "linux/can/raw.h"

    46

#include "net/if.h"

    47

#define PF_CAN 29

    48

#define AF_CAN PF_CAN

    49

//#include "af_can.h"

    50

#define CAN_IFNAME     "can%s"

    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_ERRNO(err) errno

    58

#endif

    59

    60

#include "can_driver.h"

    61

    62

#if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON

    63

#include "def.h"

    64

    65

#define MyCase(fc) case fc: printf(#fc);break;

    66

void print_message(Message *m)

    67

{

    68

    int i;

    69

    switch(m->cob_id.w >> 7)

    70

    {

    71

        MyCase(SYNC)

    72

        MyCase(TIME_STAMP)

    73

        MyCase(PDO1tx)

    74

        MyCase(PDO1rx)

    75

        MyCase(PDO2tx)

    76

        MyCase(PDO2rx)

    77

        MyCase(PDO3tx)

    78

        MyCase(PDO3rx)

    79

        MyCase(PDO4tx)

    80

        MyCase(PDO4rx)

    81

        MyCase(SDOtx)

    82

        MyCase(SDOrx)

    83

        MyCase(NODE_GUARD)

    84

        MyCase(NMT)

    85

    }

    86

    printf(" rtr:%d", m->rtr);

    87

    printf(" len:%d", m->len);

    88

    for (i = 0 ; i < m->len ; i++)

    89

        printf(" %02x", m->data[i]);

    90

    printf("\n");

    91

}

    92

    93

#endif

    94

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

    95

UNS8

    96

canReceive_driver (CAN_HANDLE fd0, Message * m)

    97

{

    98

  int res;

    99

  struct can_frame frame;

   100

   101

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

   102

  if (res < 0)

   103

    {

   104

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

   105

      return 1;

   106

    }

   107

   108

  m->cob_id.w = frame.can_id & CAN_EFF_MASK;

   109

  m->len = frame.can_dlc;

   110

  if (frame.can_id & CAN_RTR_FLAG)

   111

    m->rtr = 1;

   112

  else

   113

    m->rtr = 0;

   114

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

   115

   116

#if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON

   117

  printf("in : ");

   118

  print_message(m);

   119

#endif

   120

  return 0;

   121

}

   122

   123

   124

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

   125

UNS8

   126

canSend_driver (CAN_HANDLE fd0, Message * m)

   127

{

   128

  int res;

   129

  struct can_frame frame;

   130

   131

  frame.can_id = m->cob_id.w;

   132

  if (frame.can_id >= 0x800)

   133

    frame.can_id |= CAN_EFF_FLAG;

   134

  frame.can_dlc = m->len;

   135

  if (m->rtr)

   136

    frame.can_id |= CAN_RTR_FLAG;

   137

  else

   138

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

   139

   140

#if defined DEBUG_WAR_CONSOLE_ON || defined DEBUG_ERR_CONSOLE_ON

   141

  printf("out : ");

   142

  print_message(m);

   143

#endif

   144

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

   145

  if (res < 0)

   146

    {

   147

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

   148

      return 1;

   149

    }

   150

   151

  return 0;

   152

}

   153

   154

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

   155

#ifdef RTCAN_SOCKET

   156

int

   157

TranslateBaudRate (const char *optarg)

   158

{

   159

  int baudrate;

   160

  int val, len;

   161

  char *pos = NULL;

   162

   163

  len = strlen (optarg);

   164

  if (!len)

   165

    return 0;

   166

   167

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

   168

    {

   169

    case 'M':

   170

      baudrate = 1000000;

   171

      break;

   172

    case 'K':

   173

      baudrate = 1000;

   174

      break;

   175

    default:

   176

      baudrate = 1;

   177

      break;

   178

    }

   179

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

   180

    baudrate *= val;

   181

  else

   182

    baudrate = 0;;

   183

   184

  return baudrate;

   185

}

   186

#endif

   187

   188

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

   189

CAN_HANDLE

   190

canOpen_driver (s_BOARD * board)

   191

{

   192

  struct ifreq ifr;

   193

  struct sockaddr_can addr;

   194

  int err;

   195

  CAN_HANDLE fd0 = malloc (sizeof (int));

   196

#ifdef RTCAN_SOCKET

   197

  can_baudrate_t *baudrate;

   198

  can_mode_t *mode;

   199

#endif

   200

   201

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

   202

  if (*(int *) fd0 < 0)

   203

    {

   204

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

   205

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

   206

      goto error_ret;

   207

    }

   208

   209

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

   210

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

   211

  else

   212

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

   213

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

   214

  if (err)

   215

    {

   216

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

   217

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

   218

      goto error_close;

   219

    }

   220

   221

  {

   222

    int loopback = 1;

   223

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

   224

               &loopback, sizeof(loopback));

   225

  }

   226

   227

  {

   228

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

   229

   230

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

   231

               &recv_own_msgs, sizeof(recv_own_msgs));

   232

  }

   233

   234

  addr.can_family = AF_CAN;

   235

  addr.can_ifindex = ifr.ifr_ifindex;

   236

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

   237

  if (err)

   238

    {

   239

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

   240

      goto error_close;

   241

    }

   242

   243

#ifdef RTCAN_SOCKET

   244

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   245

  *baudrate = TranslateBaudRate (board->baudrate);

   246

  if (!*baudrate)

   247

    goto error_close;

   248

   249

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

   250

  if (err)

   251

    {

   252

      fprintf (stderr,

   253

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

   254

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

   255

      goto error_close;

   256

    }

   257

   258

  mode = (can_mode_t *) & ifr.ifr_ifru;

   259

  *mode = CAN_MODE_START;

   260

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

   261

  if (err)

   262

    {

   263

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

   264

	       strerror (CAN_ERRNO (err)));

   265

      goto error_close;

   266

    }

   267

#endif

   268

   269

  return fd0;

   270

   271

error_close:

   272

  CAN_CLOSE (*(int *) fd0);

   273

   274

error_ret:

   275

  free (fd0);

   276

  return NULL;

   277

}

   278

   279

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

   280

int

   281

canClose_driver (CAN_HANDLE fd0)

   282

{

   283

  if (fd0)

   284

    {

   285

      CAN_CLOSE (*(int *) fd0);

   286

      free (fd0);

   287

    }

   288

  return 0;

   289

}