Commit 45dfa45f authored by John Whittington's avatar John Whittington Committed by Marc Kleine-Budde
Browse files

can: gs_usb: add RX and TX hardware timestamp support 



Add support for hardware timestamps, if the firmware includes it as a
feature via the GS_CAN_FEATURE_HW_TIMESTAMP flag. Check for this
feature during probe, extend the RX expected length if it is and
enable it during open.

The struct classic_can_ts and struct canfd_ts are extended to include
the µs timestamp following data as defined in the firmware. The
timestamp is then captured and set using skb_hwtstamps() on each RX
and TX.

The frame µs timestamp is provided from a 32 bit 1 MHz timer which
rolls over every 4294 seconds, so a cyclecounter, timecounter, and
delayed worker are used to convert the timer into a proper ns
timestamp - same implementation as commit efd8d98d ("can:
mcp251xfd: add HW timestamp infrastructure").

Hardware timestamps are added to capabilities as commit
b1f6b93e ("can: mcp251xfd: advertise timestamping capabilities and
add ioctl support").

Signed-off-by: default avatarJohn Whittington <git@jbrengineering.co.uk>
Link: https://github.com/candle-usb/candleLight_fw/issues/100
Link: https://lore.kernel.org/all/20220827221548.3291393-3-mkl@pengutronix.de


Co-developed-by: default avatarMarc Kleine-Budde <mkl@pengutronix.de>
Signed-off-by: default avatarMarc Kleine-Budde <mkl@pengutronix.de>
parent 49c007b9

drivers/net/can/usb/gs_usb.c

+186 −7
Original line number Diff line number Diff line
@@ -10,12 +10,16 @@ 
 */



#include <linux/bitfield.h>

#include <linux/clocksource.h>

#include <linux/ethtool.h>

#include <linux/init.h>

#include <linux/module.h>

#include <linux/netdevice.h>

#include <linux/signal.h>

#include <linux/timecounter.h>

#include <linux/units.h>

#include <linux/usb.h>

#include <linux/workqueue.h>



#include <linux/can.h>

#include <linux/can/dev.h>
@@ -37,6 +41,14 @@ 
#define GS_USB_ENDPOINT_IN 1

#define GS_USB_ENDPOINT_OUT 2



/* Timestamp 32 bit timer runs at 1 MHz (1 µs tick). Worker accounts

 * for timer overflow (will be after ~71 minutes)

 */

#define GS_USB_TIMESTAMP_TIMER_HZ (1 * HZ_PER_MHZ)

#define GS_USB_TIMESTAMP_WORK_DELAY_SEC 1800

static_assert(GS_USB_TIMESTAMP_WORK_DELAY_SEC <

	      CYCLECOUNTER_MASK(32) / GS_USB_TIMESTAMP_TIMER_HZ / 2);



/* Device specific constants */

enum gs_usb_breq {

	GS_USB_BREQ_HOST_FORMAT = 0,
@@ -199,6 +211,11 @@ struct classic_can { 
	u8 data[8];

} __packed;



struct classic_can_ts {

	u8 data[8];

	__le32 timestamp_us;

} __packed;



struct classic_can_quirk {

	u8 data[8];

	u8 quirk;
@@ -208,6 +225,11 @@ struct canfd { 
	u8 data[64];

} __packed;



struct canfd_ts {

	u8 data[64];

	__le32 timestamp_us;

} __packed;



struct canfd_quirk {

	u8 data[64];

	u8 quirk;
@@ -224,8 +246,10 @@ struct gs_host_frame { 


	union {

		DECLARE_FLEX_ARRAY(struct classic_can, classic_can);

		DECLARE_FLEX_ARRAY(struct classic_can_ts, classic_can_ts);

		DECLARE_FLEX_ARRAY(struct classic_can_quirk, classic_can_quirk);

		DECLARE_FLEX_ARRAY(struct canfd, canfd);

		DECLARE_FLEX_ARRAY(struct canfd_ts, canfd_ts);

		DECLARE_FLEX_ARRAY(struct canfd_quirk, canfd_quirk);

	};

} __packed;
@@ -259,6 +283,11 @@ struct gs_can { 
	struct can_bittiming_const bt_const, data_bt_const;

	unsigned int channel;	/* channel number */



	/* time counter for hardware timestamps */

	struct cyclecounter cc;

	struct timecounter tc;

	struct delayed_work timestamp;



	u32 feature;

	unsigned int hf_size_tx;
@@ -351,6 +380,87 @@ static int gs_cmd_reset(struct gs_can *gsdev) 
	return rc;

}



static inline int gs_usb_get_timestamp(const struct gs_can *dev,

				       u32 *timestamp_p)

{

	__le32 timestamp;

	int rc;



	rc = usb_control_msg_recv(interface_to_usbdev(dev->iface),

				  usb_sndctrlpipe(interface_to_usbdev(dev->iface), 0),

				  GS_USB_BREQ_TIMESTAMP,

				  USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,

				  dev->channel, 0,

				  &timestamp, sizeof(timestamp),

				  USB_CTRL_GET_TIMEOUT,

				  GFP_KERNEL);

	if (rc)

		return rc;



	*timestamp_p = le32_to_cpu(timestamp);



	return 0;

}



static u64 gs_usb_timestamp_read(const struct cyclecounter *cc)

{

	const struct gs_can *dev;

	u32 timestamp = 0;

	int err;



	dev = container_of(cc, struct gs_can, cc);

	err = gs_usb_get_timestamp(dev, &timestamp);

	if (err)

		netdev_err(dev->netdev,

			   "Error %d while reading timestamp. HW timestamps may be inaccurate.",

			   err);



	return timestamp;

}



static void gs_usb_timestamp_work(struct work_struct *work)

{

	struct delayed_work *delayed_work = to_delayed_work(work);

	struct gs_can *dev;



	dev = container_of(delayed_work, struct gs_can, timestamp);

	timecounter_read(&dev->tc);



	schedule_delayed_work(&dev->timestamp,

			      GS_USB_TIMESTAMP_WORK_DELAY_SEC * HZ);

}



static void gs_usb_skb_set_timestamp(const struct gs_can *dev,

				     struct sk_buff *skb, u32 timestamp)

{

	struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);

	u64 ns;



	ns = timecounter_cyc2time(&dev->tc, timestamp);

	hwtstamps->hwtstamp = ns_to_ktime(ns);

}



static void gs_usb_timestamp_init(struct gs_can *dev)

{

	struct cyclecounter *cc = &dev->cc;



	cc->read = gs_usb_timestamp_read;

	cc->mask = CYCLECOUNTER_MASK(32);

	cc->shift = 32 - bits_per(NSEC_PER_SEC / GS_USB_TIMESTAMP_TIMER_HZ);

	cc->mult = clocksource_hz2mult(GS_USB_TIMESTAMP_TIMER_HZ, cc->shift);



	timecounter_init(&dev->tc, &dev->cc, ktime_get_real_ns());



	INIT_DELAYED_WORK(&dev->timestamp, gs_usb_timestamp_work);

	schedule_delayed_work(&dev->timestamp,

			      GS_USB_TIMESTAMP_WORK_DELAY_SEC * HZ);

}



static void gs_usb_timestamp_stop(struct gs_can *dev)

{

	cancel_delayed_work_sync(&dev->timestamp);

}



static void gs_update_state(struct gs_can *dev, struct can_frame *cf)

{

	struct can_device_stats *can_stats = &dev->can.can_stats;
@@ -376,6 +486,24 @@ static void gs_update_state(struct gs_can *dev, struct can_frame *cf) 
	}

}



static void gs_usb_set_timestamp(const struct gs_can *dev, struct sk_buff *skb,

				 const struct gs_host_frame *hf)

{

	u32 timestamp;



	if (!(dev->feature & GS_CAN_FEATURE_HW_TIMESTAMP))

		return;



	if (hf->flags & GS_CAN_FLAG_FD)

		timestamp = le32_to_cpu(hf->canfd_ts->timestamp_us);

	else

		timestamp = le32_to_cpu(hf->classic_can_ts->timestamp_us);



	gs_usb_skb_set_timestamp(dev, skb, timestamp);



	return;

}



static void gs_usb_receive_bulk_callback(struct urb *urb)

{

	struct gs_usb *usbcan = urb->context;
@@ -443,6 +571,8 @@ static void gs_usb_receive_bulk_callback(struct urb *urb) 
				gs_update_state(dev, cf);

		}



		gs_usb_set_timestamp(dev, skb, hf);



		netdev->stats.rx_packets++;

		netdev->stats.rx_bytes += hf->can_dlc;
@@ -465,6 +595,9 @@ static void gs_usb_receive_bulk_callback(struct urb *urb) 
			goto resubmit_urb;

		}



		skb = dev->can.echo_skb[hf->echo_id];

		gs_usb_set_timestamp(dev, skb, hf);



		netdev->stats.tx_packets++;

		netdev->stats.tx_bytes += can_get_echo_skb(netdev, hf->echo_id,

							   NULL);
@@ -823,6 +956,10 @@ static int gs_can_open(struct net_device *netdev) 
	if (ctrlmode & CAN_CTRLMODE_3_SAMPLES)

		flags |= GS_CAN_MODE_TRIPLE_SAMPLE;



	/* if hardware supports timestamps, enable it */

	if (dev->feature & GS_CAN_FEATURE_HW_TIMESTAMP)

		flags |= GS_CAN_MODE_HW_TIMESTAMP;



	/* finally start device */

	dm->mode = cpu_to_le32(GS_CAN_MODE_START);

	dm->flags = cpu_to_le32(flags);
@@ -840,6 +977,10 @@ static int gs_can_open(struct net_device *netdev) 


	kfree(dm);



	/* start polling timestamp */

	if (dev->feature & GS_CAN_FEATURE_HW_TIMESTAMP)

		gs_usb_timestamp_init(dev);



	dev->can.state = CAN_STATE_ERROR_ACTIVE;



	parent->active_channels++;
@@ -858,6 +999,10 @@ static int gs_can_close(struct net_device *netdev) 


	netif_stop_queue(netdev);



	/* stop polling timestamp */

	if (dev->feature & GS_CAN_FEATURE_HW_TIMESTAMP)

		gs_usb_timestamp_stop(dev);



	/* Stop polling */

	parent->active_channels--;

	if (!parent->active_channels) {
@@ -890,11 +1035,22 @@ static int gs_can_close(struct net_device *netdev) 
	return 0;

}



static int gs_can_eth_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)

{

	const struct gs_can *dev = netdev_priv(netdev);



	if (dev->feature & GS_CAN_FEATURE_HW_TIMESTAMP)

		return can_eth_ioctl_hwts(netdev, ifr, cmd);



	return -EOPNOTSUPP;

}



static const struct net_device_ops gs_usb_netdev_ops = {

	.ndo_open = gs_can_open,

	.ndo_stop = gs_can_close,

	.ndo_start_xmit = gs_can_start_xmit,

	.ndo_change_mtu = can_change_mtu,

	.ndo_eth_ioctl = gs_can_eth_ioctl,

};



static int gs_usb_set_identify(struct net_device *netdev, bool do_identify)
@@ -944,9 +1100,21 @@ static int gs_usb_set_phys_id(struct net_device *dev, 
	return rc;

}



static int gs_usb_get_ts_info(struct net_device *netdev,

			      struct ethtool_ts_info *info)

{

	struct gs_can *dev = netdev_priv(netdev);



	/* report if device supports HW timestamps */

	if (dev->feature & GS_CAN_FEATURE_HW_TIMESTAMP)

		return can_ethtool_op_get_ts_info_hwts(netdev, info);



	return ethtool_op_get_ts_info(netdev, info);

}



static const struct ethtool_ops gs_usb_ethtool_ops = {

	.set_phys_id = gs_usb_set_phys_id,

	.get_ts_info = ethtool_op_get_ts_info,

	.get_ts_info = gs_usb_get_ts_info,

};



static struct gs_can *gs_make_candev(unsigned int channel,
@@ -1202,15 +1370,13 @@ static int gs_usb_probe(struct usb_interface *intf, 
	}



	init_usb_anchor(&dev->rx_submitted);

	/* default to classic CAN, switch to CAN-FD if at least one of

	 * our channels support CAN-FD.

	 */

	dev->hf_size_rx = struct_size(hf, classic_can, 1);



	usb_set_intfdata(intf, dev);

	dev->udev = udev;



	for (i = 0; i < icount; i++) {

		unsigned int hf_size_rx = 0;



		dev->canch[i] = gs_make_candev(i, intf, dconf);

		if (IS_ERR_OR_NULL(dev->canch[i])) {

			/* save error code to return later */
@@ -1228,8 +1394,21 @@ static int gs_usb_probe(struct usb_interface *intf, 
		}

		dev->canch[i]->parent = dev;



		if (dev->canch[i]->can.ctrlmode_supported & CAN_CTRLMODE_FD)

			dev->hf_size_rx = struct_size(hf, canfd, 1);

		/* set RX packet size based on FD and if hardware

                * timestamps are supported.

		*/

		if (dev->canch[i]->can.ctrlmode_supported & CAN_CTRLMODE_FD) {

			if (dev->canch[i]->feature & GS_CAN_FEATURE_HW_TIMESTAMP)

				hf_size_rx = struct_size(hf, canfd_ts, 1);

			else

				hf_size_rx = struct_size(hf, canfd, 1);

		} else {

			if (dev->canch[i]->feature & GS_CAN_FEATURE_HW_TIMESTAMP)

				hf_size_rx = struct_size(hf, classic_can_ts, 1);

			else

				hf_size_rx = struct_size(hf, classic_can, 1);

		}

		dev->hf_size_rx = max(dev->hf_size_rx, hf_size_rx);

	}



	kfree(dconf);