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         ucsi_unregister_partner_pdos(con);
 791         ucsi_unregister_altmodes(con, UCSI_RECIPIENT_SOP);
 792         typec_unregister_partner(con->partner);
 793         con->partner = NULL;
 794 }
 795
 796 static void ucsi_partner_change(struct ucsi_connector *con)
 797 {
 798         enum usb_role u_role = USB_ROLE_NONE;
 799         int ret;
 800
 801         switch (UCSI_CONSTAT_PARTNER_TYPE(con->status.flags)) {
 802         case UCSI_CONSTAT_PARTNER_TYPE_UFP:
 803         case UCSI_CONSTAT_PARTNER_TYPE_CABLE_AND_UFP:
 804                 u_role = USB_ROLE_HOST;
 805                 fallthrough;
 806         case UCSI_CONSTAT_PARTNER_TYPE_CABLE:
 807                 typec_set_data_role(con->port, TYPEC_HOST);
 808                 break;
 809         case UCSI_CONSTAT_PARTNER_TYPE_DFP:
 810                 u_role = USB_ROLE_DEVICE;
 811                 typec_set_data_role(con->port, TYPEC_DEVICE);
 812                 break;
 813         default:
 814                 break;
 815         }
 816
 817         if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
 818                 switch (UCSI_CONSTAT_PARTNER_TYPE(con->status.flags)) {
 819                 case UCSI_CONSTAT_PARTNER_TYPE_DEBUG:
 820                         typec_set_mode(con->port, TYPEC_MODE_DEBUG);
 821                         break;
 822                 case UCSI_CONSTAT_PARTNER_TYPE_AUDIO:
 823                         typec_set_mode(con->port, TYPEC_MODE_AUDIO);
 824                         break;
 825                 default:
 826                         if (UCSI_CONSTAT_PARTNER_FLAGS(con->status.flags) ==
 827                                         UCSI_CONSTAT_PARTNER_FLAG_USB)
 828                                 typec_set_mode(con->port, TYPEC_STATE_USB);
 829                 }
 830         }
 831
 832         /* Only notify USB controller if partner supports USB data */
 833         if (!(UCSI_CONSTAT_PARTNER_FLAGS(con->status.flags) & UCSI_CONSTAT_PARTNER_FLAG_USB))
 834                 u_role = USB_ROLE_NONE;
 835
 836         ret = usb_role_switch_set_role(con->usb_role_sw, u_role);
 837         if (ret)
 838                 dev_err(con->ucsi->dev, "con:%d: failed to set usb role:%d\n",
 839                         con->num, u_role);
 840 }
 841
 842 static int ucsi_check_connection(struct ucsi_connector *con)
 843 {
 844         u8 prev_flags = con->status.flags;
 845         u64 command;
 846         int ret;
 847
 848         command = UCSI_GET_CONNECTOR_STATUS | UCSI_CONNECTOR_NUMBER(con->num);
 849         ret = ucsi_send_command(con->ucsi, command, &con->status, sizeof(con->status));
 850         if (ret < 0) {
 851                 dev_err(con->ucsi->dev, "GET_CONNECTOR_STATUS failed (%d)\n", ret);
 852                 return ret;
 853         }
 854
 855         if (con->status.flags == prev_flags)
 856                 return 0;
 857
 858         if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
 859                 ucsi_register_partner(con);
 860                 ucsi_pwr_opmode_change(con);
 861                 ucsi_partner_change(con);
 862         } else {
 863                 ucsi_partner_change(con);
 864                 ucsi_port_psy_changed(con);
 865                 ucsi_unregister_partner(con);
 866         }
 867
 868         return 0;
 869 }
 870
 871 static void ucsi_handle_connector_change(struct work_struct *work)
 872 {
 873         struct ucsi_connector *con = container_of(work, struct ucsi_connector,
 874                                                   work);
 875         struct ucsi *ucsi = con->ucsi;
 876         enum typec_role role;
 877         u64 command;
 878         int ret;
 879
 880         mutex_lock(&con->lock);
 881
 882         command = UCSI_GET_CONNECTOR_STATUS | UCSI_CONNECTOR_NUMBER(con->num);
 883         ret = ucsi_send_command(ucsi, command, &con->status, sizeof(con->status));
 884         if (ret < 0) {
 885                 dev_err(ucsi->dev, "%s: GET_CONNECTOR_STATUS failed (%d)\n",
 886                         __func__, ret);
 887                 goto out_unlock;
 888         }
 889
 890         trace_ucsi_connector_change(con->num, &con->status);
 891
 892         role = !!(con->status.flags & UCSI_CONSTAT_PWR_DIR);
 893
 894         if (con->status.change & UCSI_CONSTAT_POWER_DIR_CHANGE) {
 895                 typec_set_pwr_role(con->port, role);
 896
 897                 /* Complete pending power role swap */
 898                 if (!completion_done(&con->complete))
 899                         complete(&con->complete);
 900         }
 901
 902         if (con->status.change & UCSI_CONSTAT_CONNECT_CHANGE) {
 903                 typec_set_pwr_role(con->port, role);
 904                 ucsi_port_psy_changed(con);
 905                 ucsi_partner_change(con);
 906
 907                 if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
 908                         ucsi_register_partner(con);
 909                         ucsi_partner_task(con, ucsi_check_connection, 1, HZ);
 910
 911                         if (UCSI_CONSTAT_PWR_OPMODE(con->status.flags) ==
 912                             UCSI_CONSTAT_PWR_OPMODE_PD)
 913                                 ucsi_partner_task(con, ucsi_register_partner_pdos, 1, HZ);
 914                 } else {
 915                         ucsi_unregister_partner(con);
 916                 }
 917         }
 918
 919         if (con->status.change & UCSI_CONSTAT_POWER_OPMODE_CHANGE ||
 920             con->status.change & UCSI_CONSTAT_POWER_LEVEL_CHANGE)
 921                 ucsi_pwr_opmode_change(con);
 922
 923         if (con->partner && con->status.change & UCSI_CONSTAT_PARTNER_CHANGE) {
 924                 ucsi_partner_change(con);
 925
 926                 /* Complete pending data role swap */
 927                 if (!completion_done(&con->complete))
 928                         complete(&con->complete);
 929         }
 930
 931         if (con->status.change & UCSI_CONSTAT_CAM_CHANGE)
 932                 ucsi_partner_task(con, ucsi_check_altmodes, 1, 0);
 933
 934         clear_bit(EVENT_PENDING, &con->ucsi->flags);
 935
 936         ret = ucsi_acknowledge_connector_change(ucsi);
 937         if (ret)
 938                 dev_err(ucsi->dev, "%s: ACK failed (%d)", __func__, ret);
 939
 940 out_unlock:
 941         mutex_unlock(&con->lock);
 942 }
 943
 944 /**
 945  * ucsi_connector_change - Process Connector Change Event
 946  * @ucsi: UCSI Interface
 947  * @num: Connector number
 948  */
 949 void ucsi_connector_change(struct ucsi *ucsi, u8 num)
 950 {
 951         struct ucsi_connector *con = &ucsi->connector[num - 1];
 952
 953         if (!(ucsi->ntfy & UCSI_ENABLE_NTFY_CONNECTOR_CHANGE)) {
 954                 dev_dbg(ucsi->dev, "Bogus connector change event\n");
 955                 return;
 956         }
 957
 958         if (!test_and_set_bit(EVENT_PENDING, &ucsi->flags))
 959                 schedule_work(&con->work);
 960 }
 961 EXPORT_SYMBOL_GPL(ucsi_connector_change);
 962
 963 /* -------------------------------------------------------------------------- */
 964
 965 static int ucsi_reset_connector(struct ucsi_connector *con, bool hard)
 966 {
 967         u64 command;
 968
 969         command = UCSI_CONNECTOR_RESET | UCSI_CONNECTOR_NUMBER(con->num);
 970         command |= hard ? UCSI_CONNECTOR_RESET_HARD : 0;
 971
 972         return ucsi_send_command(con->ucsi, command, NULL, 0);
 973 }
 974
 975 static int ucsi_reset_ppm(struct ucsi *ucsi)
 976 {
 977         u64 command = UCSI_PPM_RESET;
 978         unsigned long tmo;
 979         u32 cci;
 980         int ret;
 981
 982         mutex_lock(&ucsi->ppm_lock);
 983
 984         ret = ucsi->ops->async_write(ucsi, UCSI_CONTROL, &command,
 985                                      sizeof(command));
 986         if (ret < 0)
 987                 goto out;
 988
 989         tmo = jiffies + msecs_to_jiffies(UCSI_TIMEOUT_MS);
 990
 991         do {
 992                 if (time_is_before_jiffies(tmo)) {
 993                         ret = -ETIMEDOUT;
 994                         goto out;
 995                 }
 996
 997                 ret = ucsi->ops->read(ucsi, UCSI_CCI, &cci, sizeof(cci));
 998                 if (ret)
 999                         goto out;
1000
1001                 /* If the PPM is still doing something else, reset it again. */
1002                 if (cci & ~UCSI_CCI_RESET_COMPLETE) {
1003                         ret = ucsi->ops->async_write(ucsi, UCSI_CONTROL,
1004                                                      &command,
1005                                                      sizeof(command));
1006                         if (ret < 0)
1007                                 goto out;
1008                 }
1009
1010                 msleep(20);
1011         } while (!(cci & UCSI_CCI_RESET_COMPLETE));
1012
1013 out:
1014         mutex_unlock(&ucsi->ppm_lock);
1015         return ret;
1016 }
1017
1018 static int ucsi_role_cmd(struct ucsi_connector *con, u64 command)
1019 {
1020         int ret;
1021
1022         ret = ucsi_send_command(con->ucsi, command, NULL, 0);
1023         if (ret == -ETIMEDOUT) {
1024                 u64 c;
1025
1026                 /* PPM most likely stopped responding. Resetting everything. */
1027                 ucsi_reset_ppm(con->ucsi);
1028
1029                 c = UCSI_SET_NOTIFICATION_ENABLE | con->ucsi->ntfy;
1030                 ucsi_send_command(con->ucsi, c, NULL, 0);
1031
1032                 ucsi_reset_connector(con, true);
1033         }
1034
1035         return ret;
1036 }
1037
1038 static int ucsi_dr_swap(struct typec_port *port, enum typec_data_role role)
1039 {
1040         struct ucsi_connector *con = typec_get_drvdata(port);
1041         u8 partner_type;
1042         u64 command;
1043         int ret = 0;
1044
1045         mutex_lock(&con->lock);
1046
1047         if (!con->partner) {
1048                 ret = -ENOTCONN;
1049                 goto out_unlock;
1050         }
1051
1052         partner_type = UCSI_CONSTAT_PARTNER_TYPE(con->status.flags);
1053         if ((partner_type == UCSI_CONSTAT_PARTNER_TYPE_DFP &&
1054              role == TYPEC_DEVICE) ||
1055             (partner_type == UCSI_CONSTAT_PARTNER_TYPE_UFP &&
1056              role == TYPEC_HOST))
1057                 goto out_unlock;
1058
1059         reinit_completion(&con->complete);
1060
1061         command = UCSI_SET_UOR | UCSI_CONNECTOR_NUMBER(con->num);
1062         command |= UCSI_SET_UOR_ROLE(role);
1063         command |= UCSI_SET_UOR_ACCEPT_ROLE_SWAPS;
1064         ret = ucsi_role_cmd(con, command);
1065         if (ret < 0)
1066                 goto out_unlock;
1067
1068         mutex_unlock(&con->lock);
1069
1070         if (!wait_for_completion_timeout(&con->complete,
1071                                          msecs_to_jiffies(UCSI_SWAP_TIMEOUT_MS)))
1072                 return -ETIMEDOUT;
1073
1074         return 0;
1075
1076 out_unlock:
1077         mutex_unlock(&con->lock);
1078
1079         return ret;
1080 }
1081
1082 static int ucsi_pr_swap(struct typec_port *port, enum typec_role role)
1083 {
1084         struct ucsi_connector *con = typec_get_drvdata(port);
1085         enum typec_role cur_role;
1086         u64 command;
1087         int ret = 0;
1088
1089         mutex_lock(&con->lock);
1090
1091         if (!con->partner) {
1092                 ret = -ENOTCONN;
1093                 goto out_unlock;
1094         }
1095
1096         cur_role = !!(con->status.flags & UCSI_CONSTAT_PWR_DIR);
1097
1098         if (cur_role == role)
1099                 goto out_unlock;
1100
1101         reinit_completion(&con->complete);
1102
1103         command = UCSI_SET_PDR | UCSI_CONNECTOR_NUMBER(con->num);
1104         command |= UCSI_SET_PDR_ROLE(role);
1105         command |= UCSI_SET_PDR_ACCEPT_ROLE_SWAPS;
1106         ret = ucsi_role_cmd(con, command);
1107         if (ret < 0)
1108                 goto out_unlock;
1109
1110         mutex_unlock(&con->lock);
1111
1112         if (!wait_for_completion_timeout(&con->complete,
1113                                          msecs_to_jiffies(UCSI_SWAP_TIMEOUT_MS)))
1114                 return -ETIMEDOUT;
1115
1116         mutex_lock(&con->lock);
1117
1118         /* Something has gone wrong while swapping the role */
1119         if (UCSI_CONSTAT_PWR_OPMODE(con->status.flags) !=
1120             UCSI_CONSTAT_PWR_OPMODE_PD) {
1121                 ucsi_reset_connector(con, true);
1122                 ret = -EPROTO;
1123         }
1124
1125 out_unlock:
1126         mutex_unlock(&con->lock);
1127
1128         return ret;
1129 }
1130
1131 static const struct typec_operations ucsi_ops = {
1132         .dr_set = ucsi_dr_swap,
1133         .pr_set = ucsi_pr_swap
1134 };
1135
1136 /* Caller must call fwnode_handle_put() after use */
1137 static struct fwnode_handle *ucsi_find_fwnode(struct ucsi_connector *con)
1138 {
1139         struct fwnode_handle *fwnode;
1140         int i = 1;
1141
1142         device_for_each_child_node(con->ucsi->dev, fwnode)
1143                 if (i++ == con->num)
1144                         return fwnode;
1145         return NULL;
1146 }
1147
1148 static int ucsi_register_port(struct ucsi *ucsi, struct ucsi_connector *con)
1149 {
1150         struct usb_power_delivery_desc desc = { ucsi->cap.pd_version};
1151         struct usb_power_delivery_capabilities_desc pd_caps;
1152         struct usb_power_delivery_capabilities *pd_cap;
1153         struct typec_capability *cap = &con->typec_cap;
1154         enum typec_accessory *accessory = cap->accessory;
1155         enum usb_role u_role = USB_ROLE_NONE;
1156         u64 command;
1157         char *name;
1158         int ret;
1159
1160         name = kasprintf(GFP_KERNEL, "%s-con%d", dev_name(ucsi->dev), con->num);
1161         if (!name)
1162                 return -ENOMEM;
1163
1164         con->wq = create_singlethread_workqueue(name);
1165         kfree(name);
1166         if (!con->wq)
1167                 return -ENOMEM;
1168
1169         INIT_WORK(&con->work, ucsi_handle_connector_change);
1170         init_completion(&con->complete);
1171         mutex_init(&con->lock);
1172         INIT_LIST_HEAD(&con->partner_tasks);
1173         con->ucsi = ucsi;
1174
1175         cap->fwnode = ucsi_find_fwnode(con);
1176         con->usb_role_sw = fwnode_usb_role_switch_get(cap->fwnode);
1177         if (IS_ERR(con->usb_role_sw))
1178                 return dev_err_probe(ucsi->dev, PTR_ERR(con->usb_role_sw),
1179                         "con%d: failed to get usb role switch\n", con->num);
1180
1181         /* Delay other interactions with the con until registration is complete */
1182         mutex_lock(&con->lock);
1183
1184         /* Get connector capability */
1185         command = UCSI_GET_CONNECTOR_CAPABILITY;
1186         command |= UCSI_CONNECTOR_NUMBER(con->num);
1187         ret = ucsi_send_command(ucsi, command, &con->cap, sizeof(con->cap));
1188         if (ret < 0)
1189                 goto out_unlock;
1190
1191         if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DRP)
1192                 cap->data = TYPEC_PORT_DRD;
1193         else if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DFP)
1194                 cap->data = TYPEC_PORT_DFP;
1195         else if (con->cap.op_mode & UCSI_CONCAP_OPMODE_UFP)
1196                 cap->data = TYPEC_PORT_UFP;
1197
1198         if ((con->cap.flags & UCSI_CONCAP_FLAG_PROVIDER) &&
1199             (con->cap.flags & UCSI_CONCAP_FLAG_CONSUMER))
1200                 cap->type = TYPEC_PORT_DRP;
1201         else if (con->cap.flags & UCSI_CONCAP_FLAG_PROVIDER)
1202                 cap->type = TYPEC_PORT_SRC;
1203         else if (con->cap.flags & UCSI_CONCAP_FLAG_CONSUMER)
1204                 cap->type = TYPEC_PORT_SNK;
1205
1206         cap->revision = ucsi->cap.typec_version;
1207         cap->pd_revision = ucsi->cap.pd_version;
1208         cap->svdm_version = SVDM_VER_2_0;
1209         cap->prefer_role = TYPEC_NO_PREFERRED_ROLE;
1210
1211         if (con->cap.op_mode & UCSI_CONCAP_OPMODE_AUDIO_ACCESSORY)
1212                 *accessory++ = TYPEC_ACCESSORY_AUDIO;
1213         if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DEBUG_ACCESSORY)
1214                 *accessory = TYPEC_ACCESSORY_DEBUG;
1215
1216         cap->driver_data = con;
1217         cap->ops = &ucsi_ops;
1218
1219         ret = ucsi_register_port_psy(con);
1220         if (ret)
1221                 goto out;
1222
1223         /* Register the connector */
1224         con->port = typec_register_port(ucsi->dev, cap);
1225         if (IS_ERR(con->port)) {
1226                 ret = PTR_ERR(con->port);
1227                 goto out;
1228         }
1229
1230         con->pd = usb_power_delivery_register(ucsi->dev, &desc);
1231
1232         ret = ucsi_get_pdos(con, TYPEC_SOURCE, 0, pd_caps.pdo);
1233         if (ret > 0) {
1234                 if (ret < PDO_MAX_OBJECTS)
1235                         pd_caps.pdo[ret] = 0;
1236
1237                 pd_caps.role = TYPEC_SOURCE;
1238                 pd_cap = usb_power_delivery_register_capabilities(con->pd, &pd_caps);
1239                 if (IS_ERR(pd_cap)) {
1240                         ret = PTR_ERR(pd_cap);
1241                         goto out;
1242                 }
1243
1244                 con->port_source_caps = pd_cap;
1245                 typec_port_set_usb_power_delivery(con->port, con->pd);
1246         }
1247
1248         memset(&pd_caps, 0, sizeof(pd_caps));
1249         ret = ucsi_get_pdos(con, TYPEC_SINK, 0, pd_caps.pdo);
1250         if (ret > 0) {
1251                 if (ret < PDO_MAX_OBJECTS)
1252                         pd_caps.pdo[ret] = 0;
1253
1254                 pd_caps.role = TYPEC_SINK;
1255                 pd_cap = usb_power_delivery_register_capabilities(con->pd, &pd_caps);
1256                 if (IS_ERR(pd_cap)) {
1257                         ret = PTR_ERR(pd_cap);
1258                         goto out;
1259                 }
1260
1261                 con->port_sink_caps = pd_cap;
1262                 typec_port_set_usb_power_delivery(con->port, con->pd);
1263         }
1264
1265         /* Alternate modes */
1266         ret = ucsi_register_altmodes(con, UCSI_RECIPIENT_CON);
1267         if (ret) {
1268                 dev_err(ucsi->dev, "con%d: failed to register alt modes\n",
1269                         con->num);
1270                 goto out;
1271         }
1272
1273         /* Get the status */
1274         command = UCSI_GET_CONNECTOR_STATUS | UCSI_CONNECTOR_NUMBER(con->num);
1275         ret = ucsi_send_command(ucsi, command, &con->status, sizeof(con->status));
1276         if (ret < 0) {
1277                 dev_err(ucsi->dev, "con%d: failed to get status\n", con->num);
1278                 ret = 0;
1279                 goto out;
1280         }
1281         ret = 0; /* ucsi_send_command() returns length on success */
1282
1283         switch (UCSI_CONSTAT_PARTNER_TYPE(con->status.flags)) {
1284         case UCSI_CONSTAT_PARTNER_TYPE_UFP:
1285         case UCSI_CONSTAT_PARTNER_TYPE_CABLE_AND_UFP:
1286                 u_role = USB_ROLE_HOST;
1287                 fallthrough;
1288         case UCSI_CONSTAT_PARTNER_TYPE_CABLE:
1289                 typec_set_data_role(con->port, TYPEC_HOST);
1290                 break;
1291         case UCSI_CONSTAT_PARTNER_TYPE_DFP:
1292                 u_role = USB_ROLE_DEVICE;
1293                 typec_set_data_role(con->port, TYPEC_DEVICE);
1294                 break;
1295         default:
1296                 break;
1297         }
1298
1299         /* Check if there is already something connected */
1300         if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
1301                 typec_set_pwr_role(con->port,
1302                                   !!(con->status.flags & UCSI_CONSTAT_PWR_DIR));
1303                 ucsi_register_partner(con);
1304                 ucsi_pwr_opmode_change(con);
1305                 ucsi_port_psy_changed(con);
1306         }
1307
1308         /* Only notify USB controller if partner supports USB data */
1309         if (!(UCSI_CONSTAT_PARTNER_FLAGS(con->status.flags) & UCSI_CONSTAT_PARTNER_FLAG_USB))
1310                 u_role = USB_ROLE_NONE;
1311
1312         ret = usb_role_switch_set_role(con->usb_role_sw, u_role);
1313         if (ret) {
1314                 dev_err(ucsi->dev, "con:%d: failed to set usb role:%d\n",
1315                         con->num, u_role);
1316                 ret = 0;
1317         }
1318
1319         if (con->partner &&
1320             UCSI_CONSTAT_PWR_OPMODE(con->status.flags) ==
1321             UCSI_CONSTAT_PWR_OPMODE_PD) {
1322                 ucsi_get_src_pdos(con);
1323                 ucsi_check_altmodes(con);
1324         }
1325
1326         trace_ucsi_register_port(con->num, &con->status);
1327
1328 out:
1329         fwnode_handle_put(cap->fwnode);
1330 out_unlock:
1331         mutex_unlock(&con->lock);
1332
1333         if (ret && con->wq) {
1334                 destroy_workqueue(con->wq);
1335                 con->wq = NULL;
1336         }
1337
1338         return ret;
1339 }
1340
1341 /**
1342  * ucsi_init - Initialize UCSI interface
1343  * @ucsi: UCSI to be initialized
1344  *
1345  * Registers all ports @ucsi has and enables all notification events.
1346  */
1347 static int ucsi_init(struct ucsi *ucsi)
1348 {
1349         struct ucsi_connector *con, *connector;
1350         u64 command, ntfy;
1351         int ret;
1352         int i;
1353
1354         /* Reset the PPM */
1355         ret = ucsi_reset_ppm(ucsi);
1356         if (ret) {
1357                 dev_err(ucsi->dev, "failed to reset PPM!\n");
1358                 goto err;
1359         }
1360
1361         /* Enable basic notifications */
1362         ntfy = UCSI_ENABLE_NTFY_CMD_COMPLETE | UCSI_ENABLE_NTFY_ERROR;
1363         command = UCSI_SET_NOTIFICATION_ENABLE | ntfy;
1364         ret = ucsi_send_command(ucsi, command, NULL, 0);
1365         if (ret < 0)
1366                 goto err_reset;
1367
1368         /* Get PPM capabilities */
1369         command = UCSI_GET_CAPABILITY;
1370         ret = ucsi_send_command(ucsi, command, &ucsi->cap, sizeof(ucsi->cap));
1371         if (ret < 0)
1372                 goto err_reset;
1373
1374         if (!ucsi->cap.num_connectors) {
1375                 ret = -ENODEV;
1376                 goto err_reset;
1377         }
1378
1379         /* Allocate the connectors. Released in ucsi_unregister() */
1380         connector = kcalloc(ucsi->cap.num_connectors + 1, sizeof(*connector), GFP_KERNEL);
1381         if (!connector) {
1382                 ret = -ENOMEM;
1383                 goto err_reset;
1384         }
1385
1386         /* Register all connectors */
1387         for (i = 0; i < ucsi->cap.num_connectors; i++) {
1388                 connector[i].num = i + 1;
1389                 ret = ucsi_register_port(ucsi, &connector[i]);
1390                 if (ret)
1391                         goto err_unregister;
1392         }
1393
1394         /* Enable all notifications */
1395         ntfy = UCSI_ENABLE_NTFY_ALL;
1396         command = UCSI_SET_NOTIFICATION_ENABLE | ntfy;
1397         ret = ucsi_send_command(ucsi, command, NULL, 0);
1398         if (ret < 0)
1399                 goto err_unregister;
1400
1401         ucsi->connector = connector;
1402         ucsi->ntfy = ntfy;
1403         return 0;
1404
1405 err_unregister:
1406         for (con = connector; con->port; con++) {
1407                 ucsi_unregister_partner(con);
1408                 ucsi_unregister_altmodes(con, UCSI_RECIPIENT_CON);
1409                 ucsi_unregister_port_psy(con);
1410                 if (con->wq)
1411                         destroy_workqueue(con->wq);
1412
1413                 usb_power_delivery_unregister_capabilities(con->port_sink_caps);
1414                 con->port_sink_caps = NULL;
1415                 usb_power_delivery_unregister_capabilities(con->port_source_caps);
1416                 con->port_source_caps = NULL;
1417                 usb_power_delivery_unregister(con->pd);
1418                 con->pd = NULL;
1419                 typec_unregister_port(con->port);
1420                 con->port = NULL;
1421         }
1422         kfree(connector);
1423 err_reset:
1424         memset(&ucsi->cap, 0, sizeof(ucsi->cap));
1425         ucsi_reset_ppm(ucsi);
1426 err:
1427         return ret;
1428 }
1429
1430 static void ucsi_resume_work(struct work_struct *work)
1431 {
1432         struct ucsi *ucsi = container_of(work, struct ucsi, resume_work);
1433         struct ucsi_connector *con;
1434         u64 command;
1435         int ret;
1436
1437         /* Restore UCSI notification enable mask after system resume */
1438         command = UCSI_SET_NOTIFICATION_ENABLE | ucsi->ntfy;
1439         ret = ucsi_send_command(ucsi, command, NULL, 0);
1440         if (ret < 0) {
1441                 dev_err(ucsi->dev, "failed to re-enable notifications (%d)\n", ret);
1442                 return;
1443         }
1444
1445         for (con = ucsi->connector; con->port; con++) {
1446                 mutex_lock(&con->lock);
1447                 ucsi_partner_task(con, ucsi_check_connection, 1, 0);
1448                 mutex_unlock(&con->lock);
1449         }
1450 }
1451
1452 int ucsi_resume(struct ucsi *ucsi)
1453 {
1454         if (ucsi->connector)
1455                 queue_work(system_long_wq, &ucsi->resume_work);
1456         return 0;
1457 }
1458 EXPORT_SYMBOL_GPL(ucsi_resume);
1459
1460 static void ucsi_init_work(struct work_struct *work)
1461 {
1462         struct ucsi *ucsi = container_of(work, struct ucsi, work.work);
1463         int ret;
1464
1465         ret = ucsi_init(ucsi);
1466         if (ret)
1467                 dev_err_probe(ucsi->dev, ret, "PPM init failed\n");
1468
1469         if (ret == -EPROBE_DEFER) {
1470                 if (ucsi->work_count++ > UCSI_ROLE_SWITCH_WAIT_COUNT) {
1471                         dev_err(ucsi->dev, "PPM init failed, stop trying\n");
1472                         return;
1473                 }
1474
1475                 queue_delayed_work(system_long_wq, &ucsi->work,
1476                                    UCSI_ROLE_SWITCH_INTERVAL);
1477         }
1478 }
1479
1480 /**
1481  * ucsi_get_drvdata - Return private driver data pointer
1482  * @ucsi: UCSI interface
1483  */
1484 void *ucsi_get_drvdata(struct ucsi *ucsi)
1485 {
1486         return ucsi->driver_data;
1487 }
1488 EXPORT_SYMBOL_GPL(ucsi_get_drvdata);
1489
1490 /**
1491  * ucsi_set_drvdata - Assign private driver data pointer
1492  * @ucsi: UCSI interface
1493  * @data: Private data pointer
1494  */
1495 void ucsi_set_drvdata(struct ucsi *ucsi, void *data)
1496 {
1497         ucsi->driver_data = data;
1498 }
1499 EXPORT_SYMBOL_GPL(ucsi_set_drvdata);
1500
1501 /**
1502  * ucsi_create - Allocate UCSI instance
1503  * @dev: Device interface to the PPM (Platform Policy Manager)
1504  * @ops: I/O routines
1505  */
1506 struct ucsi *ucsi_create(struct device *dev, const struct ucsi_operations *ops)
1507 {
1508         struct ucsi *ucsi;
1509
1510         if (!ops || !ops->read || !ops->sync_write || !ops->async_write)
1511                 return ERR_PTR(-EINVAL);
1512
1513         ucsi = kzalloc(sizeof(*ucsi), GFP_KERNEL);
1514         if (!ucsi)
1515                 return ERR_PTR(-ENOMEM);
1516
1517         INIT_WORK(&ucsi->resume_work, ucsi_resume_work);
1518         INIT_DELAYED_WORK(&ucsi->work, ucsi_init_work);
1519         mutex_init(&ucsi->ppm_lock);
1520         ucsi->dev = dev;
1521         ucsi->ops = ops;
1522
1523         return ucsi;
1524 }
1525 EXPORT_SYMBOL_GPL(ucsi_create);
1526
1527 /**
1528  * ucsi_destroy - Free UCSI instance
1529  * @ucsi: UCSI instance to be freed
1530  */
1531 void ucsi_destroy(struct ucsi *ucsi)
1532 {
1533         ucsi_debugfs_unregister(ucsi);
1534         kfree(ucsi);
1535 }
1536 EXPORT_SYMBOL_GPL(ucsi_destroy);
1537
1538 /**
1539  * ucsi_register - Register UCSI interface
1540  * @ucsi: UCSI instance
1541  */
1542 int ucsi_register(struct ucsi *ucsi)
1543 {
1544         int ret;
1545
1546         ret = ucsi->ops->read(ucsi, UCSI_VERSION, &ucsi->version,
1547                               sizeof(ucsi->version));
1548         if (ret)
1549                 return ret;
1550
1551         if (!ucsi->version)
1552                 return -ENODEV;
1553
1554         queue_delayed_work(system_long_wq, &ucsi->work, 0);
1555
1556         ucsi_debugfs_register(ucsi);
1557         return 0;
1558 }
1559 EXPORT_SYMBOL_GPL(ucsi_register);
1560
1561 /**
1562  * ucsi_unregister - Unregister UCSI interface
1563  * @ucsi: UCSI interface to be unregistered
1564  *
1565  * Unregister UCSI interface that was created with ucsi_register().
1566  */
1567 void ucsi_unregister(struct ucsi *ucsi)
1568 {
1569         u64 cmd = UCSI_SET_NOTIFICATION_ENABLE;
1570         int i;
1571
1572         /* Make sure that we are not in the middle of driver initialization */
1573         cancel_delayed_work_sync(&ucsi->work);
1574         cancel_work_sync(&ucsi->resume_work);
1575
1576         /* Disable notifications */
1577         ucsi->ops->async_write(ucsi, UCSI_CONTROL, &cmd, sizeof(cmd));
1578
1579         if (!ucsi->connector)
1580                 return;
1581
1582         for (i = 0; i < ucsi->cap.num_connectors; i++) {
1583                 cancel_work_sync(&ucsi->connector[i].work);
1584                 ucsi_unregister_partner(&ucsi->connector[i]);
1585                 ucsi_unregister_altmodes(&ucsi->connector[i],
1586                                          UCSI_RECIPIENT_CON);
1587                 ucsi_unregister_port_psy(&ucsi->connector[i]);
1588
1589                 if (ucsi->connector[i].wq) {
1590                         struct ucsi_work *uwork;
1591
1592                         mutex_lock(&ucsi->connector[i].lock);
1593                         /*
1594                          * queue delayed items immediately so they can execute
1595                          * and free themselves before the wq is destroyed
1596                          */
1597                         list_for_each_entry(uwork, &ucsi->connector[i].partner_tasks, node)
1598                                 mod_delayed_work(ucsi->connector[i].wq, &uwork->work, 0);
1599                         mutex_unlock(&ucsi->connector[i].lock);
1600                         destroy_workqueue(ucsi->connector[i].wq);
1601                 }
1602
1603                 usb_power_delivery_unregister_capabilities(ucsi->connector[i].port_sink_caps);
1604                 ucsi->connector[i].port_sink_caps = NULL;
1605                 usb_power_delivery_unregister_capabilities(ucsi->connector[i].port_source_caps);
1606                 ucsi->connector[i].port_source_caps = NULL;
1607                 usb_power_delivery_unregister(ucsi->connector[i].pd);
1608                 ucsi->connector[i].pd = NULL;
1609                 typec_unregister_port(ucsi->connector[i].port);
1610         }
1611
1612         kfree(ucsi->connector);
1613 }
1614 EXPORT_SYMBOL_GPL(ucsi_unregister);
1615
1616 static int __init ucsi_module_init(void)
1617 {
1618         ucsi_debugfs_init();
1619         return 0;
1620 }
1621 module_init(ucsi_module_init);
1622
1623 static void __exit ucsi_module_exit(void)
1624 {
1625         ucsi_debugfs_exit();
1626 }
1627 module_exit(ucsi_module_exit);
1628
1629 MODULE_AUTHOR("Heikki Krogerus <heikki.krogerus@linux.intel.com>");
1630 MODULE_LICENSE("GPL v2");
1631 MODULE_DESCRIPTION("USB Type-C Connector System Software Interface driver");