1 /* 2 * Power Supply driver for a Greybus module. 3 * 4 * Copyright 2014-2015 Google Inc. 5 * Copyright 2014-2015 Linaro Ltd. 6 * 7 * Released under the GPLv2 only. 8 */ 9 10 #include <linux/kernel.h> 11 #include <linux/module.h> 12 #include <linux/power_supply.h> 13 #include <linux/slab.h> 14 15 #include "greybus.h" 16 17 #define PROP_MAX 32 18 19 struct gb_power_supply_prop { 20 enum power_supply_property prop; 21 u32 val; 22 u32 previous_val; 23 bool is_writeable; 24 }; 25 26 struct gb_power_supply { 27 u8 id; 28 bool registered; 29 #ifdef DRIVER_OWNS_PSY_STRUCT 30 struct power_supply psy; 31 #define to_gb_power_supply(x) container_of(x, struct gb_power_supply, psy) 32 #else 33 struct power_supply *psy; 34 struct power_supply_desc desc; 35 #define to_gb_power_supply(x) power_supply_get_drvdata(x) 36 #endif 37 char name[64]; 38 struct gb_power_supplies *supplies; 39 struct delayed_work work; 40 char *manufacturer; 41 char *model_name; 42 char *serial_number; 43 u8 type; 44 u8 properties_count; 45 u8 properties_count_str; 46 unsigned long last_update; 47 unsigned int update_interval; 48 bool changed; 49 struct gb_power_supply_prop *props; 50 enum power_supply_property *props_raw; 51 }; 52 53 struct gb_power_supplies { 54 struct gb_connection *connection; 55 u8 supplies_count; 56 struct gb_power_supply *supply; 57 struct mutex supplies_lock; 58 }; 59 60 /* cache time in milliseconds, if cache_time is set to 0 cache is disable */ 61 static unsigned int cache_time = 1000; 62 /* 63 * update interval initial and maximum value, between the two will 64 * back-off exponential 65 */ 66 static unsigned int update_interval_init = 1 * HZ; 67 static unsigned int update_interval_max = 30 * HZ; 68 69 struct gb_power_supply_changes { 70 enum power_supply_property prop; 71 u32 tolerance_change; 72 }; 73 74 static const struct gb_power_supply_changes psy_props_changes[] = { 75 { .prop = GB_POWER_SUPPLY_PROP_STATUS, 76 .tolerance_change = 0, 77 }, 78 { .prop = GB_POWER_SUPPLY_PROP_TEMP, 79 .tolerance_change = 500, 80 }, 81 { .prop = GB_POWER_SUPPLY_PROP_ONLINE, 82 .tolerance_change = 0, 83 }, 84 }; 85 86 static struct gb_connection *get_conn_from_psy(struct gb_power_supply *gbpsy) 87 { 88 return gbpsy->supplies->connection; 89 } 90 91 static struct gb_power_supply_prop *get_psy_prop(struct gb_power_supply *gbpsy, 92 enum power_supply_property psp) 93 { 94 int i; 95 96 for (i = 0; i < gbpsy->properties_count; i++) 97 if (gbpsy->props[i].prop == psp) 98 return &gbpsy->props[i]; 99 return NULL; 100 } 101 102 static int is_psy_prop_writeable(struct gb_power_supply *gbpsy, 103 enum power_supply_property psp) 104 { 105 struct gb_power_supply_prop *prop; 106 107 prop = get_psy_prop(gbpsy, psp); 108 if (!prop) 109 return -ENOENT; 110 return prop->is_writeable ? 1 : 0; 111 } 112 113 static int is_prop_valint(enum power_supply_property psp) 114 { 115 return ((psp < POWER_SUPPLY_PROP_MODEL_NAME) ? 1 : 0); 116 } 117 118 static void next_interval(struct gb_power_supply *gbpsy) 119 { 120 if (gbpsy->update_interval == update_interval_max) 121 return; 122 123 /* do some exponential back-off in the update interval */ 124 gbpsy->update_interval *= 2; 125 if (gbpsy->update_interval > update_interval_max) 126 gbpsy->update_interval = update_interval_max; 127 } 128 129 #ifdef DRIVER_OWNS_PSY_STRUCT 130 static void __gb_power_supply_changed(struct gb_power_supply *gbpsy) 131 { 132 power_supply_changed(&gbpsy->psy); 133 } 134 #else 135 static void __gb_power_supply_changed(struct gb_power_supply *gbpsy) 136 { 137 power_supply_changed(gbpsy->psy); 138 } 139 #endif 140 141 static void check_changed(struct gb_power_supply *gbpsy, 142 struct gb_power_supply_prop *prop) 143 { 144 const struct gb_power_supply_changes *psyc; 145 u32 val = prop->val; 146 u32 prev_val = prop->previous_val; 147 int i; 148 149 for (i = 0; i < ARRAY_SIZE(psy_props_changes); i++) { 150 psyc = &psy_props_changes[i]; 151 if (prop->prop == psyc->prop) { 152 if (!psyc->tolerance_change) 153 gbpsy->changed = true; 154 else if (val < prev_val && 155 prev_val - val > psyc->tolerance_change) 156 gbpsy->changed = true; 157 else if (val > prev_val && 158 val - prev_val > psyc->tolerance_change) 159 gbpsy->changed = true; 160 break; 161 } 162 } 163 } 164 165 static int total_props(struct gb_power_supply *gbpsy) 166 { 167 /* this return the intval plus the strval properties */ 168 return (gbpsy->properties_count + gbpsy->properties_count_str); 169 } 170 171 static void prop_append(struct gb_power_supply *gbpsy, 172 enum power_supply_property prop) 173 { 174 enum power_supply_property *new_props_raw; 175 176 gbpsy->properties_count_str++; 177 new_props_raw = krealloc(gbpsy->props_raw, total_props(gbpsy) * 178 sizeof(enum power_supply_property), 179 GFP_KERNEL); 180 if (!new_props_raw) 181 return; 182 gbpsy->props_raw = new_props_raw; 183 gbpsy->props_raw[total_props(gbpsy) - 1] = prop; 184 } 185 186 static int __gb_power_supply_set_name(char *init_name, char *name, size_t len) 187 { 188 unsigned int i = 0; 189 int ret = 0; 190 struct power_supply *psy; 191 192 if (!strlen(init_name)) 193 init_name = "gb_power_supply"; 194 strlcpy(name, init_name, len); 195 196 while ((ret < len) && (psy = power_supply_get_by_name(name))) { 197 #ifdef PSY_HAVE_PUT 198 power_supply_put(psy); 199 #endif 200 ret = snprintf(name, len, "%s_%u", init_name, ++i); 201 } 202 if (ret >= len) 203 return -ENOMEM; 204 return i; 205 } 206 207 static void _gb_power_supply_append_props(struct gb_power_supply *gbpsy) 208 { 209 if (strlen(gbpsy->manufacturer)) 210 prop_append(gbpsy, POWER_SUPPLY_PROP_MANUFACTURER); 211 if (strlen(gbpsy->model_name)) 212 prop_append(gbpsy, POWER_SUPPLY_PROP_MODEL_NAME); 213 if (strlen(gbpsy->serial_number)) 214 prop_append(gbpsy, POWER_SUPPLY_PROP_SERIAL_NUMBER); 215 } 216 217 static int gb_power_supply_description_get(struct gb_power_supply *gbpsy) 218 { 219 struct gb_connection *connection = get_conn_from_psy(gbpsy); 220 struct gb_power_supply_get_description_request req; 221 struct gb_power_supply_get_description_response resp; 222 int ret; 223 224 req.psy_id = gbpsy->id; 225 226 ret = gb_operation_sync(connection, 227 GB_POWER_SUPPLY_TYPE_GET_DESCRIPTION, 228 &req, sizeof(req), &resp, sizeof(resp)); 229 if (ret < 0) 230 return ret; 231 232 gbpsy->manufacturer = kstrndup(resp.manufacturer, PROP_MAX, GFP_KERNEL); 233 if (!gbpsy->manufacturer) 234 return -ENOMEM; 235 gbpsy->model_name = kstrndup(resp.model, PROP_MAX, GFP_KERNEL); 236 if (!gbpsy->model_name) 237 return -ENOMEM; 238 gbpsy->serial_number = kstrndup(resp.serial_number, PROP_MAX, 239 GFP_KERNEL); 240 if (!gbpsy->serial_number) 241 return -ENOMEM; 242 243 gbpsy->type = le16_to_cpu(resp.type); 244 gbpsy->properties_count = resp.properties_count; 245 246 return 0; 247 } 248 249 static int gb_power_supply_prop_descriptors_get(struct gb_power_supply *gbpsy) 250 { 251 struct gb_connection *connection = get_conn_from_psy(gbpsy); 252 struct gb_power_supply_get_property_descriptors_request *req; 253 struct gb_power_supply_get_property_descriptors_response *resp; 254 struct gb_operation *op; 255 u8 props_count = gbpsy->properties_count; 256 int ret; 257 int i; 258 259 if (props_count == 0) 260 return 0; 261 262 op = gb_operation_create(connection, 263 GB_POWER_SUPPLY_TYPE_GET_PROP_DESCRIPTORS, 264 sizeof(req), sizeof(*resp) + props_count * 265 sizeof(struct gb_power_supply_props_desc), 266 GFP_KERNEL); 267 if (!op) 268 return -ENOMEM; 269 270 req = op->request->payload; 271 req->psy_id = gbpsy->id; 272 273 ret = gb_operation_request_send_sync(op); 274 if (ret < 0) 275 goto out_put_operation; 276 277 resp = op->response->payload; 278 279 gbpsy->props = kcalloc(gbpsy->properties_count, sizeof(*gbpsy->props), 280 GFP_KERNEL); 281 if (!gbpsy->props) { 282 ret = -ENOMEM; 283 goto out_put_operation; 284 } 285 286 gbpsy->props_raw = kcalloc(gbpsy->properties_count, 287 sizeof(*gbpsy->props_raw), GFP_KERNEL); 288 if (!gbpsy->props_raw) { 289 ret = -ENOMEM; 290 goto out_put_operation; 291 } 292 293 294 /* Store available properties */ 295 for (i = 0; i < gbpsy->properties_count; i++) { 296 gbpsy->props[i].prop = resp->props[i].property; 297 gbpsy->props_raw[i] = resp->props[i].property; 298 if (resp->props[i].is_writeable) 299 gbpsy->props[i].is_writeable = true; 300 } 301 302 /* 303 * now append the properties that we already got information in the 304 * get_description operation. (char * ones) 305 */ 306 _gb_power_supply_append_props(gbpsy); 307 308 out_put_operation: 309 gb_operation_put(op); 310 311 return ret; 312 } 313 314 static int __gb_power_supply_property_update(struct gb_power_supply *gbpsy, 315 enum power_supply_property psp) 316 { 317 struct gb_connection *connection = get_conn_from_psy(gbpsy); 318 struct gb_power_supply_prop *prop; 319 struct gb_power_supply_get_property_request req; 320 struct gb_power_supply_get_property_response resp; 321 u32 val; 322 int ret; 323 324 prop = get_psy_prop(gbpsy, psp); 325 if (!prop) 326 return -EINVAL; 327 req.psy_id = gbpsy->id; 328 req.property = (u8)psp; 329 330 ret = gb_operation_sync(connection, GB_POWER_SUPPLY_TYPE_GET_PROPERTY, 331 &req, sizeof(req), &resp, sizeof(resp)); 332 if (ret < 0) 333 return ret; 334 335 val = le32_to_cpu(resp.prop_val); 336 if (val == prop->val) 337 return 0; 338 339 prop->previous_val = prop->val; 340 prop->val = val; 341 342 check_changed(gbpsy, prop); 343 344 return 0; 345 } 346 347 static int __gb_power_supply_property_get(struct gb_power_supply *gbpsy, 348 enum power_supply_property psp, 349 union power_supply_propval *val) 350 { 351 struct gb_power_supply_prop *prop; 352 353 prop = get_psy_prop(gbpsy, psp); 354 if (!prop) 355 return -EINVAL; 356 357 val->intval = prop->val; 358 return 0; 359 } 360 361 static int __gb_power_supply_property_strval_get(struct gb_power_supply *gbpsy, 362 enum power_supply_property psp, 363 union power_supply_propval *val) 364 { 365 switch (psp) { 366 case POWER_SUPPLY_PROP_MODEL_NAME: 367 val->strval = gbpsy->model_name; 368 break; 369 case POWER_SUPPLY_PROP_MANUFACTURER: 370 val->strval = gbpsy->manufacturer; 371 break; 372 case POWER_SUPPLY_PROP_SERIAL_NUMBER: 373 val->strval = gbpsy->serial_number; 374 break; 375 default: 376 break; 377 } 378 379 return 0; 380 } 381 382 static int _gb_power_supply_property_get(struct gb_power_supply *gbpsy, 383 enum power_supply_property psp, 384 union power_supply_propval *val) 385 { 386 struct gb_connection *connection = get_conn_from_psy(gbpsy); 387 int ret; 388 389 /* 390 * Properties of type const char *, were already fetched on 391 * get_description operation and should be cached in gb 392 */ 393 if (is_prop_valint(psp)) 394 ret = __gb_power_supply_property_get(gbpsy, psp, val); 395 else 396 ret = __gb_power_supply_property_strval_get(gbpsy, psp, val); 397 398 if (ret < 0) 399 dev_err(&connection->bundle->dev, "get property %u\n", psp); 400 401 return 0; 402 } 403 404 static int gb_power_supply_status_get(struct gb_power_supply *gbpsy) 405 { 406 int ret = 0; 407 int i; 408 409 /* check if cache is good enough */ 410 if (gbpsy->last_update && 411 time_is_after_jiffies(gbpsy->last_update + 412 msecs_to_jiffies(cache_time))) 413 return 0; 414 415 for (i = 0; i < gbpsy->properties_count; i++) { 416 ret = __gb_power_supply_property_update(gbpsy, 417 gbpsy->props[i].prop); 418 if (ret < 0) 419 break; 420 } 421 422 if (ret == 0) 423 gbpsy->last_update = jiffies; 424 425 return ret; 426 } 427 428 static void gb_power_supply_status_update(struct gb_power_supply *gbpsy) 429 { 430 /* check if there a change that need to be reported */ 431 gb_power_supply_status_get(gbpsy); 432 433 if (!gbpsy->changed) 434 return; 435 436 gbpsy->update_interval = update_interval_init; 437 __gb_power_supply_changed(gbpsy); 438 gbpsy->changed = false; 439 } 440 441 static void gb_power_supply_work(struct work_struct *work) 442 { 443 struct gb_power_supply *gbpsy = container_of(work, 444 struct gb_power_supply, 445 work.work); 446 447 /* 448 * if the poll interval is not set, disable polling, this is helpful 449 * specially at unregister time. 450 */ 451 if (!gbpsy->update_interval) 452 return; 453 454 gb_power_supply_status_update(gbpsy); 455 next_interval(gbpsy); 456 schedule_delayed_work(&gbpsy->work, gbpsy->update_interval); 457 } 458 459 static int get_property(struct power_supply *b, 460 enum power_supply_property psp, 461 union power_supply_propval *val) 462 { 463 struct gb_power_supply *gbpsy = to_gb_power_supply(b); 464 465 gb_power_supply_status_get(gbpsy); 466 467 return _gb_power_supply_property_get(gbpsy, psp, val); 468 } 469 470 static int gb_power_supply_property_set(struct gb_power_supply *gbpsy, 471 enum power_supply_property psp, 472 int val) 473 { 474 struct gb_connection *connection = get_conn_from_psy(gbpsy); 475 struct gb_power_supply_prop *prop; 476 struct gb_power_supply_set_property_request req; 477 int ret; 478 479 prop = get_psy_prop(gbpsy, psp); 480 if (!prop) 481 return -EINVAL; 482 req.psy_id = gbpsy->id; 483 req.property = (u8)psp; 484 req.prop_val = cpu_to_le32(val); 485 486 ret = gb_operation_sync(connection, GB_POWER_SUPPLY_TYPE_SET_PROPERTY, 487 &req, sizeof(req), NULL, 0); 488 if (ret < 0) 489 goto out; 490 491 /* cache immediately the new value */ 492 prop->val = val; 493 494 out: 495 return ret; 496 } 497 498 static int set_property(struct power_supply *b, 499 enum power_supply_property psp, 500 const union power_supply_propval *val) 501 { 502 struct gb_power_supply *gbpsy = to_gb_power_supply(b); 503 504 return gb_power_supply_property_set(gbpsy, psp, val->intval); 505 } 506 507 static int property_is_writeable(struct power_supply *b, 508 enum power_supply_property psp) 509 { 510 struct gb_power_supply *gbpsy = to_gb_power_supply(b); 511 512 return is_psy_prop_writeable(gbpsy, psp); 513 } 514 515 516 #ifdef DRIVER_OWNS_PSY_STRUCT 517 static int gb_power_supply_register(struct gb_power_supply *gbpsy) 518 { 519 struct gb_connection *connection = get_conn_from_psy(gbpsy); 520 521 gbpsy->psy.name = gbpsy->name; 522 gbpsy->psy.type = gbpsy->type; 523 gbpsy->psy.properties = gbpsy->props_raw; 524 gbpsy->psy.num_properties = total_props(gbpsy); 525 gbpsy->psy.get_property = get_property; 526 gbpsy->psy.set_property = set_property; 527 gbpsy->psy.property_is_writeable = property_is_writeable; 528 529 return power_supply_register(&connection->bundle->dev, 530 &gbpsy->psy); 531 } 532 #else 533 static int gb_power_supply_register(struct gb_power_supply *gbpsy) 534 { 535 struct gb_connection *connection = get_conn_from_psy(gbpsy); 536 struct power_supply_config cfg = {}; 537 538 cfg.drv_data = gbpsy; 539 540 gbpsy->desc.name = gbpsy->name; 541 gbpsy->desc.type = gbpsy->type; 542 gbpsy->desc.properties = gbpsy->props_raw; 543 gbpsy->desc.num_properties = total_props(gbpsy); 544 gbpsy->desc.get_property = get_property; 545 gbpsy->desc.set_property = set_property; 546 gbpsy->desc.property_is_writeable = property_is_writeable; 547 548 gbpsy->psy = power_supply_register(&connection->bundle->dev, 549 &gbpsy->desc, &cfg); 550 if (IS_ERR(gbpsy->psy)) 551 return PTR_ERR(gbpsy->psy); 552 553 return 0; 554 } 555 #endif 556 557 static void _gb_power_supply_free(struct gb_power_supply *gbpsy) 558 { 559 kfree(gbpsy->serial_number); 560 kfree(gbpsy->model_name); 561 kfree(gbpsy->manufacturer); 562 kfree(gbpsy->props_raw); 563 kfree(gbpsy->props); 564 } 565 566 static void _gb_power_supply_release(struct gb_power_supply *gbpsy) 567 { 568 569 gbpsy->update_interval = 0; 570 571 cancel_delayed_work_sync(&gbpsy->work); 572 #ifdef DRIVER_OWNS_PSY_STRUCT 573 if (gbpsy->registered) 574 power_supply_unregister(&gbpsy->psy); 575 #else 576 if (gbpsy->registered) 577 power_supply_unregister(gbpsy->psy); 578 #endif 579 580 _gb_power_supply_free(gbpsy); 581 } 582 583 static void _gb_power_supplies_release(struct gb_power_supplies *supplies) 584 { 585 int i; 586 587 if (!supplies->supply) 588 return; 589 590 mutex_lock(&supplies->supplies_lock); 591 for (i = 0; i < supplies->supplies_count; i++) 592 _gb_power_supply_release(&supplies->supply[i]); 593 kfree(supplies->supply); 594 mutex_unlock(&supplies->supplies_lock); 595 kfree(supplies); 596 } 597 598 static int gb_power_supplies_get_count(struct gb_power_supplies *supplies) 599 { 600 struct gb_power_supply_get_supplies_response resp; 601 int ret; 602 603 ret = gb_operation_sync(supplies->connection, 604 GB_POWER_SUPPLY_TYPE_GET_SUPPLIES, 605 NULL, 0, &resp, sizeof(resp)); 606 if (ret < 0) 607 return ret; 608 609 if (!resp.supplies_count) 610 return -EINVAL; 611 612 supplies->supplies_count = resp.supplies_count; 613 614 return ret; 615 } 616 617 static int gb_power_supply_config(struct gb_power_supplies *supplies, int id) 618 { 619 struct gb_power_supply *gbpsy = &supplies->supply[id]; 620 int ret; 621 622 gbpsy->supplies = supplies; 623 gbpsy->id = id; 624 625 ret = gb_power_supply_description_get(gbpsy); 626 if (ret < 0) 627 return ret; 628 629 ret = gb_power_supply_prop_descriptors_get(gbpsy); 630 if (ret < 0) 631 return ret; 632 633 /* guarantee that we have an unique name, before register */ 634 return __gb_power_supply_set_name(gbpsy->model_name, gbpsy->name, 635 sizeof(gbpsy->name)); 636 } 637 638 static int gb_power_supply_enable(struct gb_power_supply *gbpsy) 639 { 640 int ret; 641 642 ret = gb_power_supply_register(gbpsy); 643 if (ret < 0) 644 return ret; 645 646 gbpsy->update_interval = update_interval_init; 647 INIT_DELAYED_WORK(&gbpsy->work, gb_power_supply_work); 648 schedule_delayed_work(&gbpsy->work, 0); 649 650 /* everything went fine, mark it for release code to know */ 651 gbpsy->registered = true; 652 653 return 0; 654 } 655 656 static int gb_power_supplies_setup(struct gb_power_supplies *supplies) 657 { 658 struct gb_connection *connection = supplies->connection; 659 int ret; 660 int i; 661 662 mutex_lock(&supplies->supplies_lock); 663 664 ret = gb_power_supplies_get_count(supplies); 665 if (ret < 0) 666 goto out; 667 668 supplies->supply = kzalloc(supplies->supplies_count * 669 sizeof(struct gb_power_supply), 670 GFP_KERNEL); 671 672 if (!supplies->supply) { 673 ret = -ENOMEM; 674 goto out; 675 } 676 677 for (i = 0; i < supplies->supplies_count; i++) { 678 ret = gb_power_supply_config(supplies, i); 679 if (ret < 0) { 680 dev_err(&connection->bundle->dev, 681 "Fail to configure supplies devices\n"); 682 goto out; 683 } 684 } 685 out: 686 mutex_unlock(&supplies->supplies_lock); 687 return ret; 688 } 689 690 static int gb_power_supplies_register(struct gb_power_supplies *supplies) 691 { 692 struct gb_connection *connection = supplies->connection; 693 int ret = 0; 694 int i; 695 696 mutex_lock(&supplies->supplies_lock); 697 698 for (i = 0; i < supplies->supplies_count; i++) { 699 ret = gb_power_supply_enable(&supplies->supply[i]); 700 if (ret < 0) { 701 dev_err(&connection->bundle->dev, 702 "Fail to enable supplies devices\n"); 703 break; 704 } 705 } 706 707 mutex_unlock(&supplies->supplies_lock); 708 return ret; 709 } 710 711 static int gb_supplies_request_handler(struct gb_operation *op) 712 { 713 struct gb_connection *connection = op->connection; 714 struct gb_power_supplies *supplies = connection->private; 715 struct gb_power_supply *gbpsy; 716 struct gb_message *request; 717 struct gb_power_supply_event_request *payload; 718 u8 psy_id; 719 u8 event; 720 int ret = 0; 721 722 if (op->type != GB_POWER_SUPPLY_TYPE_EVENT) { 723 dev_err(&connection->bundle->dev, 724 "Unsupported unsolicited event: %u\n", op->type); 725 return -EINVAL; 726 } 727 728 request = op->request; 729 730 if (request->payload_size < sizeof(*payload)) { 731 dev_err(&connection->bundle->dev, 732 "Wrong event size received (%zu < %zu)\n", 733 request->payload_size, sizeof(*payload)); 734 return -EINVAL; 735 } 736 737 payload = request->payload; 738 psy_id = payload->psy_id; 739 mutex_lock(&supplies->supplies_lock); 740 if (psy_id >= supplies->supplies_count || 741 !supplies->supply[psy_id].registered) { 742 dev_err(&connection->bundle->dev, 743 "Event received for unconfigured power_supply id: %d\n", 744 psy_id); 745 ret = -EINVAL; 746 goto out_unlock; 747 } 748 749 event = payload->event; 750 /* 751 * we will only handle events after setup is done and before release is 752 * running. For that just check update_interval. 753 */ 754 gbpsy = &supplies->supply[psy_id]; 755 if (gbpsy->update_interval) { 756 ret = -ESHUTDOWN; 757 goto out_unlock; 758 } 759 760 if (event & GB_POWER_SUPPLY_UPDATE) 761 gb_power_supply_status_update(gbpsy); 762 763 out_unlock: 764 mutex_unlock(&supplies->supplies_lock); 765 return ret; 766 } 767 768 static int gb_power_supply_probe(struct gb_bundle *bundle, 769 const struct greybus_bundle_id *id) 770 { 771 struct greybus_descriptor_cport *cport_desc; 772 struct gb_connection *connection; 773 struct gb_power_supplies *supplies; 774 int ret; 775 776 if (bundle->num_cports != 1) 777 return -ENODEV; 778 779 cport_desc = &bundle->cport_desc[0]; 780 if (cport_desc->protocol_id != GREYBUS_PROTOCOL_POWER_SUPPLY) 781 return -ENODEV; 782 783 supplies = kzalloc(sizeof(*supplies), GFP_KERNEL); 784 if (!supplies) 785 return -ENOMEM; 786 787 connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id), 788 gb_supplies_request_handler); 789 if (IS_ERR(connection)) { 790 ret = PTR_ERR(connection); 791 goto out; 792 } 793 794 supplies->connection = connection; 795 connection->private = supplies; 796 797 mutex_init(&supplies->supplies_lock); 798 799 greybus_set_drvdata(bundle, supplies); 800 801 /* We aren't ready to receive an incoming request yet */ 802 ret = gb_connection_enable_tx(connection); 803 if (ret) 804 goto error_connection_destroy; 805 806 ret = gb_power_supplies_setup(supplies); 807 if (ret < 0) 808 goto error_connection_disable; 809 810 /* We are ready to receive an incoming request now, enable RX as well */ 811 ret = gb_connection_enable(connection); 812 if (ret) 813 goto error_connection_disable; 814 815 ret = gb_power_supplies_register(supplies); 816 if (ret < 0) 817 goto error_connection_disable; 818 819 return 0; 820 821 error_connection_disable: 822 gb_connection_disable(connection); 823 error_connection_destroy: 824 gb_connection_destroy(connection); 825 out: 826 _gb_power_supplies_release(supplies); 827 return ret; 828 } 829 830 static void gb_power_supply_disconnect(struct gb_bundle *bundle) 831 { 832 struct gb_power_supplies *supplies = greybus_get_drvdata(bundle); 833 834 gb_connection_disable(supplies->connection); 835 gb_connection_destroy(supplies->connection); 836 837 _gb_power_supplies_release(supplies); 838 } 839 840 static const struct greybus_bundle_id gb_power_supply_id_table[] = { 841 { GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_POWER_SUPPLY) }, 842 { } 843 }; 844 MODULE_DEVICE_TABLE(greybus, gb_power_supply_id_table); 845 846 static struct greybus_driver gb_power_supply_driver = { 847 .name = "power_supply", 848 .probe = gb_power_supply_probe, 849 .disconnect = gb_power_supply_disconnect, 850 .id_table = gb_power_supply_id_table, 851 }; 852 module_greybus_driver(gb_power_supply_driver); 853 854 MODULE_LICENSE("GPL v2"); 855