linux/drivers/siox/siox-core.c
<<
>>
Prefs
   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
  39static bool siox_is_registered;
  40
  41static void siox_master_lock(struct siox_master *smaster)
  42{
  43        mutex_lock(&smaster->lock);
  44}
  45
  46static void siox_master_unlock(struct siox_master *smaster)
  47{
  48        mutex_unlock(&smaster->lock);
  49}
  50
  51static 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
  68static bool siox_device_counter_error(struct siox_device *sdevice,
  69                                      u8 status_clean)
  70{
  71        return (status_clean & SIOX_STATUS_COUNTER) != 0;
  72}
  73
  74static 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
  97static 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 */
 105bool 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}
 113EXPORT_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 */
 119bool 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}
 126EXPORT_SYMBOL_GPL(siox_device_connected);
 127
 128static 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 bits */
 219                if (siox_device_counter_error(sdevice, status_clean)) {
 220                        bool prev_counter_error;
 221
 222                        synced = false;
 223
 224                        /* only report a new error if the last cycle was ok */
 225                        prev_counter_error =
 226                                siox_device_counter_error(sdevice,
 227                                                          prev_status_clean);
 228                        if (!prev_counter_error) {
 229                                sdevice->status_errors++;
 230                                sysfs_notify_dirent(sdevice->status_errors_kn);
 231                        }
 232                }
 233
 234                /* Check type bits */
 235                if (siox_device_type_error(sdevice, status_clean))
 236                        synced = false;
 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
 278static 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
 327static 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
 344static 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
 355static 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
 373static 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
 384static 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
 392static DEVICE_ATTR_RO(type);
 393
 394static 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
 402static DEVICE_ATTR_RO(inbytes);
 403
 404static 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
 412static DEVICE_ATTR_RO(outbytes);
 413
 414static 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
 429static DEVICE_ATTR_RO(status_errors);
 430
 431static 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
 446static DEVICE_ATTR_RO(connected);
 447
 448static 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
 463static DEVICE_ATTR_RO(watchdog);
 464
 465static 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
 480static DEVICE_ATTR_RO(watchdog_errors);
 481
 482static 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};
 492ATTRIBUTE_GROUPS(siox_device);
 493
 494static void siox_device_release(struct device *dev)
 495{
 496        struct siox_device *sdevice = to_siox_device(dev);
 497
 498        kfree(sdevice);
 499}
 500
 501static struct device_type siox_device_type = {
 502        .groups = siox_device_groups,
 503        .release = siox_device_release,
 504};
 505
 506static 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
 515static struct bus_type siox_bus_type = {
 516        .name = "siox",
 517        .match = siox_match,
 518};
 519
 520static 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
 530static 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
 541static 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
 550static 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
 558static 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
 581static DEVICE_ATTR_RW(active);
 582
 583static struct siox_device *siox_device_add(struct siox_master *smaster,
 584                                           const char *type, size_t inbytes,
 585                                           size_t outbytes, u8 statustype);
 586
 587static 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
 610static DEVICE_ATTR_WO(device_add);
 611
 612static void siox_device_remove(struct siox_master *smaster);
 613
 614static 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
 626static DEVICE_ATTR_WO(device_remove);
 627
 628static 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
 636static 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
 657static DEVICE_ATTR_RW(poll_interval_ns);
 658
 659static 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};
 666ATTRIBUTE_GROUPS(siox_master);
 667
 668static void siox_master_release(struct device *dev)
 669{
 670        struct siox_master *smaster = to_siox_master(dev);
 671
 672        kfree(smaster);
 673}
 674
 675static struct device_type siox_master_type = {
 676        .groups = siox_master_groups,
 677        .release = siox_master_release,
 678};
 679
 680struct 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}
 704EXPORT_SYMBOL_GPL(siox_master_alloc);
 705
 706int 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}
 735EXPORT_SYMBOL_GPL(siox_master_register);
 736
 737void 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}
 765EXPORT_SYMBOL_GPL(siox_master_unregister);
 766
 767static 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
 836err_device_register:
 837        /* don't care to make the buffer smaller again */
 838
 839err_buf_alloc:
 840        siox_master_unlock(smaster);
 841
 842        kfree(sdevice);
 843
 844        return ERR_PTR(ret);
 845}
 846
 847static 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
 878int __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}
 908EXPORT_SYMBOL_GPL(__siox_driver_register);
 909
 910static 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}
 924subsys_initcall(siox_init);
 925
 926static void __exit siox_exit(void)
 927{
 928        bus_unregister(&siox_bus_type);
 929}
 930module_exit(siox_exit);
 931
 932MODULE_AUTHOR("Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>");
 933MODULE_DESCRIPTION("Eckelmann SIOX driver core");
 934MODULE_LICENSE("GPL v2");
 935