siox-core.c 21 KB

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