nuttx-apps/examples/opencyphal/socketcan.c

/****************************************************************************
 * apps/examples/opencyphal/socketcan.c
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.  The
 * ASF licenses this file to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance with the
 * License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include "socketcan.h"

#include <net/if.h>
#include <sys/ioctl.h>
#include <string.h>
#include <stdio.h>

/****************************************************************************
 * Public Functions
 ****************************************************************************/

int16_t socketcanopen(canardsocketinstance *ins,
                      const char *const can_iface_name, const bool can_fd)
{
  struct sockaddr_can addr;
  struct ifreq ifr;

  ins->can_fd = can_fd;

  /* open socket */

  if ((ins->s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
    {
      perror("socket");
      return -1;
    }

  strlcpy(ifr.ifr_name, can_iface_name, IFNAMSIZ);
  ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);

  if (!ifr.ifr_ifindex)
    {
      perror("if_nametoindex");
      return -1;
    }

  memset(&addr, 0, sizeof(addr));
  addr.can_family = AF_CAN;
  addr.can_ifindex = ifr.ifr_ifindex;

  const int on = 1;

  /* RX Timestamping */

  if (setsockopt(ins->s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0)
    {
      perror("SO_TIMESTAMP is disabled");
      return -1;
    }

  /* NuttX Feature: Enable TX deadline when sending CAN frames
   * When a deadline occurs the driver will remove the CAN frame
   */

  if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE,
                 &on, sizeof(on)) < 0)
    {
      perror("CAN_RAW_TX_DEADLINE is disabled");
      return -1;
    }

  if (can_fd)
    {
      if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES,
                     &on, sizeof(on)) < 0)
        {
          perror("no CAN FD support");
          return -1;
        }
    }

  if (bind(ins->s, (struct sockaddr *)&addr, sizeof(addr)) < 0)
    {
      perror("bind");
      return -1;
    }

  /* Setup TX msg */

  ins->send_iov.iov_base = &ins->send_frame;

  if (ins->can_fd)
    {
      ins->send_iov.iov_len = sizeof(struct canfd_frame);
    }
  else
    {
      ins->send_iov.iov_len = sizeof(struct can_frame);
    }

  memset(&ins->send_control, 0x00, sizeof(ins->send_control));

  ins->send_msg.msg_iov    = &ins->send_iov;
  ins->send_msg.msg_iovlen = 1;
  ins->send_msg.msg_control = &ins->send_control;
  ins->send_msg.msg_controllen = sizeof(ins->send_control);

  ins->send_cmsg = CMSG_FIRSTHDR(&ins->send_msg);
  ins->send_cmsg->cmsg_level = SOL_CAN_RAW;
  ins->send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE;
  ins->send_cmsg->cmsg_len = sizeof(struct timeval);
  ins->send_tv = (struct timeval *)CMSG_DATA(&ins->send_cmsg);

  /* Setup RX msg */

  ins->recv_iov.iov_base = &ins->recv_frame;

  if (can_fd)
    {
      ins->recv_iov.iov_len = sizeof(struct canfd_frame);
    }
  else
    {
      ins->recv_iov.iov_len = sizeof(struct can_frame);
    }

  memset(&ins->recv_control, 0x00, sizeof(ins->recv_control));

  ins->recv_msg.msg_iov    = &ins->recv_iov;
  ins->recv_msg.msg_iovlen = 1;
  ins->recv_msg.msg_control = &ins->recv_control;
  ins->recv_msg.msg_controllen = sizeof(ins->recv_control);
  ins->recv_cmsg = CMSG_FIRSTHDR(&ins->recv_msg);

  return 0;
}

int16_t socketcantransmit(canardsocketinstance *ins, const CanardFrame *txf)
{
  /* Copy CanardFrame to can_frame/canfd_frame */

  if (ins->can_fd)
    {
      ins->send_frame.can_id = txf->extended_can_id;
      ins->send_frame.can_id |= CAN_EFF_FLAG;
      ins->send_frame.len = txf->payload_size;
      memcpy(&ins->send_frame.data, txf->payload, txf->payload_size);
    }
  else
    {
      struct can_frame *frame = (struct can_frame *)&ins->send_frame;
      frame->can_id = txf->extended_can_id;
      frame->can_id |= CAN_EFF_FLAG;
      frame->can_dlc = txf->payload_size;
      memcpy(&frame->data, txf->payload, txf->payload_size);
    }

  /* Set CAN_RAW_TX_DEADLINE timestamp  */

  ins->send_tv->tv_usec = txf->timestamp_usec % 1000000ULL;
  ins->send_tv->tv_sec = (txf->timestamp_usec - ins->send_tv->tv_usec)
                         / 1000000ULL;

  return sendmsg(ins->s, &ins->send_msg, 0);
}

int16_t socketcanreceive(canardsocketinstance *ins, CanardFrame *rxf)
{
  int32_t result = recvmsg(ins->s, &ins->recv_msg, 0);

  if (result < 0)
    {
      return result;
    }

  /* Copy CAN frame to CanardFrame */

  if (ins->can_fd)
    {
      struct canfd_frame *recv_frame =
                         (struct canfd_frame *)&ins->recv_frame;
      rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
      rxf->payload_size = recv_frame->len;
      rxf->payload = &recv_frame->data;
    }
  else
    {
      struct can_frame *recv_frame = (struct can_frame *)&ins->recv_frame;
      rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
      rxf->payload_size = recv_frame->can_dlc;
      rxf->payload = &recv_frame->data;
    }

  /* Read SO_TIMESTAMP value */

  if (ins->recv_cmsg->cmsg_level == SOL_SOCKET
        && ins->recv_cmsg->cmsg_type == SO_TIMESTAMP)
    {
      struct timeval *tv = (struct timeval *)CMSG_DATA(ins->recv_cmsg);
      rxf->timestamp_usec = tv->tv_sec * 1000000ULL + tv->tv_usec;
    }

  return result;
}

/* TODO implement corresponding IOCTL */

int16_t socketcanconfigurefilter(const fd_t fd, const size_t num_filters,
                                 const struct can_filter *filters)
{
  return -1;
}