From e3e12f52b214121840b44c91de5b3e5af5d3eb84 Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Mon, 06 Nov 2023 03:04:41 +0000
Subject: [PATCH] rk3568 rt init
---
kernel/drivers/mfd/fusb302.c | 119 ++++++++++++++++++++++++++++++++++++++++++++---------------
1 files changed, 88 insertions(+), 31 deletions(-)
diff --git a/kernel/drivers/mfd/fusb302.c b/kernel/drivers/mfd/fusb302.c
index 205739f..1044b59 100644
--- a/kernel/drivers/mfd/fusb302.c
+++ b/kernel/drivers/mfd/fusb302.c
@@ -17,6 +17,8 @@
#include <linux/of_gpio.h>
#include <linux/regmap.h>
#include <linux/power_supply.h>
+#include <linux/kthread.h>
+#include <uapi/linux/sched/types.h>
#include "fusb302.h"
@@ -188,12 +190,12 @@
break;
case 1:
/* Battery */
- if ((CAP_VPDO_VOLTAGE(chip->rec_load[i]) * 50) <=
+ if ((CAP_VPDO_MAX_VOLTAGE(chip->rec_load[i]) * 50) <=
max_vol &&
(CAP_VPDO_CURRENT(chip->rec_load[i]) * 10) <=
max_cur) {
chip->pos_power = i + 1;
- tmp = CAP_VPDO_VOLTAGE(chip->rec_load[i]);
+ tmp = CAP_VPDO_MAX_VOLTAGE(chip->rec_load[i]);
chip->pd_output_vol = tmp * 50;
tmp = CAP_VPDO_CURRENT(chip->rec_load[i]);
chip->pd_output_cur = tmp * 10;
@@ -929,6 +931,7 @@
static void set_mesg(struct fusb30x_chip *chip, int cmd, int is_DMT)
{
int i;
+ uint32_t rec_load;
struct PD_CAP_INFO *pd_cap_info = &chip->pd_cap_info;
chip->send_head = ((chip->msg_id & 0x7) << 9) |
@@ -962,23 +965,62 @@
(0 << 25) |
(0 << 24);
- switch (CAP_POWER_TYPE(chip->rec_load[chip->pos_power - 1])) {
+ rec_load = chip->rec_load[chip->pos_power - 1];
+ switch (CAP_POWER_TYPE(rec_load)) {
case 0:
/* Fixed Supply */
- chip->send_load[0] |= ((CAP_FPDO_VOLTAGE(chip->rec_load[chip->pos_power - 1]) << 10) & 0x3ff);
- chip->send_load[0] |= (CAP_FPDO_CURRENT(chip->rec_load[chip->pos_power - 1]) & 0x3ff);
+ chip->sink_supply_type = 0;
+ chip->sink_volt = CAP_FPDO_VOLTAGE(rec_load);
+ chip->sink_opr_cur = CAP_FPDO_CURRENT(rec_load);
+ chip->send_load[0] |= chip->sink_volt << 10;
+ chip->send_load[0] |= chip->sink_opr_cur;
break;
case 1:
- /* Battery */
- chip->send_load[0] |= ((CAP_VPDO_VOLTAGE(chip->rec_load[chip->pos_power - 1]) << 10) & 0x3ff);
- chip->send_load[0] |= (CAP_VPDO_CURRENT(chip->rec_load[chip->pos_power - 1]) & 0x3ff);
+ /* Battery Supply */
+ chip->sink_supply_type = 1;
+ chip->sink_max_volt =
+ CAP_VPDO_MAX_VOLTAGE(rec_load);
+ chip->sink_min_volt =
+ CAP_VPDO_MIN_VOLTAGE(rec_load);
+ chip->sink_opr_power =
+ CAP_VPDO_CURRENT(rec_load);
+ chip->send_load[0] |= chip->sink_max_volt << 20;
+ chip->send_load[0] |= chip->sink_min_volt << 10;
+ chip->send_load[0] |= chip->sink_opr_power;
break;
default:
- /* not meet battery caps */
+ dev_warn(chip->dev, "No support supply req type %d\n",
+ CAP_POWER_TYPE(rec_load));
break;
}
break;
case DMT_SINKCAPABILITIES:
+ chip->send_head |= (1 << 12) | (cmd & 0xf);
+ switch (chip->sink_supply_type) {
+ case 0:
+ /*
+ * Fixed Supply
+ * bit26 for 'USB Communiications Capable'
+ */
+ chip->send_load[0] =
+ (chip->sink_supply_type << 30) |
+ (1 << 26) |
+ (chip->sink_volt << 10) |
+ (chip->sink_opr_cur);
+ break;
+ case 1:
+ /* Battery Supply */
+ chip->send_load[0] =
+ (chip->sink_supply_type << 30) |
+ (chip->sink_max_volt << 20) |
+ (chip->sink_min_volt << 10) |
+ (chip->sink_opr_cur);
+ break;
+ default:
+ dev_warn(chip->dev, "No support sink supply type %d\n",
+ chip->sink_supply_type);
+ break;
+ }
break;
case DMT_VENDERDEFINED:
break;
@@ -2529,7 +2571,7 @@
break;
case 1:
/* Battery */
- if (CAP_VPDO_VOLTAGE(chip->rec_load[tmp]) <= 100)
+ if (CAP_VPDO_MAX_VOLTAGE(chip->rec_load[tmp]) <= 100)
chip->pos_power = tmp + 1;
break;
default:
@@ -2620,6 +2662,14 @@
chip->notify.is_pd_connected = true;
dev_info(chip->dev,
"PD connected as UFP, fetching 5V\n");
+ tcpm_get_message(chip);
+ if (PACKET_IS_CONTROL_MSG(chip->rec_head,
+ CMT_GETSINKCAP)) {
+ set_mesg(chip, DMT_SINKCAPABILITIES,
+ DATAMESSAGE);
+ chip->tx_state = tx_idle;
+ policy_send_data(chip);
+ }
set_state(chip, policy_snk_ready);
} else if (PACKET_IS_DATA_MSG(chip->rec_head,
DMT_SOURCECAPABILITIES)) {
@@ -2671,7 +2721,12 @@
static void fusb_state_snk_ready(struct fusb30x_chip *chip, u32 evt)
{
if (evt & EVENT_RX) {
- if (PACKET_IS_DATA_MSG(chip->rec_head, DMT_VENDERDEFINED)) {
+ if (PACKET_IS_CONTROL_MSG(chip->rec_head, CMT_GETSINKCAP)) {
+ set_mesg(chip, DMT_SINKCAPABILITIES, DATAMESSAGE);
+ chip->tx_state = tx_idle;
+ policy_send_data(chip);
+ } else if (PACKET_IS_DATA_MSG(chip->rec_head,
+ DMT_VENDERDEFINED)) {
process_vdm_msg(chip);
chip->work_continue |= EVENT_WORK_CONTINUE;
chip->timer_state = T_DISABLED;
@@ -3189,22 +3244,22 @@
BACK:
if (chip->work_continue) {
- queue_work(chip->fusb30x_wq, &chip->work);
+ kthread_queue_work(chip->irq_worker, &chip->irq_work);
return;
}
if (!platform_get_device_irq_state(chip))
fusb_irq_enable(chip);
else
- queue_work(chip->fusb30x_wq, &chip->work);
+ kthread_queue_work(chip->irq_worker, &chip->irq_work);
}
static irqreturn_t cc_interrupt_handler(int irq, void *dev_id)
{
struct fusb30x_chip *chip = dev_id;
- queue_work(chip->fusb30x_wq, &chip->work);
fusb_irq_disable(chip);
+ kthread_queue_work(chip->irq_worker, &chip->irq_work);
return IRQ_HANDLED;
}
@@ -3266,8 +3321,8 @@
}
if (i != fusb30x_port_used)
- queue_work(fusb30x_port_info[i]->fusb30x_wq,
- &fusb30x_port_info[i]->work);
+ kthread_queue_work(fusb30x_port_info[i]->irq_worker,
+ &fusb30x_port_info[i]->irq_work);
return HRTIMER_NORESTART;
}
@@ -3286,11 +3341,11 @@
chip->timer_mux = T_DISABLED;
}
-static void fusb302_work_func(struct work_struct *work)
+static void fusb302_work_func(struct kthread_work *work)
{
struct fusb30x_chip *chip;
- chip = container_of(work, struct fusb30x_chip, work);
+ chip = container_of(work, struct fusb30x_chip, irq_work);
if (!chip->suspended)
state_machine_typec(chip);
}
@@ -3300,6 +3355,7 @@
{
struct fusb30x_chip *chip;
struct PD_CAP_INFO *pd_cap_info;
+ struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
int ret;
char *string[2];
@@ -3326,8 +3382,11 @@
fusb_initialize_timer(chip);
- chip->fusb30x_wq = create_workqueue("fusb302_wq");
- INIT_WORK(&chip->work, fusb302_work_func);
+ chip->irq_worker = kthread_create_worker(0, dev_name(chip->dev));
+ if (IS_ERR(chip->irq_worker))
+ return PTR_ERR(chip->irq_worker);
+ sched_setscheduler_nocheck(chip->irq_worker->task, SCHED_FIFO, ¶m);
+ kthread_init_work(&chip->irq_work, fusb302_work_func);
chip->role = ROLE_MODE_NONE;
chip->try_role = ROLE_MODE_NONE;
@@ -3368,6 +3427,8 @@
chip->n_caps_used = 1;
chip->source_power_supply[0] = 0x64;
chip->source_max_current[0] = 0x96;
+ chip->sink_volt = 100;
+ chip->sink_opr_cur = 200;
pd_cap_info = &chip->pd_cap_info;
pd_cap_info->dual_role_power = 1;
@@ -3468,13 +3529,8 @@
goto IRQ_ERR;
}
- ret = devm_request_threaded_irq(&client->dev,
- chip->gpio_int_irq,
- NULL,
- cc_interrupt_handler,
- IRQF_ONESHOT | IRQF_TRIGGER_LOW,
- client->name,
- chip);
+ ret = request_irq(chip->gpio_int_irq, cc_interrupt_handler,
+ IRQF_TRIGGER_LOW, "fsc_interrupt_int_n", chip);
if (ret) {
dev_err(&client->dev, "irq request failed\n");
goto IRQ_ERR;
@@ -3503,7 +3559,7 @@
}
return 0;
IRQ_ERR:
- destroy_workqueue(chip->fusb30x_wq);
+ kthread_destroy_worker(chip->irq_worker);
return ret;
}
@@ -3511,7 +3567,8 @@
{
struct fusb30x_chip *chip = i2c_get_clientdata(client);
- destroy_workqueue(chip->fusb30x_wq);
+ free_irq(chip->gpio_int_irq, chip);
+ kthread_destroy_worker(chip->irq_worker);
return 0;
}
@@ -3534,7 +3591,7 @@
fusb_irq_disable(chip);
chip->suspended = true;
- cancel_work_sync(&chip->work);
+ kthread_cancel_work_sync(&chip->irq_work);
return 0;
}
@@ -3545,7 +3602,7 @@
fusb_irq_enable(chip);
chip->suspended = false;
- queue_work(chip->fusb30x_wq, &chip->work);
+ kthread_queue_work(chip->irq_worker, &chip->irq_work);
return 0;
}
--
Gitblit v1.6.2