2 files changed, 57 insertions, 49 deletions

diff --git a/drivers/connector/cn_queue.c b/drivers/connector/cn_queue.c
index 966632182e2d..9f2f00d82917 100644
--- a/drivers/connector/cn_queue.c
+++ b/drivers/connector/cn_queue.c
@@ -31,16 +31,19 @@
 #include <linux/connector.h>
 #include <linux/delay.h>
 
-static void cn_queue_wrapper(void *data)
+void cn_queue_wrapper(void *data)
 {
-        struct cn_callback_entry *cbq = data;
+        struct cn_callback_data *d = data;
 
-        cbq->cb->callback(cbq->cb->priv);
-        cbq->destruct_data(cbq->ddata);
-        cbq->ddata = NULL;
+        d->callback(d->callback_priv);
+
+        d->destruct_data(d->ddata);
+        d->ddata = NULL;
+
+        kfree(d->free);
 }
 
-static struct cn_callback_entry *cn_queue_alloc_callback_entry(struct cn_callback *cb)
+static struct cn_callback_entry *cn_queue_alloc_callback_entry(char *name, struct cb_id *id, void (*callback)(void *))
 {
         struct cn_callback_entry *cbq;
 
@@ -50,8 +53,11 @@ static struct cn_callback_entry *cn_queue_alloc_callback_entry(struct cn_callbac
                 return NULL;
         }
 
-        cbq->cb = cb;
-        INIT_WORK(&cbq->work, &cn_queue_wrapper, cbq);
+        snprintf(cbq->id.name, sizeof(cbq->id.name), "%s", name);
+        memcpy(&cbq->id.id, id, sizeof(struct cb_id));
+        cbq->data.callback = callback;
+        
+        INIT_WORK(&cbq->work, &cn_queue_wrapper, &cbq->data);
         return cbq;
 }
 
@@ -68,12 +74,12 @@ int cn_cb_equal(struct cb_id *i1, struct cb_id *i2)
         return ((i1->idx == i2->idx) && (i1->val == i2->val));
 }
 
-int cn_queue_add_callback(struct cn_queue_dev *dev, struct cn_callback *cb)
+int cn_queue_add_callback(struct cn_queue_dev *dev, char *name, struct cb_id *id, void (*callback)(void *))
 {
         struct cn_callback_entry *cbq, *__cbq;
         int found = 0;
 
-        cbq = cn_queue_alloc_callback_entry(cb);
+        cbq = cn_queue_alloc_callback_entry(name, id, callback);
         if (!cbq)
                 return -ENOMEM;
 
@@ -82,7 +88,7 @@ int cn_queue_add_callback(struct cn_queue_dev *dev, struct cn_callback *cb)
 
         spin_lock_bh(&dev->queue_lock);
         list_for_each_entry(__cbq, &dev->queue_list, callback_entry) {
-                if (cn_cb_equal(&__cbq->cb->id, &cb->id)) {
+                if (cn_cb_equal(&__cbq->id.id, id)) {
                         found = 1;
                         break;
                 }
@@ -99,7 +105,7 @@ int cn_queue_add_callback(struct cn_queue_dev *dev, struct cn_callback *cb)
 
         cbq->nls = dev->nls;
         cbq->seq = 0;
-        cbq->group = cbq->cb->id.idx;
+        cbq->group = cbq->id.id.idx;
 
         return 0;
 }
@@ -111,7 +117,7 @@ void cn_queue_del_callback(struct cn_queue_dev *dev, struct cb_id *id)
 
         spin_lock_bh(&dev->queue_lock);
         list_for_each_entry_safe(cbq, n, &dev->queue_list, callback_entry) {
-                if (cn_cb_equal(&cbq->cb->id, id)) {
+                if (cn_cb_equal(&cbq->id.id, id)) {
                         list_del(&cbq->callback_entry);
                         found = 1;
                         break;
diff --git a/drivers/connector/connector.c b/drivers/connector/connector.c
index aaf6d468a8b9..bb0b3a8de14b 100644
--- a/drivers/connector/connector.c
+++ b/drivers/connector/connector.c
@@ -84,7 +84,7 @@ int cn_netlink_send(struct cn_msg *msg, u32 __group, int gfp_mask)
                 spin_lock_bh(&dev->cbdev->queue_lock);
                 list_for_each_entry(__cbq, &dev->cbdev->queue_list,
                                     callback_entry) {
-                        if (cn_cb_equal(&__cbq->cb->id, &msg->id)) {
+                        if (cn_cb_equal(&__cbq->id.id, &msg->id)) {
                                 found = 1;
                                 group = __cbq->group;
                         }
@@ -127,42 +127,56 @@ static int cn_call_callback(struct cn_msg *msg, void (*destruct_data)(void *), v
 {
         struct cn_callback_entry *__cbq;
         struct cn_dev *dev = &cdev;
-        int found = 0;
+        int err = -ENODEV;
 
         spin_lock_bh(&dev->cbdev->queue_lock);
         list_for_each_entry(__cbq, &dev->cbdev->queue_list, callback_entry) {
-                if (cn_cb_equal(&__cbq->cb->id, &msg->id)) {
-                        /*
-                         * Let's scream if there is some magic and the
-                         * data will arrive asynchronously here.
-                         * [i.e. netlink messages will be queued].
-                         * After the first warning I will fix it
-                         * quickly, but now I think it is
-                         * impossible. --zbr (2004_04_27).
-                         */
+                if (cn_cb_equal(&__cbq->id.id, &msg->id)) {
                         if (likely(!test_bit(0, &__cbq->work.pending) &&
-                                        __cbq->ddata == NULL)) {
-                                __cbq->cb->priv = msg;
+                                        __cbq->data.ddata == NULL)) {
+                                __cbq->data.callback_priv = msg;
 
-                                __cbq->ddata = data;
-                                __cbq->destruct_data = destruct_data;
+                                __cbq->data.ddata = data;
+                                __cbq->data.destruct_data = destruct_data;
 
                                 if (queue_work(dev->cbdev->cn_queue,
                                                 &__cbq->work))
-                                        found = 1;
+                                        err = 0;
                         } else {
-                                printk("%s: cbq->data=%p, "
-                                       "work->pending=%08lx.\n",
-                                       __func__, __cbq->ddata,
-                                       __cbq->work.pending);
-                                WARN_ON(1);
+                                struct work_struct *w;
+                                struct cn_callback_data *d;
+                                
+                                w = kzalloc(sizeof(*w) + sizeof(*d), GFP_ATOMIC);
+                                if (w) {
+                                        d = (struct cn_callback_data *)(w+1);
+
+                                        d->callback_priv = msg;
+                                        d->callback = __cbq->data.callback;
+                                        d->ddata = data;
+                                        d->destruct_data = destruct_data;
+                                        d->free = w;
+
+                                        INIT_LIST_HEAD(&w->entry);
+                                        w->pending = 0;
+                                        w->func = &cn_queue_wrapper;
+                                        w->data = d;
+                                        init_timer(&w->timer);
+                                        
+                                        if (queue_work(dev->cbdev->cn_queue, w))
+                                                err = 0;
+                                        else {
+                                                kfree(w);
+                                                err = -EINVAL;
+                                        }
+                                } else
+                                        err = -ENOMEM;
                         }
                         break;
                 }
         }
         spin_unlock_bh(&dev->cbdev->queue_lock);
 
-        return found ? 0 : -ENODEV;
+        return err;
 }
 
 /*
@@ -291,22 +305,10 @@ int cn_add_callback(struct cb_id *id, char *name, void (*callback)(void *))
 {
         int err;
         struct cn_dev *dev = &cdev;
-        struct cn_callback *cb;
-
-        cb = kzalloc(sizeof(*cb), GFP_KERNEL);
-        if (!cb)
-                return -ENOMEM;
-
-        scnprintf(cb->name, sizeof(cb->name), "%s", name);
 
-        memcpy(&cb->id, id, sizeof(cb->id));
-        cb->callback = callback;
-
-        err = cn_queue_add_callback(dev->cbdev, cb);
-        if (err) {
-                kfree(cb);
+        err = cn_queue_add_callback(dev->cbdev, name, id, callback);
+        if (err)
                 return err;
-        }
 
         cn_notify(id, 0);