drivers/staging/greybus/svc.c

   1 /*
   2  * SVC Greybus driver.
   3  *
   4  * Copyright 2015 Google Inc.
   5  * Copyright 2015 Linaro Ltd.
   6  *
   7  * Released under the GPLv2 only.
   8  */
   9
  10 #include <linux/debugfs.h>
  11 #include <linux/workqueue.h>
  12
  13 #include "greybus.h"
  14
  15 #define SVC_INTF_EJECT_TIMEOUT          9000
  16 #define SVC_INTF_ACTIVATE_TIMEOUT       6000
  17 #define SVC_INTF_RESUME_TIMEOUT 3000
  18
  19 struct gb_svc_deferred_request {
  20         struct work_struct work;
  21         struct gb_operation *operation;
  22 };
  23
  24
  25 static int gb_svc_queue_deferred_request(struct gb_operation *operation);
  26
  27 static ssize_t endo_id_show(struct device *dev,
  28                         struct device_attribute *attr, char *buf)
  29 {
  30         struct gb_svc *svc = to_gb_svc(dev);
  31
  32         return sprintf(buf, "0x%04x\n", svc->endo_id);
  33 }
  34 static DEVICE_ATTR_RO(endo_id);
  35
  36 static ssize_t ap_intf_id_show(struct device *dev,
  37                         struct device_attribute *attr, char *buf)
  38 {
  39         struct gb_svc *svc = to_gb_svc(dev);
  40
  41         return sprintf(buf, "%u\n", svc->ap_intf_id);
  42 }
  43 static DEVICE_ATTR_RO(ap_intf_id);
  44
  45 // FIXME
  46 // This is a hack, we need to do this "right" and clean the interface up
  47 // properly, not just forcibly yank the thing out of the system and hope for the
  48 // best.  But for now, people want their modules to come out without having to
  49 // throw the thing to the ground or get out a screwdriver.
  50 static ssize_t intf_eject_store(struct device *dev,
  51                                 struct device_attribute *attr, const char *buf,
  52                                 size_t len)
  53 {
  54         struct gb_svc *svc = to_gb_svc(dev);
  55         unsigned short intf_id;
  56         int ret;
  57
  58         ret = kstrtou16(buf, 10, &intf_id);
  59         if (ret < 0)
  60                 return ret;
  61
  62         dev_warn(dev, "Forcibly trying to eject interface %d\n", intf_id);
  63
  64         ret = gb_svc_intf_eject(svc, intf_id);
  65         if (ret < 0)
  66                 return ret;
  67
  68         return len;
  69 }
  70 static DEVICE_ATTR_WO(intf_eject);
  71
  72 static ssize_t watchdog_show(struct device *dev, struct device_attribute *attr,
  73                              char *buf)
  74 {
  75         struct gb_svc *svc = to_gb_svc(dev);
  76
  77         return sprintf(buf, "%s\n",
  78                        gb_svc_watchdog_enabled(svc) ? "enabled" : "disabled");
  79 }
  80
  81 static ssize_t watchdog_store(struct device *dev,
  82                               struct device_attribute *attr, const char *buf,
  83                               size_t len)
  84 {
  85         struct gb_svc *svc = to_gb_svc(dev);
  86         int retval;
  87         bool user_request;
  88
  89         retval = strtobool(buf, &user_request);
  90         if (retval)
  91                 return retval;
  92
  93         if (user_request)
  94                 retval = gb_svc_watchdog_enable(svc);
  95         else
  96                 retval = gb_svc_watchdog_disable(svc);
  97         if (retval)
  98                 return retval;
  99         return len;
 100 }
 101 static DEVICE_ATTR_RW(watchdog);
 102
 103 static int gb_svc_pwrmon_rail_count_get(struct gb_svc *svc, u8 *value)
 104 {
 105         struct gb_svc_pwrmon_rail_count_get_response response;
 106         int ret;
 107
 108         ret = gb_operation_sync(svc->connection,
 109                                 GB_SVC_TYPE_PWRMON_RAIL_COUNT_GET, NULL, 0,
 110                                 &response, sizeof(response));
 111         if (ret) {
 112                 dev_err(&svc->dev, "failed to get rail count: %d\n", ret);
 113                 return ret;
 114         }
 115
 116         *value = response.rail_count;
 117
 118         return 0;
 119 }
 120
 121 static int gb_svc_pwrmon_rail_names_get(struct gb_svc *svc,
 122                 struct gb_svc_pwrmon_rail_names_get_response *response,
 123                 size_t bufsize)
 124 {
 125         int ret;
 126
 127         ret = gb_operation_sync(svc->connection,
 128                                 GB_SVC_TYPE_PWRMON_RAIL_NAMES_GET, NULL, 0,
 129                                 response, bufsize);
 130         if (ret) {
 131                 dev_err(&svc->dev, "failed to get rail names: %d\n", ret);
 132                 return ret;
 133         }
 134
 135         if (response->status != GB_SVC_OP_SUCCESS) {
 136                 dev_err(&svc->dev,
 137                         "SVC error while getting rail names: %u\n",
 138                         response->status);
 139                 return -EREMOTEIO;
 140         }
 141
 142         return 0;
 143 }
 144
 145 static int gb_svc_pwrmon_sample_get(struct gb_svc *svc, u8 rail_id,
 146                                     u8 measurement_type, u32 *value)
 147 {
 148         struct gb_svc_pwrmon_sample_get_request request;
 149         struct gb_svc_pwrmon_sample_get_response response;
 150         int ret;
 151
 152         request.rail_id = rail_id;
 153         request.measurement_type = measurement_type;
 154
 155         ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_PWRMON_SAMPLE_GET,
 156                                 &request, sizeof(request),
 157                                 &response, sizeof(response));
 158         if (ret) {
 159                 dev_err(&svc->dev, "failed to get rail sample: %d\n", ret);
 160                 return ret;
 161         }
 162
 163         if (response.result) {
 164                 dev_err(&svc->dev,
 165                         "UniPro error while getting rail power sample (%d %d): %d\n",
 166                         rail_id, measurement_type, response.result);
 167                 switch (response.result) {
 168                 case GB_SVC_PWRMON_GET_SAMPLE_INVAL:
 169                         return -EINVAL;
 170                 case GB_SVC_PWRMON_GET_SAMPLE_NOSUPP:
 171                         return -ENOMSG;
 172                 default:
 173                         return -EREMOTEIO;
 174                 }
 175         }
 176
 177         *value = le32_to_cpu(response.measurement);
 178
 179         return 0;
 180 }
 181
 182 int gb_svc_pwrmon_intf_sample_get(struct gb_svc *svc, u8 intf_id,
 183                                   u8 measurement_type, u32 *value)
 184 {
 185         struct gb_svc_pwrmon_intf_sample_get_request request;
 186         struct gb_svc_pwrmon_intf_sample_get_response response;
 187         int ret;
 188
 189         request.intf_id = intf_id;
 190         request.measurement_type = measurement_type;
 191
 192         ret = gb_operation_sync(svc->connection,
 193                                 GB_SVC_TYPE_PWRMON_INTF_SAMPLE_GET,
 194                                 &request, sizeof(request),
 195                                 &response, sizeof(response));
 196         if (ret) {
 197                 dev_err(&svc->dev, "failed to get intf sample: %d\n", ret);
 198                 return ret;
 199         }
 200
 201         if (response.result) {
 202                 dev_err(&svc->dev,
 203                         "UniPro error while getting intf power sample (%d %d): %d\n",
 204                         intf_id, measurement_type, response.result);
 205                 switch (response.result) {
 206                 case GB_SVC_PWRMON_GET_SAMPLE_INVAL:
 207                         return -EINVAL;
 208                 case GB_SVC_PWRMON_GET_SAMPLE_NOSUPP:
 209                         return -ENOMSG;
 210                 default:
 211                         return -EREMOTEIO;
 212                 }
 213         }
 214
 215         *value = le32_to_cpu(response.measurement);
 216
 217         return 0;
 218 }
 219
 220 static struct attribute *svc_attrs[] = {
 221         &dev_attr_endo_id.attr,
 222         &dev_attr_ap_intf_id.attr,
 223         &dev_attr_intf_eject.attr,
 224         &dev_attr_watchdog.attr,
 225         NULL,
 226 };
 227 ATTRIBUTE_GROUPS(svc);
 228
 229 int gb_svc_intf_device_id(struct gb_svc *svc, u8 intf_id, u8 device_id)
 230 {
 231         struct gb_svc_intf_device_id_request request;
 232
 233         request.intf_id = intf_id;
 234         request.device_id = device_id;
 235
 236         return gb_operation_sync(svc->connection, GB_SVC_TYPE_INTF_DEVICE_ID,
 237                                  &request, sizeof(request), NULL, 0);
 238 }
 239
 240 int gb_svc_intf_eject(struct gb_svc *svc, u8 intf_id)
 241 {
 242         struct gb_svc_intf_eject_request request;
 243         int ret;
 244
 245         request.intf_id = intf_id;
 246
 247         /*
 248          * The pulse width for module release in svc is long so we need to
 249          * increase the timeout so the operation will not return to soon.
 250          */
 251         ret = gb_operation_sync_timeout(svc->connection,
 252                                         GB_SVC_TYPE_INTF_EJECT, &request,
 253                                         sizeof(request), NULL, 0,
 254                                         SVC_INTF_EJECT_TIMEOUT);
 255         if (ret) {
 256                 dev_err(&svc->dev, "failed to eject interface %u\n", intf_id);
 257                 return ret;
 258         }
 259
 260         return 0;
 261 }
 262
 263 int gb_svc_intf_vsys_set(struct gb_svc *svc, u8 intf_id, bool enable)
 264 {
 265         struct gb_svc_intf_vsys_request request;
 266         struct gb_svc_intf_vsys_response response;
 267         int type, ret;
 268
 269         request.intf_id = intf_id;
 270
 271         if (enable)
 272                 type = GB_SVC_TYPE_INTF_VSYS_ENABLE;
 273         else
 274                 type = GB_SVC_TYPE_INTF_VSYS_DISABLE;
 275
 276         ret = gb_operation_sync(svc->connection, type,
 277                         &request, sizeof(request),
 278                         &response, sizeof(response));
 279         if (ret < 0)
 280                 return ret;
 281         if (response.result_code != GB_SVC_INTF_VSYS_OK)
 282                 return -EREMOTEIO;
 283         return 0;
 284 }
 285
 286 int gb_svc_intf_refclk_set(struct gb_svc *svc, u8 intf_id, bool enable)
 287 {
 288         struct gb_svc_intf_refclk_request request;
 289         struct gb_svc_intf_refclk_response response;
 290         int type, ret;
 291
 292         request.intf_id = intf_id;
 293
 294         if (enable)
 295                 type = GB_SVC_TYPE_INTF_REFCLK_ENABLE;
 296         else
 297                 type = GB_SVC_TYPE_INTF_REFCLK_DISABLE;
 298
 299         ret = gb_operation_sync(svc->connection, type,
 300                         &request, sizeof(request),
 301                         &response, sizeof(response));
 302         if (ret < 0)
 303                 return ret;
 304         if (response.result_code != GB_SVC_INTF_REFCLK_OK)
 305                 return -EREMOTEIO;
 306         return 0;
 307 }
 308
 309 int gb_svc_intf_unipro_set(struct gb_svc *svc, u8 intf_id, bool enable)
 310 {
 311         struct gb_svc_intf_unipro_request request;
 312         struct gb_svc_intf_unipro_response response;
 313         int type, ret;
 314
 315         request.intf_id = intf_id;
 316
 317         if (enable)
 318                 type = GB_SVC_TYPE_INTF_UNIPRO_ENABLE;
 319         else
 320                 type = GB_SVC_TYPE_INTF_UNIPRO_DISABLE;
 321
 322         ret = gb_operation_sync(svc->connection, type,
 323                         &request, sizeof(request),
 324                         &response, sizeof(response));
 325         if (ret < 0)
 326                 return ret;
 327         if (response.result_code != GB_SVC_INTF_UNIPRO_OK)
 328                 return -EREMOTEIO;
 329         return 0;
 330 }
 331
 332 int gb_svc_intf_activate(struct gb_svc *svc, u8 intf_id, u8 *intf_type)
 333 {
 334         struct gb_svc_intf_activate_request request;
 335         struct gb_svc_intf_activate_response response;
 336         int ret;
 337
 338         request.intf_id = intf_id;
 339
 340         ret = gb_operation_sync_timeout(svc->connection,
 341                         GB_SVC_TYPE_INTF_ACTIVATE,
 342                         &request, sizeof(request),
 343                         &response, sizeof(response),
 344                         SVC_INTF_ACTIVATE_TIMEOUT);
 345         if (ret < 0)
 346                 return ret;
 347         if (response.status != GB_SVC_OP_SUCCESS) {
 348                 dev_err(&svc->dev, "failed to activate interface %u: %u\n",
 349                                 intf_id, response.status);
 350                 return -EREMOTEIO;
 351         }
 352
 353         *intf_type = response.intf_type;
 354
 355         return 0;
 356 }
 357
 358 int gb_svc_intf_resume(struct gb_svc *svc, u8 intf_id)
 359 {
 360         struct gb_svc_intf_resume_request request;
 361         struct gb_svc_intf_resume_response response;
 362         int ret;
 363
 364         request.intf_id = intf_id;
 365
 366         ret = gb_operation_sync_timeout(svc->connection,
 367                                         GB_SVC_TYPE_INTF_RESUME,
 368                                         &request, sizeof(request),
 369                                         &response, sizeof(response),
 370                                         SVC_INTF_RESUME_TIMEOUT);
 371         if (ret < 0) {
 372                 dev_err(&svc->dev, "failed to send interface resume %u: %d\n",
 373                         intf_id, ret);
 374                 return ret;
 375         }
 376
 377         if (response.status != GB_SVC_OP_SUCCESS) {
 378                 dev_err(&svc->dev, "failed to resume interface %u: %u\n",
 379                         intf_id, response.status);
 380                 return -EREMOTEIO;
 381         }
 382
 383         return 0;
 384 }
 385
 386 int gb_svc_dme_peer_get(struct gb_svc *svc, u8 intf_id, u16 attr, u16 selector,
 387                         u32 *value)
 388 {
 389         struct gb_svc_dme_peer_get_request request;
 390         struct gb_svc_dme_peer_get_response response;
 391         u16 result;
 392         int ret;
 393
 394         request.intf_id = intf_id;
 395         request.attr = cpu_to_le16(attr);
 396         request.selector = cpu_to_le16(selector);
 397
 398         ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_DME_PEER_GET,
 399                                 &request, sizeof(request),
 400                                 &response, sizeof(response));
 401         if (ret) {
 402                 dev_err(&svc->dev, "failed to get DME attribute (%u 0x%04x %u): %d\n",
 403                                 intf_id, attr, selector, ret);
 404                 return ret;
 405         }
 406
 407         result = le16_to_cpu(response.result_code);
 408         if (result) {
 409                 dev_err(&svc->dev, "UniPro error while getting DME attribute (%u 0x%04x %u): %u\n",
 410                                 intf_id, attr, selector, result);
 411                 return -EREMOTEIO;
 412         }
 413
 414         if (value)
 415                 *value = le32_to_cpu(response.attr_value);
 416
 417         return 0;
 418 }
 419 EXPORT_SYMBOL_GPL(gb_svc_dme_peer_get);
 420
 421 int gb_svc_dme_peer_set(struct gb_svc *svc, u8 intf_id, u16 attr, u16 selector,
 422                         u32 value)
 423 {
 424         struct gb_svc_dme_peer_set_request request;
 425         struct gb_svc_dme_peer_set_response response;
 426         u16 result;
 427         int ret;
 428
 429         request.intf_id = intf_id;
 430         request.attr = cpu_to_le16(attr);
 431         request.selector = cpu_to_le16(selector);
 432         request.value = cpu_to_le32(value);
 433
 434         ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_DME_PEER_SET,
 435                                 &request, sizeof(request),
 436                                 &response, sizeof(response));
 437         if (ret) {
 438                 dev_err(&svc->dev, "failed to set DME attribute (%u 0x%04x %u %u): %d\n",
 439                                 intf_id, attr, selector, value, ret);
 440                 return ret;
 441         }
 442
 443         result = le16_to_cpu(response.result_code);
 444         if (result) {
 445                 dev_err(&svc->dev, "UniPro error while setting DME attribute (%u 0x%04x %u %u): %u\n",
 446                                 intf_id, attr, selector, value, result);
 447                 return -EREMOTEIO;
 448         }
 449
 450         return 0;
 451 }
 452 EXPORT_SYMBOL_GPL(gb_svc_dme_peer_set);
 453
 454 int gb_svc_connection_create(struct gb_svc *svc,
 455                                 u8 intf1_id, u16 cport1_id,
 456                                 u8 intf2_id, u16 cport2_id,
 457                                 u8 cport_flags)
 458 {
 459         struct gb_svc_conn_create_request request;
 460
 461         request.intf1_id = intf1_id;
 462         request.cport1_id = cpu_to_le16(cport1_id);
 463         request.intf2_id = intf2_id;
 464         request.cport2_id = cpu_to_le16(cport2_id);
 465         request.tc = 0;         /* TC0 */
 466         request.flags = cport_flags;
 467
 468         return gb_operation_sync(svc->connection, GB_SVC_TYPE_CONN_CREATE,
 469                                  &request, sizeof(request), NULL, 0);
 470 }
 471 EXPORT_SYMBOL_GPL(gb_svc_connection_create);
 472
 473 void gb_svc_connection_destroy(struct gb_svc *svc, u8 intf1_id, u16 cport1_id,
 474                                u8 intf2_id, u16 cport2_id)
 475 {
 476         struct gb_svc_conn_destroy_request request;
 477         struct gb_connection *connection = svc->connection;
 478         int ret;
 479
 480         request.intf1_id = intf1_id;
 481         request.cport1_id = cpu_to_le16(cport1_id);
 482         request.intf2_id = intf2_id;
 483         request.cport2_id = cpu_to_le16(cport2_id);
 484
 485         ret = gb_operation_sync(connection, GB_SVC_TYPE_CONN_DESTROY,
 486                                 &request, sizeof(request), NULL, 0);
 487         if (ret) {
 488                 dev_err(&svc->dev, "failed to destroy connection (%u:%u %u:%u): %d\n",
 489                                 intf1_id, cport1_id, intf2_id, cport2_id, ret);
 490         }
 491 }
 492 EXPORT_SYMBOL_GPL(gb_svc_connection_destroy);
 493
 494 int gb_svc_timesync_enable(struct gb_svc *svc, u8 count, u64 frame_time,
 495                            u32 strobe_delay, u32 refclk)
 496 {
 497         struct gb_connection *connection = svc->connection;
 498         struct gb_svc_timesync_enable_request request;
 499
 500         request.count = count;
 501         request.frame_time = cpu_to_le64(frame_time);
 502         request.strobe_delay = cpu_to_le32(strobe_delay);
 503         request.refclk = cpu_to_le32(refclk);
 504         return gb_operation_sync(connection,
 505                                  GB_SVC_TYPE_TIMESYNC_ENABLE,
 506                                  &request, sizeof(request), NULL, 0);
 507 }
 508 EXPORT_SYMBOL_GPL(gb_svc_timesync_enable);
 509
 510 int gb_svc_timesync_disable(struct gb_svc *svc)
 511 {
 512         struct gb_connection *connection = svc->connection;
 513
 514         return gb_operation_sync(connection,
 515                                  GB_SVC_TYPE_TIMESYNC_DISABLE,
 516                                  NULL, 0, NULL, 0);
 517 }
 518 EXPORT_SYMBOL_GPL(gb_svc_timesync_disable);
 519
 520 int gb_svc_timesync_authoritative(struct gb_svc *svc, u64 *frame_time)
 521 {
 522         struct gb_connection *connection = svc->connection;
 523         struct gb_svc_timesync_authoritative_response response;
 524         int ret, i;
 525
 526         ret = gb_operation_sync(connection,
 527                                 GB_SVC_TYPE_TIMESYNC_AUTHORITATIVE, NULL, 0,
 528                                 &response, sizeof(response));
 529         if (ret < 0)
 530                 return ret;
 531
 532         for (i = 0; i < GB_TIMESYNC_MAX_STROBES; i++)
 533                 frame_time[i] = le64_to_cpu(response.frame_time[i]);
 534         return 0;
 535 }
 536 EXPORT_SYMBOL_GPL(gb_svc_timesync_authoritative);
 537
 538 int gb_svc_timesync_ping(struct gb_svc *svc, u64 *frame_time)
 539 {
 540         struct gb_connection *connection = svc->connection;
 541         struct gb_svc_timesync_ping_response response;
 542         int ret;
 543
 544         ret = gb_operation_sync(connection,
 545                                 GB_SVC_TYPE_TIMESYNC_PING,
 546                                 NULL, 0,
 547                                 &response, sizeof(response));
 548         if (ret < 0)
 549                 return ret;
 550
 551         *frame_time = le64_to_cpu(response.frame_time);
 552         return 0;
 553 }
 554 EXPORT_SYMBOL_GPL(gb_svc_timesync_ping);
 555
 556 int gb_svc_timesync_wake_pins_acquire(struct gb_svc *svc, u32 strobe_mask)
 557 {
 558         struct gb_connection *connection = svc->connection;
 559         struct gb_svc_timesync_wake_pins_acquire_request request;
 560
 561         request.strobe_mask = cpu_to_le32(strobe_mask);
 562         return gb_operation_sync(connection,
 563                                  GB_SVC_TYPE_TIMESYNC_WAKE_PINS_ACQUIRE,
 564                                  &request, sizeof(request),
 565                                  NULL, 0);
 566 }
 567 EXPORT_SYMBOL_GPL(gb_svc_timesync_wake_pins_acquire);
 568
 569 int gb_svc_timesync_wake_pins_release(struct gb_svc *svc)
 570 {
 571         struct gb_connection *connection = svc->connection;
 572
 573         return gb_operation_sync(connection,
 574                                  GB_SVC_TYPE_TIMESYNC_WAKE_PINS_RELEASE,
 575                                  NULL, 0, NULL, 0);
 576 }
 577 EXPORT_SYMBOL_GPL(gb_svc_timesync_wake_pins_release);
 578
 579 /* Creates bi-directional routes between the devices */
 580 int gb_svc_route_create(struct gb_svc *svc, u8 intf1_id, u8 dev1_id,
 581                                u8 intf2_id, u8 dev2_id)
 582 {
 583         struct gb_svc_route_create_request request;
 584
 585         request.intf1_id = intf1_id;
 586         request.dev1_id = dev1_id;
 587         request.intf2_id = intf2_id;
 588         request.dev2_id = dev2_id;
 589
 590         return gb_operation_sync(svc->connection, GB_SVC_TYPE_ROUTE_CREATE,
 591                                  &request, sizeof(request), NULL, 0);
 592 }
 593
 594 /* Destroys bi-directional routes between the devices */
 595 void gb_svc_route_destroy(struct gb_svc *svc, u8 intf1_id, u8 intf2_id)
 596 {
 597         struct gb_svc_route_destroy_request request;
 598         int ret;
 599
 600         request.intf1_id = intf1_id;
 601         request.intf2_id = intf2_id;
 602
 603         ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_ROUTE_DESTROY,
 604                                 &request, sizeof(request), NULL, 0);
 605         if (ret) {
 606                 dev_err(&svc->dev, "failed to destroy route (%u %u): %d\n",
 607                                 intf1_id, intf2_id, ret);
 608         }
 609 }
 610
 611 int gb_svc_intf_set_power_mode(struct gb_svc *svc, u8 intf_id, u8 hs_series,
 612                                u8 tx_mode, u8 tx_gear, u8 tx_nlanes,
 613                                u8 tx_amplitude, u8 tx_hs_equalizer,
 614                                u8 rx_mode, u8 rx_gear, u8 rx_nlanes,
 615                                u8 flags, u32 quirks,
 616                                struct gb_svc_l2_timer_cfg *local,
 617                                struct gb_svc_l2_timer_cfg *remote)
 618 {
 619         struct gb_svc_intf_set_pwrm_request request;
 620         struct gb_svc_intf_set_pwrm_response response;
 621         int ret;
 622         u16 result_code;
 623
 624         memset(&request, 0, sizeof(request));
 625
 626         request.intf_id = intf_id;
 627         request.hs_series = hs_series;
 628         request.tx_mode = tx_mode;
 629         request.tx_gear = tx_gear;
 630         request.tx_nlanes = tx_nlanes;
 631         request.tx_amplitude = tx_amplitude;
 632         request.tx_hs_equalizer = tx_hs_equalizer;
 633         request.rx_mode = rx_mode;
 634         request.rx_gear = rx_gear;
 635         request.rx_nlanes = rx_nlanes;
 636         request.flags = flags;
 637         request.quirks = cpu_to_le32(quirks);
 638         if (local)
 639                 request.local_l2timerdata = *local;
 640         if (remote)
 641                 request.remote_l2timerdata = *remote;
 642
 643         ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_INTF_SET_PWRM,
 644                                 &request, sizeof(request),
 645                                 &response, sizeof(response));
 646         if (ret < 0)
 647                 return ret;
 648
 649         result_code = response.result_code;
 650         if (result_code != GB_SVC_SETPWRM_PWR_LOCAL) {
 651                 dev_err(&svc->dev, "set power mode = %d\n", result_code);
 652                 return -EIO;
 653         }
 654
 655         return 0;
 656 }
 657 EXPORT_SYMBOL_GPL(gb_svc_intf_set_power_mode);
 658
 659 int gb_svc_intf_set_power_mode_hibernate(struct gb_svc *svc, u8 intf_id)
 660 {
 661         struct gb_svc_intf_set_pwrm_request request;
 662         struct gb_svc_intf_set_pwrm_response response;
 663         int ret;
 664         u16 result_code;
 665
 666         memset(&request, 0, sizeof(request));
 667
 668         request.intf_id = intf_id;
 669         request.hs_series = GB_SVC_UNIPRO_HS_SERIES_A;
 670         request.tx_mode = GB_SVC_UNIPRO_HIBERNATE_MODE;
 671         request.rx_mode = GB_SVC_UNIPRO_HIBERNATE_MODE;
 672
 673         ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_INTF_SET_PWRM,
 674                                 &request, sizeof(request),
 675                                 &response, sizeof(response));
 676         if (ret < 0) {
 677                 dev_err(&svc->dev,
 678                         "failed to send set power mode operation to interface %u: %d\n",
 679                         intf_id, ret);
 680                 return ret;
 681         }
 682
 683         result_code = response.result_code;
 684         if (result_code != GB_SVC_SETPWRM_PWR_OK) {
 685                 dev_err(&svc->dev,
 686                         "failed to hibernate the link for interface %u: %u\n",
 687                         intf_id, result_code);
 688                 return -EIO;
 689         }
 690
 691         return 0;
 692 }
 693
 694 int gb_svc_ping(struct gb_svc *svc)
 695 {
 696         return gb_operation_sync_timeout(svc->connection, GB_SVC_TYPE_PING,
 697                                          NULL, 0, NULL, 0,
 698                                          GB_OPERATION_TIMEOUT_DEFAULT * 2);
 699 }
 700 EXPORT_SYMBOL_GPL(gb_svc_ping);
 701
 702 static int gb_svc_version_request(struct gb_operation *op)
 703 {
 704         struct gb_connection *connection = op->connection;
 705         struct gb_svc *svc = gb_connection_get_data(connection);
 706         struct gb_svc_version_request *request;
 707         struct gb_svc_version_response *response;
 708
 709         if (op->request->payload_size < sizeof(*request)) {
 710                 dev_err(&svc->dev, "short version request (%zu < %zu)\n",
 711                                 op->request->payload_size,
 712                                 sizeof(*request));
 713                 return -EINVAL;
 714         }
 715
 716         request = op->request->payload;
 717
 718         if (request->major > GB_SVC_VERSION_MAJOR) {
 719                 dev_warn(&svc->dev, "unsupported major version (%u > %u)\n",
 720                                 request->major, GB_SVC_VERSION_MAJOR);
 721                 return -ENOTSUPP;
 722         }
 723
 724         svc->protocol_major = request->major;
 725         svc->protocol_minor = request->minor;
 726
 727         if (!gb_operation_response_alloc(op, sizeof(*response), GFP_KERNEL))
 728                 return -ENOMEM;
 729
 730         response = op->response->payload;
 731         response->major = svc->protocol_major;
 732         response->minor = svc->protocol_minor;
 733
 734         return 0;
 735 }
 736
 737 static ssize_t pwr_debugfs_voltage_read(struct file *file, char __user *buf,
 738                                         size_t len, loff_t *offset)
 739 {
 740         struct svc_debugfs_pwrmon_rail *pwrmon_rails = file->f_inode->i_private;
 741         struct gb_svc *svc = pwrmon_rails->svc;
 742         int ret, desc;
 743         u32 value;
 744         char buff[16];
 745
 746         ret = gb_svc_pwrmon_sample_get(svc, pwrmon_rails->id,
 747                                        GB_SVC_PWRMON_TYPE_VOL, &value);
 748         if (ret) {
 749                 dev_err(&svc->dev,
 750                         "failed to get voltage sample %u: %d\n",
 751                         pwrmon_rails->id, ret);
 752                 return ret;
 753         }
 754
 755         desc = scnprintf(buff, sizeof(buff), "%u\n", value);
 756
 757         return simple_read_from_buffer(buf, len, offset, buff, desc);
 758 }
 759
 760 static ssize_t pwr_debugfs_current_read(struct file *file, char __user *buf,
 761                                         size_t len, loff_t *offset)
 762 {
 763         struct svc_debugfs_pwrmon_rail *pwrmon_rails = file->f_inode->i_private;
 764         struct gb_svc *svc = pwrmon_rails->svc;
 765         int ret, desc;
 766         u32 value;
 767         char buff[16];
 768
 769         ret = gb_svc_pwrmon_sample_get(svc, pwrmon_rails->id,
 770                                        GB_SVC_PWRMON_TYPE_CURR, &value);
 771         if (ret) {
 772                 dev_err(&svc->dev,
 773                         "failed to get current sample %u: %d\n",
 774                         pwrmon_rails->id, ret);
 775                 return ret;
 776         }
 777
 778         desc = scnprintf(buff, sizeof(buff), "%u\n", value);
 779
 780         return simple_read_from_buffer(buf, len, offset, buff, desc);
 781 }
 782
 783 static ssize_t pwr_debugfs_power_read(struct file *file, char __user *buf,
 784                                       size_t len, loff_t *offset)
 785 {
 786         struct svc_debugfs_pwrmon_rail *pwrmon_rails = file->f_inode->i_private;
 787         struct gb_svc *svc = pwrmon_rails->svc;
 788         int ret, desc;
 789         u32 value;
 790         char buff[16];
 791
 792         ret = gb_svc_pwrmon_sample_get(svc, pwrmon_rails->id,
 793                                        GB_SVC_PWRMON_TYPE_PWR, &value);
 794         if (ret) {
 795                 dev_err(&svc->dev, "failed to get power sample %u: %d\n",
 796                         pwrmon_rails->id, ret);
 797                 return ret;
 798         }
 799
 800         desc = scnprintf(buff, sizeof(buff), "%u\n", value);
 801
 802         return simple_read_from_buffer(buf, len, offset, buff, desc);
 803 }
 804
 805 static const struct file_operations pwrmon_debugfs_voltage_fops = {
 806         .read           = pwr_debugfs_voltage_read,
 807 };
 808
 809 static const struct file_operations pwrmon_debugfs_current_fops = {
 810         .read           = pwr_debugfs_current_read,
 811 };
 812
 813 static const struct file_operations pwrmon_debugfs_power_fops = {
 814         .read           = pwr_debugfs_power_read,
 815 };
 816
 817 static void gb_svc_pwrmon_debugfs_init(struct gb_svc *svc)
 818 {
 819         int i;
 820         size_t bufsize;
 821         struct dentry *dent;
 822         struct gb_svc_pwrmon_rail_names_get_response *rail_names;
 823         u8 rail_count;
 824
 825         dent = debugfs_create_dir("pwrmon", svc->debugfs_dentry);
 826         if (IS_ERR_OR_NULL(dent))
 827                 return;
 828
 829         if (gb_svc_pwrmon_rail_count_get(svc, &rail_count))
 830                 goto err_pwrmon_debugfs;
 831
 832         if (!rail_count || rail_count > GB_SVC_PWRMON_MAX_RAIL_COUNT)
 833                 goto err_pwrmon_debugfs;
 834
 835         bufsize = sizeof(*rail_names) +
 836                 GB_SVC_PWRMON_RAIL_NAME_BUFSIZE * rail_count;
 837
 838         rail_names = kzalloc(bufsize, GFP_KERNEL);
 839         if (!rail_names)
 840                 goto err_pwrmon_debugfs;
 841
 842         svc->pwrmon_rails = kcalloc(rail_count, sizeof(*svc->pwrmon_rails),
 843                                     GFP_KERNEL);
 844         if (!svc->pwrmon_rails)
 845                 goto err_pwrmon_debugfs_free;
 846
 847         if (gb_svc_pwrmon_rail_names_get(svc, rail_names, bufsize))
 848                 goto err_pwrmon_debugfs_free;
 849
 850         for (i = 0; i < rail_count; i++) {
 851                 struct dentry *dir;
 852                 struct svc_debugfs_pwrmon_rail *rail = &svc->pwrmon_rails[i];
 853                 char fname[GB_SVC_PWRMON_RAIL_NAME_BUFSIZE];
 854
 855                 snprintf(fname, sizeof(fname), "%s",
 856                          (char *)&rail_names->name[i]);
 857
 858                 rail->id = i;
 859                 rail->svc = svc;
 860
 861                 dir = debugfs_create_dir(fname, dent);
 862                 debugfs_create_file("voltage_now", S_IRUGO, dir, rail,
 863                                     &pwrmon_debugfs_voltage_fops);
 864                 debugfs_create_file("current_now", S_IRUGO, dir, rail,
 865                                     &pwrmon_debugfs_current_fops);
 866                 debugfs_create_file("power_now", S_IRUGO, dir, rail,
 867                                     &pwrmon_debugfs_power_fops);
 868         }
 869
 870         kfree(rail_names);
 871         return;
 872
 873 err_pwrmon_debugfs_free:
 874         kfree(rail_names);
 875         kfree(svc->pwrmon_rails);
 876         svc->pwrmon_rails = NULL;
 877
 878 err_pwrmon_debugfs:
 879         debugfs_remove(dent);
 880 }
 881
 882 static void gb_svc_debugfs_init(struct gb_svc *svc)
 883 {
 884         svc->debugfs_dentry = debugfs_create_dir(dev_name(&svc->dev),
 885                                                  gb_debugfs_get());
 886         gb_svc_pwrmon_debugfs_init(svc);
 887 }
 888
 889 static void gb_svc_debugfs_exit(struct gb_svc *svc)
 890 {
 891         debugfs_remove_recursive(svc->debugfs_dentry);
 892         kfree(svc->pwrmon_rails);
 893         svc->pwrmon_rails = NULL;
 894 }
 895
 896 static int gb_svc_hello(struct gb_operation *op)
 897 {
 898         struct gb_connection *connection = op->connection;
 899         struct gb_svc *svc = gb_connection_get_data(connection);
 900         struct gb_svc_hello_request *hello_request;
 901         int ret;
 902
 903         if (op->request->payload_size < sizeof(*hello_request)) {
 904                 dev_warn(&svc->dev, "short hello request (%zu < %zu)\n",
 905                                 op->request->payload_size,
 906                                 sizeof(*hello_request));
 907                 return -EINVAL;
 908         }
 909
 910         hello_request = op->request->payload;
 911         svc->endo_id = le16_to_cpu(hello_request->endo_id);
 912         svc->ap_intf_id = hello_request->interface_id;
 913
 914         ret = device_add(&svc->dev);
 915         if (ret) {
 916                 dev_err(&svc->dev, "failed to register svc device: %d\n", ret);
 917                 return ret;
 918         }
 919
 920         ret = gb_svc_watchdog_create(svc);
 921         if (ret) {
 922                 dev_err(&svc->dev, "failed to create watchdog: %d\n", ret);
 923                 goto err_unregister_device;
 924         }
 925
 926         gb_svc_debugfs_init(svc);
 927
 928         ret = gb_timesync_svc_add(svc);
 929         if (ret) {
 930                 dev_err(&svc->dev, "failed to add SVC to timesync: %d\n", ret);
 931                 gb_svc_debugfs_exit(svc);
 932                 goto err_unregister_device;
 933         }
 934
 935         return gb_svc_queue_deferred_request(op);
 936
 937 err_unregister_device:
 938         gb_svc_watchdog_destroy(svc);
 939         device_del(&svc->dev);
 940         return ret;
 941 }
 942
 943 static struct gb_interface *gb_svc_interface_lookup(struct gb_svc *svc,
 944                                                         u8 intf_id)
 945 {
 946         struct gb_host_device *hd = svc->hd;
 947         struct gb_module *module;
 948         size_t num_interfaces;
 949         u8 module_id;
 950
 951         list_for_each_entry(module, &hd->modules, hd_node) {
 952                 module_id = module->module_id;
 953                 num_interfaces = module->num_interfaces;
 954
 955                 if (intf_id >= module_id &&
 956                                 intf_id < module_id + num_interfaces) {
 957                         return module->interfaces[intf_id - module_id];
 958                 }
 959         }
 960
 961         return NULL;
 962 }
 963
 964 static struct gb_module *gb_svc_module_lookup(struct gb_svc *svc, u8 module_id)
 965 {
 966         struct gb_host_device *hd = svc->hd;
 967         struct gb_module *module;
 968
 969         list_for_each_entry(module, &hd->modules, hd_node) {
 970                 if (module->module_id == module_id)
 971                         return module;
 972         }
 973
 974         return NULL;
 975 }
 976
 977 static void gb_svc_process_hello_deferred(struct gb_operation *operation)
 978 {
 979         struct gb_connection *connection = operation->connection;
 980         struct gb_svc *svc = gb_connection_get_data(connection);
 981         int ret;
 982
 983         /*
 984          * XXX This is a hack/work-around to reconfigure the APBridgeA-Switch
 985          * link to PWM G2, 1 Lane, Slow Auto, so that it has sufficient
 986          * bandwidth for 3 audio streams plus boot-over-UniPro of a hot-plugged
 987          * module.
 988          *
 989          * The code should be removed once SW-2217, Heuristic for UniPro
 990          * Power Mode Changes is resolved.
 991          */
 992         ret = gb_svc_intf_set_power_mode(svc, svc->ap_intf_id,
 993                                         GB_SVC_UNIPRO_HS_SERIES_A,
 994                                         GB_SVC_UNIPRO_SLOW_AUTO_MODE,
 995                                         2, 1,
 996                                         GB_SVC_SMALL_AMPLITUDE, GB_SVC_NO_DE_EMPHASIS,
 997                                         GB_SVC_UNIPRO_SLOW_AUTO_MODE,
 998                                         2, 1,
 999                                         0, 0,
1000                                         NULL, NULL);
1001
1002         if (ret)
1003                 dev_warn(&svc->dev,
1004                         "power mode change failed on AP to switch link: %d\n",
1005                         ret);
1006 }
1007
1008 static void gb_svc_process_module_inserted(struct gb_operation *operation)
1009 {
1010         struct gb_svc_module_inserted_request *request;
1011         struct gb_connection *connection = operation->connection;
1012         struct gb_svc *svc = gb_connection_get_data(connection);
1013         struct gb_host_device *hd = svc->hd;
1014         struct gb_module *module;
1015         size_t num_interfaces;
1016         u8 module_id;
1017         u16 flags;
1018         int ret;
1019
1020         /* The request message size has already been verified. */
1021         request = operation->request->payload;
1022         module_id = request->primary_intf_id;
1023         num_interfaces = request->intf_count;
1024         flags = le16_to_cpu(request->flags);
1025
1026         dev_dbg(&svc->dev, "%s - id = %u, num_interfaces = %zu, flags = 0x%04x\n",
1027                         __func__, module_id, num_interfaces, flags);
1028
1029         if (flags & GB_SVC_MODULE_INSERTED_FLAG_NO_PRIMARY) {
1030                 dev_warn(&svc->dev, "no primary interface detected on module %u\n",
1031                                 module_id);
1032         }
1033
1034         module = gb_svc_module_lookup(svc, module_id);
1035         if (module) {
1036                 dev_warn(&svc->dev, "unexpected module-inserted event %u\n",
1037                                 module_id);
1038                 return;
1039         }
1040
1041         module = gb_module_create(hd, module_id, num_interfaces);
1042         if (!module) {
1043                 dev_err(&svc->dev, "failed to create module\n");
1044                 return;
1045         }
1046
1047         ret = gb_module_add(module);
1048         if (ret) {
1049                 gb_module_put(module);
1050                 return;
1051         }
1052
1053         list_add(&module->hd_node, &hd->modules);
1054 }
1055
1056 static void gb_svc_process_module_removed(struct gb_operation *operation)
1057 {
1058         struct gb_svc_module_removed_request *request;
1059         struct gb_connection *connection = operation->connection;
1060         struct gb_svc *svc = gb_connection_get_data(connection);
1061         struct gb_module *module;
1062         u8 module_id;
1063
1064         /* The request message size has already been verified. */
1065         request = operation->request->payload;
1066         module_id = request->primary_intf_id;
1067
1068         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, module_id);
1069
1070         module = gb_svc_module_lookup(svc, module_id);
1071         if (!module) {
1072                 dev_warn(&svc->dev, "unexpected module-removed event %u\n",
1073                                 module_id);
1074                 return;
1075         }
1076
1077         module->disconnected = true;
1078
1079         gb_module_del(module);
1080         list_del(&module->hd_node);
1081         gb_module_put(module);
1082 }
1083
1084 static void gb_svc_process_intf_mailbox_event(struct gb_operation *operation)
1085 {
1086         struct gb_svc_intf_mailbox_event_request *request;
1087         struct gb_connection *connection = operation->connection;
1088         struct gb_svc *svc = gb_connection_get_data(connection);
1089         struct gb_interface *intf;
1090         u8 intf_id;
1091         u16 result_code;
1092         u32 mailbox;
1093
1094         /* The request message size has already been verified. */
1095         request = operation->request->payload;
1096         intf_id = request->intf_id;
1097         result_code = le16_to_cpu(request->result_code);
1098         mailbox = le32_to_cpu(request->mailbox);
1099
1100         dev_dbg(&svc->dev, "%s - id = %u, result = 0x%04x, mailbox = 0x%08x\n",
1101                         __func__, intf_id, result_code, mailbox);
1102
1103         intf = gb_svc_interface_lookup(svc, intf_id);
1104         if (!intf) {
1105                 dev_warn(&svc->dev, "unexpected mailbox event %u\n", intf_id);
1106                 return;
1107         }
1108
1109         gb_interface_mailbox_event(intf, result_code, mailbox);
1110 }
1111
1112 static void gb_svc_process_deferred_request(struct work_struct *work)
1113 {
1114         struct gb_svc_deferred_request *dr;
1115         struct gb_operation *operation;
1116         struct gb_svc *svc;
1117         u8 type;
1118
1119         dr = container_of(work, struct gb_svc_deferred_request, work);
1120         operation = dr->operation;
1121         svc = gb_connection_get_data(operation->connection);
1122         type = operation->request->header->type;
1123
1124         switch (type) {
1125         case GB_SVC_TYPE_SVC_HELLO:
1126                 gb_svc_process_hello_deferred(operation);
1127                 break;
1128         case GB_SVC_TYPE_MODULE_INSERTED:
1129                 gb_svc_process_module_inserted(operation);
1130                 break;
1131         case GB_SVC_TYPE_MODULE_REMOVED:
1132                 gb_svc_process_module_removed(operation);
1133                 break;
1134         case GB_SVC_TYPE_INTF_MAILBOX_EVENT:
1135                 gb_svc_process_intf_mailbox_event(operation);
1136                 break;
1137         default:
1138                 dev_err(&svc->dev, "bad deferred request type: 0x%02x\n", type);
1139         }
1140
1141         gb_operation_put(operation);
1142         kfree(dr);
1143 }
1144
1145 static int gb_svc_queue_deferred_request(struct gb_operation *operation)
1146 {
1147         struct gb_svc *svc = gb_connection_get_data(operation->connection);
1148         struct gb_svc_deferred_request *dr;
1149
1150         dr = kmalloc(sizeof(*dr), GFP_KERNEL);
1151         if (!dr)
1152                 return -ENOMEM;
1153
1154         gb_operation_get(operation);
1155
1156         dr->operation = operation;
1157         INIT_WORK(&dr->work, gb_svc_process_deferred_request);
1158
1159         queue_work(svc->wq, &dr->work);
1160
1161         return 0;
1162 }
1163
1164 static int gb_svc_intf_reset_recv(struct gb_operation *op)
1165 {
1166         struct gb_svc *svc = gb_connection_get_data(op->connection);
1167         struct gb_message *request = op->request;
1168         struct gb_svc_intf_reset_request *reset;
1169         u8 intf_id;
1170
1171         if (request->payload_size < sizeof(*reset)) {
1172                 dev_warn(&svc->dev, "short reset request received (%zu < %zu)\n",
1173                                 request->payload_size, sizeof(*reset));
1174                 return -EINVAL;
1175         }
1176         reset = request->payload;
1177
1178         intf_id = reset->intf_id;
1179
1180         /* FIXME Reset the interface here */
1181
1182         return 0;
1183 }
1184
1185 static int gb_svc_module_inserted_recv(struct gb_operation *op)
1186 {
1187         struct gb_svc *svc = gb_connection_get_data(op->connection);
1188         struct gb_svc_module_inserted_request *request;
1189
1190         if (op->request->payload_size < sizeof(*request)) {
1191                 dev_warn(&svc->dev, "short module-inserted request received (%zu < %zu)\n",
1192                                 op->request->payload_size, sizeof(*request));
1193                 return -EINVAL;
1194         }
1195
1196         request = op->request->payload;
1197
1198         dev_dbg(&svc->dev, "%s - id = %u\n", __func__,
1199                         request->primary_intf_id);
1200
1201         return gb_svc_queue_deferred_request(op);
1202 }
1203
1204 static int gb_svc_module_removed_recv(struct gb_operation *op)
1205 {
1206         struct gb_svc *svc = gb_connection_get_data(op->connection);
1207         struct gb_svc_module_removed_request *request;
1208
1209         if (op->request->payload_size < sizeof(*request)) {
1210                 dev_warn(&svc->dev, "short module-removed request received (%zu < %zu)\n",
1211                                 op->request->payload_size, sizeof(*request));
1212                 return -EINVAL;
1213         }
1214
1215         request = op->request->payload;
1216
1217         dev_dbg(&svc->dev, "%s - id = %u\n", __func__,
1218                         request->primary_intf_id);
1219
1220         return gb_svc_queue_deferred_request(op);
1221 }
1222
1223 static int gb_svc_intf_mailbox_event_recv(struct gb_operation *op)
1224 {
1225         struct gb_svc *svc = gb_connection_get_data(op->connection);
1226         struct gb_svc_intf_mailbox_event_request *request;
1227
1228         if (op->request->payload_size < sizeof(*request)) {
1229                 dev_warn(&svc->dev, "short mailbox request received (%zu < %zu)\n",
1230                                 op->request->payload_size, sizeof(*request));
1231                 return -EINVAL;
1232         }
1233
1234         request = op->request->payload;
1235
1236         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, request->intf_id);
1237
1238         return gb_svc_queue_deferred_request(op);
1239 }
1240
1241 static int gb_svc_request_handler(struct gb_operation *op)
1242 {
1243         struct gb_connection *connection = op->connection;
1244         struct gb_svc *svc = gb_connection_get_data(connection);
1245         u8 type = op->type;
1246         int ret = 0;
1247
1248         /*
1249          * SVC requests need to follow a specific order (at least initially) and
1250          * below code takes care of enforcing that. The expected order is:
1251          * - PROTOCOL_VERSION
1252          * - SVC_HELLO
1253          * - Any other request, but the earlier two.
1254          *
1255          * Incoming requests are guaranteed to be serialized and so we don't
1256          * need to protect 'state' for any races.
1257          */
1258         switch (type) {
1259         case GB_SVC_TYPE_PROTOCOL_VERSION:
1260                 if (svc->state != GB_SVC_STATE_RESET)
1261                         ret = -EINVAL;
1262                 break;
1263         case GB_SVC_TYPE_SVC_HELLO:
1264                 if (svc->state != GB_SVC_STATE_PROTOCOL_VERSION)
1265                         ret = -EINVAL;
1266                 break;
1267         default:
1268                 if (svc->state != GB_SVC_STATE_SVC_HELLO)
1269                         ret = -EINVAL;
1270                 break;
1271         }
1272
1273         if (ret) {
1274                 dev_warn(&svc->dev, "unexpected request 0x%02x received (state %u)\n",
1275                                 type, svc->state);
1276                 return ret;
1277         }
1278
1279         switch (type) {
1280         case GB_SVC_TYPE_PROTOCOL_VERSION:
1281                 ret = gb_svc_version_request(op);
1282                 if (!ret)
1283                         svc->state = GB_SVC_STATE_PROTOCOL_VERSION;
1284                 return ret;
1285         case GB_SVC_TYPE_SVC_HELLO:
1286                 ret = gb_svc_hello(op);
1287                 if (!ret)
1288                         svc->state = GB_SVC_STATE_SVC_HELLO;
1289                 return ret;
1290         case GB_SVC_TYPE_INTF_RESET:
1291                 return gb_svc_intf_reset_recv(op);
1292         case GB_SVC_TYPE_MODULE_INSERTED:
1293                 return gb_svc_module_inserted_recv(op);
1294         case GB_SVC_TYPE_MODULE_REMOVED:
1295                 return gb_svc_module_removed_recv(op);
1296         case GB_SVC_TYPE_INTF_MAILBOX_EVENT:
1297                 return gb_svc_intf_mailbox_event_recv(op);
1298         default:
1299                 dev_warn(&svc->dev, "unsupported request 0x%02x\n", type);
1300                 return -EINVAL;
1301         }
1302 }
1303
1304 static void gb_svc_release(struct device *dev)
1305 {
1306         struct gb_svc *svc = to_gb_svc(dev);
1307
1308         if (svc->connection)
1309                 gb_connection_destroy(svc->connection);
1310         ida_destroy(&svc->device_id_map);
1311         destroy_workqueue(svc->wq);
1312         kfree(svc);
1313 }
1314
1315 struct device_type greybus_svc_type = {
1316         .name           = "greybus_svc",
1317         .release        = gb_svc_release,
1318 };
1319
1320 struct gb_svc *gb_svc_create(struct gb_host_device *hd)
1321 {
1322         struct gb_svc *svc;
1323
1324         svc = kzalloc(sizeof(*svc), GFP_KERNEL);
1325         if (!svc)
1326                 return NULL;
1327
1328         svc->wq = alloc_workqueue("%s:svc", WQ_UNBOUND, 1, dev_name(&hd->dev));
1329         if (!svc->wq) {
1330                 kfree(svc);
1331                 return NULL;
1332         }
1333
1334         svc->dev.parent = &hd->dev;
1335         svc->dev.bus = &greybus_bus_type;
1336         svc->dev.type = &greybus_svc_type;
1337         svc->dev.groups = svc_groups;
1338         svc->dev.dma_mask = svc->dev.parent->dma_mask;
1339         device_initialize(&svc->dev);
1340
1341         dev_set_name(&svc->dev, "%d-svc", hd->bus_id);
1342
1343         ida_init(&svc->device_id_map);
1344         svc->state = GB_SVC_STATE_RESET;
1345         svc->hd = hd;
1346
1347         svc->connection = gb_connection_create_static(hd, GB_SVC_CPORT_ID,
1348                                                 gb_svc_request_handler);
1349         if (IS_ERR(svc->connection)) {
1350                 dev_err(&svc->dev, "failed to create connection: %ld\n",
1351                                 PTR_ERR(svc->connection));
1352                 goto err_put_device;
1353         }
1354
1355         gb_connection_set_data(svc->connection, svc);
1356
1357         return svc;
1358
1359 err_put_device:
1360         put_device(&svc->dev);
1361         return NULL;
1362 }
1363
1364 int gb_svc_add(struct gb_svc *svc)
1365 {
1366         int ret;
1367
1368         /*
1369          * The SVC protocol is currently driven by the SVC, so the SVC device
1370          * is added from the connection request handler when enough
1371          * information has been received.
1372          */
1373         ret = gb_connection_enable(svc->connection);
1374         if (ret)
1375                 return ret;
1376
1377         return 0;
1378 }
1379
1380 static void gb_svc_remove_modules(struct gb_svc *svc)
1381 {
1382         struct gb_host_device *hd = svc->hd;
1383         struct gb_module *module, *tmp;
1384
1385         list_for_each_entry_safe(module, tmp, &hd->modules, hd_node) {
1386                 gb_module_del(module);
1387                 list_del(&module->hd_node);
1388                 gb_module_put(module);
1389         }
1390 }
1391
1392 void gb_svc_del(struct gb_svc *svc)
1393 {
1394         gb_connection_disable_rx(svc->connection);
1395
1396         /*
1397          * The SVC device may have been registered from the request handler.
1398          */
1399         if (device_is_registered(&svc->dev)) {
1400                 gb_timesync_svc_remove(svc);
1401                 gb_svc_debugfs_exit(svc);
1402                 gb_svc_watchdog_destroy(svc);
1403                 device_del(&svc->dev);
1404         }
1405
1406         flush_workqueue(svc->wq);
1407
1408         gb_svc_remove_modules(svc);
1409
1410         gb_connection_disable(svc->connection);
1411 }
1412
1413 void gb_svc_put(struct gb_svc *svc)
1414 {
1415         put_device(&svc->dev);
1416 }