diff --git a/drivers/connector/cn_queue.c b/drivers/connector/cn_queue.c
index 96663218..9f2f00d 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 @@
 		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 @@
 	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 @@
 
 	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 @@
 
 	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 @@
 
 	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 aaf6d46..bb0b3a8 100644
--- a/drivers/connector/connector.c
+++ b/drivers/connector/connector.c
@@ -84,7 +84,7 @@
 		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 @@
 {
 	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 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);
 
diff --git a/include/linux/connector.h b/include/linux/connector.h
index 96de263..86d4b0a 100644
--- a/include/linux/connector.h
+++ b/include/linux/connector.h
@@ -104,12 +104,19 @@
 	struct sock *nls;
 };
 
-struct cn_callback {
+struct cn_callback_id {
 	unsigned char name[CN_CBQ_NAMELEN];
-
 	struct cb_id id;
+};
+
+struct cn_callback_data {
+	void (*destruct_data) (void *);
+	void *ddata;
+	
+	void *callback_priv;
 	void (*callback) (void *);
-	void *priv;
+
+	void *free;
 };
 
 struct cn_callback_entry {
@@ -118,8 +125,8 @@
 	struct work_struct work;
 	struct cn_queue_dev *pdev;
 
-	void (*destruct_data) (void *);
-	void *ddata;
+	struct cn_callback_id id;
+	struct cn_callback_data data;
 
 	int seq, group;
 	struct sock *nls;
@@ -144,7 +151,7 @@
 void cn_del_callback(struct cb_id *);
 int cn_netlink_send(struct cn_msg *, u32, int);
 
-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 *));
 void cn_queue_del_callback(struct cn_queue_dev *dev, struct cb_id *id);
 
 struct cn_queue_dev *cn_queue_alloc_dev(char *name, struct sock *);
@@ -152,6 +159,8 @@
 
 int cn_cb_equal(struct cb_id *, struct cb_id *);
 
+void cn_queue_wrapper(void *data);
+
 extern int cn_already_initialized;
 
 #endif				/* __KERNEL__ */