@@ -39,11 +39,35 @@
*/
#define UCSI_SWAP_TIMEOUT_MS 5000
+static void ucsi_update_get_current_cam_cmd(struct ucsi_connector *con,
+ struct ucsi_data *data)
+{
+ u8 cam, new_cam;
+
+ if (data->cci.data_length == 0x1) {
+ cam = data->message_in[0];
+ new_cam = con->port_alt[cam].linked_idx;
+ data->message_in[0] = new_cam;
+ con->new_port_alt[new_cam].active_idx = cam;
+ }
+}
+
static inline int ucsi_sync(struct ucsi *ucsi)
{
- if (ucsi->ppm && ucsi->ppm->sync)
- return ucsi->ppm->sync(ucsi->ppm);
- return 0;
+ struct ucsi_connector *con = ucsi->connector;
+ struct ucsi_data *data = ucsi->ppm->data;
+ int ret = 0;
+
+ if (ucsi->ppm && ucsi->ppm->sync) {
+ ret = ucsi->ppm->sync(ucsi->ppm);
+ if (ret)
+ return ret;
+ }
+
+ if (data->ctrl.alt.cmd == UCSI_GET_CURRENT_CAM && con->has_multiple_dp)
+ ucsi_update_get_current_cam_cmd(con, data);
+
+ return ret;
}
static int ucsi_command(struct ucsi *ucsi, struct ucsi_control *ctrl)
@@ -101,14 +125,65 @@ static int ucsi_ack(struct ucsi *ucsi, u8 ack)
return ret;
}
+static void ucsi_update_set_new_cam_cmd(struct ucsi_connector *con,
+ struct ucsi_control *ctrl)
+{
+ struct new_ucsi_altmode *new_port, *port;
+ struct typec_altmode *alt = NULL;
+ u64 cmd;
+ u8 new_cam, cam, pin;
+ bool enter_new_mode;
+ int i, j, k = 0xff;
+
+ cmd = ctrl->raw_cmd;
+ new_cam = (cmd >> 24) & 0xff;
+ new_port = &con->new_port_alt[new_cam];
+ cam = new_port->linked_idx;
+ enter_new_mode = (cmd >> 23) & 1;
+
+ if (cam == UCSI_MULTI_LINKED_INDEX) {
+ if (enter_new_mode) {
+ port = con->port_alt;
+ for (i = 0; con->partner_altmode[i]; i++) {
+ alt = con->partner_altmode[i];
+ if (alt->svid == new_port->svid)
+ break;
+ }
+ for (j = 0; port[j].svid; j++) {
+ pin = DP_CONF_GET_PIN_ASSIGN(port[j].mid);
+ if (alt && port[j].svid == alt->svid &&
+ (pin & DP_CONF_GET_PIN_ASSIGN(alt->vdo))) {
+ /* prioritize pin E->D->C */
+ if (k == 0xff || (k != 0xff && pin >
+ DP_CONF_GET_PIN_ASSIGN(port[k].mid))
+ ) {
+ k = j;
+ }
+ }
+ }
+ cam = k;
+ new_port->active_idx = cam;
+ } else {
+ cam = new_port->active_idx;
+ }
+ }
+ cmd &= ~(0xff << 24);
+ cmd |= (cam << 24);
+ ctrl->raw_cmd = cmd;
+}
+
static int ucsi_run_command(struct ucsi *ucsi, struct ucsi_control *ctrl,
void *data, size_t size)
{
struct ucsi_control _ctrl;
+ struct ucsi_connector *con = ucsi->connector;
u8 data_length;
u16 error;
int ret;
+ if (ctrl->alt.cmd == UCSI_SET_NEW_CAM && con->has_multiple_dp)
+ ucsi_update_set_new_cam_cmd(con, ctrl);
+
ret = ucsi_command(ucsi, ctrl);
if (ret)
goto err;
@@ -364,10 +439,24 @@ static int ucsi_register_altmodes(struct ucsi_connector *con, u8 recipient)
for (i = 0; i < max_altmodes;) {
memset(alt, 0, sizeof(alt));
- UCSI_CMD_GET_ALTERNATE_MODES(ctrl, recipient, con->num, i, 1);
- len = ucsi_run_command(con->ucsi, &ctrl, alt, sizeof(alt));
- if (len <= 0)
- return len;
+
+ if (recipient == UCSI_RECIPIENT_CON) {
+ if (con->has_multiple_dp) {
+ alt[0].svid = con->new_port_alt[i].svid;
+ alt[0].mid = con->new_port_alt[i].mid;
+ } else {
+ alt[0].svid = con->port_alt[i].svid;
+ alt[0].mid = con->port_alt[i].mid;
+ }
+ len = sizeof(alt[0]);
+ } else {
+ UCSI_CMD_GET_ALTERNATE_MODES(ctrl, recipient,
+ con->num, i, 1);
+ len = ucsi_run_command(con->ucsi, &ctrl, alt,
+ sizeof(alt));
+ if (len <= 0)
+ return len;
+ }
/*
* This code is requesting one alt mode at a time, but some PPMs
@@ -521,6 +610,103 @@ static void ucsi_partner_change(struct ucsi_connector *con)
ucsi_altmode_update_active(con);
}
+static void ucsi_update_con_altmodes(struct ucsi_connector *con)
+{
+ int max_altmodes = con->ucsi->cap.num_alt_modes;
+ struct new_ucsi_altmode *alt, *new_alt;
+ int i, j, k = 0;
+ bool found = false;
+
+ alt = con->port_alt;
+ new_alt = con->new_port_alt;
+ memset(con->new_port_alt, 0, sizeof(con->new_port_alt));
+
+ for (i = 0; i < max_altmodes; i++) {
+ if (!alt[i].svid)
+ break;
+
+ /* already checked and considered */
+ if (alt[i].checked)
+ continue;
+
+ if (!DP_CONF_GET_PIN_ASSIGN(alt[i].mid)) {
+ /* Found Non DP altmode */
+ new_alt[k].svid = alt[i].svid;
+ new_alt[k].mid |= alt[i].mid;
+ new_alt[k].linked_idx = i;
+ alt[i].linked_idx = k;
+ k++;
+ continue;
+ }
+
+ for (j = i + 1; j < max_altmodes; j++) {
+ if (alt[i].svid != alt[j].svid ||
+ !DP_CONF_GET_PIN_ASSIGN(alt[j].mid)) {
+ continue;
+ } else {
+ /* Found duplicate DP mode */
+ new_alt[k].svid = alt[i].svid;
+ new_alt[k].mid |= alt[i].mid | alt[j].mid;
+ new_alt[k].linked_idx = UCSI_MULTI_LINKED_INDEX;
+ alt[i].linked_idx = k;
+ alt[j].linked_idx = k;
+ alt[j].checked = true;
+ found = true;
+ }
+ }
+ if (found) {
+ con->has_multiple_dp = true;
+ } else {
+ /* Didn't find any duplicate DP altmode */
+ new_alt[k].svid = alt[i].svid;
+ new_alt[k].mid |= alt[i].mid;
+ new_alt[k].linked_idx = i;
+ alt[i].linked_idx = k;
+ }
+ k++;
+ }
+}
+
+static int ucsi_get_altmodes(struct ucsi_connector *con)
+{
+ int max_altmodes = con->ucsi->cap.num_alt_modes;
+ struct ucsi_altmode alt[2];
+ struct ucsi_control ctrl;
+ int num = 1;
+ int len;
+ int j;
+ int i;
+ int k = 0;
+
+ memset(con->port_alt, 0, sizeof(con->port_alt));
+
+ for (i = 0; i < max_altmodes;) {
+ memset(alt, 0, sizeof(alt));
+ UCSI_CMD_GET_ALTERNATE_MODES(ctrl, UCSI_RECIPIENT_CON,
+ con->num, i, 1);
+ len = ucsi_run_command(con->ucsi, &ctrl, alt, sizeof(alt));
+ if (len <= 0)
+ return len;
+
+ /*
+ * This code is requesting one alt mode at a time, but some PPMs
+ * may still return two.
+ */
+ num = len / sizeof(alt[0]);
+ i += num;
+
+ for (j = 0; j < num; j++) {
+ if (!alt[j].svid)
+ return 0;
+
+ con->port_alt[k].mid = alt[j].mid;
+ con->port_alt[k].svid = alt[j].svid;
+ k++;
+ }
+ }
+ return 0;
+}
+
static void ucsi_connector_change(struct work_struct *work)
{
struct ucsi_connector *con = container_of(work, struct ucsi_connector,
@@ -851,6 +1037,16 @@ static int ucsi_register_port(struct ucsi *ucsi, int index)
if (IS_ERR(con->port))
return PTR_ERR(con->port);
+ /* Get Alternate modes */
+ ret = ucsi_get_altmodes(con);
+ if (ret) {
+ dev_err(ucsi->dev,
+ "%s: con%d failed (%d)to get port alternate modes\n",
+ __func__, con->num, ret);
+ return 0;
+ }
+ ucsi_update_con_altmodes(con);
+
/* Alternate modes */
ret = ucsi_register_altmodes(con, UCSI_RECIPIENT_CON);
if (ret)
@@ -274,6 +274,15 @@ struct ucsi_connector_capability {
u8:6; /* reserved */
} __packed;
+struct new_ucsi_altmode {
+ u16 svid;
+ u32 mid;
+ u8 linked_idx;
+ u8 active_idx;
+#define UCSI_MULTI_LINKED_INDEX (0xff)
+ bool checked;
+} __packed;
+
struct ucsi_altmode {
u16 svid;
u32 mid;
@@ -408,6 +417,7 @@ struct ucsi {
struct ucsi_connector {
int num;
+ bool has_multiple_dp;
struct ucsi *ucsi;
struct mutex lock; /* port lock */
@@ -424,6 +434,8 @@ struct ucsi_connector {
struct ucsi_connector_status status;
struct ucsi_connector_capability cap;
+ struct new_ucsi_altmode port_alt[UCSI_MAX_ALTMODES];
+ struct new_ucsi_altmode new_port_alt[UCSI_MAX_ALTMODES];
};
int ucsi_send_command(struct ucsi *ucsi, struct ucsi_control *ctrl,