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 struct bus_type siox_bus_type = {
 516         .name = "siox",
 517         .match = siox_match,
 518 };
 519
 520 static int siox_driver_probe(struct device *dev)
 521 {
 522         struct siox_driver *sdriver = to_siox_driver(dev->driver);
 523         struct siox_device *sdevice = to_siox_device(dev);
 524         int ret;
 525
 526         ret = sdriver->probe(sdevice);
 527         return ret;
 528 }
 529
 530 static int siox_driver_remove(struct device *dev)
 531 {
 532         struct siox_driver *sdriver =
 533                 container_of(dev->driver, struct siox_driver, driver);
 534         struct siox_device *sdevice = to_siox_device(dev);
 535         int ret;
 536
 537         ret = sdriver->remove(sdevice);
 538         return ret;
 539 }
 540
 541 static void siox_driver_shutdown(struct device *dev)
 542 {
 543         struct siox_driver *sdriver =
 544                 container_of(dev->driver, struct siox_driver, driver);
 545         struct siox_device *sdevice = to_siox_device(dev);
 546
 547         sdriver->shutdown(sdevice);
 548 }
 549
 550 static ssize_t active_show(struct device *dev,
 551                            struct device_attribute *attr, char *buf)
 552 {
 553         struct siox_master *smaster = to_siox_master(dev);
 554
 555         return sprintf(buf, "%d\n", smaster->active);
 556 }
 557
 558 static ssize_t active_store(struct device *dev,
 559                             struct device_attribute *attr,
 560                             const char *buf, size_t count)
 561 {
 562         struct siox_master *smaster = to_siox_master(dev);
 563         int ret;
 564         int active;
 565
 566         ret = kstrtoint(buf, 0, &active);
 567         if (ret < 0)
 568                 return ret;
 569
 570         if (active)
 571                 ret = siox_start(smaster);
 572         else
 573                 ret = siox_stop(smaster);
 574
 575         if (ret < 0)
 576                 return ret;
 577
 578         return count;
 579 }
 580
 581 static DEVICE_ATTR_RW(active);
 582
 583 static struct siox_device *siox_device_add(struct siox_master *smaster,
 584                                            const char *type, size_t inbytes,
 585                                            size_t outbytes, u8 statustype);
 586
 587 static ssize_t device_add_store(struct device *dev,
 588                                 struct device_attribute *attr,
 589                                 const char *buf, size_t count)
 590 {
 591         struct siox_master *smaster = to_siox_master(dev);
 592         int ret;
 593         char type[20] = "";
 594         size_t inbytes = 0, outbytes = 0;
 595         u8 statustype = 0;
 596
 597         ret = sscanf(buf, "%19s %zu %zu %hhu", type, &inbytes,
 598                      &outbytes, &statustype);
 599         if (ret != 3 && ret != 4)
 600                 return -EINVAL;
 601
 602         if (strcmp(type, "siox-12x8") || inbytes != 2 || outbytes != 4)
 603                 return -EINVAL;
 604
 605         siox_device_add(smaster, "siox-12x8", inbytes, outbytes, statustype);
 606
 607         return count;
 608 }
 609
 610 static DEVICE_ATTR_WO(device_add);
 611
 612 static void siox_device_remove(struct siox_master *smaster);
 613
 614 static ssize_t device_remove_store(struct device *dev,
 615                                    struct device_attribute *attr,
 616                                    const char *buf, size_t count)
 617 {
 618         struct siox_master *smaster = to_siox_master(dev);
 619
 620         /* XXX? require to write <type> <inbytes> <outbytes> */
 621         siox_device_remove(smaster);
 622
 623         return count;
 624 }
 625
 626 static DEVICE_ATTR_WO(device_remove);
 627
 628 static ssize_t poll_interval_ns_show(struct device *dev,
 629                                      struct device_attribute *attr, char *buf)
 630 {
 631         struct siox_master *smaster = to_siox_master(dev);
 632
 633         return sprintf(buf, "%lld\n", jiffies_to_nsecs(smaster->poll_interval));
 634 }
 635
 636 static ssize_t poll_interval_ns_store(struct device *dev,
 637                                       struct device_attribute *attr,
 638                                       const char *buf, size_t count)
 639 {
 640         struct siox_master *smaster = to_siox_master(dev);
 641         int ret;
 642         u64 val;
 643
 644         ret = kstrtou64(buf, 0, &val);
 645         if (ret < 0)
 646                 return ret;
 647
 648         siox_master_lock(smaster);
 649
 650         smaster->poll_interval = nsecs_to_jiffies(val);
 651
 652         siox_master_unlock(smaster);
 653
 654         return count;
 655 }
 656
 657 static DEVICE_ATTR_RW(poll_interval_ns);
 658
 659 static struct attribute *siox_master_attrs[] = {
 660         &dev_attr_active.attr,
 661         &dev_attr_device_add.attr,
 662         &dev_attr_device_remove.attr,
 663         &dev_attr_poll_interval_ns.attr,
 664         NULL
 665 };
 666 ATTRIBUTE_GROUPS(siox_master);
 667
 668 static void siox_master_release(struct device *dev)
 669 {
 670         struct siox_master *smaster = to_siox_master(dev);
 671
 672         kfree(smaster);
 673 }
 674
 675 static struct device_type siox_master_type = {
 676         .groups = siox_master_groups,
 677         .release = siox_master_release,
 678 };
 679
 680 struct siox_master *siox_master_alloc(struct device *dev,
 681                                       size_t size)
 682 {
 683         struct siox_master *smaster;
 684
 685         if (!dev)
 686                 return NULL;
 687
 688         smaster = kzalloc(sizeof(*smaster) + size, GFP_KERNEL);
 689         if (!smaster)
 690                 return NULL;
 691
 692         device_initialize(&smaster->dev);
 693
 694         smaster->busno = -1;
 695         smaster->dev.bus = &siox_bus_type;
 696         smaster->dev.type = &siox_master_type;
 697         smaster->dev.parent = dev;
 698         smaster->poll_interval = DIV_ROUND_UP(HZ, 40);
 699
 700         dev_set_drvdata(&smaster->dev, &smaster[1]);
 701
 702         return smaster;
 703 }
 704 EXPORT_SYMBOL_GPL(siox_master_alloc);
 705
 706 int siox_master_register(struct siox_master *smaster)
 707 {
 708         int ret;
 709
 710         if (!siox_is_registered)
 711                 return -EPROBE_DEFER;
 712
 713         if (!smaster->pushpull)
 714                 return -EINVAL;
 715
 716         dev_set_name(&smaster->dev, "siox-%d", smaster->busno);
 717
 718         smaster->last_poll = jiffies;
 719         smaster->poll_thread = kthread_create(siox_poll_thread, smaster,
 720                                               "siox-%d", smaster->busno);
 721         if (IS_ERR(smaster->poll_thread)) {
 722                 smaster->active = 0;
 723                 return PTR_ERR(smaster->poll_thread);
 724         }
 725
 726         mutex_init(&smaster->lock);
 727         INIT_LIST_HEAD(&smaster->devices);
 728
 729         ret = device_add(&smaster->dev);
 730         if (ret)
 731                 kthread_stop(smaster->poll_thread);
 732
 733         return ret;
 734 }
 735 EXPORT_SYMBOL_GPL(siox_master_register);
 736
 737 void siox_master_unregister(struct siox_master *smaster)
 738 {
 739         /* remove device */
 740         device_del(&smaster->dev);
 741
 742         siox_master_lock(smaster);
 743
 744         __siox_stop(smaster);
 745
 746         while (smaster->num_devices) {
 747                 struct siox_device *sdevice;
 748
 749                 sdevice = container_of(smaster->devices.prev,
 750                                        struct siox_device, node);
 751                 list_del(&sdevice->node);
 752                 smaster->num_devices--;
 753
 754                 siox_master_unlock(smaster);
 755
 756                 device_unregister(&sdevice->dev);
 757
 758                 siox_master_lock(smaster);
 759         }
 760
 761         siox_master_unlock(smaster);
 762
 763         put_device(&smaster->dev);
 764 }
 765 EXPORT_SYMBOL_GPL(siox_master_unregister);
 766
 767 static struct siox_device *siox_device_add(struct siox_master *smaster,
 768                                            const char *type, size_t inbytes,
 769                                            size_t outbytes, u8 statustype)
 770 {
 771         struct siox_device *sdevice;
 772         int ret;
 773         size_t buf_len;
 774
 775         sdevice = kzalloc(sizeof(*sdevice), GFP_KERNEL);
 776         if (!sdevice)
 777                 return ERR_PTR(-ENOMEM);
 778
 779         sdevice->type = type;
 780         sdevice->inbytes = inbytes;
 781         sdevice->outbytes = outbytes;
 782         sdevice->statustype = statustype;
 783
 784         sdevice->smaster = smaster;
 785         sdevice->dev.parent = &smaster->dev;
 786         sdevice->dev.bus = &siox_bus_type;
 787         sdevice->dev.type = &siox_device_type;
 788
 789         siox_master_lock(smaster);
 790
 791         dev_set_name(&sdevice->dev, "siox-%d-%d",
 792                      smaster->busno, smaster->num_devices);
 793
 794         buf_len = smaster->setbuf_len + inbytes +
 795                 smaster->getbuf_len + outbytes;
 796         if (smaster->buf_len < buf_len) {
 797                 u8 *buf = krealloc(smaster->buf, buf_len, GFP_KERNEL);
 798
 799                 if (!buf) {
 800                         dev_err(&smaster->dev,
 801                                 "failed to realloc buffer to %zu\n", buf_len);
 802                         ret = -ENOMEM;
 803                         goto err_buf_alloc;
 804                 }
 805
 806                 smaster->buf_len = buf_len;
 807                 smaster->buf = buf;
 808         }
 809
 810         ret = device_register(&sdevice->dev);
 811         if (ret) {
 812                 dev_err(&smaster->dev, "failed to register device: %d\n", ret);
 813
 814                 goto err_device_register;
 815         }
 816
 817         smaster->num_devices++;
 818         list_add_tail(&sdevice->node, &smaster->devices);
 819
 820         smaster->setbuf_len += sdevice->inbytes;
 821         smaster->getbuf_len += sdevice->outbytes;
 822
 823         sdevice->status_errors_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 824                                                      "status_errors");
 825         sdevice->watchdog_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 826                                                 "watchdog");
 827         sdevice->watchdog_errors_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 828                                                        "watchdog_errors");
 829         sdevice->connected_kn = sysfs_get_dirent(sdevice->dev.kobj.sd,
 830                                                  "connected");
 831
 832         siox_master_unlock(smaster);
 833
 834         return sdevice;
 835
 836 err_device_register:
 837         /* don't care to make the buffer smaller again */
 838
 839 err_buf_alloc:
 840         siox_master_unlock(smaster);
 841
 842         kfree(sdevice);
 843
 844         return ERR_PTR(ret);
 845 }
 846
 847 static void siox_device_remove(struct siox_master *smaster)
 848 {
 849         struct siox_device *sdevice;
 850
 851         siox_master_lock(smaster);
 852
 853         if (!smaster->num_devices) {
 854                 siox_master_unlock(smaster);
 855                 return;
 856         }
 857
 858         sdevice = container_of(smaster->devices.prev, struct siox_device, node);
 859         list_del(&sdevice->node);
 860         smaster->num_devices--;
 861
 862         smaster->setbuf_len -= sdevice->inbytes;
 863         smaster->getbuf_len -= sdevice->outbytes;
 864
 865         if (!smaster->num_devices)
 866                 __siox_stop(smaster);
 867
 868         siox_master_unlock(smaster);
 869
 870         /*
 871          * This must be done without holding the master lock because we're
 872          * called from device_remove_store which also holds a sysfs mutex.
 873          * device_unregister tries to aquire the same lock.
 874          */
 875         device_unregister(&sdevice->dev);
 876 }
 877
 878 int __siox_driver_register(struct siox_driver *sdriver, struct module *owner)
 879 {
 880         int ret;
 881
 882         if (unlikely(!siox_is_registered))
 883                 return -EPROBE_DEFER;
 884
 885         if (!sdriver->set_data && !sdriver->get_data) {
 886                 pr_err("Driver %s doesn't provide needed callbacks\n",
 887                        sdriver->driver.name);
 888                 return -EINVAL;
 889         }
 890
 891         sdriver->driver.owner = owner;
 892         sdriver->driver.bus = &siox_bus_type;
 893
 894         if (sdriver->probe)
 895                 sdriver->driver.probe = siox_driver_probe;
 896         if (sdriver->remove)
 897                 sdriver->driver.remove = siox_driver_remove;
 898         if (sdriver->shutdown)
 899                 sdriver->driver.shutdown = siox_driver_shutdown;
 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");