drivers/siox/siox-core.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2015-2017 Pengutronix, Uwe Kleine-König <kernel@pengutronix.de>
   4  */
   5 #include <linux/kernel.h>
   6 #include <linux/device.h>
   7 #include <linux/module.h>
   8 #include <linux/slab.h>
   9 #include <linux/sysfs.h>
  10
  11 #include "siox.h"
  12
  13 /*
  14  * The lowest bit in the SIOX status word signals if the in-device watchdog is
  15  * ok. If the bit is set, the device is functional.
  16  *
  17  * On writing the watchdog timer is reset when this bit toggles.
  18  */
  19 #define SIOX_STATUS_WDG                 0x01
  20
  21 /*
  22  * Bits 1 to 3 of the status word read as the bitwise negation of what was
  23  * clocked in before. The value clocked in is changed in each cycle and so
  24  * allows to detect transmit/receive problems.
  25  */
  26 #define SIOX_STATUS_COUNTER             0x0e
  27
  28 /*
  29  * Each Siox-Device has a 4 bit type number that is neither 0 nor 15. This is
  30  * available in the upper nibble of the read status.
  31  *
  32  * On write these bits are DC.
  33  */
  34 #define SIOX_STATUS_TYPE                0xf0
  35
  36 #define CREATE_TRACE_POINTS
  37 #include <trace/events/siox.h>
  38
  39 static bool siox_is_registered;
  40
  41 static void siox_master_lock(struct siox_master *smaster)
  42 {
  43         mutex_lock(&smaster->lock);
  44 }
  45
  46 static void siox_master_unlock(struct siox_master *smaster)
  47 {
  48         mutex_unlock(&smaster->lock);
  49 }
  50
  51 static inline u8 siox_status_clean(u8 status_read, u8 status_written)
  52 {
  53         /*
  54          * bits 3:1 of status sample the respective bit in the status
  55          * byte written in the previous cycle but inverted. So if you wrote the
  56          * status word as 0xa before (counter = 0b101), it is expected to get
  57          * back the counter bits as 0b010.
  58          *
  59          * So given the last status written this function toggles the there
  60          * unset counter bits in the read value such that the counter bits in
  61          * the return value are all zero iff the bits were read as expected to
  62          * simplify error detection.
  63          */
  64
  65         return status_read ^ (~status_written & 0xe);
  66 }
  67
  68 static bool siox_device_counter_error(struct siox_device *sdevice,
  69                                       u8 status_clean)
  70 {
  71         return (status_clean & SIOX_STATUS_COUNTER) != 0;
  72 }
  73
  74 static bool siox_device_type_error(struct siox_device *sdevice, u8 status_clean)
  75 {
  76         u8 statustype = (status_clean & SIOX_STATUS_TYPE) >> 4;
  77
  78         /*
  79          * If the device knows which value the type bits should have, check
  80          * against this value otherwise just rule out the invalid values 0b0000
  81          * and 0b1111.
  82          */
  83         if (sdevice->statustype) {
  84                 if (statustype != sdevice->statustype)
  85                         return true;
  86         } else {
  87                 switch (statustype) {
  88                 case 0:
  89                 case 0xf:
  90                         return true;
  91                 }
  92         }
  93
  94         return false;
  95 }
  96
  97 static bool siox_device_wdg_error(struct siox_device *sdevice, u8 status_clean)
  98 {
  99         return (status_clean & SIOX_STATUS_WDG) == 0;
 100 }
 101
 102 /*
 103  * If there is a type or counter error the device is called "unsynced".
 104  */
 105 bool siox_device_synced(struct siox_device *sdevice)
 106 {
 107         if (siox_device_type_error(sdevice, sdevice->status_read_clean))
 108                 return false;
 109
 110         return !siox_device_counter_error(sdevice, sdevice->status_read_clean);
 111
 112 }
 113 EXPORT_SYMBOL_GPL(siox_device_synced);
 114
 115 /*
 116  * A device is called "connected" if it is synced and the watchdog is not
 117  * asserted.
 118  */
 119 bool siox_device_connected(struct siox_device *sdevice)
 120 {
 121         if (!siox_device_synced(sdevice))
 122                 return false;
 123
 124         return !siox_device_wdg_error(sdevice, sdevice->status_read_clean);
 125 }
 126 EXPORT_SYMBOL_GPL(siox_device_connected);
 127
 128 static void siox_poll(struct siox_master *smaster)
 129 {
 130         struct siox_device *sdevice;
 131         size_t i = smaster->setbuf_len;
 132         unsigned int devno = 0;
 133         int unsync_error = 0;
 134
 135         smaster->last_poll = jiffies;
 136
 137         /*
 138          * The counter bits change in each second cycle, the watchdog bit
 139          * toggles each time.
 140          * The counter bits hold values from [0, 6]. 7 would be possible
 141          * theoretically but the protocol designer considered that a bad idea
 142          * for reasons unknown today. (Maybe that's because then the status read
 143          * back has only zeros in the counter bits then which might be confused
 144          * with a stuck-at-0 error. But for the same reason (with s/0/1/) 0
 145          * could be skipped.)
 146          */
 147         if (++smaster->status > 0x0d)
 148                 smaster->status = 0;
 149
 150         memset(smaster->buf, 0, smaster->setbuf_len);
 151
 152         /* prepare data pushed out to devices in buf[0..setbuf_len) */
 153         list_for_each_entry(sdevice, &smaster->devices, node) {
 154                 struct siox_driver *sdriver =
 155                         to_siox_driver(sdevice->dev.driver);
 156                 sdevice->status_written = smaster->status;
 157
 158                 i -= sdevice->inbytes;
 159
 160                 /*
 161                  * If the device or a previous one is unsynced, don't pet the
 162                  * watchdog. This is done to ensure that the device is kept in
 163                  * reset when something is wrong.
 164                  */
 165                 if (!siox_device_synced(sdevice))
 166                         unsync_error = 1;
 167
 168                 if (sdriver && !unsync_error)
 169                         sdriver->set_data(sdevice, sdevice->status_written,
 170                                           &smaster->buf[i + 1]);
 171                 else
 172                         /*
 173                          * Don't trigger watchdog if there is no driver or a
 174                          * sync problem
 175                          */
 176                         sdevice->status_written &= ~SIOX_STATUS_WDG;
 177
 178                 smaster->buf[i] = sdevice->status_written;
 179
 180                 trace_siox_set_data(smaster, sdevice, devno, i);
 181
 182                 devno++;
 183         }
 184
 185         smaster->pushpull(smaster, smaster->setbuf_len, smaster->buf,
 186                           smaster->getbuf_len,
 187                           smaster->buf + smaster->setbuf_len);
 188
 189         unsync_error = 0;
 190
 191         /* interpret data pulled in from devices in buf[setbuf_len..] */
 192         devno = 0;
 193         i = smaster->setbuf_len;
 194         list_for_each_entry(sdevice, &smaster->devices, node) {
 195                 struct siox_driver *sdriver =
 196                         to_siox_driver(sdevice->dev.driver);
 197                 u8 status = smaster->buf[i + sdevice->outbytes - 1];
 198                 u8 status_clean;
 199                 u8 prev_status_clean = sdevice->status_read_clean;
 200                 bool synced = true;
 201                 bool connected = true;
 202
 203                 if (!siox_device_synced(sdevice))
 204                         unsync_error = 1;
 205
 206                 /*
 207                  * If the watchdog bit wasn't toggled in this cycle, report the
 208                  * watchdog as active to give a consistent view for drivers and
 209                  * sysfs consumers.
 210                  */
 211                 if (!sdriver || unsync_error)
 212                         status &= ~SIOX_STATUS_WDG;
 213
 214                 status_clean =
 215                         siox_status_clean(status,
 216                                           sdevice->status_written_lastcycle);
 217
 218                 /* Check counter and type bits */
 219                 if (siox_device_counter_error(sdevice, status_clean) ||
 220                     siox_device_type_error(sdevice, status_clean)) {
 221                         bool prev_error;
 222
 223                         synced = false;
 224
 225                         /* only report a new error if the last cycle was ok */
 226                         prev_error =
 227                                 siox_device_counter_error(sdevice,
 228                                                           prev_status_clean) ||
 229                                 siox_device_type_error(sdevice,
 230                                                        prev_status_clean);
 231
 232                         if (!prev_error) {
 233                                 sdevice->status_errors++;
 234                                 sysfs_notify_dirent(sdevice->status_errors_kn);
 235                         }
 236                 }
 237
 238                 /* If the device is unsynced report the watchdog as active */
 239                 if (!synced) {
 240                         status &= ~SIOX_STATUS_WDG;
 241                         status_clean &= ~SIOX_STATUS_WDG;
 242                 }
 243
 244                 if (siox_device_wdg_error(sdevice, status_clean))
 245                         connected = false;
 246
 247                 /* The watchdog state changed just now */
 248                 if ((status_clean ^ prev_status_clean) & SIOX_STATUS_WDG) {
 249                         sysfs_notify_dirent(sdevice->watchdog_kn);
 250
 251                         if (siox_device_wdg_error(sdevice, status_clean)) {
 252                                 struct kernfs_node *wd_errs =
 253                                         sdevice->watchdog_errors_kn;
 254
 255                                 sdevice->watchdog_errors++;
 256                                 sysfs_notify_dirent(wd_errs);
 257                         }
 258                 }
 259
 260                 if (connected != sdevice->connected)
 261                         sysfs_notify_dirent(sdevice->connected_kn);
 262
 263                 sdevice->status_read_clean = status_clean;
 264                 sdevice->status_written_lastcycle = sdevice->status_written;
 265                 sdevice->connected = connected;
 266
 267                 trace_siox_get_data(smaster, sdevice, devno, status_clean, i);
 268
 269                 /* only give data read to driver if the device is connected */
 270                 if (sdriver && connected)
 271                         sdriver->get_data(sdevice, &smaster->buf[i]);
 272
 273                 devno++;
 274                 i += sdevice->outbytes;
 275         }
 276 }
 277
 278 static int siox_poll_thread(void *data)
 279 {
 280         struct siox_master *smaster = data;
 281         signed long timeout = 0;
 282
 283         get_device(&smaster->dev);
 284
 285         for (;;) {
 286                 if (kthread_should_stop()) {
 287                         put_device(&smaster->dev);
 288                         return 0;
 289                 }
 290
 291                 siox_master_lock(smaster);
 292
 293                 if (smaster->active) {
 294                         unsigned long next_poll =
 295                                 smaster->last_poll + smaster->poll_interval;
 296                         if (time_is_before_eq_jiffies(next_poll))
 297                                 siox_poll(smaster);
 298
 299                         timeout = smaster->poll_interval -
 300                                 (jiffies - smaster->last_poll);
 301                 } else {
 302                         timeout = MAX_SCHEDULE_TIMEOUT;
 303                 }
 304
 305                 /*
 306                  * Set the task to idle while holding the lock. This makes sure
 307                  * that we don't sleep too long when the bus is reenabled before
 308                  * schedule_timeout is reached.
 309                  */
 310                 if (timeout > 0)
 311                         set_current_state(TASK_IDLE);
 312
 313                 siox_master_unlock(smaster);
 314
 315                 if (timeout > 0)
 316                         schedule_timeout(timeout);
 317
 318                 /*
 319                  * I'm not clear if/why it is important to set the state to
 320                  * RUNNING again, but it fixes a "do not call blocking ops when
 321                  * !TASK_RUNNING;"-warning.
 322                  */
 323                 set_current_state(TASK_RUNNING);
 324         }
 325 }
 326
 327 static int __siox_start(struct siox_master *smaster)
 328 {
 329         if (!(smaster->setbuf_len + smaster->getbuf_len))
 330                 return -ENODEV;
 331
 332         if (!smaster->buf)
 333                 return -ENOMEM;
 334
 335         if (smaster->active)
 336                 return 0;
 337
 338         smaster->active = 1;
 339         wake_up_process(smaster->poll_thread);
 340
 341         return 1;
 342 }
 343
 344 static int siox_start(struct siox_master *smaster)
 345 {
 346         int ret;
 347
 348         siox_master_lock(smaster);
 349         ret = __siox_start(smaster);
 350         siox_master_unlock(smaster);
 351
 352         return ret;
 353 }
 354
 355 static int __siox_stop(struct siox_master *smaster)
 356 {
 357         if (smaster->active) {
 358                 struct siox_device *sdevice;
 359
 360                 smaster->active = 0;
 361
 362                 list_for_each_entry(sdevice, &smaster->devices, node) {
 363                         if (sdevice->connected)
 364                                 sysfs_notify_dirent(sdevice->connected_kn);
 365                         sdevice->connected = false;
 366                 }
 367
 368                 return 1;
 369         }
 370         return 0;
 371 }
 372
 373 static int siox_stop(struct siox_master *smaster)
 374 {
 375         int ret;
 376
 377         siox_master_lock(smaster);
 378         ret = __siox_stop(smaster);
 379         siox_master_unlock(smaster);
 380
 381         return ret;
 382 }
 383
 384 static ssize_t type_show(struct device *dev,
 385                          struct device_attribute *attr, char *buf)
 386 {
 387         struct siox_device *sdev = to_siox_device(dev);
 388
 389         return sprintf(buf, "%s\n", sdev->type);
 390 }
 391
 392 static DEVICE_ATTR_RO(type);
 393
 394 static ssize_t inbytes_show(struct device *dev,
 395                             struct device_attribute *attr, char *buf)
 396 {
 397         struct siox_device *sdev = to_siox_device(dev);
 398
 399         return sprintf(buf, "%zu\n", sdev->inbytes);
 400 }
 401
 402 static DEVICE_ATTR_RO(inbytes);
 403
 404 static ssize_t outbytes_show(struct device *dev,
 405                              struct device_attribute *attr, char *buf)
 406 {
 407         struct siox_device *sdev = to_siox_device(dev);
 408
 409         return sprintf(buf, "%zu\n", sdev->outbytes);
 410 }
 411
 412 static DEVICE_ATTR_RO(outbytes);
 413
 414 static ssize_t status_errors_show(struct device *dev,
 415                                   struct device_attribute *attr, char *buf)
 416 {
 417         struct siox_device *sdev = to_siox_device(dev);
 418         unsigned int status_errors;
 419
 420         siox_master_lock(sdev->smaster);
 421
 422         status_errors = sdev->status_errors;
 423
 424         siox_master_unlock(sdev->smaster);
 425
 426         return sprintf(buf, "%u\n", status_errors);
 427 }
 428
 429 static DEVICE_ATTR_RO(status_errors);
 430
 431 static ssize_t connected_show(struct device *dev,
 432                               struct device_attribute *attr, char *buf)
 433 {
 434         struct siox_device *sdev = to_siox_device(dev);
 435         bool connected;
 436
 437         siox_master_lock(sdev->smaster);
 438
 439         connected = sdev->connected;
 440
 441         siox_master_unlock(sdev->smaster);
 442
 443         return sprintf(buf, "%u\n", connected);
 444 }
 445
 446 static DEVICE_ATTR_RO(connected);
 447
 448 static ssize_t watchdog_show(struct device *dev,
 449                              struct device_attribute *attr, char *buf)
 450 {
 451         struct siox_device *sdev = to_siox_device(dev);
 452         u8 status;
 453
 454         siox_master_lock(sdev->smaster);
 455
 456         status = sdev->status_read_clean;
 457
 458         siox_master_unlock(sdev->smaster);
 459
 460         return sprintf(buf, "%d\n", status & SIOX_STATUS_WDG);
 461 }
 462
 463 static DEVICE_ATTR_RO(watchdog);
 464
 465 static ssize_t watchdog_errors_show(struct device *dev,
 466                                     struct device_attribute *attr, char *buf)
 467 {
 468         struct siox_device *sdev = to_siox_device(dev);
 469         unsigned int watchdog_errors;
 470
 471         siox_master_lock(sdev->smaster);
 472
 473         watchdog_errors = sdev->watchdog_errors;
 474
 475         siox_master_unlock(sdev->smaster);
 476
 477         return sprintf(buf, "%u\n", watchdog_errors);
 478 }
 479
 480 static DEVICE_ATTR_RO(watchdog_errors);
 481
 482 static struct attribute *siox_device_attrs[] = {
 483         &dev_attr_type.attr,
 484         &dev_attr_inbytes.attr,
 485         &dev_attr_outbytes.attr,
 486         &dev_attr_status_errors.attr,
 487         &dev_attr_connected.attr,
 488         &dev_attr_watchdog.attr,
 489         &dev_attr_watchdog_errors.attr,
 490         NULL
 491 };
 492 ATTRIBUTE_GROUPS(siox_device);
 493
 494 static void siox_device_release(struct device *dev)
 495 {
 496         struct siox_device *sdevice = to_siox_device(dev);
 497
 498         kfree(sdevice);
 499 }
 500
 501 static struct device_type siox_device_type = {
 502         .groups = siox_device_groups,
 503         .release = siox_device_release,
 504 };
 505
 506 static int siox_match(struct device *dev, struct device_driver *drv)
 507 {
 508         if (dev->type != &siox_device_type)
 509                 return 0;
 510
 511         /* up to now there is only a single driver so keeping this simple */
 512         return 1;
 513 }
 514
 515 static int siox_probe(struct device *dev)
 516 {
 517         struct siox_driver *sdriver = to_siox_driver(dev->driver);
 518         struct siox_device *sdevice = to_siox_device(dev);
 519
 520         return sdriver->probe(sdevice);
 521 }
 522
 523 static int siox_remove(struct device *dev)
 524 {
 525         struct siox_driver *sdriver =
 526                 container_of(dev->driver, struct siox_driver, driver);
 527         struct siox_device *sdevice = to_siox_device(dev);
 528
 529         if (sdriver->remove)
 530                 sdriver->remove(sdevice);
 531
 532         return 0;
 533 }
 534
 535 static void siox_shutdown(struct device *dev)
 536 {
 537         struct siox_device *sdevice = to_siox_device(dev);
 538         struct siox_driver *sdriver;
 539
 540         if (!dev->driver)
 541                 return;
 542
 543         sdriver = container_of(dev->driver, struct siox_driver, driver);
 544         if (sdriver->shutdown)
 545                 sdriver->shutdown(sdevice);
 546 }
 547
 548 static struct bus_type siox_bus_type = {
 549         .name = "siox",
 550         .match = siox_match,
 551         .probe = siox_probe,
 552         .remove = siox_remove,
 553         .shutdown = siox_shutdown,
 554 };
 555
 556 static ssize_t active_show(struct device *dev,
 557                            struct device_attribute *attr, char *buf)
 558 {
 559         struct siox_master *smaster = to_siox_master(dev);
 560
 561         return sprintf(buf, "%d\n", smaster->active);
 562 }
 563
 564 static ssize_t active_store(struct device *dev,
 565                             struct device_attribute *attr,
 566                             const char *buf, size_t count)
 567 {
 568         struct siox_master *smaster = to_siox_master(dev);
 569         int ret;
 570         int active;
 571
 572         ret = kstrtoint(buf, 0, &active);
 573         if (ret < 0)
 574                 return ret;
 575
 576         if (active)
 577                 ret = siox_start(smaster);
 578         else
 579                 ret = siox_stop(smaster);
 580
 581         if (ret < 0)
 582                 return ret;
 583
 584         return count;
 585 }
 586
 587 static DEVICE_ATTR_RW(active);
 588
 589 static struct siox_device *siox_device_add(struct siox_master *smaster,
 590                                            const char *type, size_t inbytes,
 591                                            size_t outbytes, u8 statustype);
 592
 593 static ssize_t device_add_store(struct device *dev,
 594                                 struct device_attribute *attr,
 595                                 const char *buf, size_t count)
 596 {
 597         struct siox_master *smaster = to_siox_master(dev);
 598         int ret;
 599         char type[20] = "";
 600         size_t inbytes = 0, outbytes = 0;
 601         u8 statustype = 0;
 602
 603         ret = sscanf(buf, "%19s %zu %zu %hhu", type, &inbytes,
 604                      &outbytes, &statustype);
 605         if (ret != 3 && ret != 4)
 606                 return -EINVAL;
 607
 608         if (strcmp(type, "siox-12x8") || inbytes != 2 || outbytes != 4)
 609                 return -EINVAL;
 610
 611         siox_device_add(smaster, "siox-12x8", inbytes, outbytes, statustype);
 612
 613         return count;
 614 }
 615
 616 static DEVICE_ATTR_WO(device_add);
 617
 618 static void siox_device_remove(struct siox_master *smaster);
 619
 620 static ssize_t device_remove_store(struct device *dev,
 621                                    struct device_attribute *attr,
 622                                    const char *buf, size_t count)
 623 {
 624         struct siox_master *smaster = to_siox_master(dev);
 625
 626         /* XXX? require to write <type> <inbytes> <outbytes> */
 627         siox_device_remove(smaster);
 628
 629         return count;
 630 }
 631
 632 static DEVICE_ATTR_WO(device_remove);
 633
 634 static ssize_t poll_interval_ns_show(struct device *dev,
 635                                      struct device_attribute *attr, char *buf)
 636 {
 637         struct siox_master *smaster = to_siox_master(dev);
 638
 639         return sprintf(buf, "%lld\n", jiffies_to_nsecs(smaster->poll_interval));
 640 }
 641
 642 static ssize_t poll_interval_ns_store(struct device *dev,
 643                                       struct device_attribute *attr,
 644                                       const char *buf, size_t count)
 645 {
 646         struct siox_master *smaster = to_siox_master(dev);
 647         int ret;
 648         u64 val;
 649
 650         ret = kstrtou64(buf, 0, &val);
 651         if (ret < 0)
 652                 return ret;
 653
 654         siox_master_lock(smaster);
 655
 656         smaster->poll_interval = nsecs_to_jiffies(val);
 657
 658         siox_master_unlock(smaster);
 659
 660         return count;
 661 }
 662
 663 static DEVICE_ATTR_RW(poll_interval_ns);
 664
 665 static struct attribute *siox_master_attrs[] = {
 666         &dev_attr_active.attr,
 667         &dev_attr_device_add.attr,
 668         &dev_attr_device_remove.attr,
 669         &dev_attr_poll_interval_ns.attr,
 670         NULL
 671 };
 672 ATTRIBUTE_GROUPS(siox_master);
 673
 674 static void siox_master_release(struct device *dev)
 675 {
 676         struct siox_master *smaster = to_siox_master(dev);
 677
 678         kfree(smaster);
 679 }
 680
 681 static struct device_type siox_master_type = {
 682         .groups = siox_master_groups,
 683         .release = siox_master_release,
 684 };
 685
 686 struct siox_master *siox_master_alloc(struct device *dev,
 687                                       size_t size)
 688 {
 689         struct siox_master *smaster;
 690
 691         if (!dev)
 692                 return NULL;
 693
 694         smaster = kzalloc(sizeof(*smaster) + size, GFP_KERNEL);
 695         if (!smaster)
 696                 return NULL;
 697
 698         device_initialize(&smaster->dev);
 699
 700         smaster->busno = -1;
 701         smaster->dev.bus = &siox_bus_type;
 702         smaster->dev.type = &siox_master_type;
 703         smaster->dev.parent = dev;
 704         smaster->poll_interval = DIV_ROUND_UP(HZ, 40);
 705
 706         dev_set_drvdata(&smaster->dev, &smaster[1]);
 707
 708         return smaster;
 709 }
 710 EXPORT_SYMBOL_GPL(siox_master_alloc);
 711
 712 int siox_master_register(struct siox_master *smaster)
 713 {
 714         int ret;
 715
 716         if (!siox_is_registered)
 717                 return -EPROBE_DEFER;
 718
 719         if (!smaster->pushpull)
 720                 return -EINVAL;
 721
 722         dev_set_name(&smaster->dev, "siox-%d", smaster->busno);
 723
 724         mutex_init(&smaster->lock);
 725         INIT_LIST_HEAD(&smaster->devices);
 726
 727         smaster->last_poll = jiffies;
 728         smaster->poll_thread = kthread_run(siox_poll_thread, smaster,
 729                                            "siox-%d", smaster->busno);
 730         if (IS_ERR(smaster->poll_thread)) {
 731                 smaster->active = 0;
 732                 return PTR_ERR(smaster->poll_thread);
 733         }
 734
 735         ret = device_add(&smaster->dev);
 736         if (ret)
 737                 kthread_stop(smaster->poll_thread);
 738
 739         return ret;
 740 }
 741 EXPORT_SYMBOL_GPL(siox_master_register);
 742
 743 void siox_master_unregister(struct siox_master *smaster)
 744 {
 745         /* remove device */
 746         device_del(&smaster->dev);
 747
 748         siox_master_lock(smaster);
 749
 750         __siox_stop(smaster);
 751
 752         while (smaster->num_devices) {
 753                 struct siox_device *sdevice;
 754
 755                 sdevice = container_of(smaster->devices.prev,
 756                                        struct siox_device, node);
 757                 list_del(&sdevice->node);
 758                 smaster->num_devices--;
 759
 760                 siox_master_unlock(smaster);
 761
 762                 device_unregister(&sdevice->dev);
 763
 764                 siox_master_lock(smaster);
 765         }
 766
 767         siox_master_unlock(smaster);
 768
 769         put_device(&smaster->dev);
 770 }
 771 EXPORT_SYMBOL_GPL(siox_master_unregister);
 772
 773 static struct siox_device *siox_device_add(struct siox_master *smaster,
 774                                            const char *type, size_t inbytes,
 775                                            size_t outbytes, u8 statustype)
 776 {
 777         struct siox_device *sdevice;
 778         int ret;
 779         size_t buf_len;
 780
 781         sdevice = kzalloc(sizeof(*sdevice), GFP_KERNEL);
 782         if (!sdevice)
 783                 return ERR_PTR(-ENOMEM);
 784
 785         sdevice->type = type;
 786         sdevice->inbytes = inbytes;
 787         sdevice->outbytes = outbytes;
 788         sdevice->statustype = statustype;
 789
 790         sdevice->smaster = smaster;
 791         sdevice->dev.parent = &smaster->dev;
 792         sdevice->dev.bus = &siox_bus_type;
 793         sdevice->dev.type = &siox_device_type;
 794
 795         siox_master_lock(smaster);
 796
 797         dev_set_name(&sdevice->dev, "siox-%d-%d",
 798                      smaster->busno, smaster->num_devices);
 799
 800         buf_len = smaster->setbuf_len + inbytes +
 801                 smaster->getbuf_len + outbytes;
 802         if (smaster->buf_len < buf_len) {
 803                 u8 *buf = krealloc(smaster->buf, buf_len, GFP_KERNEL);
 804
 805                 if (!buf) {
 806                         dev_err(&smaster->dev,
 807                                 "failed to realloc buffer to %zu\n", buf_len);
 808                         ret = -ENOMEM;
 809                         goto err_buf_alloc;
 810                 }
 811
 812                 smaster->buf_len = buf_len;
 813                 smaster->buf = buf;
 814         }
 815
 816         ret = device_register(&sdevice->dev);
 817         if (ret) {
 818                 dev_err(&smaster->dev, "failed to register device: %d\n", ret);
 819
 820                 goto err_device_register;
 821         }
 822
 823         smaster->num_devices++;
 824         list_add_tail(&sdevice->node, &smaster->devices);
 825
 826         smaster->setbuf_len += sdevice->inbytes;
 827         smaster->getbuf_len += sdevice->outbytes;
 828
 829         sdevice->status_errors_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 830                                                      "status_errors");
 831         sdevice->watchdog_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 832                                                 "watchdog");
 833         sdevice->watchdog_errors_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 834                                                        "watchdog_errors");
 835         sdevice->connected_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 836                                                  "connected");
 837
 838         siox_master_unlock(smaster);
 839
 840         return sdevice;
 841
 842 err_device_register:
 843         /* don't care to make the buffer smaller again */
 844
 845 err_buf_alloc:
 846         siox_master_unlock(smaster);
 847
 848         kfree(sdevice);
 849
 850         return ERR_PTR(ret);
 851 }
 852
 853 static void siox_device_remove(struct siox_master *smaster)
 854 {
 855         struct siox_device *sdevice;
 856
 857         siox_master_lock(smaster);
 858
 859         if (!smaster->num_devices) {
 860                 siox_master_unlock(smaster);
 861                 return;
 862         }
 863
 864         sdevice = container_of(smaster->devices.prev, struct siox_device, node);
 865         list_del(&sdevice->node);
 866         smaster->num_devices--;
 867
 868         smaster->setbuf_len -= sdevice->inbytes;
 869         smaster->getbuf_len -= sdevice->outbytes;
 870
 871         if (!smaster->num_devices)
 872                 __siox_stop(smaster);
 873
 874         siox_master_unlock(smaster);
 875
 876         /*
 877          * This must be done without holding the master lock because we're
 878          * called from device_remove_store which also holds a sysfs mutex.
 879          * device_unregister tries to aquire the same lock.
 880          */
 881         device_unregister(&sdevice->dev);
 882 }
 883
 884 int __siox_driver_register(struct siox_driver *sdriver, struct module *owner)
 885 {
 886         int ret;
 887
 888         if (unlikely(!siox_is_registered))
 889                 return -EPROBE_DEFER;
 890
 891         if (!sdriver->probe ||
 892             (!sdriver->set_data && !sdriver->get_data)) {
 893                 pr_err("Driver %s doesn't provide needed callbacks\n",
 894                        sdriver->driver.name);
 895                 return -EINVAL;
 896         }
 897
 898         sdriver->driver.owner = owner;
 899         sdriver->driver.bus = &siox_bus_type;
 900
 901         ret = driver_register(&sdriver->driver);
 902         if (ret)
 903                 pr_err("Failed to register siox driver %s (%d)\n",
 904                        sdriver->driver.name, ret);
 905
 906         return ret;
 907 }
 908 EXPORT_SYMBOL_GPL(__siox_driver_register);
 909
 910 static int __init siox_init(void)
 911 {
 912         int ret;
 913
 914         ret = bus_register(&siox_bus_type);
 915         if (ret) {
 916                 pr_err("Registration of SIOX bus type failed: %d\n", ret);
 917                 return ret;
 918         }
 919
 920         siox_is_registered = true;
 921
 922         return 0;
 923 }
 924 subsys_initcall(siox_init);
 925
 926 static void __exit siox_exit(void)
 927 {
 928         bus_unregister(&siox_bus_type);
 929 }
 930 module_exit(siox_exit);
 931
 932 MODULE_AUTHOR("Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>");
 933 MODULE_DESCRIPTION("Eckelmann SIOX driver core");
 934 MODULE_LICENSE("GPL v2");