From: Miroslav Ondra <ondra@faster.cz>
Date: Tue, 11 Sep 2018 10:50:17 +0000 (+0200)
Subject: move gpio,leds,iio init/remove from unipispi to platform
X-Git-Url: http://git.graph-it.com/?a=commitdiff_plain;h=f90af69a83fce9cf94de7f26bd19c977f3e5a2bf;p=graphit%2Funipi-kernel.git

move gpio,leds,iio init/remove from unipispi to platform
---

diff --git a/modules/unipi/src/unipi_common.h b/modules/unipi/src/unipi_common.h
index bf9698e..69ee25e 100644
--- a/modules/unipi/src/unipi_common.h
+++ b/modules/unipi/src/unipi_common.h
@@ -51,7 +51,7 @@
 #if NEURONSPI_SCHED_REQUIRED > 0
 	#include <uapi/linux/sched/types.h>
 #endif
-#define NEURONSPI_MAJOR_VERSIONSTRING "Version 1.15:2018:09:03 (devel)"
+#define NEURONSPI_MAJOR_VERSIONSTRING "Version 1.15:2018:09:07 (devel)"
 
 #define NEURONSPI_MAX_DEVS				3
 #define NEURONSPI_MAX_UART				16
@@ -69,7 +69,7 @@
 #define NEURONSPI_MAX_BAUD				115200
 #define NEURONSPI_FIFO_SIZE				256
 #define NEURONSPI_FIFO_MIN_CONTINUOUS	50
-#define NEURONSPI_DETAILED_DEBUG		1
+#define NEURONSPI_DETAILED_DEBUG		0
 #define NEURONSPI_LAST_TRANSFER_DELAY	40
 #define MAX_RX_QUEUE_LEN                16
 
@@ -96,28 +96,6 @@ static const u16 NEURONSPI_NO_INTERRUPT_MODELS[NEURONSPI_NO_INTERRUPT_MODELS_LEN
  * Data Structures *
  *******************/
 
-enum neuron_str_attribute_type {
-		NEURON_SATTR_MODEL,
-		NEURON_SATTR_EEPROM,
-		NEURON_SATTR_BOARD_NAME,
-		NEURON_SATTR_GPIO_GROUP_NAME
-};
-
-enum neuron_num_attribute_type {
-		NEURON_NATTR_BOARDCOUNT,
-		NEURON_NATTR_MODE,
-		NEURON_NATTR_CURRENT_VALUE
-};
-
-/*
- * struct neuronspi_devtype
-{
-	u8	name[10];
-	s32	nr_gpio;
-	s32	nr_uart;
-};
-*/
-
 struct neuronspi_op_buffer
 {
     u8  first_message[8];
@@ -161,25 +139,14 @@ struct neuronspi_driver_data
 {
 	s32 neuron_index;
 	u8 reserved_device;
-	//struct spi_driver *spi_driver;
-	//struct neuronspi_char_driver *char_driver;
-	//struct uart_driver *serial_driver; -- this is global variable neuronspi_uart_driver_global
-	//struct neuronspi_uart_data *uart_data; -- this global var neuronspi_uar_data_global
-	struct platform_device *board_device;
-	struct neuronspi_led_driver *led_driver;
-	struct neuronspi_gpio_driver *di_driver;
-	struct neuronspi_gpio_driver *do_driver;
-	struct neuronspi_gpio_driver *ro_driver;
-	struct iio_dev *stm_ai_driver;
-	struct iio_dev *stm_ao_driver;
-	struct iio_dev **sec_ai_driver;
-	struct iio_dev **sec_ao_driver;
 
 	struct kthread_worker   *primary_worker;
 	struct kthread_work		irq_work;
     struct hrtimer			poll_timer;
     int                     poll_enabled;
 
+	struct platform_device *board_device;
+
 	struct regmap *reg_map;
 	struct mutex device_lock;
 	struct neuronspi_board_features *features;
@@ -200,66 +167,22 @@ struct neuronspi_driver_data
 };
 
 
-struct neuronspi_gpio_port {
-	struct spi_device* spi;
-	struct gpio_chip gpio_c;
-	struct platform_device *plat_dev;
-	u8 io_index;
-};
-
-struct neuronspi_gpio_driver {
-    int count;
-    struct neuronspi_gpio_port ports[1];
-};
-
-struct neuronspi_sec_ai_driver
-{
-	struct iio *devices;
-	u16 dev_count;
-};
-
-struct neuronspi_sec_ao_driver
-{
-	struct iio *devices;
-	u16 dev_count;
-};
-
-struct neuronspi_analog_data
-{
-	u32 index;
-	u32 mode;
-	struct spi_device *parent;
-};
-
-
-// Instantiated once per LED
-struct neuronspi_led_driver
-{
-	struct led_classdev	ldev;
-	struct spi_device	*spi;
-	struct kthread_work	led_work;
-	int					id;
-	int					brightness;
-	char				name[sizeof("unipi:green:uled-x1")];
-	spinlock_t			lock;
-};
-
-
+/*
 struct neuronspi_direct_acc
 {
 	void __iomem		*vaddr;
 	u32					size;
 };
+*/
 
 /*********************
  * Data Declarations *
  *********************/
 
-extern struct spi_device* neuronspi_s_dev[NEURONSPI_MAX_DEVS];
-extern struct task_struct *neuronspi_invalidate_thread;
+extern struct spi_device    *neuronspi_s_dev[NEURONSPI_MAX_DEVS];
+extern struct task_struct   *neuronspi_invalidate_thread;
 
-extern int neuronspi_model_id;
+extern int                  neuronspi_model_id;
 
-//#define NEURON_FIRMWARE_VERSION(neuronspi_driver_data) (*(uint16_t*) (neuronspi_driver_data->firmware_version))
 
 #endif /* MODULES_NEURON_SPI_SRC_UNIPI_COMMON_H_ */
diff --git a/modules/unipi/src/unipi_gpio.c b/modules/unipi/src/unipi_gpio.c
index ad2ad72..c913266 100644
--- a/modules/unipi/src/unipi_gpio.c
+++ b/modules/unipi/src/unipi_gpio.c
@@ -144,7 +144,7 @@ struct neuronspi_gpio_driver * neuronspi_di_probe(int io_count, int neuron_index
 struct neuronspi_gpio_driver * neuronspi_ro_probe(int io_count, int neuron_index, struct platform_device *board_device)
 {
 	struct neuronspi_gpio_driver* ro_driver;
-	struct neuronspi_gpio_port* pro_driver;
+	struct neuronspi_gpio_port* gpio_port;
 	int i;
     char buf[20];
 
@@ -152,28 +152,28 @@ struct neuronspi_gpio_driver * neuronspi_ro_probe(int io_count, int neuron_index
 
 	ro_driver = kzalloc(sizeof(struct neuronspi_gpio_driver) + (sizeof(struct neuronspi_gpio_port)) * (io_count-1), GFP_ATOMIC);
 	for (i = 0; i < io_count; i++) {
-		pro_driver = ro_driver->ports + i;
+		gpio_port = ro_driver->ports + i;
 
         scnprintf(buf, 20, "ro_%d_%02d", neuron_index+1, i+1);
-		pro_driver->io_index = i;
-		pro_driver->spi = neuronspi_s_dev[neuron_index];
+		gpio_port->io_index = i;
+		gpio_port->spi = neuronspi_s_dev[neuron_index];
 				
-		pro_driver->plat_dev = platform_device_alloc(buf, -1);
-		pro_driver->plat_dev->dev.parent = &(board_device->dev);
-		pro_driver->plat_dev->dev.groups = neuron_gpio_ro_attr_groups;
-		pro_driver->plat_dev->dev.driver = &neuronspi_spi_driver.driver;
-		platform_device_add(pro_driver->plat_dev);
-
-		platform_set_drvdata(pro_driver->plat_dev, pro_driver);
-		pro_driver->gpio_c.owner = THIS_MODULE;
-		pro_driver->gpio_c.parent = &(pro_driver->plat_dev->dev);
-		pro_driver->gpio_c.label = "neuron_ro";
-		pro_driver->gpio_c.can_sleep = 1;
-		pro_driver->gpio_c.ngpio = 1;
-		pro_driver->gpio_c.base = -1;
-		pro_driver->gpio_c.direction_output = neuronspi_gpio_ro_direction_output;
-		pro_driver->gpio_c.set = neuronspi_gpio_ro_set;
-		gpiochip_add_data(&pro_driver->gpio_c, pro_driver);
+		gpio_port->plat_dev = platform_device_alloc(buf, -1);
+		gpio_port->plat_dev->dev.parent = &(board_device->dev);
+		gpio_port->plat_dev->dev.groups = neuron_gpio_ro_attr_groups;
+		gpio_port->plat_dev->dev.driver = &neuronspi_spi_driver.driver;
+		platform_device_add(gpio_port->plat_dev);
+
+		platform_set_drvdata(gpio_port->plat_dev, gpio_port);
+		gpio_port->gpio_c.owner = THIS_MODULE;
+		gpio_port->gpio_c.parent = &(gpio_port->plat_dev->dev);
+		gpio_port->gpio_c.label = "neuron_ro";
+		gpio_port->gpio_c.can_sleep = 1;
+		gpio_port->gpio_c.ngpio = 1;
+		gpio_port->gpio_c.base = -1;
+		gpio_port->gpio_c.direction_output = neuronspi_gpio_ro_direction_output;
+		gpio_port->gpio_c.set = neuronspi_gpio_ro_set;
+		gpiochip_add_data(&gpio_port->gpio_c, gpio_port);
 
 	}
     ro_driver->count = io_count;
@@ -183,7 +183,7 @@ struct neuronspi_gpio_driver * neuronspi_ro_probe(int io_count, int neuron_index
 struct neuronspi_gpio_driver * neuronspi_do_probe(int io_count, int neuron_index, struct platform_device *board_device)
 {
 	struct neuronspi_gpio_driver* do_driver;
-	struct neuronspi_gpio_port* pdo_driver;
+	struct neuronspi_gpio_port* gpio_port;
 	int i;
     char buf[20];
 
@@ -191,28 +191,28 @@ struct neuronspi_gpio_driver * neuronspi_do_probe(int io_count, int neuron_index
 
 	do_driver = kzalloc(sizeof(struct neuronspi_gpio_driver) + (sizeof(struct neuronspi_gpio_port)) * (io_count-1), GFP_ATOMIC);
 	for (i = 0; i < io_count; i++) {
-		pdo_driver = do_driver->ports + i;
+		gpio_port = do_driver->ports + i;
 
         scnprintf(buf, 20, "do_%d_%02d", neuron_index+1, i+1);
-		pdo_driver->io_index = i;
-		pdo_driver->spi = neuronspi_s_dev[neuron_index];
+		gpio_port->io_index = i;
+		gpio_port->spi = neuronspi_s_dev[neuron_index];
 				
-		pdo_driver->plat_dev = platform_device_alloc(buf, -1);
-		pdo_driver->plat_dev->dev.parent = &(board_device->dev);
-		pdo_driver->plat_dev->dev.groups = neuron_gpio_do_attr_groups;
-		pdo_driver->plat_dev->dev.driver = &neuronspi_spi_driver.driver;
-		platform_device_add(pdo_driver->plat_dev);
-
-		platform_set_drvdata(pdo_driver->plat_dev, pdo_driver);
-		pdo_driver->gpio_c.owner = THIS_MODULE;
-		pdo_driver->gpio_c.parent = &(pdo_driver->plat_dev->dev);
-		pdo_driver->gpio_c.label = "neuron_do";
-		pdo_driver->gpio_c.can_sleep = 1;
-		pdo_driver->gpio_c.ngpio = 1;
-		pdo_driver->gpio_c.base = -1;
-		pdo_driver->gpio_c.direction_output = neuronspi_gpio_do_direction_output;
-		pdo_driver->gpio_c.set = neuronspi_gpio_do_set;
-		gpiochip_add_data(&pdo_driver->gpio_c, pdo_driver);
+		gpio_port->plat_dev = platform_device_alloc(buf, -1);
+		gpio_port->plat_dev->dev.parent = &(board_device->dev);
+		gpio_port->plat_dev->dev.groups = neuron_gpio_do_attr_groups;
+		gpio_port->plat_dev->dev.driver = &neuronspi_spi_driver.driver;
+		platform_device_add(gpio_port->plat_dev);
+
+		platform_set_drvdata(gpio_port->plat_dev, gpio_port);
+		gpio_port->gpio_c.owner = THIS_MODULE;
+		gpio_port->gpio_c.parent = &(gpio_port->plat_dev->dev);
+		gpio_port->gpio_c.label = "neuron_do";
+		gpio_port->gpio_c.can_sleep = 1;
+		gpio_port->gpio_c.ngpio = 1;
+		gpio_port->gpio_c.base = -1;
+		gpio_port->gpio_c.direction_output = neuronspi_gpio_do_direction_output;
+		gpio_port->gpio_c.set = neuronspi_gpio_do_set;
+		gpiochip_add_data(&gpio_port->gpio_c, gpio_port);
 
 	}
     do_driver->count = io_count;
diff --git a/modules/unipi/src/unipi_gpio.h b/modules/unipi/src/unipi_gpio.h
index 4a1f129..f379046 100644
--- a/modules/unipi/src/unipi_gpio.h
+++ b/modules/unipi/src/unipi_gpio.h
@@ -21,20 +21,24 @@
 
 #include "unipi_common.h"
 
+
+struct neuronspi_gpio_port {
+	struct spi_device* spi;
+	struct gpio_chip gpio_c;
+	struct platform_device *plat_dev;
+	u8 io_index;
+};
+
+struct neuronspi_gpio_driver {
+    int count;
+    struct neuronspi_gpio_port ports[1];
+};
+
+
 /*************************
  * Function Declarations *
  *************************/
 
-int neuronspi_gpio_di_direction_input(struct gpio_chip *chip, unsigned offset);
-int neuronspi_gpio_di_direction_output(struct gpio_chip *chip, unsigned offset, int value);
-int	neuronspi_gpio_di_get(struct gpio_chip *chip, unsigned offset);
-int neuronspi_gpio_do_direction_input(struct gpio_chip *chip, unsigned offset);
-int neuronspi_gpio_do_direction_output(struct gpio_chip *chip, unsigned offset, int value);
-void neuronspi_gpio_do_set(struct gpio_chip *chip, unsigned offset, int value);
-int neuronspi_gpio_ro_direction_input(struct gpio_chip *chip, unsigned offset);
-int neuronspi_gpio_ro_direction_output(struct gpio_chip *chip, unsigned offset, int value);
-void neuronspi_gpio_ro_set(struct gpio_chip *chip, unsigned offset, int value);
-
 struct neuronspi_gpio_driver * neuronspi_di_probe(int di_count, int neuron_index, struct platform_device *board_device);
 struct neuronspi_gpio_driver * neuronspi_ro_probe(int ro_count, int neuron_index, struct platform_device *board_device);
 struct neuronspi_gpio_driver * neuronspi_do_probe(int do_count, int neuron_index, struct platform_device *board_device);
diff --git a/modules/unipi/src/unipi_iio.h b/modules/unipi/src/unipi_iio.h
index 2b0eb9e..e7d0d2c 100644
--- a/modules/unipi/src/unipi_iio.h
+++ b/modules/unipi/src/unipi_iio.h
@@ -102,21 +102,34 @@ static const struct iio_chan_spec neuronspi_sec_ao_chan_spec[] = {
 };
 
 
+struct neuronspi_sec_ai_driver
+{
+	struct iio *devices;
+	u16 dev_count;
+};
+
+struct neuronspi_sec_ao_driver
+{
+	struct iio *devices;
+	u16 dev_count;
+};
+
+struct neuronspi_analog_data
+{
+	u32 index;
+	u32 mode;
+	struct spi_device *parent;
+};
+
+
 /*************************
  * Function Declarations *
  *************************/
 
-int neuronspi_iio_stm_ai_read_raw(struct iio_dev *iio_dev, struct iio_chan_spec const *ch, int *val, int *val2, long mask);
-int neuronspi_iio_stm_ao_read_raw(struct iio_dev *iio_dev, struct iio_chan_spec const *ch, int *val, int *val2, long mask);
-int neuronspi_iio_stm_ao_write_raw(struct iio_dev *indio_dev, struct iio_chan_spec const *chan, int val, int val2, long mask);
-int neuronspi_iio_sec_ai_read_raw(struct iio_dev *iio_dev, struct iio_chan_spec const *ch, int *val, int *val2, long mask);
-int neuronspi_iio_sec_ao_write_raw(struct iio_dev *indio_dev, struct iio_chan_spec const *chan, int val, int val2, long mask);
-
 struct iio_dev* neuronspi_stm_ai_probe(int io_count, int neuron_index, struct platform_device *board_device);
 struct iio_dev* neuronspi_stm_ao_probe(int io_count, int neuron_index, struct platform_device *board_device);
 struct iio_dev** neuronspi_sec_ai_probe(int io_count, int neuron_index, struct platform_device *board_device);
 struct iio_dev** neuronspi_sec_ao_probe(int io_count, int neuron_index, struct platform_device *board_device);
 
 
-
 #endif /* MODULES_NEURON_SPI_SRC_UNIPI_IIO_H_ */
diff --git a/modules/unipi/src/unipi_misc.c b/modules/unipi/src/unipi_misc.c
index 9ab94b8..dc286a5 100644
--- a/modules/unipi/src/unipi_misc.c
+++ b/modules/unipi/src/unipi_misc.c
@@ -23,27 +23,19 @@
  * Non-static Functions *
  ************************/
 
-void neuronspi_spi_led_set_brightness(struct spi_device* spi_dev, enum led_brightness brightness, int id)
+void neuronspi_spi_led_set_brightness(struct spi_device* spi_dev, int brightness, u16 coil)
 {
-	struct neuronspi_driver_data *d_data = spi_get_drvdata(spi_dev);
-        u16 coil;
-#if NEURONSPI_DETAILED_DEBUG > 0
-	printk(KERN_INFO "UNIPISPI: SPI LED Set, Dev-CS:%d, led id:%d\n", spi_dev->chip_select, id);
-#endif
-
-	if (d_data->features != NULL) {
-		coil = d_data->features->di_count + d_data->features->do_count + d_data->features->ro_count + id;
-	} else {
-		coil = 8 + id;
-	}
-    unipispi_modbus_write_coil(spi_dev, coil, brightness > 0);
+    unipispi_modbus_write_coil(spi_dev, coil, brightness);
 }
 
 
 void neuronspi_led_proc(struct kthread_work *ws)
 {
 	struct neuronspi_led_driver *led = to_led_driver(ws, led_work);
-	neuronspi_spi_led_set_brightness(led->spi, led->brightness, led->id);
+	neuronspi_spi_led_set_brightness(led->spi, led->brightness > 0, led->coil);
+#if NEURONSPI_DETAILED_DEBUG > 0
+	printk(KERN_INFO "UNIPISPI: SPI LED Set, Dev-CS:%d, led id:%d\n", led->spi->chip_select, led->id);
+#endif
 }
 
 void neuronspi_led_set_brightness(struct led_classdev *ldev, enum led_brightness brightness)
@@ -58,23 +50,27 @@ void neuronspi_led_set_brightness(struct led_classdev *ldev, enum led_brightness
 }
 
 
+
 struct neuronspi_led_driver * neuronspi_led_probe(int led_count, int neuron_index, struct platform_device *board_device)
 {
+    struct spi_device* spi = neuronspi_s_dev[neuron_index];
+	struct neuronspi_driver_data *n_spi = spi_get_drvdata(spi);
 	struct neuronspi_led_driver * led_driver = kzalloc(sizeof(struct neuronspi_led_driver) * led_count, GFP_ATOMIC);
-	int i;
+	int i, coil;
+
+	if (n_spi->features != NULL) {
+		coil = n_spi->features->di_count + n_spi->features->do_count + n_spi->features->ro_count;
+	} else {
+		coil = 8;
+	}
 	
 	for (i = 0; i < led_count; i++) {
 		scnprintf(led_driver[i].name, sizeof(led_driver[i].name), "unipi:green:uled-x%x", i);
-        /*strcpy(led_driver[i].name, "unipi:green:uled-x1");
-		if (i < 9) {
-			led_driver[i].name[18] = i + '1';
-		} else {
-			led_driver[i].name[18] = i - 9 + 'a';
-		}*/
 		// Initialise the rest of the structure
 		led_driver[i].id = i;
+		led_driver[i].coil = coil+i;
 		led_driver[i].brightness = LED_OFF;
-		led_driver[i].spi = neuronspi_s_dev[neuron_index];
+		led_driver[i].spi = spi;
 
 		spin_lock_init(&led_driver[i].lock);
 		led_driver[i].ldev.name = led_driver[i].name;
diff --git a/modules/unipi/src/unipi_misc.h b/modules/unipi/src/unipi_misc.h
index c70c4f3..6305ec2 100644
--- a/modules/unipi/src/unipi_misc.h
+++ b/modules/unipi/src/unipi_misc.h
@@ -21,6 +21,20 @@
 
 #include "unipi_common.h"
 
+// Instantiated once per LED
+struct neuronspi_led_driver
+{
+	struct led_classdev	ldev;
+	struct spi_device	*spi;
+	struct kthread_work	led_work;
+    int                 id;
+    u16                 coil;
+	int					brightness;
+	char				name[sizeof("unipi:green:uled-x1")];
+	spinlock_t			lock;
+};
+
+
 /*************************
  * Function Declarations *
  *************************/
diff --git a/modules/unipi/src/unipi_platform.c b/modules/unipi/src/unipi_platform.c
index 3b34ded..999d188 100644
--- a/modules/unipi/src/unipi_platform.c
+++ b/modules/unipi/src/unipi_platform.c
@@ -19,6 +19,9 @@
 #include "unipi_spi.h"
 #include "unipi_common.h"
 #include "unipi_sysfs.h"
+#include "unipi_misc.h"
+#include "unipi_gpio.h"
+#include "unipi_iio.h"
 
 /***************************
  * Static Data Definitions *
@@ -1798,9 +1801,9 @@ int neuronspi_regmap_hw_gather_write(void *context, const void *reg, size_t reg_
 	u16 *mb_reg_buf = (u16*)reg;
 	//u16 *mb_val_buf = (u16*)val;
 	struct spi_device *spi = context;
-
+#if NEURONSPI_DETAILED_DEBUG > 0
 	printk(KERN_INFO "UNIPISPI: Regmap_hw_gather_write reg[%d](%zu) val(%zu):%8ph\n", *mb_reg_buf, reg_size, val_size, val);
-    
+#endif    
 	if (reg_size == sizeof(u16)) {
 		return unipispi_modbus_write_register(spi, mb_reg_buf[0], *((u16*)val));
 	}
@@ -1931,7 +1934,10 @@ int neuronspi_regmap_hw_read(void *context, const void *reg_buf, size_t reg_size
 	if (context == NULL) {
 		return 0;
 	}
+
+#if NEURONSPI_DETAILED_DEBUG > 0
 	printk(KERN_INFO "UNIPISPI: Regmap_hw_read reg[%d](%zuB) val(%zuB):%8ph\n", *mb_reg_buf, reg_size, val_size, val_buf);
+#endif
 
     if (val_size == sizeof(u16)) {
         return unipispi_modbus_read_register(spi, *mb_reg_buf, (u16*) val_buf);
@@ -2019,14 +2025,62 @@ s32 neuronspi_find_model_id(u32 probe_count)
 }
 
 
+
+void neuronspi_board_device_remove(struct platform_device * board_device)
+{
+    struct neuronspi_board_device_data *board_data = platform_get_drvdata(board_device);
+    struct neuronspi_driver_data *n_spi = board_data->n_spi;
+    int i;
+    
+    if (board_data->led_driver) {
+        for (i = 0; i < n_spi->features->led_count; i++) {
+            led_classdev_unregister(&(board_data->led_driver[i].ldev));
+            kthread_flush_work(&(board_data->led_driver[i].led_work));
+        }
+        kfree(board_data->led_driver);
+        board_data->led_driver = NULL;
+        //unipi_spi_trace(KERN_INFO "UNIPISPI: LED Driver unregistered\n");
+    }
+    if (board_data->di_driver) { neuronspi_gpio_remove(board_data->di_driver); }
+    if (board_data->do_driver) { neuronspi_gpio_remove(board_data->do_driver); }
+    if (board_data->ro_driver) { neuronspi_gpio_remove(board_data->ro_driver); }
+    //unipi_spi_trace(KERN_INFO "UNIPISPI: GPIO Driver unregistered\n");
+        
+    if (board_data->stm_ai_driver) { iio_device_unregister(board_data->stm_ai_driver); }
+    if (board_data->stm_ao_driver) {	iio_device_unregister(board_data->stm_ao_driver); }
+    if (board_data->sec_ai_driver) {
+        for (i = 0; i < n_spi->features->sec_ai_count; i++) {
+            iio_device_unregister(board_data->sec_ai_driver[i]);
+        }
+        kfree(board_data->sec_ai_driver);
+        board_data->sec_ai_driver = NULL;
+    }
+    if (board_data->sec_ao_driver) {
+        for (i = 0; i < n_spi->features->sec_ao_count; i++) {
+            iio_device_unregister(board_data->sec_ao_driver[i]);
+        }
+        kfree(board_data->sec_ao_driver);
+        board_data->sec_ao_driver = NULL;
+    }
+    //unipi_spi_trace(KERN_INFO "UNIPISPI: IIO Driver unregistered\n");
+
+		
+    platform_set_drvdata(board_device, 0);
+    kfree(board_data);
+    platform_device_unregister(board_device);
+
+}
+
+
 struct platform_device * neuronspi_board_device_probe(struct neuronspi_driver_data *n_spi)
 {
     char buf[20];
     struct platform_device * board_device;
+    struct neuronspi_board_device_data *board_data;
+
     scnprintf(buf, 20, "io_group%d", n_spi->neuron_index+1);
+    board_data = kzalloc(sizeof(struct neuronspi_board_device_data), GFP_ATOMIC);
 
-	//strcpy(n_spi->platform_name, "io_group0");
-	//n_spi->platform_name[8] = n_spi->neuron_index + '1';
 
 	board_device = platform_device_alloc(buf, -1);
 	board_device->dev.parent = &(neuron_plc_dev->dev);
@@ -2036,9 +2090,41 @@ struct platform_device * neuronspi_board_device_probe(struct neuronspi_driver_da
 	}
 
 	board_device->dev.driver = &neuronspi_spi_driver.driver;
+
+    board_data->n_spi = n_spi;
 	platform_device_add(board_device);
-	platform_set_drvdata(board_device, n_spi);
+	platform_set_drvdata(board_device, board_data);
+
+	if (n_spi->features) {
+		if (n_spi->features->led_count) {
+			printk(KERN_INFO "UNIPISPI: %d User LEDs detected at nspi%d\n", n_spi->features->led_count, n_spi->neuron_index);
+			board_data->led_driver = neuronspi_led_probe(n_spi->features->led_count, n_spi->neuron_index, board_device);
+		}
+#ifdef CONFIG_GPIOLIB
+		if (n_spi->features->di_count) {
+			board_data->di_driver = neuronspi_di_probe(n_spi->features->di_count, n_spi->neuron_index, board_device);
+		}
+		if (n_spi->features->do_count) {
+			board_data->do_driver = neuronspi_do_probe(n_spi->features->do_count, n_spi->neuron_index, board_device);
+		}
 
+		if (n_spi->features->ro_count) {
+			board_data->ro_driver = neuronspi_ro_probe(n_spi->features->ro_count, n_spi->neuron_index, board_device);
+		}
+#endif
+		if (n_spi->features->stm_ai_count) {
+			board_data->stm_ai_driver = neuronspi_stm_ai_probe(n_spi->features->stm_ai_count, n_spi->neuron_index, board_device);
+		}
+		if (n_spi->features->stm_ao_count) {
+			board_data->stm_ao_driver = neuronspi_stm_ao_probe(n_spi->features->stm_ao_count, n_spi->neuron_index, board_device);
+		}
+		if (n_spi->features->sec_ai_count) {
+			board_data->sec_ai_driver = neuronspi_sec_ai_probe(n_spi->features->sec_ai_count, n_spi->neuron_index, board_device);
+		}
+		if (n_spi->features->sec_ao_count) {
+			board_data->sec_ao_driver = neuronspi_sec_ao_probe(n_spi->features->sec_ao_count, n_spi->neuron_index, board_device);
+		}
+	}
     return board_device;
 }
 
diff --git a/modules/unipi/src/unipi_platform.h b/modules/unipi/src/unipi_platform.h
index 5d84ef7..6f3def2 100644
--- a/modules/unipi/src/unipi_platform.h
+++ b/modules/unipi/src/unipi_platform.h
@@ -20,6 +20,9 @@
  ************/
 
 #include "unipi_common.h"
+#include "unipi_gpio.h"
+#include "unipi_misc.h"
+#include "unipi_iio.h"
 
 /*******************
  * Data Structures *
@@ -136,6 +139,18 @@ struct neuronspi_model_definition
     u32                                 first_cs;
 };
 
+struct neuronspi_board_device_data {
+    struct neuronspi_driver_data *n_spi;
+    struct neuronspi_led_driver *led_driver;
+	struct neuronspi_gpio_driver *di_driver;
+	struct neuronspi_gpio_driver *do_driver;
+	struct neuronspi_gpio_driver *ro_driver;
+	struct iio_dev *stm_ai_driver;
+	struct iio_dev *stm_ao_driver;
+	struct iio_dev **sec_ai_driver;
+	struct iio_dev **sec_ao_driver;
+};
+
 /***************
  * Definitions *
  ***************/
@@ -359,5 +374,6 @@ s32 neuronspi_find_reg_start(struct neuronspi_board_combination *board, u16 regf
 s32 neuronspi_find_model_id(u32 probe_count);
 
 struct platform_device * neuronspi_board_device_probe(struct neuronspi_driver_data *n_spi);
+void neuronspi_board_device_remove(struct platform_device * board_device);
 
 #endif /* MODULES_NEURON_SPI_SRC_UNIPI_PLATFORM_H_ */
diff --git a/modules/unipi/src/unipi_spi.c b/modules/unipi/src/unipi_spi.c
index 833bdac..54abaa7 100644
--- a/modules/unipi/src/unipi_spi.c
+++ b/modules/unipi/src/unipi_spi.c
@@ -941,7 +941,7 @@ s32 neuronspi_spi_probe(struct spi_device *spi)
 	sched_setscheduler(worker->task, SCHED_FIFO, &neuronspi_sched_param);
     n_spi->primary_worker = worker;
     
-/*
+    // Prepare Register map
 	n_spi->reg_map = regmap_init(&(spi->dev), &neuronspi_regmap_bus, spi, &neuronspi_regmap_config_default);
 	spin_lock_init(&n_spi->sysfs_regmap_lock);
 	if (n_spi->features) {
@@ -950,8 +950,8 @@ s32 neuronspi_spi_probe(struct spi_device *spi)
 	} else {
 		n_spi->regstart_table = NULL;
 	}
-*/
-    // save device into global array
+
+    // Save spi device into global array
 	spin_lock_irqsave(neuronspi_probe_spinlock, flags);
 	spi_set_drvdata(spi, n_spi);
 	neuronspi_s_dev[n_spi->neuron_index] = spi;
@@ -972,40 +972,10 @@ s32 neuronspi_spi_probe(struct spi_device *spi)
 		neuron_plc_dev->dev.groups = neuron_plc_attr_groups;
 		platform_device_add(neuron_plc_dev);
 	}
-/*    
+    // Add platform iogroup_x and LEDs, GPIOs, IIOs
     n_spi->board_device = neuronspi_board_device_probe(n_spi);
 
-	if (n_spi->features) {
-		if (n_spi->features->led_count) {
-			printk(KERN_INFO "UNIPISPI: LED: %d User leds detected at CS%d\n", n_spi->features->led_count, spi->chip_select);
-			n_spi->led_driver = neuronspi_led_probe(n_spi->features->led_count, n_spi->neuron_index, n_spi->board_device);
-		}
-#ifdef CONFIG_GPIOLIB
-		if (n_spi->features->di_count) {
-			n_spi->di_driver = neuronspi_di_probe(n_spi->features->di_count, n_spi->neuron_index, n_spi->board_device);
-		}
-		if (n_spi->features->do_count) {
-			n_spi->do_driver = neuronspi_do_probe(n_spi->features->do_count, n_spi->neuron_index, n_spi->board_device);
-		}
-
-		if (n_spi->features->ro_count) {
-			n_spi->ro_driver = neuronspi_ro_probe(n_spi->features->ro_count, n_spi->neuron_index, n_spi->board_device);
-		}
-#endif
-		if (n_spi->features->stm_ai_count) {
-			n_spi->stm_ai_driver = neuronspi_stm_ai_probe(n_spi->features->stm_ai_count, n_spi->neuron_index, n_spi->board_device);
-		}
-		if (n_spi->features->stm_ao_count) {
-			n_spi->stm_ao_driver = neuronspi_stm_ao_probe(n_spi->features->stm_ao_count, n_spi->neuron_index, n_spi->board_device);
-		}
-		if (n_spi->features->sec_ai_count) {
-			n_spi->sec_ai_driver = neuronspi_sec_ai_probe(n_spi->features->sec_ai_count, n_spi->neuron_index, n_spi->board_device);
-		}
-		if (n_spi->features->sec_ao_count) {
-			n_spi->sec_ao_driver = neuronspi_sec_ao_probe(n_spi->features->sec_ao_count, n_spi->neuron_index, n_spi->board_device);
-		}
-	}
-*/
+ 
     n_spi->uart_count_to_probe = uart_count;
 	if (uart_count) {
         if (neuronspi_uart_driver_global != NULL) {	
@@ -1040,10 +1010,10 @@ s32 neuronspi_spi_probe(struct spi_device *spi)
 
 s32 neuronspi_spi_remove(struct spi_device *spi)
 {
-	int i;
     int neuron_index;
 	struct neuronspi_driver_data *n_spi = spi_get_drvdata(spi);
-	if (n_spi) {
+
+    if (n_spi) {
         neuron_index = n_spi->neuron_index; 
 		if (n_spi->no_irq) {
             hrtimer_cancel(&n_spi->poll_timer);
@@ -1051,48 +1021,17 @@ s32 neuronspi_spi_remove(struct spi_device *spi)
             devm_free_irq(&(spi->dev), spi->irq, spi);             
         }
         neuronspi_uart_remove(spi);
-/*
-		if (n_spi->led_driver) {
-			for (i = 0; i < n_spi->features->led_count; i++) {
-				led_classdev_unregister(&(n_spi->led_driver[i].ldev));
-                kthread_flush_work(&(n_spi->led_driver[i].led_work));
-			}
-			kfree(n_spi->led_driver);
-			n_spi->led_driver = NULL;
-            unipi_spi_trace(KERN_INFO "UNIPISPI: LED Driver unregistered\n");
-		}
-        
-		if (n_spi->di_driver) { neuronspi_gpio_remove(n_spi->di_driver); }
-		if (n_spi->do_driver) { neuronspi_gpio_remove(n_spi->do_driver); }
-		if (n_spi->ro_driver) { neuronspi_gpio_remove(n_spi->ro_driver); }
-		unipi_spi_trace(KERN_INFO "UNIPISPI: GPIO Driver unregistered\n");
-        
-		if (n_spi->stm_ai_driver) { iio_device_unregister(n_spi->stm_ai_driver); }
-		if (n_spi->stm_ao_driver) {	iio_device_unregister(n_spi->stm_ao_driver); }
-		if (n_spi->sec_ai_driver) {
-			for (i = 0; i < n_spi->features->sec_ai_count; i++) {
-				iio_device_unregister(n_spi->sec_ai_driver[i]);
-			}
-			kfree(n_spi->sec_ai_driver);
-			n_spi->sec_ai_driver = NULL;
-		}
-		if (n_spi->sec_ao_driver) {
-			for (i = 0; i < n_spi->features->sec_ao_count; i++) {
-				iio_device_unregister(n_spi->sec_ao_driver[i]);
-			}
-			kfree(n_spi->sec_ao_driver);
-			n_spi->sec_ao_driver = NULL;
-		}
-		unipi_spi_trace(KERN_INFO "UNIPISPI: IIO Driver unregistered\n");
-*/
+
 		if (n_spi->board_device) {
-			platform_set_drvdata(n_spi->board_device, 0);
-			platform_device_unregister(n_spi->board_device);
+            neuronspi_board_device_remove(n_spi->board_device);
 		}
-        /*if (n_spi->reg_map) 
-            regmap_exit(n_spi->reg_map); */
+        if (n_spi->reg_map) 
+            regmap_exit(n_spi->reg_map); 
+        if (n_spi->regstart_table)
+            kfree(n_spi->regstart_table);
 
         kthread_destroy_worker(n_spi->primary_worker);
+        // clear global array item
         neuronspi_s_dev[neuron_index] = NULL; 
 		kfree(n_spi);
         printk(KERN_INFO "UNIPISPI: UniPi Board nspi%d removed\n", neuron_index);
@@ -1137,8 +1076,11 @@ int char_register_driver(void)
 	unipi_spi_trace_1(KERN_DEBUG "UNIPISPI: CDEV Device class registered\n");
 
 	// Device driver registration
-	neuronspi_cdrv.dev = device_create_with_groups(neuronspi_cdrv.driver_class, &(neuron_plc_dev->dev), \
-                            MKDEV(major, 0), NULL, neuron_plc_attr_groups, NEURON_DEVICE_NAME);
+	/*neuronspi_cdrv.dev = device_create_with_groups(neuronspi_cdrv.driver_class, &(neuron_plc_dev->dev), \
+                            MKDEV(major, 0), NULL, neuron_plc_attr_groups, NEURON_DEVICE_NAME);*/
+            
+	neuronspi_cdrv.dev = device_create(neuronspi_cdrv.driver_class, &(neuron_plc_dev->dev), MKDEV(major, 0), \
+                            neuron_plc_dev, NEURON_DEVICE_NAME);
 	if (IS_ERR(neuronspi_cdrv.dev)) {
 		class_destroy(neuronspi_cdrv.driver_class);
         unregister_chrdev(major, NEURON_DEVICE_NAME);
@@ -1156,7 +1098,6 @@ void char_unregister_driver(void)
     if (neuronspi_cdrv.major_number < 0) return;
 
 	device_destroy(neuronspi_cdrv.driver_class, MKDEV(neuronspi_cdrv.major_number, 0));     // Destroy the device
-	//class_unregister(neuronspi_cdrv.driver_class);                          				// registrace nebyla volana
 	class_destroy(neuronspi_cdrv.driver_class);                             				// Destroy the class
 	unregister_chrdev(neuronspi_cdrv.major_number, NEURON_DEVICE_NAME);             		// Unregister the major number
 	unipi_spi_trace(KERN_INFO "UNIPISPI: CDEV unloaded\n");
diff --git a/modules/unipi/src/unipi_sysfs.c b/modules/unipi/src/unipi_sysfs.c
index 4961e89..7b32fc3 100644
--- a/modules/unipi/src/unipi_sysfs.c
+++ b/modules/unipi/src/unipi_sysfs.c
@@ -18,6 +18,7 @@
 
 #include "unipi_sysfs.h"
 #include "unipi_spi.h"
+#include "unipi_platform.h"
 
 /*********************
  * Data Declarations *
@@ -93,9 +94,9 @@ static ssize_t neuronspi_spi_show_serial(struct device *dev, struct device_attri
 {
 	ssize_t ret = 0;
 	u32 val[2] = {0, 0};
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->sys_serial_num, val);
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->sys_serial_num + 1, &(val[1]));
@@ -108,9 +109,9 @@ static ssize_t neuronspi_spi_show_hw_version(struct device *dev, struct device_a
 {
 	ssize_t ret = 0;
 	u32 val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->sys_hw_ver, &val);
 		ret = scnprintf(buf, 255, "%x.%x\n", (val & 0xF0) >> 4, val & 0xF);
@@ -122,9 +123,9 @@ static ssize_t neuronspi_spi_show_hw_flash_version(struct device *dev, struct de
 {
 	ssize_t ret = 0;
 	u32 val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->sys_hw_flash_ver, &val);
 		ret = scnprintf(buf, 255, "%x.%x\n", (val & 0xF0) >> 4, val & 0xF);
@@ -132,13 +133,14 @@ static ssize_t neuronspi_spi_show_hw_flash_version(struct device *dev, struct de
 	return ret;
 }
 
+
 static ssize_t neuronspi_spi_show_fw_version(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	ssize_t ret = 0;
 	u32 val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->sys_sw_ver, &val);
 		ret = scnprintf(buf, 255, "%x.%d\n", (val & 0xF00) >> 8, (int)(val & 0xFF));
@@ -150,9 +152,9 @@ static ssize_t neuronspi_spi_show_uart_queue_length(struct device *dev, struct d
 {
 	ssize_t ret = 0;
 	u32 val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map && n_spi->regstart_table->uart_queue_reg) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->uart_queue_reg, &val);
 		ret = scnprintf(buf, 255, "%d\n", val);
@@ -164,9 +166,9 @@ static ssize_t neuronspi_spi_show_uart_config(struct device *dev, struct device_
 {
 	ssize_t ret = 0;
 	u32 val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map && n_spi->regstart_table->uart_conf_reg) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->uart_conf_reg, &val);
 		ret = scnprintf(buf, 255, "%x\n", val);
@@ -178,9 +180,9 @@ static ssize_t neuronspi_spi_store_uart_config(struct device *dev, struct device
 {
 	ssize_t err = 0;
 	unsigned int val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map && n_spi->regstart_table->uart_conf_reg) {
@@ -194,30 +196,18 @@ static ssize_t neuronspi_spi_show_uart_timeout(struct device *dev, struct device
 {
 	ssize_t ret = 0;
 	u16 value = 0;
-/*	u32 val = 0;
-	u8 *inp_buf, *outp_buf;
-	int read_length; */
 	struct spi_device *spi;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 #if NEURONSPI_DETAILED_DEBUG > 0
 	printk(KERN_INFO "UNIPISPI: Index %d\n", n_spi->neuron_index);
 #endif
-	spi = neuronspi_s_dev[n_spi->neuron_index];
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map && n_spi->regstart_table->uart_conf_reg) {
+        spi = neuronspi_s_dev[n_spi->neuron_index];
         if (unipispi_modbus_read_register(spi, 504, &value) == 0) {
              ret = scnprintf(buf, 255, "%d\n", value);
         }
-        /*
-		read_length = neuronspi_spi_compose_single_register_read(504, &inp_buf, &outp_buf);
-		neuronspi___spi_send_message(spi, inp_buf, outp_buf, read_length, n_spi->ideal_frequency, 25, 1, 0);
-		val = outp_buf[11];
-		memcpy(&val, &outp_buf[NEURONSPI_HEADER_LENGTH], sizeof(u16));
-		kfree(inp_buf);
-		kfree(outp_buf);
-		ret = scnprintf(buf, 255, "%d\n", val);
-         */ 
 	}
 	return ret;
 }
@@ -226,26 +216,18 @@ static ssize_t neuronspi_spi_store_uart_timeout(struct device *dev, struct devic
 {
 	ssize_t err = 0;
 	unsigned int val = 0;
-/*	u8 *inp_buf, *outp_buf;
-	int write_length;*/
 	struct spi_device *spi;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 #if NEURONSPI_DETAILED_DEBUG > 0
 	printk(KERN_INFO "UNIPISPI: Index %d\n", n_spi->neuron_index);
 #endif
-	spi = neuronspi_s_dev[n_spi->neuron_index];
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map && n_spi->regstart_table->uart_conf_reg) {
+        spi = neuronspi_s_dev[n_spi->neuron_index];
         unipispi_modbus_write_register(spi, 504, val);
-/*
-		write_length = neuronspi_spi_compose_single_register_write(504, &inp_buf, &outp_buf, val);
-		neuronspi___spi_send_message(spi, inp_buf, outp_buf, write_length, n_spi->ideal_frequency, 25, 1, 0);
-		kfree(inp_buf);
-		kfree(outp_buf);
-*/
 	}
 err_end:
 	return count;
@@ -255,9 +237,9 @@ static ssize_t neuronspi_spi_show_watchdog_status(struct device *dev, struct dev
 {
 	ssize_t ret = 0;
 	u32 val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->wd_val_reg, &val);
 		ret = scnprintf(buf, 255, "%x\n", val);
@@ -269,9 +251,9 @@ static ssize_t neuronspi_spi_store_watchdog_status(struct device *dev, struct de
 {
 	ssize_t err = 0;
 	unsigned int val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
@@ -285,9 +267,9 @@ static ssize_t neuronspi_spi_show_watchdog_timeout(struct device *dev, struct de
 {
 	ssize_t ret = 0;
 	u32 val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
 		regmap_read(n_spi->reg_map, n_spi->regstart_table->wd_timeout_reg, &val);
 		ret = scnprintf(buf, 255, "%d\n", val);
@@ -295,14 +277,13 @@ static ssize_t neuronspi_spi_show_watchdog_timeout(struct device *dev, struct de
 	return ret;
 }
 
-#if 1
 static ssize_t neuronspi_spi_store_watchdog_timeout(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
 {
 	ssize_t err = 0;
 	unsigned int val = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
 	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
@@ -312,29 +293,6 @@ static ssize_t neuronspi_spi_store_watchdog_timeout(struct device *dev, struct d
 err_end:
 	return count;
 }
-#else
-static ssize_t neuronspi_spi_store_watchdog_timeout(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
-{
-	ssize_t err = 0;
-	unsigned int val = 0;
-	struct spi_device *spi;
-	struct neuronspi_driver_data *n_spi;
-	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
-#if NEURONSPI_DETAILED_DEBUG > 0
-	printk(KERN_INFO "UNIPISPI: Index %d\n", n_spi->neuron_index);
-#endif
-	spi = neuronspi_s_dev[n_spi->neuron_index];
-	err = kstrtouint(buf, 0, &val);
-	if ((err < 0) || (val < 0)) goto err_end;
-	if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map) {
-        if (val > 0xffff) val = 0xffff;
-        unipispi_modbus_write_register(spi, n_spi->regstart_table->wd_timeout_reg, val);
-	}
-err_end:
-	return count;
-}
-#endif
 
 
 static ssize_t neuronspi_spi_gpio_show_pwm_presc(struct device *dev, struct device_attribute *attr, char *buf)
@@ -754,10 +712,10 @@ static ssize_t neuronspi_spi_show_register(struct device *dev, struct device_att
 	u16 val = 0;
     u16 reg;
 	unsigned long flags;
-	struct neuronspi_driver_data *n_spi;
 	struct spi_device *spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	spi = neuronspi_s_dev[n_spi->neuron_index];
 	if (n_spi && n_spi->reg_map) {
 		spin_lock_irqsave(&n_spi->sysfs_regmap_lock, flags);
@@ -782,10 +740,10 @@ static ssize_t neuronspi_spi_store_register_value(struct device *dev, struct dev
 	unsigned int val = 0;
     u16 reg;
 	unsigned long flags;
-	struct neuronspi_driver_data *n_spi;
 	struct spi_device *spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	spi = neuronspi_s_dev[n_spi->neuron_index];
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
@@ -807,9 +765,9 @@ static ssize_t neuronspi_spi_store_register(struct device *dev, struct device_at
 	ssize_t err = 0;
 	unsigned int val = 0;
 	unsigned long flags;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
 	if (n_spi && n_spi->reg_map && val < 65536) {
@@ -829,11 +787,11 @@ static ssize_t neuronspi_show_regmap(struct device *dev, struct device_attribute
 	unsigned int target = 0;
 	u32 val = 0;
 	unsigned long flags;
-	struct neuronspi_driver_data *n_spi;
-	struct spi_device *spi;
+	//struct spi_device *spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
-	spi = neuronspi_s_dev[n_spi->neuron_index];
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
+	//spi = neuronspi_s_dev[n_spi->neuron_index];
 	if (n_spi && n_spi->reg_map) {
 		spin_lock_irqsave(&n_spi->sysfs_regmap_lock, flags);
 		target = n_spi->sysfs_regmap_target;
@@ -849,9 +807,9 @@ static ssize_t neuronspi_store_regmap(struct device *dev, struct device_attribut
 	ssize_t err = 0;
 	unsigned int val = 0;
 	unsigned long flags;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
 	if (n_spi && n_spi->reg_map && val < 65536) {
@@ -869,9 +827,9 @@ static ssize_t neuronspi_store_regmap_value(struct device *dev, struct device_at
 	unsigned int val = 0;
 	unsigned int target = 0;
 	unsigned long flags;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	err = kstrtouint(buf, 0, &val);
 	if (err < 0) goto err_end;
 	if (n_spi && n_spi->reg_map && val < 65536) {
@@ -887,9 +845,9 @@ err_end:
 static ssize_t neuronspi_spi_show_board(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	ssize_t ret = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi->combination_id != 0xFF && n_spi->combination_id < NEURONSPI_BOARDTABLE_LEN) {
 		ret = scnprintf(buf, 255, "%s\n", NEURONSPI_BOARDTABLE[n_spi->combination_id].definition->combination_name);
 	}
@@ -899,9 +857,9 @@ static ssize_t neuronspi_spi_show_board(struct device *dev, struct device_attrib
 static ssize_t neuronspi_spi_show_lboard_id(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	ssize_t ret = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi->combination_id != 0xFF && n_spi->combination_id < NEURONSPI_BOARDTABLE_LEN) {
 		ret = scnprintf(buf, 255, "%d\n", NEURONSPI_BOARDTABLE[n_spi->combination_id].definition->lower_board_id);
 	}
@@ -911,9 +869,9 @@ static ssize_t neuronspi_spi_show_lboard_id(struct device *dev, struct device_at
 static ssize_t neuronspi_spi_show_uboard_id(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	ssize_t ret = 0;
-	struct neuronspi_driver_data *n_spi;
 	struct platform_device *plat = to_platform_device(dev);
-	n_spi = platform_get_drvdata(plat);
+    struct neuronspi_board_device_data *board_device_data = platform_get_drvdata(plat);
+	struct neuronspi_driver_data *n_spi = board_device_data->n_spi;
 	if (n_spi->combination_id != 0xFF && n_spi->combination_id < NEURONSPI_BOARDTABLE_LEN) {
 		ret = scnprintf(buf, 255, "%d\n", NEURONSPI_BOARDTABLE[n_spi->combination_id].definition->upper_board_id);
 	}
@@ -928,7 +886,7 @@ static ssize_t neuronspi_spi_gpio_show_do_prefix(struct device *dev, struct devi
 	struct platform_device *plat = to_platform_device(dev);
 	n_do = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_do->spi);
-	if (n_spi->features && n_spi->features->do_count > 0 && n_spi->do_driver) {
+	if (n_spi->features && n_spi->features->do_count > 0) {
 		ret = scnprintf(buf, 255, "%s_%d\n", n_do->gpio_c.label, n_spi->neuron_index + 1);
 	}
 	return ret;
@@ -942,7 +900,7 @@ static ssize_t neuronspi_spi_gpio_show_di_prefix(struct device *dev, struct devi
 	struct platform_device *plat = to_platform_device(dev);
 	n_di = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_di->spi);
-	if (n_spi->features && n_spi->features->di_count > 0 && n_spi->di_driver) {
+	if (n_spi->features && n_spi->features->di_count > 0) {
 		ret = scnprintf(buf, 255, "%s_%d\n", n_di->gpio_c.label, n_spi->neuron_index + 1);
 	}
 	return ret;
@@ -956,7 +914,7 @@ static ssize_t neuronspi_spi_gpio_show_ro_prefix(struct device *dev, struct devi
 	struct platform_device *plat = to_platform_device(dev);
 	n_ro = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_ro->spi);
-	if (n_spi->features && n_spi->features->ro_count > 0 && n_spi->ro_driver) {
+	if (n_spi->features && n_spi->features->ro_count > 0) {
 		ret = scnprintf(buf, 255, "%s_%d\n", n_ro->gpio_c.label, n_spi->neuron_index + 1);
 	}
 	return ret;
@@ -970,7 +928,7 @@ static ssize_t neuronspi_spi_gpio_show_do_base(struct device *dev, struct device
 	struct platform_device *plat = to_platform_device(dev);
 	n_do = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_do->spi);
-	if (n_spi->features && n_spi->features->do_count > 0 && n_spi->do_driver) {
+	if (n_spi->features && n_spi->features->do_count > 0) {
 		ret = scnprintf(buf, 255, "%d\n", n_do->gpio_c.base);
 	}
 	return ret;
@@ -984,7 +942,7 @@ static ssize_t neuronspi_spi_gpio_show_di_base(struct device *dev, struct device
 	struct platform_device *plat = to_platform_device(dev);
 	n_di = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_di->spi);
-	if (n_spi->features && n_spi->features->di_count > 0 && n_spi->di_driver) {
+	if (n_spi->features && n_spi->features->di_count > 0) {
 		ret = scnprintf(buf, 255, "%d\n", n_di->gpio_c.base);
 	}
 	return ret;
@@ -998,7 +956,7 @@ static ssize_t neuronspi_spi_gpio_show_ro_base(struct device *dev, struct device
 	struct platform_device *plat = to_platform_device(dev);
 	n_ro = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_ro->spi);
-	if (n_spi->features && n_spi->features->ro_count > 0 && n_spi->ro_driver) {
+	if (n_spi->features && n_spi->features->ro_count > 0 ) {
 		ret = scnprintf(buf, 255, "%d\n", n_ro->gpio_c.base);
 	}
 	return ret;
@@ -1012,7 +970,7 @@ static ssize_t neuronspi_spi_gpio_show_do_count(struct device *dev, struct devic
 	struct platform_device *plat = to_platform_device(dev);
 	n_do = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_do->spi);
-	if (n_spi->features && n_spi->features->do_count > 0 && n_spi->do_driver) {
+	if (n_spi->features && n_spi->features->do_count > 0) {
 		ret = scnprintf(buf, 255, "%d\n", n_do->gpio_c.ngpio);
 	}
 	return ret;
@@ -1026,7 +984,7 @@ static ssize_t neuronspi_spi_gpio_show_di_count(struct device *dev, struct devic
 	struct platform_device *plat = to_platform_device(dev);
 	n_di = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_di->spi);
-	if (n_spi->features && n_spi->features->di_count > 0 && n_spi->di_driver) {
+	if (n_spi->features && n_spi->features->di_count > 0) {
 		ret = scnprintf(buf, 255, "%d\n", n_di->gpio_c.ngpio);
 	}
 	return ret;
@@ -1040,7 +998,7 @@ static ssize_t neuronspi_spi_gpio_show_ro_count(struct device *dev, struct devic
 	struct platform_device *plat = to_platform_device(dev);
 	n_ro = platform_get_drvdata(plat);
 	n_spi = spi_get_drvdata(n_ro->spi);
-	if (n_spi->features && n_spi->features->ro_count > 0 && n_spi->ro_driver) {
+	if (n_spi->features && n_spi->features->ro_count > 0 ) {
 		ret = scnprintf(buf, 255, "%d\n", n_ro->gpio_c.ngpio);
 	}
 	return ret;
@@ -1086,9 +1044,6 @@ static ssize_t neuronspi_iio_show_stm_ao_mode(struct device *dev, struct device_
 	struct spi_device *spi = ao_data->parent;
 	struct neuronspi_driver_data *n_spi = spi_get_drvdata(spi);
 	regmap_read(n_spi->reg_map, n_spi->regstart_table->stm_ao_mode_reg + ao_data->index, &val);
-#if NEURONSPI_DETAILED_DEBUG > 0
-	printk(KERN_INFO "UNIPISPI: Mode register %d set to %x", n_spi->regstart_table->stm_ao_mode_reg + ao_data->index, val);
-#endif
 	ret = scnprintf(buf, 255, "%d\n", val);
 	return ret;
 }
diff --git a/modules/unipi/src/unipi_tty.c b/modules/unipi/src/unipi_tty.c
index c8e0acc..53dbd9e 100644
--- a/modules/unipi/src/unipi_tty.c
+++ b/modules/unipi/src/unipi_tty.c
@@ -1,7 +1,9 @@
 /*
  * Implements line discpline for using with Neuron/Axon.
+ * 
+ * Author: Miroslav Ondra <ondra@faster.cz>
  *
- * Derivated from n_tty.c --- implements the N_TTY line discipline.
+ * Derivated from n_tty.c --- implements the N_PROFIBUS line discipline.
  *
  * This code used to be in tty_io.c, but things are getting hairy
  * enough that it made sense to split things off.  (The N_TTY
diff --git a/modules/unipi/src/unipi_uart.c b/modules/unipi/src/unipi_uart.c
index f11f337..93a8add 100644
--- a/modules/unipi/src/unipi_uart.c
+++ b/modules/unipi/src/unipi_uart.c
@@ -41,23 +41,21 @@
  * Data Definitions *
  ********************/
 
-struct neuronspi_uart_data* neuronspi_uart_data_global = NULL;
-struct uart_driver* neuronspi_uart_driver_global = NULL;
-//unsigned long neuronspi_lines;
+struct neuronspi_uart_data  *neuronspi_uart_data_global = NULL;
+struct uart_driver          *neuronspi_uart_driver_global = NULL;
 
-//static struct sched_param neuronspi_sched_param = { .sched_priority = MAX_RT_PRIO / 2 };
-
-void neuronspi_uart_update_timeout(struct neuronspi_port *n_port, unsigned int cflag, unsigned int baud);
 
 /********************
  * Static Functions *
  ********************/
 
+void neuronspi_uart_update_timeout(struct neuronspi_port *n_port, unsigned int cflag, unsigned int baud);
+
 #define NEURONSPI_UART_CFLAGS_REGISTER 	500
 #define NEURONSPI_UART_IFLAGS_REGISTER 	502
 #define NEURONSPI_UART_LDISC_REGISTER 	503
 #define NEURONSPI_UART_TIMEOUT_REGISTER 504
-//#define NEURONSPI_UART_FIFO_REGISTER    505
+#define NEURONSPI_UART_FIFO_REGISTER    505
 
 static inline int port_to_uartregs(u8 port, u16 reg)
 {
@@ -170,7 +168,6 @@ int	neuronspi_uart_ioctl (struct uart_port *port, unsigned int ioctl_code, unsig
     u32 value;
 	struct neuronspi_port *n_port = to_neuronspi_port(port, port);
 	struct spi_device *spi = neuronspi_s_dev[n_port->dev_index];
-	//struct neuronspi_driver_data *n_spi = spi_get_drvdata(spi);
 
 	switch (ioctl_code) {
 	case TIOCSETD: {
@@ -282,7 +279,6 @@ void neuronspi_uart_start_tx(struct uart_port *port)
 	unipi_uart_trace("Start TX\n");
 
 	if (!kthread_queue_work(n_port->n_spi->primary_worker, &n_port->tx_work)) {
-	//if (!kthread_queue_work(&neuronspi_uart_data_global->kworker, &n_port->tx_work)) {
 		//unipi_uart_trace("TX WORK OVERFLOW\n");
 	}
 }
@@ -395,7 +391,6 @@ void neuronspi_uart_handle_tx(struct neuronspi_port *port)
 		spin_unlock_irqrestore(&port->port.lock, flags);
 		port->port.x_char = 0;
         kthread_queue_work(port->n_spi->primary_worker, &port->tx_work);
-        //kthread_queue_work(&neuronspi_uart_data_global->kworker, &port->tx_work);
 		return;
 	}
 
@@ -462,20 +457,12 @@ void neuronspi_uart_handle_tx(struct neuronspi_port *port)
         if (to_send) {
             // reschedule work
 			kthread_queue_work(port->n_spi->primary_worker, &port->tx_work);
-			//kthread_queue_work(&neuronspi_uart_data_global->kworker, &port->tx_work);
 		} else {
             // set timer to check tx_empty
             unipi_uart_trace_1("ttyNS%d Handle TX. Start timer=%llu", port->port.line, to_send_packet * port->one_char_nsec);
             start_tx_timer(port, to_send_packet, 2);
         }
 	}
-/*
-	spin_lock_irqsave(&port->port.lock, flags);
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS) {
-		uart_write_wakeup(&port->port);
-	}
-	spin_unlock_irqrestore(&port->port.lock, flags);
-*/
 }
 
 // callback of tx_timer. Schedule port->tx_work
@@ -484,7 +471,6 @@ static enum hrtimer_restart neuronspi_uart_timer_func(struct hrtimer *timer)
     struct neuronspi_port* n_port = ((container_of((timer), struct neuronspi_port, tx_timer)));
 
 	kthread_queue_work(n_port->n_spi->primary_worker, &n_port->tx_work);
-	//kthread_queue_work(&neuronspi_uart_data_global->kworker, &n_port->tx_work);
 	return HRTIMER_NORESTART;
 }
 
@@ -539,7 +525,6 @@ void neuronspi_uart_rx_proc(struct kthread_work *ws)
 
     if (n_port->rx_remain > 0) {
 		kthread_queue_work(n_port->n_spi->primary_worker, &n_port->rx_work);
-        //kthread_queue_work(&neuronspi_uart_data_global->kworker, &n_port->rx_work);
     }
 	kfree(recv_buf);
 }
@@ -644,7 +629,9 @@ int neuronspi_uart_probe(struct spi_device* spi, struct neuronspi_driver_data *n
             port->rx_queue_secondary = kzalloc(MAX_RX_QUEUE_LEN, GFP_ATOMIC); 
 
             port->tx_fifo_len = 0x7fff; //set it to big number; invoke reading current value from Neuron
-            if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map && n_spi->regstart_table->uart_queue_reg) {
+            if (n_spi && (n_spi->firmware_version >= 0x0519) ) {
+                port->tx_fifo_reg = port_to_uartregs(i,NEURONSPI_UART_FIFO_REGISTER);        // define modbus register
+            } else if (n_spi && n_spi->combination_id != 0xFF && n_spi->reg_map && n_spi->regstart_table->uart_queue_reg) {
                 port->tx_fifo_reg = n_spi->regstart_table->uart_queue_reg;      // define modbus register
             }
 
@@ -692,20 +679,6 @@ int neuronspi_uart_probe_all(void)
             unipi_uart_trace("Allocated port structure for %d ttyNS devices", NEURONSPI_MAX_UART);
         }
         
-        /*
-        if (neuronspi_uart_data_global->kworker_task == NULL) {
-
-            kthread_init_worker(&neuronspi_uart_data_global->kworker);
-
-            neuronspi_uart_data_global->kworker_task = kthread_run(kthread_worker_fn, &neuronspi_uart_data_global->kworker,
-						  "unipiuart");
-            if (IS_ERR(neuronspi_uart_data_global->kworker_task)) {
-                ret = PTR_ERR(neuronspi_uart_data_global->kworker_task);
-            }
-            sched_setscheduler(neuronspi_uart_data_global->kworker_task, SCHED_FIFO, &neuronspi_sched_param);
-            unipi_uart_trace("KWorker unipiuart started\n");
-        }
-        */
         ret = neuronspi_uart_probe(spi, n_spi);
         if (ret)  break; // max number of uarts reached
 	}
diff --git a/modules/unipi/src/unipi_uart.h b/modules/unipi/src/unipi_uart.h
index 57a85a8..636515a 100644
--- a/modules/unipi/src/unipi_uart.h
+++ b/modules/unipi/src/unipi_uart.h
@@ -27,8 +27,6 @@
  ***************/
 
 
-#define NEURONSPI_MAX_TX_WORK	4
-
 /*************************
  * Function Declarations *
  *************************/
@@ -46,7 +44,7 @@ int neuronspi_uart_probe_all(void);
  * Data Declarations *
  *********************/
 
-extern struct neuronspi_uart_data* neuronspi_uart_data_global;
-extern struct uart_driver* neuronspi_uart_driver_global;
+extern struct neuronspi_uart_data   *neuronspi_uart_data_global;
+extern struct uart_driver           *neuronspi_uart_driver_global;
 
 #endif /* MODULES_NEURON_SPI_SRC_UNIPI_UART_H_ */