rk3568 rt init

hc

2023-11-06 e3e12f52b214121840b44c91de5b3e5af5d3eb84

 kernel/drivers/mfd/fusb302.c
..
..
@@ -17,6 +17,8 @@
17
17
 #include <linux/of_gpio.h>
18
18
 #include <linux/regmap.h>
19
19
 #include <linux/power_supply.h>
20
+#include <linux/kthread.h>
21
+#include <uapi/linux/sched/types.h>
20
22
 
21
23
 #include "fusb302.h"
22
24
 
..
..
@@ -188,12 +190,12 @@
188
190
 			break;
189
191
 		case 1:
190
192
 			/* Battery */
191
-			if ((CAP_VPDO_VOLTAGE(chip->rec_load[i]) * 50) <=
193
+			if ((CAP_VPDO_MAX_VOLTAGE(chip->rec_load[i]) * 50) <=
192
194
 			    max_vol &&
193
195
 			    (CAP_VPDO_CURRENT(chip->rec_load[i]) * 10) <=
194
196
 			    max_cur) {
195
197
 				chip->pos_power = i + 1;
196
-				tmp = CAP_VPDO_VOLTAGE(chip->rec_load[i]);
198
+				tmp = CAP_VPDO_MAX_VOLTAGE(chip->rec_load[i]);
197
199
 				chip->pd_output_vol = tmp * 50;
198
200
 				tmp = CAP_VPDO_CURRENT(chip->rec_load[i]);
199
201
 				chip->pd_output_cur = tmp * 10;
..
..
@@ -929,6 +931,7 @@
929
931
 static void set_mesg(struct fusb30x_chip *chip, int cmd, int is_DMT)
930
932
 {
931
933
 	int i;
934
+	uint32_t rec_load;
932
935
 	struct PD_CAP_INFO *pd_cap_info = &chip->pd_cap_info;
933
936
 
934
937
 	chip->send_head = ((chip->msg_id & 0x7) << 9) |
..
..
@@ -962,23 +965,62 @@
962
965
 					    (0 << 25) |
963
966
 					    (0 << 24);
964
967
 
965
-			switch (CAP_POWER_TYPE(chip->rec_load[chip->pos_power - 1])) {
968
+			rec_load = chip->rec_load[chip->pos_power - 1];
969
+			switch (CAP_POWER_TYPE(rec_load)) {
966
970
 			case 0:
967
971
 				/* Fixed Supply */
968
-				chip->send_load[0] |= ((CAP_FPDO_VOLTAGE(chip->rec_load[chip->pos_power - 1]) << 10) & 0x3ff);
969
-				chip->send_load[0] |= (CAP_FPDO_CURRENT(chip->rec_load[chip->pos_power - 1]) & 0x3ff);
972
+				chip->sink_supply_type = 0;
973
+				chip->sink_volt = CAP_FPDO_VOLTAGE(rec_load);
974
+				chip->sink_opr_cur = CAP_FPDO_CURRENT(rec_load);
975
+				chip->send_load[0] |= chip->sink_volt << 10;
976
+				chip->send_load[0] |= chip->sink_opr_cur;
970
977
 				break;
971
978
 			case 1:
972
-				/* Battery */
973
-				chip->send_load[0] |= ((CAP_VPDO_VOLTAGE(chip->rec_load[chip->pos_power - 1]) << 10) & 0x3ff);
974
-				chip->send_load[0] |= (CAP_VPDO_CURRENT(chip->rec_load[chip->pos_power - 1]) & 0x3ff);
979
+				/* Battery Supply */
980
+				chip->sink_supply_type = 1;
981
+				chip->sink_max_volt =
982
+					CAP_VPDO_MAX_VOLTAGE(rec_load);
983
+				chip->sink_min_volt =
984
+					CAP_VPDO_MIN_VOLTAGE(rec_load);
985
+				chip->sink_opr_power =
986
+					CAP_VPDO_CURRENT(rec_load);
987
+				chip->send_load[0] |= chip->sink_max_volt << 20;
988
+				chip->send_load[0] |= chip->sink_min_volt << 10;
989
+				chip->send_load[0] |= chip->sink_opr_power;
975
990
 				break;
976
991
 			default:
977
-				/* not meet battery caps */
992
+				dev_warn(chip->dev, "No support supply req type %d\n",
993
+					 CAP_POWER_TYPE(rec_load));
978
994
 				break;
979
995
 			}
980
996
 			break;
981
997
 		case DMT_SINKCAPABILITIES:
998
+			chip->send_head |= (1 << 12) | (cmd & 0xf);
999
+			switch (chip->sink_supply_type) {
1000
+			case 0:
1001
+				/*
1002
+				 * Fixed Supply
1003
+				 * bit26 for 'USB Communiications Capable'
1004
+				 */
1005
+				chip->send_load[0] =
1006
+					(chip->sink_supply_type << 30) |
1007
+					(1 << 26) |
1008
+					(chip->sink_volt << 10) |
1009
+					(chip->sink_opr_cur);
1010
+				break;
1011
+			case 1:
1012
+				/* Battery Supply */
1013
+				chip->send_load[0] =
1014
+					(chip->sink_supply_type << 30) |
1015
+					(chip->sink_max_volt << 20) |
1016
+					(chip->sink_min_volt << 10) |
1017
+					(chip->sink_opr_cur);
1018
+				break;
1019
+			default:
1020
+				dev_warn(chip->dev, "No support sink supply type %d\n",
1021
+					 chip->sink_supply_type);
1022
+				break;
1023
+			}
982
1024
 			break;
983
1025
 		case DMT_VENDERDEFINED:
984
1026
 			break;
..
..
@@ -2529,7 +2571,7 @@
2529
2571
 			break;
2530
2572
 		case 1:
2531
2573
 			/* Battery */
2532
-			if (CAP_VPDO_VOLTAGE(chip->rec_load[tmp]) <= 100)
2574
+			if (CAP_VPDO_MAX_VOLTAGE(chip->rec_load[tmp]) <= 100)
2533
2575
 				chip->pos_power = tmp + 1;
2534
2576
 			break;
2535
2577
 		default:
..
..
@@ -2620,6 +2662,14 @@
2620
2662
 			chip->notify.is_pd_connected = true;
2621
2663
 			dev_info(chip->dev,
2622
2664
 				 "PD connected as UFP, fetching 5V\n");
2665
+			tcpm_get_message(chip);
2666
+			if (PACKET_IS_CONTROL_MSG(chip->rec_head,
2667
+						  CMT_GETSINKCAP)) {
2668
+				set_mesg(chip, DMT_SINKCAPABILITIES,
2669
+					 DATAMESSAGE);
2670
+				chip->tx_state = tx_idle;
2671
+				policy_send_data(chip);
2672
+			}
2623
2673
 			set_state(chip, policy_snk_ready);
2624
2674
 		} else if (PACKET_IS_DATA_MSG(chip->rec_head,
2625
2675
 					      DMT_SOURCECAPABILITIES)) {
..
..
@@ -2671,7 +2721,12 @@
2671
2721
 static void fusb_state_snk_ready(struct fusb30x_chip *chip, u32 evt)
2672
2722
 {
2673
2723
 	if (evt & EVENT_RX) {
2674
-		if (PACKET_IS_DATA_MSG(chip->rec_head, DMT_VENDERDEFINED)) {
2724
+		if (PACKET_IS_CONTROL_MSG(chip->rec_head, CMT_GETSINKCAP)) {
2725
+			set_mesg(chip, DMT_SINKCAPABILITIES, DATAMESSAGE);
2726
+			chip->tx_state = tx_idle;
2727
+			policy_send_data(chip);
2728
+		} else if (PACKET_IS_DATA_MSG(chip->rec_head,
2729
+					      DMT_VENDERDEFINED)) {
2675
2730
 			process_vdm_msg(chip);
2676
2731
 			chip->work_continue |= EVENT_WORK_CONTINUE;
2677
2732
 			chip->timer_state = T_DISABLED;
..
..
@@ -3189,22 +3244,22 @@
3189
3244
 
3190
3245
 BACK:
3191
3246
 	if (chip->work_continue) {
3192
-		queue_work(chip->fusb30x_wq, &chip->work);
3247
+		kthread_queue_work(chip->irq_worker, &chip->irq_work);
3193
3248
 		return;
3194
3249
 	}
3195
3250
 
3196
3251
 	if (!platform_get_device_irq_state(chip))
3197
3252
 		fusb_irq_enable(chip);
3198
3253
 	else
3199
-		queue_work(chip->fusb30x_wq, &chip->work);
3254
+		kthread_queue_work(chip->irq_worker, &chip->irq_work);
3200
3255
 }
3201
3256
 
3202
3257
 static irqreturn_t cc_interrupt_handler(int irq, void *dev_id)
3203
3258
 {
3204
3259
 	struct fusb30x_chip *chip = dev_id;
3205
3260
 
3206
-	queue_work(chip->fusb30x_wq, &chip->work);
3207
3261
 	fusb_irq_disable(chip);
3262
+	kthread_queue_work(chip->irq_worker, &chip->irq_work);
3208
3263
 	return IRQ_HANDLED;
3209
3264
 }
3210
3265
 
..
..
@@ -3266,8 +3321,8 @@
3266
3321
 	}
3267
3322
 
3268
3323
 	if (i != fusb30x_port_used)
3269
-		queue_work(fusb30x_port_info[i]->fusb30x_wq,
3270
-			   &fusb30x_port_info[i]->work);
3324
+		kthread_queue_work(fusb30x_port_info[i]->irq_worker,
3325
+				   &fusb30x_port_info[i]->irq_work);
3271
3326
 
3272
3327
 	return HRTIMER_NORESTART;
3273
3328
 }
..
..
@@ -3286,11 +3341,11 @@
3286
3341
 	chip->timer_mux = T_DISABLED;
3287
3342
 }
3288
3343
 
3289
-static void fusb302_work_func(struct work_struct *work)
3344
+static void fusb302_work_func(struct kthread_work *work)
3290
3345
 {
3291
3346
 	struct fusb30x_chip *chip;
3292
3347
 
3293
-	chip = container_of(work, struct fusb30x_chip, work);
3348
+	chip = container_of(work, struct fusb30x_chip, irq_work);
3294
3349
 	if (!chip->suspended)
3295
3350
 		state_machine_typec(chip);
3296
3351
 }
..
..
@@ -3300,6 +3355,7 @@
3300
3355
 {
3301
3356
 	struct fusb30x_chip *chip;
3302
3357
 	struct PD_CAP_INFO *pd_cap_info;
3358
+	struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
3303
3359
 	int ret;
3304
3360
 	char *string[2];
3305
3361
 
..
..
@@ -3326,8 +3382,11 @@
3326
3382
 
3327
3383
 	fusb_initialize_timer(chip);
3328
3384
 
3329
-	chip->fusb30x_wq = create_workqueue("fusb302_wq");
3330
-	INIT_WORK(&chip->work, fusb302_work_func);
3385
+	chip->irq_worker = kthread_create_worker(0, dev_name(chip->dev));
3386
+	if (IS_ERR(chip->irq_worker))
3387
+		return PTR_ERR(chip->irq_worker);
3388
+	sched_setscheduler_nocheck(chip->irq_worker->task, SCHED_FIFO, &param);
3389
+	kthread_init_work(&chip->irq_work, fusb302_work_func);
3331
3390
 
3332
3391
 	chip->role = ROLE_MODE_NONE;
3333
3392
 	chip->try_role = ROLE_MODE_NONE;
..
..
@@ -3368,6 +3427,8 @@
3368
3427
 	chip->n_caps_used = 1;
3369
3428
 	chip->source_power_supply[0] = 0x64;
3370
3429
 	chip->source_max_current[0] = 0x96;
3430
+	chip->sink_volt = 100;
3431
+	chip->sink_opr_cur = 200;
3371
3432
 
3372
3433
 	pd_cap_info = &chip->pd_cap_info;
3373
3434
 	pd_cap_info->dual_role_power = 1;
..
..
@@ -3468,13 +3529,8 @@
3468
3529
 		goto IRQ_ERR;
3469
3530
 	}
3470
3531
 
3471
-	ret = devm_request_threaded_irq(&client->dev,
3472
-					chip->gpio_int_irq,
3473
-					NULL,
3474
-					cc_interrupt_handler,
3475
-					IRQF_ONESHOT | IRQF_TRIGGER_LOW,
3476
-					client->name,
3477
-					chip);
3532
+	ret = request_irq(chip->gpio_int_irq, cc_interrupt_handler,
3533
+			  IRQF_TRIGGER_LOW, "fsc_interrupt_int_n", chip);
3478
3534
 	if (ret) {
3479
3535
 		dev_err(&client->dev, "irq request failed\n");
3480
3536
 		goto IRQ_ERR;
..
..
@@ -3503,7 +3559,7 @@
3503
3559
 	}
3504
3560
 	return 0;
3505
3561
 IRQ_ERR:
3506
-	destroy_workqueue(chip->fusb30x_wq);
3562
+	kthread_destroy_worker(chip->irq_worker);
3507
3563
 	return ret;
3508
3564
 }
3509
3565
 
..
..
@@ -3511,7 +3567,8 @@
3511
3567
 {
3512
3568
 	struct fusb30x_chip *chip = i2c_get_clientdata(client);
3513
3569
 
3514
-	destroy_workqueue(chip->fusb30x_wq);
3570
+	free_irq(chip->gpio_int_irq, chip);
3571
+	kthread_destroy_worker(chip->irq_worker);
3515
3572
 	return 0;
3516
3573
 }
3517
3574
 
..
..
@@ -3534,7 +3591,7 @@
3534
3591
 
3535
3592
 	fusb_irq_disable(chip);
3536
3593
 	chip->suspended = true;
3537
-	cancel_work_sync(&chip->work);
3594
+	kthread_cancel_work_sync(&chip->irq_work);
3538
3595
 
3539
3596
 	return 0;
3540
3597
 }
..
..
@@ -3545,7 +3602,7 @@
3545
3602
 
3546
3603
 	fusb_irq_enable(chip);
3547
3604
 	chip->suspended = false;
3548
-	queue_work(chip->fusb30x_wq, &chip->work);
3605
+	kthread_queue_work(chip->irq_worker, &chip->irq_work);
3549
3606
 
3550
3607
 	return 0;
3551
3608
 }