drivers/usb/typec/ucsi/ucsi.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * USB Type-C Connector System Software Interface driver
   4  *
   5  * Copyright (C) 2017, Intel Corporation
   6  * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
   7  */
   8
   9 #include <linux/completion.h>
  10 #include <linux/property.h>
  11 #include <linux/device.h>
  12 #include <linux/module.h>
  13 #include <linux/delay.h>
  14 #include <linux/slab.h>
  15 #include <linux/usb/typec_dp.h>
  16
  17 #include "ucsi.h"
  18 #include "trace.h"
  19
  20 /*
  21  * UCSI_TIMEOUT_MS - PPM communication timeout
  22  *
  23  * Ideally we could use MIN_TIME_TO_RESPOND_WITH_BUSY (which is defined in UCSI
  24  * specification) here as reference, but unfortunately we can't. It is very
  25  * difficult to estimate the time it takes for the system to process the command
  26  * before it is actually passed to the PPM.
  27  */
  28 #define UCSI_TIMEOUT_MS         5000
  29
  30 /*
  31  * UCSI_SWAP_TIMEOUT_MS - Timeout for role swap requests
  32  *
  33  * 5 seconds is close to the time it takes for CapsCounter to reach 0, so even
  34  * if the PPM does not generate Connector Change events before that with
  35  * partners that do not support USB Power Delivery, this should still work.
  36  */
  37 #define UCSI_SWAP_TIMEOUT_MS    5000
  38
  39 static int ucsi_acknowledge_command(struct ucsi *ucsi)
  40 {
  41         u64 ctrl;
  42
  43         ctrl = UCSI_ACK_CC_CI;
  44         ctrl |= UCSI_ACK_COMMAND_COMPLETE;
  45
  46         return ucsi->ops->sync_write(ucsi, UCSI_CONTROL, &ctrl, sizeof(ctrl));
  47 }
  48
  49 static int ucsi_acknowledge_connector_change(struct ucsi *ucsi)
  50 {
  51         u64 ctrl;
  52
  53         ctrl = UCSI_ACK_CC_CI;
  54         ctrl |= UCSI_ACK_CONNECTOR_CHANGE;
  55
  56         return ucsi->ops->sync_write(ucsi, UCSI_CONTROL, &ctrl, sizeof(ctrl));
  57 }
  58
  59 static int ucsi_exec_command(struct ucsi *ucsi, u64 command);
  60
  61 static int ucsi_read_error(struct ucsi *ucsi)
  62 {
  63         u16 error;
  64         int ret;
  65
  66         /* Acknowledge the command that failed */
  67         ret = ucsi_acknowledge_command(ucsi);
  68         if (ret)
  69                 return ret;
  70
  71         ret = ucsi_exec_command(ucsi, UCSI_GET_ERROR_STATUS);
  72         if (ret < 0)
  73                 return ret;
  74
  75         ret = ucsi->ops->read(ucsi, UCSI_MESSAGE_IN, &error, sizeof(error));
  76         if (ret)
  77                 return ret;
  78
  79         ret = ucsi_acknowledge_command(ucsi);
  80         if (ret)
  81                 return ret;
  82
  83         switch (error) {
  84         case UCSI_ERROR_INCOMPATIBLE_PARTNER:
  85                 return -EOPNOTSUPP;
  86         case UCSI_ERROR_CC_COMMUNICATION_ERR:
  87                 return -ECOMM;
  88         case UCSI_ERROR_CONTRACT_NEGOTIATION_FAIL:
  89                 return -EPROTO;
  90         case UCSI_ERROR_DEAD_BATTERY:
  91                 dev_warn(ucsi->dev, "Dead battery condition!\n");
  92                 return -EPERM;
  93         case UCSI_ERROR_INVALID_CON_NUM:
  94         case UCSI_ERROR_UNREGONIZED_CMD:
  95         case UCSI_ERROR_INVALID_CMD_ARGUMENT:
  96                 dev_err(ucsi->dev, "possible UCSI driver bug %u\n", error);
  97                 return -EINVAL;
  98         case UCSI_ERROR_OVERCURRENT:
  99                 dev_warn(ucsi->dev, "Overcurrent condition\n");
 100                 break;
 101         case UCSI_ERROR_PARTNER_REJECTED_SWAP:
 102                 dev_warn(ucsi->dev, "Partner rejected swap\n");
 103                 break;
 104         case UCSI_ERROR_HARD_RESET:
 105                 dev_warn(ucsi->dev, "Hard reset occurred\n");
 106                 break;
 107         case UCSI_ERROR_PPM_POLICY_CONFLICT:
 108                 dev_warn(ucsi->dev, "PPM Policy conflict\n");
 109                 break;
 110         case UCSI_ERROR_SWAP_REJECTED:
 111                 dev_warn(ucsi->dev, "Swap rejected\n");
 112                 break;
 113         case UCSI_ERROR_UNDEFINED:
 114         default:
 115                 dev_err(ucsi->dev, "unknown error %u\n", error);
 116                 break;
 117         }
 118
 119         return -EIO;
 120 }
 121
 122 static int ucsi_exec_command(struct ucsi *ucsi, u64 cmd)
 123 {
 124         u32 cci;
 125         int ret;
 126
 127         ret = ucsi->ops->sync_write(ucsi, UCSI_CONTROL, &cmd, sizeof(cmd));
 128         if (ret)
 129                 return ret;
 130
 131         ret = ucsi->ops->read(ucsi, UCSI_CCI, &cci, sizeof(cci));
 132         if (ret)
 133                 return ret;
 134
 135         if (cmd != UCSI_CANCEL && cci & UCSI_CCI_BUSY)
 136                 return ucsi_exec_command(ucsi, UCSI_CANCEL);
 137
 138         if (!(cci & UCSI_CCI_COMMAND_COMPLETE))
 139                 return -EIO;
 140
 141         if (cci & UCSI_CCI_NOT_SUPPORTED)
 142                 return -EOPNOTSUPP;
 143
 144         if (cci & UCSI_CCI_ERROR) {
 145                 if (cmd == UCSI_GET_ERROR_STATUS)
 146                         return -EIO;
 147                 return ucsi_read_error(ucsi);
 148         }
 149
 150         if (cmd == UCSI_CANCEL && cci & UCSI_CCI_CANCEL_COMPLETE) {
 151                 ret = ucsi_acknowledge_command(ucsi);
 152                 return ret ? ret : -EBUSY;
 153         }
 154
 155         return UCSI_CCI_LENGTH(cci);
 156 }
 157
 158 int ucsi_send_command(struct ucsi *ucsi, u64 command,
 159                       void *data, size_t size)
 160 {
 161         u8 length;
 162         int ret;
 163
 164         mutex_lock(&ucsi->ppm_lock);
 165
 166         ret = ucsi_exec_command(ucsi, command);
 167         if (ret < 0)
 168                 goto out;
 169
 170         length = ret;
 171
 172         if (data) {
 173                 ret = ucsi->ops->read(ucsi, UCSI_MESSAGE_IN, data, size);
 174                 if (ret)
 175                         goto out;
 176         }
 177
 178         ret = ucsi_acknowledge_command(ucsi);
 179         if (ret)
 180                 goto out;
 181
 182         ret = length;
 183 out:
 184         mutex_unlock(&ucsi->ppm_lock);
 185         return ret;
 186 }
 187 EXPORT_SYMBOL_GPL(ucsi_send_command);
 188
 189 /* -------------------------------------------------------------------------- */
 190
 191 struct ucsi_work {
 192         struct delayed_work work;
 193         struct list_head node;
 194         unsigned long delay;
 195         unsigned int count;
 196         struct ucsi_connector *con;
 197         int (*cb)(struct ucsi_connector *);
 198 };
 199
 200 static void ucsi_poll_worker(struct work_struct *work)
 201 {
 202         struct ucsi_work *uwork = container_of(work, struct ucsi_work, work.work);
 203         struct ucsi_connector *con = uwork->con;
 204         int ret;
 205
 206         mutex_lock(&con->lock);
 207
 208         if (!con->partner) {
 209                 list_del(&uwork->node);
 210                 mutex_unlock(&con->lock);
 211                 kfree(uwork);
 212                 return;
 213         }
 214
 215         ret = uwork->cb(con);
 216
 217         if (uwork->count-- && (ret == -EBUSY || ret == -ETIMEDOUT)) {
 218                 queue_delayed_work(con->wq, &uwork->work, uwork->delay);
 219         } else {
 220                 list_del(&uwork->node);
 221                 kfree(uwork);
 222         }
 223
 224         mutex_unlock(&con->lock);
 225 }
 226
 227 static int ucsi_partner_task(struct ucsi_connector *con,
 228                              int (*cb)(struct ucsi_connector *),
 229                              int retries, unsigned long delay)
 230 {
 231         struct ucsi_work *uwork;
 232
 233         if (!con->partner)
 234                 return 0;
 235
 236         uwork = kzalloc(sizeof(*uwork), GFP_KERNEL);
 237         if (!uwork)
 238                 return -ENOMEM;
 239
 240         INIT_DELAYED_WORK(&uwork->work, ucsi_poll_worker);
 241         uwork->count = retries;
 242         uwork->delay = delay;
 243         uwork->con = con;
 244         uwork->cb = cb;
 245
 246         list_add_tail(&uwork->node, &con->partner_tasks);
 247         queue_delayed_work(con->wq, &uwork->work, delay);
 248
 249         return 0;
 250 }
 251
 252 /* -------------------------------------------------------------------------- */
 253
 254 void ucsi_altmode_update_active(struct ucsi_connector *con)
 255 {
 256         const struct typec_altmode *altmode = NULL;
 257         u64 command;
 258         int ret;
 259         u8 cur;
 260         int i;
 261
 262         command = UCSI_GET_CURRENT_CAM | UCSI_CONNECTOR_NUMBER(con->num);
 263         ret = ucsi_send_command(con->ucsi, command, &cur, sizeof(cur));
 264         if (ret < 0) {
 265                 if (con->ucsi->version > 0x0100) {
 266                         dev_err(con->ucsi->dev,
 267                                 "GET_CURRENT_CAM command failed\n");
 268                         return;
 269                 }
 270                 cur = 0xff;
 271         }
 272
 273         if (cur < UCSI_MAX_ALTMODES)
 274                 altmode = typec_altmode_get_partner(con->port_altmode[cur]);
 275
 276         for (i = 0; con->partner_altmode[i]; i++)
 277                 typec_altmode_update_active(con->partner_altmode[i],
 278                                             con->partner_altmode[i] == altmode);
 279 }
 280
 281 static int ucsi_altmode_next_mode(struct typec_altmode **alt, u16 svid)
 282 {
 283         u8 mode = 1;
 284         int i;
 285
 286         for (i = 0; alt[i]; i++) {
 287                 if (i > MODE_DISCOVERY_MAX)
 288                         return -ERANGE;
 289
 290                 if (alt[i]->svid == svid)
 291                         mode++;
 292         }
 293
 294         return mode;
 295 }
 296
 297 static int ucsi_next_altmode(struct typec_altmode **alt)
 298 {
 299         int i = 0;
 300
 301         for (i = 0; i < UCSI_MAX_ALTMODES; i++)
 302                 if (!alt[i])
 303                         return i;
 304
 305         return -ENOENT;
 306 }
 307
 308 static int ucsi_get_num_altmode(struct typec_altmode **alt)
 309 {
 310         int i;
 311
 312         for (i = 0; i < UCSI_MAX_ALTMODES; i++)
 313                 if (!alt[i])
 314                         break;
 315
 316         return i;
 317 }
 318
 319 static int ucsi_register_altmode(struct ucsi_connector *con,
 320                                  struct typec_altmode_desc *desc,
 321                                  u8 recipient)
 322 {
 323         struct typec_altmode *alt;
 324         bool override;
 325         int ret;
 326         int i;
 327
 328         override = !!(con->ucsi->cap.features & UCSI_CAP_ALT_MODE_OVERRIDE);
 329
 330         switch (recipient) {
 331         case UCSI_RECIPIENT_CON:
 332                 i = ucsi_next_altmode(con->port_altmode);
 333                 if (i < 0) {
 334                         ret = i;
 335                         goto err;
 336                 }
 337
 338                 ret = ucsi_altmode_next_mode(con->port_altmode, desc->svid);
 339                 if (ret < 0)
 340                         return ret;
 341
 342                 desc->mode = ret;
 343
 344                 switch (desc->svid) {
 345                 case USB_TYPEC_DP_SID:
 346                         alt = ucsi_register_displayport(con, override, i, desc);
 347                         break;
 348                 case USB_TYPEC_NVIDIA_VLINK_SID:
 349                         if (desc->vdo == USB_TYPEC_NVIDIA_VLINK_DBG_VDO)
 350                                 alt = typec_port_register_altmode(con->port,
 351                                                                   desc);
 352                         else
 353                                 alt = ucsi_register_displayport(con, override,
 354                                                                 i, desc);
 355                         break;
 356                 default:
 357                         alt = typec_port_register_altmode(con->port, desc);
 358                         break;
 359                 }
 360
 361                 if (IS_ERR(alt)) {
 362                         ret = PTR_ERR(alt);
 363                         goto err;
 364                 }
 365
 366                 con->port_altmode[i] = alt;
 367                 break;
 368         case UCSI_RECIPIENT_SOP:
 369                 i = ucsi_next_altmode(con->partner_altmode);
 370                 if (i < 0) {
 371                         ret = i;
 372                         goto err;
 373                 }
 374
 375                 ret = ucsi_altmode_next_mode(con->partner_altmode, desc->svid);
 376                 if (ret < 0)
 377                         return ret;
 378
 379                 desc->mode = ret;
 380
 381                 alt = typec_partner_register_altmode(con->partner, desc);
 382                 if (IS_ERR(alt)) {
 383                         ret = PTR_ERR(alt);
 384                         goto err;
 385                 }
 386
 387                 con->partner_altmode[i] = alt;
 388                 break;
 389         default:
 390                 return -EINVAL;
 391         }
 392
 393         trace_ucsi_register_altmode(recipient, alt);
 394
 395         return 0;
 396
 397 err:
 398         dev_err(con->ucsi->dev, "failed to registers svid 0x%04x mode %d\n",
 399                 desc->svid, desc->mode);
 400
 401         return ret;
 402 }
 403
 404 static int
 405 ucsi_register_altmodes_nvidia(struct ucsi_connector *con, u8 recipient)
 406 {
 407         int max_altmodes = UCSI_MAX_ALTMODES;
 408         struct typec_altmode_desc desc;
 409         struct ucsi_altmode alt;
 410         struct ucsi_altmode orig[UCSI_MAX_ALTMODES];
 411         struct ucsi_altmode updated[UCSI_MAX_ALTMODES];
 412         struct ucsi *ucsi = con->ucsi;
 413         bool multi_dp = false;
 414         u64 command;
 415         int ret;
 416         int len;
 417         int i;
 418         int k = 0;
 419
 420         if (recipient == UCSI_RECIPIENT_CON)
 421                 max_altmodes = con->ucsi->cap.num_alt_modes;
 422
 423         memset(orig, 0, sizeof(orig));
 424         memset(updated, 0, sizeof(updated));
 425
 426         /* First get all the alternate modes */
 427         for (i = 0; i < max_altmodes; i++) {
 428                 memset(&alt, 0, sizeof(alt));
 429                 command = UCSI_GET_ALTERNATE_MODES;
 430                 command |= UCSI_GET_ALTMODE_RECIPIENT(recipient);
 431                 command |= UCSI_GET_ALTMODE_CONNECTOR_NUMBER(con->num);
 432                 command |= UCSI_GET_ALTMODE_OFFSET(i);
 433                 len = ucsi_send_command(con->ucsi, command, &alt, sizeof(alt));
 434                 /*
 435                  * We are collecting all altmodes first and then registering.
 436                  * Some type-C device will return zero length data beyond last
 437                  * alternate modes. We should not return if length is zero.
 438                  */
 439                 if (len < 0)
 440                         return len;
 441
 442                 /* We got all altmodes, now break out and register them */
 443                 if (!len || !alt.svid)
 444                         break;
 445
 446                 orig[k].mid = alt.mid;
 447                 orig[k].svid = alt.svid;
 448                 k++;
 449         }
 450         /*
 451          * Update the original altmode table as some ppms may report
 452          * multiple DP altmodes.
 453          */
 454         if (recipient == UCSI_RECIPIENT_CON)
 455                 multi_dp = ucsi->ops->update_altmodes(ucsi, orig, updated);
 456
 457         /* now register altmodes */
 458         for (i = 0; i < max_altmodes; i++) {
 459                 memset(&desc, 0, sizeof(desc));
 460                 if (multi_dp && recipient == UCSI_RECIPIENT_CON) {
 461                         desc.svid = updated[i].svid;
 462                         desc.vdo = updated[i].mid;
 463                 } else {
 464                         desc.svid = orig[i].svid;
 465                         desc.vdo = orig[i].mid;
 466                 }
 467                 desc.roles = TYPEC_PORT_DRD;
 468
 469                 if (!desc.svid)
 470                         return 0;
 471
 472                 ret = ucsi_register_altmode(con, &desc, recipient);
 473                 if (ret)
 474                         return ret;
 475         }
 476
 477         return 0;
 478 }
 479
 480 static int ucsi_register_altmodes(struct ucsi_connector *con, u8 recipient)
 481 {
 482         int max_altmodes = UCSI_MAX_ALTMODES;
 483         struct typec_altmode_desc desc;
 484         struct ucsi_altmode alt[2];
 485         u64 command;
 486         int num;
 487         int ret;
 488         int len;
 489         int j;
 490         int i;
 491
 492         if (!(con->ucsi->cap.features & UCSI_CAP_ALT_MODE_DETAILS))
 493                 return 0;
 494
 495         if (recipient == UCSI_RECIPIENT_SOP && con->partner_altmode[0])
 496                 return 0;
 497
 498         if (con->ucsi->ops->update_altmodes)
 499                 return ucsi_register_altmodes_nvidia(con, recipient);
 500
 501         if (recipient == UCSI_RECIPIENT_CON)
 502                 max_altmodes = con->ucsi->cap.num_alt_modes;
 503
 504         for (i = 0; i < max_altmodes;) {
 505                 memset(alt, 0, sizeof(alt));
 506                 command = UCSI_GET_ALTERNATE_MODES;
 507                 command |= UCSI_GET_ALTMODE_RECIPIENT(recipient);
 508                 command |= UCSI_GET_ALTMODE_CONNECTOR_NUMBER(con->num);
 509                 command |= UCSI_GET_ALTMODE_OFFSET(i);
 510                 len = ucsi_send_command(con->ucsi, command, alt, sizeof(alt));
 511                 if (len == -EBUSY)
 512                         continue;
 513                 if (len <= 0)
 514                         return len;
 515
 516                 /*
 517                  * This code is requesting one alt mode at a time, but some PPMs
 518                  * may still return two. If that happens both alt modes need be
 519                  * registered and the offset for the next alt mode has to be
 520                  * incremented.
 521                  */
 522                 num = len / sizeof(alt[0]);
 523                 i += num;
 524
 525                 for (j = 0; j < num; j++) {
 526                         if (!alt[j].svid)
 527                                 return 0;
 528
 529                         memset(&desc, 0, sizeof(desc));
 530                         desc.vdo = alt[j].mid;
 531                         desc.svid = alt[j].svid;
 532                         desc.roles = TYPEC_PORT_DRD;
 533
 534                         ret = ucsi_register_altmode(con, &desc, recipient);
 535                         if (ret)
 536                                 return ret;
 537                 }
 538         }
 539
 540         return 0;
 541 }
 542
 543 static void ucsi_unregister_altmodes(struct ucsi_connector *con, u8 recipient)
 544 {
 545         const struct typec_altmode *pdev;
 546         struct typec_altmode **adev;
 547         int i = 0;
 548
 549         switch (recipient) {
 550         case UCSI_RECIPIENT_CON:
 551                 adev = con->port_altmode;
 552                 break;
 553         case UCSI_RECIPIENT_SOP:
 554                 adev = con->partner_altmode;
 555                 break;
 556         default:
 557                 return;
 558         }
 559
 560         while (adev[i]) {
 561                 if (recipient == UCSI_RECIPIENT_SOP &&
 562                     (adev[i]->svid == USB_TYPEC_DP_SID ||
 563                         (adev[i]->svid == USB_TYPEC_NVIDIA_VLINK_SID &&
 564                         adev[i]->vdo != USB_TYPEC_NVIDIA_VLINK_DBG_VDO))) {
 565                         pdev = typec_altmode_get_partner(adev[i]);
 566                         ucsi_displayport_remove_partner((void *)pdev);
 567                 }
 568                 typec_unregister_altmode(adev[i]);
 569                 adev[i++] = NULL;
 570         }
 571 }
 572
 573 static int ucsi_read_pdos(struct ucsi_connector *con,
 574                           enum typec_role role, int is_partner,
 575                           u32 *pdos, int offset, int num_pdos)
 576 {
 577         struct ucsi *ucsi = con->ucsi;
 578         u64 command;
 579         int ret;
 580
 581         command = UCSI_COMMAND(UCSI_GET_PDOS) | UCSI_CONNECTOR_NUMBER(con->num);
 582         command |= UCSI_GET_PDOS_PARTNER_PDO(is_partner);
 583         command |= UCSI_GET_PDOS_PDO_OFFSET(offset);
 584         command |= UCSI_GET_PDOS_NUM_PDOS(num_pdos - 1);
 585         command |= is_source(role) ? UCSI_GET_PDOS_SRC_PDOS : 0;
 586         ret = ucsi_send_command(ucsi, command, pdos + offset,
 587                                 num_pdos * sizeof(u32));
 588         if (ret < 0 && ret != -ETIMEDOUT)
 589                 dev_err(ucsi->dev, "UCSI_GET_PDOS failed (%d)\n", ret);
 590
 591         return ret;
 592 }
 593
 594 static int ucsi_get_pdos(struct ucsi_connector *con, enum typec_role role,
 595                          int is_partner, u32 *pdos)
 596 {
 597         u8 num_pdos;
 598         int ret;
 599
 600         /* UCSI max payload means only getting at most 4 PDOs at a time */
 601         ret = ucsi_read_pdos(con, role, is_partner, pdos, 0, UCSI_MAX_PDOS);
 602         if (ret < 0)
 603                 return ret;
 604
 605         num_pdos = ret / sizeof(u32); /* number of bytes to 32-bit PDOs */
 606         if (num_pdos < UCSI_MAX_PDOS)
 607                 return num_pdos;
 608
 609         /* get the remaining PDOs, if any */
 610         ret = ucsi_read_pdos(con, role, is_partner, pdos, UCSI_MAX_PDOS,
 611                              PDO_MAX_OBJECTS - UCSI_MAX_PDOS);
 612         if (ret < 0)
 613                 return ret;
 614
 615         return ret / sizeof(u32) + num_pdos;
 616 }
 617
 618 static int ucsi_get_src_pdos(struct ucsi_connector *con)
 619 {
 620         int ret;
 621
 622         ret = ucsi_get_pdos(con, TYPEC_SOURCE, 1, con->src_pdos);
 623         if (ret < 0)
 624                 return ret;
 625
 626         con->num_pdos = ret;
 627
 628         ucsi_port_psy_changed(con);
 629
 630         return ret;
 631 }
 632
 633 static int ucsi_check_altmodes(struct ucsi_connector *con)
 634 {
 635         int ret, num_partner_am;
 636
 637         ret = ucsi_register_altmodes(con, UCSI_RECIPIENT_SOP);
 638         if (ret && ret != -ETIMEDOUT)
 639                 dev_err(con->ucsi->dev,
 640                         "con%d: failed to register partner alt modes (%d)\n",
 641                         con->num, ret);
 642
 643         /* Ignoring the errors in this case. */
 644         if (con->partner_altmode[0]) {
 645                 num_partner_am = ucsi_get_num_altmode(con->partner_altmode);
 646                 if (num_partner_am > 0)
 647                         typec_partner_set_num_altmodes(con->partner, num_partner_am);
 648                 ucsi_altmode_update_active(con);
 649                 return 0;
 650         }
 651
 652         return ret;
 653 }
 654
 655 static int ucsi_register_partner_pdos(struct ucsi_connector *con)
 656 {
 657         struct usb_power_delivery_desc desc = { con->ucsi->cap.pd_version };
 658         struct usb_power_delivery_capabilities_desc caps;
 659         struct usb_power_delivery_capabilities *cap;
 660         int ret;
 661
 662         if (con->partner_pd)
 663                 return 0;
 664
 665         con->partner_pd = usb_power_delivery_register(NULL, &desc);
 666         if (IS_ERR(con->partner_pd))
 667                 return PTR_ERR(con->partner_pd);
 668
 669         ret = ucsi_get_pdos(con, TYPEC_SOURCE, 1, caps.pdo);
 670         if (ret > 0) {
 671                 if (ret < PDO_MAX_OBJECTS)
 672                         caps.pdo[ret] = 0;
 673
 674                 caps.role = TYPEC_SOURCE;
 675                 cap = usb_power_delivery_register_capabilities(con->partner_pd, &caps);
 676                 if (IS_ERR(cap))
 677                         return PTR_ERR(cap);
 678
 679                 con->partner_source_caps = cap;
 680
 681                 ret = typec_partner_set_usb_power_delivery(con->partner, con->partner_pd);
 682                 if (ret) {
 683                         usb_power_delivery_unregister_capabilities(con->partner_source_caps);
 684                         return ret;
 685                 }
 686         }
 687
 688         ret = ucsi_get_pdos(con, TYPEC_SINK, 1, caps.pdo);
 689         if (ret > 0) {
 690                 if (ret < PDO_MAX_OBJECTS)
 691                         caps.pdo[ret] = 0;
 692
 693                 caps.role = TYPEC_SINK;
 694
 695                 cap = usb_power_delivery_register_capabilities(con->partner_pd, &caps);
 696                 if (IS_ERR(cap))
 697                         return PTR_ERR(cap);
 698
 699                 con->partner_sink_caps = cap;
 700
 701                 ret = typec_partner_set_usb_power_delivery(con->partner, con->partner_pd);
 702                 if (ret) {
 703                         usb_power_delivery_unregister_capabilities(con->partner_sink_caps);
 704                         return ret;
 705                 }
 706         }
 707
 708         return 0;
 709 }
 710
 711 static void ucsi_unregister_partner_pdos(struct ucsi_connector *con)
 712 {
 713         usb_power_delivery_unregister_capabilities(con->partner_sink_caps);
 714         con->partner_sink_caps = NULL;
 715         usb_power_delivery_unregister_capabilities(con->partner_source_caps);
 716         con->partner_source_caps = NULL;
 717         usb_power_delivery_unregister(con->partner_pd);
 718         con->partner_pd = NULL;
 719 }
 720
 721 static void ucsi_pwr_opmode_change(struct ucsi_connector *con)
 722 {
 723         switch (UCSI_CONSTAT_PWR_OPMODE(con->status.flags)) {
 724         case UCSI_CONSTAT_PWR_OPMODE_PD:
 725                 con->rdo = con->status.request_data_obj;
 726                 typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_PD);
 727                 ucsi_partner_task(con, ucsi_get_src_pdos, 30, 0);
 728                 ucsi_partner_task(con, ucsi_check_altmodes, 30, 0);
 729                 ucsi_partner_task(con, ucsi_register_partner_pdos, 1, HZ);
 730                 break;
 731         case UCSI_CONSTAT_PWR_OPMODE_TYPEC1_5:
 732                 con->rdo = 0;
 733                 typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_1_5A);
 734                 break;
 735         case UCSI_CONSTAT_PWR_OPMODE_TYPEC3_0:
 736                 con->rdo = 0;
 737                 typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_3_0A);
 738                 break;
 739         default:
 740                 con->rdo = 0;
 741                 typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_USB);
 742                 break;
 743         }
 744 }
 745
 746 static int ucsi_register_partner(struct ucsi_connector *con)
 747 {
 748         u8 pwr_opmode = UCSI_CONSTAT_PWR_OPMODE(con->status.flags);
 749         struct typec_partner_desc desc;
 750         struct typec_partner *partner;
 751
 752         if (con->partner)
 753                 return 0;
 754
 755         memset(&desc, 0, sizeof(desc));
 756
 757         switch (UCSI_CONSTAT_PARTNER_TYPE(con->status.flags)) {
 758         case UCSI_CONSTAT_PARTNER_TYPE_DEBUG:
 759                 desc.accessory = TYPEC_ACCESSORY_DEBUG;
 760                 break;
 761         case UCSI_CONSTAT_PARTNER_TYPE_AUDIO:
 762                 desc.accessory = TYPEC_ACCESSORY_AUDIO;
 763                 break;
 764         default:
 765                 break;
 766         }
 767
 768         desc.usb_pd = pwr_opmode == UCSI_CONSTAT_PWR_OPMODE_PD;
 769
 770         partner = typec_register_partner(con->port, &desc);
 771         if (IS_ERR(partner)) {
 772                 dev_err(con->ucsi->dev,
 773                         "con%d: failed to register partner (%ld)\n", con->num,
 774                         PTR_ERR(partner));
 775                 return PTR_ERR(partner);
 776         }
 777
 778         con->partner = partner;
 779
 780         return 0;
 781 }
 782
 783 static void ucsi_unregister_partner(struct ucsi_connector *con)
 784 {
 785         if (!con->partner)
 786                 return;
 787
 788         typec_set_mode(con->port, TYPEC_STATE_SAFE);
 789
 790         typec_partner_set_usb_power_delivery(con->partner, NULL);
 791         ucsi_unregister_partner_pdos(con);
 792         ucsi_unregister_altmodes(con, UCSI_RECIPIENT_SOP);
 793         typec_unregister_partner(con->partner);
 794         con->partner = NULL;
 795 }
 796
 797 static void ucsi_partner_change(struct ucsi_connector *con)
 798 {
 799         enum usb_role u_role = USB_ROLE_NONE;
 800         int ret;
 801
 802         switch (UCSI_CONSTAT_PARTNER_TYPE(con->status.flags)) {
 803         case UCSI_CONSTAT_PARTNER_TYPE_UFP:
 804         case UCSI_CONSTAT_PARTNER_TYPE_CABLE_AND_UFP:
 805                 u_role = USB_ROLE_HOST;
 806                 fallthrough;
 807         case UCSI_CONSTAT_PARTNER_TYPE_CABLE:
 808                 typec_set_data_role(con->port, TYPEC_HOST);
 809                 break;
 810         case UCSI_CONSTAT_PARTNER_TYPE_DFP:
 811                 u_role = USB_ROLE_DEVICE;
 812                 typec_set_data_role(con->port, TYPEC_DEVICE);
 813                 break;
 814         default:
 815                 break;
 816         }
 817
 818         if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
 819                 switch (UCSI_CONSTAT_PARTNER_TYPE(con->status.flags)) {
 820                 case UCSI_CONSTAT_PARTNER_TYPE_DEBUG:
 821                         typec_set_mode(con->port, TYPEC_MODE_DEBUG);
 822                         break;
 823                 case UCSI_CONSTAT_PARTNER_TYPE_AUDIO:
 824                         typec_set_mode(con->port, TYPEC_MODE_AUDIO);
 825                         break;
 826                 default:
 827                         if (UCSI_CONSTAT_PARTNER_FLAGS(con->status.flags) ==
 828                                         UCSI_CONSTAT_PARTNER_FLAG_USB)
 829                                 typec_set_mode(con->port, TYPEC_STATE_USB);
 830                 }
 831         }
 832
 833         /* Only notify USB controller if partner supports USB data */
 834         if (!(UCSI_CONSTAT_PARTNER_FLAGS(con->status.flags) & UCSI_CONSTAT_PARTNER_FLAG_USB))
 835                 u_role = USB_ROLE_NONE;
 836
 837         ret = usb_role_switch_set_role(con->usb_role_sw, u_role);
 838         if (ret)
 839                 dev_err(con->ucsi->dev, "con:%d: failed to set usb role:%d\n",
 840                         con->num, u_role);
 841 }
 842
 843 static int ucsi_check_connection(struct ucsi_connector *con)
 844 {
 845         u8 prev_flags = con->status.flags;
 846         u64 command;
 847         int ret;
 848
 849         command = UCSI_GET_CONNECTOR_STATUS | UCSI_CONNECTOR_NUMBER(con->num);
 850         ret = ucsi_send_command(con->ucsi, command, &con->status, sizeof(con->status));
 851         if (ret < 0) {
 852                 dev_err(con->ucsi->dev, "GET_CONNECTOR_STATUS failed (%d)\n", ret);
 853                 return ret;
 854         }
 855
 856         if (con->status.flags == prev_flags)
 857                 return 0;
 858
 859         if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
 860                 ucsi_register_partner(con);
 861                 ucsi_pwr_opmode_change(con);
 862                 ucsi_partner_change(con);
 863         } else {
 864                 ucsi_partner_change(con);
 865                 ucsi_port_psy_changed(con);
 866                 ucsi_unregister_partner(con);
 867         }
 868
 869         return 0;
 870 }
 871
 872 static void ucsi_handle_connector_change(struct work_struct *work)
 873 {
 874         struct ucsi_connector *con = container_of(work, struct ucsi_connector,
 875                                                   work);
 876         struct ucsi *ucsi = con->ucsi;
 877         enum typec_role role;
 878         u64 command;
 879         int ret;
 880
 881         mutex_lock(&con->lock);
 882
 883         command = UCSI_GET_CONNECTOR_STATUS | UCSI_CONNECTOR_NUMBER(con->num);
 884         ret = ucsi_send_command(ucsi, command, &con->status, sizeof(con->status));
 885         if (ret < 0) {
 886                 dev_err(ucsi->dev, "%s: GET_CONNECTOR_STATUS failed (%d)\n",
 887                         __func__, ret);
 888                 clear_bit(EVENT_PENDING, &con->ucsi->flags);
 889                 goto out_unlock;
 890         }
 891
 892         trace_ucsi_connector_change(con->num, &con->status);
 893
 894         role = !!(con->status.flags & UCSI_CONSTAT_PWR_DIR);
 895
 896         if (con->status.change & UCSI_CONSTAT_POWER_DIR_CHANGE) {
 897                 typec_set_pwr_role(con->port, role);
 898
 899                 /* Complete pending power role swap */
 900                 if (!completion_done(&con->complete))
 901                         complete(&con->complete);
 902         }
 903
 904         if (con->status.change & UCSI_CONSTAT_CONNECT_CHANGE) {
 905                 typec_set_pwr_role(con->port, role);
 906                 ucsi_port_psy_changed(con);
 907                 ucsi_partner_change(con);
 908
 909                 if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
 910                         ucsi_register_partner(con);
 911                         ucsi_partner_task(con, ucsi_check_connection, 1, HZ);
 912
 913                         if (UCSI_CONSTAT_PWR_OPMODE(con->status.flags) ==
 914                             UCSI_CONSTAT_PWR_OPMODE_PD)
 915                                 ucsi_partner_task(con, ucsi_register_partner_pdos, 1, HZ);
 916                 } else {
 917                         ucsi_unregister_partner(con);
 918                 }
 919         }
 920
 921         if (con->status.change & UCSI_CONSTAT_POWER_OPMODE_CHANGE ||
 922             con->status.change & UCSI_CONSTAT_POWER_LEVEL_CHANGE)
 923                 ucsi_pwr_opmode_change(con);
 924
 925         if (con->partner && con->status.change & UCSI_CONSTAT_PARTNER_CHANGE) {
 926                 ucsi_partner_change(con);
 927
 928                 /* Complete pending data role swap */
 929                 if (!completion_done(&con->complete))
 930                         complete(&con->complete);
 931         }
 932
 933         if (con->status.change & UCSI_CONSTAT_CAM_CHANGE)
 934                 ucsi_partner_task(con, ucsi_check_altmodes, 1, 0);
 935
 936         clear_bit(EVENT_PENDING, &con->ucsi->flags);
 937
 938         ret = ucsi_acknowledge_connector_change(ucsi);
 939         if (ret)
 940                 dev_err(ucsi->dev, "%s: ACK failed (%d)", __func__, ret);
 941
 942 out_unlock:
 943         mutex_unlock(&con->lock);
 944 }
 945
 946 /**
 947  * ucsi_connector_change - Process Connector Change Event
 948  * @ucsi: UCSI Interface
 949  * @num: Connector number
 950  */
 951 void ucsi_connector_change(struct ucsi *ucsi, u8 num)
 952 {
 953         struct ucsi_connector *con = &ucsi->connector[num - 1];
 954
 955         if (!(ucsi->ntfy & UCSI_ENABLE_NTFY_CONNECTOR_CHANGE)) {
 956                 dev_dbg(ucsi->dev, "Bogus connector change event\n");
 957                 return;
 958         }
 959
 960         if (!test_and_set_bit(EVENT_PENDING, &ucsi->flags))
 961                 schedule_work(&con->work);
 962 }
 963 EXPORT_SYMBOL_GPL(ucsi_connector_change);
 964
 965 /* -------------------------------------------------------------------------- */
 966
 967 static int ucsi_reset_connector(struct ucsi_connector *con, bool hard)
 968 {
 969         u64 command;
 970
 971         command = UCSI_CONNECTOR_RESET | UCSI_CONNECTOR_NUMBER(con->num);
 972         command |= hard ? UCSI_CONNECTOR_RESET_HARD : 0;
 973
 974         return ucsi_send_command(con->ucsi, command, NULL, 0);
 975 }
 976
 977 static int ucsi_reset_ppm(struct ucsi *ucsi)
 978 {
 979         u64 command = UCSI_PPM_RESET;
 980         unsigned long tmo;
 981         u32 cci;
 982         int ret;
 983
 984         mutex_lock(&ucsi->ppm_lock);
 985
 986         ret = ucsi->ops->async_write(ucsi, UCSI_CONTROL, &command,
 987                                      sizeof(command));
 988         if (ret < 0)
 989                 goto out;
 990
 991         tmo = jiffies + msecs_to_jiffies(UCSI_TIMEOUT_MS);
 992
 993         do {
 994                 if (time_is_before_jiffies(tmo)) {
 995                         ret = -ETIMEDOUT;
 996                         goto out;
 997                 }
 998
 999                 ret = ucsi->ops->read(ucsi, UCSI_CCI, &cci, sizeof(cci));
1000                 if (ret)
1001                         goto out;
1002
1003                 /* If the PPM is still doing something else, reset it again. */
1004                 if (cci & ~UCSI_CCI_RESET_COMPLETE) {
1005                         ret = ucsi->ops->async_write(ucsi, UCSI_CONTROL,
1006                                                      &command,
1007                                                      sizeof(command));
1008                         if (ret < 0)
1009                                 goto out;
1010                 }
1011
1012                 msleep(20);
1013         } while (!(cci & UCSI_CCI_RESET_COMPLETE));
1014
1015 out:
1016         mutex_unlock(&ucsi->ppm_lock);
1017         return ret;
1018 }
1019
1020 static int ucsi_role_cmd(struct ucsi_connector *con, u64 command)
1021 {
1022         int ret;
1023
1024         ret = ucsi_send_command(con->ucsi, command, NULL, 0);
1025         if (ret == -ETIMEDOUT) {
1026                 u64 c;
1027
1028                 /* PPM most likely stopped responding. Resetting everything. */
1029                 ucsi_reset_ppm(con->ucsi);
1030
1031                 c = UCSI_SET_NOTIFICATION_ENABLE | con->ucsi->ntfy;
1032                 ucsi_send_command(con->ucsi, c, NULL, 0);
1033
1034                 ucsi_reset_connector(con, true);
1035         }
1036
1037         return ret;
1038 }
1039
1040 static int ucsi_dr_swap(struct typec_port *port, enum typec_data_role role)
1041 {
1042         struct ucsi_connector *con = typec_get_drvdata(port);
1043         u8 partner_type;
1044         u64 command;
1045         int ret = 0;
1046
1047         mutex_lock(&con->lock);
1048
1049         if (!con->partner) {
1050                 ret = -ENOTCONN;
1051                 goto out_unlock;
1052         }
1053
1054         partner_type = UCSI_CONSTAT_PARTNER_TYPE(con->status.flags);
1055         if ((partner_type == UCSI_CONSTAT_PARTNER_TYPE_DFP &&
1056              role == TYPEC_DEVICE) ||
1057             (partner_type == UCSI_CONSTAT_PARTNER_TYPE_UFP &&
1058              role == TYPEC_HOST))
1059                 goto out_unlock;
1060
1061         reinit_completion(&con->complete);
1062
1063         command = UCSI_SET_UOR | UCSI_CONNECTOR_NUMBER(con->num);
1064         command |= UCSI_SET_UOR_ROLE(role);
1065         command |= UCSI_SET_UOR_ACCEPT_ROLE_SWAPS;
1066         ret = ucsi_role_cmd(con, command);
1067         if (ret < 0)
1068                 goto out_unlock;
1069
1070         mutex_unlock(&con->lock);
1071
1072         if (!wait_for_completion_timeout(&con->complete,
1073                                          msecs_to_jiffies(UCSI_SWAP_TIMEOUT_MS)))
1074                 return -ETIMEDOUT;
1075
1076         return 0;
1077
1078 out_unlock:
1079         mutex_unlock(&con->lock);
1080
1081         return ret;
1082 }
1083
1084 static int ucsi_pr_swap(struct typec_port *port, enum typec_role role)
1085 {
1086         struct ucsi_connector *con = typec_get_drvdata(port);
1087         enum typec_role cur_role;
1088         u64 command;
1089         int ret = 0;
1090
1091         mutex_lock(&con->lock);
1092
1093         if (!con->partner) {
1094                 ret = -ENOTCONN;
1095                 goto out_unlock;
1096         }
1097
1098         cur_role = !!(con->status.flags & UCSI_CONSTAT_PWR_DIR);
1099
1100         if (cur_role == role)
1101                 goto out_unlock;
1102
1103         reinit_completion(&con->complete);
1104
1105         command = UCSI_SET_PDR | UCSI_CONNECTOR_NUMBER(con->num);
1106         command |= UCSI_SET_PDR_ROLE(role);
1107         command |= UCSI_SET_PDR_ACCEPT_ROLE_SWAPS;
1108         ret = ucsi_role_cmd(con, command);
1109         if (ret < 0)
1110                 goto out_unlock;
1111
1112         mutex_unlock(&con->lock);
1113
1114         if (!wait_for_completion_timeout(&con->complete,
1115                                          msecs_to_jiffies(UCSI_SWAP_TIMEOUT_MS)))
1116                 return -ETIMEDOUT;
1117
1118         mutex_lock(&con->lock);
1119
1120         /* Something has gone wrong while swapping the role */
1121         if (UCSI_CONSTAT_PWR_OPMODE(con->status.flags) !=
1122             UCSI_CONSTAT_PWR_OPMODE_PD) {
1123                 ucsi_reset_connector(con, true);
1124                 ret = -EPROTO;
1125         }
1126
1127 out_unlock:
1128         mutex_unlock(&con->lock);
1129
1130         return ret;
1131 }
1132
1133 static const struct typec_operations ucsi_ops = {
1134         .dr_set = ucsi_dr_swap,
1135         .pr_set = ucsi_pr_swap
1136 };
1137
1138 /* Caller must call fwnode_handle_put() after use */
1139 static struct fwnode_handle *ucsi_find_fwnode(struct ucsi_connector *con)
1140 {
1141         struct fwnode_handle *fwnode;
1142         int i = 1;
1143
1144         device_for_each_child_node(con->ucsi->dev, fwnode)
1145                 if (i++ == con->num)
1146                         return fwnode;
1147         return NULL;
1148 }
1149
1150 static int ucsi_register_port(struct ucsi *ucsi, struct ucsi_connector *con)
1151 {
1152         struct usb_power_delivery_desc desc = { ucsi->cap.pd_version};
1153         struct usb_power_delivery_capabilities_desc pd_caps;
1154         struct usb_power_delivery_capabilities *pd_cap;
1155         struct typec_capability *cap = &con->typec_cap;
1156         enum typec_accessory *accessory = cap->accessory;
1157         enum usb_role u_role = USB_ROLE_NONE;
1158         u64 command;
1159         char *name;
1160         int ret;
1161
1162         name = kasprintf(GFP_KERNEL, "%s-con%d", dev_name(ucsi->dev), con->num);
1163         if (!name)
1164                 return -ENOMEM;
1165
1166         con->wq = create_singlethread_workqueue(name);
1167         kfree(name);
1168         if (!con->wq)
1169                 return -ENOMEM;
1170
1171         INIT_WORK(&con->work, ucsi_handle_connector_change);
1172         init_completion(&con->complete);
1173         mutex_init(&con->lock);
1174         INIT_LIST_HEAD(&con->partner_tasks);
1175         con->ucsi = ucsi;
1176
1177         cap->fwnode = ucsi_find_fwnode(con);
1178         con->usb_role_sw = fwnode_usb_role_switch_get(cap->fwnode);
1179         if (IS_ERR(con->usb_role_sw))
1180                 return dev_err_probe(ucsi->dev, PTR_ERR(con->usb_role_sw),
1181                         "con%d: failed to get usb role switch\n", con->num);
1182
1183         /* Delay other interactions with the con until registration is complete */
1184         mutex_lock(&con->lock);
1185
1186         /* Get connector capability */
1187         command = UCSI_GET_CONNECTOR_CAPABILITY;
1188         command |= UCSI_CONNECTOR_NUMBER(con->num);
1189         ret = ucsi_send_command(ucsi, command, &con->cap, sizeof(con->cap));
1190         if (ret < 0)
1191                 goto out_unlock;
1192
1193         if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DRP)
1194                 cap->data = TYPEC_PORT_DRD;
1195         else if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DFP)
1196                 cap->data = TYPEC_PORT_DFP;
1197         else if (con->cap.op_mode & UCSI_CONCAP_OPMODE_UFP)
1198                 cap->data = TYPEC_PORT_UFP;
1199
1200         if ((con->cap.flags & UCSI_CONCAP_FLAG_PROVIDER) &&
1201             (con->cap.flags & UCSI_CONCAP_FLAG_CONSUMER))
1202                 cap->type = TYPEC_PORT_DRP;
1203         else if (con->cap.flags & UCSI_CONCAP_FLAG_PROVIDER)
1204                 cap->type = TYPEC_PORT_SRC;
1205         else if (con->cap.flags & UCSI_CONCAP_FLAG_CONSUMER)
1206                 cap->type = TYPEC_PORT_SNK;
1207
1208         cap->revision = ucsi->cap.typec_version;
1209         cap->pd_revision = ucsi->cap.pd_version;
1210         cap->svdm_version = SVDM_VER_2_0;
1211         cap->prefer_role = TYPEC_NO_PREFERRED_ROLE;
1212
1213         if (con->cap.op_mode & UCSI_CONCAP_OPMODE_AUDIO_ACCESSORY)
1214                 *accessory++ = TYPEC_ACCESSORY_AUDIO;
1215         if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DEBUG_ACCESSORY)
1216                 *accessory = TYPEC_ACCESSORY_DEBUG;
1217
1218         cap->driver_data = con;
1219         cap->ops = &ucsi_ops;
1220
1221         ret = ucsi_register_port_psy(con);
1222         if (ret)
1223                 goto out;
1224
1225         /* Register the connector */
1226         con->port = typec_register_port(ucsi->dev, cap);
1227         if (IS_ERR(con->port)) {
1228                 ret = PTR_ERR(con->port);
1229                 goto out;
1230         }
1231
1232         con->pd = usb_power_delivery_register(ucsi->dev, &desc);
1233
1234         ret = ucsi_get_pdos(con, TYPEC_SOURCE, 0, pd_caps.pdo);
1235         if (ret > 0) {
1236                 if (ret < PDO_MAX_OBJECTS)
1237                         pd_caps.pdo[ret] = 0;
1238
1239                 pd_caps.role = TYPEC_SOURCE;
1240                 pd_cap = usb_power_delivery_register_capabilities(con->pd, &pd_caps);
1241                 if (IS_ERR(pd_cap)) {
1242                         ret = PTR_ERR(pd_cap);
1243                         goto out;
1244                 }
1245
1246                 con->port_source_caps = pd_cap;
1247                 typec_port_set_usb_power_delivery(con->port, con->pd);
1248         }
1249
1250         memset(&pd_caps, 0, sizeof(pd_caps));
1251         ret = ucsi_get_pdos(con, TYPEC_SINK, 0, pd_caps.pdo);
1252         if (ret > 0) {
1253                 if (ret < PDO_MAX_OBJECTS)
1254                         pd_caps.pdo[ret] = 0;
1255
1256                 pd_caps.role = TYPEC_SINK;
1257                 pd_cap = usb_power_delivery_register_capabilities(con->pd, &pd_caps);
1258                 if (IS_ERR(pd_cap)) {
1259                         ret = PTR_ERR(pd_cap);
1260                         goto out;
1261                 }
1262
1263                 con->port_sink_caps = pd_cap;
1264                 typec_port_set_usb_power_delivery(con->port, con->pd);
1265         }
1266
1267         /* Alternate modes */
1268         ret = ucsi_register_altmodes(con, UCSI_RECIPIENT_CON);
1269         if (ret) {
1270                 dev_err(ucsi->dev, "con%d: failed to register alt modes\n",
1271                         con->num);
1272                 goto out;
1273         }
1274
1275         /* Get the status */
1276         command = UCSI_GET_CONNECTOR_STATUS | UCSI_CONNECTOR_NUMBER(con->num);
1277         ret = ucsi_send_command(ucsi, command, &con->status, sizeof(con->status));
1278         if (ret < 0) {
1279                 dev_err(ucsi->dev, "con%d: failed to get status\n", con->num);
1280                 ret = 0;
1281                 goto out;
1282         }
1283         ret = 0; /* ucsi_send_command() returns length on success */
1284
1285         switch (UCSI_CONSTAT_PARTNER_TYPE(con->status.flags)) {
1286         case UCSI_CONSTAT_PARTNER_TYPE_UFP:
1287         case UCSI_CONSTAT_PARTNER_TYPE_CABLE_AND_UFP:
1288                 u_role = USB_ROLE_HOST;
1289                 fallthrough;
1290         case UCSI_CONSTAT_PARTNER_TYPE_CABLE:
1291                 typec_set_data_role(con->port, TYPEC_HOST);
1292                 break;
1293         case UCSI_CONSTAT_PARTNER_TYPE_DFP:
1294                 u_role = USB_ROLE_DEVICE;
1295                 typec_set_data_role(con->port, TYPEC_DEVICE);
1296                 break;
1297         default:
1298                 break;
1299         }
1300
1301         /* Check if there is already something connected */
1302         if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
1303                 typec_set_pwr_role(con->port,
1304                                   !!(con->status.flags & UCSI_CONSTAT_PWR_DIR));
1305                 ucsi_register_partner(con);
1306                 ucsi_pwr_opmode_change(con);
1307                 ucsi_port_psy_changed(con);
1308         }
1309
1310         /* Only notify USB controller if partner supports USB data */
1311         if (!(UCSI_CONSTAT_PARTNER_FLAGS(con->status.flags) & UCSI_CONSTAT_PARTNER_FLAG_USB))
1312                 u_role = USB_ROLE_NONE;
1313
1314         ret = usb_role_switch_set_role(con->usb_role_sw, u_role);
1315         if (ret) {
1316                 dev_err(ucsi->dev, "con:%d: failed to set usb role:%d\n",
1317                         con->num, u_role);
1318                 ret = 0;
1319         }
1320
1321         if (con->partner &&
1322             UCSI_CONSTAT_PWR_OPMODE(con->status.flags) ==
1323             UCSI_CONSTAT_PWR_OPMODE_PD) {
1324                 ucsi_get_src_pdos(con);
1325                 ucsi_check_altmodes(con);
1326         }
1327
1328         trace_ucsi_register_port(con->num, &con->status);
1329
1330 out:
1331         fwnode_handle_put(cap->fwnode);
1332 out_unlock:
1333         mutex_unlock(&con->lock);
1334
1335         if (ret && con->wq) {
1336                 destroy_workqueue(con->wq);
1337                 con->wq = NULL;
1338         }
1339
1340         return ret;
1341 }
1342
1343 /**
1344  * ucsi_init - Initialize UCSI interface
1345  * @ucsi: UCSI to be initialized
1346  *
1347  * Registers all ports @ucsi has and enables all notification events.
1348  */
1349 static int ucsi_init(struct ucsi *ucsi)
1350 {
1351         struct ucsi_connector *con, *connector;
1352         u64 command, ntfy;
1353         int ret;
1354         int i;
1355
1356         /* Reset the PPM */
1357         ret = ucsi_reset_ppm(ucsi);
1358         if (ret) {
1359                 dev_err(ucsi->dev, "failed to reset PPM!\n");
1360                 goto err;
1361         }
1362
1363         /* Enable basic notifications */
1364         ntfy = UCSI_ENABLE_NTFY_CMD_COMPLETE | UCSI_ENABLE_NTFY_ERROR;
1365         command = UCSI_SET_NOTIFICATION_ENABLE | ntfy;
1366         ret = ucsi_send_command(ucsi, command, NULL, 0);
1367         if (ret < 0)
1368                 goto err_reset;
1369
1370         /* Get PPM capabilities */
1371         command = UCSI_GET_CAPABILITY;
1372         ret = ucsi_send_command(ucsi, command, &ucsi->cap, sizeof(ucsi->cap));
1373         if (ret < 0)
1374                 goto err_reset;
1375
1376         if (!ucsi->cap.num_connectors) {
1377                 ret = -ENODEV;
1378                 goto err_reset;
1379         }
1380
1381         /* Allocate the connectors. Released in ucsi_unregister() */
1382         connector = kcalloc(ucsi->cap.num_connectors + 1, sizeof(*connector), GFP_KERNEL);
1383         if (!connector) {
1384                 ret = -ENOMEM;
1385                 goto err_reset;
1386         }
1387
1388         /* Register all connectors */
1389         for (i = 0; i < ucsi->cap.num_connectors; i++) {
1390                 connector[i].num = i + 1;
1391                 ret = ucsi_register_port(ucsi, &connector[i]);
1392                 if (ret)
1393                         goto err_unregister;
1394         }
1395
1396         /* Enable all notifications */
1397         ntfy = UCSI_ENABLE_NTFY_ALL;
1398         command = UCSI_SET_NOTIFICATION_ENABLE | ntfy;
1399         ret = ucsi_send_command(ucsi, command, NULL, 0);
1400         if (ret < 0)
1401                 goto err_unregister;
1402
1403         ucsi->connector = connector;
1404         ucsi->ntfy = ntfy;
1405         return 0;
1406
1407 err_unregister:
1408         for (con = connector; con->port; con++) {
1409                 ucsi_unregister_partner(con);
1410                 ucsi_unregister_altmodes(con, UCSI_RECIPIENT_CON);
1411                 ucsi_unregister_port_psy(con);
1412                 if (con->wq)
1413                         destroy_workqueue(con->wq);
1414
1415                 usb_power_delivery_unregister_capabilities(con->port_sink_caps);
1416                 con->port_sink_caps = NULL;
1417                 usb_power_delivery_unregister_capabilities(con->port_source_caps);
1418                 con->port_source_caps = NULL;
1419                 usb_power_delivery_unregister(con->pd);
1420                 con->pd = NULL;
1421                 typec_unregister_port(con->port);
1422                 con->port = NULL;
1423         }
1424         kfree(connector);
1425 err_reset:
1426         memset(&ucsi->cap, 0, sizeof(ucsi->cap));
1427         ucsi_reset_ppm(ucsi);
1428 err:
1429         return ret;
1430 }
1431
1432 static void ucsi_resume_work(struct work_struct *work)
1433 {
1434         struct ucsi *ucsi = container_of(work, struct ucsi, resume_work);
1435         struct ucsi_connector *con;
1436         u64 command;
1437         int ret;
1438
1439         /* Restore UCSI notification enable mask after system resume */
1440         command = UCSI_SET_NOTIFICATION_ENABLE | ucsi->ntfy;
1441         ret = ucsi_send_command(ucsi, command, NULL, 0);
1442         if (ret < 0) {
1443                 dev_err(ucsi->dev, "failed to re-enable notifications (%d)\n", ret);
1444                 return;
1445         }
1446
1447         for (con = ucsi->connector; con->port; con++) {
1448                 mutex_lock(&con->lock);
1449                 ucsi_partner_task(con, ucsi_check_connection, 1, 0);
1450                 mutex_unlock(&con->lock);
1451         }
1452 }
1453
1454 int ucsi_resume(struct ucsi *ucsi)
1455 {
1456         if (ucsi->connector)
1457                 queue_work(system_long_wq, &ucsi->resume_work);
1458         return 0;
1459 }
1460 EXPORT_SYMBOL_GPL(ucsi_resume);
1461
1462 static void ucsi_init_work(struct work_struct *work)
1463 {
1464         struct ucsi *ucsi = container_of(work, struct ucsi, work.work);
1465         int ret;
1466
1467         ret = ucsi_init(ucsi);
1468         if (ret)
1469                 dev_err_probe(ucsi->dev, ret, "PPM init failed\n");
1470
1471         if (ret == -EPROBE_DEFER) {
1472                 if (ucsi->work_count++ > UCSI_ROLE_SWITCH_WAIT_COUNT) {
1473                         dev_err(ucsi->dev, "PPM init failed, stop trying\n");
1474                         return;
1475                 }
1476
1477                 queue_delayed_work(system_long_wq, &ucsi->work,
1478                                    UCSI_ROLE_SWITCH_INTERVAL);
1479         }
1480 }
1481
1482 /**
1483  * ucsi_get_drvdata - Return private driver data pointer
1484  * @ucsi: UCSI interface
1485  */
1486 void *ucsi_get_drvdata(struct ucsi *ucsi)
1487 {
1488         return ucsi->driver_data;
1489 }
1490 EXPORT_SYMBOL_GPL(ucsi_get_drvdata);
1491
1492 /**
1493  * ucsi_set_drvdata - Assign private driver data pointer
1494  * @ucsi: UCSI interface
1495  * @data: Private data pointer
1496  */
1497 void ucsi_set_drvdata(struct ucsi *ucsi, void *data)
1498 {
1499         ucsi->driver_data = data;
1500 }
1501 EXPORT_SYMBOL_GPL(ucsi_set_drvdata);
1502
1503 /**
1504  * ucsi_create - Allocate UCSI instance
1505  * @dev: Device interface to the PPM (Platform Policy Manager)
1506  * @ops: I/O routines
1507  */
1508 struct ucsi *ucsi_create(struct device *dev, const struct ucsi_operations *ops)
1509 {
1510         struct ucsi *ucsi;
1511
1512         if (!ops || !ops->read || !ops->sync_write || !ops->async_write)
1513                 return ERR_PTR(-EINVAL);
1514
1515         ucsi = kzalloc(sizeof(*ucsi), GFP_KERNEL);
1516         if (!ucsi)
1517                 return ERR_PTR(-ENOMEM);
1518
1519         INIT_WORK(&ucsi->resume_work, ucsi_resume_work);
1520         INIT_DELAYED_WORK(&ucsi->work, ucsi_init_work);
1521         mutex_init(&ucsi->ppm_lock);
1522         ucsi->dev = dev;
1523         ucsi->ops = ops;
1524
1525         return ucsi;
1526 }
1527 EXPORT_SYMBOL_GPL(ucsi_create);
1528
1529 /**
1530  * ucsi_destroy - Free UCSI instance
1531  * @ucsi: UCSI instance to be freed
1532  */
1533 void ucsi_destroy(struct ucsi *ucsi)
1534 {
1535         ucsi_debugfs_unregister(ucsi);
1536         kfree(ucsi);
1537 }
1538 EXPORT_SYMBOL_GPL(ucsi_destroy);
1539
1540 /**
1541  * ucsi_register - Register UCSI interface
1542  * @ucsi: UCSI instance
1543  */
1544 int ucsi_register(struct ucsi *ucsi)
1545 {
1546         int ret;
1547
1548         ret = ucsi->ops->read(ucsi, UCSI_VERSION, &ucsi->version,
1549                               sizeof(ucsi->version));
1550         if (ret)
1551                 return ret;
1552
1553         if (!ucsi->version)
1554                 return -ENODEV;
1555
1556         queue_delayed_work(system_long_wq, &ucsi->work, 0);
1557
1558         ucsi_debugfs_register(ucsi);
1559         return 0;
1560 }
1561 EXPORT_SYMBOL_GPL(ucsi_register);
1562
1563 /**
1564  * ucsi_unregister - Unregister UCSI interface
1565  * @ucsi: UCSI interface to be unregistered
1566  *
1567  * Unregister UCSI interface that was created with ucsi_register().
1568  */
1569 void ucsi_unregister(struct ucsi *ucsi)
1570 {
1571         u64 cmd = UCSI_SET_NOTIFICATION_ENABLE;
1572         int i;
1573
1574         /* Make sure that we are not in the middle of driver initialization */
1575         cancel_delayed_work_sync(&ucsi->work);
1576         cancel_work_sync(&ucsi->resume_work);
1577
1578         /* Disable notifications */
1579         ucsi->ops->async_write(ucsi, UCSI_CONTROL, &cmd, sizeof(cmd));
1580
1581         if (!ucsi->connector)
1582                 return;
1583
1584         for (i = 0; i < ucsi->cap.num_connectors; i++) {
1585                 cancel_work_sync(&ucsi->connector[i].work);
1586                 ucsi_unregister_partner(&ucsi->connector[i]);
1587                 ucsi_unregister_altmodes(&ucsi->connector[i],
1588                                          UCSI_RECIPIENT_CON);
1589                 ucsi_unregister_port_psy(&ucsi->connector[i]);
1590
1591                 if (ucsi->connector[i].wq) {
1592                         struct ucsi_work *uwork;
1593
1594                         mutex_lock(&ucsi->connector[i].lock);
1595                         /*
1596                          * queue delayed items immediately so they can execute
1597                          * and free themselves before the wq is destroyed
1598                          */
1599                         list_for_each_entry(uwork, &ucsi->connector[i].partner_tasks, node)
1600                                 mod_delayed_work(ucsi->connector[i].wq, &uwork->work, 0);
1601                         mutex_unlock(&ucsi->connector[i].lock);
1602                         destroy_workqueue(ucsi->connector[i].wq);
1603                 }
1604
1605                 usb_power_delivery_unregister_capabilities(ucsi->connector[i].port_sink_caps);
1606                 ucsi->connector[i].port_sink_caps = NULL;
1607                 usb_power_delivery_unregister_capabilities(ucsi->connector[i].port_source_caps);
1608                 ucsi->connector[i].port_source_caps = NULL;
1609                 usb_power_delivery_unregister(ucsi->connector[i].pd);
1610                 ucsi->connector[i].pd = NULL;
1611                 typec_unregister_port(ucsi->connector[i].port);
1612         }
1613
1614         kfree(ucsi->connector);
1615 }
1616 EXPORT_SYMBOL_GPL(ucsi_unregister);
1617
1618 static int __init ucsi_module_init(void)
1619 {
1620         ucsi_debugfs_init();
1621         return 0;
1622 }
1623 module_init(ucsi_module_init);
1624
1625 static void __exit ucsi_module_exit(void)
1626 {
1627         ucsi_debugfs_exit();
1628 }
1629 module_exit(ucsi_module_exit);
1630
1631 MODULE_AUTHOR("Heikki Krogerus <heikki.krogerus@linux.intel.com>");
1632 MODULE_LICENSE("GPL v2");
1633 MODULE_DESCRIPTION("USB Type-C Connector System Software Interface driver");