diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c index 0e1cec346e0f..1d8b7df59ff4 100644 --- a/drivers/usb/typec/ucsi/ucsi.c +++ b/drivers/usb/typec/ucsi/ucsi.c @@ -495,7 +495,8 @@ static void ucsi_unregister_altmodes(struct ucsi_connector *con, u8 recipient) } } -static void ucsi_get_pdos(struct ucsi_connector *con, int is_partner) +static int ucsi_get_pdos(struct ucsi_connector *con, int is_partner, + u32 *pdos, int offset, int num_pdos) { struct ucsi *ucsi = con->ucsi; u64 command; @@ -503,17 +504,39 @@ static void ucsi_get_pdos(struct ucsi_connector *con, int is_partner) command = UCSI_COMMAND(UCSI_GET_PDOS) | UCSI_CONNECTOR_NUMBER(con->num); command |= UCSI_GET_PDOS_PARTNER_PDO(is_partner); - command |= UCSI_GET_PDOS_NUM_PDOS(UCSI_MAX_PDOS - 1); + command |= UCSI_GET_PDOS_PDO_OFFSET(offset); + command |= UCSI_GET_PDOS_NUM_PDOS(num_pdos - 1); command |= UCSI_GET_PDOS_SRC_PDOS; - ret = ucsi_send_command(ucsi, command, con->src_pdos, - sizeof(con->src_pdos)); - if (ret < 0) { + ret = ucsi_send_command(ucsi, command, pdos + offset, + num_pdos * sizeof(u32)); + if (ret < 0) dev_err(ucsi->dev, "UCSI_GET_PDOS failed (%d)\n", ret); - return; - } - con->num_pdos = ret / sizeof(u32); /* number of bytes to 32-bit PDOs */ - if (ret == 0) + if (ret == 0 && offset == 0) dev_warn(ucsi->dev, "UCSI_GET_PDOS returned 0 bytes\n"); + + return ret; +} + +static void ucsi_get_src_pdos(struct ucsi_connector *con, int is_partner) +{ + int ret; + + /* UCSI max payload means only getting at most 4 PDOs at a time */ + ret = ucsi_get_pdos(con, 1, con->src_pdos, 0, UCSI_MAX_PDOS); + if (ret < 0) + return; + + con->num_pdos = ret / sizeof(u32); /* number of bytes to 32-bit PDOs */ + if (con->num_pdos < UCSI_MAX_PDOS) + return; + + /* get the remaining PDOs, if any */ + ret = ucsi_get_pdos(con, 1, con->src_pdos, UCSI_MAX_PDOS, + PDO_MAX_OBJECTS - UCSI_MAX_PDOS); + if (ret < 0) + return; + + con->num_pdos += ret / sizeof(u32); } static void ucsi_pwr_opmode_change(struct ucsi_connector *con) @@ -522,7 +545,7 @@ static void ucsi_pwr_opmode_change(struct ucsi_connector *con) case UCSI_CONSTAT_PWR_OPMODE_PD: con->rdo = con->status.request_data_obj; typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_PD); - ucsi_get_pdos(con, 1); + ucsi_get_src_pdos(con, 1); break; case UCSI_CONSTAT_PWR_OPMODE_TYPEC1_5: con->rdo = 0; diff --git a/drivers/usb/typec/ucsi/ucsi.h b/drivers/usb/typec/ucsi/ucsi.h index 3920e20a9e9e..cee666790907 100644 --- a/drivers/usb/typec/ucsi/ucsi.h +++ b/drivers/usb/typec/ucsi/ucsi.h @@ -8,6 +8,7 @@ #include #include #include +#include #include /* -------------------------------------------------------------------------- */ @@ -134,7 +135,9 @@ void ucsi_connector_change(struct ucsi *ucsi, u8 num); /* GET_PDOS command bits */ #define UCSI_GET_PDOS_PARTNER_PDO(_r_) ((u64)(_r_) << 23) +#define UCSI_GET_PDOS_PDO_OFFSET(_r_) ((u64)(_r_) << 24) #define UCSI_GET_PDOS_NUM_PDOS(_r_) ((u64)(_r_) << 32) +#define UCSI_MAX_PDOS (4) #define UCSI_GET_PDOS_SRC_PDOS ((u64)1 << 34) /* -------------------------------------------------------------------------- */ @@ -302,7 +305,6 @@ struct ucsi { #define UCSI_MAX_SVID 5 #define UCSI_MAX_ALTMODES (UCSI_MAX_SVID * 6) -#define UCSI_MAX_PDOS (4) #define UCSI_TYPEC_VSAFE5V 5000 #define UCSI_TYPEC_1_5_CURRENT 1500 @@ -330,7 +332,7 @@ struct ucsi_connector { struct power_supply *psy; struct power_supply_desc psy_desc; u32 rdo; - u32 src_pdos[UCSI_MAX_PDOS]; + u32 src_pdos[PDO_MAX_OBJECTS]; int num_pdos; struct usb_role_switch *usb_role_sw;