kernel_5.10 no rt

hc

2023-12-11 6778948f9de86c3cfaf36725a7c87dcff9ba247f

 kernel/drivers/net/can/rockchip/rockchip_canfd.c
..
..
@@ -27,6 +27,7 @@
27
27
 #include <linux/can/led.h>
28
28
 #include <linux/reset.h>
29
29
 #include <linux/pm_runtime.h>
30
+#include <linux/rockchip/cpu.h>
30
31
 
31
32
 /* registers definition */
32
33
 enum rockchip_canfd_reg {
..
..
@@ -105,6 +106,7 @@
105
106
 	ROCKCHIP_CANFD_MODE = 0,
106
107
 	ROCKCHIP_CAN_MODE,
107
108
 	ROCKCHIP_RK3568_CAN_MODE,
109
+	ROCKCHIP_RK3568_CAN_MODE_V2,
108
110
 };
109
111
 
110
112
 #define DATE_LENGTH_12_BYTE	(0x9)
..
..
@@ -220,6 +222,7 @@
220
222
 struct rockchip_canfd {
221
223
 	struct can_priv can;
222
224
 	struct device *dev;
225
+	struct napi_struct napi;
223
226
 	struct clk_bulk_data *clks;
224
227
 	int num_clks;
225
228
 	struct reset_control *reset;
..
..
@@ -292,8 +295,6 @@
292
295
 
293
296
 	val = rockchip_canfd_read(rcan, CAN_MODE);
294
297
 	val |= WORK_MODE;
295
-	if (rcan->mode >= ROCKCHIP_CAN_MODE && rcan->txtorx)
296
-		val |= MODE_RXSTX;
297
298
 	rockchip_canfd_write(rcan, CAN_MODE, val);
298
299
 
299
300
 	netdev_dbg(ndev, "%s MODE=0x%08x\n", __func__,
..
..
@@ -427,8 +428,6 @@
427
428
 	if (rcan->can.ctrlmode & CAN_CTRLMODE_LOOPBACK)
428
429
 		val |= MODE_SELF_TEST | MODE_LBACK;
429
430
 
430
-	val |= MODE_AUTO_RETX;
431
-
432
431
 	rockchip_canfd_write(rcan, CAN_MODE, val);
433
432
 
434
433
 	rockchip_canfd_set_bittiming(ndev);
..
..
@@ -488,19 +487,13 @@
488
487
 {
489
488
 	struct rockchip_canfd *rcan =
490
489
 		container_of(work, struct rockchip_canfd, tx_err_work.work);
491
-	u32 mode, err_code, id;
490
+	u32 mode;
492
491
 
493
-	id = rockchip_canfd_read(rcan, CAN_TXID);
494
-	err_code = rockchip_canfd_read(rcan, CAN_ERR_CODE);
495
-	if (err_code & 0x1fe0000) {
496
-		mode = rockchip_canfd_read(rcan, CAN_MODE);
497
-		rockchip_canfd_write(rcan, CAN_MODE, 0);
498
-		rockchip_canfd_write(rcan, CAN_MODE, mode);
499
-		rockchip_canfd_write(rcan, CAN_CMD, CAN_TX0_REQ);
500
-		schedule_delayed_work(&rcan->tx_err_work, 1);
501
-	} else if (rcan->txtorx && rcan->mode >= ROCKCHIP_CAN_MODE && id & CAN_EFF_FLAG) {
502
-		schedule_delayed_work(&rcan->tx_err_work, 1);
503
-	}
492
+	mode = rockchip_canfd_read(rcan, CAN_MODE);
493
+	rockchip_canfd_write(rcan, CAN_MODE, 0);
494
+	rockchip_canfd_write(rcan, CAN_MODE, mode);
495
+	rockchip_canfd_write(rcan, CAN_CMD, CAN_TX0_REQ);
496
+	schedule_delayed_work(&rcan->tx_err_work, 1);
504
497
 }
505
498
 
506
499
 /* transmit a CAN message
..
..
@@ -552,7 +545,14 @@
552
545
 			dlc |= TX_FD_BRS_ENABLE;
553
546
 	}
554
547
 
555
-	if (!rcan->txtorx && rcan->mode >= ROCKCHIP_CAN_MODE && cf->can_id & CAN_EFF_FLAG) {
548
+	if (rcan->txtorx && rcan->mode <= ROCKCHIP_RK3568_CAN_MODE && cf->can_id & CAN_EFF_FLAG)
549
+		rockchip_canfd_write(rcan, CAN_MODE,
550
+				     rockchip_canfd_read(rcan, CAN_MODE) | MODE_RXSTX);
551
+	else
552
+		rockchip_canfd_write(rcan, CAN_MODE,
553
+				     rockchip_canfd_read(rcan, CAN_MODE) & (~MODE_RXSTX));
554
+
555
+	if (!rcan->txtorx && rcan->mode <= ROCKCHIP_RK3568_CAN_MODE && cf->can_id & CAN_EFF_FLAG) {
556
556
 		/* Two frames are sent consecutively.
557
557
 		 * Before the first frame is tx finished,
558
558
 		 * the register of the second frame is configured.
..
..
@@ -583,10 +583,9 @@
583
583
 		rockchip_canfd_write(rcan, CAN_TXDAT0 + i,
584
584
 				     *(u32 *)(cf->data + i));
585
585
 
586
-	rockchip_canfd_write(rcan, CAN_CMD, CAN_TX1_REQ);
586
+	rockchip_canfd_write(rcan, CAN_CMD, cmd);
587
587
 
588
-	if (rcan->txtorx && rcan->mode >= ROCKCHIP_CAN_MODE && cf->can_id & CAN_EFF_FLAG)
589
-		schedule_delayed_work(&rcan->tx_err_work, 1);
588
+	schedule_delayed_work(&rcan->tx_err_work, 1);
590
589
 
591
590
 	can_put_echo_skb(skb, ndev, 0);
592
591
 
..
..
@@ -602,15 +601,15 @@
602
601
 	u32 id_rockchip_canfd, dlc;
603
602
 	int i = 0;
604
603
 	u32 __maybe_unused ts, ret;
605
-	u32 data[16] = {0};
604
+	u32 data[16];
606
605
 
607
606
 	dlc = rockchip_canfd_read(rcan, CAN_RXFRD);
608
607
 	id_rockchip_canfd = rockchip_canfd_read(rcan, CAN_RXFRD);
609
608
 	ts = rockchip_canfd_read(rcan, CAN_RXFRD);
610
-	for (i = 0; i < 16; i++)
609
+	for (i = 0; i < ARRAY_SIZE(data); i++)
611
610
 		data[i] = rockchip_canfd_read(rcan, CAN_RXFRD);
612
611
 
613
-	if (rcan->mode >= ROCKCHIP_CAN_MODE) {
612
+	if (rcan->mode <= ROCKCHIP_RK3568_CAN_MODE) {
614
613
 		/* may be an empty frame */
615
614
 		if (!dlc && !id_rockchip_canfd)
616
615
 			return 1;
..
..
@@ -618,11 +617,10 @@
618
617
 		if (rcan->txtorx) {
619
618
 			if (rockchip_canfd_read(rcan, CAN_TX_CHECK_FIC) & FORMAT_MASK) {
620
619
 				ret = rockchip_canfd_read(rcan, CAN_TXID) & CAN_SFF_MASK;
621
-				if (id_rockchip_canfd == ret) {
620
+				if ((id_rockchip_canfd == ret) && !(dlc & FORMAT_MASK))
622
621
 					rockchip_canfd_write(rcan, CAN_TX_CHECK_FIC,
623
622
 							     ts | CAN_TX0_REQ);
624
-					return 1;
625
-				}
623
+				return 1;
626
624
 			}
627
625
 		}
628
626
 	}
..
..
@@ -675,6 +673,53 @@
675
673
 	return 1;
676
674
 }
677
675
 
676
+static int rockchip_canfd_get_rx_fifo_cnt(struct net_device *ndev)
677
+{
678
+	struct rockchip_canfd *rcan = netdev_priv(ndev);
679
+	int quota = 0;
680
+
681
+	if (read_poll_timeout_atomic(rockchip_canfd_read, quota,
682
+				     (quota & rcan->rx_fifo_mask) >> rcan->rx_fifo_shift,
683
+				     0, 500000, false, rcan, CAN_RXFC))
684
+		netdev_dbg(ndev, "Warning: get fifo cnt failed\n");
685
+
686
+	quota = (quota & rcan->rx_fifo_mask) >> rcan->rx_fifo_shift;
687
+
688
+	return quota;
689
+}
690
+
691
+/* rockchip_canfd_rx_poll - Poll routine for rx packets (NAPI)
692
+ * @napi:	napi structure pointer
693
+ * @quota:	Max number of rx packets to be processed.
694
+ *
695
+ * This is the poll routine for rx part.
696
+ * It will process the packets maximux quota value.
697
+ *
698
+ * Return: number of packets received
699
+ */
700
+static int rockchip_canfd_rx_poll(struct napi_struct *napi, int quota)
701
+{
702
+	struct net_device *ndev = napi->dev;
703
+	struct rockchip_canfd *rcan = netdev_priv(ndev);
704
+	int work_done = 0;
705
+
706
+	quota = rockchip_canfd_get_rx_fifo_cnt(ndev);
707
+	if (quota) {
708
+		while (work_done < quota)
709
+			work_done += rockchip_canfd_rx(ndev);
710
+	}
711
+
712
+	if (work_done)
713
+		can_led_event(ndev, CAN_LED_EVENT_RX);
714
+
715
+	if (work_done < 6) {
716
+		napi_complete_done(napi, work_done);
717
+		rockchip_canfd_write(rcan, CAN_INT_MASK, 0);
718
+	}
719
+
720
+	return work_done;
721
+}
722
+
678
723
 static int rockchip_canfd_err(struct net_device *ndev, u32 isr)
679
724
 {
680
725
 	struct rockchip_canfd *rcan = netdev_priv(ndev);
..
..
@@ -694,6 +739,9 @@
694
739
 		cf->data[6] = txerr;
695
740
 		cf->data[7] = rxerr;
696
741
 	}
742
+
743
+	if (isr & TX_LOSTARB_INT)
744
+		schedule_delayed_work(&rcan->tx_err_work, 1);
697
745
 
698
746
 	if (isr & BUS_OFF_INT) {
699
747
 		rcan->can.state = CAN_STATE_BUS_OFF;
..
..
@@ -754,12 +802,10 @@
754
802
 		else
755
803
 			stats->tx_bytes += (dlc & DLC_MASK);
756
804
 		stats->tx_packets++;
757
-		if (rcan->txtorx && rcan->mode >= ROCKCHIP_CAN_MODE && dlc & FORMAT_MASK) {
758
-			cancel_delayed_work(&rcan->tx_err_work);
805
+		cancel_delayed_work(&rcan->tx_err_work);
806
+		if (rcan->txtorx && rcan->mode <= ROCKCHIP_RK3568_CAN_MODE && dlc & FORMAT_MASK) {
759
807
 			rockchip_canfd_write(rcan, CAN_TX_CHECK_FIC, FORMAT_MASK);
760
-			quota = (rockchip_canfd_read(rcan, CAN_RXFC) &
761
-				 rcan->rx_fifo_mask) >>
762
-				rcan->rx_fifo_shift;
808
+			quota = rockchip_canfd_get_rx_fifo_cnt(ndev);
763
809
 			if (quota) {
764
810
 				while (work_done < quota)
765
811
 					work_done += rockchip_canfd_rx(ndev);
..
..
@@ -775,11 +821,15 @@
775
821
 	}
776
822
 
777
823
 	if (isr & RX_FINISH_INT) {
778
-		quota = (rockchip_canfd_read(rcan, CAN_RXFC) & rcan->rx_fifo_mask) >>
779
-			rcan->rx_fifo_shift;
780
-		if (quota) {
781
-			while (work_done < quota)
782
-				work_done += rockchip_canfd_rx(ndev);
824
+		if (rcan->mode == ROCKCHIP_RK3568_CAN_MODE_V2) {
825
+			rockchip_canfd_write(rcan, CAN_INT_MASK, 0x1);
826
+			napi_schedule(&rcan->napi);
827
+		} else {
828
+			quota = rockchip_canfd_get_rx_fifo_cnt(ndev);
829
+			if (quota) {
830
+				while (work_done < quota)
831
+					work_done += rockchip_canfd_rx(ndev);
832
+			}
783
833
 		}
784
834
 	}
785
835
 
..
..
@@ -817,6 +867,8 @@
817
867
 	}
818
868
 
819
869
 	can_led_event(ndev, CAN_LED_EVENT_OPEN);
870
+	if (rcan->mode == ROCKCHIP_RK3568_CAN_MODE_V2)
871
+		napi_enable(&rcan->napi);
820
872
 	netif_start_queue(ndev);
821
873
 
822
874
 	netdev_dbg(ndev, "%s\n", __func__);
..
..
@@ -834,6 +886,8 @@
834
886
 	struct rockchip_canfd *rcan = netdev_priv(ndev);
835
887
 
836
888
 	netif_stop_queue(ndev);
889
+	if (rcan->mode == ROCKCHIP_RK3568_CAN_MODE_V2)
890
+		napi_disable(&rcan->napi);
837
891
 	rockchip_canfd_stop(ndev);
838
892
 	close_candev(ndev);
839
893
 	can_led_event(ndev, CAN_LED_EVENT_STOP);
..
..
@@ -1011,6 +1065,9 @@
1011
1065
 
1012
1066
 	rcan->mode = (unsigned long)of_device_get_match_data(&pdev->dev);
1013
1067
 
1068
+	if ((cpu_is_rk3566() || cpu_is_rk3568()) && (rockchip_get_cpu_version() == 3))
1069
+		rcan->mode = ROCKCHIP_RK3568_CAN_MODE_V2;
1070
+
1014
1071
 	rcan->base = addr;
1015
1072
 	rcan->can.clock.freq = clk_get_rate(rcan->clks[0].clk);
1016
1073
 	rcan->dev = &pdev->dev;
..
..
@@ -1032,6 +1089,7 @@
1032
1089
 		break;
1033
1090
 	case ROCKCHIP_CAN_MODE:
1034
1091
 	case ROCKCHIP_RK3568_CAN_MODE:
1092
+	case ROCKCHIP_RK3568_CAN_MODE_V2:
1035
1093
 		rcan->can.bittiming_const = &rockchip_canfd_bittiming_const;
1036
1094
 		rcan->can.do_set_mode = rockchip_canfd_set_mode;
1037
1095
 		rcan->can.do_get_berr_counter = rockchip_canfd_get_berr_counter;
..
..
@@ -1056,10 +1114,17 @@
1056
1114
 					   rcan->tx_invalid, 4))
1057
1115
 		rcan->txtorx = 1;
1058
1116
 
1117
+	if (rcan->mode == ROCKCHIP_RK3568_CAN_MODE_V2) {
1118
+		rcan->txtorx = 0;
1119
+		netif_napi_add(ndev, &rcan->napi, rockchip_canfd_rx_poll, 6);
1120
+	}
1121
+
1059
1122
 	ndev->netdev_ops = &rockchip_canfd_netdev_ops;
1060
1123
 	ndev->irq = irq;
1061
1124
 	ndev->flags |= IFF_ECHO;
1062
1125
 	rcan->can.restart_ms = 1;
1126
+
1127
+	irq_set_affinity_hint(irq, get_cpu_mask(num_online_cpus() - 1));
1063
1128
 
1064
1129
 	INIT_DELAYED_WORK(&rcan->tx_err_work, rockchip_canfd_tx_err_delay_work);
1065
1130
 
..
..
@@ -1097,9 +1162,12 @@
1097
1162
 static int rockchip_canfd_remove(struct platform_device *pdev)
1098
1163
 {
1099
1164
 	struct net_device *ndev = platform_get_drvdata(pdev);
1165
+	struct rockchip_canfd *rcan = netdev_priv(ndev);
1100
1166
 
1101
1167
 	unregister_netdev(ndev);
1102
1168
 	pm_runtime_disable(&pdev->dev);
1169
+	if (rcan->mode == ROCKCHIP_RK3568_CAN_MODE_V2)
1170
+		netif_napi_del(&rcan->napi);
1103
1171
 	free_candev(ndev);
1104
1172
 
1105
1173
 	return 0;