26

#include <stddef.h>		/* for NULL */

    40

#define CAN_ERRNO(err) (-err)

    57

#define CAN_ERRNO(err) errno

    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.w = 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

  return 0;

    85

}

    86

    87

    88

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

    89

UNS8

    90

canSend_driver (CAN_HANDLE fd0, Message * m)

    91

{

    92

  int res;

    93

  struct can_frame frame;

    94

    95

  frame.can_id = m->cob_id.w;

    96

  if (frame.can_id >= 0x800)

    97

    frame.can_id |= CAN_EFF_FLAG;

    98

  frame.can_dlc = m->len;

    99

  if (m->rtr)

   100

    frame.can_id |= CAN_RTR_FLAG;

   101

  else

   102

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

   103

   104

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

   105

  if (res < 0)

   106

    {

   107

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

   108

      return 1;

   109

    }

   110

   111

  return 0;

   112

}

   113

   114

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

   115

#ifdef RTCAN_SOCKET

   116

int

   117

TranslateBaudRate (const char *optarg)

   118

{

   119

  int baudrate;

   120

  int val, len;

   121

  char *pos = NULL;

   122

   123

  len = strlen (optarg);

   124

  if (!len)

   125

    return 0;

   126

   127

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

   128

    {

   129

    case 'M':

   130

      baudrate = 1000000;

   131

      break;

   132

    case 'K':

   133

      baudrate = 1000;

   134

      break;

   135

    default:

   136

      baudrate = 1;

   137

      break;

   138

    }

   139

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

   140

    baudrate *= val;

   141

  else

   142

    baudrate = 0;;

   143

   144

  return baudrate;

   145

}

   146

#endif

   147

   148

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

   149

CAN_HANDLE

   150

canOpen_driver (s_BOARD * board)

   151

{

   152

  struct ifreq ifr;

   153

  struct sockaddr_can addr;

   154

  int err;

   155

  CAN_HANDLE fd0 = malloc (sizeof (int));

   156

#ifdef RTCAN_SOCKET

   157

  can_baudrate_t *baudrate;

   158

  can_mode_t *mode;

   159

#endif

   160

   161

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

   162

  if (*(int *) fd0 < 0)

   163

    {

   164

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

   165

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

   166

      goto error_ret;

   167

    }

   168

   169

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

   170

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

   171

  else

   172

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

   173

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

   174

  if (err)

   175

    {

   176

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

   177

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

   178

      goto error_close;

   179

    }

   180

   181

  addr.can_family = AF_CAN;

   182

  addr.can_ifindex = ifr.ifr_ifindex;

   183

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

   184

  if (err)

   185

    {

   186

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

   187

      goto error_close;

   188

    }

   189

   190

#ifdef RTCAN_SOCKET

   191

  baudrate = (can_baudrate_t *) & ifr.ifr_ifru;

   192

  *baudrate = TranslateBaudRate (board->baudrate);

   193

  if (!*baudrate)

   194

    goto error_close;

   195

   196

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

   197

  if (err)

   198

    {

   199

      fprintf (stderr,

   200

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

   201

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

   202

      goto error_close;

   203

    }

   204

   205

  mode = (can_mode_t *) & ifr.ifr_ifru;

   206

  *mode = CAN_MODE_START;

   207

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

   208

  if (err)

   209

    {

   210

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

   211

	       strerror (CAN_ERRNO (err)));

   212

      goto error_close;

   213

    }

   214

#endif

   215

   216

  return fd0;

   217

   218

error_close:

   219

  CAN_CLOSE (*(int *) fd0);

   220

   221

error_ret:

   222

  free (fd0);

   223

  return NULL;

   224

}

   225

   226

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

   227

int

   228

canClose_driver (CAN_HANDLE fd0)

   229

{

   230

  if (fd0)

   231

    {

   232

      CAN_CLOSE (*(int *) fd0);

   233

      free (fd0);

   234

    }

   235

  return 0;

   236

}