cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

siox-core.c (21711B)


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