Merge "drivers: net: can: Timestamp offset correction" (857b40cc) · Commits · e / devices / android_kernel_fairphone_FP3

drivers/net/can/spi/qti-can.c

+59 −8

Original line number	Diff line number	Diff line
		@@ -46,6 +46,8 @@
		#define DRIVER_MODE_AMB 2
		#define QUERY_FIRMWARE_TIMEOUT_MS 300
		#define EUPGRADE 140
		#define TIME_OFFSET_MAX_THD 5
		#define TIME_OFFSET_MIN_THD -5

		struct qti_can {
		struct net_device **netdev;
		@@ -290,6 +292,9 @@ static void qti_can_receive_frame(struct qti_can *priv_data,
		struct net_device *netdev;
		int i;
		struct device *dev;
		s64 ts_offset_corrected;
		static u16 buff_frames_disc_cntr;
		static u8 disp_disc_cntr = 1;

		dev = &priv_data->spidev->dev;
		if (frame->can_if >= priv_data->max_can_channels) {
		@@ -314,14 +319,31 @@ static void qti_can_receive_frame(struct qti_can *priv_data,
		for (i = 0; i < cf->can_dlc; i++)
		cf->data[i] = frame->data[i];

		nsec = ms_to_ktime(le64_to_cpu(frame->ts)
		+ priv_data->time_diff);
		ts_offset_corrected = le64_to_cpu(frame->ts)
		+ priv_data->time_diff;

		/* CAN frames which are received before SOC powers up are discarded */
		if (ts_offset_corrected > 0) {
		if (disp_disc_cntr == 1) {
		dev_info(&priv_data->spidev->dev,
		"No of buff frames discarded is %lld\n",
		buff_frames_disc_cntr);
		disp_disc_cntr = 0;
		}

		nsec = ms_to_ktime(ts_offset_corrected);
		skt = skb_hwtstamps(skb);
		skt->hwtstamp = nsec;
		skb->tstamp = nsec;

		netif_rx(skb);

		LOGDI("hwtstamp: %lld\n", ktime_to_ms(skt->hwtstamp));
		netdev->stats.rx_packets++;
		} else {
		buff_frames_disc_cntr++;
		dev_kfree_skb(skb);
		}
		}

		static void qti_can_receive_property(struct qti_can *priv_data,
		@@ -370,6 +392,9 @@ static int qti_can_process_response(struct qti_can *priv_data,
		int ret = 0;
		u64 mstime;
		ktime_t ktime_now;
		static s64 prev_time_diff;
		static u8 first_offset_est = 1;
		s64 offset_variation = 0;

		LOGDI("<%x %2d [%d]\n", resp->cmd, resp->len, resp->seq);
		if (resp->cmd == CMD_CAN_RECEIVE_FRAME) {
		@@ -437,6 +462,32 @@ static int qti_can_process_response(struct qti_can *priv_data,
		mstime = ktime_to_ms(ktime_now);
		priv_data->time_diff = mstime -
		(le64_to_cpu(time_data->time));

		if (first_offset_est == 1) {
		prev_time_diff = priv_data->time_diff;
		first_offset_est = 0;
		}

		offset_variation = priv_data->time_diff -
		prev_time_diff;

		if ((offset_variation > TIME_OFFSET_MAX_THD) \|\|
		(offset_variation < TIME_OFFSET_MIN_THD)) {
		dev_info(&priv_data->spidev->dev,
		"Off Exceeded: Curr off is %lld\n",
		priv_data->time_diff);
		dev_info(&priv_data->spidev->dev,
		"Prev off is %lld\n",
		prev_time_diff);
		/* Set curr off to prev off if */
		/* variation is beyond threshold */
		priv_data->time_diff = prev_time_diff;

		} else {
		/* Set prev off to curr off if */
		/* variation is within threshold */
		prev_time_diff = priv_data->time_diff;
		}
		}

		exit: