修改4g拨号为QMI,需要在系统里后台执行quectel-CM

hc

2024-10-22 8ac6c7a54ed1b98d142dce24b11c6de6a1e239a5

 kernel/drivers/iio/trigger/stm32-timer-trigger.c
..
..
@@ -75,14 +75,27 @@
75
75
 	{ }, /* timer 17 */
76
76
 };
77
77
 
78
+struct stm32_timer_trigger_regs {
79
+	u32 cr1;
80
+	u32 cr2;
81
+	u32 psc;
82
+	u32 arr;
83
+	u32 cnt;
84
+	u32 smcr;
85
+};
86
+
78
87
 struct stm32_timer_trigger {
79
88
 	struct device *dev;
80
89
 	struct regmap *regmap;
81
90
 	struct clk *clk;
91
+	bool enabled;
82
92
 	u32 max_arr;
83
93
 	const void *triggers;
84
94
 	const void *valids;
85
95
 	bool has_trgo2;
96
+	struct mutex lock; /* concurrent sysfs configuration */
97
+	struct list_head tr_list;
98
+	struct stm32_timer_trigger_regs bak;
86
99
 };
87
100
 
88
101
 struct stm32_timer_trigger_cfg {
..
..
@@ -106,7 +119,7 @@
106
119
 {
107
120
 	unsigned long long prd, div;
108
121
 	int prescaler = 0;
109
-	u32 ccer, cr1;
122
+	u32 ccer;
110
123
 
111
124
 	/* Period and prescaler values depends of clock rate */
112
125
 	div = (unsigned long long)clk_get_rate(priv->clk);
..
..
@@ -136,9 +149,11 @@
136
149
 	if (ccer & TIM_CCER_CCXE)
137
150
 		return -EBUSY;
138
151
 
139
-	regmap_read(priv->regmap, TIM_CR1, &cr1);
140
-	if (!(cr1 & TIM_CR1_CEN))
152
+	mutex_lock(&priv->lock);
153
+	if (!priv->enabled) {
154
+		priv->enabled = true;
141
155
 		clk_enable(priv->clk);
156
+	}
142
157
 
143
158
 	regmap_write(priv->regmap, TIM_PSC, prescaler);
144
159
 	regmap_write(priv->regmap, TIM_ARR, prd - 1);
..
..
@@ -157,6 +172,7 @@
157
172
 
158
173
 	/* Enable controller */
159
174
 	regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, TIM_CR1_CEN);
175
+	mutex_unlock(&priv->lock);
160
176
 
161
177
 	return 0;
162
178
 }
..
..
@@ -164,16 +180,13 @@
164
180
 static void stm32_timer_stop(struct stm32_timer_trigger *priv,
165
181
 			     struct iio_trigger *trig)
166
182
 {
167
-	u32 ccer, cr1;
183
+	u32 ccer;
168
184
 
169
185
 	regmap_read(priv->regmap, TIM_CCER, &ccer);
170
186
 	if (ccer & TIM_CCER_CCXE)
171
187
 		return;
172
188
 
173
-	regmap_read(priv->regmap, TIM_CR1, &cr1);
174
-	if (cr1 & TIM_CR1_CEN)
175
-		clk_disable(priv->clk);
176
-
189
+	mutex_lock(&priv->lock);
177
190
 	/* Stop timer */
178
191
 	regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_ARPE, 0);
179
192
 	regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, 0);
..
..
@@ -188,6 +201,12 @@
188
201
 
189
202
 	/* Make sure that registers are updated */
190
203
 	regmap_update_bits(priv->regmap, TIM_EGR, TIM_EGR_UG, TIM_EGR_UG);
204
+
205
+	if (priv->enabled) {
206
+		priv->enabled = false;
207
+		clk_disable(priv->clk);
208
+	}
209
+	mutex_unlock(&priv->lock);
191
210
 }
192
211
 
193
212
 static ssize_t stm32_tt_store_frequency(struct device *dev,
..
..
@@ -302,11 +321,15 @@
302
321
 	for (i = 0; i <= master_mode_max; i++) {
303
322
 		if (!strncmp(master_mode_table[i], buf,
304
323
 			     strlen(master_mode_table[i]))) {
324
+			mutex_lock(&priv->lock);
325
+			if (!priv->enabled) {
326
+				/* Clock should be enabled first */
327
+				priv->enabled = true;
328
+				clk_enable(priv->clk);
329
+			}
305
330
 			regmap_update_bits(priv->regmap, TIM_CR2, mask,
306
331
 					   i << shift);
307
-			/* Make sure that registers are updated */
308
-			regmap_update_bits(priv->regmap, TIM_EGR,
309
-					   TIM_EGR_UG, TIM_EGR_UG);
332
+			mutex_unlock(&priv->lock);
310
333
 			return len;
311
334
 		}
312
335
 	}
..
..
@@ -364,10 +387,20 @@
364
387
 static const struct iio_trigger_ops timer_trigger_ops = {
365
388
 };
366
389
 
367
-static int stm32_setup_iio_triggers(struct stm32_timer_trigger *priv)
390
+static void stm32_unregister_iio_triggers(struct stm32_timer_trigger *priv)
391
+{
392
+	struct iio_trigger *tr;
393
+
394
+	list_for_each_entry(tr, &priv->tr_list, alloc_list)
395
+		iio_trigger_unregister(tr);
396
+}
397
+
398
+static int stm32_register_iio_triggers(struct stm32_timer_trigger *priv)
368
399
 {
369
400
 	int ret;
370
401
 	const char * const *cur = priv->triggers;
402
+
403
+	INIT_LIST_HEAD(&priv->tr_list);
371
404
 
372
405
 	while (cur && *cur) {
373
406
 		struct iio_trigger *trig;
..
..
@@ -395,9 +428,13 @@
395
428
 
396
429
 		iio_trigger_set_drvdata(trig, priv);
397
430
 
398
-		ret = devm_iio_trigger_register(priv->dev, trig);
399
-		if (ret)
431
+		ret = iio_trigger_register(trig);
432
+		if (ret) {
433
+			stm32_unregister_iio_triggers(priv);
400
434
 			return ret;
435
+		}
436
+
437
+		list_add_tail(&trig->alloc_list, &priv->tr_list);
401
438
 		cur++;
402
439
 	}
403
440
 
..
..
@@ -444,7 +481,6 @@
444
481
 				   int val, int val2, long mask)
445
482
 {
446
483
 	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
447
-	u32 dat;
448
484
 
449
485
 	switch (mask) {
450
486
 	case IIO_CHAN_INFO_RAW:
..
..
@@ -455,19 +491,23 @@
455
491
 		return -EINVAL;
456
492
 
457
493
 	case IIO_CHAN_INFO_ENABLE:
494
+		mutex_lock(&priv->lock);
458
495
 		if (val) {
459
-			regmap_read(priv->regmap, TIM_CR1, &dat);
460
-			if (!(dat & TIM_CR1_CEN))
496
+			if (!priv->enabled) {
497
+				priv->enabled = true;
461
498
 				clk_enable(priv->clk);
499
+			}
462
500
 			regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN,
463
501
 					   TIM_CR1_CEN);
464
502
 		} else {
465
-			regmap_read(priv->regmap, TIM_CR1, &dat);
466
503
 			regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN,
467
504
 					   0);
468
-			if (dat & TIM_CR1_CEN)
505
+			if (priv->enabled) {
506
+				priv->enabled = false;
469
507
 				clk_disable(priv->clk);
508
+			}
470
509
 		}
510
+		mutex_unlock(&priv->lock);
471
511
 		return 0;
472
512
 	}
473
513
 
..
..
@@ -563,7 +603,6 @@
563
603
 {
564
604
 	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
565
605
 	int sms = stm32_enable_mode2sms(mode);
566
-	u32 val;
567
606
 
568
607
 	if (sms < 0)
569
608
 		return sms;
..
..
@@ -571,11 +610,12 @@
571
610
 	 * Triggered mode sets CEN bit automatically by hardware. So, first
572
611
 	 * enable counter clock, so it can use it. Keeps it in sync with CEN.
573
612
 	 */
574
-	if (sms == 6) {
575
-		regmap_read(priv->regmap, TIM_CR1, &val);
576
-		if (!(val & TIM_CR1_CEN))
577
-			clk_enable(priv->clk);
613
+	mutex_lock(&priv->lock);
614
+	if (sms == 6 && !priv->enabled) {
615
+		clk_enable(priv->clk);
616
+		priv->enabled = true;
578
617
 	}
618
+	mutex_unlock(&priv->lock);
579
619
 
580
620
 	regmap_update_bits(priv->regmap, TIM_SMCR, TIM_SMCR_SMS, sms);
581
621
 
..
..
@@ -613,86 +653,6 @@
613
653
 	.num_items = ARRAY_SIZE(stm32_enable_modes),
614
654
 	.set = stm32_set_enable_mode,
615
655
 	.get = stm32_get_enable_mode
616
-};
617
-
618
-static const char *const stm32_quadrature_modes[] = {
619
-	"channel_A",
620
-	"channel_B",
621
-	"quadrature",
622
-};
623
-
624
-static int stm32_set_quadrature_mode(struct iio_dev *indio_dev,
625
-				     const struct iio_chan_spec *chan,
626
-				     unsigned int mode)
627
-{
628
-	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
629
-
630
-	regmap_update_bits(priv->regmap, TIM_SMCR, TIM_SMCR_SMS, mode + 1);
631
-
632
-	return 0;
633
-}
634
-
635
-static int stm32_get_quadrature_mode(struct iio_dev *indio_dev,
636
-				     const struct iio_chan_spec *chan)
637
-{
638
-	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
639
-	u32 smcr;
640
-	int mode;
641
-
642
-	regmap_read(priv->regmap, TIM_SMCR, &smcr);
643
-	mode = (smcr & TIM_SMCR_SMS) - 1;
644
-	if ((mode < 0) || (mode > ARRAY_SIZE(stm32_quadrature_modes)))
645
-		return -EINVAL;
646
-
647
-	return mode;
648
-}
649
-
650
-static const struct iio_enum stm32_quadrature_mode_enum = {
651
-	.items = stm32_quadrature_modes,
652
-	.num_items = ARRAY_SIZE(stm32_quadrature_modes),
653
-	.set = stm32_set_quadrature_mode,
654
-	.get = stm32_get_quadrature_mode
655
-};
656
-
657
-static const char *const stm32_count_direction_states[] = {
658
-	"up",
659
-	"down"
660
-};
661
-
662
-static int stm32_set_count_direction(struct iio_dev *indio_dev,
663
-				     const struct iio_chan_spec *chan,
664
-				     unsigned int dir)
665
-{
666
-	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
667
-	u32 val;
668
-	int mode;
669
-
670
-	/* In encoder mode, direction is RO (given by TI1/TI2 signals) */
671
-	regmap_read(priv->regmap, TIM_SMCR, &val);
672
-	mode = (val & TIM_SMCR_SMS) - 1;
673
-	if ((mode >= 0) || (mode < ARRAY_SIZE(stm32_quadrature_modes)))
674
-		return -EBUSY;
675
-
676
-	return regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_DIR,
677
-				  dir ? TIM_CR1_DIR : 0);
678
-}
679
-
680
-static int stm32_get_count_direction(struct iio_dev *indio_dev,
681
-				     const struct iio_chan_spec *chan)
682
-{
683
-	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
684
-	u32 cr1;
685
-
686
-	regmap_read(priv->regmap, TIM_CR1, &cr1);
687
-
688
-	return ((cr1 & TIM_CR1_DIR) ? 1 : 0);
689
-}
690
-
691
-static const struct iio_enum stm32_count_direction_enum = {
692
-	.items = stm32_count_direction_states,
693
-	.num_items = ARRAY_SIZE(stm32_count_direction_states),
694
-	.set = stm32_set_count_direction,
695
-	.get = stm32_get_count_direction
696
656
 };
697
657
 
698
658
 static ssize_t stm32_count_get_preset(struct iio_dev *indio_dev,
..
..
@@ -735,10 +695,6 @@
735
695
 		.read = stm32_count_get_preset,
736
696
 		.write = stm32_count_set_preset
737
697
 	},
738
-	IIO_ENUM("count_direction", IIO_SEPARATE, &stm32_count_direction_enum),
739
-	IIO_ENUM_AVAILABLE("count_direction", &stm32_count_direction_enum),
740
-	IIO_ENUM("quadrature_mode", IIO_SEPARATE, &stm32_quadrature_mode_enum),
741
-	IIO_ENUM_AVAILABLE("quadrature_mode", &stm32_quadrature_mode_enum),
742
698
 	IIO_ENUM("enable_mode", IIO_SEPARATE, &stm32_enable_mode_enum),
743
699
 	IIO_ENUM_AVAILABLE("enable_mode", &stm32_enable_mode_enum),
744
700
 	IIO_ENUM("trigger_mode", IIO_SEPARATE, &stm32_trigger_mode_enum),
..
..
@@ -767,12 +723,10 @@
767
723
 		return NULL;
768
724
 
769
725
 	indio_dev->name = dev_name(dev);
770
-	indio_dev->dev.parent = dev;
771
726
 	indio_dev->info = &stm32_trigger_info;
772
727
 	indio_dev->modes = INDIO_HARDWARE_TRIGGERED;
773
728
 	indio_dev->num_channels = 1;
774
729
 	indio_dev->channels = &stm32_trigger_channel;
775
-	indio_dev->dev.of_node = dev->of_node;
776
730
 
777
731
 	ret = devm_iio_device_register(dev, indio_dev);
778
732
 	if (ret)
..
..
@@ -843,8 +797,9 @@
843
797
 	priv->triggers = triggers_table[index];
844
798
 	priv->valids = cfg->valids_table[index];
845
799
 	stm32_timer_detect_trgo2(priv);
800
+	mutex_init(&priv->lock);
846
801
 
847
-	ret = stm32_setup_iio_triggers(priv);
802
+	ret = stm32_register_iio_triggers(priv);
848
803
 	if (ret)
849
804
 		return ret;
850
805
 
..
..
@@ -852,6 +807,77 @@
852
807
 
853
808
 	return 0;
854
809
 }
810
+
811
+static int stm32_timer_trigger_remove(struct platform_device *pdev)
812
+{
813
+	struct stm32_timer_trigger *priv = platform_get_drvdata(pdev);
814
+	u32 val;
815
+
816
+	/* Unregister triggers before everything can be safely turned off */
817
+	stm32_unregister_iio_triggers(priv);
818
+
819
+	/* Check if nobody else use the timer, then disable it */
820
+	regmap_read(priv->regmap, TIM_CCER, &val);
821
+	if (!(val & TIM_CCER_CCXE))
822
+		regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, 0);
823
+
824
+	if (priv->enabled)
825
+		clk_disable(priv->clk);
826
+
827
+	return 0;
828
+}
829
+
830
+static int __maybe_unused stm32_timer_trigger_suspend(struct device *dev)
831
+{
832
+	struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
833
+
834
+	/* Only take care of enabled timer: don't disturb other MFD child */
835
+	if (priv->enabled) {
836
+		/* Backup registers that may get lost in low power mode */
837
+		regmap_read(priv->regmap, TIM_CR1, &priv->bak.cr1);
838
+		regmap_read(priv->regmap, TIM_CR2, &priv->bak.cr2);
839
+		regmap_read(priv->regmap, TIM_PSC, &priv->bak.psc);
840
+		regmap_read(priv->regmap, TIM_ARR, &priv->bak.arr);
841
+		regmap_read(priv->regmap, TIM_CNT, &priv->bak.cnt);
842
+		regmap_read(priv->regmap, TIM_SMCR, &priv->bak.smcr);
843
+
844
+		/* Disable the timer */
845
+		regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, 0);
846
+		clk_disable(priv->clk);
847
+	}
848
+
849
+	return 0;
850
+}
851
+
852
+static int __maybe_unused stm32_timer_trigger_resume(struct device *dev)
853
+{
854
+	struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
855
+	int ret;
856
+
857
+	if (priv->enabled) {
858
+		ret = clk_enable(priv->clk);
859
+		if (ret)
860
+			return ret;
861
+
862
+		/* restore master/slave modes */
863
+		regmap_write(priv->regmap, TIM_SMCR, priv->bak.smcr);
864
+		regmap_write(priv->regmap, TIM_CR2, priv->bak.cr2);
865
+
866
+		/* restore sampling_frequency (trgo / trgo2 triggers) */
867
+		regmap_write(priv->regmap, TIM_PSC, priv->bak.psc);
868
+		regmap_write(priv->regmap, TIM_ARR, priv->bak.arr);
869
+		regmap_write(priv->regmap, TIM_CNT, priv->bak.cnt);
870
+
871
+		/* Also re-enables the timer */
872
+		regmap_write(priv->regmap, TIM_CR1, priv->bak.cr1);
873
+	}
874
+
875
+	return 0;
876
+}
877
+
878
+static SIMPLE_DEV_PM_OPS(stm32_timer_trigger_pm_ops,
879
+			 stm32_timer_trigger_suspend,
880
+			 stm32_timer_trigger_resume);
855
881
 
856
882
 static const struct stm32_timer_trigger_cfg stm32_timer_trg_cfg = {
857
883
 	.valids_table = valids_table,
..
..
@@ -877,9 +903,11 @@
877
903
 
878
904
 static struct platform_driver stm32_timer_trigger_driver = {
879
905
 	.probe = stm32_timer_trigger_probe,
906
+	.remove = stm32_timer_trigger_remove,
880
907
 	.driver = {
881
908
 		.name = "stm32-timer-trigger",
882
909
 		.of_match_table = stm32_trig_of_match,
910
+		.pm = &stm32_timer_trigger_pm_ops,
883
911
 	},
884
912
 };
885
913
 module_platform_driver(stm32_timer_trigger_driver);