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