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
 585         dent = debugfs_create_dir("pwrmon", svc->debugfs_dentry);
 586         if (IS_ERR_OR_NULL(dent))
 587                 return;
 588
 589         if (gb_svc_pwrmon_rail_count_get(svc, &svc->rail_count))
 590                 goto err_pwrmon_debugfs;
 591
 592         if (!svc->rail_count || svc->rail_count > GB_SVC_PWRMON_MAX_RAIL_COUNT)
 593                 goto err_pwrmon_debugfs;
 594
 595         bufsize = GB_SVC_PWRMON_RAIL_NAME_BUFSIZE * svc->rail_count;
 596
 597         svc->rail_names = kzalloc(bufsize, GFP_KERNEL);
 598         if (!svc->rail_names)
 599                 goto err_pwrmon_debugfs;
 600
 601         svc->pwrmon_rails = kcalloc(svc->rail_count, sizeof(*svc->pwrmon_rails),
 602                                     GFP_KERNEL);
 603         if (!svc->pwrmon_rails)
 604                 goto err_pwrmon_debugfs_free;
 605
 606         if (gb_svc_pwrmon_rail_names_get(svc, svc->rail_names, bufsize))
 607                 goto err_pwrmon_debugfs_free;
 608
 609         for (i = 0; i < svc->rail_count; i++) {
 610                 struct dentry *dir;
 611                 struct svc_debugfs_pwrmon_rail *rail = &svc->pwrmon_rails[i];
 612                 char fname[GB_SVC_PWRMON_RAIL_NAME_BUFSIZE];
 613
 614                 snprintf(fname, sizeof(fname), "%s",
 615                          (char *)&svc->rail_names->name[i]);
 616
 617                 rail->id = i;
 618                 rail->svc = svc;
 619
 620                 dir = debugfs_create_dir(fname, dent);
 621                 debugfs_create_file("voltage_now", S_IRUGO, dir, rail,
 622                                     &pwrmon_debugfs_voltage_fops);
 623                 debugfs_create_file("current_now", S_IRUGO, dir, rail,
 624                                     &pwrmon_debugfs_current_fops);
 625                 debugfs_create_file("power_now", S_IRUGO, dir, rail,
 626                                     &pwrmon_debugfs_power_fops);
 627         };
 628         return;
 629
 630 err_pwrmon_debugfs_free:
 631         kfree(svc->rail_names);
 632         svc->rail_names = NULL;
 633
 634         kfree(svc->pwrmon_rails);
 635         svc->pwrmon_rails = NULL;
 636
 637 err_pwrmon_debugfs:
 638         debugfs_remove(dent);
 639 }
 640
 641 static void gb_svc_debugfs_init(struct gb_svc *svc)
 642 {
 643         svc->debugfs_dentry = debugfs_create_dir(dev_name(&svc->dev),
 644                                                  gb_debugfs_get());
 645         gb_svc_pwrmon_debugfs_init(svc);
 646 }
 647
 648 static void gb_svc_debugfs_exit(struct gb_svc *svc)
 649 {
 650         debugfs_remove_recursive(svc->debugfs_dentry);
 651         kfree(svc->rail_names);
 652 }
 653
 654 static int gb_svc_hello(struct gb_operation *op)
 655 {
 656         struct gb_connection *connection = op->connection;
 657         struct gb_svc *svc = gb_connection_get_data(connection);
 658         struct gb_svc_hello_request *hello_request;
 659         int ret;
 660
 661         if (op->request->payload_size < sizeof(*hello_request)) {
 662                 dev_warn(&svc->dev, "short hello request (%zu < %zu)\n",
 663                                 op->request->payload_size,
 664                                 sizeof(*hello_request));
 665                 return -EINVAL;
 666         }
 667
 668         hello_request = op->request->payload;
 669         svc->endo_id = le16_to_cpu(hello_request->endo_id);
 670         svc->ap_intf_id = hello_request->interface_id;
 671
 672         ret = device_add(&svc->dev);
 673         if (ret) {
 674                 dev_err(&svc->dev, "failed to register svc device: %d\n", ret);
 675                 return ret;
 676         }
 677
 678         ret = input_register_device(svc->input);
 679         if (ret) {
 680                 dev_err(&svc->dev, "failed to register input: %d\n", ret);
 681                 device_del(&svc->dev);
 682                 return ret;
 683         }
 684
 685         ret = gb_svc_watchdog_create(svc);
 686         if (ret) {
 687                 dev_err(&svc->dev, "failed to create watchdog: %d\n", ret);
 688                 input_unregister_device(svc->input);
 689                 device_del(&svc->dev);
 690                 return ret;
 691         }
 692
 693         gb_svc_debugfs_init(svc);
 694
 695         return 0;
 696 }
 697
 698 static struct gb_interface *gb_svc_interface_lookup(struct gb_svc *svc,
 699                                                         u8 intf_id)
 700 {
 701         struct gb_host_device *hd = svc->hd;
 702         struct gb_module *module;
 703         size_t num_interfaces;
 704         u8 module_id;
 705
 706         list_for_each_entry(module, &hd->modules, hd_node) {
 707                 module_id = module->module_id;
 708                 num_interfaces = module->num_interfaces;
 709
 710                 if (intf_id >= module_id &&
 711                                 intf_id < module_id + num_interfaces) {
 712                         return module->interfaces[intf_id - module_id];
 713                 }
 714         }
 715
 716         return NULL;
 717 }
 718
 719 static struct gb_module *gb_svc_module_lookup(struct gb_svc *svc, u8 module_id)
 720 {
 721         struct gb_host_device *hd = svc->hd;
 722         struct gb_module *module;
 723
 724         list_for_each_entry(module, &hd->modules, hd_node) {
 725                 if (module->module_id == module_id)
 726                         return module;
 727         }
 728
 729         return NULL;
 730 }
 731
 732 static void gb_svc_intf_reenable(struct gb_svc *svc, struct gb_interface *intf)
 733 {
 734         int ret;
 735
 736         mutex_lock(&intf->mutex);
 737
 738         /* Mark as disconnected to prevent I/O during disable. */
 739         intf->disconnected = true;
 740         gb_interface_disable(intf);
 741         intf->disconnected = false;
 742
 743         ret = gb_interface_enable(intf);
 744         if (ret) {
 745                 dev_err(&svc->dev, "failed to enable interface %u: %d\n",
 746                                 intf->interface_id, ret);
 747
 748                 gb_interface_deactivate(intf);
 749         }
 750
 751         mutex_unlock(&intf->mutex);
 752 }
 753
 754 static void gb_svc_process_intf_hotplug(struct gb_operation *operation)
 755 {
 756         struct gb_svc_intf_hotplug_request *request;
 757         struct gb_connection *connection = operation->connection;
 758         struct gb_svc *svc = gb_connection_get_data(connection);
 759         struct gb_host_device *hd = connection->hd;
 760         struct gb_module *module;
 761         u8 intf_id;
 762         int ret;
 763
 764         /* The request message size has already been verified. */
 765         request = operation->request->payload;
 766         intf_id = request->intf_id;
 767
 768         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, intf_id);
 769
 770         /* All modules are considered 1x2 for now */
 771         module = gb_svc_module_lookup(svc, intf_id);
 772         if (module) {
 773                 dev_info(&svc->dev, "mode switch detected on interface %u\n",
 774                                 intf_id);
 775
 776                 return gb_svc_intf_reenable(svc, module->interfaces[0]);
 777         }
 778
 779         module = gb_module_create(hd, intf_id, 1);
 780         if (!module) {
 781                 dev_err(&svc->dev, "failed to create module\n");
 782                 return;
 783         }
 784
 785         ret = gb_module_add(module);
 786         if (ret) {
 787                 gb_module_put(module);
 788                 return;
 789         }
 790
 791         list_add(&module->hd_node, &hd->modules);
 792 }
 793
 794 static void gb_svc_process_intf_hot_unplug(struct gb_operation *operation)
 795 {
 796         struct gb_svc *svc = gb_connection_get_data(operation->connection);
 797         struct gb_svc_intf_hot_unplug_request *request;
 798         struct gb_module *module;
 799         u8 intf_id;
 800
 801         /* The request message size has already been verified. */
 802         request = operation->request->payload;
 803         intf_id = request->intf_id;
 804
 805         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, intf_id);
 806
 807         /* All modules are considered 1x2 for now */
 808         module = gb_svc_module_lookup(svc, intf_id);
 809         if (!module) {
 810                 dev_warn(&svc->dev, "could not find hot-unplug interface %u\n",
 811                                 intf_id);
 812                 return;
 813         }
 814
 815         module->disconnected = true;
 816
 817         gb_module_del(module);
 818         list_del(&module->hd_node);
 819         gb_module_put(module);
 820 }
 821
 822 static void gb_svc_process_module_inserted(struct gb_operation *operation)
 823 {
 824         struct gb_svc_module_inserted_request *request;
 825         struct gb_connection *connection = operation->connection;
 826         struct gb_svc *svc = gb_connection_get_data(connection);
 827         struct gb_host_device *hd = svc->hd;
 828         struct gb_module *module;
 829         size_t num_interfaces;
 830         u8 module_id;
 831         u16 flags;
 832         int ret;
 833
 834         /* The request message size has already been verified. */
 835         request = operation->request->payload;
 836         module_id = request->primary_intf_id;
 837         num_interfaces = request->intf_count;
 838         flags = le16_to_cpu(request->flags);
 839
 840         dev_dbg(&svc->dev, "%s - id = %u, num_interfaces = %zu, flags = 0x%04x\n",
 841                         __func__, module_id, num_interfaces, flags);
 842
 843         if (flags & GB_SVC_MODULE_INSERTED_FLAG_NO_PRIMARY) {
 844                 dev_warn(&svc->dev, "no primary interface detected on module %u\n",
 845                                 module_id);
 846         }
 847
 848         module = gb_svc_module_lookup(svc, module_id);
 849         if (module) {
 850                 dev_warn(&svc->dev, "unexpected module-inserted event %u\n",
 851                                 module_id);
 852                 return;
 853         }
 854
 855         module = gb_module_create(hd, module_id, num_interfaces);
 856         if (!module) {
 857                 dev_err(&svc->dev, "failed to create module\n");
 858                 return;
 859         }
 860
 861         ret = gb_module_add(module);
 862         if (ret) {
 863                 gb_module_put(module);
 864                 return;
 865         }
 866
 867         list_add(&module->hd_node, &hd->modules);
 868 }
 869
 870 static void gb_svc_process_module_removed(struct gb_operation *operation)
 871 {
 872         struct gb_svc_module_removed_request *request;
 873         struct gb_connection *connection = operation->connection;
 874         struct gb_svc *svc = gb_connection_get_data(connection);
 875         struct gb_module *module;
 876         u8 module_id;
 877
 878         /* The request message size has already been verified. */
 879         request = operation->request->payload;
 880         module_id = request->primary_intf_id;
 881
 882         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, module_id);
 883
 884         module = gb_svc_module_lookup(svc, module_id);
 885         if (!module) {
 886                 dev_warn(&svc->dev, "unexpected module-removed event %u\n",
 887                                 module_id);
 888                 return;
 889         }
 890
 891         module->disconnected = true;
 892
 893         gb_module_del(module);
 894         list_del(&module->hd_node);
 895         gb_module_put(module);
 896 }
 897
 898 static void gb_svc_process_intf_mailbox_event(struct gb_operation *operation)
 899 {
 900         struct gb_svc_intf_mailbox_event_request *request;
 901         struct gb_connection *connection = operation->connection;
 902         struct gb_svc *svc = gb_connection_get_data(connection);
 903         struct gb_interface *intf;
 904         u8 intf_id;
 905         u16 result_code;
 906         u32 mailbox;
 907
 908         /* The request message size has already been verified. */
 909         request = operation->request->payload;
 910         intf_id = request->intf_id;
 911         result_code = le16_to_cpu(request->result_code);
 912         mailbox = le32_to_cpu(request->mailbox);
 913
 914         dev_dbg(&svc->dev, "%s - id = %u, result = 0x%04x, mailbox = 0x%08x\n",
 915                         __func__, intf_id, result_code, mailbox);
 916
 917         intf = gb_svc_interface_lookup(svc, intf_id);
 918         if (!intf) {
 919                 dev_warn(&svc->dev, "unexpected mailbox event %u\n", intf_id);
 920                 return;
 921         }
 922
 923         if (result_code) {
 924                 dev_warn(&svc->dev,
 925                                 "mailbox event %u with UniPro error: 0x%04x\n",
 926                                 intf_id, result_code);
 927                 goto err_disable_interface;
 928         }
 929
 930         if (mailbox != GB_SVC_INTF_MAILBOX_GREYBUS) {
 931                 dev_warn(&svc->dev,
 932                                 "mailbox event %u with unexected value: 0x%08x\n",
 933                                 intf_id, mailbox);
 934                 goto err_disable_interface;
 935         }
 936
 937         dev_info(&svc->dev, "mode switch detected on interface %u\n", intf_id);
 938
 939         gb_svc_intf_reenable(svc, intf);
 940
 941         return;
 942
 943 err_disable_interface:
 944         mutex_lock(&intf->mutex);
 945         gb_interface_disable(intf);
 946         gb_interface_deactivate(intf);
 947         mutex_unlock(&intf->mutex);
 948 }
 949
 950 static void gb_svc_process_deferred_request(struct work_struct *work)
 951 {
 952         struct gb_svc_deferred_request *dr;
 953         struct gb_operation *operation;
 954         struct gb_svc *svc;
 955         u8 type;
 956
 957         dr = container_of(work, struct gb_svc_deferred_request, work);
 958         operation = dr->operation;
 959         svc = gb_connection_get_data(operation->connection);
 960         type = operation->request->header->type;
 961
 962         switch (type) {
 963         case GB_SVC_TYPE_INTF_HOTPLUG:
 964                 gb_svc_process_intf_hotplug(operation);
 965                 break;
 966         case GB_SVC_TYPE_INTF_HOT_UNPLUG:
 967                 gb_svc_process_intf_hot_unplug(operation);
 968                 break;
 969         case GB_SVC_TYPE_MODULE_INSERTED:
 970                 gb_svc_process_module_inserted(operation);
 971                 break;
 972         case GB_SVC_TYPE_MODULE_REMOVED:
 973                 gb_svc_process_module_removed(operation);
 974                 break;
 975         case GB_SVC_TYPE_INTF_MAILBOX_EVENT:
 976                 gb_svc_process_intf_mailbox_event(operation);
 977                 break;
 978         default:
 979                 dev_err(&svc->dev, "bad deferred request type: 0x%02x\n", type);
 980         }
 981
 982         gb_operation_put(operation);
 983         kfree(dr);
 984 }
 985
 986 static int gb_svc_queue_deferred_request(struct gb_operation *operation)
 987 {
 988         struct gb_svc *svc = gb_connection_get_data(operation->connection);
 989         struct gb_svc_deferred_request *dr;
 990
 991         dr = kmalloc(sizeof(*dr), GFP_KERNEL);
 992         if (!dr)
 993                 return -ENOMEM;
 994
 995         gb_operation_get(operation);
 996
 997         dr->operation = operation;
 998         INIT_WORK(&dr->work, gb_svc_process_deferred_request);
 999
1000         queue_work(svc->wq, &dr->work);
1001
1002         return 0;
1003 }
1004
1005 /*
1006  * Bringing up a module can be time consuming, as that may require lots of
1007  * initialization on the module side. Over that, we may also need to download
1008  * the firmware first and flash that on the module.
1009  *
1010  * In order not to make other svc events wait for all this to finish,
1011  * handle most of module hotplug stuff outside of the hotplug callback, with
1012  * help of a workqueue.
1013  */
1014 static int gb_svc_intf_hotplug_recv(struct gb_operation *op)
1015 {
1016         struct gb_svc *svc = gb_connection_get_data(op->connection);
1017         struct gb_svc_intf_hotplug_request *request;
1018
1019         if (op->request->payload_size < sizeof(*request)) {
1020                 dev_warn(&svc->dev, "short hotplug request received (%zu < %zu)\n",
1021                                 op->request->payload_size, sizeof(*request));
1022                 return -EINVAL;
1023         }
1024
1025         request = op->request->payload;
1026
1027         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, request->intf_id);
1028
1029         return gb_svc_queue_deferred_request(op);
1030 }
1031
1032 static int gb_svc_intf_hot_unplug_recv(struct gb_operation *op)
1033 {
1034         struct gb_svc *svc = gb_connection_get_data(op->connection);
1035         struct gb_svc_intf_hot_unplug_request *request;
1036
1037         if (op->request->payload_size < sizeof(*request)) {
1038                 dev_warn(&svc->dev, "short hot unplug request received (%zu < %zu)\n",
1039                                 op->request->payload_size, sizeof(*request));
1040                 return -EINVAL;
1041         }
1042
1043         request = op->request->payload;
1044
1045         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, request->intf_id);
1046
1047         return gb_svc_queue_deferred_request(op);
1048 }
1049
1050 static int gb_svc_intf_reset_recv(struct gb_operation *op)
1051 {
1052         struct gb_svc *svc = gb_connection_get_data(op->connection);
1053         struct gb_message *request = op->request;
1054         struct gb_svc_intf_reset_request *reset;
1055         u8 intf_id;
1056
1057         if (request->payload_size < sizeof(*reset)) {
1058                 dev_warn(&svc->dev, "short reset request received (%zu < %zu)\n",
1059                                 request->payload_size, sizeof(*reset));
1060                 return -EINVAL;
1061         }
1062         reset = request->payload;
1063
1064         intf_id = reset->intf_id;
1065
1066         /* FIXME Reset the interface here */
1067
1068         return 0;
1069 }
1070
1071 static int gb_svc_key_code_map(struct gb_svc *svc, u16 key_code, u16 *code)
1072 {
1073         switch (key_code) {
1074         case GB_KEYCODE_ARA:
1075                 *code = SVC_KEY_ARA_BUTTON;
1076                 break;
1077         default:
1078                 dev_warn(&svc->dev, "unknown keycode received: %u\n", key_code);
1079                 return -EINVAL;
1080         }
1081
1082         return 0;
1083 }
1084
1085 static int gb_svc_key_event_recv(struct gb_operation *op)
1086 {
1087         struct gb_svc *svc = gb_connection_get_data(op->connection);
1088         struct gb_message *request = op->request;
1089         struct gb_svc_key_event_request *key;
1090         u16 code;
1091         u8 event;
1092         int ret;
1093
1094         if (request->payload_size < sizeof(*key)) {
1095                 dev_warn(&svc->dev, "short key request received (%zu < %zu)\n",
1096                          request->payload_size, sizeof(*key));
1097                 return -EINVAL;
1098         }
1099
1100         key = request->payload;
1101
1102         ret = gb_svc_key_code_map(svc, le16_to_cpu(key->key_code), &code);
1103         if (ret < 0)
1104                 return ret;
1105
1106         event = key->key_event;
1107         if ((event != GB_SVC_KEY_PRESSED) && (event != GB_SVC_KEY_RELEASED)) {
1108                 dev_warn(&svc->dev, "unknown key event received: %u\n", event);
1109                 return -EINVAL;
1110         }
1111
1112         input_report_key(svc->input, code, (event == GB_SVC_KEY_PRESSED));
1113         input_sync(svc->input);
1114
1115         return 0;
1116 }
1117
1118 static int gb_svc_module_inserted_recv(struct gb_operation *op)
1119 {
1120         struct gb_svc *svc = gb_connection_get_data(op->connection);
1121         struct gb_svc_module_inserted_request *request;
1122
1123         if (op->request->payload_size < sizeof(*request)) {
1124                 dev_warn(&svc->dev, "short module-inserted request received (%zu < %zu)\n",
1125                                 op->request->payload_size, sizeof(*request));
1126                 return -EINVAL;
1127         }
1128
1129         request = op->request->payload;
1130
1131         dev_dbg(&svc->dev, "%s - id = %u\n", __func__,
1132                         request->primary_intf_id);
1133
1134         return gb_svc_queue_deferred_request(op);
1135 }
1136
1137 static int gb_svc_module_removed_recv(struct gb_operation *op)
1138 {
1139         struct gb_svc *svc = gb_connection_get_data(op->connection);
1140         struct gb_svc_module_removed_request *request;
1141
1142         if (op->request->payload_size < sizeof(*request)) {
1143                 dev_warn(&svc->dev, "short module-removed request received (%zu < %zu)\n",
1144                                 op->request->payload_size, sizeof(*request));
1145                 return -EINVAL;
1146         }
1147
1148         request = op->request->payload;
1149
1150         dev_dbg(&svc->dev, "%s - id = %u\n", __func__,
1151                         request->primary_intf_id);
1152
1153         return gb_svc_queue_deferred_request(op);
1154 }
1155
1156 static int gb_svc_intf_mailbox_event_recv(struct gb_operation *op)
1157 {
1158         struct gb_svc *svc = gb_connection_get_data(op->connection);
1159         struct gb_svc_intf_mailbox_event_request *request;
1160
1161         if (op->request->payload_size < sizeof(*request)) {
1162                 dev_warn(&svc->dev, "short mailbox request received (%zu < %zu)\n",
1163                                 op->request->payload_size, sizeof(*request));
1164                 return -EINVAL;
1165         }
1166
1167         request = op->request->payload;
1168
1169         dev_dbg(&svc->dev, "%s - id = %u\n", __func__, request->intf_id);
1170
1171         return gb_svc_queue_deferred_request(op);
1172 }
1173
1174 static int gb_svc_request_handler(struct gb_operation *op)
1175 {
1176         struct gb_connection *connection = op->connection;
1177         struct gb_svc *svc = gb_connection_get_data(connection);
1178         u8 type = op->type;
1179         int ret = 0;
1180
1181         /*
1182          * SVC requests need to follow a specific order (at least initially) and
1183          * below code takes care of enforcing that. The expected order is:
1184          * - PROTOCOL_VERSION
1185          * - SVC_HELLO
1186          * - Any other request, but the earlier two.
1187          *
1188          * Incoming requests are guaranteed to be serialized and so we don't
1189          * need to protect 'state' for any races.
1190          */
1191         switch (type) {
1192         case GB_REQUEST_TYPE_PROTOCOL_VERSION:
1193                 if (svc->state != GB_SVC_STATE_RESET)
1194                         ret = -EINVAL;
1195                 break;
1196         case GB_SVC_TYPE_SVC_HELLO:
1197                 if (svc->state != GB_SVC_STATE_PROTOCOL_VERSION)
1198                         ret = -EINVAL;
1199                 break;
1200         default:
1201                 if (svc->state != GB_SVC_STATE_SVC_HELLO)
1202                         ret = -EINVAL;
1203                 break;
1204         }
1205
1206         if (ret) {
1207                 dev_warn(&svc->dev, "unexpected request 0x%02x received (state %u)\n",
1208                                 type, svc->state);
1209                 return ret;
1210         }
1211
1212         switch (type) {
1213         case GB_REQUEST_TYPE_PROTOCOL_VERSION:
1214                 ret = gb_svc_version_request(op);
1215                 if (!ret)
1216                         svc->state = GB_SVC_STATE_PROTOCOL_VERSION;
1217                 return ret;
1218         case GB_SVC_TYPE_SVC_HELLO:
1219                 ret = gb_svc_hello(op);
1220                 if (!ret)
1221                         svc->state = GB_SVC_STATE_SVC_HELLO;
1222                 return ret;
1223         case GB_SVC_TYPE_INTF_HOTPLUG:
1224                 return gb_svc_intf_hotplug_recv(op);
1225         case GB_SVC_TYPE_INTF_HOT_UNPLUG:
1226                 return gb_svc_intf_hot_unplug_recv(op);
1227         case GB_SVC_TYPE_INTF_RESET:
1228                 return gb_svc_intf_reset_recv(op);
1229         case GB_SVC_TYPE_KEY_EVENT:
1230                 return gb_svc_key_event_recv(op);
1231         case GB_SVC_TYPE_MODULE_INSERTED:
1232                 return gb_svc_module_inserted_recv(op);
1233         case GB_SVC_TYPE_MODULE_REMOVED:
1234                 return gb_svc_module_removed_recv(op);
1235         case GB_SVC_TYPE_INTF_MAILBOX_EVENT:
1236                 return gb_svc_intf_mailbox_event_recv(op);
1237         default:
1238                 dev_warn(&svc->dev, "unsupported request 0x%02x\n", type);
1239                 return -EINVAL;
1240         }
1241 }
1242
1243 static struct input_dev *gb_svc_input_create(struct gb_svc *svc)
1244 {
1245         struct input_dev *input_dev;
1246
1247         input_dev = input_allocate_device();
1248         if (!input_dev)
1249                 return ERR_PTR(-ENOMEM);
1250
1251         input_dev->name = dev_name(&svc->dev);
1252         svc->input_phys = kasprintf(GFP_KERNEL, "greybus-%s/input0",
1253                                     input_dev->name);
1254         if (!svc->input_phys)
1255                 goto err_free_input;
1256
1257         input_dev->phys = svc->input_phys;
1258         input_dev->dev.parent = &svc->dev;
1259
1260         input_set_drvdata(input_dev, svc);
1261
1262         input_set_capability(input_dev, EV_KEY, SVC_KEY_ARA_BUTTON);
1263
1264         return input_dev;
1265
1266 err_free_input:
1267         input_free_device(svc->input);
1268         return ERR_PTR(-ENOMEM);
1269 }
1270
1271 static void gb_svc_release(struct device *dev)
1272 {
1273         struct gb_svc *svc = to_gb_svc(dev);
1274
1275         if (svc->connection)
1276                 gb_connection_destroy(svc->connection);
1277         ida_destroy(&svc->device_id_map);
1278         destroy_workqueue(svc->wq);
1279         kfree(svc->input_phys);
1280         kfree(svc);
1281 }
1282
1283 struct device_type greybus_svc_type = {
1284         .name           = "greybus_svc",
1285         .release        = gb_svc_release,
1286 };
1287
1288 struct gb_svc *gb_svc_create(struct gb_host_device *hd)
1289 {
1290         struct gb_svc *svc;
1291
1292         svc = kzalloc(sizeof(*svc), GFP_KERNEL);
1293         if (!svc)
1294                 return NULL;
1295
1296         svc->wq = alloc_workqueue("%s:svc", WQ_UNBOUND, 1, dev_name(&hd->dev));
1297         if (!svc->wq) {
1298                 kfree(svc);
1299                 return NULL;
1300         }
1301
1302         svc->dev.parent = &hd->dev;
1303         svc->dev.bus = &greybus_bus_type;
1304         svc->dev.type = &greybus_svc_type;
1305         svc->dev.groups = svc_groups;
1306         svc->dev.dma_mask = svc->dev.parent->dma_mask;
1307         device_initialize(&svc->dev);
1308
1309         dev_set_name(&svc->dev, "%d-svc", hd->bus_id);
1310
1311         ida_init(&svc->device_id_map);
1312         svc->state = GB_SVC_STATE_RESET;
1313         svc->hd = hd;
1314
1315         svc->input = gb_svc_input_create(svc);
1316         if (IS_ERR(svc->input)) {
1317                 dev_err(&svc->dev, "failed to create input device: %ld\n",
1318                         PTR_ERR(svc->input));
1319                 goto err_put_device;
1320         }
1321
1322         svc->connection = gb_connection_create_static(hd, GB_SVC_CPORT_ID,
1323                                                 gb_svc_request_handler);
1324         if (IS_ERR(svc->connection)) {
1325                 dev_err(&svc->dev, "failed to create connection: %ld\n",
1326                                 PTR_ERR(svc->connection));
1327                 goto err_free_input;
1328         }
1329
1330         gb_connection_set_data(svc->connection, svc);
1331
1332         return svc;
1333
1334 err_free_input:
1335         input_free_device(svc->input);
1336 err_put_device:
1337         put_device(&svc->dev);
1338         return NULL;
1339 }
1340
1341 int gb_svc_add(struct gb_svc *svc)
1342 {
1343         int ret;
1344
1345         /*
1346          * The SVC protocol is currently driven by the SVC, so the SVC device
1347          * is added from the connection request handler when enough
1348          * information has been received.
1349          */
1350         ret = gb_connection_enable(svc->connection);
1351         if (ret)
1352                 return ret;
1353
1354         return 0;
1355 }
1356
1357 static void gb_svc_remove_modules(struct gb_svc *svc)
1358 {
1359         struct gb_host_device *hd = svc->hd;
1360         struct gb_module *module, *tmp;
1361
1362         list_for_each_entry_safe(module, tmp, &hd->modules, hd_node) {
1363                 gb_module_del(module);
1364                 list_del(&module->hd_node);
1365                 gb_module_put(module);
1366         }
1367 }
1368
1369 void gb_svc_del(struct gb_svc *svc)
1370 {
1371         gb_connection_disable(svc->connection);
1372
1373         /*
1374          * The SVC device and input device may have been registered
1375          * from the request handler.
1376          */
1377         if (device_is_registered(&svc->dev)) {
1378                 gb_svc_debugfs_exit(svc);
1379                 gb_svc_watchdog_destroy(svc);
1380                 input_unregister_device(svc->input);
1381                 device_del(&svc->dev);
1382         }
1383
1384         flush_workqueue(svc->wq);
1385
1386         gb_svc_remove_modules(svc);
1387 }
1388
1389 void gb_svc_put(struct gb_svc *svc)
1390 {
1391         put_device(&svc->dev);
1392 }