canxl: add virtual CAN network identifier support

With the 'tx pass through' option CAN_RAW_XL_VCID_TX_PASS all valid VCID values can be send, e.g. to replay full qualified CAN XL traffic.

The VCID value provided for the CAN_RAW_XL_VCID_TX_SET option will override the VCID value in the struct canxl_frame.vcid element defined for CAN_RAW_XL_VCID_TX_PASS when both flags are set.

With a rx_vcid_mask of zero all possible VCID values (0x00 - 0xFF) are passed to the user space when the CAN_RAW_XL_VCID_RX_FILTER flag is set. Without this flag only untagged CAN XL frames (VCID = 0x00) are delivered to the user space.

Signed-off-by Oliver Hartkopp <[email protected]>

---
  include/uapi/linux/can.h     | 39 ++++++++++++++-------
  include/uapi/linux/can/raw.h | 14 ++++++++
  net/can/af_can.c             |  2 ++
  net/can/raw.c                | 66 ++++++++++++++++++++++++++++++++++--
  4 files changed, 105 insertions(+), 16 deletions(-)

include/uapi/linux/can.h +26 -13

index 939db2388208..b20268a944e6 100644
@@ -193,26 +193,39 @@ struct canfd_frame {
  #define CANXL_XLF 0x80 /* mandatory CAN XL frame flag (must always be set!) */
  #define CANXL_SEC 0x01 /* Simple Extended Content (security/segmentation) */
 
  /**
   * struct canxl_frame - CAN with e'X'tended frame 'L'ength frame structure
- * @prio:  11 bit arbitration priority with zero'ed CAN_*_FLAG flags
- * @flags: additional flags for CAN XL
- * @sdt:   SDU (service data unit) type
- * @len:   frame payload length in byte (CANXL_MIN_DLEN .. CANXL_MAX_DLEN)
- * @af:    acceptance field
- * @data:  CAN XL frame payload (CANXL_MIN_DLEN .. CANXL_MAX_DLEN byte)
+ * @prio:   11 bit arbitration priority with zero'ed CAN_*_FLAG flags
+ * @vcid:   virtual CAN network identifier
+ * @__res0: reserved / padding must be set to 0
+ * @flags:  additional flags for CAN XL
+ * @sdt:    SDU (service data unit) type
+ * @len:    frame payload length in byte (CANXL_MIN_DLEN .. CANXL_MAX_DLEN)
+ * @af:     acceptance field
+ * @data:   CAN XL frame payload (CANXL_MIN_DLEN .. CANXL_MAX_DLEN byte)
   *
- * @prio shares the same position as @can_id from struct can[fd]_frame.
+ * @prio shares the lower 16 bits of @can_id from struct can[fd]_frame.
+ * @__res0 holds the @can_id flags CAN_xxx_FLAG and has to be set to zero
   */
  struct canxl_frame {
-	canid_t prio;  /* 11 bit priority for arbitration (canid_t) */
-	__u8    flags; /* additional flags for CAN XL */
-	__u8    sdt;   /* SDU (service data unit) type */
-	__u16   len;   /* frame payload length in byte */
-	__u32   af;    /* acceptance field */
-	__u8    data[CANXL_MAX_DLEN];
+#if defined(__LITTLE_ENDIAN)
+	__u16 prio;   /* 11 bit priority for arbitration */
+	__u8  vcid;   /* virtual CAN network identifier */
+	__u8  __res0; /* reserved / padding must be set to 0 */
+#elif defined(__BIG_ENDIAN)
+	__u8  __res0; /* reserved / padding must be set to 0 */
+	__u8  vcid;   /* virtual CAN network identifier */
+	__u16 prio;   /* 11 bit priority for arbitration */
+#else
+#error "Unknown endianness"
+#endif
+	__u8  flags;  /* additional flags for CAN XL */
+	__u8  sdt;    /* SDU (service data unit) type */
+	__u16 len;    /* frame payload length in byte */
+	__u32 af;     /* acceptance field */
+	__u8  data[CANXL_MAX_DLEN];
  };
 
  #define CAN_MTU		(sizeof(struct can_frame))
  #define CANFD_MTU	(sizeof(struct canfd_frame))
  #define CANXL_MTU	(sizeof(struct canxl_frame))

include/uapi/linux/can/raw.h +14 -0

index 31622c9b7988..8890b0d2fd48 100644
@@ -63,8 +63,22 @@ enum {
  	CAN_RAW_LOOPBACK,	/* local loopback (default:on)       */
  	CAN_RAW_RECV_OWN_MSGS,	/* receive my own msgs (default:off) */
  	CAN_RAW_FD_FRAMES,	/* allow CAN FD frames (default:off) */
  	CAN_RAW_JOIN_FILTERS,	/* all filters must match to trigger */
  	CAN_RAW_XL_FRAMES,	/* allow CAN XL frames (default:off) */
+	CAN_RAW_XL_VCID_OPTS,	/* CAN XL VCID configuration options */
  };
 
+struct can_raw_vcid_options {
+
+	__u8 flags;		/* flags for vcid (filter) behaviour */
+	__u8 tx_vcid;		/* VCID value set into canxl_frame.prio */
+	__u8 rx_vcid;		/* VCID value for VCID filter */
+	__u8 rx_vcid_mask;	/* VCID mask for VCID filter */
+
+};
+
+#define CAN_RAW_XL_VCID_TX_SET		0x01
+#define CAN_RAW_XL_VCID_TX_PASS		0x02
+#define CAN_RAW_XL_VCID_RX_FILTER	0x04
+
  #endif /* !_UAPI_CAN_RAW_H */

net/can/af_can.c +2 -0

index 7343fd487dbe..707576eeeb58 100644
@@ -863,10 +863,12 @@ static __init int can_init(void)
  	int err;
 
  	/* check for correct padding to be able to use the structs similarly */
  	BUILD_BUG_ON(offsetof(struct can_frame, len) !=
  		     offsetof(struct canfd_frame, len) ||
+		     offsetof(struct can_frame, len) !=
+		     offsetof(struct canxl_frame, flags) ||
  		     offsetof(struct can_frame, data) !=
  		     offsetof(struct canfd_frame, data));
 
  	pr_info("can: controller area network core\n");
 

net/can/raw.c +63 -3

index e6b822624ba2..3083df64723c 100644
@@ -89,10 +89,11 @@ struct raw_sock {
  	struct list_head notifier;
  	int loopback;
  	int recv_own_msgs;
  	int fd_frames;
  	int xl_frames;
+	struct can_raw_vcid_options raw_vcid_opts;
  	int join_filters;
  	int count;                 /* number of active filters */
  	struct can_filter dfilter; /* default/single filter */
  	struct can_filter *filter; /* pointer to filter(s) */
  	can_err_mask_t err_mask;

@@ -132,14 +133,34 @@ static void raw_rcv(struct sk_buff *oskb, void *data)
  	/* check the received tx sock reference */
  	if (!ro->recv_own_msgs && oskb->sk == sk)
  		return;
 
  	/* make sure to not pass oversized frames to the socket */
-	if ((!ro->fd_frames && can_is_canfd_skb(oskb)) ||
-	    (!ro->xl_frames && can_is_canxl_skb(oskb)))
+	if (!ro->fd_frames && can_is_canfd_skb(oskb))
  		return;
 
+	if (can_is_canxl_skb(oskb)) {
+		struct canxl_frame *cxl = (struct canxl_frame *)oskb->data;
+		struct can_raw_vcid_options *ropts = &ro->raw_vcid_opts;
+
+		/* make sure to not pass oversized frames to the socket */
+		if (!ro->xl_frames)
+			return;
+
+		/* filter CAN XL VCID content */
+		if (ropts->flags & CAN_RAW_XL_VCID_RX_FILTER) {
+			/* apply VCID filter if user enabled the filter */
+			if ((cxl->vcid & ropts->rx_vcid_mask) !=
+			    (ropts->rx_vcid & ropts->rx_vcid_mask))
+				return;
+		} else {
+			/* no filter => do not forward VCID tagged frames */
+			if (cxl->vcid)
+				return;
+		}
+	}
+
  	/* eliminate multiple filter matches for the same skb */
  	if (this_cpu_ptr(ro->uniq)->skb == oskb &&
  	    this_cpu_ptr(ro->uniq)->skbcnt == can_skb_prv(oskb)->skbcnt) {
  		if (!ro->join_filters)
  			return;

@@ -696,10 +717,19 @@ static int raw_setsockopt(struct socket *sock, int level, int optname,
  		/* Enabling CAN XL includes CAN FD */
  		if (ro->xl_frames)
  			ro->fd_frames = ro->xl_frames;
  		break;
 
+	case CAN_RAW_XL_VCID_OPTS:
+		if (optlen != sizeof(ro->raw_vcid_opts))
+			return -EINVAL;
+
+		if (copy_from_sockptr(&ro->raw_vcid_opts, optval, optlen))
+			return -EFAULT;
+
+		break;
+
  	case CAN_RAW_JOIN_FILTERS:
  		if (optlen != sizeof(ro->join_filters))
  			return -EINVAL;
 
  		if (copy_from_sockptr(&ro->join_filters, optval, optlen))

@@ -784,10 +814,25 @@ static int raw_getsockopt(struct socket *sock, int level, int optname,
  		if (len > sizeof(int))
  			len = sizeof(int);
  		val = &ro->xl_frames;
  		break;
 
+	case CAN_RAW_XL_VCID_OPTS:
+		/* user space buffer to small for VCID opts? */
+		if (len < sizeof(ro->raw_vcid_opts)) {
+			/* return -ERANGE and needed space in optlen */
+			err = -ERANGE;
+			if (put_user(sizeof(ro->raw_vcid_opts), optlen))
+				err = -EFAULT;
+		} else {
+			if (len > sizeof(ro->raw_vcid_opts))
+				len = sizeof(ro->raw_vcid_opts);
+			if (copy_to_user(optval, &ro->raw_vcid_opts, len))
+				err = -EFAULT;
+		}
+		break;
+
  	case CAN_RAW_JOIN_FILTERS:
  		if (len > sizeof(int))
  			len = sizeof(int);
  		val = &ro->join_filters;
  		break;

@@ -814,12 +859,27 @@ static bool raw_bad_txframe(struct raw_sock *ro, struct sk_buff *skb, int mtu)
  	    (mtu == CANFD_MTU || can_is_canxl_dev_mtu(mtu)))
  		return false;
 
  	/* CAN XL -> needs to be enabled and a CAN XL device */
  	if (ro->xl_frames && can_is_canxl_skb(skb) &&
-	    can_is_canxl_dev_mtu(mtu))
+	    can_is_canxl_dev_mtu(mtu)) {
+		struct canxl_frame *cxl = (struct canxl_frame *)skb->data;
+		struct can_raw_vcid_options *ropts = &ro->raw_vcid_opts;
+
+		/* sanitize non CAN XL bits */
+		cxl->__res0 = 0;
+
+		/* clear VCID in CAN XL frame if pass through is disabled */
+		if (!(ropts->flags & CAN_RAW_XL_VCID_TX_PASS))
+			cxl->vcid = 0;
+
+		/* set VCID in CAN XL frame if enabled */
+		if (ropts->flags & CAN_RAW_XL_VCID_TX_SET)
+			cxl->vcid = ropts->tx_vcid;
+
  		return false;
+	}
 
  	return true;
  }
 
  static int raw_sendmsg(struct socket *sock, struct msghdr *msg, size_t size)