Changes in uspace/drv/char/i8042/i8042.c [8486c07:56fd7cf] in mainline

File:

• uspace/drv/char/i8042/i8042.c (modified) (10 diffs)

uspace/drv/char/i8042/i8042.c

@@ -39 +39 @@
  */
 
-#include <devman.h>
 #include <device/hw_res.h>
 #include <ddi.h>
@@ -140 +139 @@
 };
 
+/** Get i8042 soft state from device node. */
+static i8042_t *dev_i8042(ddf_dev_t *dev)
+{
+        return ddf_dev_data_get(dev);
+}
+
 /** Wait until it is safe to write to the device. */
 static void wait_ready(i8042_t *dev)
@@ -159 +164 @@
     ipc_call_t *call)
 {
-        if ((!dev) || (!dev->driver_data))
-                return;
-        
-        i8042_t *controller = dev->driver_data;
+        i8042_t *controller = dev_i8042(dev);
         
         const uint8_t status = IPC_GET_ARG1(*call);
@@ -188 +190 @@
     int irq_mouse, ddf_dev_t *ddf_dev)
 {
-        assert(ddf_dev);
-        assert(dev);
-        
-        if (reg_size < sizeof(i8042_regs_t))
-                return EINVAL;
-        
-        if (pio_enable(regs, sizeof(i8042_regs_t), (void **) &dev->regs) != 0)
-                return -1;
+        const size_t range_count = sizeof(i8042_ranges) /
+            sizeof(irq_pio_range_t);
+        irq_pio_range_t ranges[range_count];
+        const size_t cmd_count = sizeof(i8042_cmds) / sizeof(irq_cmd_t);
+        irq_cmd_t cmds[cmd_count];
+
+        int rc;
+        bool kbd_bound = false;
+        bool aux_bound = false;
+
+        dev->kbd_fun = NULL;
+        dev->aux_fun = NULL;
+        
+        if (reg_size < sizeof(i8042_regs_t)) {
+                rc = EINVAL;
+                goto error;
+        }
+        
+        if (pio_enable(regs, sizeof(i8042_regs_t), (void **) &dev->regs) != 0) {
+                rc = EIO;
+                goto error;
+        }
         
         dev->kbd_fun = ddf_fun_create(ddf_dev, fun_inner, "ps2a");
-        if (!dev->kbd_fun)
-                return ENOMEM;
-        
-        int ret = ddf_fun_add_match_id(dev->kbd_fun, "char/xtkbd", 90);
-        if (ret != EOK) {
-                ddf_fun_destroy(dev->kbd_fun);
-                return ret;
-        }
+        if (dev->kbd_fun == NULL) {
+                rc = ENOMEM;
+                goto error;
+        };
+        
+        rc = ddf_fun_add_match_id(dev->kbd_fun, "char/xtkbd", 90);
+        if (rc != EOK)
+                goto error;
         
         dev->aux_fun = ddf_fun_create(ddf_dev, fun_inner, "ps2b");
-        if (!dev->aux_fun) {
-                ddf_fun_destroy(dev->kbd_fun);
-                return ENOMEM;
-        }
-        
-        ret = ddf_fun_add_match_id(dev->aux_fun, "char/ps2mouse", 90);
-        if (ret != EOK) {
-                ddf_fun_destroy(dev->kbd_fun);
-                ddf_fun_destroy(dev->aux_fun);
-                return ret;
-        }
-        
-        dev->kbd_fun->ops = &ops;
-        dev->aux_fun->ops = &ops;
-        dev->kbd_fun->driver_data = dev;
-        dev->aux_fun->driver_data = dev;
+        if (dev->aux_fun == NULL) {
+                rc = ENOMEM;
+                goto error;
+        }
+        
+        rc = ddf_fun_add_match_id(dev->aux_fun, "char/ps2mouse", 90);
+        if (rc != EOK)
+                goto error;
+        
+        ddf_fun_set_ops(dev->kbd_fun, &ops);
+        ddf_fun_set_ops(dev->aux_fun, &ops);
         
         buffer_init(&dev->kbd_buffer, dev->kbd_data, BUFFER_SIZE);
@@ -229 +240 @@
         fibril_mutex_initialize(&dev->write_guard);
         
-        ret = ddf_fun_bind(dev->kbd_fun);
-        CHECK_RET_DESTROY(ret, "Failed to bind keyboard function: %s.",
-            str_error(ret));
-        
-        ret = ddf_fun_bind(dev->aux_fun);
-        CHECK_RET_DESTROY(ret, "Failed to bind mouse function: %s.",
-            str_error(ret));
+        rc = ddf_fun_bind(dev->kbd_fun);
+        if (rc != EOK) {
+                ddf_msg(LVL_ERROR, "Failed to bind keyboard function: %s.",
+                    ddf_fun_get_name(dev->kbd_fun));
+                goto error;
+        }
+        kbd_bound = true;
+        
+        rc = ddf_fun_bind(dev->aux_fun);
+        if (rc != EOK) {
+                ddf_msg(LVL_ERROR, "Failed to bind aux function: %s.",
+                    ddf_fun_get_name(dev->aux_fun));
+                goto error;
+        }
+        aux_bound = true;
         
         /* Disable kbd and aux */
@@ -247 +266 @@
                 (void) pio_read_8(&dev->regs->data);
 
-        const size_t range_count = sizeof(i8042_ranges) /
-            sizeof(irq_pio_range_t);
-        irq_pio_range_t ranges[range_count];
         memcpy(ranges, i8042_ranges, sizeof(i8042_ranges));
         ranges[0].base = (uintptr_t) regs;
 
-        const size_t cmd_count = sizeof(i8042_cmds) / sizeof(irq_cmd_t);
-        irq_cmd_t cmds[cmd_count];
         memcpy(cmds, i8042_cmds, sizeof(i8042_cmds));
         cmds[0].addr = (void *) &(((i8042_regs_t *) regs)->status);
@@ -266 +280 @@
         };
         
-        ret = register_interrupt_handler(ddf_dev, irq_kbd, i8042_irq_handler,
+        rc = register_interrupt_handler(ddf_dev, irq_kbd, i8042_irq_handler,
             &irq_code);
-        CHECK_RET_UNBIND_DESTROY(ret, "Failed set handler for kbd: %s.",
-            str_error(ret));
-        
-        ret = register_interrupt_handler(ddf_dev, irq_mouse, i8042_irq_handler,
+        if (rc != EOK) {
+                ddf_msg(LVL_ERROR, "Failed set handler for kbd: %s.",
+                    ddf_dev_get_name(ddf_dev));
+                goto error;
+        }
+        
+        rc = register_interrupt_handler(ddf_dev, irq_mouse, i8042_irq_handler,
             &irq_code);
-        CHECK_RET_UNBIND_DESTROY(ret, "Failed set handler for mouse: %s.",
-            str_error(ret));
+        if (rc != EOK) {
+                ddf_msg(LVL_ERROR, "Failed set handler for mouse: %s.",
+                    ddf_dev_get_name(ddf_dev));
+                goto error;
+        }
         
         /* Enable interrupts */
-        async_sess_t *parent_sess =
-            devman_parent_device_connect(EXCHANGE_SERIALIZE, ddf_dev->handle,
-            IPC_FLAG_BLOCKING);
-        ret = parent_sess ? EOK : ENOMEM;
-        CHECK_RET_UNBIND_DESTROY(ret, "Failed to create parent connection.");
+        async_sess_t *parent_sess = ddf_dev_parent_sess_get(ddf_dev);
+        assert(parent_sess != NULL);
         
         const bool enabled = hw_res_enable_interrupt(parent_sess);
-        async_hangup(parent_sess);
-        ret = enabled ? EOK : EIO;
-        CHECK_RET_UNBIND_DESTROY(ret, "Failed to enable interrupts: %s.");
+        if (!enabled) {
+                log_msg(LVL_ERROR, "Failed to enable interrupts: %s.",
+                    ddf_dev_get_name(ddf_dev));
+                rc = EIO;
+                goto error;
+        }
         
         /* Enable port interrupts. */
@@ -296 +316 @@
         
         return EOK;
+error:
+        if (kbd_bound)
+                ddf_fun_unbind(dev->kbd_fun);
+        if (aux_bound)
+                ddf_fun_unbind(dev->aux_fun);
+        if (dev->kbd_fun != NULL)
+                ddf_fun_destroy(dev->kbd_fun);
+        if (dev->aux_fun != NULL)
+                ddf_fun_destroy(dev->aux_fun);
+
+        return rc;
 }
 
@@ -315 +346 @@
 static int i8042_write(ddf_fun_t *fun, char *buffer, size_t size)
 {
-        assert(fun);
-        assert(fun->driver_data);
-        
-        i8042_t *controller = fun->driver_data;
+        i8042_t *controller = dev_i8042(ddf_fun_get_dev(fun));
         fibril_mutex_lock(&controller->write_guard);
         
@@ -347 +375 @@
 static int i8042_read(ddf_fun_t *fun, char *data, size_t size)
 {
-        assert(fun);
-        assert(fun->driver_data);
-        
-        i8042_t *controller = fun->driver_data;
+        i8042_t *controller = dev_i8042(ddf_fun_get_dev(fun));
         buffer_t *buffer = (fun == controller->aux_fun) ?
             &controller->aux_buffer : &controller->kbd_buffer;